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

Fix issue with cone limit of ball-and-socket joint

Browse Source
This commit is contained in:
Daniel Chappuis 2021-07-04 15:42:42 +02:00
parent 4dae9449c9
commit 312a4cc9a8
2 changed files with 7 additions and 7 deletions

2
src/components/BallAndSocketJointComponents.cpp
View File

@ -157,7 +157,7 @@ void BallAndSocketJointComponents::addComponent(Entity jointEntity, bool isSleep
mBConeLimit[index] = decimal(0.0);
mIsConeLimitViolated[index] = false;
new (mConeLimitLocalAxisBody1 + index) Vector3(1, 0, 0);
new (mConeLimitLocalAxisBody2 + 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

12
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -118,8 +118,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
}
// Convert local-space cone axis of bodies to world-space
const Vector3 coneAxisBody1World = orientationBody1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[jointIndex];
const Vector3 coneAxisBody2World = orientationBody2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[jointIndex];
const Vector3 coneAxisBody1World = orientationBody1 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody1[i];
const Vector3 coneAxisBody2World = orientationBody2 * mBallAndSocketJointComponents.mConeLimitLocalAxisBody2[i];
mBallAndSocketJointComponents.mConeLimitACrossB[i] = coneAxisBody1World.cross(coneAxisBody2World);
@ -400,8 +400,8 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
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];
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);
decimal coneLimitError = mBallAndSocketJointComponents.mConeLimitHalfAngle[i] - coneAngle;
@ -411,8 +411,8 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
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]);
decimal inverseMassMatrixConeLimit = mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI1[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]) +
mBallAndSocketJointComponents.mConeLimitACrossB[i].dot(mBallAndSocketJointComponents.mI2[i] * mBallAndSocketJointComponents.mConeLimitACrossB[i]);
mBallAndSocketJointComponents.mInverseMassMatrixConeLimit[i] = (inverseMassMatrixConeLimit > decimal(0.0)) ?
decimal(1.0) / inverseMassMatrixConeLimit : decimal(0.0);