mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Make possible to lock the linear/angular axis of a RigidBody

Browse Source
This commit is contained in:
Daniel Chappuis 2021-01-15 23:47:29 +01:00
parent afe8f100d6
commit a3ceafff07
11 changed files with 295 additions and 180 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/reactphysics3d/body/RigidBody.h
View File

@ -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);

49
include/reactphysics3d/components/RigidBodyComponents.h
View File

@ -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<uint>* 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<Entity>& 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<Entity>& RigidBodyComponents::getJoints(Entity bodyEntity) const {

14
include/reactphysics3d/systems/ContactSolverSystem.h
View File

@ -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;

20
src/body/RigidBody.cpp
View File

@ -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

19
src/components/RigidBodyComponents.cpp
View File

@ -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<Entity>) + sizeof(Array<uint>)) {
sizeof(bool) + sizeof(bool) + sizeof(Array<Entity>) + sizeof(Array<uint>) +
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<bool*>(newIsGravityEnabled + nbComponentsToAllocate);
Array<Entity>* newJoints = reinterpret_cast<Array<Entity>*>(newIsAlreadyInIsland + nbComponentsToAllocate);
Array<uint>* newContactPairs = reinterpret_cast<Array<uint>*>(newJoints + nbComponentsToAllocate);
Vector3* newLinearLockAxisFactors = reinterpret_cast<Vector3*>(newContactPairs + nbComponentsToAllocate);
Vector3* newAngularLockAxisFactors = reinterpret_cast<Vector3*>(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<Entity>));
memcpy(newContactPairs, mContactPairs, mNbComponents * sizeof(Array<uint>));
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<Entity>(mMemoryAllocator);
new (mContactPairs + index) Array<uint>(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<Entity, uint32>(bodyEntity, index));
@ -245,6 +254,8 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex
mIsAlreadyInIsland[destIndex] = mIsAlreadyInIsland[srcIndex];
new (mJoints + destIndex) Array<Entity>(mJoints[srcIndex]);
new (mContactPairs + destIndex) Array<uint>(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<Entity> joints1 = mJoints[index1];
Array<uint> 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<Entity>(joints1);
new (mContactPairs + index2) Array<uint>(contactPairs1);
new (mLinearLockAxisFactors + index2) Vector3(linearLockAxisFactor1);
new (mAngularLockAxisFactors + index2) Vector3(angularLockAxisFactor1);
// Update the entity to component index mapping
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(entity1, index2));
@ -363,4 +378,6 @@ void RigidBodyComponents::destroyComponent(uint32 index) {
mCentersOfMassWorld[index].~Vector3();
mJoints[index].~Array<Entity>();
mContactPairs[index].~Array<uint>();
mLinearLockAxisFactors[index].~Vector3();
mAngularLockAxisFactors[index].~Vector3();
}

144
src/systems/ContactSolverSystem.cpp
View File

@ -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;

9
src/systems/DynamicsSystem.cpp
View File

@ -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;
}
}
}

38
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -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;

50
src/systems/SolveFixedJointSystem.cpp
View File

@ -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);

52
src/systems/SolveHingeJointSystem.cpp
View File

@ -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);

68
src/systems/SolveSliderJointSystem.cpp
View File

@ -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;