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

Add SolveHingeJointSystem class

Browse Source
This commit is contained in:
Daniel Chappuis 2019-10-05 17:45:35 +02:00
parent ab02d98f3a
commit afb34b4355
22 changed files with 1178 additions and 888 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -134,6 +134,7 @@ SET (REACTPHYSICS3D_HEADERS
"src/systems/CollisionDetectionSystem.h" "src/systems/CollisionDetectionSystem.h"
"src/systems/SolveBallAndSocketJointSystem.h" "src/systems/SolveBallAndSocketJointSystem.h"
"src/systems/SolveFixedJointSystem.h" "src/systems/SolveFixedJointSystem.h"
"src/systems/SolveHingeJointSystem.h"
"src/engine/DynamicsWorld.h" "src/engine/DynamicsWorld.h"
"src/engine/EventListener.h" "src/engine/EventListener.h"
"src/engine/Island.h" "src/engine/Island.h"
@ -232,6 +233,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/systems/CollisionDetectionSystem.cpp" "src/systems/CollisionDetectionSystem.cpp"
"src/systems/SolveBallAndSocketJointSystem.cpp" "src/systems/SolveBallAndSocketJointSystem.cpp"
"src/systems/SolveFixedJointSystem.cpp" "src/systems/SolveFixedJointSystem.cpp"
"src/systems/SolveHingeJointSystem.cpp"
"src/engine/DynamicsWorld.cpp" "src/engine/DynamicsWorld.cpp"
"src/engine/Island.cpp" "src/engine/Island.cpp"
"src/engine/Material.cpp" "src/engine/Material.cpp"

44
src/body/RigidBody.cpp
View File

@ -86,7 +86,6 @@ void RigidBody::setType(BodyType type) {
// Reset the inverse mass and inverse inertia tensor to zero // Reset the inverse mass and inverse inertia tensor to zero
mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0)); mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0));
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
} }
else { // If it is a dynamic body else { // If it is a dynamic body
mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mRigidBodyComponents.getInitMass(mEntity)); mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(1.0) / mWorld.mRigidBodyComponents.getInitMass(mEntity));
@ -96,9 +95,6 @@ void RigidBody::setType(BodyType type) {
} }
} }
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Awake the body // Awake the body
setIsSleeping(false); setIsSleeping(false);
@ -130,10 +126,9 @@ const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const {
* @return The 3x3 inverse inertia tensor matrix of the body in world-space * @return The 3x3 inverse inertia tensor matrix of the body in world-space
* coordinates * coordinates
*/ */
Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { const Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// Compute and return the inertia tensor in world coordinates return getInertiaTensorInverseWorld(mWorld, mEntity);
return mWorld.mRigidBodyComponents.getInertiaTensorWorldInverse(mEntity);
} }
// Method that return the mass of the body // Method that return the mass of the body
@ -192,9 +187,6 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
// Compute the inverse local inertia tensor // Compute the inverse local inertia tensor
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string()); "Body " + std::to_string(mEntity.id) + ": Set inertiaTensorLocal=" + inertiaTensorLocal.to_string());
} }
@ -255,9 +247,6 @@ void RigidBody::setInverseInertiaTensorLocal(const Matrix3x3& inverseInertiaTens
// Compute the inverse local inertia tensor // Compute the inverse local inertia tensor
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body,
"Body " + std::to_string(mEntity.id) + ": Set inverseInertiaTensorLocal=" + inverseInertiaTensorLocal.to_string()); "Body " + std::to_string(mEntity.id) + ": Set inverseInertiaTensorLocal=" + inverseInertiaTensorLocal.to_string());
} }
@ -317,20 +306,6 @@ void RigidBody::setMass(decimal mass) {
"Body " + std::to_string(mEntity.id) + ": Set mass=" + std::to_string(mass)); "Body " + std::to_string(mEntity.id) + ": Set mass=" + std::to_string(mass));
} }
// Update the world inverse inertia tensor of the body
/// The inertia tensor I_w in world coordinates is computed with the
/// local inverse inertia tensor I_b^-1 in body coordinates
/// by I_w = R * I_b^-1 * R^T
/// where R is the rotation matrix (and R^T its transpose) of the
/// current orientation quaternion of the body
void RigidBody::updateInertiaTensorInverseWorld() {
// TODO : Make sure we do this in a system
Matrix3x3 orientation = mWorld.mTransformComponents.getTransform(mEntity).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = mWorld.mRigidBodyComponents.getInertiaTensorLocalInverse(mEntity);
mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, orientation * inverseInertiaLocalTensor * orientation.getTranspose());
}
// Add a collision shape to the body. // Add a collision shape to the body.
/// When you add a collision shape to the body, an internal copy of this /// When you add a collision shape to the body, an internal copy of this
@ -555,9 +530,6 @@ void RigidBody::setTransform(const Transform& transform) {
// Update the transform of the body // Update the transform of the body
mWorld.mTransformComponents.setTransform(mEntity, transform); mWorld.mTransformComponents.setTransform(mEntity, transform);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Awake the body if it is sleeping // Awake the body if it is sleeping
setIsSleeping(false); setIsSleeping(false);
} }
@ -569,7 +541,6 @@ void RigidBody::recomputeMassInformation() {
mWorld.mRigidBodyComponents.setInitMass(mEntity, decimal(0.0)); mWorld.mRigidBodyComponents.setInitMass(mEntity, decimal(0.0));
mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0.0)); mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0.0));
if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero());
if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero());
if (!mIsCenterOfMassSetByUser) mWorld.mRigidBodyComponents.setCenterOfMassLocal(mEntity, Vector3::zero()); if (!mIsCenterOfMassSetByUser) mWorld.mRigidBodyComponents.setCenterOfMassLocal(mEntity, Vector3::zero());
Matrix3x3 inertiaTensorLocal; Matrix3x3 inertiaTensorLocal;
inertiaTensorLocal.setToZero(); inertiaTensorLocal.setToZero();
@ -651,9 +622,6 @@ void RigidBody::recomputeMassInformation() {
mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse()); mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse());
} }
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Update the linear velocity of the center of mass // Update the linear velocity of the center of mass
Vector3 linearVelocity = mWorld.mRigidBodyComponents.getLinearVelocity(mEntity); Vector3 linearVelocity = mWorld.mRigidBodyComponents.getLinearVelocity(mEntity);
Vector3 angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); Vector3 angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity);
@ -748,6 +716,14 @@ void RigidBody::setIsSleeping(bool isSleeping) {
(isSleeping ? "true" : "false")); (isSleeping ? "true" : "false"));
} }
/// Return the inverse of the inertia tensor in world coordinates.
const Matrix3x3 RigidBody::getInertiaTensorInverseWorld(CollisionWorld& world, Entity bodyEntity) {
Matrix3x3 orientation = world.mTransformComponents.getTransform(bodyEntity).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = world.mRigidBodyComponents.getInertiaTensorLocalInverse(bodyEntity);
return orientation * inverseInertiaLocalTensor * orientation.getTranspose();
}
// Set whether or not the body is allowed to go to sleep // Set whether or not the body is allowed to go to sleep
/** /**
* @param isAllowedToSleep True if the body is allowed to sleep * @param isAllowedToSleep True if the body is allowed to sleep

12
src/body/RigidBody.h
View File

@ -73,12 +73,12 @@ class RigidBody : public CollisionBody {
/// Update the transform of the body after a change of the center of mass /// Update the transform of the body after a change of the center of mass
void updateTransformWithCenterOfMass(); void updateTransformWithCenterOfMass();
/// Update the world inverse inertia tensor of the body
void updateInertiaTensorInverseWorld();
/// Set the variable to know whether or not the body is sleeping /// Set the variable to know whether or not the body is sleeping
void setIsSleeping(bool isSleeping); void setIsSleeping(bool isSleeping);
/// Return the inverse of the inertia tensor in world coordinates.
static const Matrix3x3 getInertiaTensorInverseWorld(CollisionWorld& world, Entity bodyEntity);
public : public :
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
@ -123,7 +123,7 @@ class RigidBody : public CollisionBody {
const Matrix3x3& getInverseInertiaTensorLocal() const; const Matrix3x3& getInverseInertiaTensorLocal() const;
/// Return the inverse of the inertia tensor in world coordinates. /// Return the inverse of the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorInverseWorld() const; const Matrix3x3 getInertiaTensorInverseWorld() const;
/// Set the local center of mass of the body (in local-space coordinates) /// Set the local center of mass of the body (in local-space coordinates)
void setCenterOfMassLocal(const Vector3& centerOfMassLocal); void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
@ -215,11 +215,15 @@ class RigidBody : public CollisionBody {
friend class DynamicsWorld; friend class DynamicsWorld;
friend class ContactSolverSystem; friend class ContactSolverSystem;
friend class DynamicsSystem;
friend class Joint; friend class Joint;
friend class BallAndSocketJoint; friend class BallAndSocketJoint;
friend class SliderJoint; friend class SliderJoint;
friend class HingeJoint; friend class HingeJoint;
friend class FixedJoint; friend class FixedJoint;
friend class SolveBallAndSocketJointSystem;
friend class SolveFixedJointSystem;
friend class SolveHingeJointSystem;
}; };
// Return a reference to the material properties of the rigid body // Return a reference to the material properties of the rigid body

1
src/components/HingeJointComponents.h
View File

@ -411,6 +411,7 @@ class HingeJointComponents : public Components {
// -------------------- Friendship -------------------- // // -------------------- Friendship -------------------- //
friend class BroadPhaseSystem; friend class BroadPhaseSystem;
friend class SolveHingeJointSystem;
}; };
// Return a pointer to a given joint // Return a pointer to a given joint

12
src/components/RigidBodyComponents.cpp
View File

@ -40,7 +40,7 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator)
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) +
sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) +
sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) +
sizeof(bool) + sizeof(bool) + sizeof(List<Entity>)) { sizeof(bool) + sizeof(bool) + sizeof(List<Entity>)) {
@ -77,8 +77,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate); decimal* newInitMasses = reinterpret_cast<decimal*>(newAngularDampings + nbComponentsToAllocate);
decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate); decimal* newInverseMasses = reinterpret_cast<decimal*>(newInitMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate); Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast<Matrix3x3*>(newInverseMasses + nbComponentsToAllocate);
Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast<Matrix3x3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate); Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorLocalInverses + nbComponentsToAllocate);
Vector3* newConstrainedLinearVelocities = reinterpret_cast<Vector3*>(newInertiaTensorWorldInverses + nbComponentsToAllocate);
Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate); Vector3* newConstrainedAngularVelocities = reinterpret_cast<Vector3*>(newConstrainedLinearVelocities + nbComponentsToAllocate);
Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate); Vector3* newSplitLinearVelocities = reinterpret_cast<Vector3*>(newConstrainedAngularVelocities + nbComponentsToAllocate);
Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate); Vector3* newSplitAngularVelocities = reinterpret_cast<Vector3*>(newSplitLinearVelocities + nbComponentsToAllocate);
@ -109,7 +108,6 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal)); memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal));
memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal)); memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal));
memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3)); memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3));
memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3));
memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3)); memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3));
memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3)); memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3));
memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3)); memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3));
@ -143,7 +141,6 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) {
mInitMasses = newInitMasses; mInitMasses = newInitMasses;
mInverseMasses = newInverseMasses; mInverseMasses = newInverseMasses;
mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses; mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses;
mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses;
mConstrainedLinearVelocities = newConstrainedLinearVelocities; mConstrainedLinearVelocities = newConstrainedLinearVelocities;
mConstrainedAngularVelocities = newConstrainedAngularVelocities; mConstrainedAngularVelocities = newConstrainedAngularVelocities;
mSplitLinearVelocities = newSplitLinearVelocities; mSplitLinearVelocities = newSplitLinearVelocities;
@ -179,7 +176,6 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const
mInitMasses[index] = decimal(1.0); mInitMasses[index] = decimal(1.0);
mInverseMasses[index] = decimal(1.0); mInverseMasses[index] = decimal(1.0);
new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); new (mInverseInertiaTensorsLocal + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
new (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0); new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0);
new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0); new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0);
new (mSplitLinearVelocities + index) Vector3(0, 0, 0); new (mSplitLinearVelocities + index) Vector3(0, 0, 0);
@ -223,7 +219,6 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex
mInitMasses[destIndex] = mInitMasses[srcIndex]; mInitMasses[destIndex] = mInitMasses[srcIndex];
mInverseMasses[destIndex] = mInverseMasses[srcIndex]; mInverseMasses[destIndex] = mInverseMasses[srcIndex];
new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]); new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]);
new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]);
new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]); new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]);
new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]); new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]);
new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]); new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]);
@ -266,7 +261,6 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
decimal initMass1 = mInitMasses[index1]; decimal initMass1 = mInitMasses[index1];
decimal inverseMass1 = mInverseMasses[index1]; decimal inverseMass1 = mInverseMasses[index1];
Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1]; Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1];
Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1];
Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]); Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]);
Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]); Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]);
Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]); Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]);
@ -300,7 +294,6 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) {
mInitMasses[index2] = initMass1; mInitMasses[index2] = initMass1;
mInverseMasses[index2] = inverseMass1; mInverseMasses[index2] = inverseMass1;
mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1; mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1;
mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1;
new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1); new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1);
new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1); new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1);
new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1); new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1);
@ -337,7 +330,6 @@ void RigidBodyComponents::destroyComponent(uint32 index) {
mExternalForces[index].~Vector3(); mExternalForces[index].~Vector3();
mExternalTorques[index].~Vector3(); mExternalTorques[index].~Vector3();
mInverseInertiaTensorsLocal[index].~Matrix3x3(); mInverseInertiaTensorsLocal[index].~Matrix3x3();
mInverseInertiaTensorsWorld[index].~Matrix3x3();
mConstrainedLinearVelocities[index].~Vector3(); mConstrainedLinearVelocities[index].~Vector3();
mConstrainedAngularVelocities[index].~Vector3(); mConstrainedAngularVelocities[index].~Vector3();
mSplitLinearVelocities[index].~Vector3(); mSplitLinearVelocities[index].~Vector3();

34
src/components/RigidBodyComponents.h
View File

@ -109,9 +109,6 @@ class RigidBodyComponents : public Components {
/// Array with the inverse of the inertia tensor of each component /// Array with the inverse of the inertia tensor of each component
Matrix3x3* mInverseInertiaTensorsLocal; Matrix3x3* mInverseInertiaTensorsLocal;
/// Array with the inverse of the world inertia tensor of each component
Matrix3x3* mInverseInertiaTensorsWorld;
/// Array with the constrained linear velocity of each component /// Array with the constrained linear velocity of each component
Vector3* mConstrainedLinearVelocities; Vector3* mConstrainedLinearVelocities;
@ -246,9 +243,6 @@ class RigidBodyComponents : public Components {
/// Return the inverse local inertia tensor of an entity /// Return the inverse local inertia tensor of an entity
const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity); const Matrix3x3& getInertiaTensorLocalInverse(Entity bodyEntity);
/// Return the inverse world inertia tensor of an entity
const Matrix3x3& getInertiaTensorWorldInverse(Entity bodyEntity);
/// Set the external force of an entity /// Set the external force of an entity
void setExternalForce(Entity bodyEntity, const Vector3& externalForce); void setExternalForce(Entity bodyEntity, const Vector3& externalForce);
@ -270,9 +264,6 @@ class RigidBodyComponents : public Components {
/// Set the inverse local inertia tensor of an entity /// Set the inverse local inertia tensor of an entity
void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse); void setInverseInertiaTensorLocal(Entity bodyEntity, const Matrix3x3& inertiaTensorLocalInverse);
/// Set the inverse world inertia tensor of an entity
void setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse);
/// Return the constrained linear velocity of an entity /// Return the constrained linear velocity of an entity
const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const; const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const;
@ -286,10 +277,10 @@ class RigidBodyComponents : public Components {
const Vector3& getSplitAngularVelocity(Entity bodyEntity) const; const Vector3& getSplitAngularVelocity(Entity bodyEntity) const;
/// Return the constrained position of an entity /// Return the constrained position of an entity
const Vector3& getConstrainedPosition(Entity bodyEntity); Vector3& getConstrainedPosition(Entity bodyEntity);
/// Return the constrained orientation of an entity /// Return the constrained orientation of an entity
const Quaternion& getConstrainedOrientation(Entity bodyEntity); Quaternion& getConstrainedOrientation(Entity bodyEntity);
/// Return the local center of mass of an entity /// Return the local center of mass of an entity
const Vector3& getCenterOfMassLocal(Entity bodyEntity); const Vector3& getCenterOfMassLocal(Entity bodyEntity);
@ -348,6 +339,7 @@ class RigidBodyComponents : public Components {
friend class ContactSolverSystem; friend class ContactSolverSystem;
friend class SolveBallAndSocketJointSystem; friend class SolveBallAndSocketJointSystem;
friend class SolveFixedJointSystem; friend class SolveFixedJointSystem;
friend class SolveHingeJointSystem;
friend class DynamicsSystem; friend class DynamicsSystem;
friend class BallAndSocketJoint; friend class BallAndSocketJoint;
friend class FixedJoint; friend class FixedJoint;
@ -515,14 +507,6 @@ inline const Matrix3x3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity
return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]]; return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]];
} }
// Return the inverse world inertia tensor of an entity
inline const Matrix3x3& RigidBodyComponents::getInertiaTensorWorldInverse(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]];
}
// Set the external force of an entity // Set the external force of an entity
inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) { inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) {
@ -579,14 +563,6 @@ inline void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity,
mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse; mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorLocalInverse;
} }
// Set the inverse world inertia tensor of an entity
inline void RigidBodyComponents::setInverseInertiaTensorWorld(Entity bodyEntity, const Matrix3x3& inertiaTensorWorldInverse) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]] = inertiaTensorWorldInverse;
}
// Return the constrained linear velocity of an entity // Return the constrained linear velocity of an entity
inline const Vector3& RigidBodyComponents::getConstrainedLinearVelocity(Entity bodyEntity) const { inline const Vector3& RigidBodyComponents::getConstrainedLinearVelocity(Entity bodyEntity) const {
@ -620,7 +596,7 @@ inline const Vector3& RigidBodyComponents::getSplitAngularVelocity(Entity bodyEn
} }
// Return the constrained position of an entity // Return the constrained position of an entity
inline const Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) { inline Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); assert(mMapEntityToComponentIndex.containsKey(bodyEntity));
@ -628,7 +604,7 @@ inline const Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEnt
} }
// Return the constrained orientation of an entity // Return the constrained orientation of an entity
inline const Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) { inline Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) {
assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); assert(mMapEntityToComponentIndex.containsKey(bodyEntity));

715
src/constraint/HingeJoint.cpp
View File

@ -31,9 +31,6 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Static variables definition
const decimal HingeJoint::BETA = decimal(0.2);
// Constructor // Constructor
HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) : Joint(entity, world) { HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo& jointInfo) : Joint(entity, world) {
@ -67,651 +64,21 @@ HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo
// Initialize before solving the constraint // Initialize before solving the constraint
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) { void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Get the bodies positions and orientations
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity);
const Quaternion& orientationBody1 = mWorld.mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation();
// Get the inertia tensor of bodies
mWorld.mHingeJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
mWorld.mHingeJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Compute the vector from body center to the anchor point in world-space
mWorld.mHingeJointsComponents.setR1World(mEntity, orientationBody1 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity));
mWorld.mHingeJointsComponents.setR2World(mEntity, orientationBody2 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity));
// Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(orientationBody1, orientationBody2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
decimal upperLimitError = mWorld.mHingeJointsComponents.getUpperLimit(mEntity) - hingeAngle;
bool oldIsLowerLimitViolated = mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity);
bool isLowerLimitViolated = lowerLimitError <= 0;
mWorld.mHingeJointsComponents.setIsLowerLimitViolated(mEntity, isLowerLimitViolated);
if (isLowerLimitViolated != oldIsLowerLimitViolated) {
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
}
bool oldIsUpperLimitViolated = mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity);
bool isUpperLimitViolated = upperLimitError <= 0;
mWorld.mHingeJointsComponents.setIsUpperLimitViolated(mEntity, isUpperLimitViolated);
if (isUpperLimitViolated != oldIsUpperLimitViolated) {
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
}
// Compute vectors needed in the Jacobian
Vector3 a1 = orientationBody1 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity);
Vector3 a2 = orientationBody2 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity);
a1.normalize();
a2.normalize();
mWorld.mHingeJointsComponents.setA1(mEntity, a1);
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
mWorld.mHingeJointsComponents.setB2CrossA1(mEntity, b2.cross(a1));
mWorld.mHingeJointsComponents.setC2CrossA1(mEntity, c2.cross(a1));
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mWorld.mHingeJointsComponents.getR1World(mEntity));
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mWorld.mHingeJointsComponents.getR2World(mEntity));
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity());
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mWorld.mHingeJointsComponents.getI1(mEntity) * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mWorld.mHingeJointsComponents.getI2(mEntity) * skewSymmetricMatrixU2.getTranspose();
Matrix3x3& inverseMassMatrixTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity);
inverseMassMatrixTranslation.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mWorld.mHingeJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse());
}
// Compute the bias "b" of the translation constraints
Vector3& bTranslation = mWorld.mHingeJointsComponents.getBiasTranslation(mEntity);
bTranslation.setToZero();
decimal biasFactor = (BETA / constraintSolverData.timeStep);
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
bTranslation = biasFactor * (x2 + mWorld.mHingeJointsComponents.getR2World(mEntity) - x1 - mWorld.mHingeJointsComponents.getR1World(mEntity));
mWorld.mHingeJointsComponents.setBiasTranslation(mEntity, bTranslation);
}
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 i1B2CrossA1 = i1 * b2CrossA1;
Vector3 i1C2CrossA1 = i1 * c2CrossA1;
Vector3 i2B2CrossA1 = i2 * b2CrossA1;
Vector3 i2C2CrossA1 = i2 * c2CrossA1;
const decimal el11 = b2CrossA1.dot(i1B2CrossA1) +
b2CrossA1.dot(i2B2CrossA1);
const decimal el12 = b2CrossA1.dot(i1C2CrossA1) +
b2CrossA1.dot(i2C2CrossA1);
const decimal el21 = c2CrossA1.dot(i1B2CrossA1) +
c2CrossA1.dot(i2B2CrossA1);
const decimal el22 = c2CrossA1.dot(i1C2CrossA1) +
c2CrossA1.dot(i2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
Matrix2x2& inverseMassMatrixRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity);
inverseMassMatrixRotation.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mWorld.mHingeJointsComponents.setInverseMassMatrixRotation(mEntity, matrixKRotation.getInverse());
}
// Compute the bias "b" of the rotation constraints
Vector2& biasRotation = mWorld.mHingeJointsComponents.getBiasRotation(mEntity);
biasRotation.setToZero();
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mWorld.mHingeJointsComponents.setBiasRotation(mEntity, biasFactor * Vector2(a1.dot(b2), a1.dot(c2)));
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
// Reset all the accumulated impulses
Vector3& impulseTranslation = mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity);
Vector2& impulseRotation = mWorld.mHingeJointsComponents.getImpulseRotation(mEntity);
impulseTranslation.setToZero();
impulseRotation.setToZero();
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, decimal(0.0));
}
// If the motor or limits are enabled
if (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity) ||
(mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity) && (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity) ||
mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)))) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
decimal inverseMassMatrixLimitMotor = a1.dot(i1 * a1) + a1.dot(i2 * a1);
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
mWorld.mHingeJointsComponents.setInverseMassMatrixLimitMotor(mEntity, inverseMassMatrixLimitMotor);
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
// Compute the bias "b" of the lower limit constraint
mWorld.mHingeJointsComponents.setBLowerLimit(mEntity, decimal(0.0));
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mWorld.mHingeJointsComponents.setBLowerLimit(mEntity, biasFactor * lowerLimitError);
}
// Compute the bias "b" of the upper limit constraint
mWorld.mHingeJointsComponents.setBUpperLimit(mEntity, decimal(0.0));
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mWorld.mHingeJointsComponents.setBUpperLimit(mEntity, biasFactor * upperLimitError);
}
}
}
} }
// Warm start the constraint (apply the previous impulse at the beginning of the step) // Warm start the constraint (apply the previous impulse at the beginning of the step)
void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) { void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const Vector3& impulseTranslation = mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity);
const Vector2& impulseRotation = mWorld.mHingeJointsComponents.getImpulseRotation(mEntity);
const decimal impulseLowerLimit = mWorld.mHingeJointsComponents.getImpulseLowerLimit(mEntity);
const decimal impulseUpperLimit = mWorld.mHingeJointsComponents.getImpulseUpperLimit(mEntity);
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
const Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -b2CrossA1 * impulseRotation.x - mWorld.mHingeJointsComponents.getC2CrossA1(mEntity) * impulseRotation.y;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
const Vector3 limitsImpulse = (impulseUpperLimit - impulseLowerLimit) * a1;
// Compute the impulse P=J^T * lambda for the motor constraint
const Vector3 motorImpulse = -mWorld.mHingeJointsComponents.getImpulseMotor(mEntity) * a1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
Vector3 linearImpulseBody1 = -impulseTranslation;
Vector3 angularImpulseBody1 = impulseTranslation.cross(mWorld.mHingeJointsComponents.getR1World(mEntity));
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 += rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
angularImpulseBody1 += limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
angularImpulseBody1 += motorImpulse;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mWorld.mHingeJointsComponents.getI1(mEntity) * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2
Vector3 angularImpulseBody2 = -impulseTranslation.cross(mWorld.mHingeJointsComponents.getR2World(mEntity));
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
angularImpulseBody2 += -rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
angularImpulseBody2 += -limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint of body 2
angularImpulseBody2 += -motorImpulse;
// Apply the impulse to the body 2
v2 += inverseMassBody2 * impulseTranslation;
w2 += mWorld.mHingeJointsComponents.getI2(mEntity) * angularImpulseBody2;
} }
// Solve the velocity constraint // Solve the velocity constraint
void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
const Vector3& r1World = mWorld.mHingeJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mHingeJointsComponents.getR2World(mEntity);
const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
const Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
const decimal inverseMassMatrixLimitMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity);
// --------------- Translation Constraints --------------- //
// Compute J*v
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambdaTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity) *
(-JvTranslation - mWorld.mHingeJointsComponents.getBiasTranslation(mEntity));
mWorld.mHingeJointsComponents.setImpulseTranslation(mEntity, deltaLambdaTranslation + mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity));
// Compute the impulse P=J^T * lambda of body 1
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(r1World);
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * 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;
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 2 rotation constraints
const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2),
-c2CrossA1.dot(w1) + c2CrossA1.dot(w2));
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
Vector2 deltaLambdaRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity) *
(-JvRotation - mWorld.mHingeJointsComponents.getBiasRotation(mEntity));
mWorld.mHingeJointsComponents.setImpulseRotation(mEntity, deltaLambdaRotation + mWorld.mHingeJointsComponents.getImpulseRotation(mEntity));
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x -
c2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body 1
w1 += i1 * 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;
// --------------- Limits Constraints --------------- //
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
// If the lower limit is violated
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity)) {
decimal impulseLowerLimit = mWorld.mHingeJointsComponents.getImpulseLowerLimit(mEntity);
// Compute J*v for the lower limit constraint
const decimal JvLowerLimit = (w2 - w1).dot(a1);
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal deltaLambdaLower = inverseMassMatrixLimitMotor * (-JvLowerLimit -mWorld.mHingeJointsComponents.getBLowerLimit(mEntity));
decimal lambdaTemp = impulseLowerLimit;
impulseLowerLimit = std::max(impulseLowerLimit + deltaLambdaLower, decimal(0.0));
deltaLambdaLower = impulseLowerLimit - lambdaTemp;
mWorld.mHingeJointsComponents.setImpulseLowerLimit(mEntity, impulseLowerLimit);
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1;
// Apply the impulse to the body 1
w1 += i1 * 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;
}
// If the upper limit is violated
if (mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
decimal impulseUpperLimit = mWorld.mHingeJointsComponents.getImpulseUpperLimit(mEntity);
// Compute J*v for the upper limit constraint
const decimal JvUpperLimit = -(w2 - w1).dot(a1);
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal deltaLambdaUpper = inverseMassMatrixLimitMotor * (-JvUpperLimit -mWorld.mHingeJointsComponents.getBUpperLimit(mEntity));
decimal lambdaTemp = impulseUpperLimit;
impulseUpperLimit = std::max(impulseUpperLimit + deltaLambdaUpper, decimal(0.0));
deltaLambdaUpper = impulseUpperLimit - lambdaTemp;
mWorld.mHingeJointsComponents.setImpulseUpperLimit(mEntity, impulseUpperLimit);
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1;
// Apply the impulse to the body 1
w1 += i1 * 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;
}
}
// --------------- Motor --------------- //
// If the motor is enabled
if (mWorld.mHingeJointsComponents.getIsMotorEnabled(mEntity)) {
decimal impulseMotor = mWorld.mHingeJointsComponents.getImpulseMotor(mEntity);
// Compute J*v for the motor
const decimal JvMotor = a1.dot(w1 - w2);
// Compute the Lagrange multiplier lambda for the motor
const decimal maxMotorImpulse = mWorld.mHingeJointsComponents.getMaxMotorTorque(mEntity) * constraintSolverData.timeStep;
decimal deltaLambdaMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity) * (-JvMotor - mWorld.mHingeJointsComponents.getMotorSpeed(mEntity));
decimal lambdaTemp = impulseMotor;
impulseMotor = clamp(impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = impulseMotor - lambdaTemp;
mWorld.mHingeJointsComponents.setImpulseMotor(mEntity, impulseMotor);
// Compute the impulse P=J^T * lambda for the motor of body 1
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1;
// Apply the impulse to the body 1
w1 += i1 * 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;
}
} }
// Solve the position constraint (for position error correction) // Solve the position constraint (for position error correction)
void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) { void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
const Matrix3x3& i1 = mWorld.mHingeJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mHingeJointsComponents.getI2(mEntity);
const Vector3& r1World = mWorld.mHingeJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mHingeJointsComponents.getR2World(mEntity);
Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.getB2CrossA1(mEntity);
Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.getC2CrossA1(mEntity);
Vector3& a1 = mWorld.mHingeJointsComponents.getA1(mEntity);
// Get the bodies positions and orientations
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Recompute the inverse inertia tensors
mWorld.mHingeJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
mWorld.mHingeJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Compute the vector from body center to the anchor point in world-space
mWorld.mHingeJointsComponents.setR1World(mEntity, q1 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody1(mEntity));
mWorld.mHingeJointsComponents.setR2World(mEntity, q2 * mWorld.mHingeJointsComponents.getLocalAnchorPointBody2(mEntity));
// Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(q1, q2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mWorld.mHingeJointsComponents.getLowerLimit(mEntity);
decimal upperLimitError = mWorld.mHingeJointsComponents.getUpperLimit(mEntity) - hingeAngle;
mWorld.mHingeJointsComponents.setIsLowerLimitViolated(mEntity, lowerLimitError <= 0);
mWorld.mHingeJointsComponents.setIsUpperLimitViolated(mEntity, upperLimitError <= 0);
// Compute vectors needed in the Jacobian
a1 = q1 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody1(mEntity);
Vector3 a2 = q2 * mWorld.mHingeJointsComponents.getHingeLocalAxisBody2(mEntity);
a1.normalize();
mWorld.mHingeJointsComponents.setA1(mEntity, a1);
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
b2CrossA1 = b2.cross(a1);
mWorld.mHingeJointsComponents.setB2CrossA1(mEntity, b2CrossA1);
c2CrossA1 = c2.cross(a1);
mWorld.mHingeJointsComponents.setC2CrossA1(mEntity, c2CrossA1);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
const decimal body1InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal body2InverseMass = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose();
Matrix3x3& inverseMassMatrixTranslation = mWorld.mHingeJointsComponents.getInverseMassMatrixTranslation(mEntity);
inverseMassMatrixTranslation.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
inverseMassMatrixTranslation = massMatrix.getInverse();
mWorld.mHingeJointsComponents.setInverseMassMatrixTranslation(mEntity, inverseMassMatrixTranslation);
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = inverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1 = i1 * b2CrossA1;
Vector3 I1C2CrossA1 = i1 * c2CrossA1;
Vector3 I2B2CrossA1 = i2 * b2CrossA1;
Vector3 I2C2CrossA1 = i2 * c2CrossA1;
const decimal el11 = b2CrossA1.dot(I1B2CrossA1) +
b2CrossA1.dot(I2B2CrossA1);
const decimal el12 = b2CrossA1.dot(I1C2CrossA1) +
b2CrossA1.dot(I2C2CrossA1);
const decimal el21 = c2CrossA1.dot(I1B2CrossA1) +
c2CrossA1.dot(I2B2CrossA1);
const decimal el22 = c2CrossA1.dot(I1C2CrossA1) +
c2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
Matrix2x2& inverseMassMatrixRotation = mWorld.mHingeJointsComponents.getInverseMassMatrixRotation(mEntity);
inverseMassMatrixRotation.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mWorld.mHingeJointsComponents.setInverseMassMatrixRotation(mEntity, matrixKRotation.getInverse());
}
// Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = inverseMassMatrixRotation * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 1
w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 2
w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Limits Constraints --------------- //
if (mWorld.mHingeJointsComponents.getIsLimitEnabled(mEntity)) {
decimal inverseMassMatrixLimitMotor = mWorld.mHingeJointsComponents.getInverseMassMatrixLimitMotor(mEntity);
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity) || mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
inverseMassMatrixLimitMotor = a1.dot(i1 * a1) + a1.dot(i2 * a1);
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
mWorld.mHingeJointsComponents.setInverseMassMatrixLimitMotor(mEntity, inverseMassMatrixLimitMotor);
}
// If the lower limit is violated
if (mWorld.mHingeJointsComponents.getIsLowerLimitViolated(mEntity)) {
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = inverseMassMatrixLimitMotor * (-lowerLimitError );
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * a1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse P=J^T * lambda of body 2
const Vector3 angularImpulseBody2 = lambdaLowerLimit * a1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// If the upper limit is violated
if (mWorld.mHingeJointsComponents.getIsUpperLimitViolated(mEntity)) {
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = inverseMassMatrixLimitMotor * (-upperLimitError);
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = lambdaUpperLimit * a1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse P=J^T * lambda of body 2
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * a1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
} }
@ -824,88 +191,6 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) {
} }
} }
// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
decimal HingeJoint::computeNormalizedAngle(decimal angle) const {
// Convert it into the range [-2*pi; 2*pi]
angle = std::fmod(angle, PI_TIMES_2);
// Convert it into the range [-pi; pi]
if (angle < -PI) {
return angle + PI_TIMES_2;
}
else if (angle > PI) {
return angle - PI_TIMES_2;
}
else {
return angle;
}
}
// Given an "inputAngle" in the range [-pi, pi], this method returns an
// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
// two angle limits in arguments.
decimal HingeJoint::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, decimal upperLimitAngle) const {
if (upperLimitAngle <= lowerLimitAngle) {
return inputAngle;
}
else if (inputAngle > upperLimitAngle) {
decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle;
}
else if (inputAngle < lowerLimitAngle) {
decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
}
else {
return inputAngle;
}
}
// Compute the current angle around the hinge axis
decimal HingeJoint::computeCurrentHingeAngle(const Quaternion& orientationBody1, const Quaternion& orientationBody2) {
decimal hingeAngle;
// Compute the current orientation difference between the two bodies
Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
currentOrientationDiff.normalize();
// Compute the relative rotation considering the initial orientation difference
Quaternion relativeRotation = currentOrientationDiff * mWorld.mHingeJointsComponents.getInitOrientationDifferenceInv(mEntity);
relativeRotation.normalize();
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
// length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with :
// |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any
// rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation
// axis is not pointing in the same direction as the hinge axis, we use the rotation -q which
// has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details
// about this trick is explained in the source code of OpenTissue (http://www.opentissue.org).
decimal cosHalfAngle = relativeRotation.w;
decimal sinHalfAngleAbs = relativeRotation.getVectorV().length();
// Compute the dot product of the relative rotation axis and the hinge axis
decimal dotProduct = relativeRotation.getVectorV().dot(mWorld.mHingeJointsComponents.getA1(mEntity));
// If the relative rotation axis and the hinge axis are pointing the same direction
if (dotProduct >= decimal(0.0)) {
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
}
else {
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle);
}
// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
hingeAngle = computeNormalizedAngle(hingeAngle);
// Compute and return the corresponding angle near one the two limits
return computeCorrespondingAngleNearLimits(hingeAngle,
mWorld.mHingeJointsComponents.getLowerLimit(mEntity),
mWorld.mHingeJointsComponents.getUpperLimit(mEntity));
}
// Return true if the limits of the joint are enabled // Return true if the limits of the joint are enabled
/** /**

17
src/constraint/HingeJoint.h
View File

@ -144,9 +144,6 @@ class HingeJoint : public Joint {
// -------------------- Constants -------------------- // // -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
@ -155,20 +152,6 @@ class HingeJoint : public Joint {
/// Reset the limits /// Reset the limits
void resetLimits(); void resetLimits();
/// Given an angle in radian, this method returns the corresponding
/// angle in the range [-pi; pi]
decimal computeNormalizedAngle(decimal angle) const;
/// Given an "inputAngle" in the range [-pi, pi], this method returns an
/// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
/// two angle limits in arguments.
decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
decimal upperLimitAngle) const;
/// Compute the current angle around the hinge axis
decimal computeCurrentHingeAngle(const Quaternion& orientationBody1,
const Quaternion& orientationBody2);
/// Return the number of bytes used by the joint /// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const override; virtual size_t getSizeInBytes() const override;

8
src/constraint/SliderJoint.cpp
View File

@ -88,8 +88,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation(); const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation();
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); mWorld.mSliderJointsComponents.setI1(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity));
mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); mWorld.mSliderJointsComponents.setI2(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity));
// Vector from body center to the anchor point // Vector from body center to the anchor point
mWorld.mSliderJointsComponents.setR1(mEntity, orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity)); mWorld.mSliderJointsComponents.setR1(mEntity, orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity));
@ -529,8 +529,8 @@ void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraint
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
// Recompute the inertia tensor of bodies // Recompute the inertia tensor of bodies
mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); mWorld.mSliderJointsComponents.setI1(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity));
mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); mWorld.mSliderJointsComponents.setI2(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity));
// Vector from body center to the anchor point // Vector from body center to the anchor point
mWorld.mSliderJointsComponents.setR1(mEntity, q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity)); mWorld.mSliderJointsComponents.setR1(mEntity, q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity));

11
src/engine/DynamicsWorld.cpp
View File

@ -50,11 +50,11 @@ using namespace std;
DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler) DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler)
: CollisionWorld(worldSettings, logger, profiler), : CollisionWorld(worldSettings, logger, profiler),
mIslands(mMemoryManager.getSingleFrameAllocator()), mIslands(mMemoryManager.getSingleFrameAllocator()),
mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents, mContactSolverSystem(mMemoryManager, *this, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
mProxyShapesComponents, mConfig), mProxyShapesComponents, mConfig),
mConstraintSolverSystem(mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents, mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
mBallAndSocketJointsComponents, mFixedJointsComponents), mBallAndSocketJointsComponents, mFixedJointsComponents, mHingeJointsComponents),
mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity), mDynamicsSystem(*this, mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()),
@ -249,9 +249,6 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
// Compute the inverse mass // Compute the inverse mass
mRigidBodyComponents.setMassInverse(entity, decimal(1.0) / mRigidBodyComponents.getInitMass(entity)); mRigidBodyComponents.setMassInverse(entity, decimal(1.0) / mRigidBodyComponents.getInitMass(entity));
// Update the world inverse inertia tensor
rigidBody->updateInertiaTensorInverseWorld();
// Add the rigid body to the physics world // Add the rigid body to the physics world
mBodies.add(rigidBody); mBodies.add(rigidBody);
mRigidBodies.add(rigidBody); mRigidBodies.add(rigidBody);

27
src/systems/ConstraintSolverSystem.cpp
View File

@ -33,17 +33,19 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents, ConstraintSolverSystem::ConstraintSolverSystem(DynamicsWorld& world, Islands& islands, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents, TransformComponents& transformComponents,
JointComponents& jointComponents, JointComponents& jointComponents,
BallAndSocketJointComponents& ballAndSocketJointComponents, BallAndSocketJointComponents& ballAndSocketJointComponents,
FixedJointComponents& fixedJointComponents) FixedJointComponents& fixedJointComponents,
HingeJointComponents& hingeJointComponents)
: mIsWarmStartingActive(true), mIslands(islands), : mIsWarmStartingActive(true), mIslands(islands),
mConstraintSolverData(rigidBodyComponents, jointComponents), mConstraintSolverData(rigidBodyComponents, jointComponents),
mSolveBallAndSocketJointSystem(rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents), mSolveBallAndSocketJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents),
mSolveFixedJointSystem(rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents), mSolveFixedJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents),
mSolveHingeJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, hingeJointComponents),
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents), mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents),
mFixedJointComponents(fixedJointComponents) { mFixedJointComponents(fixedJointComponents), mHingeJointComponents(hingeJointComponents) {
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE
@ -69,13 +71,17 @@ void ConstraintSolverSystem::initialize(decimal dt) {
mSolveBallAndSocketJointSystem.setIsWarmStartingActive(mIsWarmStartingActive); mSolveBallAndSocketJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
mSolveFixedJointSystem.setTimeStep(dt); mSolveFixedJointSystem.setTimeStep(dt);
mSolveFixedJointSystem.setIsWarmStartingActive(mIsWarmStartingActive); mSolveFixedJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
mSolveHingeJointSystem.setTimeStep(dt);
mSolveHingeJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
mSolveBallAndSocketJointSystem.initBeforeSolve(); mSolveBallAndSocketJointSystem.initBeforeSolve();
mSolveFixedJointSystem.initBeforeSolve(); mSolveFixedJointSystem.initBeforeSolve();
mSolveHingeJointSystem.initBeforeSolve();
if (mIsWarmStartingActive) { if (mIsWarmStartingActive) {
mSolveBallAndSocketJointSystem.warmstart(); mSolveBallAndSocketJointSystem.warmstart();
mSolveFixedJointSystem.warmstart(); mSolveFixedJointSystem.warmstart();
mSolveHingeJointSystem.warmstart();
} }
// For each joint // For each joint
@ -84,7 +90,8 @@ void ConstraintSolverSystem::initialize(decimal dt) {
// TODO : DELETE THIS // TODO : DELETE THIS
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i]; Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
if (mBallAndSocketJointComponents.hasComponent(jointEntity) || if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
mFixedJointComponents.hasComponent(jointEntity)) { mFixedJointComponents.hasComponent(jointEntity) ||
mHingeJointComponents.hasComponent(jointEntity)) {
continue; continue;
} }
@ -110,6 +117,7 @@ void ConstraintSolverSystem::solveVelocityConstraints() {
mSolveBallAndSocketJointSystem.solveVelocityConstraint(); mSolveBallAndSocketJointSystem.solveVelocityConstraint();
mSolveFixedJointSystem.solveVelocityConstraint(); mSolveFixedJointSystem.solveVelocityConstraint();
mSolveHingeJointSystem.solveVelocityConstraint();
// For each joint // For each joint
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) { for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
@ -117,7 +125,8 @@ void ConstraintSolverSystem::solveVelocityConstraints() {
// TODO : DELETE THIS // TODO : DELETE THIS
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i]; Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
if (mBallAndSocketJointComponents.hasComponent(jointEntity) || if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
mFixedJointComponents.hasComponent(jointEntity)) { mFixedJointComponents.hasComponent(jointEntity) ||
mHingeJointComponents.hasComponent(jointEntity)) {
continue; continue;
} }
@ -133,6 +142,7 @@ void ConstraintSolverSystem::solvePositionConstraints() {
mSolveBallAndSocketJointSystem.solvePositionConstraint(); mSolveBallAndSocketJointSystem.solvePositionConstraint();
mSolveFixedJointSystem.solvePositionConstraint(); mSolveFixedJointSystem.solvePositionConstraint();
mSolveHingeJointSystem.solvePositionConstraint();
// For each joint // For each joint
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) { for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
@ -140,7 +150,8 @@ void ConstraintSolverSystem::solvePositionConstraints() {
// TODO : DELETE THIS // TODO : DELETE THIS
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i]; Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
if (mBallAndSocketJointComponents.hasComponent(jointEntity) || if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
mFixedJointComponents.hasComponent(jointEntity)) { mFixedJointComponents.hasComponent(jointEntity) ||
mHingeJointComponents.hasComponent(jointEntity)) {
continue; continue;
} }

11
src/systems/ConstraintSolverSystem.h
View File

@ -32,6 +32,7 @@
#include "engine/Islands.h" #include "engine/Islands.h"
#include "systems/SolveBallAndSocketJointSystem.h" #include "systems/SolveBallAndSocketJointSystem.h"
#include "systems/SolveFixedJointSystem.h" #include "systems/SolveFixedJointSystem.h"
#include "systems/SolveHingeJointSystem.h"
namespace reactphysics3d { namespace reactphysics3d {
@ -165,6 +166,9 @@ class ConstraintSolverSystem {
/// Solver for the FixedJoint constraints /// Solver for the FixedJoint constraints
SolveFixedJointSystem mSolveFixedJointSystem; SolveFixedJointSystem mSolveFixedJointSystem;
/// Solver for the HingeJoint constraints
SolveHingeJointSystem mSolveHingeJointSystem;
// TODO : Delete this // TODO : Delete this
JointComponents& mJointComponents; JointComponents& mJointComponents;
@ -174,6 +178,9 @@ class ConstraintSolverSystem {
// TODO : Delete this // TODO : Delete this
FixedJointComponents& mFixedJointComponents; FixedJointComponents& mFixedJointComponents;
// TODO : Delete this
HingeJointComponents& mHingeJointComponents;
#ifdef IS_PROFILING_ACTIVE #ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler /// Pointer to the profiler
@ -185,11 +192,11 @@ class ConstraintSolverSystem {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents, ConstraintSolverSystem(DynamicsWorld& world, Islands& islands, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents, TransformComponents& transformComponents,
JointComponents& jointComponents, JointComponents& jointComponents,
BallAndSocketJointComponents& ballAndSocketJointComponents, BallAndSocketJointComponents& ballAndSocketJointComponents,
FixedJointComponents& fixedJointComponents); FixedJointComponents& fixedJointComponents, HingeJointComponents &hingeJointComponents);
/// Destructor /// Destructor
~ConstraintSolverSystem() = default; ~ConstraintSolverSystem() = default;

8
src/systems/ContactSolverSystem.cpp
View File

@ -43,10 +43,10 @@ const decimal ContactSolverSystem::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolverSystem::SLOP = decimal(0.01); const decimal ContactSolverSystem::SLOP = decimal(0.01);
// Constructor // Constructor
ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, ContactSolverSystem::ContactSolverSystem(MemoryManager& memoryManager, DynamicsWorld& world, Islands& islands, CollisionBodyComponents& bodyComponents,
RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents, RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
const WorldSettings& worldSettings) const WorldSettings& worldSettings)
:mMemoryManager(memoryManager), mContactConstraints(nullptr), mContactPoints(nullptr), :mMemoryManager(memoryManager), mWorld(world), mContactConstraints(nullptr), mContactPoints(nullptr),
mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr), mIslands(islands), mAllContactManifolds(nullptr), mAllContactPoints(nullptr),
mBodyComponents(bodyComponents), mRigidBodyComponents(rigidBodyComponents), mBodyComponents(bodyComponents), mRigidBodyComponents(rigidBodyComponents),
mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true), mProxyShapeComponents(proxyShapeComponents), mIsSplitImpulseActive(true),
@ -140,8 +140,8 @@ void ContactSolverSystem::initializeForIsland(uint islandIndex) {
new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver(); new (mContactConstraints + mNbContactManifolds) ContactManifoldSolver();
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1->getEntity()); mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1->getEntity());
mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2->getEntity()); mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2->getEntity());
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity1);
mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity2);
mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.getMassInverse(body1->getEntity()); mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.getMassInverse(body1->getEntity());
mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.getMassInverse(body2->getEntity()); mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.getMassInverse(body2->getEntity());
mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints; mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints;

5
src/systems/ContactSolverSystem.h
View File

@ -284,6 +284,9 @@ class ContactSolverSystem {
/// Memory manager /// Memory manager
MemoryManager& mMemoryManager; MemoryManager& mMemoryManager;
/// Physics world
DynamicsWorld& mWorld;
/// Current time step /// Current time step
decimal mTimeStep; decimal mTimeStep;
@ -360,7 +363,7 @@ class ContactSolverSystem {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, ContactSolverSystem(MemoryManager& memoryManager, DynamicsWorld& world, Islands& islands, CollisionBodyComponents& bodyComponents,
RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents, RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents,
const WorldSettings& worldSettings); const WorldSettings& worldSettings);

16
src/systems/DynamicsSystem.cpp
View File

@ -25,12 +25,14 @@
// Libraries // Libraries
#include "systems/DynamicsSystem.h" #include "systems/DynamicsSystem.h"
#include "body/RigidBody.h"
#include "engine/DynamicsWorld.h"
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
DynamicsSystem::DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity) DynamicsSystem::DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity)
:mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled), :mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled),
mGravity(gravity) { mGravity(gravity) {
} }
@ -95,14 +97,6 @@ void DynamicsSystem::updateBodiesState() {
const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i]; const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i];
transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal); transform.setPosition(centerOfMassWorld - transform.getOrientation() * centerOfMassLocal);
} }
// Update the world inverse inertia tensor of the body
for (uint32 i=0; i < mRigidBodyComponents.getNbEnabledComponents(); i++) {
Matrix3x3 orientation = mTransformComponents.getTransform(mRigidBodyComponents.mBodiesEntities[i]).getOrientation().getMatrix();
const Matrix3x3& inverseInertiaLocalTensor = mRigidBodyComponents.mInverseInertiaTensorsLocal[i];
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] = orientation * inverseInertiaLocalTensor * orientation.getTranspose();
}
} }
// Integrate the velocities of rigid bodies. // Integrate the velocities of rigid bodies.
@ -130,7 +124,7 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) {
mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep * mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep *
mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i]; mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i];
mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep *
mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i]; RigidBody::getInertiaTensorInverseWorld(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i];
} }
// Apply gravity force // Apply gravity force

7
src/systems/DynamicsSystem.h
View File

@ -33,6 +33,8 @@
namespace reactphysics3d { namespace reactphysics3d {
class DynamicsWorld;
// Class DynamicsSystem // Class DynamicsSystem
/** /**
* This class is responsible to compute and update the dynamics of the bodies that are simulated * This class is responsible to compute and update the dynamics of the bodies that are simulated
@ -44,6 +46,9 @@ class DynamicsSystem {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Physics world
DynamicsWorld& mWorld;
/// Reference to the rigid body components /// Reference to the rigid body components
RigidBodyComponents& mRigidBodyComponents; RigidBodyComponents& mRigidBodyComponents;
@ -67,7 +72,7 @@ class DynamicsSystem {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
bool& isGravityEnabled, Vector3& gravity); bool& isGravityEnabled, Vector3& gravity);
/// Destructor /// Destructor

21
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -25,6 +25,7 @@
// Libraries // Libraries
#include "systems/SolveBallAndSocketJointSystem.h" #include "systems/SolveBallAndSocketJointSystem.h"
#include "engine/DynamicsWorld.h"
#include "body/RigidBody.h" #include "body/RigidBody.h"
using namespace reactphysics3d; using namespace reactphysics3d;
@ -33,11 +34,11 @@ using namespace reactphysics3d;
const decimal SolveBallAndSocketJointSystem::BETA = decimal(0.2); const decimal SolveBallAndSocketJointSystem::BETA = decimal(0.2);
// Constructor // Constructor
SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(RigidBodyComponents& rigidBodyComponents, SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents, TransformComponents& transformComponents,
JointComponents& jointComponents, JointComponents& jointComponents,
BallAndSocketJointComponents& ballAndSocketJointComponents) BallAndSocketJointComponents& ballAndSocketJointComponents)
:mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), :mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents), mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents),
mTimeStep(0), mIsWarmStartingActive(true) { mTimeStep(0), mIsWarmStartingActive(true) {
@ -55,13 +56,9 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
const RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
const RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mBallAndSocketJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld(); mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mBallAndSocketJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld(); mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
} }
// For each joint // For each joint
@ -260,13 +257,9 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
const RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
const RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mBallAndSocketJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld(); mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mBallAndSocketJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld(); mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
} }
// For each joint component // For each joint component

8
src/systems/SolveBallAndSocketJointSystem.h
View File

@ -35,6 +35,9 @@
namespace reactphysics3d { namespace reactphysics3d {
// Forward declarations
class DynamicsWorld;
// Class SolveBallAndSocketJointSystem // Class SolveBallAndSocketJointSystem
/** /**
* This class is responsible to solve the BallAndSocketJoint constraints * This class is responsible to solve the BallAndSocketJoint constraints
@ -50,6 +53,9 @@ class SolveBallAndSocketJointSystem {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Physics world
DynamicsWorld& mWorld;
/// Reference to the rigid body components /// Reference to the rigid body components
RigidBodyComponents& mRigidBodyComponents; RigidBodyComponents& mRigidBodyComponents;
@ -79,7 +85,7 @@ class SolveBallAndSocketJointSystem {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
SolveBallAndSocketJointSystem(RigidBodyComponents& rigidBodyComponents, SolveBallAndSocketJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents, TransformComponents& transformComponents,
JointComponents& jointComponents, JointComponents& jointComponents,
BallAndSocketJointComponents& ballAndSocketJointComponents); BallAndSocketJointComponents& ballAndSocketJointComponents);

34
src/systems/SolveFixedJointSystem.cpp
View File

@ -25,6 +25,7 @@
// Libraries // Libraries
#include "systems/SolveFixedJointSystem.h" #include "systems/SolveFixedJointSystem.h"
#include "engine/DynamicsWorld.h"
#include "body/RigidBody.h" #include "body/RigidBody.h"
using namespace reactphysics3d; using namespace reactphysics3d;
@ -33,11 +34,11 @@ using namespace reactphysics3d;
const decimal SolveFixedJointSystem::BETA = decimal(0.2); const decimal SolveFixedJointSystem::BETA = decimal(0.2);
// Constructor // Constructor
SolveFixedJointSystem::SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents, SolveFixedJointSystem::SolveFixedJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents, TransformComponents& transformComponents,
JointComponents& jointComponents, JointComponents& jointComponents,
FixedJointComponents& fixedJointComponents) FixedJointComponents& fixedJointComponents)
:mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), :mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
mJointComponents(jointComponents), mFixedJointComponents(fixedJointComponents), mJointComponents(jointComponents), mFixedJointComponents(fixedJointComponents),
mTimeStep(0), mIsWarmStartingActive(true) { mTimeStep(0), mIsWarmStartingActive(true) {
@ -55,13 +56,9 @@ void SolveFixedJointSystem::initBeforeSolve() {
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
// Get the inertia tensor of bodies // Get the inertia tensor of bodies
mFixedJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld(); mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mFixedJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld(); mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
} }
// For each joint // For each joint
@ -346,13 +343,9 @@ void SolveFixedJointSystem::solvePositionConstraint() {
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
const RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
const RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
// Recompute the inverse inertia tensors // Recompute the inverse inertia tensors
mFixedJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld(); mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mFixedJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld(); mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
} }
// For each joint // For each joint
@ -439,10 +432,10 @@ void SolveFixedJointSystem::solvePositionConstraint() {
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
Vector3 x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3 x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
Quaternion q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
Quaternion q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
// Compute position error for the 3 translation constraints // Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + r2World - x1 - r1World; const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
@ -536,10 +529,5 @@ void SolveFixedJointSystem::solvePositionConstraint() {
// Update the body position/orientation of body 2 // Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize(); q2.normalize();
mRigidBodyComponents.mConstrainedPositions[componentIndexBody1] = x1;
mRigidBodyComponents.mConstrainedPositions[componentIndexBody2] = x2;
mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1] = q1;
mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2] = q2;
} }
} }

7
src/systems/SolveFixedJointSystem.h
View File

@ -35,6 +35,8 @@
namespace reactphysics3d { namespace reactphysics3d {
class DynamicsWorld;
// Class SolveFixedJointSystem // Class SolveFixedJointSystem
/** /**
* This class is responsible to solve the FixedJoint constraints * This class is responsible to solve the FixedJoint constraints
@ -50,6 +52,9 @@ class SolveFixedJointSystem {
// -------------------- Attributes -------------------- // // -------------------- Attributes -------------------- //
/// Physics world
DynamicsWorld& mWorld;
/// Reference to the rigid body components /// Reference to the rigid body components
RigidBodyComponents& mRigidBodyComponents; RigidBodyComponents& mRigidBodyComponents;
@ -79,7 +84,7 @@ class SolveFixedJointSystem {
// -------------------- Methods -------------------- // // -------------------- Methods -------------------- //
/// Constructor /// Constructor
SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, SolveFixedJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
JointComponents& jointComponents, FixedJointComponents& fixedJointComponents); JointComponents& jointComponents, FixedJointComponents& fixedJointComponents);
/// Destructor /// Destructor

902
src/systems/SolveHingeJointSystem.cpp Normal file
View File

@ -0,0 +1,902 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "systems/SolveHingeJointSystem.h"
#include "engine/DynamicsWorld.h"
#include "body/RigidBody.h"
using namespace reactphysics3d;
// Static variables definition
const decimal SolveHingeJointSystem::BETA = decimal(0.2);
// Constructor
SolveHingeJointSystem::SolveHingeJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents,
JointComponents& jointComponents,
HingeJointComponents& hingeJointComponents)
:mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
mJointComponents(jointComponents), mHingeJointComponents(hingeJointComponents),
mTimeStep(0), mIsWarmStartingActive(true) {
}
// Initialize before solving the constraint
void SolveHingeJointSystem::initBeforeSolve() {
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Get the inertia tensor of bodies
mHingeJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mHingeJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
}
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
// Compute the vector from body center to the anchor point in world-space
mHingeJointComponents.mR1World[i] = orientationBody1 * mHingeJointComponents.mLocalAnchorPointBody1[i];
mHingeJointComponents.mR2World[i] = orientationBody2 * mHingeJointComponents.mLocalAnchorPointBody2[i];
}
const decimal biasFactor = (BETA / mTimeStep);
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
// Compute vectors needed in the Jacobian
Vector3& a1 = mHingeJointComponents.mA1[i];
a1 = orientationBody1 * mHingeJointComponents.mHingeLocalAxisBody1[i];
Vector3 a2 = orientationBody2 * mHingeJointComponents.mHingeLocalAxisBody2[i];
a1.normalize();
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
mHingeJointComponents.mB2CrossA1[i] = b2.cross(a1);
mHingeJointComponents.mC2CrossA1[i] = c2.cross(a1);
// Compute the bias "b" of the rotation constraints
mHingeJointComponents.mBiasRotation[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mHingeJointComponents.mBiasRotation[i] = biasFactor * Vector2(a1.dot(b2), a1.dot(c2));
}
}
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation constraints (3x3 matrix)
decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
Matrix3x3& inverseMassMatrixTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i];
inverseMassMatrixTranslation.setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
}
}
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Get the bodies positions and orientations
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity);
const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity);
// Compute the bias "b" of the translation constraints
mHingeJointComponents.mBiasTranslation[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mHingeJointComponents.mBiasTranslation[i] = biasFactor * (x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i]);
}
}
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Matrix3x3& i1 = mHingeJointComponents.mI1[i];
const Matrix3x3& i2 = mHingeJointComponents.mI2[i];
const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
const Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 i1B2CrossA1 = i1 * b2CrossA1;
Vector3 i1C2CrossA1 = i1 * c2CrossA1;
Vector3 i2B2CrossA1 = i2 * b2CrossA1;
Vector3 i2C2CrossA1 = i2 * c2CrossA1;
const decimal el11 = b2CrossA1.dot(i1B2CrossA1) + b2CrossA1.dot(i2B2CrossA1);
const decimal el12 = b2CrossA1.dot(i1C2CrossA1) + b2CrossA1.dot(i2C2CrossA1);
const decimal el21 = c2CrossA1.dot(i1B2CrossA1) + c2CrossA1.dot(i2B2CrossA1);
const decimal el22 = c2CrossA1.dot(i1C2CrossA1) + c2CrossA1.dot(i2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero();
if (mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse();
}
}
// If warm-starting is not enabled
if (!mIsWarmStartingActive) {
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
// Reset all the accumulated impulses
mHingeJointComponents.mImpulseTranslation[i].setToZero();
mHingeJointComponents.mImpulseRotation[i].setToZero();
mHingeJointComponents.mImpulseLowerLimit[i] = decimal(0.0);
mHingeJointComponents.mImpulseUpperLimit[i] = decimal(0.0);
mHingeJointComponents.mImpulseMotor[i] = decimal(0.0);
}
}
// For each joint
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
// Compute the current angle around the hinge axis
decimal hingeAngle = computeCurrentHingeAngle(jointEntity, orientationBody1, orientationBody2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mHingeJointComponents.mLowerLimit[i];
decimal upperLimitError = mHingeJointComponents.mUpperLimit[i] - hingeAngle;
bool oldIsLowerLimitViolated = mHingeJointComponents.mIsLowerLimitViolated[i];
bool isLowerLimitViolated = lowerLimitError <= 0;
mHingeJointComponents.mIsLowerLimitViolated[i] = isLowerLimitViolated;
if (isLowerLimitViolated != oldIsLowerLimitViolated) {
mHingeJointComponents.mImpulseLowerLimit[i] = decimal(0.0);
}
bool oldIsUpperLimitViolated = mHingeJointComponents.mIsUpperLimitViolated[i];
bool isUpperLimitViolated = upperLimitError <= 0;
mHingeJointComponents.mIsUpperLimitViolated[i] = isUpperLimitViolated;
if (isUpperLimitViolated != oldIsUpperLimitViolated) {
mHingeJointComponents.mImpulseUpperLimit[i] = decimal(0.0);
}
// If the motor or limits are enabled
if (mHingeJointComponents.mIsMotorEnabled[i] ||
(mHingeJointComponents.mIsLimitEnabled[i] && (mHingeJointComponents.mIsLowerLimitViolated[i] ||
mHingeJointComponents.mIsUpperLimitViolated[i]))) {
Vector3& a1 = mHingeJointComponents.mA1[i];
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
decimal inverseMassMatrixLimitMotor = a1.dot(mHingeJointComponents.mI1[i] * a1) + a1.dot(mHingeJointComponents.mI2[i] * a1);
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = inverseMassMatrixLimitMotor;
if (mHingeJointComponents.mIsLimitEnabled[i]) {
// Compute the bias "b" of the lower limit constraint
mHingeJointComponents.mBLowerLimit[i] = decimal(0.0);
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mHingeJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError;
}
// Compute the bias "b" of the upper limit constraint
mHingeJointComponents.mBUpperLimit[i] = decimal(0.0);
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mHingeJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError;
}
}
}
}
}
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void SolveHingeJointSystem::warmstart() {
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
const Vector3& impulseTranslation = mHingeJointComponents.mImpulseTranslation[i];
const Vector2& impulseRotation = mHingeJointComponents.mImpulseRotation[i];
const decimal impulseLowerLimit = mHingeJointComponents.mImpulseLowerLimit[i];
const decimal impulseUpperLimit = mHingeJointComponents.mImpulseUpperLimit[i];
const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
const Vector3& a1 = mHingeJointComponents.mA1[i];
// Compute the impulse P=J^T * lambda for the 2 rotation constraints
Vector3 rotationImpulse = -b2CrossA1 * impulseRotation.x - mHingeJointComponents.mC2CrossA1[i] * impulseRotation.y;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
const Vector3 limitsImpulse = (impulseUpperLimit - impulseLowerLimit) * a1;
// Compute the impulse P=J^T * lambda for the motor constraint
const Vector3 motorImpulse = -mHingeJointComponents.mImpulseMotor[i] * a1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 1
Vector3 linearImpulseBody1 = -impulseTranslation;
Vector3 angularImpulseBody1 = impulseTranslation.cross(mHingeJointComponents.mR1World[i]);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 += rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
angularImpulseBody1 += limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
angularImpulseBody1 += motorImpulse;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += mHingeJointComponents.mI1[i] * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints of body 2
Vector3 angularImpulseBody2 = -impulseTranslation.cross(mHingeJointComponents.mR2World[i]);
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 2
angularImpulseBody2 += -rotationImpulse;
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
angularImpulseBody2 += -limitsImpulse;
// Compute the impulse P=J^T * lambda for the motor constraint of body 2
angularImpulseBody2 += -motorImpulse;
// Apply the impulse to the body 2
v2 += inverseMassBody2 * impulseTranslation;
w2 += mHingeJointComponents.mI2[i] * angularImpulseBody2;
}
}
// Solve the velocity constraint
void SolveHingeJointSystem::solveVelocityConstraint() {
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
const Matrix3x3& i1 = mHingeJointComponents.mI1[i];
const Matrix3x3& i2 = mHingeJointComponents.mI2[i];
const Vector3& r1World = mHingeJointComponents.mR1World[i];
const Vector3& r2World = mHingeJointComponents.mR2World[i];
const Vector3& a1 = mHingeJointComponents.mA1[i];
const decimal inverseMassMatrixLimitMotor = mHingeJointComponents.mInverseMassMatrixLimitMotor[i];
// --------------- Translation Constraints --------------- //
// Compute J*v
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] *
(-JvTranslation - mHingeJointComponents.mBiasTranslation[i]);
mHingeJointComponents.mImpulseTranslation[i] += deltaLambdaTranslation;
// Compute the impulse P=J^T * lambda of body 1
const Vector3 linearImpulseBody1 = -deltaLambdaTranslation;
Vector3 angularImpulseBody1 = deltaLambdaTranslation.cross(r1World);
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * 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;
// --------------- Rotation Constraints --------------- //
const Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
const Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
// Compute J*v for the 2 rotation constraints
const Vector2 JvRotation(-b2CrossA1.dot(w1) + b2CrossA1.dot(w2),
-c2CrossA1.dot(w1) + c2CrossA1.dot(w2));
// Compute the Lagrange multiplier lambda for the 2 rotation constraints
Vector2 deltaLambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] *
(-JvRotation - mHingeJointComponents.mBiasRotation[i]);
mHingeJointComponents.mImpulseRotation[i] += deltaLambdaRotation;
// Compute the impulse P=J^T * lambda for the 2 rotation constraints of body 1
angularImpulseBody1 = -b2CrossA1 * deltaLambdaRotation.x - c2CrossA1 * deltaLambdaRotation.y;
// Apply the impulse to the body 1
w1 += i1 * 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;
// --------------- Limits Constraints --------------- //
if (mHingeJointComponents.mIsLimitEnabled[i]) {
// If the lower limit is violated
if (mHingeJointComponents.mIsLowerLimitViolated[i]) {
// Compute J*v for the lower limit constraint
const decimal JvLowerLimit = (w2 - w1).dot(a1);
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal deltaLambdaLower = inverseMassMatrixLimitMotor * (-JvLowerLimit -mHingeJointComponents.mBLowerLimit[i]);
decimal lambdaTemp = mHingeJointComponents.mImpulseLowerLimit[i];
mHingeJointComponents.mImpulseLowerLimit[i] = std::max(mHingeJointComponents.mImpulseLowerLimit[i] + deltaLambdaLower, decimal(0.0));
deltaLambdaLower = mHingeJointComponents.mImpulseLowerLimit[i] - lambdaTemp;
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
const Vector3 angularImpulseBody1 = -deltaLambdaLower * a1;
// Apply the impulse to the body 1
w1 += i1 * 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;
}
// If the upper limit is violated
if (mHingeJointComponents.mIsUpperLimitViolated[i]) {
// Compute J*v for the upper limit constraint
const decimal JvUpperLimit = -(w2 - w1).dot(a1);
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal deltaLambdaUpper = inverseMassMatrixLimitMotor * (-JvUpperLimit -mHingeJointComponents.mBUpperLimit[i]);
decimal lambdaTemp = mHingeJointComponents.mImpulseUpperLimit[i];
mHingeJointComponents.mImpulseUpperLimit[i] = std::max(mHingeJointComponents.mImpulseUpperLimit[i] + deltaLambdaUpper, decimal(0.0));
deltaLambdaUpper = mHingeJointComponents.mImpulseUpperLimit[i] - lambdaTemp;
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
const Vector3 angularImpulseBody1 = deltaLambdaUpper * a1;
// Apply the impulse to the body 1
w1 += i1 * 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;
}
}
// --------------- Motor --------------- //
// If the motor is enabled
if (mHingeJointComponents.mIsMotorEnabled[i]) {
// Compute J*v for the motor
const decimal JvMotor = a1.dot(w1 - w2);
// Compute the Lagrange multiplier lambda for the motor
const decimal maxMotorImpulse = mHingeJointComponents.mMaxMotorTorque[i] * mTimeStep;
decimal deltaLambdaMotor = mHingeJointComponents.mInverseMassMatrixLimitMotor[i] * (-JvMotor - mHingeJointComponents.mMotorSpeed[i]);
decimal lambdaTemp = mHingeJointComponents.mImpulseMotor[i];
mHingeJointComponents.mImpulseMotor[i] = clamp(mHingeJointComponents.mImpulseMotor[i] + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
deltaLambdaMotor = mHingeJointComponents.mImpulseMotor[i] - lambdaTemp;
// Compute the impulse P=J^T * lambda for the motor of body 1
const Vector3 angularImpulseBody1 = -deltaLambdaMotor * a1;
// Apply the impulse to the body 1
w1 += i1 * 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;
}
}
}
// Solve the position constraint (for position error correction)
void SolveHingeJointSystem::solvePositionConstraint() {
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Recompute the inverse inertia tensors
mHingeJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
mHingeJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
}
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
// Compute the vector from body center to the anchor point in world-space
mHingeJointComponents.mR1World[i] = q1 * mHingeJointComponents.mLocalAnchorPointBody1[i];
mHingeJointComponents.mR2World[i] = q2 * mHingeJointComponents.mLocalAnchorPointBody2[i];
}
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]);
Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR2World[i]);
// --------------- Translation Constraints --------------- //
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 body1InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
const decimal body2InverseMass = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
decimal inverseMassBodies = body1InverseMass + body2InverseMass;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mHingeJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mHingeJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
mHingeJointComponents.mInverseMassMatrixTranslation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
}
}
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
Vector3& b2CrossA1 = mHingeJointComponents.mB2CrossA1[i];
Vector3& c2CrossA1 = mHingeJointComponents.mC2CrossA1[i];
Vector3& a1 = mHingeJointComponents.mA1[i];
// Compute vectors needed in the Jacobian
a1 = q1 * mHingeJointComponents.mHingeLocalAxisBody1[i];
Vector3 a2 = q2 * mHingeJointComponents.mHingeLocalAxisBody2[i];
a1.normalize();
mHingeJointComponents.mA1[i] = a1;
a2.normalize();
const Vector3 b2 = a2.getOneUnitOrthogonalVector();
const Vector3 c2 = a2.cross(b2);
b2CrossA1 = b2.cross(a1);
mHingeJointComponents.mB2CrossA1[i] = b2CrossA1;
c2CrossA1 = c2.cross(a1);
mHingeJointComponents.mC2CrossA1[i] = c2CrossA1;
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + mHingeJointComponents.mR2World[i] - x1 - mHingeJointComponents.mR1World[i];
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mHingeJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(mHingeJointComponents.mR1World[i]);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
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;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation constraints (2x2 matrix)
Vector3 I1B2CrossA1 = mHingeJointComponents.mI1[i] * b2CrossA1;
Vector3 I1C2CrossA1 = mHingeJointComponents.mI1[i] * c2CrossA1;
Vector3 I2B2CrossA1 = mHingeJointComponents.mI2[i] * b2CrossA1;
Vector3 I2C2CrossA1 = mHingeJointComponents.mI2[i] * c2CrossA1;
const decimal el11 = b2CrossA1.dot(I1B2CrossA1) +
b2CrossA1.dot(I2B2CrossA1);
const decimal el12 = b2CrossA1.dot(I1C2CrossA1) +
b2CrossA1.dot(I2C2CrossA1);
const decimal el21 = c2CrossA1.dot(I1B2CrossA1) +
c2CrossA1.dot(I2B2CrossA1);
const decimal el22 = c2CrossA1.dot(I1C2CrossA1) +
c2CrossA1.dot(I2C2CrossA1);
const Matrix2x2 matrixKRotation(el11, el12, el21, el22);
mHingeJointComponents.mInverseMassMatrixRotation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mHingeJointComponents.mInverseMassMatrixRotation[i] = matrixKRotation.getInverse();
}
// Compute the position error for the 3 rotation constraints
const Vector2 errorRotation = Vector2(a1.dot(b2), a1.dot(c2));
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector2 lambdaRotation = mHingeJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -b2CrossA1 * lambdaRotation.x - c2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 1
w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
angularImpulseBody2 = b2CrossA1 * lambdaRotation.x + c2CrossA1 * lambdaRotation.y;
// Compute the pseudo velocity of body 2
w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// For each joint component
for (uint32 i=0; i < mHingeJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mHingeJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
// Compute the current angle around the hinge axis
const decimal hingeAngle = computeCurrentHingeAngle(jointEntity, q1, q2);
// Check if the limit constraints are violated or not
decimal lowerLimitError = hingeAngle - mHingeJointComponents.mLowerLimit[i];
decimal upperLimitError = mHingeJointComponents.mUpperLimit[i] - hingeAngle;
mHingeJointComponents.mIsLowerLimitViolated[i] = lowerLimitError <= 0;
mHingeJointComponents.mIsUpperLimitViolated[i] = upperLimitError <= 0;
// --------------- Limits Constraints --------------- //
if (mHingeJointComponents.mIsLimitEnabled[i]) {
decimal inverseMassMatrixLimitMotor = mHingeJointComponents.mInverseMassMatrixLimitMotor[i];
Vector3& a1 = mHingeJointComponents.mA1[i];
if (mHingeJointComponents.mIsLowerLimitViolated[i] || mHingeJointComponents.mIsUpperLimitViolated[i]) {
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = a1.dot(mHingeJointComponents.mI1[i] * a1) + a1.dot(mHingeJointComponents.mI2[i] * a1);
mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
decimal(1.0) / mHingeJointComponents.mInverseMassMatrixLimitMotor[i] : decimal(0.0);
}
// If the lower limit is violated
if (mHingeJointComponents.mIsLowerLimitViolated[i]) {
// Compute the Lagrange multiplier lambda for the lower limit constraint
decimal lambdaLowerLimit = inverseMassMatrixLimitMotor * (-lowerLimitError );
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * a1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse P=J^T * lambda of body 2
const Vector3 angularImpulseBody2 = lambdaLowerLimit * a1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
// If the upper limit is violated
if (mHingeJointComponents.mIsUpperLimitViolated[i]) {
// Compute the Lagrange multiplier lambda for the upper limit constraint
decimal lambdaUpperLimit = inverseMassMatrixLimitMotor * (-upperLimitError);
// Compute the impulse P=J^T * lambda of body 1
const Vector3 angularImpulseBody1 = lambdaUpperLimit * a1;
// Compute the pseudo velocity of body 1
const Vector3 w1 = mHingeJointComponents.mI1[i] * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse P=J^T * lambda of body 2
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * a1;
// Compute the pseudo velocity of body 2
const Vector3 w2 = mHingeJointComponents.mI2[i] * angularImpulseBody2;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
}
}
}
}
// Given an angle in radian, this method returns the corresponding angle in the range [-pi; pi]
decimal SolveHingeJointSystem::computeNormalizedAngle(decimal angle) const {
// Convert it into the range [-2*pi; 2*pi]
angle = std::fmod(angle, PI_TIMES_2);
// Convert it into the range [-pi; pi]
if (angle < -PI) {
return angle + PI_TIMES_2;
}
else if (angle > PI) {
return angle - PI_TIMES_2;
}
else {
return angle;
}
}
// Given an "inputAngle" in the range [-pi, pi], this method returns an
// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
// two angle limits in arguments.
decimal SolveHingeJointSystem::computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle, decimal upperLimitAngle) const {
if (upperLimitAngle <= lowerLimitAngle) {
return inputAngle;
}
else if (inputAngle > upperLimitAngle) {
decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(inputAngle - upperLimitAngle));
decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - PI_TIMES_2) : inputAngle;
}
else if (inputAngle < lowerLimitAngle) {
decimal diffToUpperLimit = std::fabs(computeNormalizedAngle(upperLimitAngle - inputAngle));
decimal diffToLowerLimit = std::fabs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + PI_TIMES_2);
}
else {
return inputAngle;
}
}
// Compute the current angle around the hinge axis
decimal SolveHingeJointSystem::computeCurrentHingeAngle(Entity jointEntity, const Quaternion& orientationBody1, const Quaternion& orientationBody2) {
decimal hingeAngle;
// Compute the current orientation difference between the two bodies
Quaternion currentOrientationDiff = orientationBody2 * orientationBody1.getInverse();
currentOrientationDiff.normalize();
// Compute the relative rotation considering the initial orientation difference
Quaternion relativeRotation = currentOrientationDiff * mHingeJointComponents.getInitOrientationDifferenceInv(jointEntity);
relativeRotation.normalize();
// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
// length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with :
// |sin(theta/2)| = q.getVectorV().length() since rotAxis is unit length. Note that any
// rotation can be represented by a quaternion q and -q. Therefore, if the relative rotation
// axis is not pointing in the same direction as the hinge axis, we use the rotation -q which
// has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details
// about this trick is explained in the source code of OpenTissue (http://www.opentissue.org).
decimal cosHalfAngle = relativeRotation.w;
decimal sinHalfAngleAbs = relativeRotation.getVectorV().length();
// Compute the dot product of the relative rotation axis and the hinge axis
decimal dotProduct = relativeRotation.getVectorV().dot(mHingeJointComponents.getA1(jointEntity));
// If the relative rotation axis and the hinge axis are pointing the same direction
if (dotProduct >= decimal(0.0)) {
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, cosHalfAngle);
}
else {
hingeAngle = decimal(2.0) * std::atan2(sinHalfAngleAbs, -cosHalfAngle);
}
// Convert the angle from range [-2*pi; 2*pi] into the range [-pi; pi]
hingeAngle = computeNormalizedAngle(hingeAngle);
// Compute and return the corresponding angle near one the two limits
return computeCorrespondingAngleNearLimits(hingeAngle,
mHingeJointComponents.getLowerLimit(jointEntity),
mHingeJointComponents.getUpperLimit(jointEntity));
}

160
src/systems/SolveHingeJointSystem.h Normal file
View File

@ -0,0 +1,160 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SOLVE_HINGE_JOINT_SYSTEM_H
#define REACTPHYSICS3D_SOLVE_HINGE_JOINT_SYSTEM_H
// Libraries
#include "utils/Profiler.h"
#include "components/RigidBodyComponents.h"
#include "components/JointComponents.h"
#include "components/HingeJointComponents.h"
#include "components/TransformComponents.h"
namespace reactphysics3d {
class DynamicsWorld;
// Class SolveHingeJointSystem
/**
* This class is responsible to solve the BallAndSocketJoint constraints
*/
class SolveHingeJointSystem {
private :
// -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
// -------------------- Attributes -------------------- //
/// Physics world
DynamicsWorld& mWorld;
/// Reference to the rigid body components
RigidBodyComponents& mRigidBodyComponents;
/// Reference to transform components
TransformComponents& mTransformComponents;
/// Reference to the joint components
JointComponents& mJointComponents;
/// Reference to the hinge joint components
HingeJointComponents& mHingeJointComponents;
/// Current time step of the simulation
decimal mTimeStep;
/// True if warm starting of the solver is active
bool mIsWarmStartingActive;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Given an angle in radian, this method returns the corresponding
/// angle in the range [-pi; pi]
decimal computeNormalizedAngle(decimal angle) const;
/// Given an "inputAngle" in the range [-pi, pi], this method returns an
/// angle (modulo 2*pi) in the range [-2*pi; 2*pi] that is closest to one of the
/// two angle limits in arguments.
decimal computeCorrespondingAngleNearLimits(decimal inputAngle, decimal lowerLimitAngle,
decimal upperLimitAngle) const;
/// Compute the current angle around the hinge axis
decimal computeCurrentHingeAngle(Entity jointEntity, const Quaternion& orientationBody1,
const Quaternion& orientationBody2);
public :
// -------------------- Methods -------------------- //
/// Constructor
SolveHingeJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents,
JointComponents& jointComponents,
HingeJointComponents& hingeJointComponents);
/// Destructor
~SolveHingeJointSystem() = default;
/// Initialize before solving the constraint
void initBeforeSolve();
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
void warmstart();
/// Solve the velocity constraint
void solveVelocityConstraint();
/// Solve the position constraint (for position error correction)
void solvePositionConstraint();
/// Set the time step
void setTimeStep(decimal timeStep);
/// Set to true to enable warm starting
void setIsWarmStartingActive(bool isWarmStartingActive);
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void SolveHingeJointSystem::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
// Set the time step
inline void SolveHingeJointSystem::setTimeStep(decimal timeStep) {
assert(timeStep > decimal(0.0));
mTimeStep = timeStep;
}
// Set to true to enable warm starting
inline void SolveHingeJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) {
mIsWarmStartingActive = isWarmStartingActive;
}
#endif
}
#endif