diff --git a/include/reactphysics3d/body/RigidBody.h b/include/reactphysics3d/body/RigidBody.h index 3e262dcb..d82a019f 100644 --- a/include/reactphysics3d/body/RigidBody.h +++ b/include/reactphysics3d/body/RigidBody.h @@ -153,6 +153,18 @@ class RigidBody : public CollisionBody { /// Set the angular damping factor void setAngularDamping(decimal angularDamping); + /// Return the lock translation factor + const Vector3& getLinearLockAxisFactor() const; + + /// Set the linear lock factor + void setLinearLockAxisFactor(const Vector3& linearLockAxisFactor) const; + + /// Return the lock rotation factor + const Vector3& getAngularLockAxisFactor() const; + + /// Set the lock rotation factor + void setAngularLockAxisFactor(const Vector3& angularLockAxisFactor) const; + /// Apply an external force to the body at its center of mass. void applyForceToCenterOfMass(const Vector3& force); diff --git a/include/reactphysics3d/components/RigidBodyComponents.h b/include/reactphysics3d/components/RigidBodyComponents.h index 69266262..bf7457c3 100644 --- a/include/reactphysics3d/components/RigidBodyComponents.h +++ b/include/reactphysics3d/components/RigidBodyComponents.h @@ -151,6 +151,12 @@ class RigidBodyComponents : public Components { /// For each body, the array of the indices of contact pairs in which the body is involved Array* mContactPairs; + /// For each body, the array of lock translation vectors + Vector3* mLinearLockAxisFactors; + + /// For each body, the array of lock rotation vectors + Vector3* mAngularLockAxisFactors; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -312,6 +318,12 @@ class RigidBodyComponents : public Components { /// Return true if the entity is already in an island bool getIsAlreadyInIsland(Entity bodyEntity) const; + /// Return the lock translation factor + const Vector3& getLinearLockAxisFactor(Entity bodyEntity) const; + + /// Return the lock rotation factor + const Vector3& getAngularLockAxisFactor(Entity bodyEntity) const; + /// Set the constrained linear velocity of an entity void setConstrainedLinearVelocity(Entity bodyEntity, const Vector3& constrainedLinearVelocity); @@ -342,6 +354,12 @@ class RigidBodyComponents : public Components { /// Set the value to know if the entity is already in an island void setIsAlreadyInIsland(Entity bodyEntity, bool isAlreadyInIsland); + /// Set the lock translation factor + void setLinearLockAxisFactor(Entity bodyEntity, const Vector3& lockTranslationFactor); + + /// Set the lock rotation factor + void setAngularLockAxisFactor(Entity bodyEntity, const Vector3& rotationTranslationFactor); + /// Return the array of joints of a body const Array& getJoints(Entity bodyEntity) const; @@ -754,6 +772,23 @@ RP3D_FORCE_INLINE bool RigidBodyComponents::getIsAlreadyInIsland(Entity bodyEnti return mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]]; } + +// Return the linear lock axis factor +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getLinearLockAxisFactor(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mLinearLockAxisFactors[mMapEntityToComponentIndex[bodyEntity]]; +} + +// Return the angular lock axis factor +RP3D_FORCE_INLINE const Vector3& RigidBodyComponents::getAngularLockAxisFactor(Entity bodyEntity) const { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + + return mAngularLockAxisFactors[mMapEntityToComponentIndex[bodyEntity]]; +} + // Set the value to know if the gravity is enabled for this entity RP3D_FORCE_INLINE void RigidBodyComponents::setIsGravityEnabled(Entity bodyEntity, bool isGravityEnabled) { @@ -769,6 +804,20 @@ RP3D_FORCE_INLINE void RigidBodyComponents::setIsAlreadyInIsland(Entity bodyEnti mIsAlreadyInIsland[mMapEntityToComponentIndex[bodyEntity]] = isAlreadyInIsland; } +// Set the linear lock axis factor +RP3D_FORCE_INLINE void RigidBodyComponents::setLinearLockAxisFactor(Entity bodyEntity, const Vector3& lockTranslationFactor) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + mLinearLockAxisFactors[mMapEntityToComponentIndex[bodyEntity]] = lockTranslationFactor; +} + +// Set the angular lock axis factor +RP3D_FORCE_INLINE void RigidBodyComponents::setAngularLockAxisFactor(Entity bodyEntity, const Vector3& lockRotationFactor) { + + assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); + mAngularLockAxisFactors[mMapEntityToComponentIndex[bodyEntity]] = lockRotationFactor; +} + // Return the array of joints of a body RP3D_FORCE_INLINE const Array& RigidBodyComponents::getJoints(Entity bodyEntity) const { diff --git a/include/reactphysics3d/systems/ContactSolverSystem.h b/include/reactphysics3d/systems/ContactSolverSystem.h index 213e9baa..af69b8f6 100644 --- a/include/reactphysics3d/systems/ContactSolverSystem.h +++ b/include/reactphysics3d/systems/ContactSolverSystem.h @@ -188,9 +188,21 @@ class ContactSolverSystem { /// Inverse of the mass of body 1 decimal massInverseBody1; - // Inverse of the mass of body 2 + /// Inverse of the mass of body 2 decimal massInverseBody2; + /// Linear lock axis factor of body 1 + Vector3 linearLockAxisFactorBody1; + + /// Linear lock axis factor of body 2 + Vector3 linearLockAxisFactorBody2; + + /// Angular lock axis factor of body 1 + Vector3 angularLockAxisFactorBody1; + + /// Angular lock axis factor of body 2 + Vector3 angularLockAxisFactorBody2; + /// Inverse inertia tensor of body 1 Matrix3x3 inverseInertiaTensorBody1; diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 752e5f52..e85eaf26 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -781,6 +781,26 @@ bool RigidBody::isGravityEnabled() const { return mWorld.mRigidBodyComponents.getIsGravityEnabled(mEntity); } +// Return the linear lock axis factor +const Vector3& RigidBody::getLinearLockAxisFactor() const { + return mWorld.mRigidBodyComponents.getLinearLockAxisFactor(mEntity); +} + +// Set the linear lock axis factor +void RigidBody::setLinearLockAxisFactor(const Vector3& linearLockAxisFactor) const { + mWorld.mRigidBodyComponents.setLinearLockAxisFactor(mEntity, linearLockAxisFactor); +} + +// Return the angular lock axis factor +const Vector3& RigidBody::getAngularLockAxisFactor() const { + return mWorld.mRigidBodyComponents.getAngularLockAxisFactor(mEntity); +} + +// Set the angular lock axis factor +void RigidBody::setAngularLockAxisFactor(const Vector3& angularLockAxisFactor) const { + mWorld.mRigidBodyComponents.setAngularLockAxisFactor(mEntity, angularLockAxisFactor); +} + // Apply an external torque to the body. /// If the body is sleeping, calling this method will wake it up. Note that the /// force will we added to the sum of the applied torques and that this sum will be diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index ce851b4d..ccc7d238 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/src/components/RigidBodyComponents.cpp @@ -43,7 +43,8 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator) 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(Array) + sizeof(Array)) { + sizeof(bool) + sizeof(bool) + sizeof(Array) + sizeof(Array) + + sizeof(Vector3) + sizeof(Vector3)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -91,6 +92,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { bool* newIsAlreadyInIsland = reinterpret_cast(newIsGravityEnabled + nbComponentsToAllocate); Array* newJoints = reinterpret_cast*>(newIsAlreadyInIsland + nbComponentsToAllocate); Array* newContactPairs = reinterpret_cast*>(newJoints + nbComponentsToAllocate); + Vector3* newLinearLockAxisFactors = reinterpret_cast(newContactPairs + nbComponentsToAllocate); + Vector3* newAngularLockAxisFactors = reinterpret_cast(newLinearLockAxisFactors + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -125,6 +128,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newIsAlreadyInIsland, mIsAlreadyInIsland, mNbComponents * sizeof(bool)); memcpy(newJoints, mJoints, mNbComponents * sizeof(Array)); memcpy(newContactPairs, mContactPairs, mNbComponents * sizeof(Array)); + memcpy(newLinearLockAxisFactors, mLinearLockAxisFactors, mNbComponents * sizeof(Vector3)); + memcpy(newAngularLockAxisFactors, mAngularLockAxisFactors, mNbComponents * sizeof(Vector3)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -161,6 +166,8 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { mIsAlreadyInIsland = newIsAlreadyInIsland; mJoints = newJoints; mContactPairs = newContactPairs; + mLinearLockAxisFactors = newLinearLockAxisFactors; + mAngularLockAxisFactors = newAngularLockAxisFactors; } // Add a component @@ -199,6 +206,8 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const mIsAlreadyInIsland[index] = false; new (mJoints + index) Array(mMemoryAllocator); new (mContactPairs + index) Array(mMemoryAllocator); + new (mLinearLockAxisFactors + index) Vector3(1, 1, 1); + new (mAngularLockAxisFactors + index) Vector3(1, 1, 1); // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(bodyEntity, index)); @@ -245,6 +254,8 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex]; new (mJoints + destIndex) Array(mJoints[srcIndex]); new (mContactPairs + destIndex) Array(mContactPairs[srcIndex]); + new (mLinearLockAxisFactors + destIndex) Vector3(mLinearLockAxisFactors[srcIndex]); + new (mAngularLockAxisFactors + destIndex) Vector3(mAngularLockAxisFactors[srcIndex]); // Destroy the source component destroyComponent(srcIndex); @@ -290,6 +301,8 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { bool isAlreadyInIsland1 = mIsAlreadyInIsland[index1]; Array joints1 = mJoints[index1]; Array contactPairs1 = mContactPairs[index1]; + Vector3 linearLockAxisFactor1(mLinearLockAxisFactors[index1]); + Vector3 angularLockAxisFactor1(mAngularLockAxisFactors[index1]); // Destroy component 1 destroyComponent(index1); @@ -326,6 +339,8 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { mIsAlreadyInIsland[index2] = isAlreadyInIsland1; new (mJoints + index2) Array(joints1); new (mContactPairs + index2) Array(contactPairs1); + new (mLinearLockAxisFactors + index2) Vector3(linearLockAxisFactor1); + new (mAngularLockAxisFactors + index2) Vector3(angularLockAxisFactor1); // Update the entity to component index mapping mMapEntityToComponentIndex.add(Pair(entity1, index2)); @@ -363,4 +378,6 @@ void RigidBodyComponents::destroyComponent(uint32 index) { mCentersOfMassWorld[index].~Vector3(); mJoints[index].~Array(); mContactPairs[index].~Array(); + mLinearLockAxisFactors[index].~Vector3(); + mAngularLockAxisFactors[index].~Vector3(); } diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index 18139b8f..d029d9e9 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -149,6 +149,10 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) { mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = mRigidBodyComponents.mInverseInertiaTensorsWorld[rigidBodyIndex2]; mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex1]; mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.mInverseMasses[rigidBodyIndex2]; + mContactConstraints[mNbContactManifolds].linearLockAxisFactorBody1 = mRigidBodyComponents.mLinearLockAxisFactors[rigidBodyIndex1]; + mContactConstraints[mNbContactManifolds].linearLockAxisFactorBody2 = mRigidBodyComponents.mLinearLockAxisFactors[rigidBodyIndex2]; + mContactConstraints[mNbContactManifolds].angularLockAxisFactorBody1 = mRigidBodyComponents.mAngularLockAxisFactors[rigidBodyIndex1]; + mContactConstraints[mNbContactManifolds].angularLockAxisFactorBody2 = mRigidBodyComponents.mAngularLockAxisFactors[rigidBodyIndex2]; mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints; mContactConstraints[mNbContactManifolds].frictionCoefficient = computeMixedFrictionCoefficient(mColliderComponents.mMaterials[collider1Index], mColliderComponents.mMaterials[collider2Index]); mContactConstraints[mNbContactManifolds].externalContactManifold = &externalManifold; @@ -349,22 +353,22 @@ void ContactSolverSystem::warmStart() { Vector3 impulsePenetration(mContactPoints[contactPointIndex].normal.x * mContactPoints[contactPointIndex].penetrationImpulse, mContactPoints[contactPointIndex].normal.y * mContactPoints[contactPointIndex].penetrationImpulse, mContactPoints[contactPointIndex].normal.z * mContactPoints[contactPointIndex].penetrationImpulse); - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * impulsePenetration.x * mContactConstraints[c].linearLockAxisFactorBody1.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * impulsePenetration.y * mContactConstraints[c].linearLockAxisFactorBody1.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * impulsePenetration.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactConstraints[c].angularLockAxisFactorBody1.x * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactConstraints[c].angularLockAxisFactorBody1.y * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactConstraints[c].angularLockAxisFactorBody1.z * mContactPoints[contactPointIndex].penetrationImpulse; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * impulsePenetration.x * mContactConstraints[c].linearLockAxisFactorBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * impulsePenetration.y * mContactConstraints[c].linearLockAxisFactorBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * impulsePenetration.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactPoints[contactPointIndex].penetrationImpulse; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactPoints[contactPointIndex].penetrationImpulse; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactConstraints[c].angularLockAxisFactorBody2.x * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactConstraints[c].angularLockAxisFactorBody2.y * mContactPoints[contactPointIndex].penetrationImpulse; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactConstraints[c].angularLockAxisFactorBody2.z * mContactPoints[contactPointIndex].penetrationImpulse; } else { // If it is a new contact point @@ -407,12 +411,12 @@ void ContactSolverSystem::warmStart() { const uint32 rigidBody2Index = mContactConstraints[c].rigidBodyComponentIndexBody2; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index] -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2 * mContactConstraints[c].linearLockAxisFactorBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index] += mContactConstraints[c].massInverseBody2 * linearImpulseBody2 * mContactConstraints[c].linearLockAxisFactorBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; // ------ Second friction constraint at the center of the contact manifold ----- // @@ -428,18 +432,18 @@ void ContactSolverSystem::warmStart() { angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * mContactConstraints[c].friction2Impulse; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x * mContactConstraints[c].linearLockAxisFactorBody1.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody1.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x * mContactConstraints[c].linearLockAxisFactorBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; // ------ Twist friction constraint at the center of the contact manifold ------ // @@ -453,16 +457,16 @@ void ContactSolverSystem::warmStart() { angularImpulseBody2.z = mContactConstraints[c].normal.z * mContactConstraints[c].frictionTwistImpulse; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] += mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] -= mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index] -= mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody2; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index] += mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; } else { // If it is a new contact manifold @@ -539,22 +543,22 @@ void ContactSolverSystem::solve() { mContactPoints[contactPointIndex].normal.z * deltaLambda); // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x * mContactConstraints[c].linearLockAxisFactorBody1.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y * mContactConstraints[c].linearLockAxisFactorBody1.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambda; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambda; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactConstraints[c].angularLockAxisFactorBody1.x * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactConstraints[c].angularLockAxisFactorBody1.y * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactConstraints[c].angularLockAxisFactorBody1.z * deltaLambda; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x * mContactConstraints[c].linearLockAxisFactorBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y * mContactConstraints[c].linearLockAxisFactorBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambda; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambda; - mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactConstraints[c].angularLockAxisFactorBody2.x * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactConstraints[c].angularLockAxisFactorBody2.y * deltaLambda; + mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactConstraints[c].angularLockAxisFactorBody2.z * deltaLambda; sumPenetrationImpulse += mContactPoints[contactPointIndex].penetrationImpulse; @@ -590,22 +594,22 @@ void ContactSolverSystem::solve() { mContactPoints[contactPointIndex].normal.z * deltaLambdaSplit); // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x; - mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y; - mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulse.x * mContactConstraints[c].linearLockAxisFactorBody1.x; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulse.y * mContactConstraints[c].linearLockAxisFactorBody1.y; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulse.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * deltaLambdaSplit; - mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * deltaLambdaSplit; - mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].x -= mContactPoints[contactPointIndex].i1TimesR1CrossN.x * mContactConstraints[c].angularLockAxisFactorBody1.x * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].y -= mContactPoints[contactPointIndex].i1TimesR1CrossN.y * mContactConstraints[c].angularLockAxisFactorBody1.y * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody1Index].z -= mContactPoints[contactPointIndex].i1TimesR1CrossN.z * mContactConstraints[c].angularLockAxisFactorBody1.z * deltaLambdaSplit; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x; - mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y; - mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulse.x * mContactConstraints[c].linearLockAxisFactorBody2.x; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulse.y * mContactConstraints[c].linearLockAxisFactorBody2.y; + mRigidBodyComponents.mSplitLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulse.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * deltaLambdaSplit; - mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * deltaLambdaSplit; - mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].x += mContactPoints[contactPointIndex].i2TimesR2CrossN.x * mContactConstraints[c].angularLockAxisFactorBody2.x * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].y += mContactPoints[contactPointIndex].i2TimesR2CrossN.y * mContactConstraints[c].angularLockAxisFactorBody2.y * deltaLambdaSplit; + mRigidBodyComponents.mSplitAngularVelocities[rigidBody2Index].z += mContactPoints[contactPointIndex].i2TimesR2CrossN.z * mContactConstraints[c].angularLockAxisFactorBody2.z * deltaLambdaSplit; } contactPointIndex++; @@ -649,21 +653,21 @@ void ContactSolverSystem::solve() { // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x * mContactConstraints[c].linearLockAxisFactorBody1.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody1.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - Vector3 angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + Vector3 angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x * mContactConstraints[c].linearLockAxisFactorBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - Vector3 angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + Vector3 angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; @@ -704,21 +708,21 @@ void ContactSolverSystem::solve() { angularImpulseBody2.z = mContactConstraints[c].r2CrossT2.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x * mContactConstraints[c].linearLockAxisFactorBody1.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody1.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody1Index].z -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody1.z; - angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody1; + angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody1; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x += angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y += angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z += angularVelocity1.z; // Update the velocities of the body 2 by applying the impulse P - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y; - mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].x += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.x * mContactConstraints[c].linearLockAxisFactorBody2.x; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].y += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.y * mContactConstraints[c].linearLockAxisFactorBody2.y; + mRigidBodyComponents.mConstrainedLinearVelocities[rigidBody2Index].z += mContactConstraints[c].massInverseBody2 * linearImpulseBody2.z * mContactConstraints[c].linearLockAxisFactorBody2.z; - angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; @@ -744,13 +748,13 @@ void ContactSolverSystem::solve() { angularImpulseBody2.z = mContactConstraints[c].normal.z * deltaLambda; // Update the velocities of the body 1 by applying the impulse P - angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * angularImpulseBody2; + angularVelocity1 = mContactConstraints[c].inverseInertiaTensorBody1 * mContactConstraints[c].angularLockAxisFactorBody1 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].x -= angularVelocity1.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].y -= angularVelocity1.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody1Index].z -= angularVelocity1.z; // Update the velocities of the body 1 by applying the impulse P - angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * angularImpulseBody2; + angularVelocity2 = mContactConstraints[c].inverseInertiaTensorBody2 * mContactConstraints[c].angularLockAxisFactorBody2 * angularImpulseBody2; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].x += angularVelocity2.x; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].y += angularVelocity2.y; mRigidBodyComponents.mConstrainedAngularVelocities[rigidBody2Index].z += angularVelocity2.z; diff --git a/src/systems/DynamicsSystem.cpp b/src/systems/DynamicsSystem.cpp index 409c9dac..d0a9aa5b 100644 --- a/src/systems/DynamicsSystem.cpp +++ b/src/systems/DynamicsSystem.cpp @@ -132,10 +132,10 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { const Vector3& angularVelocity = mRigidBodyComponents.mAngularVelocities[i]; // Integrate the external force to get the new velocity of the body - mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep * - mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i]; + mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep * mRigidBodyComponents.mInverseMasses[i] * + mRigidBodyComponents.mLinearLockAxisFactors[i] * mRigidBodyComponents.mExternalForces[i]; mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * - mRigidBodyComponents.mExternalTorques[i]; + mRigidBodyComponents.mAngularLockAxisFactors[i] * mRigidBodyComponents.mExternalTorques[i]; } // Apply gravity force @@ -149,7 +149,8 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { // Integrate the gravity force mRigidBodyComponents.mConstrainedLinearVelocities[i] = mRigidBodyComponents.mConstrainedLinearVelocities[i] + timeStep * - mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mMasses[i] * mGravity; + mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mLinearLockAxisFactors[i] * + mRigidBodyComponents.mMasses[i] * mGravity; } } } diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index b4902b98..5ee605d1 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -60,12 +60,15 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); // Get the inertia tensor of bodies - mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); - mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity); + mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1]; + mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -80,9 +83,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World); Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World); - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // Compute the matrix K=JM^-1J^t (3x3 matrix) const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; @@ -102,8 +102,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { mBallAndSocketJointComponents.mInverseMassMatrix[i] = massMatrix.getInverse(); } - const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); - const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity); + const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1]; + const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2]; // Compute the bias "b" of the constraint mBallAndSocketJointComponents.mBiasVector[i].setToZero(); @@ -153,15 +153,15 @@ void SolveBallAndSocketJointSystem::warmstart() { const Vector3 angularImpulseBody1 = mBallAndSocketJointComponents.mImpulse[i].cross(r1World); // Apply the impulse to the body 1 - v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * linearImpulseBody1; - w1 += i1 * angularImpulseBody1; + v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 const Vector3 angularImpulseBody2 = -mBallAndSocketJointComponents.mImpulse[i].cross(r2World); // Apply the impulse to the body to the body 2 - v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mBallAndSocketJointComponents.mImpulse[i]; - w2 += i2 * angularImpulseBody2; + v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * mBallAndSocketJointComponents.mImpulse[i]; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } } @@ -202,15 +202,15 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = deltaLambda.cross(mBallAndSocketJointComponents.mR1World[i]); // Apply the impulse to the body 1 - v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * linearImpulseBody1; - w1 += i1 * angularImpulseBody1; + v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the body 2 const Vector3 angularImpulseBody2 = -deltaLambda.cross(mBallAndSocketJointComponents.mR2World[i]); // Apply the impulse to the body 2 - v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * deltaLambda; - w2 += i2 * angularImpulseBody2; + v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambda; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } } @@ -290,8 +290,8 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody1 = lambda.cross(r1World); // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1; + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + const Vector3 w1 = mBallAndSocketJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body center of mass and orientation of body 1 x1 += v1; @@ -302,8 +302,8 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody2 = -lambda.cross(r2World); // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambda; - const Vector3 w2 = mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2; + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambda; + const Vector3 w2 = mBallAndSocketJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index e120bfcc..c117c4a2 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -60,12 +60,15 @@ void SolveFixedJointSystem::initBeforeSolve() { const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex]; const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex]; + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity)); assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity)); // Get the inertia tensor of bodies - mFixedJointComponents.mI1[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body1Entity); - mFixedJointComponents.mI2[i] = mRigidBodyComponents.getInertiaTensorWorldInverse(body2Entity); + mFixedJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1]; + mFixedJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2]; const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); @@ -78,9 +81,6 @@ void SolveFixedJointSystem::initBeforeSolve() { Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]); Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR2World[i]); - const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); - const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; @@ -99,8 +99,8 @@ void SolveFixedJointSystem::initBeforeSolve() { } // Get the bodies positions and orientations - const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity); - const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity); + const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1]; + const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2]; const Vector3& r1World = mFixedJointComponents.mR1World[i]; const Vector3& r2World = mFixedJointComponents.mR2World[i]; @@ -113,8 +113,8 @@ void SolveFixedJointSystem::initBeforeSolve() { // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation contraints (3x3 matrix) mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i]; - if (mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || - mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(); } @@ -179,8 +179,8 @@ void SolveFixedJointSystem::warmstart() { const Matrix3x3& i1 = mFixedJointComponents.mI1[i]; // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += i1 * angularImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World); @@ -191,8 +191,8 @@ void SolveFixedJointSystem::warmstart() { const Matrix3x3& i2 = mFixedJointComponents.mI2[i]; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * impulseTranslation; - w2 += i2 * angularImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * impulseTranslation; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } } @@ -244,8 +244,8 @@ void SolveFixedJointSystem::solveVelocityConstraint() { const Matrix3x3& i1 = mFixedJointComponents.mI1[i]; // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += i1 * angularImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for body 2 const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World); @@ -253,8 +253,8 @@ void SolveFixedJointSystem::solveVelocityConstraint() { const Matrix3x3& i2 = mFixedJointComponents.mI2[i]; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * deltaLambda; - w2 += i2 * angularImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambda; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // --------------- Rotation Constraints --------------- // @@ -272,10 +272,10 @@ void SolveFixedJointSystem::solveVelocityConstraint() { angularImpulseBody1 = -deltaLambda2; // Apply the impulse to the body 1 - w1 += i1 * angularImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Apply the impulse to the body 2 - w2 += i2 * deltaLambda2; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * deltaLambda2; } } @@ -354,8 +354,8 @@ void SolveFixedJointSystem::solvePositionConstraint() { Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World); // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = mFixedJointComponents.mI1[i] * angularImpulseBody1; + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + Vector3 w1 = mFixedJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -366,8 +366,8 @@ void SolveFixedJointSystem::solvePositionConstraint() { Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World); // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambdaTranslation; - Vector3 w2 = mFixedJointComponents.mI2[i] * angularImpulseBody2; + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; + Vector3 w2 = mFixedJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -419,14 +419,14 @@ void SolveFixedJointSystem::solvePositionConstraint() { angularImpulseBody1 = -lambdaRotation; // Compute the pseudo velocity of body 1 - w1 = mFixedJointComponents.mI1[i] * angularImpulseBody1; + w1 = mFixedJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); q1.normalize(); // Compute the pseudo velocity of body 2 - w2 = mFixedJointComponents.mI2[i] * lambdaRotation; + w2 = mFixedJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * lambdaRotation; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index 5a061f3f..8725b5a3 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -266,8 +266,8 @@ void SolveHingeJointSystem::warmstart() { angularImpulseBody1 += motorImpulse; // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mHingeJointComponents.mI1[i] * angularImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2 Vector3 angularImpulseBody2 = -impulseTranslation.cross(mHingeJointComponents.mR2World[i]); @@ -282,8 +282,8 @@ void SolveHingeJointSystem::warmstart() { angularImpulseBody2 += -motorImpulse; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * impulseTranslation; - w2 += mHingeJointComponents.mI2[i] * angularImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * impulseTranslation; + w2 += mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } } @@ -339,15 +339,15 @@ void SolveHingeJointSystem::solveVelocityConstraint() { Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(r1World); // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += i1 * angularImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda of body 2 Vector3 angularImpulseBody2 = -deltaLambdaTranslation.cross(r2World); // Apply the impulse to the body 2 - v2 += inverseMassBody2 * deltaLambdaTranslation; - w2 += i2 * angularImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambdaTranslation; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // --------------- Rotation Constraints --------------- // @@ -367,13 +367,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() { angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - c2CrossA1 * deltaLambdaRotation.y; // Apply the impulse to the body 1 - w1 += i1 * angularImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2 angularImpulseBody2 = b2CrossA1 * deltaLambdaRotation.x + c2CrossA1 * deltaLambdaRotation.y; // Apply the impulse to the body 2 - w2 += i2 * angularImpulseBody2; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // --------------- Limits Constraints --------------- // @@ -395,13 +395,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1; // Apply the impulse to the body 1 - w1 += i1 * angularImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 const Vector3 angularImpulseBody2 = deltaLambdaLower * a1; // Apply the impulse to the body 2 - w2 += i2 * angularImpulseBody2; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } // If the upper limit is violated @@ -420,13 +420,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1; // Apply the impulse to the body 1 - w1 += i1 * angularImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 const Vector3 angularImpulseBody2 = -deltaLambdaUpper * a1; // Apply the impulse to the body 2 - w2 += i2 * angularImpulseBody2; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } } @@ -449,13 +449,13 @@ void SolveHingeJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1; // Apply the impulse to the body 1 - w1 += i1 * angularImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the motor of body 2 const Vector3 angularImpulseBody2 = deltaLambdaMotor * a1; // Apply the impulse to the body 2 - w2 += i2 * angularImpulseBody2; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } } } @@ -551,8 +551,8 @@ void SolveHingeJointSystem::solvePositionConstraint() { decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; // Compute the pseudo velocity of body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1; + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + Vector3 w1 = mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -563,8 +563,8 @@ void SolveHingeJointSystem::solvePositionConstraint() { Vector3 angularImpulseBody2 = -lambdaTranslation.cross(mHingeJointComponents.mR2World[i]); // Compute the pseudo velocity of body 2 - const Vector3 v2 = inverseMassBody2 * lambdaTranslation; - Vector3 w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2; + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * lambdaTranslation; + Vector3 w2 = mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -603,7 +603,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y; // Compute the pseudo velocity of body 1 - w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1; + w1 = mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -613,7 +613,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y; // Compute the pseudo velocity of body 2 - w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2; + w2 = mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -654,7 +654,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody1 = -lambdaLowerLimit * a1; // Compute the pseudo velocity of body 1 - const Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1; + const Vector3 w1 = mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -664,7 +664,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody2 = lambdaLowerLimit * a1; // Compute the pseudo velocity of body 2 - const Vector3 w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2; + const Vector3 w2 = mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -681,7 +681,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody1 = lambdaUpperLimit * a1; // Compute the pseudo velocity of body 1 - const Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1; + const Vector3 w1 = mHingeJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -691,7 +691,7 @@ void SolveHingeJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody2 = -lambdaUpperLimit * a1; // Compute the pseudo velocity of body 2 - const Vector3 w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2; + const Vector3 w2 = mHingeJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index 851be91f..5c5e8c8e 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -289,8 +289,8 @@ void SolveSliderJointSystem::warmstart() { linearImpulseBody1 += impulseMotor; // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 Vector3 linearImpulseBody2 = n1 * impulseTranslation.x + n2 * impulseTranslation.y; @@ -308,8 +308,8 @@ void SolveSliderJointSystem::warmstart() { linearImpulseBody2 += -impulseMotor; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + w2 += mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } } @@ -370,16 +370,16 @@ void SolveSliderJointSystem::solveVelocityConstraint() { r1PlusUCrossN2 * deltaLambda.y; // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += i1 * angularImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2 const Vector3 linearImpulseBody2 = n1 * deltaLambda.x + n2 * deltaLambda.y; Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += i2 * angularImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + w2 += i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // --------------- Rotation Constraints --------------- // @@ -395,13 +395,13 @@ void SolveSliderJointSystem::solveVelocityConstraint() { angularImpulseBody1 = -deltaLambda2; // Apply the impulse to the body 1 - w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1; + w1 += mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2 angularImpulseBody2 = deltaLambda2; // Apply the impulse to the body 2 - w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2; + w2 += mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i]; const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i]; @@ -435,16 +435,16 @@ void SolveSliderJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = -deltaLambdaLower * r1PlusUCrossSliderAxis; // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 const Vector3 linearImpulseBody2 = deltaLambdaLower * sliderAxisWorld; const Vector3 angularImpulseBody2 = deltaLambdaLower * r2CrossSliderAxis; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + w2 += mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } // If the upper limit is violated @@ -465,16 +465,16 @@ void SolveSliderJointSystem::solveVelocityConstraint() { const Vector3 angularImpulseBody1 = deltaLambdaUpper * r1PlusUCrossSliderAxis; // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + w1 += mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Compute the impulse P=J^T * lambda for the upper limit constraint of body 2 const Vector3 linearImpulseBody2 = -deltaLambdaUpper * sliderAxisWorld; const Vector3 angularImpulseBody2 = -deltaLambdaUpper * r2CrossSliderAxis; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; - w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + w2 += mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; } } @@ -496,13 +496,13 @@ void SolveSliderJointSystem::solveVelocityConstraint() { const Vector3 linearImpulseBody1 = deltaLambdaMotor * sliderAxisWorld; // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; + v1 += inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; // Compute the impulse P=J^T * lambda for the motor of body 2 const Vector3 linearImpulseBody2 = -deltaLambdaMotor * sliderAxisWorld; // Apply the impulse to the body 2 - v2 += inverseMassBody2 * linearImpulseBody2; + v2 += inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; } } } @@ -626,8 +626,8 @@ void SolveSliderJointSystem::solvePositionConstraint() { r1PlusUCrossN2 * lambdaTranslation.y; // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - Vector3 w1 = i1 * angularImpulseBody1; + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + Vector3 w1 = i1 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -639,8 +639,8 @@ void SolveSliderJointSystem::solvePositionConstraint() { Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y; // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - Vector3 w2 = i2 * angularImpulseBody2; + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + Vector3 w2 = i2 * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -692,7 +692,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { angularImpulseBody1 = -lambdaRotation; // Apply the impulse to the body 1 - w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1; + w1 = mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 q1 += Quaternion(0, w1) * q1 * decimal(0.5); @@ -702,7 +702,7 @@ void SolveSliderJointSystem::solvePositionConstraint() { angularImpulseBody2 = lambdaRotation; // Apply the impulse to the body 2 - w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2; + w2 = mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); @@ -750,8 +750,8 @@ void SolveSliderJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody1 = -lambdaLowerLimit * r1PlusUCrossSliderAxis; // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1; + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + const Vector3 w1 = mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -763,8 +763,8 @@ void SolveSliderJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody2 = lambdaLowerLimit * r2CrossSliderAxis; // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - const Vector3 w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2; + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + const Vector3 w2 = mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2; @@ -789,8 +789,8 @@ void SolveSliderJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody1 = lambdaUpperLimit * r1PlusUCrossSliderAxis; // Apply the impulse to the body 1 - const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; - const Vector3 w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1; + const Vector3 v1 = inverseMassBody1 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; + const Vector3 w1 = mSliderJointComponents.mI1[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * angularImpulseBody1; // Update the body position/orientation of body 1 x1 += v1; @@ -802,8 +802,8 @@ void SolveSliderJointSystem::solvePositionConstraint() { const Vector3 angularImpulseBody2 = -lambdaUpperLimit * r2CrossSliderAxis; // Apply the impulse to the body 2 - const Vector3 v2 = inverseMassBody2 * linearImpulseBody2; - const Vector3 w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2; + const Vector3 v2 = inverseMassBody2 * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * linearImpulseBody2; + const Vector3 w2 = mSliderJointComponents.mI2[i] * mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * angularImpulseBody2; // Update the body position/orientation of body 2 x2 += v2;