Add methods Joint::getReactionForce() and Joint::getReactionTorque() to retrieve the reaction force and torque of a joint

CHANGELOG.md

@@ -10,18 +10,22 @@ do not hesitate to take a look at the user manual.
  - Method RigidBody::resetForce() to reset the accumulated external force on a rigid body has been added
  - Method RigidBody::resetTorque() to reset the accumulated external torque on a rigid body has been added
  - Constructors with local-space anchor/axis have been added to BallAndSocketJointInfo, HingeJointInfo, FixedJointInfo and SliderJointInfo classes
- - Robustness of polyhedron vs polyhedron collision detection has been improved in SAT algorithm (face contacts are favored over edge-edge contacts for better stability)
  - Method HingeJoint::getAngle() to get the current angle of the hinge joint has been added 
+ - Method Joint::getReactionForce() has been added to retrieve the current reaction force of a joint
+ - Method Joint::getReactionTorque() has been added to retrieve the current reaction torque of a joint
+ - Method RigidBody::setLinearLockAxisFactor() to lock the translational movement of a body along the world-space x, y and z axes
+ - Method RigidBody::setAngularLockAxisFactor() to lock the rotational movement of a body around the world-space x, y and z axes
 
 ### Changed
 
  - The PhysicsWorld::setGravity() method now takes a const parameter
- - Rolling resistance constraint is not solved anymore in the solver. Angular damping needs to be used instead to simulate this
+ - Rolling resistance constraint is not solved anymore in the solver. Angular damping needs to be used instead to simulate it.
  - The List class has been renamed to Array
  - The default number of iterations for the velocity solver is now 6 instead of 10
  - The default number of iterations for the position solver is now 3 instead of 5
  - The raycasting broad-phase performance has been improved
  - The raycasting performance against HeighFieldShape has been improved (better middle-phase algorithm)
+ - Robustness of polyhedron vs polyhedron collision detection has been improved in SAT algorithm (face contacts are favored over edge-edge contacts for better stability)
 
 ### Removed
 

include/reactphysics3d/components/BallAndSocketJointComponents.h

@@ -314,7 +314,7 @@ RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrix(Entity
 }
 
 // Return the accumulated impulse
