diff --git a/documentation/UserManual/ReactPhysics3D-UserManual.tex b/documentation/UserManual/ReactPhysics3D-UserManual.tex index 7cb5daa0..2586d4c7 100644 --- a/documentation/UserManual/ReactPhysics3D-UserManual.tex +++ b/documentation/UserManual/ReactPhysics3D-UserManual.tex @@ -1571,17 +1571,14 @@ joint = dynamic_cast(world->createJoint(jointInfo)); \subsubsection{Cone limit} - With the ball and socket joint, it is possible to enable a cone limit. The idea is to constrain the rotation of body 2 around the anchor point - in to stay inside a cone. This can be used to limit the rotation angle of body 2 with respect to the main cone axis. \\ - - In the following example, we enable the cone limit for a given ball and socket joint. We limit the rotation with a maximum angle with respect to - the main axis of the cone (here the local-space Y axis of each body). \\ + With the ball and socket joint, it is possible to enable a cone limit. Let's call the \texttt{anchor axis} for body 1 the axis from body 1 center of mass + to the joint anchor point. The cone limit can be used to constraint the angle between the anchor axis of body 1 and the anchor axis of body 2. + The idea is to limit the angle of the anchor axis of body 2 inside a cone around the anchor axis of body 1. The cone is defined by its main axis + which is the anchor axis of body 1 and is also defined by the cone half angle. \\ + + In the following example, we enable the cone limit for a given ball and socket joint. \\ \begin{lstlisting} -// Set the main axis of the cone (in both local-spaces of body 1 and 2) -joint->setConeLimitLocalAxisBody1(rp3d::Vector3(0, 1, 0)); -joint->setConeLimitLocalAxisBody2(rp3d::Vector3(0, 1, 0)); - // Set the maximum half angle (in radians) joint->setConeLimitHalfAngle(45.0 * PI / 180.0); diff --git a/include/reactphysics3d/components/BallAndSocketJointComponents.h b/include/reactphysics3d/components/BallAndSocketJointComponents.h index 95725225..2ea7ebbc 100644 --- a/include/reactphysics3d/components/BallAndSocketJointComponents.h +++ b/include/reactphysics3d/components/BallAndSocketJointComponents.h @@ -103,12 +103,6 @@ class BallAndSocketJointComponents : public Components { /// 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; @@ -450,34 +444,6 @@ RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrixConeLim 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/components/HingeJointComponents.h b/include/reactphysics3d/components/HingeJointComponents.h index 7ade0356..ee18db76 100644 --- a/include/reactphysics3d/components/HingeJointComponents.h +++ b/include/reactphysics3d/components/HingeJointComponents.h @@ -140,10 +140,10 @@ class HingeJointComponents : public Components { /// True if the motor of the joint in enabled bool* mIsMotorEnabled; - /// Lower limit (minimum allowed rotation angle in radian) + /// Lower limit (minimum allowed rotation angle in radians) decimal* mLowerLimit; - /// Upper limit (maximum translation distance) + /// Upper limit (maximum allowed rotation angle in radians) decimal* mUpperLimit; /// True if the lower limit is violated diff --git a/include/reactphysics3d/constraint/BallAndSocketJoint.h b/include/reactphysics3d/constraint/BallAndSocketJoint.h index d86faae8..de3ad411 100644 --- a/include/reactphysics3d/constraint/BallAndSocketJoint.h +++ b/include/reactphysics3d/constraint/BallAndSocketJoint.h @@ -107,6 +107,8 @@ class BallAndSocketJoint : public Joint { /// Return the number of bytes used by the joint virtual size_t getSizeInBytes() const override; + /// Reset the limits + void resetLimits(); public : @@ -130,12 +132,6 @@ class BallAndSocketJoint : public Joint { /// 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; diff --git a/src/components/BallAndSocketJointComponents.cpp b/src/components/BallAndSocketJointComponents.cpp index 54e5fd16..a8e201b0 100644 --- a/src/components/BallAndSocketJointComponents.cpp +++ b/src/components/BallAndSocketJointComponents.cpp @@ -38,8 +38,7 @@ 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(decimal) + sizeof(bool) + sizeof(Vector3) + - sizeof(Vector3) + sizeof(Vector3)) { + sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(Vector3)) { // Allocate memory for the components data allocate(INIT_NB_ALLOCATED_COMPONENTS); @@ -75,9 +74,7 @@ void BallAndSocketJointComponents::allocate(uint32 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); + Vector3* newConeLimitACrossB = reinterpret_cast(newIsConeLimitViolated + nbComponentsToAllocate); // If there was already components before if (mNbComponents > 0) { @@ -100,8 +97,6 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { 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 @@ -127,8 +122,6 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) { mInverseMassMatrixConeLimit = newInverseMassMatrixConeLimit; mBConeLimit = newBConeLimit; mIsConeLimitViolated = newIsConeLimitViolated; - mConeLimitLocalAxisBody1 = newConeLimitLocalAxisBody1; - mConeLimitLocalAxisBody2 = newConeLimitLocalAxisBody2; mConeLimitACrossB = newConeLimitACrossB; } @@ -156,8 +149,6 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep 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 @@ -193,8 +184,6 @@ void BallAndSocketJointComponents::moveComponentToIndex(uint32 srcIndex, uint32 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 @@ -229,8 +218,6 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2) 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 @@ -256,8 +243,6 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2) 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 @@ -288,7 +273,5 @@ 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 60fafce4..e6e4231c 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -71,7 +71,7 @@ BallAndSocketJoint::BallAndSocketJoint(Entity entity, PhysicsWorld& world, const void BallAndSocketJoint::enableConeLimit(bool isLimitEnabled) { mWorld.mBallAndSocketJointsComponents.setIsConeLimitEnabled(mEntity, isLimitEnabled); - awakeBodies(); + resetLimits(); } // Return true if the cone limit or the joint is enabled @@ -92,30 +92,10 @@ void BallAndSocketJoint::setConeLimitHalfAngle(decimal coneHalfAngle) { mWorld.mBallAndSocketJointsComponents.setConeLimitHalfAngle(mEntity, coneHalfAngle); - awakeBodies(); + resetLimits(); } } -// Set the normalized cone limit axis of body 1 in local-space of body 1 -/** - * @param localAxisBody1 The normalized axis for the cone-limit in local-space of body 1 - */ -void BallAndSocketJoint::setConeLimitLocalAxisBody1(const Vector3& localAxisBody1) { - mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody1(mEntity, localAxisBody1); - - awakeBodies(); -} - -// Set the normalized cone limit axis of body 2 in local-space of body 2 -/** - * @param localAxisBody1 The normalized axis for the cone-limit in local-space of body 2 - */ -void BallAndSocketJoint::setConeLimitLocalAxisBody2(const Vector3& localAxisBody2) { - mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody2(mEntity, localAxisBody2); - - awakeBodies(); -} - // Return the cone angle limit (in radians) from [0; PI] /** * @return The half-angle (in radians) of the cone-limit @@ -138,10 +118,22 @@ decimal BallAndSocketJoint::getConeHalfAngle() const { 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); + const Vector3 r1Local = mWorld.mBallAndSocketJointsComponents.getLocalAnchorPointBody1(mEntity) - mWorld.mRigidBodyComponents.getCenterOfMassLocal(body1Entity); + const Vector3 r2Local = mWorld.mBallAndSocketJointsComponents.getLocalAnchorPointBody2(mEntity) - mWorld.mRigidBodyComponents.getCenterOfMassLocal(body2Entity); + const Vector3 r1World = transformBody1.getOrientation() * r1Local; + const Vector3 r2World = transformBody2.getOrientation() * r2Local; - return SolveBallAndSocketJointSystem::computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); + return SolveBallAndSocketJointSystem::computeCurrentConeHalfAngle(r1World.getUnit(), -r2World.getUnit()); +} + +// Reset the limits +void BallAndSocketJoint::resetLimits() { + + // Reset the accumulated impulses for the limits + mWorld.mBallAndSocketJointsComponents.setConeLimitImpulse(mEntity, decimal(0.0)); + + // Wake up the two bodies of the joint + awakeBodies(); } // 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 d7529f11..b18bd0a9 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -117,14 +117,12 @@ 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[i]; - const Vector3 coneAxisBody2World = orientationBody2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i]; - - mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World); + const Vector3 r1WorldUnit = r1World.getUnit(); + const Vector3 r2WorldUnit = r2World.getUnit(); + mBallAndSocketJointComponents.mConeLimitACrossB[i] = r1WorldUnit.cross(-r2WorldUnit); // Compute the current angle around the hinge axis - decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); + decimal coneAngle = computeCurrentConeHalfAngle(r1WorldUnit, -r2WorldUnit); // Check if the cone limit constraints is violated or not decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle; @@ -349,10 +347,10 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) { // Check if the cone limit constraints is violated or not - const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i]; - const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i]; - mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World); - decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World); + const Vector3 r1WorldUnit = r1World.getUnit(); + const Vector3 r2WorldUnit = r2World.getUnit(); + mBallAndSocketJointComponents.mConeLimitACrossB[i] = r1WorldUnit.cross(-r2WorldUnit); + decimal coneAngle = computeCurrentConeHalfAngle(r1WorldUnit, -r2WorldUnit); decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle; mBallAndSocketJointComponents.mIsConeLimitViolated[i] = coneLimitError < 0; diff --git a/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp b/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp index 681dee06..e325f658 100644 --- a/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp +++ b/testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp @@ -137,9 +137,7 @@ 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->setConeLimitHalfAngle(90.0 * rp3d::PI_RP3D / 180.0); mJoint->enableConeLimit(true); }