From 67d480c4ee30e3d57717aeb4c4a40b96c5fdd6c2 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 24 Jun 2021 23:30:08 +0200 Subject: [PATCH] Working on cone limit for Ball and Socket joint --- .../components/BallAndSocketJointComponents.h | 56 ++++++- .../constraint/BallAndSocketJoint.h | 12 +- .../systems/SolveBallAndSocketJointSystem.h | 11 +- .../BallAndSocketJointComponents.cpp | 36 ++++- src/constraint/BallAndSocketJoint.cpp | 26 +++- src/systems/SolveBallAndSocketJointSystem.cpp | 145 +++++++++++++----- .../BallAndSocketJointScene.cpp | 20 ++- .../BallAndSocketJointScene.h | 3 + 8 files changed, 260 insertions(+), 49 deletions(-) diff --git a/include/reactphysics3d/components/BallAndSocketJointComponents.h b/include/reactphysics3d/components/BallAndSocketJointComponents.h index 73412222..52886d94 100644 --- a/include/reactphysics3d/components/BallAndSocketJointComponents.h +++ b/include/reactphysics3d/components/BallAndSocketJointComponents.h @@ -97,9 +97,21 @@ class BallAndSocketJointComponents : public Components { /// Inverse of mass matrix K=JM^-1J^t for the cone limit decimal* mInverseMassMatrixConeLimit; + /// Bias of the cone limit constraint + decimal* mBConeLimit; + /// 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; + // -------------------- Methods -------------------- // /// Allocate memory for a given number of components @@ -218,7 +230,19 @@ class BallAndSocketJointComponents : public Components { bool getInverseMassMatrixConeLimit(Entity jointEntity) const; /// Set the inverse mass matrix cone limit - void setInverseMassMatrixCone(Entity jointEntity, decimal inverseMassMatrix); + void setInverseMassMatrixConeLimit(Entity jointEntity, decimal inverseMassMatrix); + + /// Get the cone limit local axis of body 1 + Vector3 getConeLimitLocalAxisBody1(Entity jointEntity) const; + + /// Set the cone limit local axis of body 1 + void setConeLimitLocalAxisBody1(Entity jointEntity, const Vector3& localAxisBody1); + + /// Get the cone limit local axis of body 2 + Vector3 getConeLimitLocalAxisBody2(Entity jointEntity) const; + + /// Set the cone limit local axis of body 2 + void setConeLimitLocalAxisBody2(Entity jointEntity, const Vector3& localAxisBody2); // -------------------- Friendship -------------------- // @@ -416,12 +440,40 @@ RP3D_FORCE_INLINE bool BallAndSocketJointComponents::getInverseMassMatrixConeLim } // Set the inverse mass matrix cone limit -RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrixCone(Entity jointEntity, decimal inverseMassMatrix) { +RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrixConeLimit(Entity jointEntity, decimal inverseMassMatrix) { assert(mMapEntityToComponentIndex.containsKey(jointEntity)); 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 diff --git a/include/reactphysics3d/constraint/BallAndSocketJoint.h b/include/reactphysics3d/constraint/BallAndSocketJoint.h index 8831d887..d86faae8 100644 --- a/include/reactphysics3d/constraint/BallAndSocketJoint.h +++ b/include/reactphysics3d/constraint/BallAndSocketJoint.h @@ -130,11 +130,17 @@ class BallAndSocketJoint : public Joint { /// Set the cone limit half angle void setConeLimitHalfAngle(decimal coneHalfAngle); - /// Return the cone limit half angle + /// 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; - /// Return the current cone angle in radians (in [0, - decimal getConeAngle() const; + /// Return the current cone half angle (in radians) + decimal getConeHalfAngle() 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 dcf60043..c349d522 100644 --- a/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h +++ b/include/reactphysics3d/systems/SolveBallAndSocketJointSystem.h @@ -112,7 +112,7 @@ class SolveBallAndSocketJointSystem { void setIsWarmStartingActive(bool isWarmStartingActive); /// Return the current cone angle (for the cone limit) - decimal computeCurrentConeAngle(uint32 jointIndex) const; + static decimal computeCurrentConeHalfAngle(const Vector3& coneLimitWorldAxisBody1, const Vector3& coneLimitWorldAxisBody2); #ifdef IS_RP3D_PROFILING_ENABLED @@ -143,6 +143,15 @@ RP3D_FORCE_INLINE void SolveBallAndSocketJointSystem::setIsWarmStartingActive(bo mIsWarmStartingActive = isWarmStartingActive; } +// Return the current cone angle (for the cone limit) +/** + * @return The positive cone angle in radian in range [0, PI] + */ +RP3D_FORCE_INLINE decimal SolveBallAndSocketJointSystem::computeCurrentConeHalfAngle(const Vector3& coneLimitWorldAxisBody1, + const Vector3& coneLimitWorldAxisBody2) { + + return std::acos(coneLimitWorldAxisBody1.dot(coneLimitWorldAxisBody2)); +} } diff --git a/src/components/BallAndSocketJointComponents.cpp b/src/components/BallAndSocketJointComponents.cpp index 11c6c6c3..d6366ffc 100644 --- a/src/components/BallAndSocketJointComponents.cpp +++ b/src/components/BallAndSocketJointComponents.cpp @@ -38,7 +38,8 @@ 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(bool)) { + 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); @@ -72,7 +73,11 @@ 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); + decimal* newBConeLimit = reinterpret_cast(newInverseMassMatrixConeLimit + nbComponentsToAllocate); + bool* newIsConeLimitViolated = reinterpret_cast(newBConeLimit + nbComponentsToAllocate); + Vector3* newConeLimitLocalAxisBody1 = reinterpret_cast(newIsConeLimitViolated + nbComponentsToAllocate); + Vector3* newConeLimitLocalAxisBody2 = reinterpret_cast(newConeLimitLocalAxisBody1 + nbComponentsToAllocate); + Vector3* newConeLimitACrossB = reinterpret_cast(newConeLimitLocalAxisBody2 + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -93,7 +98,11 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { memcpy(newConeLimitImpulse, mConeLimitImpulse, mNbComponents * sizeof(decimal)); memcpy(newConeLimitHalfAngle, mConeLimitHalfAngle, mNbComponents * sizeof(decimal)); 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 mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize); @@ -116,7 +125,11 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { mConeLimitImpulse = newConeLimitImpulse; mConeLimitHalfAngle = newConeLimitHalfAngle; mInverseMassMatrixConeLimit = newInverseMassMatrixConeLimit; + mBConeLimit = newBConeLimit; mIsConeLimitViolated = newIsConeLimitViolated; + mConeLimitLocalAxisBody1 = newConeLimitLocalAxisBody1; + mConeLimitLocalAxisBody2 = newConeLimitLocalAxisBody2; + mConeLimitACrossB = newConeLimitACrossB; } // Add a component @@ -141,7 +154,11 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep mConeLimitImpulse[index] = decimal(0.0); mConeLimitHalfAngle[index] = PI_RP3D; 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 mMapEntityToComponentIndex.add(Pair(jointEntity, index)); @@ -174,7 +191,11 @@ void BallAndSocketJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 mConeLimitImpulse[destIndex] = mConeLimitImpulse[srcIndex]; mConeLimitHalfAngle[destIndex] = mConeLimitHalfAngle[srcIndex]; 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 destroyComponent(srcIndex); @@ -206,7 +227,11 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2) decimal coneLimitImpulse1 = mConeLimitImpulse[index1]; decimal coneLimitHalfAngle1 = mConeLimitHalfAngle[index1]; 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 destroyComponent(index1); @@ -229,7 +254,11 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2) mConeLimitImpulse[index2] = coneLimitImpulse1; mConeLimitHalfAngle[index2] = coneLimitHalfAngle1; 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 mMapEntityToComponentIndex.add(Pair(jointEntity1, index2)); @@ -259,4 +288,7 @@ void BallAndSocketJointComponents::destroyComponent(uint32 index) { mBiasVector[index].~Vector3(); mInverseMassMatrix[index].~Matrix3x3(); mImpulse[index].~Vector3(); + mConeLimitLocalAxisBody1[index].~Vector3(); + mConeLimitLocalAxisBody2[index].~Vector3(); + mConeLimitACrossB[index].~Vector3(); } diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 1401432b..2b63c63c 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -79,14 +79,36 @@ void BallAndSocketJoint::setConeLimitHalfAngle(decimal coneHalfAngle) { mWorld.mBallAndSocketJointsComponents.setConeLimitHalfAngle(mEntity, coneHalfAngle); } +// Set the normalized cone limit axis of body 1 in local-space of body 1 +void BallAndSocketJoint::setConeLimitLocalAxisBody1(const Vector3& localAxisBody1) { + mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody1(mEntity, localAxisBody1); +} + +// Set the normalized cone limit axis of body 2 in local-space of body 2 +void BallAndSocketJoint::setConeLimitLocalAxisBody2(const Vector3& localAxisBody2) { + mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody2(mEntity, localAxisBody2); +} + // Return the cone angle limit (in radians) from [0; PI] decimal BallAndSocketJoint::getConeLimitHalfAngle() const { return mWorld.mBallAndSocketJointsComponents.getConeLimitHalfAngle(mEntity); } // Return the current cone angle in radians (in [0, pi]) -decimal BallAndSocketJoint::getConeAngle() const { - ... +decimal BallAndSocketJoint::getConeHalfAngle() const { + + // Get the bodies entities + const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity); + const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity); + + const Transform& transformBody1 = mWorld.mTransformComponents.getTransform(body1Entity); + 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 coneAxisBody2World = transformBody2.getOrientation() * mWorld.mBallAndSocketJointsComponents.getConeLimitLocalAxisBody2(mEntity); + + return SolveBallAndSocketJointSystem::computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); } // Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index dab3d953..b7dd3584 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -117,42 +117,38 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World); } + // Convert local-space cone axis of bodies to world-space + const Vector3 coneAxisBody1World = orientationBody1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[jointIndex]; + const Vector3 coneAxisBody2World = orientationBody2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[jointIndex]; + + mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World); + // Compute the current angle around the hinge axis - decimal coneAngle = computeCurrentConeAngle(jointIndex); + decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); // Check if the cone limit constraints is violated or not - decimal coneLimitError = coneAngle - mBallAndSocketJointComponents.mConeLimitHalfAngle[i]; + decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle; bool oldIsConeLimitViolated = mBallAndSocketJointComponents.mIsConeLimitViolated[i]; - bool isConeLimitViolated = coneAngle > mBallAndSocketJointComponents.mConeLimitHalfAngle[i]; + bool isConeLimitViolated = coneLimitError < 0; mBallAndSocketJointComponents.mIsConeLimitViolated[i] = isConeLimitViolated; - if (!isConeLimitViolated) { + if (!isConeLimitViolated || isConeLimitViolated != oldIsConeLimitViolated) { 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 cone limit + decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) + + mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]); + inverseMassMatrixConeLimit = (inverseMassMatrixConeLimit > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0); + mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = inverseMassMatrixConeLimit; - // 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; - } + // Compute the bias "b" of the lower limit constraint + mBallAndSocketJointComponents.mBConeLimit[i] = decimal(0.0); + if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) { + mBallAndSocketJointComponents.mBConeLimit[i] = biasFactor * coneLimitError; } } @@ -194,15 +190,24 @@ void SolveBallAndSocketJointSystem::warmstart() { const Matrix3x3& i2 = mBallAndSocketJointComponents.mI2[i]; // Compute the impulse P=J^T * lambda for the body 1 - const Vector3 linearImpulseBody1 = -mBallAndSocketJointComponents.mImpulse[i]; - const Vector3 angularImpulseBody1 = mBallAndSocketJointComponents.mImpulse[i].cross(r1World); + Vector3 linearImpulseBody1 = -mBallAndSocketJointComponents.mImpulse[i]; + Vector3 angularImpulseBody1 = mBallAndSocketJointComponents.mImpulse[i].cross(r1World); + + // Compute the impulse P=J^T * lambda for the lower and upper limits constraints + const Vector3 coneLimitImpulse = mBallAndSocketJointComponents.mConeLimitImpulse[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]; + + // Compute the impulse P=J^T * lambda for the cone limit constraint of body 1 + angularImpulseBody1 += coneLimitImpulse; // Apply the impulse to the body 1 v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1; w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); // Compute the impulse P=J^T * lambda for the body 2 - const Vector3 angularImpulseBody2 = -mBallAndSocketJointComponents.mImpulse[i].cross(r2World); + Vector3 angularImpulseBody2 = -mBallAndSocketJointComponents.mImpulse[i].cross(r2World); + + // Compute the impulse P=J^T * lambda for the cone limit constraint of body 2 + angularImpulseBody2 += -coneLimitImpulse; // Apply the impulse to the body to the body 2 v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * mBallAndSocketJointComponents.mImpulse[i]; @@ -256,6 +261,37 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() { // Apply the impulse to the body 2 v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambda; w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); + + // --------------- Limits Constraints --------------- // + + if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) { + + // If the cone limit is violated + if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) { + + // Compute J*v for the cone limit constraine + const decimal JvConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(w1 - w2); + + // Compute the Lagrange multiplier lambda for the cone limit constraint + decimal deltaLambdaConeLimit = mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] * (-JvConeLimit -mBallAndSocketJointComponents.mBConeLimit[i]); + decimal lambdaTemp = mBallAndSocketJointComponents.mConeLimitImpulse[i]; + mBallAndSocketJointComponents.mConeLimitImpulse[i] = std::max(mBallAndSocketJointComponents.mConeLimitImpulse[i] + deltaLambdaConeLimit, decimal(0.0)); + deltaLambdaConeLimit = mBallAndSocketJointComponents.mConeLimitImpulse[i] - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the lower limit constraint of body 1 + const Vector3 angularImpulseBody1 = deltaLambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i]; + + // Apply the impulse to the body 1 + w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1); + + // Compute the impulse P=J^T * lambda for the lower limit constraint of body 2 + const Vector3 angularImpulseBody2 = -deltaLambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i]; + + // Apply the impulse to the body 2 + w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2); + + } + } } } @@ -358,14 +394,51 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { q2 += Quaternion(0, w2) * q2 * decimal(0.5); q2.normalize(); } + + // --------------- Limits Constraints --------------- // + + if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) { + + // Check if the cone limit constraints is violated or not + const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[jointIndex]; + const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[jointIndex]; + mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World); + decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); + decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle; + mBallAndSocketJointComponents.mIsConeLimitViolated[i] = coneLimitError < 0; + + // If the cone limit is violated + if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) { + + // Compute the inverse of the mass matrix K=JM^-1J^t for the cone limit (1x1 matrix) + decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[jointIndex] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) + + mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[jointIndex] * mBallAndSocketJointComponents.mConeLimitACrossB[i]); + mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = (inverseMassMatrixConeLimit > decimal(0.0)) ? + decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0); + + // Compute the Lagrange multiplier lambda for the cone limit constraint + decimal lambdaConeLimit = mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] * (-coneLimitError ); + + // Compute the impulse P=J^T * lambda of body 1 + const Vector3 angularImpulseBody1 = lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i]; + + // Compute the pseudo velocity of body 1 + const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1); + + // Update the body position/orientation of body 1 + q1 += Quaternion(0, w1) * q1 * decimal(0.5); + q1.normalize(); + + // Compute the impulse P=J^T * lambda of body 2 + const Vector3 angularImpulseBody2 = -lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i]; + + // Compute the pseudo velocity of body 2 + const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2); + + // Update the body position/orientation of body 2 + q2 += Quaternion(0, w2) * q2 * decimal(0.5); + q2.normalize(); + } + } } } - -// 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]); -} diff --git a/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp b/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp index 452cfc1f..b26746e4 100644 --- a/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp +++ b/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp @@ -97,7 +97,7 @@ void BallAndSocketJointScene::reset() { SceneDemo::reset(); mBox1->setTransform(rp3d::Transform(rp3d::Vector3(0, 8, 0), rp3d::Quaternion::identity())); - mBox2->setTransform(rp3d::Transform(rp3d::Vector3(4, 4, 4), rp3d::Quaternion::identity())); + mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity())); } // Create the boxes and joints for the Ball-and-Socket joint example @@ -117,8 +117,8 @@ void BallAndSocketJointScene::createBallAndSocketJoint() { // --------------- Create the box 2 --------------- // - mBox2 = new Box(true, Vector3(4, 4, 4), mPhysicsCommon, mPhysicsWorld, mMeshFolderPath); - mBox2->setTransform(rp3d::Transform(rp3d::Vector3(4, 4, 4), rp3d::Quaternion::identity())); + mBox2 = new Box(true, Vector3(4, 8, 4), mPhysicsCommon, mPhysicsWorld, mMeshFolderPath); + mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity())); // Set the box color mBox2->setColor(mObjectColorDemo); @@ -139,4 +139,18 @@ void BallAndSocketJointScene::createBallAndSocketJoint() { // Create the joint in the physics world mJoint = dynamic_cast(mPhysicsWorld->createJoint(jointInfo)); + mJoint->setConeLimitLocalAxisBody1(rp3d::Vector3(0, 1, 0)); + mJoint->setConeLimitLocalAxisBody2(rp3d::Vector3(0, 1, 0)); + mJoint->setConeLimitHalfAngle(10.0 * rp3d::PI_RP3D / 180.0); + mJoint->enableConeLimit(true); + +} + +// Update the scene +void BallAndSocketJointScene::update() { + + SceneDemo::update(); + + std::cout << "Cone half angle: " << (mJoint->getConeHalfAngle() * 180.0 / rp3d::PI_RP3D) << " deg" << std::endl; + } diff --git a/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.h b/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.h index a00b634d..58bff4c5 100644 --- a/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.h +++ b/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.h @@ -74,6 +74,9 @@ class BallAndSocketJointScene : public SceneDemo { /// Reset the scene virtual void reset() override; + + /// Update the scene + virtual void update() override; }; }