mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working on cone limit for Ball And Socket joint

Browse Source
This commit is contained in:
Daniel Chappuis 2021-12-11 20:59:40 +01:00
parent b6ded4b0ae
commit fa9f187632
8 changed files with 38 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
documentation/UserManual/ReactPhysics3D-UserManual.tex
View File

@ -1571,17 +1571,14 @@ joint = dynamic_cast<BallAndSocketJoint*>(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);

34
include/reactphysics3d/components/BallAndSocketJointComponents.h
View File

@ -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

4
include/reactphysics3d/components/HingeJointComponents.h
View File

@ -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

8
include/reactphysics3d/constraint/BallAndSocketJoint.h
View File

@ -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;

21
src/components/BallAndSocketJointComponents.cpp
View File

@ -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<decimal*>(newConeLimitHalfAngle + nbComponentsToAllocate);
decimal* newBConeLimit = reinterpret_cast<decimal*>(newInverseMassMatrixConeLimit + nbComponentsToAllocate);
bool* newIsConeLimitViolated = reinterpret_cast<bool*>(newBConeLimit + nbComponentsToAllocate);
Vector3* newConeLimitLocalAxisBody1 = reinterpret_cast<Vector3*>(newIsConeLimitViolated + nbComponentsToAllocate);
Vector3* newConeLimitLocalAxisBody2 = reinterpret_cast<Vector3*>(newConeLimitLocalAxisBody1 + nbComponentsToAllocate);
Vector3* newConeLimitACrossB = reinterpret_cast<Vector3*>(newConeLimitLocalAxisBody2 + nbComponentsToAllocate);
Vector3* newConeLimitACrossB = reinterpret_cast<Vector3*>(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();
}

42
src/constraint/BallAndSocketJoint.cpp
View File

@ -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

18
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -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;

4
testbed/scenes/ballandsocketjoint/BallAndSocketJointScene.cpp
View File

@ -137,9 +137,7 @@ void BallAndSocketJointScene::createBallAndSocketJoint() {
// Create the joint in the physics world
mJoint = dynamic_cast<rp3d::BallAndSocketJoint*>(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);
}