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

Add SolveFixedJointSystem class

Browse Source
This commit is contained in:
Daniel Chappuis 2019-10-02 17:48:28 +02:00
parent 22810e0857
commit ab02d98f3a
11 changed files with 734 additions and 382 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -133,6 +133,7 @@ SET (REACTPHYSICS3D_HEADERS
"src/systems/DynamicsSystem.h"
"src/systems/CollisionDetectionSystem.h"
"src/systems/SolveBallAndSocketJointSystem.h"
"src/systems/SolveFixedJointSystem.h"
"src/engine/DynamicsWorld.h"
"src/engine/EventListener.h"
"src/engine/Island.h"
@ -230,6 +231,7 @@ SET (REACTPHYSICS3D_SOURCES
"src/systems/DynamicsSystem.cpp"
"src/systems/CollisionDetectionSystem.cpp"
"src/systems/SolveBallAndSocketJointSystem.cpp"
"src/systems/SolveFixedJointSystem.cpp"
"src/engine/DynamicsWorld.cpp"
"src/engine/Island.cpp"
"src/engine/Material.cpp"

21
src/components/FixedJointComponents.h
View File

@ -140,16 +140,16 @@ class FixedJointComponents : public Components {
void setJoint(Entity jointEntity, FixedJoint* joint) const;
/// Return the local anchor point of body 1 for a given joint
const Vector3& getLocalAnchoirPointBody1(Entity jointEntity) const;
const Vector3& getLocalAnchorPointBody1(Entity jointEntity) const;
/// Set the local anchor point of body 1 for a given joint
void setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1);
void setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchorPointBody1);
/// Return the local anchor point of body 2 for a given joint
const Vector3& getLocalAnchoirPointBody2(Entity jointEntity) const;
const Vector3& getLocalAnchorPointBody2(Entity jointEntity) const;
/// Set the local anchor point of body 2 for a given joint
void setLocalAnchoirPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2);
void setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2);
/// Return the vector from center of body 1 to anchor point in world-space
const Vector3& getR1World(Entity jointEntity) const;
@ -220,6 +220,7 @@ class FixedJointComponents : public Components {
// -------------------- Friendship -------------------- //
friend class BroadPhaseSystem;
friend class SolveFixedJointSystem;
};
// Return a pointer to a given joint
@ -237,31 +238,31 @@ inline void FixedJointComponents::setJoint(Entity jointEntity, FixedJoint* joint
}
// Return the local anchor point of body 1 for a given joint
inline const Vector3& FixedJointComponents::getLocalAnchoirPointBody1(Entity jointEntity) const {
inline const Vector3& FixedJointComponents::getLocalAnchorPointBody1(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]];
}
// Set the local anchor point of body 1 for a given joint
inline void FixedJointComponents::setLocalAnchoirPointBody1(Entity jointEntity, const Vector3& localAnchoirPointBody1) {
inline void FixedJointComponents::setLocalAnchorPointBody1(Entity jointEntity, const Vector3& localAnchorPointBody1) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1;
mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchorPointBody1;
}
// Return the local anchor point of body 2 for a given joint
inline const Vector3& FixedJointComponents::getLocalAnchoirPointBody2(Entity jointEntity) const {
inline const Vector3& FixedJointComponents::getLocalAnchorPointBody2(Entity jointEntity) const {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]];
}
// Set the local anchor point of body 2 for a given joint
inline void FixedJointComponents::setLocalAnchoirPointBody2(Entity jointEntity, const Vector3& localAnchoirPointBody2) {
inline void FixedJointComponents::setLocalAnchorPointBody2(Entity jointEntity, const Vector3& localAnchorPointBody2) {
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody2;
mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]] = localAnchorPointBody2;
}
// Return the vector from center of body 1 to anchor point in world-space

1
src/components/RigidBodyComponents.h
View File

