From afb34b4355a62972623e3eafef25e9898f2ddcc1 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 5 Oct 2019 17:45:35 +0200 Subject: [PATCH] Add SolveHingeJointSystem class --- CMakeLists.txt | 2 + src/body/RigidBody.cpp | 44 +- src/body/RigidBody.h | 12 +- src/components/HingeJointComponents.h | 1 + src/components/RigidBodyComponents.cpp | 12 +- src/components/RigidBodyComponents.h | 34 +- src/constraint/HingeJoint.cpp | 715 -------------- src/constraint/HingeJoint.h | 17 - src/constraint/SliderJoint.cpp | 8 +- src/engine/DynamicsWorld.cpp | 11 +- src/systems/ConstraintSolverSystem.cpp | 27 +- src/systems/ConstraintSolverSystem.h | 11 +- src/systems/ContactSolverSystem.cpp | 8 +- src/systems/ContactSolverSystem.h | 5 +- src/systems/DynamicsSystem.cpp | 16 +- src/systems/DynamicsSystem.h | 7 +- src/systems/SolveBallAndSocketJointSystem.cpp | 21 +- src/systems/SolveBallAndSocketJointSystem.h | 8 +- src/systems/SolveFixedJointSystem.cpp | 38 +- src/systems/SolveFixedJointSystem.h | 7 +- src/systems/SolveHingeJointSystem.cpp | 902 ++++++++++++++++++ src/systems/SolveHingeJointSystem.h | 160 ++++ 22 files changed, 1178 insertions(+), 888 deletions(-) create mode 100644 src/systems/SolveHingeJointSystem.cpp create mode 100644 src/systems/SolveHingeJointSystem.h diff --git a/CMakeLists.txt b/CMakeLists.txt index d0c969aa..378862c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,6 +134,7 @@ SET (REACTPHYSICS3D_HEADERS "src/systems/CollisionDetectionSystem.h" "src/systems/SolveBallAndSocketJointSystem.h" "src/systems/SolveFixedJointSystem.h" + "src/systems/SolveHingeJointSystem.h" "src/engine/DynamicsWorld.h" "src/engine/EventListener.h" "src/engine/Island.h" @@ -232,6 +233,7 @@ SET (REACTPHYSICS3D_SOURCES "src/systems/CollisionDetectionSystem.cpp" "src/systems/SolveBallAndSocketJointSystem.cpp" "src/systems/SolveFixedJointSystem.cpp" + "src/systems/SolveHingeJointSystem.cpp" "src/engine/DynamicsWorld.cpp" "src/engine/Island.cpp" "src/engine/Material.cpp" diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 74a1ccb7..93979cbc 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -86,7 +86,6 @@ void RigidBody::setType(BodyType type) { // Reset the inverse mass and inverse inertia tensor to zero mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0)); mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); - mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero()); } else { // If it is a dynamic body 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 setIsSleeping(false); @@ -130,10 +126,9 @@ const Matrix3x3& RigidBody::getInverseInertiaTensorLocal() const { * @return The 3x3 inverse inertia tensor matrix of the body in world-space * coordinates */ -Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { +const Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { - // Compute and return the inertia tensor in world coordinates - return mWorld.mRigidBodyComponents.getInertiaTensorWorldInverse(mEntity); + return getInertiaTensorInverseWorld(mWorld, mEntity); } // Method that return the mass of the body @@ -192,9 +187,6 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) { // Compute the inverse local inertia tensor mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); - // Update the world inverse inertia tensor - updateInertiaTensorInverseWorld(); - RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "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 mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, mUserInertiaTensorLocalInverse); - // Update the world inverse inertia tensor - updateInertiaTensorInverseWorld(); - RP3D_LOG(mLogger, Logger::Level::Information, Logger::Category::Body, "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)); } -// 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. /// 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 mWorld.mTransformComponents.setTransform(mEntity, transform); - // Update the world inverse inertia tensor - updateInertiaTensorInverseWorld(); - // Awake the body if it is sleeping setIsSleeping(false); } @@ -569,7 +541,6 @@ void RigidBody::recomputeMassInformation() { mWorld.mRigidBodyComponents.setInitMass(mEntity, decimal(0.0)); mWorld.mRigidBodyComponents.setMassInverse(mEntity, decimal(0.0)); if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, Matrix3x3::zero()); - if (!mIsInertiaTensorSetByUser) mWorld.mRigidBodyComponents.setInverseInertiaTensorWorld(mEntity, Matrix3x3::zero()); if (!mIsCenterOfMassSetByUser) mWorld.mRigidBodyComponents.setCenterOfMassLocal(mEntity, Vector3::zero()); Matrix3x3 inertiaTensorLocal; inertiaTensorLocal.setToZero(); @@ -651,9 +622,6 @@ void RigidBody::recomputeMassInformation() { mWorld.mRigidBodyComponents.setInverseInertiaTensorLocal(mEntity, inertiaTensorLocal.getInverse()); } - // Update the world inverse inertia tensor - updateInertiaTensorInverseWorld(); - // Update the linear velocity of the center of mass Vector3 linearVelocity = mWorld.mRigidBodyComponents.getLinearVelocity(mEntity); Vector3 angularVelocity = mWorld.mRigidBodyComponents.getAngularVelocity(mEntity); @@ -748,6 +716,14 @@ void RigidBody::setIsSleeping(bool isSleeping) { (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 /** * @param isAllowedToSleep True if the body is allowed to sleep diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 3d1b5a6c..2b0f7591 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -73,12 +73,12 @@ class RigidBody : public CollisionBody { /// Update the transform of the body after a change of the center of mass 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 void setIsSleeping(bool isSleeping); + /// Return the inverse of the inertia tensor in world coordinates. + static const Matrix3x3 getInertiaTensorInverseWorld(CollisionWorld& world, Entity bodyEntity); + public : // -------------------- Methods -------------------- // @@ -123,7 +123,7 @@ class RigidBody : public CollisionBody { const Matrix3x3& getInverseInertiaTensorLocal() const; /// 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) void setCenterOfMassLocal(const Vector3& centerOfMassLocal); @@ -215,11 +215,15 @@ class RigidBody : public CollisionBody { friend class DynamicsWorld; friend class ContactSolverSystem; + friend class DynamicsSystem; friend class Joint; friend class BallAndSocketJoint; friend class SliderJoint; friend class HingeJoint; friend class FixedJoint; + friend class SolveBallAndSocketJointSystem; + friend class SolveFixedJointSystem; + friend class SolveHingeJointSystem; }; // Return a reference to the material properties of the rigid body diff --git a/src/components/HingeJointComponents.h b/src/components/HingeJointComponents.h index 20c5acd3..6e0bba85 100644 --- a/src/components/HingeJointComponents.h +++ b/src/components/HingeJointComponents.h @@ -411,6 +411,7 @@ class HingeJointComponents : public Components { // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; + friend class SolveHingeJointSystem; }; // Return a pointer to a given joint diff --git a/src/components/RigidBodyComponents.cpp b/src/components/RigidBodyComponents.cpp index a748f93e..ae15df20 100644 --- a/src/components/RigidBodyComponents.cpp +++ b/src/components/RigidBodyComponents.cpp @@ -40,7 +40,7 @@ RigidBodyComponents::RigidBodyComponents(MemoryAllocator& allocator) sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(Matrix3x3) + - sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Quaternion) + sizeof(Vector3) + sizeof(Vector3) + sizeof(bool) + sizeof(bool) + sizeof(List)) { @@ -77,8 +77,7 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { decimal* newInitMasses = reinterpret_cast(newAngularDampings + nbComponentsToAllocate); decimal* newInverseMasses = reinterpret_cast(newInitMasses + nbComponentsToAllocate); Matrix3x3* newInertiaTensorLocalInverses = reinterpret_cast(newInverseMasses + nbComponentsToAllocate); - Matrix3x3* newInertiaTensorWorldInverses = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); - Vector3* newConstrainedLinearVelocities = reinterpret_cast(newInertiaTensorWorldInverses + nbComponentsToAllocate); + Vector3* newConstrainedLinearVelocities = reinterpret_cast(newInertiaTensorLocalInverses + nbComponentsToAllocate); Vector3* newConstrainedAngularVelocities = reinterpret_cast(newConstrainedLinearVelocities + nbComponentsToAllocate); Vector3* newSplitLinearVelocities = reinterpret_cast(newConstrainedAngularVelocities + nbComponentsToAllocate); Vector3* newSplitAngularVelocities = reinterpret_cast(newSplitLinearVelocities + nbComponentsToAllocate); @@ -109,7 +108,6 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newInitMasses, mInitMasses, mNbComponents * sizeof(decimal)); memcpy(newInverseMasses, mInverseMasses, mNbComponents * sizeof(decimal)); memcpy(newInertiaTensorLocalInverses, mInverseInertiaTensorsLocal, mNbComponents * sizeof(Matrix3x3)); - memcpy(newInertiaTensorWorldInverses, mInverseInertiaTensorsWorld, mNbComponents * sizeof(Matrix3x3)); memcpy(newConstrainedLinearVelocities, mConstrainedLinearVelocities, mNbComponents * sizeof(Vector3)); memcpy(newConstrainedAngularVelocities, mConstrainedAngularVelocities, mNbComponents * sizeof(Vector3)); memcpy(newSplitLinearVelocities, mSplitLinearVelocities, mNbComponents * sizeof(Vector3)); @@ -143,7 +141,6 @@ void RigidBodyComponents::allocate(uint32 nbComponentsToAllocate) { mInitMasses = newInitMasses; mInverseMasses = newInverseMasses; mInverseInertiaTensorsLocal = newInertiaTensorLocalInverses; - mInverseInertiaTensorsWorld = newInertiaTensorWorldInverses; mConstrainedLinearVelocities = newConstrainedLinearVelocities; mConstrainedAngularVelocities = newConstrainedAngularVelocities; mSplitLinearVelocities = newSplitLinearVelocities; @@ -179,7 +176,6 @@ void RigidBodyComponents::addComponent(Entity bodyEntity, bool isSleeping, const mInitMasses[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 (mInverseInertiaTensorsWorld + index) Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0); new (mConstrainedLinearVelocities + index) Vector3(0, 0, 0); new (mConstrainedAngularVelocities + index) Vector3(0, 0, 0); new (mSplitLinearVelocities + index) Vector3(0, 0, 0); @@ -223,7 +219,6 @@ void RigidBodyComponents::moveComponentToIndex(uint32 srcIndex, uint32 destIndex mInitMasses[destIndex] = mInitMasses[srcIndex]; mInverseMasses[destIndex] = mInverseMasses[srcIndex]; new (mInverseInertiaTensorsLocal + destIndex) Matrix3x3(mInverseInertiaTensorsLocal[srcIndex]); - new (mInverseInertiaTensorsWorld + destIndex) Matrix3x3(mInverseInertiaTensorsWorld[srcIndex]); new (mConstrainedLinearVelocities + destIndex) Vector3(mConstrainedLinearVelocities[srcIndex]); new (mConstrainedAngularVelocities + destIndex) Vector3(mConstrainedAngularVelocities[srcIndex]); new (mSplitLinearVelocities + destIndex) Vector3(mSplitLinearVelocities[srcIndex]); @@ -266,7 +261,6 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { decimal initMass1 = mInitMasses[index1]; decimal inverseMass1 = mInverseMasses[index1]; Matrix3x3 inertiaTensorLocalInverse1 = mInverseInertiaTensorsLocal[index1]; - Matrix3x3 inertiaTensorWorldInverse1 = mInverseInertiaTensorsWorld[index1]; Vector3 constrainedLinearVelocity1(mConstrainedLinearVelocities[index1]); Vector3 constrainedAngularVelocity1(mConstrainedAngularVelocities[index1]); Vector3 splitLinearVelocity1(mSplitLinearVelocities[index1]); @@ -300,7 +294,6 @@ void RigidBodyComponents::swapComponents(uint32 index1, uint32 index2) { mInitMasses[index2] = initMass1; mInverseMasses[index2] = inverseMass1; mInverseInertiaTensorsLocal[index2] = inertiaTensorLocalInverse1; - mInverseInertiaTensorsWorld[index2] = inertiaTensorWorldInverse1; new (mConstrainedLinearVelocities + index2) Vector3(constrainedLinearVelocity1); new (mConstrainedAngularVelocities + index2) Vector3(constrainedAngularVelocity1); new (mSplitLinearVelocities + index2) Vector3(splitLinearVelocity1); @@ -337,7 +330,6 @@ void RigidBodyComponents::destroyComponent(uint32 index) { mExternalForces[index].~Vector3(); mExternalTorques[index].~Vector3(); mInverseInertiaTensorsLocal[index].~Matrix3x3(); - mInverseInertiaTensorsWorld[index].~Matrix3x3(); mConstrainedLinearVelocities[index].~Vector3(); mConstrainedAngularVelocities[index].~Vector3(); mSplitLinearVelocities[index].~Vector3(); diff --git a/src/components/RigidBodyComponents.h b/src/components/RigidBodyComponents.h index 5a6aa395..8055682e 100644 --- a/src/components/RigidBodyComponents.h +++ b/src/components/RigidBodyComponents.h @@ -109,9 +109,6 @@ class RigidBodyComponents : public Components { /// Array with the inverse of the inertia tensor of each component 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 Vector3* mConstrainedLinearVelocities; @@ -246,9 +243,6 @@ class RigidBodyComponents : public Components { /// Return the inverse local inertia tensor of an entity 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 void setExternalForce(Entity bodyEntity, const Vector3& externalForce); @@ -270,9 +264,6 @@ class RigidBodyComponents : public Components { /// Set the inverse local inertia tensor of an entity 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 const Vector3& getConstrainedLinearVelocity(Entity bodyEntity) const; @@ -286,10 +277,10 @@ class RigidBodyComponents : public Components { const Vector3& getSplitAngularVelocity(Entity bodyEntity) const; /// Return the constrained position of an entity - const Vector3& getConstrainedPosition(Entity bodyEntity); + Vector3& getConstrainedPosition(Entity bodyEntity); /// 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 const Vector3& getCenterOfMassLocal(Entity bodyEntity); @@ -348,6 +339,7 @@ class RigidBodyComponents : public Components { friend class ContactSolverSystem; friend class SolveBallAndSocketJointSystem; friend class SolveFixedJointSystem; + friend class SolveHingeJointSystem; friend class DynamicsSystem; friend class BallAndSocketJoint; friend class FixedJoint; @@ -515,14 +507,6 @@ inline const Matrix3x3& RigidBodyComponents::getInertiaTensorLocalInverse(Entity return mInverseInertiaTensorsLocal[mMapEntityToComponentIndex[bodyEntity]]; } -// Return the inverse world inertia tensor of an entity -inline const Matrix3x3& RigidBodyComponents::getInertiaTensorWorldInverse(Entity bodyEntity) { - - assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); - - return mInverseInertiaTensorsWorld[mMapEntityToComponentIndex[bodyEntity]]; -} - // Set the external force of an entity inline void RigidBodyComponents::setExternalForce(Entity bodyEntity, const Vector3& externalForce) { @@ -579,14 +563,6 @@ inline void RigidBodyComponents::setInverseInertiaTensorLocal(Entity bodyEntity, 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 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 -inline const Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) { +inline Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); @@ -628,7 +604,7 @@ inline const Vector3& RigidBodyComponents::getConstrainedPosition(Entity bodyEnt } // Return the constrained orientation of an entity -inline const Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) { +inline Quaternion& RigidBodyComponents::getConstrainedOrientation(Entity bodyEntity) { assert(mMapEntityToComponentIndex.containsKey(bodyEntity)); diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 546e1310..621375a4 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -31,9 +31,6 @@ using namespace reactphysics3d; -// Static variables definition -const decimal HingeJoint::BETA = decimal(0.2); - // Constructor 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 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(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); - RigidBody* body2 = static_cast(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) 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 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) 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(mWorld.mRigidBodyComponents.getRigidBody(body1Entity)); - RigidBody* body2 = static_cast(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 /** diff --git a/src/constraint/HingeJoint.h b/src/constraint/HingeJoint.h index e16d7a40..706fd6da 100644 --- a/src/constraint/HingeJoint.h +++ b/src/constraint/HingeJoint.h @@ -144,9 +144,6 @@ class HingeJoint : public Joint { // -------------------- Constants -------------------- // - // Beta value for the bias factor of position correction - static const decimal BETA; - // -------------------- Attributes -------------------- // @@ -155,20 +152,6 @@ class HingeJoint : public Joint { /// Reset the limits 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 virtual size_t getSizeInBytes() const override; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index ab1fc812..35402a1e 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -88,8 +88,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation(); // Get the inertia tensor of bodies - mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); - mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); + mWorld.mSliderJointsComponents.setI1(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity)); + mWorld.mSliderJointsComponents.setI2(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity)); // Vector from body center to the anchor point 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); // Recompute the inertia tensor of bodies - mWorld.mSliderJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); - mWorld.mSliderJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); + mWorld.mSliderJointsComponents.setI1(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity)); + mWorld.mSliderJointsComponents.setI2(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity)); // Vector from body center to the anchor point mWorld.mSliderJointsComponents.setR1(mEntity, q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity)); diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index d162dda0..72217c06 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -50,11 +50,11 @@ using namespace std; DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldSettings, Logger* logger, Profiler* profiler) : CollisionWorld(worldSettings, logger, profiler), mIslands(mMemoryManager.getSingleFrameAllocator()), - mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents, + mContactSolverSystem(mMemoryManager, *this, mIslands, mCollisionBodyComponents, mRigidBodyComponents, mProxyShapesComponents, mConfig), - mConstraintSolverSystem(mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents, - mBallAndSocketJointsComponents, mFixedJointsComponents), - mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity), + mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents, + mBallAndSocketJointsComponents, mFixedJointsComponents, mHingeJointsComponents), + mDynamicsSystem(*this, mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), mIsSleepingEnabled(mConfig.isSleepingEnabled), mRigidBodies(mMemoryManager.getPoolAllocator()), @@ -249,9 +249,6 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { // Compute the inverse mass 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 mBodies.add(rigidBody); mRigidBodies.add(rigidBody); diff --git a/src/systems/ConstraintSolverSystem.cpp b/src/systems/ConstraintSolverSystem.cpp index 6cc3dbdd..7b8ba7d1 100644 --- a/src/systems/ConstraintSolverSystem.cpp +++ b/src/systems/ConstraintSolverSystem.cpp @@ -33,17 +33,19 @@ using namespace reactphysics3d; // Constructor -ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents, +ConstraintSolverSystem::ConstraintSolverSystem(DynamicsWorld& world, Islands& islands, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, JointComponents& jointComponents, BallAndSocketJointComponents& ballAndSocketJointComponents, - FixedJointComponents& fixedJointComponents) + FixedJointComponents& fixedJointComponents, + HingeJointComponents& hingeJointComponents) : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents, jointComponents), - mSolveBallAndSocketJointSystem(rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents), - mSolveFixedJointSystem(rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents), + mSolveBallAndSocketJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents), + mSolveFixedJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents), + mSolveHingeJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, hingeJointComponents), mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents), - mFixedJointComponents(fixedJointComponents) { + mFixedJointComponents(fixedJointComponents), mHingeJointComponents(hingeJointComponents) { #ifdef IS_PROFILING_ACTIVE @@ -69,13 +71,17 @@ void ConstraintSolverSystem::initialize(decimal dt) { mSolveBallAndSocketJointSystem.setIsWarmStartingActive(mIsWarmStartingActive); mSolveFixedJointSystem.setTimeStep(dt); mSolveFixedJointSystem.setIsWarmStartingActive(mIsWarmStartingActive); + mSolveHingeJointSystem.setTimeStep(dt); + mSolveHingeJointSystem.setIsWarmStartingActive(mIsWarmStartingActive); mSolveBallAndSocketJointSystem.initBeforeSolve(); mSolveFixedJointSystem.initBeforeSolve(); + mSolveHingeJointSystem.initBeforeSolve(); if (mIsWarmStartingActive) { mSolveBallAndSocketJointSystem.warmstart(); mSolveFixedJointSystem.warmstart(); + mSolveHingeJointSystem.warmstart(); } // For each joint @@ -84,7 +90,8 @@ void ConstraintSolverSystem::initialize(decimal dt) { // TODO : DELETE THIS Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i]; if (mBallAndSocketJointComponents.hasComponent(jointEntity) || - mFixedJointComponents.hasComponent(jointEntity)) { + mFixedJointComponents.hasComponent(jointEntity) || + mHingeJointComponents.hasComponent(jointEntity)) { continue; } @@ -110,6 +117,7 @@ void ConstraintSolverSystem::solveVelocityConstraints() { mSolveBallAndSocketJointSystem.solveVelocityConstraint(); mSolveFixedJointSystem.solveVelocityConstraint(); + mSolveHingeJointSystem.solveVelocityConstraint(); // For each joint for (uint i=0; igetEntity()); mContactConstraints[mNbContactManifolds].dynamicsComponentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2->getEntity()); - mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody1 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity1); + mContactConstraints[mNbContactManifolds].inverseInertiaTensorBody2 = RigidBody::getInertiaTensorInverseWorld(mWorld, externalManifold.bodyEntity2); mContactConstraints[mNbContactManifolds].massInverseBody1 = mRigidBodyComponents.getMassInverse(body1->getEntity()); mContactConstraints[mNbContactManifolds].massInverseBody2 = mRigidBodyComponents.getMassInverse(body2->getEntity()); mContactConstraints[mNbContactManifolds].nbContacts = externalManifold.nbContactPoints; diff --git a/src/systems/ContactSolverSystem.h b/src/systems/ContactSolverSystem.h index 15822803..f6c9a2f5 100644 --- a/src/systems/ContactSolverSystem.h +++ b/src/systems/ContactSolverSystem.h @@ -284,6 +284,9 @@ class ContactSolverSystem { /// Memory manager MemoryManager& mMemoryManager; + /// Physics world + DynamicsWorld& mWorld; + /// Current time step decimal mTimeStep; @@ -360,7 +363,7 @@ class ContactSolverSystem { // -------------------- Methods -------------------- // /// Constructor - ContactSolverSystem(MemoryManager& memoryManager, Islands& islands, CollisionBodyComponents& bodyComponents, + ContactSolverSystem(MemoryManager& memoryManager, DynamicsWorld& world, Islands& islands, CollisionBodyComponents& bodyComponents, RigidBodyComponents& rigidBodyComponents, ProxyShapeComponents& proxyShapeComponents, const WorldSettings& worldSettings); diff --git a/src/systems/DynamicsSystem.cpp b/src/systems/DynamicsSystem.cpp index 042f1c86..68a12299 100644 --- a/src/systems/DynamicsSystem.cpp +++ b/src/systems/DynamicsSystem.cpp @@ -25,12 +25,14 @@ // Libraries #include "systems/DynamicsSystem.h" +#include "body/RigidBody.h" +#include "engine/DynamicsWorld.h" using namespace reactphysics3d; // Constructor -DynamicsSystem::DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity) - :mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled), +DynamicsSystem::DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity) + :mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mIsGravityEnabled(isGravityEnabled), mGravity(gravity) { } @@ -95,14 +97,6 @@ void DynamicsSystem::updateBodiesState() { const Vector3& centerOfMassLocal = mRigidBodyComponents.mCentersOfMassLocal[i]; 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. @@ -130,7 +124,7 @@ void DynamicsSystem::integrateRigidBodiesVelocities(decimal timeStep) { mRigidBodyComponents.mConstrainedLinearVelocities[i] = linearVelocity + timeStep * mRigidBodyComponents.mInverseMasses[i] * mRigidBodyComponents.mExternalForces[i]; mRigidBodyComponents.mConstrainedAngularVelocities[i] = angularVelocity + timeStep * - mRigidBodyComponents.mInverseInertiaTensorsWorld[i] * mRigidBodyComponents.mExternalTorques[i]; + RigidBody::getInertiaTensorInverseWorld(mWorld, mRigidBodyComponents.mBodiesEntities[i]) * mRigidBodyComponents.mExternalTorques[i]; } // Apply gravity force diff --git a/src/systems/DynamicsSystem.h b/src/systems/DynamicsSystem.h index 44cef67d..a5c89c22 100644 --- a/src/systems/DynamicsSystem.h +++ b/src/systems/DynamicsSystem.h @@ -33,6 +33,8 @@ namespace reactphysics3d { +class DynamicsWorld; + // Class DynamicsSystem /** * This class is responsible to compute and update the dynamics of the bodies that are simulated @@ -44,6 +46,9 @@ class DynamicsSystem { // -------------------- Attributes -------------------- // + /// Physics world + DynamicsWorld& mWorld; + /// Reference to the rigid body components RigidBodyComponents& mRigidBodyComponents; @@ -67,7 +72,7 @@ class DynamicsSystem { // -------------------- Methods -------------------- // /// Constructor - DynamicsSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, + DynamicsSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, bool& isGravityEnabled, Vector3& gravity); /// Destructor diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index f6ba8e5c..259ed5fb 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -25,6 +25,7 @@ // Libraries #include "systems/SolveBallAndSocketJointSystem.h" +#include "engine/DynamicsWorld.h" #include "body/RigidBody.h" using namespace reactphysics3d; @@ -33,11 +34,11 @@ using namespace reactphysics3d; const decimal SolveBallAndSocketJointSystem::BETA = decimal(0.2); // Constructor -SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(RigidBodyComponents& rigidBodyComponents, +SolveBallAndSocketJointSystem::SolveBallAndSocketJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, JointComponents& jointComponents, BallAndSocketJointComponents& ballAndSocketJointComponents) - :mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), + :mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents), mTimeStep(0), mIsWarmStartingActive(true) { @@ -55,13 +56,9 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - // TODO : Remove this and use compoents instead of pointers to bodies - const RigidBody* body1 = static_cast(mRigidBodyComponents.getRigidBody(body1Entity)); - const RigidBody* body2 = static_cast(mRigidBodyComponents.getRigidBody(body2Entity)); - // Get the inertia tensor of bodies - mBallAndSocketJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld(); - mBallAndSocketJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld(); + mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity); + mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity); } // For each joint @@ -260,13 +257,9 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - // TODO : Remove this and use compoents instead of pointers to bodies - const RigidBody* body1 = static_cast(mRigidBodyComponents.getRigidBody(body1Entity)); - const RigidBody* body2 = static_cast(mRigidBodyComponents.getRigidBody(body2Entity)); - // Recompute the inverse inertia tensors - mBallAndSocketJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld(); - mBallAndSocketJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld(); + mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity); + mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity); } // For each joint component diff --git a/src/systems/SolveBallAndSocketJointSystem.h b/src/systems/SolveBallAndSocketJointSystem.h index 7bab807c..64b437b7 100644 --- a/src/systems/SolveBallAndSocketJointSystem.h +++ b/src/systems/SolveBallAndSocketJointSystem.h @@ -35,6 +35,9 @@ namespace reactphysics3d { +// Forward declarations +class DynamicsWorld; + // Class SolveBallAndSocketJointSystem /** * This class is responsible to solve the BallAndSocketJoint constraints @@ -50,6 +53,9 @@ class SolveBallAndSocketJointSystem { // -------------------- Attributes -------------------- // + /// Physics world + DynamicsWorld& mWorld; + /// Reference to the rigid body components RigidBodyComponents& mRigidBodyComponents; @@ -79,7 +85,7 @@ class SolveBallAndSocketJointSystem { // -------------------- Methods -------------------- // /// Constructor - SolveBallAndSocketJointSystem(RigidBodyComponents& rigidBodyComponents, + SolveBallAndSocketJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, JointComponents& jointComponents, BallAndSocketJointComponents& ballAndSocketJointComponents); diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index 9fb51bdd..6a2dbed7 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -25,6 +25,7 @@ // Libraries #include "systems/SolveFixedJointSystem.h" +#include "engine/DynamicsWorld.h" #include "body/RigidBody.h" using namespace reactphysics3d; @@ -33,11 +34,11 @@ using namespace reactphysics3d; const decimal SolveFixedJointSystem::BETA = decimal(0.2); // Constructor -SolveFixedJointSystem::SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents, +SolveFixedJointSystem::SolveFixedJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, JointComponents& jointComponents, FixedJointComponents& fixedJointComponents) - :mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), + :mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents), mJointComponents(jointComponents), mFixedJointComponents(fixedJointComponents), mTimeStep(0), mIsWarmStartingActive(true) { @@ -55,13 +56,9 @@ void SolveFixedJointSystem::initBeforeSolve() { const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - // TODO : Remove this and use compoents instead of pointers to bodies - RigidBody* body1 = static_cast(mRigidBodyComponents.getRigidBody(body1Entity)); - RigidBody* body2 = static_cast(mRigidBodyComponents.getRigidBody(body2Entity)); - // Get the inertia tensor of bodies - mFixedJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld(); - mFixedJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld(); + mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity); + mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity); } // For each joint @@ -91,8 +88,8 @@ void SolveFixedJointSystem::initBeforeSolve() { const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR2World[i]); + Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]); + Matrix3x3 skewSymmetricMatrixU2 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR2World[i]); const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); @@ -346,13 +343,9 @@ void SolveFixedJointSystem::solvePositionConstraint() { const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); - // TODO : Remove this and use compoents instead of pointers to bodies - const RigidBody* body1 = static_cast(mRigidBodyComponents.getRigidBody(body1Entity)); - const RigidBody* body2 = static_cast(mRigidBodyComponents.getRigidBody(body2Entity)); - // Recompute the inverse inertia tensors - mFixedJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld(); - mFixedJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld(); + mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity); + mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity); } // For each joint @@ -439,10 +432,10 @@ void SolveFixedJointSystem::solvePositionConstraint() { const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); - Vector3 x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; - Vector3 x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; - Quaternion q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; - Quaternion q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; + Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1]; + Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2]; + Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1]; + Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2]; // Compute position error for the 3 translation constraints const Vector3 errorTranslation = x2 + r2World - x1 - r1World; @@ -536,10 +529,5 @@ void SolveFixedJointSystem::solvePositionConstraint() { // Update the body position/orientation of body 2 q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); - - mRigidBodyComponents.mConstrainedPositions[componentIndexBody1] = x1; - mRigidBodyComponents.mConstrainedPositions[componentIndexBody2] = x2; - mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1] = q1; - mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2] = q2; } } diff --git a/src/systems/SolveFixedJointSystem.h b/src/systems/SolveFixedJointSystem.h index cf0a7b1f..687efa8e 100644 --- a/src/systems/SolveFixedJointSystem.h +++ b/src/systems/SolveFixedJointSystem.h @@ -35,6 +35,8 @@ namespace reactphysics3d { +class DynamicsWorld; + // Class SolveFixedJointSystem /** * This class is responsible to solve the FixedJoint constraints @@ -50,6 +52,9 @@ class SolveFixedJointSystem { // -------------------- Attributes -------------------- // + /// Physics world + DynamicsWorld& mWorld; + /// Reference to the rigid body components RigidBodyComponents& mRigidBodyComponents; @@ -79,7 +84,7 @@ class SolveFixedJointSystem { // -------------------- Methods -------------------- // /// Constructor - SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, + SolveFixedJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, JointComponents& jointComponents, FixedJointComponents& fixedJointComponents); /// Destructor diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp new file mode 100644 index 00000000..e67fb67a --- /dev/null +++ b/src/systems/SolveHingeJointSystem.cpp @@ -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)); +} diff --git a/src/systems/SolveHingeJointSystem.h b/src/systems/SolveHingeJointSystem.h new file mode 100644 index 00000000..60769091 --- /dev/null +++ b/src/systems/SolveHingeJointSystem.h @@ -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