Working on cone limit for Ball and Socket joint
This commit is contained in:
Daniel Chappuis
parent
4a802a759c
commit
67d480c4ee
|
|
@ -97,9 +97,21 @@ class BallAndSocketJointComponents : public Components {
|
|||
/// Inverse of mass matrix K=JM^-1J^t for the cone limit
|
||||
decimal* mInverseMassMatrixConeLimit;
|
||||
|
||||
/// Bias of the cone limit constraint
|
||||
decimal* mBConeLimit;
|
||||
|
||||
/// True if the cone limit is violated
|
||||
bool* mIsConeLimitViolated;
|
||||
|
||||
/// Cone limit axis in local-space of body 1
|
||||
Vector3* mConeLimitLocalAxisBody1;
|
||||
|
||||
/// Cone limit axis in local-space of body 2
|
||||
Vector3* mConeLimitLocalAxisBody2;
|
||||
|
||||
/// Cross product of cone limit axis of both bodies
|
||||
Vector3* mConeLimitACrossB;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Allocate memory for a given number of components
|
||||
|
|
@ -218,7 +230,19 @@ class BallAndSocketJointComponents : public Components {
|
|||
bool getInverseMassMatrixConeLimit(Entity jointEntity) const;
|
||||
|
||||
/// Set the inverse mass matrix cone limit
|
||||
void setInverseMassMatrixCone(Entity jointEntity, decimal inverseMassMatrix);
|
||||
void setInverseMassMatrixConeLimit(Entity jointEntity, decimal inverseMassMatrix);
|
||||
|
||||
/// Get the cone limit local axis of body 1
|
||||
Vector3 getConeLimitLocalAxisBody1(Entity jointEntity) const;
|
||||
|
||||
/// Set the cone limit local axis of body 1
|
||||
void setConeLimitLocalAxisBody1(Entity jointEntity, const Vector3& localAxisBody1);
|
||||
|
||||
/// Get the cone limit local axis of body 2
|
||||
Vector3 getConeLimitLocalAxisBody2(Entity jointEntity) const;
|
||||
|
||||
/// Set the cone limit local axis of body 2
|
||||
void setConeLimitLocalAxisBody2(Entity jointEntity, const Vector3& localAxisBody2);
|
||||
|
||||
// -------------------- Friendship -------------------- //
|
||||
|
||||
|
|
@ -416,12 +440,40 @@ RP3D_FORCE_INLINE bool BallAndSocketJointComponents::getInverseMassMatrixConeLim
|
|||
}
|
||||
|
||||
// Set the inverse mass matrix cone limit
|
||||
RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrixCone(Entity jointEntity, decimal inverseMassMatrix) {
|
||||
RP3D_FORCE_INLINE void BallAndSocketJointComponents::setInverseMassMatrixConeLimit(Entity jointEntity, decimal inverseMassMatrix) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mInverseMassMatrixConeLimit[mMapEntityToComponentIndex[jointEntity]] = inverseMassMatrix;
|
||||
}
|
||||
|
||||
// Get the cone limit local axis of body 1
|
||||
RP3D_FORCE_INLINE Vector3 BallAndSocketJointComponents::getConeLimitLocalAxisBody1(Entity jointEntity) const {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mConeLimitLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the cone limit local axis of body 1
|
||||
RP3D_FORCE_INLINE void BallAndSocketJointComponents::setConeLimitLocalAxisBody1(Entity jointEntity, const Vector3& localAxisBody1) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mConeLimitLocalAxisBody1[mMapEntityToComponentIndex[jointEntity]] = localAxisBody1;
|
||||
}
|
||||
|
||||
// Get the cone limit local axis of body 2
|
||||
RP3D_FORCE_INLINE Vector3 BallAndSocketJointComponents::getConeLimitLocalAxisBody2(Entity jointEntity) const {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
return mConeLimitLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]];
|
||||
}
|
||||
|
||||
// Set the cone limit local axis of body 2
|
||||
RP3D_FORCE_INLINE void BallAndSocketJointComponents::setConeLimitLocalAxisBody2(Entity jointEntity, const Vector3& localAxisBody2) {
|
||||
|
||||
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||
mConeLimitLocalAxisBody2[mMapEntityToComponentIndex[jointEntity]] = localAxisBody2;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -130,11 +130,17 @@ class BallAndSocketJoint : public Joint {
|
|||
/// Set the cone limit half angle
|
||||
void setConeLimitHalfAngle(decimal coneHalfAngle);
|
||||
|
||||
/// Return the cone limit half angle
|
||||
/// Set the normalized cone limit axis of body 1 in local-space of body 1
|
||||
void setConeLimitLocalAxisBody1(const Vector3& localAxisBody1);
|
||||
|
||||
/// Set the normalized cone limit axis of body 2 in local-space of body 2
|
||||
void setConeLimitLocalAxisBody2(const Vector3& localAxisBody2);
|
||||
|
||||
/// Return the cone limit half angle (in radians)
|
||||
decimal getConeLimitHalfAngle() const;
|
||||
|
||||
/// Return the current cone angle in radians (in [0,
|
||||
decimal getConeAngle() const;
|
||||
/// Return the current cone half angle (in radians)
|
||||
decimal getConeHalfAngle() const;
|
||||
|
||||
/// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
|
||||
virtual Vector3 getReactionForce(decimal timeStep) const override;
|
||||
|
|
|
|||
|
|
@ -112,7 +112,7 @@ class SolveBallAndSocketJointSystem {
|
|||
void setIsWarmStartingActive(bool isWarmStartingActive);
|
||||
|
||||
/// Return the current cone angle (for the cone limit)
|
||||
decimal computeCurrentConeAngle(uint32 jointIndex) const;
|
||||
static decimal computeCurrentConeHalfAngle(const Vector3& coneLimitWorldAxisBody1, const Vector3& coneLimitWorldAxisBody2);
|
||||
|
||||
#ifdef IS_RP3D_PROFILING_ENABLED
|
||||
|
||||
|
|
@ -143,6 +143,15 @@ RP3D_FORCE_INLINE void SolveBallAndSocketJointSystem::setIsWarmStartingActive(bo
|
|||
mIsWarmStartingActive = isWarmStartingActive;
|
||||
}
|
||||
|
||||
// Return the current cone angle (for the cone limit)
|
||||
/**
|
||||
* @return The positive cone angle in radian in range [0, PI]
|
||||
*/
|
||||
RP3D_FORCE_INLINE decimal SolveBallAndSocketJointSystem::computeCurrentConeHalfAngle(const Vector3& coneLimitWorldAxisBody1,
|
||||
const Vector3& coneLimitWorldAxisBody2) {
|
||||
|
||||
return std::acos(coneLimitWorldAxisBody1.dot(coneLimitWorldAxisBody2));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,8 @@ BallAndSocketJointComponents::BallAndSocketJointComponents(MemoryAllocator& allo
|
|||
sizeof(Vector3) + sizeof(Vector3) + sizeof(Vector3) +
|
||||
sizeof(Matrix3x3) + sizeof(Matrix3x3) + sizeof(Vector3) +
|
||||
sizeof(Matrix3x3) + sizeof(Vector3) + sizeof(bool) + sizeof(decimal) +
|
||||
sizeof(decimal) + sizeof(decimal) + sizeof(bool)) {
|
||||
sizeof(decimal) + sizeof(decimal) + sizeof(decimal) + sizeof(bool) + sizeof(Vector3) +
|
||||
sizeof(Vector3) + sizeof(Vector3)) {
|
||||
|
||||
// Allocate memory for the components data
|
||||
allocate(INIT_NB_ALLOCATED_COMPONENTS);
|
||||
|
|
@ -72,7 +73,11 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
decimal* newConeLimitImpulse = reinterpret_cast<decimal*>(newIsConeLimitEnabled + nbComponentsToAllocate);
|
||||
decimal* newConeLimitHalfAngle = reinterpret_cast<decimal*>(newConeLimitImpulse + nbComponentsToAllocate);
|
||||
decimal* newInverseMassMatrixConeLimit = reinterpret_cast<decimal*>(newConeLimitHalfAngle + nbComponentsToAllocate);
|
||||
bool* newIsConeLimitViolated = reinterpret_cast<bool*>(newInverseMassMatrixConeLimit + 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) {
|
||||
|
|
@ -93,7 +98,11 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
memcpy(newConeLimitImpulse, mConeLimitImpulse, mNbComponents * sizeof(decimal));
|
||||
memcpy(newConeLimitHalfAngle, mConeLimitHalfAngle, mNbComponents * sizeof(decimal));
|
||||
memcpy(newInverseMassMatrixConeLimit, mInverseMassMatrixConeLimit, mNbComponents * sizeof(decimal));
|
||||
memcpy(newBConeLimit, mBConeLimit, mNbComponents * sizeof(decimal));
|
||||
memcpy(newIsConeLimitViolated, mIsConeLimitViolated, mNbComponents * sizeof(bool));
|
||||
memcpy(newConeLimitLocalAxisBody1, mConeLimitLocalAxisBody1, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newConeLimitLocalAxisBody2, mConeLimitLocalAxisBody2, mNbComponents * sizeof(Vector3));
|
||||
memcpy(newConeLimitACrossB, mConeLimitACrossB, mNbComponents * sizeof(Vector3));
|
||||
|
||||
// Deallocate previous memory
|
||||
mMemoryAllocator.release(mBuffer, mNbAllocatedComponents * mComponentDataSize);
|
||||
|
|
@ -116,7 +125,11 @@ void BallAndSocketJointComponents::allocate(uint32 nbComponentsToAllocate) {
|
|||
mConeLimitImpulse = newConeLimitImpulse;
|
||||
mConeLimitHalfAngle = newConeLimitHalfAngle;
|
||||
mInverseMassMatrixConeLimit = newInverseMassMatrixConeLimit;
|
||||
mBConeLimit = newBConeLimit;
|
||||
mIsConeLimitViolated = newIsConeLimitViolated;
|
||||
mConeLimitLocalAxisBody1 = newConeLimitLocalAxisBody1;
|
||||
mConeLimitLocalAxisBody2 = newConeLimitLocalAxisBody2;
|
||||
mConeLimitACrossB = newConeLimitACrossB;
|
||||
}
|
||||
|
||||
// Add a component
|
||||
|
|
@ -141,7 +154,11 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep
|
|||
mConeLimitImpulse[index] = decimal(0.0);
|
||||
mConeLimitHalfAngle[index] = PI_RP3D;
|
||||
mInverseMassMatrixConeLimit[index] = decimal(0.0);
|
||||
mBConeLimit[index] = decimal(0.0);
|
||||
mIsConeLimitViolated[index] = false;
|
||||
new (mConeLimitLocalAxisBody1 + index) Vector3(1, 0, 0);
|
||||
new (mConeLimitLocalAxisBody2 + index) Vector3(-1, 0, 0);
|
||||
new (mConeLimitACrossB + index) Vector3(0, 0, 0);
|
||||
|
||||
// Map the entity with the new component lookup index
|
||||
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity, index));
|
||||
|
|
@ -174,7 +191,11 @@ void BallAndSocketJointComponents::moveComponentToIndex(uint32 srcIndex, uint32
|
|||
mConeLimitImpulse[destIndex] = mConeLimitImpulse[srcIndex];
|
||||
mConeLimitHalfAngle[destIndex] = mConeLimitHalfAngle[srcIndex];
|
||||
mInverseMassMatrixConeLimit[destIndex] = mInverseMassMatrixConeLimit[srcIndex];
|
||||
mBConeLimit[destIndex] = mBConeLimit[srcIndex];
|
||||
mIsConeLimitViolated[destIndex] = mIsConeLimitViolated[srcIndex];
|
||||
new (mConeLimitLocalAxisBody1 + destIndex) Vector3(mConeLimitLocalAxisBody1[srcIndex]);
|
||||
new (mConeLimitLocalAxisBody2 + destIndex) Vector3(mConeLimitLocalAxisBody2[srcIndex]);
|
||||
new (mConeLimitACrossB + destIndex) Vector3(mConeLimitACrossB[srcIndex]);
|
||||
|
||||
// Destroy the source component
|
||||
destroyComponent(srcIndex);
|
||||
|
|
@ -206,7 +227,11 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2)
|
|||
decimal coneLimitImpulse1 = mConeLimitImpulse[index1];
|
||||
decimal coneLimitHalfAngle1 = mConeLimitHalfAngle[index1];
|
||||
decimal inverseMassMatrixConeLimit1 = mInverseMassMatrixConeLimit[index1];
|
||||
decimal bConeLimit = mBConeLimit[index1];
|
||||
bool isConeLimitViolated = mIsConeLimitViolated[index1];
|
||||
Vector3 coneLimitLocalAxisBody1(mConeLimitLocalAxisBody1[index1]);
|
||||
Vector3 coneLimitLocalAxisBody2(mConeLimitLocalAxisBody2[index1]);
|
||||
Vector3 coneLimitAcrossB(mConeLimitACrossB[index1]);
|
||||
|
||||
// Destroy component 1
|
||||
destroyComponent(index1);
|
||||
|
|
@ -229,7 +254,11 @@ void BallAndSocketJointComponents::swapComponents(uint32 index1, uint32 index2)
|
|||
mConeLimitImpulse[index2] = coneLimitImpulse1;
|
||||
mConeLimitHalfAngle[index2] = coneLimitHalfAngle1;
|
||||
mInverseMassMatrixConeLimit[index2] = inverseMassMatrixConeLimit1;
|
||||
mBConeLimit[index2] = bConeLimit;
|
||||
mIsConeLimitViolated[index2] = isConeLimitViolated;
|
||||
new (mConeLimitLocalAxisBody1 + index2) Vector3(coneLimitLocalAxisBody1);
|
||||
new (mConeLimitLocalAxisBody2 + index2) Vector3(coneLimitLocalAxisBody2);
|
||||
new (mConeLimitACrossB + index2) Vector3(coneLimitAcrossB);
|
||||
|
||||
// Update the entity to component index mapping
|
||||
mMapEntityToComponentIndex.add(Pair<Entity, uint32>(jointEntity1, index2));
|
||||
|
|
@ -259,4 +288,7 @@ void BallAndSocketJointComponents::destroyComponent(uint32 index) {
|
|||
mBiasVector[index].~Vector3();
|
||||
mInverseMassMatrix[index].~Matrix3x3();
|
||||
mImpulse[index].~Vector3();
|
||||
mConeLimitLocalAxisBody1[index].~Vector3();
|
||||
mConeLimitLocalAxisBody2[index].~Vector3();
|
||||
mConeLimitACrossB[index].~Vector3();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -79,14 +79,36 @@ void BallAndSocketJoint::setConeLimitHalfAngle(decimal coneHalfAngle) {
|
|||
mWorld.mBallAndSocketJointsComponents.setConeLimitHalfAngle(mEntity, coneHalfAngle);
|
||||
}
|
||||
|
||||
// Set the normalized cone limit axis of body 1 in local-space of body 1
|
||||
void BallAndSocketJoint::setConeLimitLocalAxisBody1(const Vector3& localAxisBody1) {
|
||||
mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody1(mEntity, localAxisBody1);
|
||||
}
|
||||
|
||||
// Set the normalized cone limit axis of body 2 in local-space of body 2
|
||||
void BallAndSocketJoint::setConeLimitLocalAxisBody2(const Vector3& localAxisBody2) {
|
||||
mWorld.mBallAndSocketJointsComponents.setConeLimitLocalAxisBody2(mEntity, localAxisBody2);
|
||||
}
|
||||
|
||||
// Return the cone angle limit (in radians) from [0; PI]
|
||||
decimal BallAndSocketJoint::getConeLimitHalfAngle() const {
|
||||
return mWorld.mBallAndSocketJointsComponents.getConeLimitHalfAngle(mEntity);
|
||||
}
|
||||
|
||||
// Return the current cone angle in radians (in [0, pi])
|
||||
decimal BallAndSocketJoint::getConeAngle() const {
|
||||
...
|
||||
decimal BallAndSocketJoint::getConeHalfAngle() const {
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
|
||||
const Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
|
||||
|
||||
const Transform& transformBody1 = mWorld.mTransformComponents.getTransform(body1Entity);
|
||||
const Transform& transformBody2 = mWorld.mTransformComponents.getTransform(body2Entity);
|
||||
|
||||
// Convert local-space cone axis of bodies to world-space
|
||||
const Vector3 coneAxisBody1World = transformBody1.getOrientation() * mWorld.mBallAndSocketJointsComponents.getConeLimitLocalAxisBody1(mEntity);
|
||||
const Vector3 coneAxisBody2World = transformBody2.getOrientation() * mWorld.mBallAndSocketJointsComponents.getConeLimitLocalAxisBody2(mEntity);
|
||||
|
||||
return SolveBallAndSocketJointSystem::computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
|
||||
}
|
||||
|
||||
// Return the force (in Newtons) on body 2 required to satisfy the joint constraint in world-space
|
||||
|
|
|
|||
|
|
@ -117,42 +117,38 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World);
|
||||
}
|
||||
|
||||
// Convert local-space cone axis of bodies to world-space
|
||||
const Vector3 coneAxisBody1World = orientationBody1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[jointIndex];
|
||||
const Vector3 coneAxisBody2World = orientationBody2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[jointIndex];
|
||||
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
|
||||
|
||||
// Compute the current angle around the hinge axis
|
||||
decimal coneAngle = computeCurrentConeAngle(jointIndex);
|
||||
decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
|
||||
|
||||
// Check if the cone limit constraints is violated or not
|
||||
decimal coneLimitError = coneAngle - mBallAndSocketJointComponents.mConeLimitHalfAngle[i];
|
||||
decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle;
|
||||
bool oldIsConeLimitViolated = mBallAndSocketJointComponents.mIsConeLimitViolated[i];
|
||||
bool isConeLimitViolated = coneAngle > mBallAndSocketJointComponents.mConeLimitHalfAngle[i];
|
||||
bool isConeLimitViolated = coneLimitError < 0;
|
||||
mBallAndSocketJointComponents.mIsConeLimitViolated[i] = isConeLimitViolated;
|
||||
if (!isConeLimitViolated) {
|
||||
if (!isConeLimitViolated || isConeLimitViolated != oldIsConeLimitViolated) {
|
||||
mBallAndSocketJointComponents.mConeLimitImpulse[i] = decimal(0.0);
|
||||
}
|
||||
|
||||
// If the cone limit is enabled
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
|
||||
|
||||
Vector3& a1 = mHingeJointComponents.mA1[i];
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the cone limit
|
||||
decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) +
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]);
|
||||
inverseMassMatrixConeLimit = (inverseMassMatrixConeLimit > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0);
|
||||
mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = inverseMassMatrixConeLimit;
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
|
||||
decimal inverseMassMatrixLimitMotor = a1.dot(mHingeJointComponents.mI1[i] * a1) + a1.dot(mHingeJointComponents.mI2[i] * a1);
|
||||
inverseMassMatrixLimitMotor = (inverseMassMatrixLimitMotor > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixLimitMotor : decimal(0.0);
|
||||
mHingeJointComponents.mInverseMassMatrixLimitMotor[i] = inverseMassMatrixLimitMotor;
|
||||
|
||||
if (mHingeJointComponents.mIsLimitEnabled[i]) {
|
||||
|
||||
// Compute the bias "b" of the lower limit constraint
|
||||
mHingeJointComponents.mBLowerLimit[i] = decimal(0.0);
|
||||
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mHingeJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError;
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the upper limit constraint
|
||||
mHingeJointComponents.mBUpperLimit[i] = decimal(0.0);
|
||||
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mHingeJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError;
|
||||
}
|
||||
// Compute the bias "b" of the lower limit constraint
|
||||
mBallAndSocketJointComponents.mBConeLimit[i] = decimal(0.0);
|
||||
if (mJointComponents.mPositionCorrectionTechniques[jointIndex] == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mBallAndSocketJointComponents.mBConeLimit[i] = biasFactor * coneLimitError;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -194,15 +190,24 @@ void SolveBallAndSocketJointSystem::warmstart() {
|
|||
const Matrix3x3& i2 = mBallAndSocketJointComponents.mI2[i];
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the body 1
|
||||
const Vector3 linearImpulseBody1 = -mBallAndSocketJointComponents.mImpulse[i];
|
||||
const Vector3 angularImpulseBody1 = mBallAndSocketJointComponents.mImpulse[i].cross(r1World);
|
||||
Vector3 linearImpulseBody1 = -mBallAndSocketJointComponents.mImpulse[i];
|
||||
Vector3 angularImpulseBody1 = mBallAndSocketJointComponents.mImpulse[i].cross(r1World);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
|
||||
const Vector3 coneLimitImpulse = mBallAndSocketJointComponents.mConeLimitImpulse[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the cone limit constraint of body 1
|
||||
angularImpulseBody1 += coneLimitImpulse;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += mRigidBodyComponents.mInverseMasses[componentIndexBody1] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody1] * linearImpulseBody1;
|
||||
w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the body 2
|
||||
const Vector3 angularImpulseBody2 = -mBallAndSocketJointComponents.mImpulse[i].cross(r2World);
|
||||
Vector3 angularImpulseBody2 = -mBallAndSocketJointComponents.mImpulse[i].cross(r2World);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the cone limit constraint of body 2
|
||||
angularImpulseBody2 += -coneLimitImpulse;
|
||||
|
||||
// Apply the impulse to the body to the body 2
|
||||
v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * mBallAndSocketJointComponents.mImpulse[i];
|
||||
|
|
@ -256,6 +261,37 @@ void SolveBallAndSocketJointSystem::solveVelocityConstraint() {
|
|||
// Apply the impulse to the body 2
|
||||
v2 += mRigidBodyComponents.mInverseMasses[componentIndexBody2] * mRigidBodyComponents.mLinearLockAxisFactors[componentIndexBody2] * deltaLambda;
|
||||
w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
|
||||
|
||||
// If the cone limit is violated
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) {
|
||||
|
||||
// Compute J*v for the cone limit constraine
|
||||
const decimal JvConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(w1 - w2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the cone limit constraint
|
||||
decimal deltaLambdaConeLimit = mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] * (-JvConeLimit -mBallAndSocketJointComponents.mBConeLimit[i]);
|
||||
decimal lambdaTemp = mBallAndSocketJointComponents.mConeLimitImpulse[i];
|
||||
mBallAndSocketJointComponents.mConeLimitImpulse[i] = std::max(mBallAndSocketJointComponents.mConeLimitImpulse[i] + deltaLambdaConeLimit, decimal(0.0));
|
||||
deltaLambdaConeLimit = mBallAndSocketJointComponents.mConeLimitImpulse[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 angularImpulseBody1 = deltaLambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (i1 * angularImpulseBody1);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 angularImpulseBody2 = -deltaLambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (i2 * angularImpulseBody2);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -358,14 +394,51 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
|
|||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitEnabled[i]) {
|
||||
|
||||
// Check if the cone limit constraints is violated or not
|
||||
const Vector3 coneAxisBody1World = q1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[jointIndex];
|
||||
const Vector3 coneAxisBody2World = q2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[jointIndex];
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
|
||||
decimal coneAngle = computeCurrentConeHalfAngle(coneAxisBody1World, coneAxisBody2World);
|
||||
decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle;
|
||||
mBallAndSocketJointComponents.mIsConeLimitViolated[i] = coneLimitError < 0;
|
||||
|
||||
// If the cone limit is violated
|
||||
if (mBallAndSocketJointComponents.mIsConeLimitViolated[i]) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the cone limit (1x1 matrix)
|
||||
decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[jointIndex] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) +
|
||||
mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[jointIndex] * mBallAndSocketJointComponents.mConeLimitACrossB[i]);
|
||||
mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = (inverseMassMatrixConeLimit > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the cone limit constraint
|
||||
decimal lambdaConeLimit = mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] * (-coneLimitError );
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 1
|
||||
const Vector3 angularImpulseBody1 = lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Compute the pseudo velocity of body 1
|
||||
const Vector3 w1 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody1] * (mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1);
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda of body 2
|
||||
const Vector3 angularImpulseBody2 = -lambdaConeLimit * mBallAndSocketJointComponents.mConeLimitACrossB[i];
|
||||
|
||||
// Compute the pseudo velocity of body 2
|
||||
const Vector3 w2 = mRigidBodyComponents.mAngularLockAxisFactors[componentIndexBody2] * (mBallAndSocketJointComponents.mI2[i] * angularImpulseBody2);
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Return the current cone angle (for the cone limit)
|
||||
/**
|
||||
* @return The positive cone angle in radian in range [0, PI]
|
||||
*/
|
||||
decimal SolveBallAndSocketJointSystem::computeCurrentConeAngle(uint32 jointIndex) const {
|
||||
|
||||
return std::acos(-mBallAndSocketJointComponents.mR1World[jointIndex], mBallAndSocketJointComponents.mR2World[jointIndex]);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -97,7 +97,7 @@ void BallAndSocketJointScene::reset() {
|
|||
SceneDemo::reset();
|
||||
|
||||
mBox1->setTransform(rp3d::Transform(rp3d::Vector3(0, 8, 0), rp3d::Quaternion::identity()));
|
||||
mBox2->setTransform(rp3d::Transform(rp3d::Vector3(4, 4, 4), rp3d::Quaternion::identity()));
|
||||
mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()));
|
||||
}
|
||||
|
||||
// Create the boxes and joints for the Ball-and-Socket joint example
|
||||
|
|
@ -117,8 +117,8 @@ void BallAndSocketJointScene::createBallAndSocketJoint() {
|
|||
|
||||
// --------------- Create the box 2 --------------- //
|
||||
|
||||
mBox2 = new Box(true, Vector3(4, 4, 4), mPhysicsCommon, mPhysicsWorld, mMeshFolderPath);
|
||||
mBox2->setTransform(rp3d::Transform(rp3d::Vector3(4, 4, 4), rp3d::Quaternion::identity()));
|
||||
mBox2 = new Box(true, Vector3(4, 8, 4), mPhysicsCommon, mPhysicsWorld, mMeshFolderPath);
|
||||
mBox2->setTransform(rp3d::Transform(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity()));
|
||||
|
||||
// Set the box color
|
||||
mBox2->setColor(mObjectColorDemo);
|
||||
|
|
@ -139,4 +139,18 @@ void BallAndSocketJointScene::createBallAndSocketJoint() {
|
|||
|
||||
// Create the joint in the physics world
|
||||
mJoint = dynamic_cast<rp3d::BallAndSocketJoint*>(mPhysicsWorld->createJoint(jointInfo));
|
||||
mJoint->setConeLimitLocalAxisBody1(rp3d::Vector3(0, 1, 0));
|
||||
mJoint->setConeLimitLocalAxisBody2(rp3d::Vector3(0, 1, 0));
|
||||
mJoint->setConeLimitHalfAngle(10.0 * rp3d::PI_RP3D / 180.0);
|
||||
mJoint->enableConeLimit(true);
|
||||
|
||||
}
|
||||
|
||||
// Update the scene
|
||||
void BallAndSocketJointScene::update() {
|
||||
|
||||
SceneDemo::update();
|
||||
|
||||
std::cout << "Cone half angle: " << (mJoint->getConeHalfAngle() * 180.0 / rp3d::PI_RP3D) << " deg" << std::endl;
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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