diff --git a/include/reactphysics3d/components/BallAndSocketJointComponents.h b/include/reactphysics3d/components/BallAndSocketJointComponents.h index 34cfc436..2fa16d29 100644 --- a/include/reactphysics3d/components/BallAndSocketJointComponents.h +++ b/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 diff --git a/include/reactphysics3d/constraint/BallAndSocketJoint.h b/include/reactphysics3d/constraint/BallAndSocketJoint.h index 4343426a..8d56ad88 100644 --- a/include/reactphysics3d/constraint/BallAndSocketJoint.h +++ b/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; diff --git a/src/components/BallAndSocketJointComponents.cpp b/src/components/BallAndSocketJointComponents.cpp index 2a97d5be..fd41176b 100644 --- a/src/components/BallAndSocketJointComponents.cpp +++ b/src/components/BallAndSocketJointComponents.cpp @@ -67,6 +67,10 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { Vector3* newBiasVector = reinterpret_cast(newI2 + nbComponentsToAllocate); Matrix3x3* newInverseMassMatrix = reinterpret_cast(newBiasVector + nbComponentsToAllocate); Vector3* newImpulse = reinterpret_cast(newInverseMassMatrix + nbComponentsToAllocate); + bool* newIsConeLimitEnabled = reinterpret_cast(newImpulse + nbComponentsToAllocate); + decimal* newConeLimitImpulse = reinterpret_cast(newIsConeLimitEnabled + nbComponentsToAllocate); + decimal* newConeLimitHalfAngle = reinterpret_cast(newConeLimitImpulse + nbComponentsToAllocate); + decimal* newInverseMassMatrixConeLimit = reinterpret_cast(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(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(jointEntity1, index2)); diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 87a26520..43326646 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/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);