mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix issues in the BallAndSocketJoint and SliderJoint

Browse Source
This commit is contained in:
Daniel Chappuis 2013-05-21 23:03:14 +02:00
parent 8f37d4ac98
commit 78abbaac72
8 changed files with 253 additions and 120 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
examples/joints/Scene.cpp
View File

@ -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<rp3d::SliderJoint*>(mDynamicsWorld->createJoint(jointInfo));
}
// Create the floor
void Scene::createFloor() {

12
examples/joints/Scene.h
View File

@ -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();

63
src/constraint/BallAndSocketJoint.cpp
View File

@ -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()) {

18
src/constraint/BallAndSocketJoint.h
View File

@ -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;

175
src/constraint/SliderJoint.cpp
View File

@ -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

43
src/constraint/SliderJoint.h
View File

@ -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;

2
src/mathematics/Matrix2x2.h
View File

@ -44,7 +44,7 @@ class Matrix2x2 {
// -------------------- Attributes -------------------- //
/// Rows of the matrix;
Vector2 mRows[3];
Vector2 mRows[2];
public :

1
src/reactphysics3d.h
View File

@ -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;