From 78abbaac729038a91cb5e0994a3c23e84b0acd47 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 21 May 2013 23:03:14 +0200 Subject: [PATCH] Fix issues in the BallAndSocketJoint and SliderJoint --- examples/joints/Scene.cpp | 59 ++++++++- examples/joints/Scene.h | 12 ++ src/constraint/BallAndSocketJoint.cpp | 63 ++++++---- src/constraint/BallAndSocketJoint.h | 18 +-- src/constraint/SliderJoint.cpp | 175 ++++++++++++++++---------- src/constraint/SliderJoint.h | 43 ++++--- src/mathematics/Matrix2x2.h | 2 +- src/reactphysics3d.h | 1 + 8 files changed, 253 insertions(+), 120 deletions(-) diff --git a/examples/joints/Scene.cpp b/examples/joints/Scene.cpp index c1b5060b..419f5c24 100644 --- a/examples/joints/Scene.cpp +++ b/examples/joints/Scene.cpp @@ -59,6 +59,9 @@ Scene::Scene(GlutViewer* viewer) : mViewer(viewer), mLight0(0), // Create the Ball-and-Socket joint createBallAndSocketJoints(); + // Create the Slider joint + createSliderJoint(); + // Create the floor createFloor(); @@ -77,12 +80,17 @@ Scene::~Scene() { // Destroy the joints mDynamicsWorld->destroyJoint(mBallAndSocketJoint); + mDynamicsWorld->destroyJoint(mSliderJoint); // Destroy all the boxes of the scene mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox1->getRigidBody()); mDynamicsWorld->destroyRigidBody(mBallAndSocketJointBox2->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mSliderJointBox1->getRigidBody()); + mDynamicsWorld->destroyRigidBody(mSliderJointBox2->getRigidBody()); delete mBallAndSocketJointBox1; delete mBallAndSocketJointBox2; + delete mSliderJointBox1; + delete mSliderJointBox2; // Destroy the floor mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody()); @@ -104,10 +112,11 @@ void Scene::simulate() { // Update the position and orientation of the boxes mBallAndSocketJointBox1->updateTransform(); mBallAndSocketJointBox2->updateTransform(); + mSliderJointBox1->updateTransform(); + mSliderJointBox2->updateTransform(); // Update the position and orientation of the floor mFloor->updateTransform(); - } } @@ -139,6 +148,8 @@ void Scene::render() { // Render all the boxes mBallAndSocketJointBox1->render(mPhongShader); mBallAndSocketJointBox2->render(mPhongShader); + mSliderJointBox1->render(mPhongShader); + mSliderJointBox2->render(mPhongShader); // Render the floor mFloor->render(mPhongShader); @@ -191,6 +202,52 @@ void Scene::createBallAndSocketJoints() { mDynamicsWorld->createJoint(jointInfo)); } +/// Create the boxes and joint for the Slider joint example +void Scene::createSliderJoint() { + + // --------------- Create the first box --------------- // + + // Position of the box + openglframework::Vector3 positionBox1(-4, 6, 0); + + // Create a box and a corresponding rigid in the dynamics world + mSliderJointBox1 = new Box(BOX_SIZE, positionBox1 , BOX_MASS, mDynamicsWorld); + + // The fist box cannot move + mSliderJointBox1->getRigidBody()->setIsMotionEnabled(false); + + // Set the bouncing factor of the box + mSliderJointBox1->getRigidBody()->setRestitution(0.4); + + // --------------- Create the second box --------------- // + + // Position of the box + openglframework::Vector3 positionBox2(2, 4, 0); + + // Create a box and a corresponding rigid in the dynamics world + mSliderJointBox2 = new Box(BOX_SIZE, positionBox2 , BOX_MASS, mDynamicsWorld); + + // The second box is allowed to move + mSliderJointBox2->getRigidBody()->setIsMotionEnabled(true); + + // Set the bouncing factor of the box + mSliderJointBox2->getRigidBody()->setRestitution(0.4); + + // --------------- Create the joint --------------- // + + // Create the joint info object + rp3d::RigidBody* body1 = mSliderJointBox1->getRigidBody(); + rp3d::RigidBody* body2 = mSliderJointBox2->getRigidBody(); + const rp3d::Vector3& body1Position = body1->getTransform().getPosition(); + const rp3d::Vector3& body2Position = body2->getTransform().getPosition(); + const rp3d::Vector3 anchorPointWorldSpace = 0.5 * (body2Position + body1Position); + const rp3d::Vector3 sliderAxisWorldSpace = body2Position - body1Position; + rp3d::SliderJointInfo jointInfo(body1, body2, anchorPointWorldSpace, sliderAxisWorldSpace); + + // Create the joint in the dynamics world + mSliderJoint = dynamic_cast(mDynamicsWorld->createJoint(jointInfo)); +} + // Create the floor void Scene::createFloor() { diff --git a/examples/joints/Scene.h b/examples/joints/Scene.h index 7d89dc3a..3bc5cb5d 100644 --- a/examples/joints/Scene.h +++ b/examples/joints/Scene.h @@ -62,6 +62,15 @@ class Scene { /// Ball-and-Socket joint rp3d::BallAndSocketJoint* mBallAndSocketJoint; + /// Box 1 of Slider joint + Box* mSliderJointBox1; + + /// Box 2 of Slider joint + Box* mSliderJointBox2; + + /// Slider joint + rp3d::SliderJoint* mSliderJoint; + /// Box for the floor Box* mFloor; @@ -76,6 +85,9 @@ class Scene { /// Create the boxes and joints for the Ball-and-Socket joint example void createBallAndSocketJoints(); + /// Create the boxes and joint for the Slider joint example + void createSliderJoint(); + /// Create the floor void createFloor(); diff --git a/src/constraint/BallAndSocketJoint.cpp b/src/constraint/BallAndSocketJoint.cpp index 9b52ef22..aa3409f2 100644 --- a/src/constraint/BallAndSocketJoint.cpp +++ b/src/constraint/BallAndSocketJoint.cpp @@ -59,23 +59,36 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute the vector from body center to the anchor point in world-space - mU1World = orientationBody1 * mLocalAnchorPointBody1; - mU2World = orientationBody2 * mLocalAnchorPointBody2; + mR1World = orientationBody1 * mLocalAnchorPointBody1; + mR2World = orientationBody2 * mLocalAnchorPointBody2; // Compute the corresponding skew-symmetric matrices - Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU1World); - Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mU2World); + Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR1World); + Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mR2World); // Compute the matrix K=JM^-1J^t (3x3 matrix) - decimal inverseMassBodies = mBody1->getMassInverse() + mBody2->getMassInverse(); - Matrix3x3 massMatrix= Matrix3x3(inverseMassBodies, 0, 0, + decimal inverseMassBodies = 0.0; + if (mBody1->getIsMotionEnabled()) { + inverseMassBodies += mBody1->getMassInverse(); + } + if (mBody2->getIsMotionEnabled()) { + inverseMassBodies += mBody2->getMassInverse(); + } + Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, - 0, 0, inverseMassBodies) + - skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose() + - skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + 0, 0, inverseMassBodies); + if (mBody1->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU1 * I1 * skewSymmetricMatrixU1.getTranspose(); + } + if (mBody2->getIsMotionEnabled()) { + massMatrix += skewSymmetricMatrixU2 * I2 * skewSymmetricMatrixU2.getTranspose(); + } // Compute the inverse mass matrix K^-1 - mInverseMassMatrix = massMatrix.getInverse(); + mInverseMassMatrix.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrix = massMatrix.getInverse(); + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) @@ -88,16 +101,16 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD 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(); + const decimal inverseMassBody1 = mBody1->getMassInverse(); + const decimal inverseMassBody2 = mBody2->getMassInverse(); + const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); + const 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); + const Vector3 linearImpulseBody1 = -mImpulse; + const Vector3 angularImpulseBody1 = mImpulse.cross(mR1World); + const Vector3 linearImpulseBody2 = mImpulse; + const Vector3 angularImpulseBody2 = -mImpulse.cross(mR2World); // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { @@ -130,25 +143,25 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); // Compute J*v - Vector3 Jv = -v1 + mU1World.cross(w1) + v2 - mU2World.cross(w2); + const Vector3 Jv = -v1 + mR1World.cross(w1) + v2 - mR2World.cross(w2); // Compute the bias "b" of the constraint Vector3 b(0, 0, 0); if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) { decimal beta = decimal(0.2); // TODO : Use a constant here decimal biasFactor = (beta / constraintSolverData.timeStep); - b = biasFactor * (x2 + mU2World - x1 - mU1World); + b = biasFactor * (x2 + mR2World - x1 - mR1World); } // Compute the Lagrange multiplier lambda - Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); + const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b); mImpulse += deltaLambda; // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -deltaLambda; - Vector3 angularImpulseBody1 = deltaLambda.cross(mU1World); - Vector3 linearImpulseBody2 = deltaLambda; - Vector3 angularImpulseBody2 = -deltaLambda.cross(mU2World); + const Vector3 linearImpulseBody1 = -deltaLambda; + const Vector3 angularImpulseBody1 = deltaLambda.cross(mR1World); + const Vector3 linearImpulseBody2 = deltaLambda; + const Vector3 angularImpulseBody2 = -deltaLambda.cross(mR2World); // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { diff --git a/src/constraint/BallAndSocketJoint.h b/src/constraint/BallAndSocketJoint.h index 0c48cdad..556e316b 100644 --- a/src/constraint/BallAndSocketJoint.h +++ b/src/constraint/BallAndSocketJoint.h @@ -50,7 +50,7 @@ struct BallAndSocketJointInfo : public ConstraintInfo { BallAndSocketJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace) : ConstraintInfo(rigidBody1, rigidBody2, BALLSOCKETJOINT), - anchorPointWorldSpace(initAnchorPointWorldSpace){} + anchorPointWorldSpace(initAnchorPointWorldSpace) {} }; // Class BallAndSocketJoint @@ -64,23 +64,23 @@ class BallAndSocketJoint : public Constraint { // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local space coordinates) + /// Anchor point of body 1 (in local-space coordinates of body 1) Vector3 mLocalAnchorPointBody1; - /// Anchor point of body 2 (in local space coordinates) + /// Anchor point of body 2 (in local-space coordinates of body 2) Vector3 mLocalAnchorPointBody2; /// Vector from center of body 2 to anchor point in world-space - Vector3 mU1World; + Vector3 mR1World; /// Vector from center of body 2 to anchor point in world-space - Vector3 mU2World; + Vector3 mR2World; - /// Skew-Symmetric matrix for cross product with vector mU1World - Matrix3x3 mSkewSymmetricMatrixU1World; + /// Skew-Symmetric matrix for cross product with vector mR1World + Matrix3x3 mSkewSymmetricMatrixR1World; - /// Skew-Symmetric matrix for cross product with vector mU2World - Matrix3x3 mSkewSymmetricMatrixU2World; + /// Skew-Symmetric matrix for cross product with vector mR2World + Matrix3x3 mSkewSymmetricMatrixR2World; /// Inverse mass matrix K=JM^-1J^-t of the constraint Matrix3x3 mInverseMassMatrix; diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index 73222eef..8d395b31 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -35,6 +35,11 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo) // Compute the local-space anchor point for each body mLocalAnchorPointBody1 = mBody1->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; mLocalAnchorPointBody2 = mBody2->getTransform().getInverse() * jointInfo.anchorPointWorldSpace; + + // Compute the slider axis in local-space of body 1 + mSliderAxisBody1 = mBody1->getTransform().getOrientation().getInverse() * + jointInfo.sliderAxisWorldSpace; + mSliderAxisBody1.normalize(); } // Destructor @@ -49,6 +54,10 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second; mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second; + // Get the body positions + 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(); @@ -57,44 +66,72 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - // Compute the vector from body center to the anchor point in world-space - mU1World = orientationBody1 * mLocalAnchorPointBody1; - mU2World = orientationBody2 * mLocalAnchorPointBody2; + // Vector from body center to the anchor point + mR1 = orientationBody1 * mLocalAnchorPointBody1; + mR2 = orientationBody2 * mLocalAnchorPointBody2; - // Compute the two orthogonal vectors to vector mU1World in world-space - mN1 = mU1World.getOneUnitOrthogonalVector(); - mN2 = mU1World.cross(mN1); + // Compute the vector u + 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); // Compute the cross products used in the Jacobian - mU1WorldCrossN1 = mN2; - mU1WorldCrossN2 = mU1World.cross(mN2); - mU2WorldCrossN1 = mU2World.cross(mN1); - mU2WorldCrossN2 = mU2World.cross(mN2); + mR2CrossN1 = mR2.cross(mN1); + mR2CrossN2 = mR2.cross(mN2); + const Vector3 r1PlusU = mR1 + u; + mR1PlusUCrossN1 = (r1PlusU).cross(mN1); + mR1PlusUCrossN2 = (r1PlusU).cross(mN2); // 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); + decimal sumInverseMass = 0.0; + Vector3 I1R1PlusUCrossN1(0, 0, 0); + Vector3 I1R1PlusUCrossN2(0, 0, 0); + Vector3 I2R2CrossN1(0, 0, 0); + Vector3 I2R2CrossN2(0, 0, 0); + if (mBody1->getIsMotionEnabled()) { + sumInverseMass += mBody1->getMassInverse(); + I1R1PlusUCrossN1 = I1 * mR1PlusUCrossN1; + I1R1PlusUCrossN2 = I1 * mR1PlusUCrossN2; + } + if (mBody2->getIsMotionEnabled()) { + sumInverseMass += mBody2->getMassInverse(); + I2R2CrossN1 = I2 * mR2CrossN1; + I2R2CrossN2 = I2 * mR2CrossN2; + } + const decimal el11 = sumInverseMass * (n1Dotn1) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN1) + + mR2CrossN1.dot(I2R2CrossN1); + const decimal el12 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN1.dot(I1R1PlusUCrossN2) + + mR2CrossN1.dot(I2R2CrossN2); + const decimal el21 = sumInverseMass * (n1Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN1) + + mR2CrossN2.dot(I2R2CrossN1); + const decimal el22 = sumInverseMass * (n2Dotn2) + mR1PlusUCrossN2.dot(I1R1PlusUCrossN2) + + mR2CrossN2.dot(I2R2CrossN2); Matrix2x2 matrixKTranslation(el11, el12, el21, el22); - mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + mInverseMassMatrixTranslationConstraint.setToZero(); + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixTranslationConstraint = matrixKTranslation.getInverse(); + } // Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation // contraints (3x3 matrix) - mInverseMassMatrixRotationConstraint = I1 + I2; + mInverseMassMatrixRotationConstraint.setToZero(); + if (mBody1->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint += I1; + } + if (mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint += I2; + } + if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) { + mInverseMassMatrixRotationConstraint = mInverseMassMatrixRotationConstraint.getInverse(); + } } // Warm start the constraint (apply the previous impulse at the beginning of the step) @@ -107,20 +144,18 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) { 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 decimal inverseMassBody1 = mBody1->getMassInverse(); + const 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; + Vector3 linearImpulseBody1 = -mN1 * mImpulseTranslation.x - mN2 * mImpulseTranslation.y; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * mImpulseTranslation.x - + mR1PlusUCrossN2 * mImpulseTranslation.y; + Vector3 linearImpulseBody2 = -linearImpulseBody1; + Vector3 angularImpulseBody2 = mR2CrossN1 * mImpulseTranslation.x + + mR2CrossN2 * mImpulseTranslation.y; // Compute the impulse P=J^T * lambda for the 3 rotation constraints angularImpulseBody1 += -mImpulseRotation; @@ -156,52 +191,34 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint const Matrix3x3 I1 = mBody1->getInertiaTensorInverseWorld(); const Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld(); - // 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); + // --------------- Translation Constraints --------------- // - // Compute J*v for the 3 translation constraints - const Vector3 JvRotation = w2 - w1; + // Compute J*v for the 2 translation constraints + const decimal el1 = -mN1.dot(v1) - w1.dot(mR1PlusUCrossN1) + + mN1.dot(v2) + w2.dot(mR2CrossN1); + const decimal el2 = -mN2.dot(v1) - w1.dot(mR1PlusUCrossN2) + + 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 + mU2World - x1 - mU1World; + Vector3 deltaV = x2 + mR2 - x1 - mR1; 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; + Vector3 angularImpulseBody1 = -mR1PlusUCrossN1 * deltaLambda.x -mR1PlusUCrossN2 * deltaLambda.y; + Vector3 linearImpulseBody2 = -linearImpulseBody1; + Vector3 angularImpulseBody2 = mR2CrossN1 * deltaLambda.x + mR2CrossN2 * deltaLambda.y; // Apply the impulse to the bodies of the joint if (mBody1->getIsMotionEnabled()) { @@ -212,6 +229,36 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint v2 += inverseMassBody2 * linearImpulseBody2; w2 += I2 * angularImpulseBody2; } + + // --------------- Rotation Constraints --------------- // + + // 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); + mImpulseRotation += deltaLambda2; + + // 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()) { + w1 += I1 * angularImpulseBody1; + } + if (mBody2->getIsMotionEnabled()) { + w2 += I2 * angularImpulseBody2; + } } // Solve the position constraint diff --git a/src/constraint/SliderJoint.h b/src/constraint/SliderJoint.h index d4aab31a..44742a15 100644 --- a/src/constraint/SliderJoint.h +++ b/src/constraint/SliderJoint.h @@ -47,15 +47,15 @@ struct SliderJointInfo : public ConstraintInfo { Vector3 anchorPointWorldSpace; /// Slider axis (in world-space coordinates) - Vector3 axisWorldSpace; + Vector3 sliderAxisWorldSpace; /// Constructor SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2, const Vector3& initAnchorPointWorldSpace, - const Vector3& initAxisWorldSpace) + const Vector3& initSliderAxisWorldSpace) : ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT), anchorPointWorldSpace(initAnchorPointWorldSpace), - axisWorldSpace(initAxisWorldSpace) {} + sliderAxisWorldSpace(initSliderAxisWorldSpace) {} }; // Class SliderJoint @@ -68,35 +68,38 @@ class SliderJoint : public Constraint { // -------------------- Attributes -------------------- // - /// Anchor point of body 1 (in local space coordinates) + /// Anchor point of body 1 (in local-space coordinates of body 1) Vector3 mLocalAnchorPointBody1; - /// Anchor point of body 2 (in local space coordinates) + /// Anchor point of body 2 (in local-space coordinates of body 2) Vector3 mLocalAnchorPointBody2; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mU1World; + /// Slider axis (in local-space coordinates of body 1) + Vector3 mSliderAxisBody1; - /// Vector from center of body 2 to anchor point in world-space - Vector3 mU2World; - - /// First vector orthogonal to vector mU1World in world-space + /// First vector orthogonal to the slider axis local-space of body 1 Vector3 mN1; - /// Second vector orthogonal to vector mU1World and mN1 in world-space + /// Second vector orthogonal to the slider axis and mN1 in local-space of body 1 Vector3 mN2; - /// Cross product of mU1World and mN1 - Vector3 mU1WorldCrossN1; + /// Vector r1 in world-space coordinates + Vector3 mR1; - /// Cross product of mU1World and mN2 - Vector3 mU1WorldCrossN2; + /// Vector r2 in world-space coordinates + Vector3 mR2; - /// Cross product of mU2World and mN1 - Vector3 mU2WorldCrossN1; + /// Cross product of r2 and n1 + Vector3 mR2CrossN1; - /// Cross product of mU2World and mN2 - Vector3 mU2WorldCrossN2; + /// Cross product of r2 and n2 + Vector3 mR2CrossN2; + + /// Cross product of vector (r1 + u) and n1 + Vector3 mR1PlusUCrossN1; + + /// Cross product of vector (r1 + u) and n2 + Vector3 mR1PlusUCrossN2; /// Inverse of mass matrix K=JM^-1J^t for the translation constraint (2x2 matrix) Matrix2x2 mInverseMassMatrixTranslationConstraint; diff --git a/src/mathematics/Matrix2x2.h b/src/mathematics/Matrix2x2.h index 53a313af..d19846bc 100644 --- a/src/mathematics/Matrix2x2.h +++ b/src/mathematics/Matrix2x2.h @@ -44,7 +44,7 @@ class Matrix2x2 { // -------------------- Attributes -------------------- // /// Rows of the matrix; - Vector2 mRows[3]; + Vector2 mRows[2]; public : diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 5f463e13..df9e367c 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -48,6 +48,7 @@ #include "collision/shapes/CylinderShape.h" #include "collision/shapes/AABB.h" #include "constraint/BallAndSocketJoint.h" +#include "constraint/SliderJoint.h" /// Alias to the ReactPhysics3D namespace namespace rp3d = reactphysics3d;