diff --git a/include/reactphysics3d/components/BallAndSocketJointComponents.h b/include/reactphysics3d/components/BallAndSocketJointComponents.h index 34cfc436..52886d94 100644 --- a/include/reactphysics3d/components/BallAndSocketJointComponents.h +++ b/include/reactphysics3d/components/BallAndSocketJointComponents.h @@ -85,6 +85,33 @@ 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; + + /// 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 @@ -181,6 +208,42 @@ 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 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 -------------------- // friend class BroadPhaseSystem; @@ -327,6 +390,90 @@ 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::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 4343426a..d86faae8 100644 --- a/include/reactphysics3d/constraint/BallAndSocketJoint.h +++ b/include/reactphysics3d/constraint/BallAndSocketJoint.h @@ -121,6 +121,27 @@ 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); + + /// 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 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 43b2eec7..c349d522 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) + static decimal computeCurrentConeHalfAngle(const Vector3& coneLimitWorldAxisBody1, const Vector3& coneLimitWorldAxisBody2); + #ifdef IS_RP3D_PROFILING_ENABLED /// Set the profiler @@ -140,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 2a97d5be..d6366ffc 100644 --- a/src/components/BallAndSocketJointComponents.cpp +++ b/src/components/BallAndSocketJointComponents.cpp @@ -37,7 +37,9 @@ 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(decimal) + sizeof(bool) + sizeof(Vector3) + + sizeof(Vector3) + sizeof(Vector3)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -67,6 +69,15 @@ 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); + 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) { @@ -83,6 +94,15 @@ 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)); + 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); @@ -101,6 +121,15 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { mBiasVector = newBiasVector; mInverseMassMatrix = newInverseMassMatrix; mImpulse = newImpulse; + mIsConeLimitEnabled = newIsConeLimitEnabled; + mConeLimitImpulse = newConeLimitImpulse; + mConeLimitHalfAngle = newConeLimitHalfAngle; + mInverseMassMatrixConeLimit = newInverseMassMatrixConeLimit; + mBConeLimit = newBConeLimit; + mIsConeLimitViolated = newIsConeLimitViolated; + mConeLimitLocalAxisBody1 = newConeLimitLocalAxisBody1; + mConeLimitLocalAxisBody2 = newConeLimitLocalAxisBody2; + mConeLimitACrossB = newConeLimitACrossB; } // Add a component @@ -121,6 +150,15 @@ 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); + 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)); @@ -149,6 +187,15 @@ 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]; + 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); @@ -176,6 +223,15 @@ 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]; + 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); @@ -194,6 +250,15 @@ 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; + 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)); @@ -223,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 87a26520..2b63c63c 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -61,6 +61,56 @@ 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); +} + +// 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::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 Vector3 BallAndSocketJoint::getReactionForce(decimal timeStep) const { assert(timeStep > MACHINE_EPSILON); diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index a5afc20b..b7dd3584 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,41 @@ 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 = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); + + // Check if the cone limit constraints is violated or not + decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle; + bool oldIsConeLimitViolated = mBallAndSocketJointComponents.mIsConeLimitViolated[i]; + bool isConeLimitViolated = coneLimitError < 0; + mBallAndSocketJointComponents.mIsConeLimitViolated[i] = isConeLimitViolated; + if (!isConeLimitViolated || isConeLimitViolated != oldIsConeLimitViolated) { + mBallAndSocketJointComponents.mConeLimitImpulse[i] = decimal(0.0); + } + + // If the cone limit is enabled + if (mBallAndSocketJointComponents.mIsConeLimitEnabled[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 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; + } + } + // If warm-starting is not enabled if (!mIsWarmStartingActive) { @@ -152,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]; @@ -214,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); + + } + } } } @@ -316,5 +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(); + } + } } } diff --git a/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp b/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp index 452cfc1f..806e2db0 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,15 @@ 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(45.0 * rp3d::PI_RP3D / 180.0); + mJoint->enableConeLimit(true); + +} + +// Update the scene +void BallAndSocketJointScene::update() { + + SceneDemo::update(); } 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; }; }