More optimizations
This commit is contained in:
Daniel Chappuis
parent
0032fef473
commit
a871bfdd6a
|
|
@ -65,8 +65,8 @@ class RigidBody : public CollisionBody {
|
|||
/// Compute the local-space inertia tensor and total mass of the body using its colliders
|
||||
void computeMassAndInertiaTensorLocal(Vector3& inertiaTensorLocal, decimal& totalMass) const;
|
||||
|
||||
/// Return the inverse of the inertia tensor in world coordinates.
|
||||
static const Matrix3x3 getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity);
|
||||
/// Compute the inverse of the inertia tensor in world coordinates.
|
||||
static void computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld);
|
||||
|
||||
public :
|
||||
|
||||
|
|
|
|||
|
|
@ -109,9 +109,12 @@ class RigidBodyComponents : public Components {
|
|||
/// Array with the inertia tensor of each component
|
||||
Vector3* mLocalInertiaTensors;
|
||||
|
||||
/// Array with the inverse of the inertia tensor of each component
|
||||
/// Array with the inverse of the local inertia tensor of each component
|
||||
Vector3* mInverseInertiaTensorsLocal;
|
||||
|
||||
/// Array with the inverse of the world inertia tensor of each component
|
||||
Matrix3x3* mInverseInertiaTensorsWorld;
|
||||
|
||||
/// Array with the constrained linear velocity of each component
|
||||
Vector3* mConstrainedLinearVelocities;
|
||||
|
||||
|
|
@ -261,6 +264,9 @@ class RigidBodyComponents : public Components {
|
|||
/// Return the inverse local inertia tensor of an entity
|
||||
const Vector3& getInertiaTensorLocalInverse(Entity bodyEntity);
|
||||
|
||||
/// Return the inverse world inertia tensor of an entity
|
||||
const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
|
||||
|
||||
/// Set the external force of an entity
|
||||
void setExternalForce(Entity bodyEntity, const Vector3& externalForce);
|
||||
|
||||
|
|
@ -523,6 +529,14 @@ inline const Vector3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity b
|
|||
return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]];
|
||||
}
|
||||
|
||||
// Return the inverse world inertia tensor of an entity
|
||||
inline const Matrix3x3& RigidBodyComponents::getInertiaTensorWorldInverse(Entity bodyEntity) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
|
||||
|
||||
return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
|
||||
}
|
||||
|
||||
// Set the external force of an entity
|
||||
inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) {
|
||||
|
||||
|
|
|
|||
|
|
@ -301,6 +301,9 @@ class PhysicsWorld {
|
|||
/// Add the joint to the list of joints of the two bodies involved in the joint
|
||||
void addJointToBodies(Entity body1, Entity body2, Entity joint);
|
||||
|
||||
/// Update the world inverse inertia tensors of rigid bodies
|
||||
void updateBodiesInverseWorldInertiaTensors();
|
||||
|
||||
/// Destructor
|
||||
~PhysicsWorld();
|
||||
|
||||
|
|
|
|||
|
|
@ -886,16 +886,14 @@ void RigidBody::updateOverlappingPairs() {
|
|||
}
|
||||
}
|
||||
|
||||
/// Return the inverse of the inertia tensor in world coordinates.
|
||||
const Matrix3x3 RigidBody::getWorldInertiaTensorInverse(PhysicsWorld& world, Entity bodyEntity) {
|
||||
/// Compute the inverse of the inertia tensor in world coordinates.
|
||||
void RigidBody::computeWorldInertiaTensorInverse(const Matrix3x3& orientation, const Vector3& inverseInertiaTensorLocal, Matrix3x3& outInverseInertiaTensorWorld) {
|
||||
|
||||
Matrix3x3 orientation = world.mTransformComponents.getTransform(bodyEntity).getOrientation().getMatrix();
|
||||
const Vector3& inverseInertiaLocalTensor = world.mRigidBodyComponents.getInertiaTensorLocalInverse(bodyEntity);
|
||||
Matrix3x3 orientationTranspose = orientation.getTranspose();
|
||||
orientationTranspose[0] *= inverseInertiaLocalTensor.x;
|
||||
orientationTranspose[1] *= inverseInertiaLocalTensor.y;
|
||||
orientationTranspose[2] *= inverseInertiaLocalTensor.z;
|
||||
return orientation * orientationTranspose;
|
||||
orientationTranspose[0] *= inverseInertiaTensorLocal.x;
|
||||
orientationTranspose[1] *= inverseInertiaTensorLocal.y;
|
||||
orientationTranspose[2] *= inverseInertiaTensorLocal.z;
|
||||
outInverseInertiaTensorWorld = orientation * orientationTranspose;
|
||||
}
|
||||
|
||||
// Set whether or not the body is allowed to go to sleep
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator)
|
|||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) +
|
||||
sizeof(decimal) + sizeof(decimal) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(bool) + sizeof(bool) + sizeof(List<Entity>) + sizeof(List<uint>)) {
|
||||
|
|
@ -78,7 +78,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
decimal* newInverseMasses = reinterpret_cast<decimal*>(newMasses + nbComponentsToAllocate);
|
||||
Vector3* newInertiaTensorLocal = reinterpret_cast<Vector3*>(newInverseMasses + nbComponentsToAllocate);
|
||||
Vector3* newInertiaTensorLocalInverses = reinterpret_cast<Vector3*>(newInertiaTensorLocal + nbComponentsToAllocate);
|
||||
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
|
||||
Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
|
||||
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
|
||||
Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
|
||||
Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
|
||||
Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
|
||||
|
|
@ -111,6 +112,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
|
||||
memcpy(newInertiaTensorLocal, mLocalInertiaTensors, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
|
||||
memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
|
||||
|
|
@ -146,6 +148,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
mInverseMasses = newInverseMasses;
|
||||
mLocalInertiaTensors = newInertiaTensorLocal;
|
||||
mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
|
||||
mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
|
||||
mConstrainedLinearVelocities = newConstrainedLinearVelocities;
|
||||
mConstrainedAngularVelocities = newConstrainedAngularVelocities;
|
||||
mSplitLinearVelocities = newSplitLinearVelocities;
|
||||
|
|
@ -183,6 +186,7 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const
|
|||
mInverseMasses[index] = decimal(1.0);
|
||||
new (mLocalInertiaTensors + index) Vector3(1.0, 1.0, 1.0);
|
||||
new (mInverseInertiaTensorsLocal + index) Vector3(1.0, 1.0, 1.0);
|
||||
new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
|
||||
new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
|
||||
new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
|
||||
new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
|
||||
|
|
@ -228,6 +232,7 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex
|
|||
mInverseMasses[destIndex] = mInverseMasses[srcIndex];
|
||||
new (mLocalInertiaTensors + destIndex) Vector3(mLocalInertiaTensors[srcIndex]);
|
||||
new (mInverseInertiaTensorsLocal + destIndex) Vector3(mInverseInertiaTensorsLocal[srcIndex]);
|
||||
new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]);
|
||||
new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
|
||||
new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
|
||||
new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
|
||||
|
|
@ -272,6 +277,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
|
|||
decimal inverseMass1 = mInverseMasses[index1];
|
||||
Vector3 inertiaTensorLocal1 = mLocalInertiaTensors[index1];
|
||||
Vector3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
|
||||
Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
|
||||
Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
|
||||
Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
|
||||
Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
|
||||
|
|
@ -307,6 +313,7 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
|
|||
mInverseMasses[index2] = inverseMass1;
|
||||
mLocalInertiaTensors[index2] = inertiaTensorLocal1;
|
||||
mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
|
||||
mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
|
||||
new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1);
|
||||
new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
|
||||
new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1);
|
||||
|
|
@ -345,6 +352,7 @@ void RigidBodyComponents::destroyComponent(uint32 index) {
|
|||
mExternalTorques[index].~Vector3();
|
||||
mLocalInertiaTensors[index].~Vector3();
|
||||
mInverseInertiaTensorsLocal[index].~Vector3();
|
||||
mInverseInertiaTensorsWorld[index].~Matrix3x3();
|
||||
mConstrainedLinearVelocities[index].~Vector3();
|
||||
mConstrainedAngularVelocities[index].~Vector3();
|
||||
mSplitLinearVelocities[index].~Vector3();
|
||||
|
|
|
|||
|
|
@ -341,6 +341,9 @@ void PhysicsWorld::update(decimal timeStep) {
|
|||
// Report the contacts to the user
|
||||
mCollisionDetection.reportContactsAndTriggers();
|
||||
|
||||
// Recompute the inverse inertia tensors of rigid bodies
|
||||
updateBodiesInverseWorldInertiaTensors();
|
||||
|
||||
// Disable the joints for pair of sleeping bodies
|
||||
disableJointsOfSleepingBodies();
|
||||
|
||||
|
|
@ -379,6 +382,15 @@ void PhysicsWorld::update(decimal timeStep) {
|
|||
mMemoryManager.resetFrameAllocator();
|
||||
}
|
||||
|
||||
// Update the world inverse inertia tensors of rigid bodies
|
||||
void PhysicsWorld::updateBodiesInverseWorldInertiaTensors() {
|
||||
|
||||
for (uint i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
|
||||
const Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
|
||||
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation, mRigidBodyComponents.mInverseInertiaTensorsLocal[i], mRigidBodyComponents.mInverseInertiaTensorsWorld[i]);
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the contacts and constraints
|
||||
void PhysicsWorld::solveContactsAndConstraints(decimal timeStep) {
|
||||
|
|
|
|||
|
|
@ -147,8 +147,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
|
|||
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
|
||||
mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody1 = rigidBodyIndex1;
|
||||
mContactConstraints[mNbContactManifolds].rigidBodyComponentIndexBody2 = rigidBodyIndex2;
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity1);
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getWorldInertiaTensorInverse(mWorld, externalManifold.bodyEntity2);
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex1];
|
||||
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex2];
|
||||
mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1];
|
||||
mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2];
|
||||
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;
|
||||
|
|
@ -668,14 +668,20 @@ void ContactSolverSystem::solve() {
|
|||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
|
||||
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
Vector3 angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x += angularVelocity1.x;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y += angularVelocity1.y;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z += angularVelocity1.z;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
|
||||
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
Vector3 angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z;
|
||||
|
||||
// ------ Second friction constraint at the center of the contact manifold ----- //
|
||||
|
||||
|
|
@ -716,13 +722,21 @@ void ContactSolverSystem::solve() {
|
|||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x;
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y;
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
|
||||
angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x += angularVelocity1.x;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y += angularVelocity1.y;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z += angularVelocity1.z;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x;
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y;
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
|
||||
angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z;
|
||||
|
||||
// ------ Twist friction constraint at the center of the contact manifol ------ //
|
||||
|
||||
|
|
@ -745,10 +759,16 @@ void ContactSolverSystem::solve() {
|
|||
angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
|
||||
angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= angularVelocity1.x;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= angularVelocity1.y;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= angularVelocity1.z;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z;
|
||||
|
||||
// --------- Rolling resistance constraint at the center of the contact manifold --------- //
|
||||
|
||||
|
|
@ -766,10 +786,16 @@ void ContactSolverSystem::solve() {
|
|||
deltaLambdaRolling = mContactConstraints[c].rollingResistanceImpulse - lambdaTempRolling;
|
||||
|
||||
// Update the velocities of the body 1 by applying the impulse P
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1] -= mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
|
||||
angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * deltaLambdaRolling;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= angularVelocity1.x;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= angularVelocity1.y;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].z -= angularVelocity1.z;
|
||||
|
||||
// Update the velocities of the body 2 by applying the impulse P
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2] += mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
|
||||
angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * deltaLambdaRolling;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].x += angularVelocity2.x;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].y += angularVelocity2.y;
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[mContactConstraints[c].rigidBodyComponentIndexBody2].z += angularVelocity2.z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -130,8 +130,8 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
|
|||
// Integrate the external force to get the new velocity of the body
|
||||
mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep *
|
||||
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep *
|
||||
RigidBody::getWorldInertiaTensorInverse(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i];
|
||||
mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mInverseInertiaTensorsWorld[i] *
|
||||
mRigidBodyComponents.mExternalTorques[i];
|
||||
}
|
||||
|
||||
// Apply gravity force
|
||||
|
|
|
|||
|
|
@ -60,8 +60,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
|
||||
mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
|
||||
mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
|
||||
mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
@ -260,9 +260,14 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mBallAndSocketJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
|
||||
mBallAndSocketJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
|
||||
// Recompute the world inverse inertia tensors
|
||||
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
|
||||
mBallAndSocketJointComponents.mI1[i]);
|
||||
|
||||
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
|
||||
mBallAndSocketJointComponents.mI2[i]);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
|
|
|
|||
|
|
@ -60,8 +60,8 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
|
||||
mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
|
||||
mFixedJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
|
||||
mFixedJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
@ -346,9 +346,14 @@ void SolveFixedJointSystem::solvePositionConstraint() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mFixedJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
|
||||
mFixedJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
|
||||
// Recompute the world inverse inertia tensors
|
||||
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
|
||||
mFixedJointComponents.mI1[i]);
|
||||
|
||||
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
|
||||
mFixedJointComponents.mI2[i]);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
|
|||
|
|
@ -60,8 +60,8 @@ void SolveHingeJointSystem::initBeforeSolve() {
|
|||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
|
||||
mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
|
||||
mHingeJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
|
||||
mHingeJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
@ -539,9 +539,14 @@ void SolveHingeJointSystem::solvePositionConstraint() {
|
|||
Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mHingeJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
|
||||
mHingeJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
|
||||
// Recompute the world inverse inertia tensors
|
||||
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
|
||||
mHingeJointComponents.mI1[i]);
|
||||
|
||||
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
|
||||
mHingeJointComponents.mI2[i]);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
|
|
|
|||
|
|
@ -60,8 +60,8 @@ void SolveSliderJointSystem::initBeforeSolve() {
|
|||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
|
||||
mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
|
||||
mSliderJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity);
|
||||
mSliderJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
|
|
@ -619,9 +619,14 @@ void SolveSliderJointSystem::solvePositionConstraint() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Recompute the inverse inertia tensors
|
||||
mSliderJointComponents.mI1[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body1Entity);
|
||||
mSliderJointComponents.mI2[i] = RigidBody::getWorldInertiaTensorInverse(mWorld, body2Entity);
|
||||
// Recompute the world inverse inertia tensors
|
||||
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
|
||||
mSliderJointComponents.mI1[i]);
|
||||
|
||||
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
|
||||
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
|
||||
mSliderJointComponents.mI2[i]);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user