diff --git a/CMakeLists.txt b/CMakeLists.txt index d16403b4..d0c969aa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -133,6 +133,7 @@ SET (REACTPHYSICS3D_HEADERS "src/systems/DynamicsSystem.h" "src/systems/CollisionDetectionSystem.h" "src/systems/SolveBallAndSocketJointSystem.h" + "src/systems/SolveFixedJointSystem.h" "src/engine/DynamicsWorld.h" "src/engine/EventListener.h" "src/engine/Island.h" @@ -230,6 +231,7 @@ SET (REACTPHYSICS3D_SOURCES "src/systems/DynamicsSystem.cpp" "src/systems/CollisionDetectionSystem.cpp" "src/systems/SolveBallAndSocketJointSystem.cpp" + "src/systems/SolveFixedJointSystem.cpp" "src/engine/DynamicsWorld.cpp" "src/engine/Island.cpp" "src/engine/Material.cpp" diff --git a/src/components/FixedJointComponents.h b/src/components/FixedJointComponents.h index bfaed648..a6c10a69 100644 --- a/src/components/FixedJointComponents.h +++ b/src/components/FixedJointComponents.h @@ -140,16 +140,16 @@ class FixedJointComponents : public Components { void setJoint(Entity jointEntity, FixedJoint* joint) const; /// Return the local anchor point of body 1 for a given joint - const Vector3& getLocalAnchoirPointBody1(Entity jointEntity) const; + const Vector3& getLocalAnchorPointBody1(Entity jointEntity) const; /// Set the local anchor point of body 1 for a given joint - void setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1); + void setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchorPointBody1); /// Return the local anchor point of body 2 for a given joint - const Vector3& getLocalAnchoirPointBody2(Entity jointEntity) const; + const Vector3& getLocalAnchorPointBody2(Entity jointEntity) const; /// Set the local anchor point of body 2 for a given joint - void setLocalAnchoirPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2); + void setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2); /// Return the vector from center of body 1 to anchor point in world-space const Vector3& getR1World(Entity jointEntity) const; @@ -220,6 +220,7 @@ class FixedJointComponents : public Components { // -------------------- Friendship -------------------- // friend class BroadPhaseSystem; + friend class SolveFixedJointSystem; }; // Return a pointer to a given joint @@ -237,31 +238,31 @@ inline void FixedJointComponents::setJoint(Entity jointEntity, FixedJoint* joint } // Return the local anchor point of body 1 for a given joint -inline const Vector3& FixedJointComponents::getLocalAnchoirPointBody1(Entity jointEntity) const { +inline const Vector3& FixedJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 1 for a given joint -inline void FixedJointComponents::setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) { +inline void FixedJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchorPointBody1) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); - mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1; + mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchorPointBody1; } // Return the local anchor point of body 2 for a given joint -inline const Vector3& FixedJointComponents::getLocalAnchoirPointBody2(Entity jointEntity) const { +inline const Vector3& FixedJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]]; } // Set the local anchor point of body 2 for a given joint -inline void FixedJointComponents::setLocalAnchoirPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) { +inline void FixedJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchorPointBody2) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); - mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2; + mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchorPointBody2; } // Return the vector from center of body 1 to anchor point in world-space diff --git a/src/components/RigidBodyComponents.h b/src/components/RigidBodyComponents.h index 8ed50a09..5a6aa395 100644 --- a/src/components/RigidBodyComponents.h +++ b/src/components/RigidBodyComponents.h @@ -347,6 +347,7 @@ class RigidBodyComponents : public Components { friend class DynamicsWorld; friend class ContactSolverSystem; friend class SolveBallAndSocketJointSystem; + friend class SolveFixedJointSystem; friend class DynamicsSystem; friend class BallAndSocketJoint; friend class FixedJoint; diff --git a/src/constraint/FixedJoint.cpp b/src/constraint/FixedJoint.cpp index d62b26d5..9783e979 100644 --- a/src/constraint/FixedJoint.cpp +++ b/src/constraint/FixedJoint.cpp @@ -31,9 +31,6 @@ using namespace reactphysics3d; -// Static variables definition -const decimal FixedJoint::BETA = decimal(0.2); - // Constructor FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo) : Joint(entity, world) { @@ -42,8 +39,8 @@ FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity()); const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity()); - mWorld.mFixedJointsComponents.setLocalAnchoirPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); - mWorld.mFixedJointsComponents.setLocalAnchoirPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); + mWorld.mFixedJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace); + mWorld.mFixedJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace); // Store inverse of initial rotation from body 1 to body 2 in body 1 space: // @@ -62,379 +59,27 @@ FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo // Initialize before solving the constraint void FixedJoint::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 = body1->getTransform().getOrientation(); - const Quaternion& orientationBody2 = body2->getTransform().getOrientation(); - - // Get the inertia tensor of bodies - mWorld.mFixedJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); - mWorld.mFixedJointsComponents.setI1(mEntity, body2->getInertiaTensorInverseWorld()); - - const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity); - const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity); - - const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity); - const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity); - - // Compute the vector from body center to the anchor point in world-space - mWorld.mFixedJointsComponents.setR1World(mEntity, orientationBody1 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity)); - mWorld.mFixedJointsComponents.setR2World(mEntity, orientationBody2 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity)); - - // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World); - - // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints - const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity()); - const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity()); - const decimal inverseMassBodies = body1MassInverse + body2MassInverse; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose(); - - // Compute the inverse mass matrix K^-1 for the 3 translation constraints - Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity); - inverseMassMatrixTranslation.setToZero(); - if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || - mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mWorld.mFixedJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse()); - } - - // Compute the bias "b" of the constraint for the 3 translation constraints - const decimal biasFactor = (BETA / constraintSolverData.timeStep); - Vector3& biasTranslation = mWorld.mFixedJointsComponents.getBiasTranslation(mEntity); - biasTranslation.setToZero(); - if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - mWorld.mFixedJointsComponents.setBiasTranslation(mEntity, biasFactor * (x2 + r2World - x1 - r1World)); - } - - // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation - // contraints (3x3 matrix) - Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity); - inverseMassMatrixRotation = i1 + i2; - if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || - mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mWorld.mFixedJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity).getInverse()); - } - - // Compute the bias "b" for the 3 rotation constraints - Vector3& biasRotation = mWorld.mFixedJointsComponents.getBiasRotation(mEntity); - biasRotation.setToZero(); - - if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { - const Quaternion qError = orientationBody2 * mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity) * orientationBody1.getInverse(); - mWorld.mFixedJointsComponents.setBiasRotation(mEntity, biasFactor * decimal(2.0) * qError.getVectorV()); - } - - // If warm-starting is not enabled - if (!constraintSolverData.isWarmStartingActive) { - - Vector3& impulseTranslation = mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity); - Vector3& impulseRotation = mWorld.mFixedJointsComponents.getImpulseRotation(mEntity); - - // Reset the accumulated impulses - impulseTranslation.setToZero(); - impulseRotation.setToZero(); - } } // Warm start the constraint (apply the previous impulse at the beginning of the step) void FixedJoint::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 of the bodies - const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); - const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); - - const Vector3& impulseTranslation = mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity); - const Vector3& impulseRotation = mWorld.mFixedJointsComponents.getImpulseRotation(mEntity); - - const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity); - const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity); - - const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity); - const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity); - - // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 - Vector3 linearImpulseBody1 = -impulseTranslation; - Vector3 angularImpulseBody1 = impulseTranslation.cross(r1World); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 - angularImpulseBody1 += -impulseRotation; - - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += i1 * angularImpulseBody1; - - // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 - Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2 - angularImpulseBody2 += impulseRotation; - - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * impulseTranslation; - w2 += i2 * angularImpulseBody2; } // Solve the velocity constraint void FixedJoint::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 of the bodies - decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity); - decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity); - - const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity); - const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity); - - const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity); - const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity); - - // --------------- Translation Constraints --------------- // - - // Compute J*v for the 3 translation constraints - const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World); - - const Vector3& biasTranslation = mWorld.mFixedJointsComponents.getBiasTranslation(mEntity); - const Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity); - - // Compute the Lagrange multiplier lambda - const Vector3 deltaLambda = inverseMassMatrixTranslation * (-JvTranslation - biasTranslation); - mWorld.mFixedJointsComponents.setImpulseTranslation(mEntity, mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity) + deltaLambda); - - // Compute the impulse P=J^T * lambda for body 1 - const Vector3 linearImpulseBody1 = -deltaLambda; - Vector3 angularImpulseBody1 = deltaLambda.cross(r1World); - - // Apply the impulse to the body 1 - v1 += inverseMassBody1 * linearImpulseBody1; - w1 += i1 * angularImpulseBody1; - - // Compute the impulse P=J^T * lambda for body 2 - const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World); - - // Apply the impulse to the body 2 - v2 += inverseMassBody2 * deltaLambda; - w2 += i2 * angularImpulseBody2; - - // --------------- Rotation Constraints --------------- // - - // Compute J*v for the 3 rotation constraints - const Vector3 JvRotation = w2 - w1; - - const Vector3& biasRotation = mWorld.mFixedJointsComponents.getBiasRotation(mEntity); - const Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity); - - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 deltaLambda2 = inverseMassMatrixRotation * (-JvRotation - biasRotation); - mWorld.mFixedJointsComponents.setImpulseRotation(mEntity, mWorld.mFixedJointsComponents.getImpulseRotation(mEntity) + deltaLambda2); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 - angularImpulseBody1 = -deltaLambda2; - - // Apply the impulse to the body 1 - w1 += i1 * angularImpulseBody1; - - // Apply the impulse to the body 2 - w2 += i2 * deltaLambda2; } // Solve the position constraint (for position error correction) void FixedJoint::solvePositionConstraint(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)); - - // 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 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); - - const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity); - const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity); - - const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity); - const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity); - - // Recompute the inverse inertia tensors - mWorld.mFixedJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld()); - mWorld.mFixedJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld()); - - // Compute the vector from body center to the anchor point in world-space - mWorld.mFixedJointsComponents.setR1World(mEntity, q1 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity)); - mWorld.mFixedJointsComponents.setR2World(mEntity, q2 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity)); - - // 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 - decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2; - Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, - 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose(); - Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity); - inverseMassMatrixTranslation.setToZero(); - if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || - mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mWorld.mFixedJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse()); - } - - // 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 of the mass matrix K=JM^-1J^t for the 3 rotation - // contraints (3x3 matrix) - Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity); - inverseMassMatrixRotation = i1 + i2; - if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || - mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { - mWorld.mFixedJointsComponents.setInverseMassMatrixRotation(mEntity, inverseMassMatrixRotation.getInverse()); - } - - // Calculate difference in rotation - // - // The rotation should be: - // - // q2 = q1 r0 - // - // But because of drift the actual rotation is: - // - // q2 = qError q1 r0 - // <=> qError = q2 r0^-1 q1^-1 - // - // Where: - // q1 = current rotation of body 1 - // q2 = current rotation of body 2 - // qError = error that needs to be reduced to zero - Quaternion qError = q2 * mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity) * q1.getInverse(); - - // A quaternion can be seen as: - // - // q = [sin(theta / 2) * v, cos(theta/2)] - // - // Where: - // v = rotation vector - // theta = rotation angle - // - // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: - const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); - - // Compute the Lagrange multiplier lambda for the 3 rotation constraints - Vector3 lambdaRotation = inverseMassMatrixRotation * (-errorRotation); - - // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 - angularImpulseBody1 = -lambdaRotation; - - // 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 pseudo velocity of body 2 - w2 = i2 * lambdaRotation; - - // 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); } // Return a string representation std::string FixedJoint::to_string() const { - return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity).to_string() + - ", localAnchorPointBody2=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity).to_string() + + return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() + + ", localAnchorPointBody2=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() + ", initOrientationDifferenceInv=" + mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() + "}"; } diff --git a/src/constraint/FixedJoint.h b/src/constraint/FixedJoint.h index 57b302cf..701be60b 100644 --- a/src/constraint/FixedJoint.h +++ b/src/constraint/FixedJoint.h @@ -68,26 +68,25 @@ class FixedJoint : public Joint { private : - // -------------------- Constants -------------------- // - - // Beta value for the bias factor of position correction - static const decimal BETA; - // -------------------- Methods -------------------- // /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const override; /// Initialize before solving the constraint + // TODO : DELETE THIS virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) override; /// Warm start the constraint (apply the previous impulse at the beginning of the step) + // TODO : DELETE THIS virtual void warmstart(const ConstraintSolverData& constraintSolverData) override; /// Solve the velocity constraint + // TODO : DELETE THIS virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) override; /// Solve the position constraint (for position error correction) + // TODO : DELETE THIS virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) override; public : diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 55f8d539..d162dda0 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -53,7 +53,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents, mProxyShapesComponents, mConfig), mConstraintSolverSystem(mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents, - mBallAndSocketJointsComponents), + mBallAndSocketJointsComponents, mFixedJointsComponents), mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity), mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations), mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations), diff --git a/src/systems/ConstraintSolverSystem.cpp b/src/systems/ConstraintSolverSystem.cpp index 8af41d2b..6cc3dbdd 100644 --- a/src/systems/ConstraintSolverSystem.cpp +++ b/src/systems/ConstraintSolverSystem.cpp @@ -36,11 +36,14 @@ using namespace reactphysics3d; ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, JointComponents& jointComponents, - BallAndSocketJointComponents& ballAndSocketJointComponents) + BallAndSocketJointComponents& ballAndSocketJointComponents, + FixedJointComponents& fixedJointComponents) : mIsWarmStartingActive(true), mIslands(islands), mConstraintSolverData(rigidBodyComponents, jointComponents), mSolveBallAndSocketJointSystem(rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents), - mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents){ + mSolveFixedJointSystem(rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents), + mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents), + mFixedJointComponents(fixedJointComponents) { #ifdef IS_PROFILING_ACTIVE @@ -64,11 +67,15 @@ void ConstraintSolverSystem::initialize(decimal dt) { mSolveBallAndSocketJointSystem.setTimeStep(dt); mSolveBallAndSocketJointSystem.setIsWarmStartingActive(mIsWarmStartingActive); + mSolveFixedJointSystem.setTimeStep(dt); + mSolveFixedJointSystem.setIsWarmStartingActive(mIsWarmStartingActive); mSolveBallAndSocketJointSystem.initBeforeSolve(); + mSolveFixedJointSystem.initBeforeSolve(); if (mIsWarmStartingActive) { mSolveBallAndSocketJointSystem.warmstart(); + mSolveFixedJointSystem.warmstart(); } // For each joint @@ -76,7 +83,8 @@ void ConstraintSolverSystem::initialize(decimal dt) { // TODO : DELETE THIS Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i]; - if (mBallAndSocketJointComponents.hasComponent(jointEntity)) { + if (mBallAndSocketJointComponents.hasComponent(jointEntity) || + mFixedJointComponents.hasComponent(jointEntity)) { continue; } @@ -101,13 +109,15 @@ void ConstraintSolverSystem::solveVelocityConstraints() { RP3D_PROFILE("ConstraintSolverSystem::solveVelocityConstraints()", mProfiler); mSolveBallAndSocketJointSystem.solveVelocityConstraint(); + mSolveFixedJointSystem.solveVelocityConstraint(); // For each joint for (uint i=0; i(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(); + } + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.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 + mFixedJointComponents.mR1World[i] = orientationBody1 * mFixedJointComponents.mLocalAnchorPointBody1[i]; + mFixedJointComponents.mR2World[i] = orientationBody2 * mFixedJointComponents.mLocalAnchorPointBody2[i]; + } + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.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(mFixedJointComponents.mR1World[i]); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR2World[i]); + + const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity); + const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity); + + // Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints + const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + const decimal inverseMassBodies = body1MassInverse + body2MassInverse; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mFixedJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mFixedJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose(); + + // Compute the inverse mass matrix K^-1 for the 3 translation constraints + mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero(); + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); + } + } + + const decimal biasFactor = BETA / mTimeStep; + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.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); + + const Vector3& r1World = mFixedJointComponents.mR1World[i]; + const Vector3& r2World = mFixedJointComponents.mR2World[i]; + + // Compute the bias "b" of the constraint for the 3 translation constraints + mFixedJointComponents.mBiasTranslation[i].setToZero(); + if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + mFixedJointComponents.mBiasTranslation[i] = biasFactor * (x2 + r2World - x1 - r1World); + } + } + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + + // Get the bodies entities + const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); + const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation contraints (3x3 matrix) + mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i]; + if (mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC || + mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) { + mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(); + } + } + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + + // Get the bodies entities + const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity); + const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity); + + // Compute the bias "b" for the 3 rotation constraints + mFixedJointComponents.mBiasRotation[i].setToZero(); + + const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); + const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); + + if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + const Quaternion qError = orientationBody2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse(); + mFixedJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV(); + } + } + + // If warm-starting is not enabled + if (!mIsWarmStartingActive) { + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + // Reset the accumulated impulses + mFixedJointComponents.mImpulseTranslation[i].setToZero(); + mFixedJointComponents.mImpulseRotation[i].setToZero(); + } + } +} + +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void SolveFixedJointSystem::warmstart() { + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.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 of the bodies + const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + + const Vector3& impulseTranslation = mFixedJointComponents.mImpulseTranslation[i]; + const Vector3& impulseRotation = mFixedJointComponents.mImpulseRotation[i]; + + const Vector3& r1World = mFixedJointComponents.mR1World[i]; + const Vector3& r2World = mFixedJointComponents.mR2World[i]; + + // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1 + Vector3 linearImpulseBody1 = -impulseTranslation; + Vector3 angularImpulseBody1 = impulseTranslation.cross(r1World); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 + angularImpulseBody1 += -impulseRotation; + + const Matrix3x3& i1 = mFixedJointComponents.mI1[i]; + + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += i1 * angularImpulseBody1; + + // Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2 + Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2 + angularImpulseBody2 += impulseRotation; + + const Matrix3x3& i2 = mFixedJointComponents.mI2[i]; + + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * impulseTranslation; + w2 += i2 * angularImpulseBody2; + } +} + +// Solve the velocity constraint +void SolveFixedJointSystem::solveVelocityConstraint() { + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.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 of the bodies + decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + + const Vector3& r1World = mFixedJointComponents.mR1World[i]; + const Vector3& r2World = mFixedJointComponents.mR2World[i]; + + // --------------- Translation Constraints --------------- // + + // Compute J*v for the 3 translation constraints + const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World); + + const Matrix3x3& inverseMassMatrixTranslation = mFixedJointComponents.mInverseMassMatrixTranslation[i]; + + // Compute the Lagrange multiplier lambda + const Vector3 deltaLambda = inverseMassMatrixTranslation * (-JvTranslation - mFixedJointComponents.mBiasTranslation[i]); + mFixedJointComponents.mImpulseTranslation[i] += deltaLambda; + + // Compute the impulse P=J^T * lambda for body 1 + const Vector3 linearImpulseBody1 = -deltaLambda; + Vector3 angularImpulseBody1 = deltaLambda.cross(r1World); + + const Matrix3x3& i1 = mFixedJointComponents.mI1[i]; + + // Apply the impulse to the body 1 + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += i1 * angularImpulseBody1; + + // Compute the impulse P=J^T * lambda for body 2 + const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World); + + const Matrix3x3& i2 = mFixedJointComponents.mI2[i]; + + // Apply the impulse to the body 2 + v2 += inverseMassBody2 * deltaLambda; + w2 += i2 * angularImpulseBody2; + + // --------------- Rotation Constraints --------------- // + + // Compute J*v for the 3 rotation constraints + const Vector3 JvRotation = w2 - w1; + + const Vector3& biasRotation = mFixedJointComponents.mBiasRotation[i]; + const Matrix3x3& inverseMassMatrixRotation = mFixedJointComponents.mInverseMassMatrixRotation[i]; + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 deltaLambda2 = inverseMassMatrixRotation * (-JvRotation - biasRotation); + mFixedJointComponents.mImpulseRotation[i] += deltaLambda2; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1 + angularImpulseBody1 = -deltaLambda2; + + // Apply the impulse to the body 1 + w1 += i1 * angularImpulseBody1; + + // Apply the impulse to the body 2 + w2 += i2 * deltaLambda2; + } +} + +// Solve the position constraint (for position error correction) +void SolveFixedJointSystem::solvePositionConstraint() { + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // 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); + + // 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(); + } + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // 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); + + // Get the bodies positions and orientations + 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 + mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.getLocalAnchorPointBody1(jointEntity); + mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.getLocalAnchorPointBody2(jointEntity); + } + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // 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); + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + + const Vector3& r1World = mFixedJointComponents.mR1World[i]; + const Vector3& r2World = mFixedJointComponents.mR2World[i]; + + // 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 + decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2; + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, + 0, inverseMassBodies, 0, + 0, 0, inverseMassBodies) + + skewSymmetricMatrixU1 * mFixedJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() + + skewSymmetricMatrixU2 * mFixedJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose(); + mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero(); + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse(); + } + } + + // For each joint + for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) { + + const Entity jointEntity = mFixedJointComponents.mJointEntities[i]; + + // If the error position correction technique is not the non-linear-gauss-seidel, we do + // 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 Vector3& r1World = mFixedJointComponents.mR1World[i]; + const Vector3& r2World = mFixedJointComponents.mR2World[i]; + + 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]; + + // Compute position error for the 3 translation constraints + const Vector3 errorTranslation = x2 + r2World - x1 - r1World; + + // Compute the Lagrange multiplier lambda + const Vector3 lambdaTranslation = mFixedJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation); + + // Compute the impulse of body 1 + Vector3 linearImpulseBody1 = -lambdaTranslation; + Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World); + + const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; + + // Compute the pseudo velocity of body 1 + const Vector3 v1 = inverseMassBody1 * linearImpulseBody1; + Vector3 w1 = mFixedJointComponents.mI2[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(r2World); + + const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2]; + + // Compute the pseudo velocity of body 2 + const Vector3 v2 = inverseMassBody2 * lambdaTranslation; + Vector3 w2 = mFixedJointComponents.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 of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i]; + if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || + mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) { + mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse(); + } + + // Calculate difference in rotation + // + // The rotation should be: + // + // q2 = q1 r0 + // + // But because of drift the actual rotation is: + // + // q2 = qError q1 r0 + // <=> qError = q2 r0^-1 q1^-1 + // + // Where: + // q1 = current rotation of body 1 + // q2 = current rotation of body 2 + // qError = error that needs to be reduced to zero + Quaternion qError = q2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse(); + + // A quaternion can be seen as: + // + // q = [sin(theta / 2) * v, cos(theta/2)] + // + // Where: + // v = rotation vector + // theta = rotation angle + // + // If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is: + const Vector3 errorRotation = decimal(2.0) * qError.getVectorV(); + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 lambdaRotation = mFixedJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation); + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1 + angularImpulseBody1 = -lambdaRotation; + + // Compute the pseudo velocity of body 1 + w1 = mFixedJointComponents.mI1[i] * angularImpulseBody1; + + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the pseudo velocity of body 2 + w2 = mFixedJointComponents.mI2[i] * lambdaRotation; + + // 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 new file mode 100644 index 00000000..cf0a7b1f --- /dev/null +++ b/src/systems/SolveFixedJointSystem.h @@ -0,0 +1,137 @@ +/******************************************************************************** +* 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_FIXED_JOINT_SYSTEM_H +#define REACTPHYSICS3D_SOLVE_FIXED_JOINT_SYSTEM_H + +// Libraries +#include "utils/Profiler.h" +#include "components/RigidBodyComponents.h" +#include "components/JointComponents.h" +#include "components/FixedJointComponents.h" +#include "components/TransformComponents.h" + +namespace reactphysics3d { + +// Class SolveFixedJointSystem +/** + * This class is responsible to solve the FixedJoint constraints + */ +class SolveFixedJointSystem { + + private : + + // -------------------- Constants -------------------- // + + // Beta value for the bias factor of position correction + static const decimal BETA; + + // -------------------- Attributes -------------------- // + + /// Reference to the rigid body components + RigidBodyComponents& mRigidBodyComponents; + + /// Reference to transform components + TransformComponents& mTransformComponents; + + /// Reference to the joint components + JointComponents& mJointComponents; + + /// Reference to the fixed joint components + FixedJointComponents& mFixedJointComponents; + + /// 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 + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents, + JointComponents& jointComponents, FixedJointComponents& fixedJointComponents); + + /// Destructor + ~SolveFixedJointSystem() = 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 SolveFixedJointSystem::setProfiler(Profiler* profiler) { + mProfiler = profiler; +} + +// Set the time step +inline void SolveFixedJointSystem::setTimeStep(decimal timeStep) { + assert(timeStep > decimal(0.0)); + mTimeStep = timeStep; +} + +// Set to true to enable warm starting +inline void SolveFixedJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) { + mIsWarmStartingActive = isWarmStartingActive; +} + +#endif + +} + +#endif