Merge branch 'conelimit' into develop
This commit is contained in:
Daniel Chappuis
commit
de86e9cbbd
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Vector3*>(newI2 + nbComponentsToAllocate);
|
||||
Matrix3x3* newInverseMassMatrix = reinterpret_cast<Matrix3x3*>(newBiasVector + nbComponentsToAllocate);
|
||||
Vector3* newImpulse = reinterpret_cast<Vector3*>(newInverseMassMatrix + nbComponentsToAllocate);
|
||||
bool* newIsConeLimitEnabled = reinterpret_cast<bool*>(newImpulse + nbComponentsToAllocate);
|
||||
decimal* newConeLimitImpulse = reinterpret_cast<decimal*>(newIsConeLimitEnabled + nbComponentsToAllocate);
|
||||
decimal* newConeLimitHalfAngle = reinterpret_cast<decimal*>(newConeLimitImpulse + 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);
|
||||
|
||||
// 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<Entity, uint32>(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<Entity, uint32>(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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
#include <reactphysics3d/systems/SolveBallAndSocketJointSystem.h>
|
||||
#include <reactphysics3d/engine/PhysicsWorld.h>
|
||||
#include <reactphysics3d/body/RigidBody.h>
|
||||
#include <cmath>
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<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->enableConeLimit(true);
|
||||
|
||||
}
|
||||
|
||||
// Update the scene
|
||||
void BallAndSocketJointScene::update() {
|
||||
|
||||
SceneDemo::update();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -74,6 +74,9 @@ class BallAndSocketJointScene : public SceneDemo {
|
|||
|
||||
/// Reset the scene
|
||||
virtual void reset() override;
|
||||
|
||||
/// Update the scene
|
||||
virtual void update() override;
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user