From 0f17bd1b0b417c66de03da4557107a14ab0c171c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 12 May 2013 12:43:07 +0200 Subject: [PATCH] continue working on the slider joint --- src/constraint/BallAndSocketJoint.cpp | 40 +++++++- src/constraint/BallAndSocketJoint.h | 3 + src/constraint/Constraint.h | 3 + src/constraint/ContactPoint.cpp | 5 + src/constraint/ContactPoint.h | 3 + src/constraint/SliderJoint.cpp | 127 ++++++++++++++++++++++++-- src/constraint/SliderJoint.h | 15 +++ src/mathematics/mathematics.h | 2 + 8 files changed, 188 insertions(+), 10 deletions(-) diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index b4fc6575..9b52ef22 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -78,6 +78,38 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS mInverseMassMatrix = massMatrix.getInverse(); } +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -mImpulse; + Vector3 angularImpulseBody1 = mImpulse.cross(mU1World); + Vector3 linearImpulseBody2 = mImpulse; + Vector3 angularImpulseBody2 = -mImpulse.cross(mU2World); + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } +} + // Solve the velocity constraint void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { @@ -94,8 +126,8 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse(); - Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); + Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute J*v Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); @@ -121,11 +153,11 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { v1 += inverseMassBody1 * linearImpulseBody1; - w1 += inverseInertiaTensorBody1 * angularImpulseBody1; + w1 += I1 * angularImpulseBody1; } if (mBody2->getIsMotionEnabled()) { v2 += inverseMassBody2 * linearImpulseBody2; - w2 += inverseInertiaTensorBody2 * angularImpulseBody2; + w2 += I2 * angularImpulseBody2; } } diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 0280c28c..0c48cdad 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -104,6 +104,9 @@ class BallAndSocketJoint : public Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index 58181768..1be00170 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -149,6 +149,9 @@ class Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0; + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0; + /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0; diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 178a5e1d..1f068df3 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -56,6 +56,11 @@ void ContactPoint::initBeforeSolve(const ConstraintSolverData& constraintSolverD } +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void ContactPoint::warmstart(const ConstraintSolverData& constraintSolverData) { + +} + // Solve the velocity constraint void ContactPoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 0047c020..dc66b8b0 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -227,6 +227,9 @@ class ContactPoint : public Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 0d763e9f..73222eef 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -29,7 +29,8 @@ using namespace reactphysics3d; // Constructor -SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) : Constraint(jointInfo) { +SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) + : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0) { // Compute the local-space anchor point for each body mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; @@ -64,17 +65,76 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mN1 = mU1World.getOneUnitOrthogonalVector(); mN2 = mU1World.cross(mN1); - // Compute the cross product used in the Jacobian + // Compute the cross products used in the Jacobian mU1WorldCrossN1 = mN2; mU1WorldCrossN2 = mU1World.cross(mN2); mU2WorldCrossN1 = mU2World.cross(mN1); mU2WorldCrossN2 = mU2World.cross(mN2); - // Compute the mass matrix K=JM^-1J^t for the 2 translation constraints (2x2 matrix) + // Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation + // constraints (2x2 matrix) const decimal n1Dotn1 = mN1.lengthSquare(); const decimal n2Dotn2 = mN2.lengthSquare(); + const decimal n1Dotn2 = mN1.dot(mN2); const decimal sumInverseMass = mBody1->getMassInverse() + mBody2->getMassInverse(); + const Vector3 I1U2CrossN1 = I1 * mU2WorldCrossN1; + const Vector3 I1U2CrossN2 = I1 * mU2WorldCrossN2; + const Vector3 I2U1CrossN1 = I2 * mU1WorldCrossN1; + const Vector3 I2U1CrossN2 = I2 * mU1WorldCrossN2; + const decimal el11 = sumInverseMass * (n1Dotn1) + mU2WorldCrossN1.dot(I1U2CrossN1) + + mU1WorldCrossN1.dot(I2U1CrossN1); + const decimal el12 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN1.dot(I1U2CrossN2) + + mU1WorldCrossN1.dot(I2U1CrossN2); + const decimal el21 = sumInverseMass * (n1Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN1) + + mU1WorldCrossN2.dot(I2U1CrossN1); + const decimal el22 = sumInverseMass * (n2Dotn2) + mU2WorldCrossN2.dot(I1U2CrossN2) + + mU1WorldCrossN2.dot(I2U1CrossN2); + Matrix2x2 matrixKTranslation(el11, el12, el21, el22); + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation + // contraints (3x3 matrix) + mInverseMassMatrixRotationConstraint = I1 + I2; +} + +// Warm start the constraint (apply the previous impulse at the beginning of the step) +void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { + + // Get the velocities + Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; + Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; + Vector3& w1 = constraintSolverData.angularVelocities[mIndexBody1]; + Vector3& w2 = constraintSolverData.angularVelocities[mIndexBody2]; + + // Get the inverse mass and inverse inertia tensors of the bodies + decimal inverseMassBody1 = mBody1->getMassInverse(); + decimal inverseMassBody2 = mBody2->getMassInverse(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - + mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody1 = mU2WorldCrossN1 * mImpulseTranslation.x + + mU2WorldCrossN2 * mImpulseTranslation.y; + Vector3 linearImpulseBody2 = mN1 * mImpulseTranslation.x + + mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * mImpulseTranslation.x - + mU1WorldCrossN2 * mImpulseTranslation.y; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 += -mImpulseRotation; + angularImpulseBody2 += mImpulseRotation; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } } // Solve the velocity constraint @@ -93,10 +153,65 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Get the inverse mass and inverse inertia tensors of the bodies decimal inverseMassBody1 = mBody1->getMassInverse(); decimal inverseMassBody2 = mBody2->getMassInverse(); - Matrix3x3 inverseInertiaTensorBody1 = mBody1->getInertiaTensorInverseWorld(); - Matrix3x3 inverseInertiaTensorBody2 = mBody2->getInertiaTensorInverseWorld(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - // Compute J*v + // Compute J*v for the 2 translation constraints + const decimal el1 = -mN1.dot(v1) + mU2WorldCrossN1.dot(w1) + + mN1.dot(v2) - mU1WorldCrossN1.dot(w2); + const decimal el2 = -mN2.dot(v1) + mU2WorldCrossN2.dot(w1) + + mN2.dot(v2) - mU1WorldCrossN2.dot(w2); + const Vector2 JvTranslation(el1, el2); + + // Compute J*v for the 3 translation constraints + const Vector3 JvRotation = w2 - w1; + + // Compute the bias "b" of the translation constraint + Vector2 bTranslation(0, 0); + decimal beta = decimal(0.2); // TODO : Use a constant here + decimal biasFactor = (beta / constraintSolverData.timeStep); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Vector3 deltaV = x2 + mU2World - x1 - mU1World; + bTranslation.x = biasFactor * deltaV.dot(mN1); + bTranslation.y = biasFactor * deltaV.dot(mN2); + } + + // Compute the bias "b" of the translation constraint + Vector3 bRotation(0, 0, 0); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Quaternion q1 = mBody1->getTransform().getOrientation(); + Quaternion q2 = mBody2->getTransform().getOrientation(); + Quaternion qDiff = q1 * q2.getInverse(); + bRotation = 2.0 * qDiff.getVectorV(); + } + + // Compute the Lagrange multiplier lambda for the 2 translation constraints + Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation - bTranslation); + mImpulseTranslation += deltaLambda; + + // Compute the Lagrange multiplier lambda for the 3 rotation constraints + Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - bRotation); + mImpulseRotation += deltaLambda2; + + // Compute the impulse P=J^T * lambda for the 2 translation constraints + Vector3 linearImpulseBody1 = -mN1 * deltaLambda.x - mN2 * deltaLambda.y; + Vector3 angularImpulseBody1 = mU2WorldCrossN1 * deltaLambda.x + mU2WorldCrossN2 * deltaLambda.y; + Vector3 linearImpulseBody2 = mN1 * deltaLambda.x + mN2 * deltaLambda.y; + Vector3 angularImpulseBody2 = -mU1WorldCrossN1 * deltaLambda.x -mU1WorldCrossN2 * deltaLambda.y; + + // Compute the impulse P=J^T * lambda for the 3 rotation constraints + angularImpulseBody1 += -deltaLambda2; + angularImpulseBody2 += deltaLambda2; + + // Apply the impulse to the bodies of the joint + if (mBody1->getIsMotionEnabled()) { + v1 += inverseMassBody1 * linearImpulseBody1; + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + v2 += inverseMassBody2 * linearImpulseBody2; + w2 += I2 * angularImpulseBody2; + } } // Solve the position constraint diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index 2d9ae4ec..d4aab31a 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -98,6 +98,18 @@ class SliderJoint : public Constraint { /// Cross product of mU2World and mN2 Vector3 mU2WorldCrossN2; + /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) + Matrix2x2 mInverseMassMatrixTranslationConstraint; + + /// Inverse of mass matrix K=JM^-1J^t for the rotation constraint (3x3 matrix) + Matrix3x3 mInverseMassMatrixRotationConstraint; + + /// Impulse for the 2 translation constraints + Vector2 mImpulseTranslation; + + /// Impulse for the 3 rotation constraints + Vector3 mImpulseRotation; + public : // -------------------- Methods -------------------- // @@ -114,6 +126,9 @@ class SliderJoint : public Constraint { /// Initialize before solving the constraint virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData); + /// Warm start the constraint (apply the previous impulse at the beginning of the step) + virtual void warmstart(const ConstraintSolverData& constraintSolverData); + /// Solve the velocity constraint virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData); diff --git a/src/mathematics/mathematics.h b/src/mathematics/mathematics.h index cc5aa467..3697634f 100644 --- a/src/mathematics/mathematics.h +++ b/src/mathematics/mathematics.h @@ -28,8 +28,10 @@ // Libraries #include "Matrix3x3.h" +#include "Matrix2x2.h" #include "Quaternion.h" #include "Vector3.h" +#include "Vector2.h" #include "Transform.h" #include "../configuration.h" #include "mathematics_functions.h"