diff --git a/include/reactphysics3d/components/BallAndSocketJointComponents.h b/include/reactphysics3d/components/BallAndSocketJointComponents.h index 2fa16d29..73412222 100644 --- a/include/reactphysics3d/components/BallAndSocketJointComponents.h +++ b/include/reactphysics3d/components/BallAndSocketJointComponents.h @@ -97,6 +97,9 @@ class BallAndSocketJointComponents : public Components { /// Inverse of mass matrix K=JM^-1J^t for the cone limit decimal* mInverseMassMatrixConeLimit; + /// True if the cone limit is violated + bool* mIsConeLimitViolated; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components diff --git a/include/reactphysics3d/constraint/BallAndSocketJoint.h b/include/reactphysics3d/constraint/BallAndSocketJoint.h index 8d56ad88..8831d887 100644 --- a/include/reactphysics3d/constraint/BallAndSocketJoint.h +++ b/include/reactphysics3d/constraint/BallAndSocketJoint.h @@ -132,6 +132,9 @@ class BallAndSocketJoint : public Joint { /// Return the cone limit half angle decimal getConeLimitHalfAngle() const; + + /// Return the current cone angle in radians (in [0, + decimal getConeAngle() 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/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h b/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h index 43b2eec7..dcf60043 100644 --- a/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h +++ b/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h @@ -111,6 +111,9 @@ class SolveBallAndSocketJointSystem { /// Set to true to enable warm starting void setIsWarmStartingActive(bool isWarmStartingActive); + /// Return the current cone angle (for the cone limit) + decimal computeCurrentConeAngle(uint32 jointIndex) const; + #ifdef IS_RP3D_PROFILING_ENABLED /// Set the profiler diff --git a/src/components/BallAndSocketJointComponents.cpp b/src/components/BallAndSocketJointComponents.cpp index fd41176b..11c6c6c3 100644 --- a/src/components/BallAndSocketJointComponents.cpp +++ b/src/components/BallAndSocketJointComponents.cpp @@ -37,7 +37,8 @@ BallAndSocketJointComponents::BallAndSocketJointComponents(MemoryAllocator& allo :Components(allocator, sizeof(Entity) + sizeof(BallAndSocketJoint*) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) + sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) + - sizeof(Matrix3x3) + sizeof(Vector3)) { + sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(bool) + sizeof(decimal) + + sizeof(decimal) + sizeof(decimal) + sizeof(bool)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -71,6 +72,7 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { decimal* newConeLimitImpulse = reinterpret_cast(newIsConeLimitEnabled + nbComponentsToAllocate); decimal* newConeLimitHalfAngle = reinterpret_cast(newConeLimitImpulse + nbComponentsToAllocate); decimal* newInverseMassMatrixConeLimit = reinterpret_cast(newConeLimitHalfAngle + nbComponentsToAllocate); + bool* newIsConeLimitViolated = reinterpret_cast(newInverseMassMatrixConeLimit + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -91,6 +93,7 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newConeLimitImpulse, mConeLimitImpulse, mNbComponents * sizeof(decimal)); memcpy(newConeLimitHalfAngle, mConeLimitHalfAngle, mNbComponents * sizeof(decimal)); memcpy(newInverseMassMatrixConeLimit, mInverseMassMatrixConeLimit, mNbComponents * sizeof(decimal)); + memcpy(newIsConeLimitViolated, mIsConeLimitViolated, mNbComponents * sizeof(bool)); // Deallocate previous memory mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -113,6 +116,7 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { mConeLimitImpulse = newConeLimitImpulse; mConeLimitHalfAngle = newConeLimitHalfAngle; mInverseMassMatrixConeLimit = newInverseMassMatrixConeLimit; + mIsConeLimitViolated = newIsConeLimitViolated; } // Add a component @@ -137,6 +141,7 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep mConeLimitImpulse[index] = decimal(0.0); mConeLimitHalfAngle[index] = PI_RP3D; mInverseMassMatrixConeLimit[index] = decimal(0.0); + mIsConeLimitViolated[index] = false; // Map the entity with the new component lookup index mMapEntityToComponentIndex.add(Pair(jointEntity, index)); @@ -169,6 +174,7 @@ void BallAndSocketJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 mConeLimitImpulse[destIndex] = mConeLimitImpulse[srcIndex]; mConeLimitHalfAngle[destIndex] = mConeLimitHalfAngle[srcIndex]; mInverseMassMatrixConeLimit[destIndex] = mInverseMassMatrixConeLimit[srcIndex]; + mIsConeLimitViolated[destIndex] = mIsConeLimitViolated[srcIndex]; // Destroy the source component destroyComponent(srcIndex); @@ -200,6 +206,7 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2) decimal coneLimitImpulse1 = mConeLimitImpulse[index1]; decimal coneLimitHalfAngle1 = mConeLimitHalfAngle[index1]; decimal inverseMassMatrixConeLimit1 = mInverseMassMatrixConeLimit[index1]; + bool isConeLimitViolated = mIsConeLimitViolated[index1]; // Destroy component 1 destroyComponent(index1); @@ -222,6 +229,7 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2) mConeLimitImpulse[index2] = coneLimitImpulse1; mConeLimitHalfAngle[index2] = coneLimitHalfAngle1; mInverseMassMatrixConeLimit[index2] = inverseMassMatrixConeLimit1; + mIsConeLimitViolated[index2] = isConeLimitViolated; // 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 43326646..1401432b 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -84,6 +84,11 @@ decimal BallAndSocketJoint::getConeLimitHalfAngle() const { return mWorld.mBallAndSocketJointsComponents.getConeLimitHalfAngle(mEntity); } +// Return the current cone angle in radians (in [0, pi]) +decimal BallAndSocketJoint::getConeAngle() const { + ... +} + // 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); diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index a5afc20b..dab3d953 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -27,6 +27,7 @@ #include #include #include +#include using namespace reactphysics3d; @@ -70,8 +71,10 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { mBallAndSocketJointComponents.mI1[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody1]; mBallAndSocketJointComponents.mI2[i] = mRigidBodyComponents.mInverseInertiaTensorsWorld[componentIndexBody2]; - const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation(); - const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); + const Transform& transformBody1 = mTransformComponents.getTransform(body1Entity); + const Transform& transformBody2 = mTransformComponents.getTransform(body2Entity); + const Quaternion& orientationBody1 = transformBody1.getOrientation(); + const Quaternion& orientationBody2 = transformBody2.getOrientation(); // Compute the vector from body center to the anchor point in world-space mBallAndSocketJointComponents.mR1World[i] = orientationBody1 * mBallAndSocketJointComponents.mLocalAnchorPointBody1[i]; @@ -114,6 +117,45 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World); } + // Compute the current angle around the hinge axis + decimal coneAngle = computeCurrentConeAngle(jointIndex); + + // Check if the cone limit constraints is violated or not + decimal coneLimitError = coneAngle - mBallAndSocketJointComponents.mConeLimitHalfAngle[i]; + bool oldIsConeLimitViolated = mBallAndSocketJointComponents.mIsConeLimitViolated[i]; + bool isConeLimitViolated = coneAngle > mBallAndSocketJointComponents.mConeLimitHalfAngle[i]; + mBallAndSocketJointComponents.mIsConeLimitViolated[i] = isConeLimitViolated; + if (!isConeLimitViolated) { + mBallAndSocketJointComponents.mConeLimitImpulse[i] = decimal(0.0); + } + + // If the cone limit is enabled + if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) { + + Vector3& a1 = mHingeJointComponents.mA1[i]; + + // Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix) + decimal inverseMassMatrixLimitMotor = a1.dot(mHingeJointComponents.mI1[i] * a1) + a1.dot(mHingeJointComponents.mI2[i] * a1); + inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0); + mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = inverseMassMatrixLimitMotor; + + if (mHingeJointComponents.mIsLimitEnabled[i]) { + + // Compute the bias "b" of the lower limit constraint + mHingeJointComponents.mBLowerLimit[i] = decimal(0.0); + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + mHingeJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError; + } + + // Compute the bias "b" of the upper limit constraint + mHingeJointComponents.mBUpperLimit[i] = decimal(0.0); + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + mHingeJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError; + } + } + } + // If warm-starting is not enabled if (!mIsWarmStartingActive) { @@ -318,3 +360,12 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { } } } + +// Return the current cone angle (for the cone limit) +/** + * @return The positive cone angle in radian in range [0, PI] + */ +decimal SolveBallAndSocketJointSystem::computeCurrentConeAngle(uint32 jointIndex) const { + + return std::acos(-mBallAndSocketJointComponents.mR1World[jointIndex], mBallAndSocketJointComponents.mR2World[jointIndex]); +}