@ -347,6 +347,7 @@ class RigidBodyComponents : public Components {
friend class DynamicsWorld;
friend class ContactSolverSystem;
friend class SolveBallAndSocketJointSystem;
friend class SolveFixedJointSystem;
friend class DynamicsSystem;
friend class BallAndSocketJoint;
friend class FixedJoint;

363
src/constraint/FixedJoint.cpp
View File

@ -31,9 +31,6 @@
using namespace reactphysics3d;
// Static variables definition
const decimal FixedJoint::BETA = decimal(0.2);
// Constructor
FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo)
: Joint(entity, world) {
@ -42,8 +39,8 @@ FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo
const Transform& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
mWorld.mFixedJointsComponents.setLocalAnchoirPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace);
mWorld.mFixedJointsComponents.setLocalAnchoirPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace);
mWorld.mFixedJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace);
mWorld.mFixedJointsComponents.setLocalAnchorPointBody2(mEntity, transform2.getInverse() * jointInfo.anchorPointWorldSpace);
// Store inverse of initial rotation from body 1 to body 2 in body 1 space:
//
@ -62,379 +59,27 @@ FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo
// Initialize before solving the constraint
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// Get the bodies positions and orientations
const Vector3& x1 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body1Entity);
const Vector3& x2 = constraintSolverData.rigidBodyComponents.getCenterOfMassWorld(body2Entity);
const Quaternion& orientationBody1 = body1->getTransform().getOrientation();
const Quaternion& orientationBody2 = body2->getTransform().getOrientation();
// Get the inertia tensor of bodies
mWorld.mFixedJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
mWorld.mFixedJointsComponents.setI1(mEntity, body2->getInertiaTensorInverseWorld());
const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity);
const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity);
// Compute the vector from body center to the anchor point in world-space
mWorld.mFixedJointsComponents.setR1World(mEntity, orientationBody1 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity));
mWorld.mFixedJointsComponents.setR2World(mEntity, orientationBody2 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity));
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1->getEntity());
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2->getEntity());
const decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1 for the 3 translation constraints
Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity);
inverseMassMatrixTranslation.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mWorld.mFixedJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse());
}
// Compute the bias "b" of the constraint for the 3 translation constraints
const decimal biasFactor = (BETA / constraintSolverData.timeStep);
Vector3& biasTranslation = mWorld.mFixedJointsComponents.getBiasTranslation(mEntity);
biasTranslation.setToZero();
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mWorld.mFixedJointsComponents.setBiasTranslation(mEntity, biasFactor * (x2 + r2World - x1 - r1World));
}
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity);
inverseMassMatrixRotation = i1 + i2;
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mWorld.mFixedJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity).getInverse());
}
// Compute the bias "b" for the 3 rotation constraints
Vector3& biasRotation = mWorld.mFixedJointsComponents.getBiasRotation(mEntity);
biasRotation.setToZero();
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
const Quaternion qError = orientationBody2 * mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity) * orientationBody1.getInverse();
mWorld.mFixedJointsComponents.setBiasRotation(mEntity, biasFactor * decimal(2.0) * qError.getVectorV());
}
// If warm-starting is not enabled
if (!constraintSolverData.isWarmStartingActive) {
Vector3& impulseTranslation = mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity);
Vector3& impulseRotation = mWorld.mFixedJointsComponents.getImpulseRotation(mEntity);
// Reset the accumulated impulses
impulseTranslation.setToZero();
impulseRotation.setToZero();
}
}
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const Vector3& impulseTranslation = mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity);
const Vector3& impulseRotation = mWorld.mFixedJointsComponents.getImpulseRotation(mEntity);
const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity);
const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity);
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -impulseTranslation;
Vector3 angularImpulseBody1 = impulseTranslation.cross(r1World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 += -impulseRotation;
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
angularImpulseBody2 += impulseRotation;
// Apply the impulse to the body 2
v2 += inverseMassBody2 * impulseTranslation;
w2 += i2 * angularImpulseBody2;
}
// Solve the velocity constraint
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
uint32 dynamicsComponentIndexBody1 = constraintSolverData.rigidBodyComponents.getEntityIndex(body1Entity);
uint32 dynamicsComponentIndexBody2 = constraintSolverData.rigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody1];
Vector3& v2 = constraintSolverData.rigidBodyComponents.mConstrainedLinearVelocities[dynamicsComponentIndexBody2];
Vector3& w1 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody1];
Vector3& w2 = constraintSolverData.rigidBodyComponents.mConstrainedAngularVelocities[dynamicsComponentIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity);
const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity);
// --------------- Translation Constraints --------------- //
// Compute J*v for the 3 translation constraints
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
const Vector3& biasTranslation = mWorld.mFixedJointsComponents.getBiasTranslation(mEntity);
const Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity);
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = inverseMassMatrixTranslation * (-JvTranslation - biasTranslation);
mWorld.mFixedJointsComponents.setImpulseTranslation(mEntity, mWorld.mFixedJointsComponents.getImpulseTranslation(mEntity) + deltaLambda);
// Compute the impulse P=J^T * lambda for body 1
const Vector3 linearImpulseBody1 = -deltaLambda;
Vector3 angularImpulseBody1 = deltaLambda.cross(r1World);
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World);
// Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambda;
w2 += i2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation constraints
const Vector3 JvRotation = w2 - w1;
const Vector3& biasRotation = mWorld.mFixedJointsComponents.getBiasRotation(mEntity);
const Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity);
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = inverseMassMatrixRotation * (-JvRotation - biasRotation);
mWorld.mFixedJointsComponents.setImpulseRotation(mEntity, mWorld.mFixedJointsComponents.getImpulseRotation(mEntity) + deltaLambda2);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body 1
w1 += i1 * angularImpulseBody1;
// Apply the impulse to the body 2
w2 += i2 * deltaLambda2;
}
// Solve the position constraint (for position error correction)
void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
// Get the bodies entities
Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
Entity body2Entity = mWorld.mJointsComponents.getBody2Entity(mEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mWorld.mRigidBodyComponents.getRigidBody(body2Entity));
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
Vector3 x1 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body1Entity);
Vector3 x2 = constraintSolverData.rigidBodyComponents.getConstrainedPosition(body2Entity);
Quaternion q1 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body1Entity);
Quaternion q2 = constraintSolverData.rigidBodyComponents.getConstrainedOrientation(body2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
const Vector3& r1World = mWorld.mFixedJointsComponents.getR1World(mEntity);
const Vector3& r2World = mWorld.mFixedJointsComponents.getR2World(mEntity);
const Matrix3x3& i1 = mWorld.mFixedJointsComponents.getI1(mEntity);
const Matrix3x3& i2 = mWorld.mFixedJointsComponents.getI2(mEntity);
// Recompute the inverse inertia tensors
mWorld.mFixedJointsComponents.setI1(mEntity, body1->getInertiaTensorInverseWorld());
mWorld.mFixedJointsComponents.setI2(mEntity, body2->getInertiaTensorInverseWorld());
// Compute the vector from body center to the anchor point in world-space
mWorld.mFixedJointsComponents.setR1World(mEntity, q1 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity));
mWorld.mFixedJointsComponents.setR2World(mEntity, q2 * mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity));
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * i1 * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * i2 * skewSymmetricMatrixU2.getTranspose();
Matrix3x3& inverseMassMatrixTranslation = mWorld.mFixedJointsComponents.getInverseMassMatrixTranslation(mEntity);
inverseMassMatrixTranslation.setToZero();
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mWorld.mFixedJointsComponents.setInverseMassMatrixTranslation(mEntity, massMatrix.getInverse());
}
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = inverseMassMatrixTranslation * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World);
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World);
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = i2 * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
Matrix3x3& inverseMassMatrixRotation = mWorld.mFixedJointsComponents.getInverseMassMatrixRotation(mEntity);
inverseMassMatrixRotation = i1 + i2;
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mWorld.mFixedJointsComponents.setInverseMassMatrixRotation(mEntity, inverseMassMatrixRotation.getInverse());
}
// Calculate difference in rotation
//
// The rotation should be:
//
// q2 = q1 r0
//
// But because of drift the actual rotation is:
//
// q2 = qError q1 r0
// <=> qError = q2 r0^-1 q1^-1
//
// Where:
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// qError = error that needs to be reduced to zero
Quaternion qError = q2 * mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity) * q1.getInverse();
// A quaternion can be seen as:
//
// q = [sin(theta / 2) * v, cos(theta/2)]
//
// Where:
// v = rotation vector
// theta = rotation angle
//
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = inverseMassMatrixRotation * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity of body 1
w1 = i1 * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the pseudo velocity of body 2
w2 = i2 * lambdaRotation;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body1Entity, x1);
constraintSolverData.rigidBodyComponents.setConstrainedPosition(body2Entity, x2);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body1Entity, q1);
constraintSolverData.rigidBodyComponents.setConstrainedOrientation(body2Entity, q2);
}
// Return a string representation
std::string FixedJoint::to_string() const {
return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity).to_string() +
", localAnchorPointBody2=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity).to_string() +
return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() +
", localAnchorPointBody2=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() +
", initOrientationDifferenceInv=" + mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() +
"}";
}

