Working on cone limit for Ball And Socket joint

documentation/UserManual/ReactPhysics3D-UserManual.tex

@@ -1571,17 +1571,14 @@ joint = dynamic_cast<BallAndSocketJoint*>(world->createJoint(jointInfo));
 
     \subsubsection{Cone limit}
 
-    With the ball and socket joint, it is possible to enable a cone limit. The idea is to constrain the rotation of body 2 around the anchor point
+    With the ball and socket joint, it is possible to enable a cone limit. Let's call the \texttt{anchor axis} for body 1 the axis from body 1 center of mass
-    in to stay inside a cone. This can be used to limit the rotation angle of body 2 with respect to the main cone axis. \\
+    to the joint anchor point. The cone limit can be used to constraint the angle between the anchor axis of body 1 and the anchor axis of body 2.
+    The idea is to limit the angle of the anchor axis of body 2 inside a cone around the anchor axis of body 1. The cone is defined by its main axis
+    which is the anchor axis of body 1 and is also defined by the cone half angle. \\
 
-    In the following example, we enable the cone limit for a given ball and socket joint. We limit the rotation with a maximum angle with respect to
+    In the following example, we enable the cone limit for a given ball and socket joint. \\
-    the main axis of the cone (here the local-space Y axis of each body). \\
 
     \begin{lstlisting}
-// Set the main axis of the cone (in both local-spaces of body 1 and 2)
-joint->setConeLimitLocalAxisBody1(rp3d::Vector3(0, 1, 0));
-joint->setConeLimitLocalAxisBody2(rp3d::Vector3(0, 1, 0));
-
 // Set the maximum half angle (in radians) 
 joint->setConeLimitHalfAngle(45.0 * PI / 180.0);
 

include/reactphysics3d/components/BallAndSocketJointComponents.h

@@ -103,12 +103,6 @@ class BallAndSocketJointComponents : public Components {
         /// True if the cone limit is violated
         bool* mIsConeLimitViolated;
 
-        /// Cone limit axis in local-space of body 1
-        Vector3* mConeLimitLocalAxisBody1;
-
-        /// Cone limit axis in local-space of body 2
-        Vector3* mConeLimitLocalAxisBody2;
-
         /// Cross product of cone limit axis of both bodies
         Vector3* mConeLimitACrossB;
 
@@ -450,34 +444,6 @@ RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrixConeLim
     mInverseMassMatrixConeLimit[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
 }
 
-// Get the cone limit local axis of body 1
-RP3D_FORCE_INLINE Vector3 BallAndSocketJointComponents::getConeLimitLocalAxisBody1(Entity jointEntity) const {
-
-    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
-    return mConeLimitLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]];
-}
-
-// Set the cone limit local axis of body 1
-RP3D_FORCE_INLINE void BallAndSocketJointComponents::setConeLimitLocalAxisBody1(Entity jointEntity, const Vector3& localAxisBody1) {
-
-    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
-    mConeLimitLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]] = localAxisBody1;
-}
-
-// Get the cone limit local axis of body 2
-RP3D_FORCE_INLINE Vector3 BallAndSocketJointComponents::getConeLimitLocalAxisBody2(Entity jointEntity) const {
-
-    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
-    return mConeLimitLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]];
-}
-
-// Set the cone limit local axis of body 2
-RP3D_FORCE_INLINE void BallAndSocketJointComponents::setConeLimitLocalAxisBody2(Entity jointEntity, const Vector3& localAxisBody2) {
-
-    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
-    mConeLimitLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]] = localAxisBody2;
-}
-
 }
 
 #endif

include/reactphysics3d/components/HingeJointComponents.h