-RP3D_FORCE_INLINE Vector3 &BallAndSocketJointComponents::getImpulse(Entity jointEntity)  {
+RP3D_FORCE_INLINE Vector3& BallAndSocketJointComponents::getImpulse(Entity jointEntity)  {
 
     assert(mMapEntityToComponentIndex.containsKey(jointEntity));
     return mImpulse[mMapEntityToComponentIndex[jointEntity]];

include/reactphysics3d/components/HingeJointComponents.h

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

include/reactphysics3d/components/SliderJointComponents.h

@@ -457,6 +457,7 @@ class SliderJointComponents : public Components {
 
         friend class BroadPhaseSystem;
         friend class SolveSliderJointSystem;
+        friend class SliderJoint;
 };
 
 // Return a pointer to a given joint

include/reactphysics3d/constraint/BallAndSocketJoint.h

@@ -121,6 +121,12 @@ class BallAndSocketJoint : public Joint {
         /// Deleted copy-constructor
         BallAndSocketJoint(const BallAndSocketJoint& constraint) = delete;
 
+        /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
+        virtual Vector3 getReactionForce(decimal timeStep) const override;
+
+        /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space
+        virtual Vector3 getReactionTorque(decimal timeStep) const override;
+
         /// Return a string representation
         virtual std::string to_string() const override;
 

include/reactphysics3d/constraint/FixedJoint.h

@@ -111,6 +111,12 @@ class FixedJoint : public Joint {
         /// Deleted copy-constructor
         FixedJoint(const FixedJoint& constraint) = delete;
 
+        /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
+        virtual Vector3 getReactionForce(decimal timeStep) const override;
+
+        /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space
+        virtual Vector3 getReactionTorque(decimal timeStep) const override;
+
         /// Return a string representation
         virtual std::string to_string() const override;
 

include/reactphysics3d/constraint/HingeJoint.h

@@ -314,6 +314,12 @@ class HingeJoint : public Joint {
         /// Return the current hinge angle
         decimal getAngle() const;
 
+        /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
+        virtual Vector3 getReactionForce(decimal timeStep) const override;
+
+        /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space
+        virtual Vector3 getReactionTorque(decimal timeStep) const override;
+
         /// Return a string representation
         virtual std::string to_string() const override;
 };

include/reactphysics3d/constraint/Joint.h

@@ -137,6 +137,12 @@ class Joint {
         /// Return the type of the constraint
         JointType getType() const;
 
+        /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint
+        virtual Vector3 getReactionForce(decimal timeStep) const=0;
+
+        /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint
+        virtual Vector3 getReactionTorque(decimal timeStep) const=0;
+
         /// Return true if the collision between the two bodies of the joint is enabled
         bool isCollisionEnabled() const;
 

include/reactphysics3d/constraint/SliderJoint.h

@@ -304,6 +304,12 @@ class SliderJoint : public Joint {
         /// Return the intensity of the current force applied for the joint motor
         decimal getMotorForce(decimal timeStep) const;
 
+        /// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
+        virtual Vector3 getReactionForce(decimal timeStep) const override;
+
+        /// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space
+        virtual Vector3 getReactionTorque(decimal timeStep) const override;
+
         /// Return a string representation
         virtual std::string to_string() const override;
 };

src/constraint/BallAndSocketJoint.cpp

@@ -61,6 +61,17 @@ BallAndSocketJoint::BallAndSocketJoint(Entity entity, PhysicsWorld& world, const
     mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody2(entity, anchorPointBody2LocalSpace);
 }
 
+// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
+Vector3 BallAndSocketJoint::getReactionForce(decimal timeStep) const {
+    assert(timeStep > MACHINE_EPSILON);
+    return mWorld.mBallAndSocketJointsComponents.getImpulse(mEntity) / timeStep;
+}
+
+// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space
+Vector3 BallAndSocketJoint::getReactionTorque(decimal timeStep) const {
+    assert(timeStep > MACHINE_EPSILON);
+    return Vector3(0, 0, 0);
+}
 
 // Return a string representation
 std::string BallAndSocketJoint::to_string() const {

src/constraint/FixedJoint.cpp

@@ -71,6 +71,18 @@ FixedJoint::FixedJoint(Entity entity, PhysicsWorld& world, const FixedJointInfo&
     mWorld.mFixedJointsComponents.setInitOrientationDifferenceInv(mEntity, transform2.getOrientation().getInverse() * transform1.getOrientation());
 }
 
+// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
+Vector3 FixedJoint::getReactionForce(decimal timeStep) const {
+    assert(timeStep > MACHINE_EPSILON);
+    return mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity) / timeStep;
+}
+
+// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space
+Vector3 FixedJoint::getReactionTorque(decimal timeStep) const {
+    assert(timeStep > MACHINE_EPSILON);
+    return mWorld.mFixedJointsComponents.getImpulseRotation(mEntity) / timeStep;
+}
+
 // Return a string representation
 std::string FixedJoint::to_string() const {
     return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() +

src/constraint/HingeJoint.cpp

@@ -266,6 +266,31 @@ decimal HingeJoint::getAngle() const {
     return mWorld.mConstraintSolverSystem.mSolveHingeJointSystem.computeCurrentHingeAngle(mEntity, orientationBody1, orientationBody2);
 }
 
+// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
+Vector3 HingeJoint::getReactionForce(decimal timeStep) const {
+    assert(timeStep > MACHINE_EPSILON);
+    return mWorld.mHingeJointsComponents.getImpulseTranslation(mEntity) / timeStep;
+}
+
+// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space
+Vector3 HingeJoint::getReactionTorque(decimal timeStep) const {
+
+    assert(timeStep > MACHINE_EPSILON);
+
+    const uint32 jointIndex = mWorld.mHingeJointsComponents.getEntityIndex(mEntity);
+
+    const Vector2& impulseRotation = mWorld.mHingeJointsComponents.mImpulseRotation[jointIndex];
+    const Vector3& b2CrossA1 = mWorld.mHingeJointsComponents.mB2CrossA1[jointIndex];
+    const Vector3& c2CrossA1 = mWorld.mHingeJointsComponents.mC2CrossA1[jointIndex];
+
+    Vector3 impulseJoint = b2CrossA1 * impulseRotation.x + c2CrossA1 * impulseRotation.y;
+    Vector3 impulseLowerLimit = mWorld.mHingeJointsComponents.mImpulseLowerLimit[jointIndex] * mWorld.mHingeJointsComponents.mA1[jointIndex];
+    Vector3 impulseUpperLimit = -mWorld.mHingeJointsComponents.mImpulseUpperLimit[jointIndex] * mWorld.mHingeJointsComponents.mA1[jointIndex];
+    Vector3 impulseMotor = mWorld.mHingeJointsComponents.mImpulseMotor[jointIndex] * mWorld.mHingeJointsComponents.mA1[jointIndex];
+
+    return (impulseJoint + impulseLowerLimit + impulseUpperLimit + impulseMotor) / timeStep;
+}
+
 // Return the number of bytes used by the joint
  size_t HingeJoint::getSizeInBytes() const {
     return sizeof(HingeJoint);

src/constraint/SliderJoint.cpp

@@ -293,6 +293,33 @@ decimal SliderJoint::getMaxMotorForce() const {
 decimal SliderJoint::getMaxTranslationLimit() const {
     return mWorld.mSliderJointsComponents.getUpperLimit(mEntity);
 }
+
+// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
+Vector3 SliderJoint::getReactionForce(decimal timeStep) const {
+    assert(timeStep > MACHINE_EPSILON);
+
+    const uint32 jointIndex = mWorld.mSliderJointsComponents.getEntityIndex(mEntity);
+
+    const Vector2 translationImpulse = mWorld.mSliderJointsComponents.mImpulseTranslation[jointIndex];
+    const Vector3& n1 = mWorld.mSliderJointsComponents.mN1[jointIndex];
+    const Vector3& n2 = mWorld.mSliderJointsComponents.mN2[jointIndex];
+    const Vector3 impulseJoint = n1 * translationImpulse.x + n2 * translationImpulse.y;
+
+    const Vector3& sliderAxisWorld = mWorld.mSliderJointsComponents.mSliderAxisWorld[jointIndex];
+    const Vector3 impulseLowerLimit = mWorld.mSliderJointsComponents.mImpulseLowerLimit[jointIndex] * sliderAxisWorld;
+    const Vector3 impulseUpperLimit = -mWorld.mSliderJointsComponents.mImpulseUpperLimit[jointIndex] * sliderAxisWorld;
+    Vector3 impulseMotor = -mWorld.mSliderJointsComponents.mImpulseMotor[jointIndex] * sliderAxisWorld;
+
+    return (impulseJoint + impulseLowerLimit + impulseUpperLimit + impulseMotor) / timeStep;
+}
+
+// Return the torque (in Newtons * meters) on body 2 required to satisfy the joint constraint in world-space
+Vector3 SliderJoint::getReactionTorque(decimal timeStep) const {
+
+    assert(timeStep > MACHINE_EPSILON);
+    return mWorld.mSliderJointsComponents.getImpulseRotation(mEntity) / timeStep;
+}
+
 // Return a string representation
 std::string SliderJoint::to_string() const {
     return "SliderJoint{ lowerLimit=" + std::to_string(mWorld.mSliderJointsComponents.getLowerLimit(mEntity)) + ", upperLimit=" + std::to_string(mWorld.mSliderJointsComponents.getUpperLimit(mEntity)) +

src/systems/SolveSliderJointSystem.cpp

@@ -367,7 +367,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
         const Vector2 JvTranslation(el1, el2);
 
         // Compute the Lagrange multiplier lambda for the 2 translation constraints
-        Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]);
+        const Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]);
         mSliderJointComponents.mImpulseTranslation[i] += deltaLambda;
 
         // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
@@ -380,7 +380,7 @@ void SolveSliderJointSystem::solveVelocityConstraint() {
         w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
 
         // Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
-        const Vector3 linearImpulseBody2 = n1 * deltaLambda.x + n2 * deltaLambda.y;
+        const Vector3 linearImpulseBody2 = -linearImpulseBody1;
         Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y;
 
         // Apply the impulse to the body 2