9
src/constraint/FixedJoint.h
View File

@ -68,26 +68,25 @@ class FixedJoint : public Joint {
private :
// -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
// -------------------- Methods -------------------- //
/// Return the number of bytes used by the joint
virtual size_t getSizeInBytes() const override;
/// Initialize before solving the constraint
// TODO : DELETE THIS
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) override;
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
// TODO : DELETE THIS
virtual void warmstart(const ConstraintSolverData& constraintSolverData) override;
/// Solve the velocity constraint
// TODO : DELETE THIS
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) override;
/// Solve the position constraint (for position error correction)
// TODO : DELETE THIS
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) override;
public :

2
src/engine/DynamicsWorld.cpp
View File

@ -53,7 +53,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
mProxyShapesComponents, mConfig),
mConstraintSolverSystem(mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
mBallAndSocketJointsComponents),
mBallAndSocketJointsComponents, mFixedJointsComponents),
mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),

22
src/systems/ConstraintSolverSystem.cpp
View File

@ -36,11 +36,14 @@ using namespace reactphysics3d;
ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents,
JointComponents& jointComponents,
BallAndSocketJointComponents& ballAndSocketJointComponents)
BallAndSocketJointComponents& ballAndSocketJointComponents,
FixedJointComponents& fixedJointComponents)
: mIsWarmStartingActive(true), mIslands(islands),
mConstraintSolverData(rigidBodyComponents, jointComponents),
mSolveBallAndSocketJointSystem(rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents),
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents){
mSolveFixedJointSystem(rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents),
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents),
mFixedJointComponents(fixedJointComponents) {
#ifdef IS_PROFILING_ACTIVE
@ -64,11 +67,15 @@ void ConstraintSolverSystem::initialize(decimal dt) {
mSolveBallAndSocketJointSystem.setTimeStep(dt);
mSolveBallAndSocketJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
mSolveFixedJointSystem.setTimeStep(dt);
mSolveFixedJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
mSolveBallAndSocketJointSystem.initBeforeSolve();
mSolveFixedJointSystem.initBeforeSolve();
if (mIsWarmStartingActive) {
mSolveBallAndSocketJointSystem.warmstart();
mSolveFixedJointSystem.warmstart();
}
// For each joint
@ -76,7 +83,8 @@ void ConstraintSolverSystem::initialize(decimal dt) {
// TODO : DELETE THIS
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
if (mBallAndSocketJointComponents.hasComponent(jointEntity)) {
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
mFixedJointComponents.hasComponent(jointEntity)) {
continue;
}
@ -101,13 +109,15 @@ void ConstraintSolverSystem::solveVelocityConstraints() {
RP3D_PROFILE("ConstraintSolverSystem::solveVelocityConstraints()", mProfiler);
mSolveBallAndSocketJointSystem.solveVelocityConstraint();
mSolveFixedJointSystem.solveVelocityConstraint();
// For each joint
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
// TODO : DELETE THIS
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
if (mBallAndSocketJointComponents.hasComponent(jointEntity)) {
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
mFixedJointComponents.hasComponent(jointEntity)) {
continue;
}
@ -122,13 +132,15 @@ void ConstraintSolverSystem::solvePositionConstraints() {
RP3D_PROFILE("ConstraintSolverSystem::solvePositionConstraints()", mProfiler);
mSolveBallAndSocketJointSystem.solvePositionConstraint();
mSolveFixedJointSystem.solvePositionConstraint();
// For each joint
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
// TODO : DELETE THIS
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
if (mBallAndSocketJointComponents.hasComponent(jointEntity)) {
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
mFixedJointComponents.hasComponent(jointEntity)) {
continue;
}

11
src/systems/ConstraintSolverSystem.h
View File

@ -31,6 +31,7 @@
#include "mathematics/mathematics.h"
#include "engine/Islands.h"
#include "systems/SolveBallAndSocketJointSystem.h"
#include "systems/SolveFixedJointSystem.h"
namespace reactphysics3d {
@ -161,12 +162,18 @@ class ConstraintSolverSystem {
/// Solver for the BallAndSocketJoint constraints
SolveBallAndSocketJointSystem mSolveBallAndSocketJointSystem;
/// Solver for the FixedJoint constraints
SolveFixedJointSystem mSolveFixedJointSystem;
// TODO : Delete this
JointComponents& mJointComponents;
// TODO : Delete this
BallAndSocketJointComponents& mBallAndSocketJointComponents;
// TODO : Delete this
FixedJointComponents& mFixedJointComponents;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
@ -181,7 +188,8 @@ class ConstraintSolverSystem {
ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents,
JointComponents& jointComponents,
BallAndSocketJointComponents& ballAndSocketJointComponents);
BallAndSocketJointComponents& ballAndSocketJointComponents,
FixedJointComponents& fixedJointComponents);
/// Destructor
~ConstraintSolverSystem() = default;
@ -216,6 +224,7 @@ class ConstraintSolverSystem {
inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) {
mProfiler = profiler;
mSolveBallAndSocketJointSystem.setProfiler(profiler);
mSolveFixedJointSystem.setProfiler(profiler);
}
#endif

3
src/systems/SolveBallAndSocketJointSystem.cpp
View File

@ -119,6 +119,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
}
}
const decimal biasFactor = (BETA / mTimeStep);
// For each joint
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
@ -137,7 +139,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
// Compute the bias "b" of the constraint
mBallAndSocketJointComponents.mBiasVector[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
decimal biasFactor = (BETA / mTimeStep);
mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World);
}
}

545
src/systems/SolveFixedJointSystem.cpp Normal file
View File

@ -0,0 +1,545 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "systems/SolveFixedJointSystem.h"
#include "body/RigidBody.h"
using namespace reactphysics3d;
// Static variables definition
const decimal SolveFixedJointSystem::BETA = decimal(0.2);
// Constructor
SolveFixedJointSystem::SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents,
TransformComponents& transformComponents,
JointComponents& jointComponents,
FixedJointComponents& fixedJointComponents)
:mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
mJointComponents(jointComponents), mFixedJointComponents(fixedJointComponents),
mTimeStep(0), mIsWarmStartingActive(true) {
}
// Initialize before solving the constraint
void SolveFixedJointSystem::initBeforeSolve() {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
// Get the inertia tensor of bodies
mFixedJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld();
mFixedJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld();
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
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];
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR1World[i]);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(mFixedJointComponents.mR2World[i]);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
const decimal inverseMassBodies = body1MassInverse + body2MassInverse;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mFixedJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mFixedJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
// Compute the inverse mass matrix K^-1 for the 3 translation constraints
mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
}
}
const decimal biasFactor = BETA / mTimeStep;
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Get the bodies positions and orientations
const Vector3& x1 = mRigidBodyComponents.getCenterOfMassWorld(body1Entity);
const Vector3& x2 = mRigidBodyComponents.getCenterOfMassWorld(body2Entity);
const Vector3& r1World = mFixedJointComponents.mR1World[i];
const Vector3& r2World = mFixedJointComponents.mR2World[i];
// Compute the bias "b" of the constraint for the 3 translation constraints
mFixedJointComponents.mBiasTranslation[i].setToZero();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
mFixedJointComponents.mBiasTranslation[i] = biasFactor * (x2 + r2World - x1 - r1World);
}
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation contraints (3x3 matrix)
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i];
if (mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse();
}
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Compute the bias "b" for the 3 rotation constraints
mFixedJointComponents.mBiasRotation[i].setToZero();
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
const Quaternion& orientationBody2 = mTransformComponents.getTransform(body2Entity).getOrientation();
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
const Quaternion qError = orientationBody2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse();
mFixedJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV();
}
}
// If warm-starting is not enabled
if (!mIsWarmStartingActive) {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
// Reset the accumulated impulses
mFixedJointComponents.mImpulseTranslation[i].setToZero();
mFixedJointComponents.mImpulseRotation[i].setToZero();
}
}
}
// Warm start the constraint (apply the previous impulse at the beginning of the step)
void SolveFixedJointSystem::warmstart() {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
// Get the inverse mass of the bodies
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
const Vector3& impulseTranslation = mFixedJointComponents.mImpulseTranslation[i];
const Vector3& impulseRotation = mFixedJointComponents.mImpulseRotation[i];
const Vector3& r1World = mFixedJointComponents.mR1World[i];
const Vector3& r2World = mFixedJointComponents.mR2World[i];
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 1
Vector3 linearImpulseBody1 = -impulseTranslation;
Vector3 angularImpulseBody1 = impulseTranslation.cross(r1World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 += -impulseRotation;
const Matrix3x3& i1 = mFixedJointComponents.mI1[i];
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for the 3 translation constraints for body 2
Vector3 angularImpulseBody2 = -impulseTranslation.cross(r2World);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 2
angularImpulseBody2 += impulseRotation;
const Matrix3x3& i2 = mFixedJointComponents.mI2[i];
// Apply the impulse to the body 2
v2 += inverseMassBody2 * impulseTranslation;
w2 += i2 * angularImpulseBody2;
}
}
// Solve the velocity constraint
void SolveFixedJointSystem::solveVelocityConstraint() {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Get the velocities
Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
// Get the inverse mass of the bodies
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
const Vector3& r1World = mFixedJointComponents.mR1World[i];
const Vector3& r2World = mFixedJointComponents.mR2World[i];
// --------------- Translation Constraints --------------- //
// Compute J*v for the 3 translation constraints
const Vector3 JvTranslation = v2 + w2.cross(r2World) - v1 - w1.cross(r1World);
const Matrix3x3& inverseMassMatrixTranslation = mFixedJointComponents.mInverseMassMatrixTranslation[i];
// Compute the Lagrange multiplier lambda
const Vector3 deltaLambda = inverseMassMatrixTranslation * (-JvTranslation - mFixedJointComponents.mBiasTranslation[i]);
mFixedJointComponents.mImpulseTranslation[i] += deltaLambda;
// Compute the impulse P=J^T * lambda for body 1
const Vector3 linearImpulseBody1 = -deltaLambda;
Vector3 angularImpulseBody1 = deltaLambda.cross(r1World);
const Matrix3x3& i1 = mFixedJointComponents.mI1[i];
// Apply the impulse to the body 1
v1 += inverseMassBody1 * linearImpulseBody1;
w1 += i1 * angularImpulseBody1;
// Compute the impulse P=J^T * lambda for body 2
const Vector3 angularImpulseBody2 = -deltaLambda.cross(r2World);
const Matrix3x3& i2 = mFixedJointComponents.mI2[i];
// Apply the impulse to the body 2
v2 += inverseMassBody2 * deltaLambda;
w2 += i2 * angularImpulseBody2;
// --------------- Rotation Constraints --------------- //
// Compute J*v for the 3 rotation constraints
const Vector3 JvRotation = w2 - w1;
const Vector3& biasRotation = mFixedJointComponents.mBiasRotation[i];
const Matrix3x3& inverseMassMatrixRotation = mFixedJointComponents.mInverseMassMatrixRotation[i];
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 deltaLambda2 = inverseMassMatrixRotation * (-JvRotation - biasRotation);
mFixedJointComponents.mImpulseRotation[i] += deltaLambda2;
// Compute the impulse P=J^T * lambda for the 3 rotation constraints for body 1
angularImpulseBody1 = -deltaLambda2;
// Apply the impulse to the body 1
w1 += i1 * angularImpulseBody1;
// Apply the impulse to the body 2
w2 += i2 * deltaLambda2;
}
}
// Solve the position constraint (for position error correction)
void SolveFixedJointSystem::solvePositionConstraint() {
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// TODO : Remove this and use compoents instead of pointers to bodies
const RigidBody* body1 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body1Entity));
const RigidBody* body2 = static_cast<RigidBody*>(mRigidBodyComponents.getRigidBody(body2Entity));
// Recompute the inverse inertia tensors
mFixedJointComponents.mI1[i] = body1->getInertiaTensorInverseWorld();
mFixedJointComponents.mI2[i] = body2->getInertiaTensorInverseWorld();
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
// Get the bodies positions and orientations
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
// Compute the vector from body center to the anchor point in world-space
mFixedJointComponents.mR1World[i] = q1 * mFixedJointComponents.getLocalAnchorPointBody1(jointEntity);
mFixedJointComponents.mR2World[i] = q2 * mFixedJointComponents.getLocalAnchorPointBody2(jointEntity);
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
// Get the inverse mass and inverse inertia tensors of the bodies
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
const Vector3& r1World = mFixedJointComponents.mR1World[i];
const Vector3& r2World = mFixedJointComponents.mR2World[i];
// Compute the corresponding skew-symmetric matrices
Matrix3x3 skewSymmetricMatrixU1= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r1World);
Matrix3x3 skewSymmetricMatrixU2= Matrix3x3::computeSkewSymmetricMatrixForCrossProduct(r2World);
// --------------- Translation Constraints --------------- //
// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation constraints
decimal inverseMassBodies = inverseMassBody1 + inverseMassBody2;
Matrix3x3 massMatrix = Matrix3x3(inverseMassBodies, 0, 0,
0, inverseMassBodies, 0,
0, 0, inverseMassBodies) +
skewSymmetricMatrixU1 * mFixedJointComponents.mI1[i] * skewSymmetricMatrixU1.getTranspose() +
skewSymmetricMatrixU2 * mFixedJointComponents.mI2[i] * skewSymmetricMatrixU2.getTranspose();
mFixedJointComponents.mInverseMassMatrixTranslation[i].setToZero();
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixTranslation[i] = massMatrix.getInverse();
}
}
// For each joint
for (uint32 i=0; i < mFixedJointComponents.getNbEnabledComponents(); i++) {
const Entity jointEntity = mFixedJointComponents.mJointEntities[i];
// If the error position correction technique is not the non-linear-gauss-seidel, we do
// do not execute this method
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) != JointsPositionCorrectionTechnique::NON_LINEAR_GAUSS_SEIDEL) continue;
// Get the bodies entities
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
const Vector3& r1World = mFixedJointComponents.mR1World[i];
const Vector3& r2World = mFixedJointComponents.mR2World[i];
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
Vector3 x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
Vector3 x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
Quaternion q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
Quaternion q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
// Compute position error for the 3 translation constraints
const Vector3 errorTranslation = x2 + r2World - x1 - r1World;
// Compute the Lagrange multiplier lambda
const Vector3 lambdaTranslation = mFixedJointComponents.mInverseMassMatrixTranslation[i] * (-errorTranslation);
// Compute the impulse of body 1
Vector3 linearImpulseBody1 = -lambdaTranslation;
Vector3 angularImpulseBody1 = lambdaTranslation.cross(r1World);
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
// Compute the pseudo velocity of body 1
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
Vector3 w1 = mFixedJointComponents.mI2[i] * angularImpulseBody1;
// Update the body position/orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the impulse of body 2
Vector3 angularImpulseBody2 = -lambdaTranslation.cross(r2World);
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
// Compute the pseudo velocity of body 2
const Vector3 v2 = inverseMassBody2 * lambdaTranslation;
Vector3 w2 = mFixedJointComponents.mI2[i] * angularImpulseBody2;
// Update the body position/orientation of body 2
x2 += v2;
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
// --------------- Rotation Constraints --------------- //
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
// contraints (3x3 matrix)
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mI1[i] + mFixedJointComponents.mI2[i];
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
mFixedJointComponents.mInverseMassMatrixRotation[i] = mFixedJointComponents.mInverseMassMatrixRotation[i].getInverse();
}
// Calculate difference in rotation
//
// The rotation should be:
//
// q2 = q1 r0
//
// But because of drift the actual rotation is:
//
// q2 = qError q1 r0
// <=> qError = q2 r0^-1 q1^-1
//
// Where:
// q1 = current rotation of body 1
// q2 = current rotation of body 2
// qError = error that needs to be reduced to zero
Quaternion qError = q2 * mFixedJointComponents.mInitOrientationDifferenceInv[i] * q1.getInverse();
// A quaternion can be seen as:
//
// q = [sin(theta / 2) * v, cos(theta/2)]
//
// Where:
// v = rotation vector
// theta = rotation angle
//
// If we assume theta is small (error is small) then sin(x) = x so an approximation of the error angles is:
const Vector3 errorRotation = decimal(2.0) * qError.getVectorV();
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
Vector3 lambdaRotation = mFixedJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
angularImpulseBody1 = -lambdaRotation;
// Compute the pseudo velocity of body 1
w1 = mFixedJointComponents.mI1[i] * angularImpulseBody1;
// Update the body position/orientation of body 1
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();
// Compute the pseudo velocity of body 2
w2 = mFixedJointComponents.mI2[i] * lambdaRotation;
// Update the body position/orientation of body 2
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
q2.normalize();
mRigidBodyComponents.mConstrainedPositions[componentIndexBody1] = x1;
mRigidBodyComponents.mConstrainedPositions[componentIndexBody2] = x2;
mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1] = q1;
mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2] = q2;
}
}

