Working on cone limit for BallAndSocket joint

include/reactphysics3d/components/BallAndSocketJointComponents.h

@@ -85,6 +85,18 @@ class BallAndSocketJointComponents : public Components {
         /// Accumulated impulse
         Vector3* mImpulse;
 
+        /// True if the joint cone limit is enabled
+        bool* mIsConeLimitEnabled;
+
+        /// Cone limit impulse
+        decimal* mConeLimitImpulse;
+
+        /// Cone limit half angle
+        decimal* mConeLimitHalfAngle;
+
+        /// Inverse of mass matrix K=JM^-1J^t for the cone limit
+        decimal* mInverseMassMatrixConeLimit;
+
         // -------------------- Methods -------------------- //
 
         /// Allocate memory for a given number of components
@@ -181,6 +193,30 @@ class BallAndSocketJointComponents : public Components {
         /// Set the accumulated impulse
         void setImpulse(Entity jointEntity, const Vector3& impulse);
 
+        /// Return true if the cone limit is enabled
+        bool getIsConeLimitEnabled(Entity jointEntity) const;
+
+        /// Set to true if the cone limit is enabled
+        void setIsConeLimitEnabled(Entity jointEntity, bool isLimitEnabled);
+
+        /// Return the cone limit impulse
+        bool getConeLimitImpulse(Entity jointEntity) const;
+
+        /// Set the cone limit impulse
+        void setConeLimitImpulse(Entity jointEntity, decimal impulse);
+
+        /// Return the cone limit half angle
+        bool getConeLimitHalfAngle(Entity jointEntity) const;
+
+        /// Set the cone limit half angle
+        void setConeLimitHalfAngle(Entity jointEntity, decimal halfAngle);
+
+        /// Return the inverse mass matrix cone limit
+        bool getInverseMassMatrixConeLimit(Entity jointEntity) const;
+
+        /// Set the inverse mass matrix cone limit
+        void setInverseMassMatrixCone(Entity jointEntity, decimal inverseMassMatrix);
+
         // -------------------- Friendship -------------------- //
 
         friend class BroadPhaseSystem;
@@ -327,6 +363,62 @@ RP3D_FORCE_INLINE void BallAndSocketJointComponents::setImpulse(Entity jointEnti
     mImpulse[mMapEntityToComponentIndex[jointEntity]] = impulse;
 }
 
+// Return true if the cone limit is enabled
+RP3D_FORCE_INLINE bool BallAndSocketJointComponents::getIsConeLimitEnabled(Entity jointEntity) const {
+
+    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
+    return mIsConeLimitEnabled[mMapEntityToComponentIndex[jointEntity]];
+}
+
+// Set to true if the cone limit is enabled
+RP3D_FORCE_INLINE void BallAndSocketJointComponents::setIsConeLimitEnabled(Entity jointEntity, bool isLimitEnabled) {
+
+    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
+    mIsConeLimitEnabled[mMapEntityToComponentIndex[jointEntity]] = isLimitEnabled;
+}
+
+// Return the cone limit impulse
+RP3D_FORCE_INLINE bool BallAndSocketJointComponents::getConeLimitImpulse(Entity jointEntity) const {
+
+    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
+    return mConeLimitImpulse[mMapEntityToComponentIndex[jointEntity]];
+}
+
+// Set the cone limit impulse
+RP3D_FORCE_INLINE void BallAndSocketJointComponents::setConeLimitImpulse(Entity jointEntity, decimal impulse) {
+
+    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
+    mConeLimitImpulse[mMapEntityToComponentIndex[jointEntity]] = impulse;
+}
+
+// Return the cone limit half angle
+RP3D_FORCE_INLINE bool BallAndSocketJointComponents::getConeLimitHalfAngle(Entity jointEntity) const {
+
+    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
+    return mConeLimitHalfAngle[mMapEntityToComponentIndex[jointEntity]];
+}
+
+// Set the cone limit half angle
+RP3D_FORCE_INLINE void BallAndSocketJointComponents::setConeLimitHalfAngle(Entity jointEntity, decimal halfAngle) {
+
+    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
+    mConeLimitHalfAngle[mMapEntityToComponentIndex[jointEntity]] = halfAngle;
+}
+
+// Return the inverse mass matrix cone limit
+RP3D_FORCE_INLINE bool BallAndSocketJointComponents::getInverseMassMatrixConeLimit(Entity jointEntity) const {
+
+    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
+    return mInverseMassMatrixConeLimit[mMapEntityToComponentIndex[jointEntity]];
+}
+
+// Set the inverse mass matrix cone limit
+RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrixCone(Entity jointEntity, decimal inverseMassMatrix) {
+
+    assert(mMapEntityToComponentIndex.containsKey(jointEntity));
+    mInverseMassMatrixConeLimit[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
+}
+
 }
 
 #endif

include/reactphysics3d/constraint/BallAndSocketJoint.h

@@ -121,6 +121,18 @@ class BallAndSocketJoint : public Joint {
         /// Deleted copy-constructor
         BallAndSocketJoint(const BallAndSocketJoint& constraint) = delete;
 
+        /// Enable/disable the cone limit of the joint
+        void enableConeLimit(bool isLimitEnabled);
+
+        /// Return true if the cone limit or the joint is enabled
+        bool isConeLimitEnabled() const;
+
+        /// Set the cone limit half angle
+        void setConeLimitHalfAngle(decimal coneHalfAngle);
+
+        /// Return the cone limit half angle
+        decimal getConeLimitHalfAngle() 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;
 

src/components/BallAndSocketJointComponents.cpp

@@ -67,6 +67,10 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
     Vector3* newBiasVector = reinterpret_cast<Vector3*>(newI2 + nbComponentsToAllocate);
     Matrix3x3* newInverseMassMatrix = reinterpret_cast<Matrix3x3*>(newBiasVector + nbComponentsToAllocate);
     Vector3* newImpulse = reinterpret_cast<Vector3*>(newInverseMassMatrix + nbComponentsToAllocate);
+    bool* newIsConeLimitEnabled = reinterpret_cast<bool*>(newImpulse + nbComponentsToAllocate);
+    decimal* newConeLimitImpulse = reinterpret_cast<decimal*>(newIsConeLimitEnabled + nbComponentsToAllocate);
+    decimal* newConeLimitHalfAngle = reinterpret_cast<decimal*>(newConeLimitImpulse + nbComponentsToAllocate);
+    decimal* newInverseMassMatrixConeLimit = reinterpret_cast<decimal*>(newConeLimitHalfAngle + nbComponentsToAllocate);
 
     // If there was already components before
     if (mNbComponents > 0) {
@@ -83,6 +87,10 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
         memcpy(newBiasVector, mBiasVector, mNbComponents * sizeof(Vector3));
         memcpy(newInverseMassMatrix, mInverseMassMatrix, mNbComponents * sizeof(Matrix3x3));
         memcpy(newImpulse, mImpulse, mNbComponents * sizeof(Vector3));
+        memcpy(newIsConeLimitEnabled, mIsConeLimitEnabled, mNbComponents * sizeof(bool));
+        memcpy(newConeLimitImpulse, mConeLimitImpulse, mNbComponents * sizeof(decimal));
+        memcpy(newConeLimitHalfAngle, mConeLimitHalfAngle, mNbComponents * sizeof(decimal));
+        memcpy(newInverseMassMatrixConeLimit, mInverseMassMatrixConeLimit, mNbComponents * sizeof(decimal));
 
         // Deallocate previous memory
         mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
@@ -101,6 +109,10 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
     mBiasVector = newBiasVector;
     mInverseMassMatrix = newInverseMassMatrix;
     mImpulse = newImpulse;
+    mIsConeLimitEnabled = newIsConeLimitEnabled;
+    mConeLimitImpulse = newConeLimitImpulse;
+    mConeLimitHalfAngle = newConeLimitHalfAngle;
+    mInverseMassMatrixConeLimit = newInverseMassMatrixConeLimit;
 }
 
 // Add a component
@@ -121,6 +133,10 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep
     new (mBiasVector + index) Vector3(0, 0, 0);
     new (mInverseMassMatrix + index) Matrix3x3();
     new (mImpulse + index) Vector3(0, 0, 0);
+    mIsConeLimitEnabled[index] = false;
+    mConeLimitImpulse[index] = decimal(0.0);
+    mConeLimitHalfAngle[index] = PI_RP3D;
+    mInverseMassMatrixConeLimit[index] = decimal(0.0);
 
     // Map the entity with the new component lookup index
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity, index));
@@ -149,6 +165,10 @@ void BallAndSocketJointComponents::moveComponentToIndex(uint32 srcIndex, uint32
     new (mBiasVector + destIndex) Vector3(mBiasVector[srcIndex]);
     new (mInverseMassMatrix + destIndex) Matrix3x3(mInverseMassMatrix[srcIndex]);
     new (mImpulse + destIndex) Vector3(mImpulse[srcIndex]);
+    mIsConeLimitEnabled[destIndex] = mIsConeLimitEnabled[srcIndex];
+    mConeLimitImpulse[destIndex] = mConeLimitImpulse[srcIndex];
+    mConeLimitHalfAngle[destIndex] = mConeLimitHalfAngle[srcIndex];
+    mInverseMassMatrixConeLimit[destIndex] = mInverseMassMatrixConeLimit[srcIndex];
 
     // Destroy the source component
     destroyComponent(srcIndex);
@@ -176,6 +196,10 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2)
     Vector3 biasVector1(mBiasVector[index1]);
     Matrix3x3 inverseMassMatrix1(mInverseMassMatrix[index1]);
     Vector3 impulse1(mImpulse[index1]);
+    bool isConeLimitEnabled1 = mIsConeLimitEnabled[index1];
+    decimal coneLimitImpulse1 = mConeLimitImpulse[index1];
+    decimal coneLimitHalfAngle1 = mConeLimitHalfAngle[index1];
+    decimal inverseMassMatrixConeLimit1 = mInverseMassMatrixConeLimit[index1];
 
     // Destroy component 1
     destroyComponent(index1);
@@ -194,6 +218,10 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2)
     new (mBiasVector + index2) Vector3(biasVector1);
     new (mInverseMassMatrix + index2) Matrix3x3(inverseMassMatrix1);
     new (mImpulse + index2) Vector3(impulse1);