@@ -140,10 +140,10 @@ class HingeJointComponents : public Components {
         /// True if the motor of the joint in enabled
         bool* mIsMotorEnabled;
 
-        /// Lower limit (minimum allowed rotation angle in radian)
+        /// Lower limit (minimum allowed rotation angle in radians)
         decimal* mLowerLimit;
 
-        /// Upper limit (maximum translation distance)
+        /// Upper limit (maximum allowed rotation angle in radians)
         decimal* mUpperLimit;
 
         /// True if the lower limit is violated

include/reactphysics3d/constraint/BallAndSocketJoint.h

@@ -107,6 +107,8 @@ class BallAndSocketJoint : public Joint {
         /// Return the number of bytes used by the joint
         virtual size_t getSizeInBytes() const override;
 
+        /// Reset the limits
+        void resetLimits();
 
     public :
 
@@ -130,12 +132,6 @@ class BallAndSocketJoint : public Joint {
         /// Set the cone limit half angle
         void setConeLimitHalfAngle(decimal coneHalfAngle);
 
-        /// Set the normalized cone limit axis of body 1 in local-space of body 1
-        void setConeLimitLocalAxisBody1(const Vector3& localAxisBody1);
-
-        /// Set the normalized cone limit axis of body 2 in local-space of body 2
-        void setConeLimitLocalAxisBody2(const Vector3& localAxisBody2);
-
         /// Return the cone limit half angle (in radians)
         decimal getConeLimitHalfAngle() const;
         

src/components/BallAndSocketJointComponents.cpp

@@ -38,8 +38,7 @@ BallAndSocketJointComponents::BallAndSocketJointComponents(MemoryAllocator& allo
                                 sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
                                 sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) +
                                 sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(bool) + sizeof(decimal) +
-                                sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(Vector3) +
+                                sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(Vector3)) {
-                                sizeof(Vector3) + sizeof(Vector3)) {
 
     // Allocate memory for the components data
     allocate(INIT_NB_ALLOCATED_COMPONENTS);
@@ -75,9 +74,7 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
     decimal* newInverseMassMatrixConeLimit = reinterpret_cast<decimal*>(newConeLimitHalfAngle + nbComponentsToAllocate);
     decimal* newBConeLimit = reinterpret_cast<decimal*>(newInverseMassMatrixConeLimit + nbComponentsToAllocate);
     bool* newIsConeLimitViolated = reinterpret_cast<bool*>(newBConeLimit + nbComponentsToAllocate);
-    Vector3* newConeLimitLocalAxisBody1 = reinterpret_cast<Vector3*>(newIsConeLimitViolated + nbComponentsToAllocate);
+    Vector3* newConeLimitACrossB = reinterpret_cast<Vector3*>(newIsConeLimitViolated + nbComponentsToAllocate);
-    Vector3* newConeLimitLocalAxisBody2 = reinterpret_cast<Vector3*>(newConeLimitLocalAxisBody1 + nbComponentsToAllocate);
-    Vector3* newConeLimitACrossB = reinterpret_cast<Vector3*>(newConeLimitLocalAxisBody2 + nbComponentsToAllocate);
 
     // If there was already components before
     if (mNbComponents > 0) {
@@ -100,8 +97,6 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
         memcpy(newInverseMassMatrixConeLimit, mInverseMassMatrixConeLimit, mNbComponents * sizeof(decimal));
         memcpy(newBConeLimit, mBConeLimit, mNbComponents * sizeof(decimal));
         memcpy(newIsConeLimitViolated, mIsConeLimitViolated, mNbComponents * sizeof(bool));
-        memcpy(newConeLimitLocalAxisBody1, mConeLimitLocalAxisBody1, mNbComponents * sizeof(Vector3));
-        memcpy(newConeLimitLocalAxisBody2, mConeLimitLocalAxisBody2, mNbComponents * sizeof(Vector3));
         memcpy(newConeLimitACrossB, mConeLimitACrossB, mNbComponents * sizeof(Vector3));
 
         // Deallocate previous memory
@@ -127,8 +122,6 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
     mInverseMassMatrixConeLimit = newInverseMassMatrixConeLimit;
     mBConeLimit = newBConeLimit;
     mIsConeLimitViolated = newIsConeLimitViolated;
-    mConeLimitLocalAxisBody1 = newConeLimitLocalAxisBody1;
-    mConeLimitLocalAxisBody2 = newConeLimitLocalAxisBody2;
     mConeLimitACrossB = newConeLimitACrossB;
 }
 
@@ -156,8 +149,6 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep
     mInverseMassMatrixConeLimit[index] = decimal(0.0);
     mBConeLimit[index] = decimal(0.0);
     mIsConeLimitViolated[index] = false;
-    new (mConeLimitLocalAxisBody1 + index) Vector3(1, 0, 0);
-    new (mConeLimitLocalAxisBody2 + index) Vector3(1, 0, 0);
     new (mConeLimitACrossB + index) Vector3(0, 0, 0);
 
     // Map the entity with the new component lookup index
@@ -193,8 +184,6 @@ void BallAndSocketJointComponents::moveComponentToIndex(uint32 srcIndex, uint32
     mInverseMassMatrixConeLimit[destIndex] = mInverseMassMatrixConeLimit[srcIndex];
     mBConeLimit[destIndex] = mBConeLimit[srcIndex];
     mIsConeLimitViolated[destIndex] = mIsConeLimitViolated[srcIndex];
-    new (mConeLimitLocalAxisBody1 + destIndex) Vector3(mConeLimitLocalAxisBody1[srcIndex]);
-    new (mConeLimitLocalAxisBody2 + destIndex) Vector3(mConeLimitLocalAxisBody2[srcIndex]);
     new (mConeLimitACrossB + destIndex) Vector3(mConeLimitACrossB[srcIndex]);
 
     // Destroy the source component
@@ -229,8 +218,6 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2)
     decimal inverseMassMatrixConeLimit1 = mInverseMassMatrixConeLimit[index1];
     decimal bConeLimit = mBConeLimit[index1];
     bool isConeLimitViolated = mIsConeLimitViolated[index1];
-    Vector3 coneLimitLocalAxisBody1(mConeLimitLocalAxisBody1[index1]);
-    Vector3 coneLimitLocalAxisBody2(mConeLimitLocalAxisBody2[index1]);
     Vector3 coneLimitAcrossB(mConeLimitACrossB[index1]);
 
     // Destroy component 1
@@ -256,8 +243,6 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2)
     mInverseMassMatrixConeLimit[index2] = inverseMassMatrixConeLimit1;
     mBConeLimit[index2] = bConeLimit;
     mIsConeLimitViolated[index2] = isConeLimitViolated;
-    new (mConeLimitLocalAxisBody1 + index2) Vector3(coneLimitLocalAxisBody1);
-    new (mConeLimitLocalAxisBody2 + index2) Vector3(coneLimitLocalAxisBody2);
     new (mConeLimitACrossB + index2) Vector3(coneLimitAcrossB);
 
     // Update the entity to component index mapping
@@ -288,7 +273,5 @@ void BallAndSocketJointComponents::destroyComponent(uint32 index) {
     mBiasVector[index].~Vector3();
     mInverseMassMatrix[index].~Matrix3x3();
     mImpulse[index].~Vector3();
-    mConeLimitLocalAxisBody1[index].~Vector3();
-    mConeLimitLocalAxisBody2[index].~Vector3();
     mConeLimitACrossB[index].~Vector3();
 }

src/constraint/BallAndSocketJoint.cpp

@@ -71,7 +71,7 @@ BallAndSocketJoint::BallAndSocketJoint(Entity entity, PhysicsWorld& world, const
 void BallAndSocketJoint::enableConeLimit(bool isLimitEnabled) {
     mWorld.mBallAndSocketJointsComponents.setIsConeLimitEnabled(mEntity, isLimitEnabled);
 
-    awakeBodies();
+    resetLimits();
 }
 
 // Return true if the cone limit or the joint is enabled
@@ -92,30 +92,10 @@ void BallAndSocketJoint::setConeLimitHalfAngle(decimal coneHalfAngle) {
 
         mWorld.mBallAndSocketJointsComponents.setConeLimitHalfAngle(mEntity, coneHalfAngle);
 
-        awakeBodies();
+        resetLimits();
     }
 }
 
-// Set the normalized cone limit axis of body 1 in local-space of body 1
-/**
- * @param localAxisBody1 The normalized axis for the cone-limit in local-space of body 1
- */
-void BallAndSocketJoint::setConeLimitLocalAxisBody1(const Vector3& localAxisBody1) {
-    mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody1(mEntity, localAxisBody1);
-
-    awakeBodies();
-}
-
-// Set the normalized cone limit axis of body 2 in local-space of body 2
-/**
- * @param localAxisBody1 The normalized axis for the cone-limit in local-space of body 2
- */
-void BallAndSocketJoint::setConeLimitLocalAxisBody2(const Vector3& localAxisBody2) {
-    mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody2(mEntity, localAxisBody2);
-
-    awakeBodies();
-}
-
 // Return the cone angle limit (in radians) from [0; PI]
 /**
  * @return The half-angle (in radians) of the cone-limit
@@ -138,10 +118,22 @@ decimal BallAndSocketJoint::getConeHalfAngle() const {
     const Transform& transformBody2 = mWorld.mTransformComponents.getTransform(body2Entity);
 
     // Convert local-space cone axis of bodies to world-space
-    const Vector3 coneAxisBody1World = transformBody1.getOrientation() * mWorld.mBallAndSocketJointsComponents.getConeLimitLocalAxisBody1(mEntity);
+    const Vector3 r1Local = mWorld.mBallAndSocketJointsComponents.getLocalAnchorPointBody1(mEntity) - mWorld.mRigidBodyComponents.getCenterOfMassLocal(body1Entity);
-    const Vector3 coneAxisBody2World = transformBody2.getOrientation() * mWorld.mBallAndSocketJointsComponents.getConeLimitLocalAxisBody2(mEntity);
+    const Vector3 r2Local = mWorld.mBallAndSocketJointsComponents.getLocalAnchorPointBody2(mEntity) - mWorld.mRigidBodyComponents.getCenterOfMassLocal(body2Entity);
+    const Vector3 r1World = transformBody1.getOrientation() * r1Local;
+    const Vector3 r2World = transformBody2.getOrientation() * r2Local;
 
-    return SolveBallAndSocketJointSystem::computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
+    return SolveBallAndSocketJointSystem::computeCurrentConeHalfAngle(r1World.getUnit(), -r2World.getUnit());
+}
+
+// Reset the limits
+void BallAndSocketJoint::resetLimits() {
+
+    // Reset the accumulated impulses for the limits
+    mWorld.mBallAndSocketJointsComponents.setConeLimitImpulse(mEntity, decimal(0.0));
+
+    // Wake up the two bodies of the joint
+    awakeBodies();
 }
 
 // Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space

src/systems/SolveBallAndSocketJointSystem.cpp

@@ -117,14 +117,12 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
             mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World);
         }
 
-        // Convert local-space cone axis of bodies to world-space
+        const Vector3 r1WorldUnit = r1World.getUnit();
-        const Vector3 coneAxisBody1World = orientationBody1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i];
+        const Vector3 r2WorldUnit = r2World.getUnit();
-        const Vector3 coneAxisBody2World = orientationBody2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i];
+        mBallAndSocketJointComponents.mConeLimitACrossB[i] = r1WorldUnit.cross(-r2WorldUnit);
-
-        mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
 
         // Compute the current angle around the hinge axis
-        decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
+        decimal coneAngle = computeCurrentConeHalfAngle(r1WorldUnit, -r2WorldUnit);
 
         // Check if the cone limit constraints is violated or not
         decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle;
@@ -349,10 +347,10 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
         if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
 
             // Check if the cone limit constraints is violated or not
-            const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i];
+            const Vector3 r1WorldUnit = r1World.getUnit();
-            const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i];
+            const Vector3 r2WorldUnit = r2World.getUnit();
-            mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
+            mBallAndSocketJointComponents.mConeLimitACrossB[i] = r1WorldUnit.cross(-r2WorldUnit);
-            decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
+            decimal coneAngle = computeCurrentConeHalfAngle(r1WorldUnit, -r2WorldUnit);
             decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle;
             mBallAndSocketJointComponents.mIsConeLimitViolated[i] = coneLimitError < 0;
 

testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp

@@ -137,9 +137,7 @@ void BallAndSocketJointScene::createBallAndSocketJoint() {
 
     // Create the joint in the physics world
     mJoint = dynamic_cast<rp3d::BallAndSocketJoint*>(mPhysicsWorld->createJoint(jointInfo));
-    mJoint->setConeLimitLocalAxisBody1(rp3d::Vector3(0, 1, 0));
+    mJoint->setConeLimitHalfAngle(90.0 * rp3d::PI_RP3D / 180.0);
-    mJoint->setConeLimitLocalAxisBody2(rp3d::Vector3(0, 1, 0));
-    mJoint->setConeLimitHalfAngle(45.0 * rp3d::PI_RP3D / 180.0);
     mJoint->enableConeLimit(true);
 
 }