137
src/systems/SolveFixedJointSystem.h Normal file
View File

@ -0,0 +1,137 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2018 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SOLVE_FIXED_JOINT_SYSTEM_H
#define REACTPHYSICS3D_SOLVE_FIXED_JOINT_SYSTEM_H
// Libraries
#include "utils/Profiler.h"
#include "components/RigidBodyComponents.h"
#include "components/JointComponents.h"
#include "components/FixedJointComponents.h"
#include "components/TransformComponents.h"
namespace reactphysics3d {
// Class SolveFixedJointSystem
/**
* This class is responsible to solve the FixedJoint constraints
*/
class SolveFixedJointSystem {
private :
// -------------------- Constants -------------------- //
// Beta value for the bias factor of position correction
static const decimal BETA;
// -------------------- Attributes -------------------- //
/// Reference to the rigid body components
RigidBodyComponents& mRigidBodyComponents;
/// Reference to transform components
TransformComponents& mTransformComponents;
/// Reference to the joint components
JointComponents& mJointComponents;
/// Reference to the fixed joint components
FixedJointComponents& mFixedJointComponents;
/// Current time step of the simulation
decimal mTimeStep;
/// True if warm starting of the solver is active
bool mIsWarmStartingActive;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public :
// -------------------- Methods -------------------- //
/// Constructor
SolveFixedJointSystem(RigidBodyComponents& rigidBodyComponents, TransformComponents& transformComponents,
JointComponents& jointComponents, FixedJointComponents& fixedJointComponents);
/// Destructor
~SolveFixedJointSystem() = default;
/// Initialize before solving the constraint
void initBeforeSolve();
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
void warmstart();
/// Solve the velocity constraint
void solveVelocityConstraint();
/// Solve the position constraint (for position error correction)
void solvePositionConstraint();
/// Set the time step
void setTimeStep(decimal timeStep);
/// Set to true to enable warm starting
void setIsWarmStartingActive(bool isWarmStartingActive);
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void SolveFixedJointSystem::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
// Set the time step
inline void SolveFixedJointSystem::setTimeStep(decimal timeStep) {
assert(timeStep > decimal(0.0));
mTimeStep = timeStep;
}
// Set to true to enable warm starting
inline void SolveFixedJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) {
mIsWarmStartingActive = isWarmStartingActive;
}
#endif
}
#endif