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

Fix issue in position solver of joints (constrained orientations must be used instead of previous orientation when computing inertia tensor)

Browse Source
This commit is contained in:
Daniel Chappuis 2020-10-14 19:42:40 +02:00
parent 9be56213e0
commit 8f5a7a12cb
4 changed files with 26 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -234,13 +234,14 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
// Recompute the world inverse inertia tensors
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
RigidBody::computeWorldInertiaTensorInverse(q1.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
mBallAndSocketJointComponents.mI1[i]);
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
RigidBody::computeWorldInertiaTensorInverse(q2.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
mBallAndSocketJointComponents.mI2[i]);
// Compute the vector from body center to the anchor point in world-space
@ -292,9 +293,6 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() {
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mBallAndSocketJointComponents.mI1[i] * angularImpulseBody1;
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
// Update the body center of mass and orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);

16
src/systems/SolveFixedJointSystem.cpp
View File

@ -297,15 +297,6 @@ void SolveFixedJointSystem::solvePositionConstraint() {
const Entity body1Entity = mJointComponents.mBody1Entities[jointIndex];
const Entity body2Entity = mJointComponents.mBody2Entities[jointIndex];
// Recompute the world inverse inertia tensors
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
mFixedJointComponents.mI1[i]);
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
mFixedJointComponents.mI2[i]);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
@ -313,6 +304,13 @@ void SolveFixedJointSystem::solvePositionConstraint() {
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
// Recompute the world inverse inertia tensors
RigidBody::computeWorldInertiaTensorInverse(q1.getMatrix(), mRigidBodyComponents.getInertiaTensorLocalInverse(body1Entity),
mFixedJointComponents.mI1[i]);
RigidBody::computeWorldInertiaTensorInverse(q2.getMatrix(), mRigidBodyComponents.getInertiaTensorLocalInverse(body2Entity),
mFixedJointComponents.mI2[i]);
// Compute the vector from body center to the anchor point in world-space
mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.mLocalAnchorPointBody1[i];
mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.mLocalAnchorPointBody2[i];

16
src/systems/SolveHingeJointSystem.cpp
View File

@ -480,18 +480,16 @@ void SolveHingeJointSystem::solvePositionConstraint() {
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Recompute the world inverse inertia tensors
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
mHingeJointComponents.mI1[i]);
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
mHingeJointComponents.mI2[i]);
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
// Recompute the world inverse inertia tensors
RigidBody::computeWorldInertiaTensorInverse(q1.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
mHingeJointComponents.mI1[i]);
RigidBody::computeWorldInertiaTensorInverse(q2.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
mHingeJointComponents.mI2[i]);
// Compute the vector from body center to the anchor point in world-space
mHingeJointComponents.mR1World[i] = q1 * mHingeJointComponents.mLocalAnchorPointBody1[i];
mHingeJointComponents.mR2World[i] = q2 * mHingeJointComponents.mLocalAnchorPointBody2[i];

16
src/systems/SolveSliderJointSystem.cpp
View File

@ -528,18 +528,16 @@ void SolveSliderJointSystem::solvePositionConstraint() {
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Recompute the world inverse inertia tensors
const Matrix3x3 orientation1 = mTransformComponents.getTransform(body1Entity).getOrientation().getMatrix();
RigidBody::computeWorldInertiaTensorInverse(orientation1, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
mSliderJointComponents.mI1[i]);
const Matrix3x3 orientation2 = mTransformComponents.getTransform(body2Entity).getOrientation().getMatrix();
RigidBody::computeWorldInertiaTensorInverse(orientation2, mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
mSliderJointComponents.mI2[i]);
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
// Recompute the world inverse inertia tensors
RigidBody::computeWorldInertiaTensorInverse(q1.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody1],
mSliderJointComponents.mI1[i]);
RigidBody::computeWorldInertiaTensorInverse(q2.getMatrix(), mRigidBodyComponents.mInverseInertiaTensorsLocal[componentIndexBody2],
mSliderJointComponents.mI2[i]);
// Vector from body center to the anchor point
mSliderJointComponents.mR1[i] = q1 * mSliderJointComponents.mLocalAnchorPointBody1[i];
mSliderJointComponents.mR2[i] = q2 * mSliderJointComponents.mLocalAnchorPointBody2[i];