diff --git a/src/systems/SolveBallAndSocketJointSystem.cpp b/src/systems/SolveBallAndSocketJointSystem.cpp index 31a4df8d..d7529f11 100644 --- a/src/systems/SolveBallAndSocketJointSystem.cpp +++ b/src/systems/SolveBallAndSocketJointSystem.cpp @@ -77,8 +77,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() { 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]; - mBallAndSocketJointComponents.mR2World[i] = orientationBody2 * mBallAndSocketJointComponents.mLocalAnchorPointBody2[i]; + mBallAndSocketJointComponents.mR1World[i] = orientationBody1 * (mBallAndSocketJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); + mBallAndSocketJointComponents.mR2World[i] = orientationBody2 * (mBallAndSocketJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); // Compute the corresponding skew-symmetric matrices const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; @@ -329,9 +329,9 @@ void SolveBallAndSocketJointSystem::solvePositionConstraint() { // Compute the vector from body center to the anchor point in world-space mBallAndSocketJointComponents.mR1World[i] = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1] * - mBallAndSocketJointComponents.mLocalAnchorPointBody1[i]; + (mBallAndSocketJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); mBallAndSocketJointComponents.mR2World[i] = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2] * - mBallAndSocketJointComponents.mLocalAnchorPointBody2[i]; + (mBallAndSocketJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); const Vector3& r1World = mBallAndSocketJointComponents.mR1World[i]; const Vector3& r2World = mBallAndSocketJointComponents.mR2World[i]; diff --git a/src/systems/SolveFixedJointSystem.cpp b/src/systems/SolveFixedJointSystem.cpp index 9ce77d30..51c7ece4 100644 --- a/src/systems/SolveFixedJointSystem.cpp +++ b/src/systems/SolveFixedJointSystem.cpp @@ -74,8 +74,8 @@ void SolveFixedJointSystem::initBeforeSolve() { const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Compute the vector from body center to the anchor point in world-space - mFixedJointComponents.mR1World[i] = orientationBody1 * mFixedJointComponents.mLocalAnchorPointBody1[i]; - mFixedJointComponents.mR2World[i] = orientationBody2 * mFixedJointComponents.mLocalAnchorPointBody2[i]; + mFixedJointComponents.mR1World[i] = orientationBody1 * (mFixedJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); + mFixedJointComponents.mR2World[i] = orientationBody2 * (mFixedJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]); @@ -318,8 +318,8 @@ void SolveFixedJointSystem::solvePositionConstraint() { 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]; + mFixedJointComponents.mR1World[i] = q1 * (mFixedJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); + mFixedJointComponents.mR2World[i] = q2 * (mFixedJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1]; diff --git a/src/systems/SolveHingeJointSystem.cpp b/src/systems/SolveHingeJointSystem.cpp index 6926fb12..c1e76a65 100644 --- a/src/systems/SolveHingeJointSystem.cpp +++ b/src/systems/SolveHingeJointSystem.cpp @@ -74,8 +74,8 @@ void SolveHingeJointSystem::initBeforeSolve() { const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Compute the vector from body center to the anchor point in world-space - mHingeJointComponents.mR1World[i] = orientationBody1 * mHingeJointComponents.mLocalAnchorPointBody1[i]; - mHingeJointComponents.mR2World[i] = orientationBody2 * mHingeJointComponents.mLocalAnchorPointBody2[i]; + mHingeJointComponents.mR1World[i] = orientationBody1 * (mHingeJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); + mHingeJointComponents.mR2World[i] = orientationBody2 * (mHingeJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); // Compute vectors needed in the Jacobian Vector3& a1 = mHingeJointComponents.mA1[i]; @@ -497,8 +497,8 @@ void SolveHingeJointSystem::solvePositionConstraint() { 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]; + mHingeJointComponents.mR1World[i] = q1 * (mHingeJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); + mHingeJointComponents.mR2World[i] = q2 * (mHingeJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); // Compute the corresponding skew-symmetric matrices Matrix3x3 skewSymmetricMatrixU1 = Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mHingeJointComponents.mR1World[i]); diff --git a/src/systems/SolveSliderJointSystem.cpp b/src/systems/SolveSliderJointSystem.cpp index 2d1c9d92..1128fca6 100644 --- a/src/systems/SolveSliderJointSystem.cpp +++ b/src/systems/SolveSliderJointSystem.cpp @@ -74,8 +74,8 @@ void SolveSliderJointSystem::initBeforeSolve() { const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation(); // Vector from body center to the anchor point - mSliderJointComponents.mR1[i] = orientationBody1 * mSliderJointComponents.mLocalAnchorPointBody1[i]; - mSliderJointComponents.mR2[i] = orientationBody2 * mSliderJointComponents.mLocalAnchorPointBody2[i]; + mSliderJointComponents.mR1[i] = orientationBody1 * (mSliderJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); + mSliderJointComponents.mR2[i] = orientationBody2 * (mSliderJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); // Compute the two orthogonal vectors to the slider axis in world-space mSliderJointComponents.mSliderAxisWorld[i] = orientationBody1 * mSliderJointComponents.mSliderAxisBody1[i]; @@ -545,8 +545,8 @@ void SolveSliderJointSystem::solvePositionConstraint() { 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]; + mSliderJointComponents.mR1[i] = q1 * (mSliderJointComponents.mLocalAnchorPointBody1[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody1]); + mSliderJointComponents.mR2[i] = q2 * (mSliderJointComponents.mLocalAnchorPointBody2[i] - mRigidBodyComponents.mCentersOfMassLocal[componentIndexBody2]); // Get the inverse mass and inverse inertia tensors of the bodies const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];