Add SolveFixedJointSystem class
This commit is contained in:
Daniel Chappuis
parent
22810e0857
commit
ab02d98f3a
|
|
@ -133,6 +133,7 @@ SET (REACTPHYSICS3D_HEADERS
|
||||||
"src/systems/DynamicsSystem.h"
|
"src/systems/DynamicsSystem.h"
|
||||||
"src/systems/CollisionDetectionSystem.h"
|
"src/systems/CollisionDetectionSystem.h"
|
||||||
"src/systems/SolveBallAndSocketJointSystem.h"
|
"src/systems/SolveBallAndSocketJointSystem.h"
|
||||||
|
"src/systems/SolveFixedJointSystem.h"
|
||||||
"src/engine/DynamicsWorld.h"
|
"src/engine/DynamicsWorld.h"
|
||||||
"src/engine/EventListener.h"
|
"src/engine/EventListener.h"
|
||||||
"src/engine/Island.h"
|
"src/engine/Island.h"
|
||||||
|
|
@ -230,6 +231,7 @@ SET (REACTPHYSICS3D_SOURCES
|
||||||
"src/systems/DynamicsSystem.cpp"
|
"src/systems/DynamicsSystem.cpp"
|
||||||
"src/systems/CollisionDetectionSystem.cpp"
|
"src/systems/CollisionDetectionSystem.cpp"
|
||||||
"src/systems/SolveBallAndSocketJointSystem.cpp"
|
"src/systems/SolveBallAndSocketJointSystem.cpp"
|
||||||
|
"src/systems/SolveFixedJointSystem.cpp"
|
||||||
"src/engine/DynamicsWorld.cpp"
|
"src/engine/DynamicsWorld.cpp"
|
||||||
"src/engine/Island.cpp"
|
"src/engine/Island.cpp"
|
||||||
"src/engine/Material.cpp"
|
"src/engine/Material.cpp"
|
||||||
|
|
|
||||||
|
|
@ -140,16 +140,16 @@ class FixedJointComponents : public Components {
|
||||||
void setJoint(Entity jointEntity, FixedJoint* joint) const;
|
void setJoint(Entity jointEntity, FixedJoint* joint) const;
|
||||||
|
|
||||||
/// Return the local anchor point of body 1 for a given joint
|
/// 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
|
/// 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
|
/// 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
|
/// 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
|
/// Return the vector from center of body 1 to anchor point in world-space
|
||||||
const Vector3& getR1World(Entity jointEntity) const;
|
const Vector3& getR1World(Entity jointEntity) const;
|
||||||
|
|
@ -220,6 +220,7 @@ class FixedJointComponents : public Components {
|
||||||
// -------------------- Friendship -------------------- //
|
// -------------------- Friendship -------------------- //
|
||||||
|
|
||||||
friend class BroadPhaseSystem;
|
friend class BroadPhaseSystem;
|
||||||
|
friend class SolveFixedJointSystem;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Return a pointer to a given joint
|
// 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
|
// 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));
|
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||||
return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]];
|
return mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the local anchor point of body 1 for a given joint
|
// 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));
|
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||||
mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchoirPointBody1;
|
mLocalAnchorPointBody1[mMapEntityToComponentIndex[jointEntity]] = localAnchorPointBody1;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the local anchor point of body 2 for a given joint
|
// 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));
|
assert(mMapEntityToComponentIndex.containsKey(jointEntity));
|
||||||
return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]];
|
return mLocalAnchorPointBody2[mMapEntityToComponentIndex[jointEntity]];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the local anchor point of body 2 for a given joint
|
// 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));
|
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
|
// Return the vector from center of body 1 to anchor point in world-space
|
||||||
|
|
|
||||||
|
|
@ -347,6 +347,7 @@ class RigidBodyComponents : public Components {
|
||||||
friend class DynamicsWorld;
|
friend class DynamicsWorld;
|
||||||
friend class ContactSolverSystem;
|
friend class ContactSolverSystem;
|
||||||
friend class SolveBallAndSocketJointSystem;
|
friend class SolveBallAndSocketJointSystem;
|
||||||
|
friend class SolveFixedJointSystem;
|
||||||
friend class DynamicsSystem;
|
friend class DynamicsSystem;
|
||||||
friend class BallAndSocketJoint;
|
friend class BallAndSocketJoint;
|
||||||
friend class FixedJoint;
|
friend class FixedJoint;
|
||||||
|
|
|
||||||
|
|
@ -31,9 +31,6 @@
|
||||||
|
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
|
||||||
// Static variables definition
|
|
||||||
const decimal FixedJoint::BETA = decimal(0.2);
|
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo)
|
FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo& jointInfo)
|
||||||
: Joint(entity, world) {
|
: 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& transform1 = mWorld.mTransformComponents.getTransform(jointInfo.body1->getEntity());
|
||||||
const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
|
const Transform& transform2 = mWorld.mTransformComponents.getTransform(jointInfo.body2->getEntity());
|
||||||
|
|
||||||
mWorld.mFixedJointsComponents.setLocalAnchoirPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace);
|
mWorld.mFixedJointsComponents.setLocalAnchorPointBody1(mEntity, transform1.getInverse() * jointInfo.anchorPointWorldSpace);
|
||||||
mWorld.mFixedJointsComponents.setLocalAnchoirPointBody2(mEntity, transform2.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:
|
// 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
|
// Initialize before solving the constraint
|
||||||
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
|
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)
|
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||||
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
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
|
// Solve the velocity constraint
|
||||||
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
|
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)
|
// Solve the position constraint (for position error correction)
|
||||||
void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
|
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
|
// Return a string representation
|
||||||
std::string FixedJoint::to_string() const {
|
std::string FixedJoint::to_string() const {
|
||||||
return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody1(mEntity).to_string() +
|
return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() +
|
||||||
", localAnchorPointBody2=" + mWorld.mFixedJointsComponents.getLocalAnchoirPointBody2(mEntity).to_string() +
|
", localAnchorPointBody2=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody2(mEntity).to_string() +
|
||||||
", initOrientationDifferenceInv=" + mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() +
|
", initOrientationDifferenceInv=" + mWorld.mFixedJointsComponents.getInitOrientationDifferenceInv(mEntity).to_string() +
|
||||||
"}";
|
"}";
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -68,26 +68,25 @@ class FixedJoint : public Joint {
|
||||||
|
|
||||||
private :
|
private :
|
||||||
|
|
||||||
// -------------------- Constants -------------------- //
|
|
||||||
|
|
||||||
// Beta value for the bias factor of position correction
|
|
||||||
static const decimal BETA;
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Return the number of bytes used by the joint
|
/// Return the number of bytes used by the joint
|
||||||
virtual size_t getSizeInBytes() const override;
|
virtual size_t getSizeInBytes() const override;
|
||||||
|
|
||||||
/// Initialize before solving the constraint
|
/// Initialize before solving the constraint
|
||||||
|
// TODO : DELETE THIS
|
||||||
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) override;
|
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) override;
|
||||||
|
|
||||||
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||||
|
// TODO : DELETE THIS
|
||||||
virtual void warmstart(const ConstraintSolverData& constraintSolverData) override;
|
virtual void warmstart(const ConstraintSolverData& constraintSolverData) override;
|
||||||
|
|
||||||
/// Solve the velocity constraint
|
/// Solve the velocity constraint
|
||||||
|
// TODO : DELETE THIS
|
||||||
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) override;
|
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) override;
|
||||||
|
|
||||||
/// Solve the position constraint (for position error correction)
|
/// Solve the position constraint (for position error correction)
|
||||||
|
// TODO : DELETE THIS
|
||||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) override;
|
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) override;
|
||||||
|
|
||||||
public :
|
public :
|
||||||
|
|
|
||||||
|
|
@ -53,7 +53,7 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
|
||||||
mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
|
mContactSolverSystem(mMemoryManager, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
|
||||||
mProxyShapesComponents, mConfig),
|
mProxyShapesComponents, mConfig),
|
||||||
mConstraintSolverSystem(mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
|
mConstraintSolverSystem(mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
|
||||||
mBallAndSocketJointsComponents),
|
mBallAndSocketJointsComponents, mFixedJointsComponents),
|
||||||
mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
|
mDynamicsSystem(mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
|
||||||
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
|
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
|
||||||
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
|
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
|
||||||
|
|
|
||||||
|
|
@ -36,11 +36,14 @@ using namespace reactphysics3d;
|
||||||
ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
|
ConstraintSolverSystem::ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
|
||||||
TransformComponents& transformComponents,
|
TransformComponents& transformComponents,
|
||||||
JointComponents& jointComponents,
|
JointComponents& jointComponents,
|
||||||
BallAndSocketJointComponents& ballAndSocketJointComponents)
|
BallAndSocketJointComponents& ballAndSocketJointComponents,
|
||||||
|
FixedJointComponents& fixedJointComponents)
|
||||||
: mIsWarmStartingActive(true), mIslands(islands),
|
: mIsWarmStartingActive(true), mIslands(islands),
|
||||||
mConstraintSolverData(rigidBodyComponents, jointComponents),
|
mConstraintSolverData(rigidBodyComponents, jointComponents),
|
||||||
mSolveBallAndSocketJointSystem(rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents),
|
mSolveBallAndSocketJointSystem(rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents),
|
||||||
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents){
|
mSolveFixedJointSystem(rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents),
|
||||||
|
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents),
|
||||||
|
mFixedJointComponents(fixedJointComponents) {
|
||||||
|
|
||||||
#ifdef IS_PROFILING_ACTIVE
|
#ifdef IS_PROFILING_ACTIVE
|
||||||
|
|
||||||
|
|
@ -64,11 +67,15 @@ void ConstraintSolverSystem::initialize(decimal dt) {
|
||||||
|
|
||||||
mSolveBallAndSocketJointSystem.setTimeStep(dt);
|
mSolveBallAndSocketJointSystem.setTimeStep(dt);
|
||||||
mSolveBallAndSocketJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
mSolveBallAndSocketJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
||||||
|
mSolveFixedJointSystem.setTimeStep(dt);
|
||||||
|
mSolveFixedJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
||||||
|
|
||||||
mSolveBallAndSocketJointSystem.initBeforeSolve();
|
mSolveBallAndSocketJointSystem.initBeforeSolve();
|
||||||
|
mSolveFixedJointSystem.initBeforeSolve();
|
||||||
|
|
||||||
if (mIsWarmStartingActive) {
|
if (mIsWarmStartingActive) {
|
||||||
mSolveBallAndSocketJointSystem.warmstart();
|
mSolveBallAndSocketJointSystem.warmstart();
|
||||||
|
mSolveFixedJointSystem.warmstart();
|
||||||
}
|
}
|
||||||
|
|
||||||
// For each joint
|
// For each joint
|
||||||
|
|
@ -76,7 +83,8 @@ void ConstraintSolverSystem::initialize(decimal dt) {
|
||||||
|
|
||||||
// TODO : DELETE THIS
|
// TODO : DELETE THIS
|
||||||
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
||||||
if (mBallAndSocketJointComponents.hasComponent(jointEntity)) {
|
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
|
||||||
|
mFixedJointComponents.hasComponent(jointEntity)) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -101,13 +109,15 @@ void ConstraintSolverSystem::solveVelocityConstraints() {
|
||||||
RP3D_PROFILE("ConstraintSolverSystem::solveVelocityConstraints()", mProfiler);
|
RP3D_PROFILE("ConstraintSolverSystem::solveVelocityConstraints()", mProfiler);
|
||||||
|
|
||||||
mSolveBallAndSocketJointSystem.solveVelocityConstraint();
|
mSolveBallAndSocketJointSystem.solveVelocityConstraint();
|
||||||
|
mSolveFixedJointSystem.solveVelocityConstraint();
|
||||||
|
|
||||||
// For each joint
|
// For each joint
|
||||||
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
|
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
|
||||||
|
|
||||||
// TODO : DELETE THIS
|
// TODO : DELETE THIS
|
||||||
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
||||||
if (mBallAndSocketJointComponents.hasComponent(jointEntity)) {
|
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
|
||||||
|
mFixedJointComponents.hasComponent(jointEntity)) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -122,13 +132,15 @@ void ConstraintSolverSystem::solvePositionConstraints() {
|
||||||
RP3D_PROFILE("ConstraintSolverSystem::solvePositionConstraints()", mProfiler);
|
RP3D_PROFILE("ConstraintSolverSystem::solvePositionConstraints()", mProfiler);
|
||||||
|
|
||||||
mSolveBallAndSocketJointSystem.solvePositionConstraint();
|
mSolveBallAndSocketJointSystem.solvePositionConstraint();
|
||||||
|
mSolveFixedJointSystem.solvePositionConstraint();
|
||||||
|
|
||||||
// For each joint
|
// For each joint
|
||||||
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
|
for (uint i=0; i<mConstraintSolverData.jointComponents.getNbEnabledComponents(); i++) {
|
||||||
|
|
||||||
// TODO : DELETE THIS
|
// TODO : DELETE THIS
|
||||||
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
Entity jointEntity = mConstraintSolverData.jointComponents.mJointEntities[i];
|
||||||
if (mBallAndSocketJointComponents.hasComponent(jointEntity)) {
|
if (mBallAndSocketJointComponents.hasComponent(jointEntity) ||
|
||||||
|
mFixedJointComponents.hasComponent(jointEntity)) {
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -31,6 +31,7 @@
|
||||||
#include "mathematics/mathematics.h"
|
#include "mathematics/mathematics.h"
|
||||||
#include "engine/Islands.h"
|
#include "engine/Islands.h"
|
||||||
#include "systems/SolveBallAndSocketJointSystem.h"
|
#include "systems/SolveBallAndSocketJointSystem.h"
|
||||||
|
#include "systems/SolveFixedJointSystem.h"
|
||||||
|
|
||||||
namespace reactphysics3d {
|
namespace reactphysics3d {
|
||||||
|
|
||||||
|
|
@ -161,12 +162,18 @@ class ConstraintSolverSystem {
|
||||||
/// Solver for the BallAndSocketJoint constraints
|
/// Solver for the BallAndSocketJoint constraints
|
||||||
SolveBallAndSocketJointSystem mSolveBallAndSocketJointSystem;
|
SolveBallAndSocketJointSystem mSolveBallAndSocketJointSystem;
|
||||||
|
|
||||||
|
/// Solver for the FixedJoint constraints
|
||||||
|
SolveFixedJointSystem mSolveFixedJointSystem;
|
||||||
|
|
||||||
// TODO : Delete this
|
// TODO : Delete this
|
||||||
JointComponents& mJointComponents;
|
JointComponents& mJointComponents;
|
||||||
|
|
||||||
// TODO : Delete this
|
// TODO : Delete this
|
||||||
BallAndSocketJointComponents& mBallAndSocketJointComponents;
|
BallAndSocketJointComponents& mBallAndSocketJointComponents;
|
||||||
|
|
||||||
|
// TODO : Delete this
|
||||||
|
FixedJointComponents& mFixedJointComponents;
|
||||||
|
|
||||||
#ifdef IS_PROFILING_ACTIVE
|
#ifdef IS_PROFILING_ACTIVE
|
||||||
|
|
||||||
/// Pointer to the profiler
|
/// Pointer to the profiler
|
||||||
|
|
@ -181,7 +188,8 @@ class ConstraintSolverSystem {
|
||||||
ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
|
ConstraintSolverSystem(Islands& islands, RigidBodyComponents& rigidBodyComponents,
|
||||||
TransformComponents& transformComponents,
|
TransformComponents& transformComponents,
|
||||||
JointComponents& jointComponents,
|
JointComponents& jointComponents,
|
||||||
BallAndSocketJointComponents& ballAndSocketJointComponents);
|
BallAndSocketJointComponents& ballAndSocketJointComponents,
|
||||||
|
FixedJointComponents& fixedJointComponents);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
~ConstraintSolverSystem() = default;
|
~ConstraintSolverSystem() = default;
|
||||||
|
|
@ -216,6 +224,7 @@ class ConstraintSolverSystem {
|
||||||
inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) {
|
inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) {
|
||||||
mProfiler = profiler;
|
mProfiler = profiler;
|
||||||
mSolveBallAndSocketJointSystem.setProfiler(profiler);
|
mSolveBallAndSocketJointSystem.setProfiler(profiler);
|
||||||
|
mSolveFixedJointSystem.setProfiler(profiler);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -119,6 +119,8 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
const decimal biasFactor = (BETA / mTimeStep);
|
||||||
|
|
||||||
// For each joint
|
// For each joint
|
||||||
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
|
for (uint32 i=0; i < mBallAndSocketJointComponents.getNbEnabledComponents(); i++) {
|
||||||
|
|
||||||
|
|
@ -137,7 +139,6 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
||||||
// Compute the bias "b" of the constraint
|
// Compute the bias "b" of the constraint
|
||||||
mBallAndSocketJointComponents.mBiasVector[i].setToZero();
|
mBallAndSocketJointComponents.mBiasVector[i].setToZero();
|
||||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||||
decimal biasFactor = (BETA / mTimeStep);
|
|
||||||
mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World);
|
mBallAndSocketJointComponents.mBiasVector[i] = biasFactor * (x2 + r2World - x1 - r1World);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
545
src/systems/SolveFixedJointSystem.cpp
Normal file
545
src/systems/SolveFixedJointSystem.cpp
Normal 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
137
src/systems/SolveFixedJointSystem.h
Normal 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
|
||||||
Loading…
Reference in New Issue
Block a user