+    mIsConeLimitEnabled[index2] = isConeLimitEnabled1;
+    mConeLimitImpulse[index2] = coneLimitImpulse1;
+    mConeLimitHalfAngle[index2] = coneLimitHalfAngle1;
+    mInverseMassMatrixConeLimit[index2] = inverseMassMatrixConeLimit1;
 
     // Update the entity to component index mapping
     mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2));

src/constraint/BallAndSocketJoint.cpp

@@ -61,6 +61,29 @@ BallAndSocketJoint::BallAndSocketJoint(Entity entity, PhysicsWorld& world, const
     mWorld.mBallAndSocketJointsComponents.setLocalAnchorPointBody2(entity, anchorPointBody2LocalSpace);
 }
 
+// Enable/disable the cone limit of the joint
+void BallAndSocketJoint::enableConeLimit(bool isLimitEnabled) {
+    mWorld.mBallAndSocketJointsComponents.setIsConeLimitEnabled(mEntity, isLimitEnabled);
+}
+
+// Return true if the cone limit or the joint is enabled
+bool BallAndSocketJoint::isConeLimitEnabled() const {
+    return mWorld.mBallAndSocketJointsComponents.getIsConeLimitEnabled(mEntity);
+}
+
+// Set the cone limit half angle
+/**
+ * @param coneHalfAngle The angle of the cone limit (in radian) from [0; PI]
+ */
+void BallAndSocketJoint::setConeLimitHalfAngle(decimal coneHalfAngle) {
+    mWorld.mBallAndSocketJointsComponents.setConeLimitHalfAngle(mEntity, coneHalfAngle);
+}
+
+// Return the cone angle limit (in radians) from [0; PI]
+decimal BallAndSocketJoint::getConeLimitHalfAngle() const {
+    return mWorld.mBallAndSocketJointsComponents.getConeLimitHalfAngle(mEntity);
+}
+
 // 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);