From 8f5a7a12cb5b9bfa28697324e43f5f333fbabd4f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 14 Oct 2020 19:42:40 +0200 Subject: [PATCH] Fix issue in position solver of joints (constrained orientations must be used instead of previous orientation when computing inertia tensor) --- src/systems/SolveBallAndSocketJointSystem.cpp | 12 +++++------- src/systems/SolveFixedJointSystem.cpp | 16 +++++++--------- src/systems/SolveHingeJointSystem.cpp | 16 +++++++--------- src/systems/SolveSliderJointSystem.cpp | 16 +++++++--------- 4 files changed, 26 insertions(+), 34 deletions(-) diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index c41388c7..b4902b98 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -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); diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index ced19853..6f31972a 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -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]; diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index 8279664e..738b0192 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -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]; diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index ee47357f..4a6874e5 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -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];