From c7aa6e7e0e7837d175d9fa40b618856b89c07225 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 22 May 2013 23:38:30 +0200 Subject: [PATCH] Start working of the SliderJoint limits --- src/constraint/SliderJoint.cpp | 142 +++++++++++++++++++++++---------- src/constraint/SliderJoint.h | 67 +++++++++++++++- 2 files changed, 164 insertions(+), 45 deletions(-) diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 8d395b31..a9a322a2 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -30,11 +30,23 @@ using namespace reactphysics3d; // Constructor SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) - : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0) { + : Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0), + mImpulseLowerLimit(0), mIsLimitsActive(jointInfo.isLimitsActive), + mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit) { + + assert(mUpperLimit >= 0.0); + assert(mLowerLimit <= 0.0); // Compute the local-space anchor point for each body - mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; - mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + const Transform& transform1 = mBody1->getTransform(); + const Transform& transform2 = mBody2->getTransform(); + mLocalAnchorPointBody1 = transform1.getInverse() * jointInfo.anchorPointWorldSpace; + mLocalAnchorPointBody2 = transform2.getInverse() * jointInfo.anchorPointWorldSpace; + + // Compute the initial orientation difference between the two bodies + mInitOrientationDifference = transform2.getOrientation() * + transform1.getOrientation().getInverse(); + mInitOrientationDifference.normalize(); // Compute the slider axis in local-space of body 1 mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * @@ -54,11 +66,9 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; - // Get the body positions + // Get the bodies positions and orientations const Vector3& x1 = mBody1->getTransform().getPosition(); const Vector3& x2 = mBody2->getTransform().getPosition(); - - // Get the bodies positions and orientations const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation(); const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation(); @@ -74,23 +84,26 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const Vector3 u = x2 + mR2 - x1 - mR1; // Compute the two orthogonal vectors to the slider axis in local-space of body 1 - Vector3 sliderAxisWorld = orientationBody1 * mSliderAxisBody1; - sliderAxisWorld.normalize(); - mN1 = sliderAxisWorld.getOneUnitOrthogonalVector(); - mN2 = sliderAxisWorld.cross(mN1); + mSliderAxisWorld = orientationBody1 * mSliderAxisBody1; + mSliderAxisWorld.normalize(); + mN1 = mSliderAxisWorld.getOneUnitOrthogonalVector(); + mN2 = mSliderAxisWorld.cross(mN1); - // Compute the cross products used in the Jacobian + // Check if the limit constraints are violated or not + decimal lowerLimitError = u.dot(mSliderAxisWorld) - mLowerLimit; + mIsLowerLimitViolated = (lowerLimitError <= 0); + + // Compute the cross products used in the Jacobians mR2CrossN1 = mR2.cross(mN1); mR2CrossN2 = mR2.cross(mN2); + mR2CrossSliderAxis = mR2.cross(mSliderAxisWorld); const Vector3 r1PlusU = mR1 + u; mR1PlusUCrossN1 = (r1PlusU).cross(mN1); mR1PlusUCrossN2 = (r1PlusU).cross(mN2); + mR1PlusUCrossSliderAxis = (r1PlusU).cross(mSliderAxisWorld); // 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); decimal sumInverseMass = 0.0; Vector3 I1R1PlusUCrossN1(0, 0, 0); Vector3 I1R1PlusUCrossN2(0, 0, 0); @@ -106,13 +119,13 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa I2R2CrossN1 = I2 * mR2CrossN1; I2R2CrossN2 = I2 * mR2CrossN2; } - const decimal el11 = sumInverseMass * (n1Dotn1) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + const decimal el11 = sumInverseMass + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + mR2CrossN1.dot(I2R2CrossN1); - const decimal el12 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + const decimal el12 = mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + mR2CrossN1.dot(I2R2CrossN2); - const decimal el21 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + const decimal el21 = mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + mR2CrossN2.dot(I2R2CrossN1); - const decimal el22 = sumInverseMass * (n2Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + const decimal el22 = sumInverseMass + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); mInverseMassMatrixTranslationConstraint.setToZero(); @@ -120,6 +133,16 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); } + // Compute the bias "b" of the translation constraint + mBTranslation.setToZero(); + decimal beta = decimal(0.2); // TODO : Use a constant here + decimal biasFactor = (beta / constraintSolverData.timeStep); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBTranslation.x = u.dot(mN1); + mBTranslation.y = u.dot(mN2); + mBTranslation *= biasFactor; + } + // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) mInverseMassMatrixRotationConstraint.setToZero(); @@ -132,6 +155,27 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); } + + // Compute the bias "b" of the rotation constraint + mBRotation.setToZero(); + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + Quaternion currentOrientationDifference = orientationBody2 * orientationBody1.getInverse(); + currentOrientationDifference.normalize(); + const Quaternion qError = currentOrientationDifference * + mInitOrientationDifference.getInverse(); + mBRotation = biasFactor * decimal(2.0) * qError.getVectorV(); + } + + // Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix) + mInverseMassMatrixLowerLimit = sumInverseMass + + mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis) + + mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis); + + // Compute the bias "b" of the lower limit constraint + mBLowerLimit = 0.0; + if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { + mBLowerLimit = biasFactor * lowerLimitError; + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) @@ -175,10 +219,6 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { // Solve the velocity constraint void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) { - // Get the body positions - const Vector3& x1 = mBody1->getTransform().getPosition(); - const Vector3& x2 = mBody2->getTransform().getPosition(); - // Get the velocities Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1]; Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2]; @@ -200,18 +240,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint mN2.dot(v2) + w2.dot(mR2CrossN2); const Vector2 JvTranslation(el1, el2); - // 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 + mR2 - x1 - mR1; - bTranslation.x = biasFactor * deltaV.dot(mN1); - bTranslation.y = biasFactor * deltaV.dot(mN2); - } - // Compute the Lagrange multiplier lambda for the 2 translation constraints - Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation - bTranslation); + Vector2 deltaLambda = mInverseMassMatrixTranslationConstraint * (-JvTranslation -mBTranslation); mImpulseTranslation += deltaLambda; // Compute the impulse P=J^T * lambda for the 2 translation constraints @@ -235,17 +265,8 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint // Compute J*v for the 3 rotation constraints const Vector3 JvRotation = w2 - w1; - // Compute the bias "b" of the rotation 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 3 rotation constraints - Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - bRotation); + Vector3 deltaLambda2 = mInverseMassMatrixRotationConstraint * (-JvRotation - mBRotation); mImpulseRotation += deltaLambda2; // Compute the impulse P=J^T * lambda for the 3 rotation constraints @@ -259,6 +280,41 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint if (mBody2->getIsMotionEnabled()) { w2 += I2 * angularImpulseBody2; } + + // --------------- Limits Constraints --------------- // + + if (mIsLimitsActive) { + + // If the lower limit is violated + if (mIsLowerLimitViolated) { + + // Compute J*v for the lower limit constraint + const decimal JvLowerLimit = mSliderAxisWorld.dot(v2) + mR2CrossSliderAxis.dot(w2) - + mSliderAxisWorld.dot(v1) - mR1PlusUCrossSliderAxis.dot(w1); + + // Compute the Lagrange multiplier lambda for the lower limit constraint + decimal deltaLambdaLower = mInverseMassMatrixLowerLimit * (-JvLowerLimit -mBLowerLimit); + decimal lambdaTemp = mImpulseLowerLimit; + mImpulseLowerLimit = std::max(mImpulseLowerLimit + deltaLambdaLower, decimal(0.0)); + deltaLambdaLower = mImpulseLowerLimit - lambdaTemp; + + // Compute the impulse P=J^T * lambda for the lower limit constraint + Vector3 linearImpulseBody1 = -deltaLambdaLower * mSliderAxisWorld; + Vector3 angularImpulseBody1 = -deltaLambdaLower * mR1PlusUCrossSliderAxis; + Vector3 linearImpulseBody2 = -linearImpulseBody1; + Vector3 angularImpulseBody2 = deltaLambdaLower * mR2CrossSliderAxis; + + // 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 44742a15..1f9bef7c 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -49,13 +49,34 @@ struct SliderJointInfo : public ConstraintInfo { /// Slider axis (in world-space coordinates) Vector3 sliderAxisWorldSpace; - /// Constructor + /// True if the slider limits are active + bool isLimitsActive; + + /// Lower limit + decimal lowerLimit; + + /// Upper limit + decimal upperLimit; + + /// Constructor without limits SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, const Vector3& initSliderAxisWorldSpace) : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), - sliderAxisWorldSpace(initSliderAxisWorldSpace) {} + sliderAxisWorldSpace(initSliderAxisWorldSpace), + isLimitsActive(false), lowerLimit(-1.0), upperLimit(1.0) {} + + /// Constructor with limits + SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, + const Vector3& initAnchorPointWorldSpace, + const Vector3& initSliderAxisWorldSpace, + decimal initLowerLimit, decimal initUpperLimit) + : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), + anchorPointWorldSpace(initAnchorPointWorldSpace), + sliderAxisWorldSpace(initSliderAxisWorldSpace), + isLimitsActive(true), lowerLimit(initLowerLimit), + upperLimit(initUpperLimit) {} }; // Class SliderJoint @@ -77,6 +98,9 @@ class SliderJoint : public Constraint { /// Slider axis (in local-space coordinates of body 1) Vector3 mSliderAxisBody1; + /// Initial orientation difference between the two bodies + Quaternion mInitOrientationDifference; + /// First vector orthogonal to the slider axis local-space of body 1 Vector3 mN1; @@ -95,24 +119,63 @@ class SliderJoint : public Constraint { /// Cross product of r2 and n2 Vector3 mR2CrossN2; + /// Cross product of r2 and the slider axis + Vector3 mR2CrossSliderAxis; + /// Cross product of vector (r1 + u) and n1 Vector3 mR1PlusUCrossN1; /// Cross product of vector (r1 + u) and n2 Vector3 mR1PlusUCrossN2; + /// Cross product of vector (r1 + u) and the slider axis + Vector3 mR1PlusUCrossSliderAxis; + + /// Bias of the 2 translation constraints + Vector2 mBTranslation; + + /// Bias of the 3 rotation constraints + Vector3 mBRotation; + + /// Bias of the lower limit constraint + decimal mBLowerLimit; + /// 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; + /// Inverse of mass matrix K=JM^-1J^t for the lower limit constraint (1x1 matrix) + decimal mInverseMassMatrixLowerLimit; + /// Impulse for the 2 translation constraints Vector2 mImpulseTranslation; /// Impulse for the 3 rotation constraints Vector3 mImpulseRotation; + /// Impulse for the lower limit constraint + decimal mImpulseLowerLimit; + + /// True if the slider limits are active + bool mIsLimitsActive; + + /// Slider axis in world-space coordinates + Vector3 mSliderAxisWorld; + + /// Lower limit + decimal mLowerLimit; + + /// Upper limit + decimal mUpperLimit; + + /// True if the lower limit is violated + bool mIsLowerLimitViolated; + + /// True if the upper limit is violated + bool mIsUpperLimitViolated; + public : // -------------------- Methods -------------------- //