Add SolveSliderJointSystem class
This commit is contained in:
Daniel Chappuis
parent
afb34b4355
commit
d491665332
|
|
@ -135,6 +135,7 @@ SET (REACTPHYSICS3D_HEADERS
|
|||
"src/systems/SolveBallAndSocketJointSystem.h"
|
||||
"src/systems/SolveFixedJointSystem.h"
|
||||
"src/systems/SolveHingeJointSystem.h"
|
||||
"src/systems/SolveSliderJointSystem.h"
|
||||
"src/engine/DynamicsWorld.h"
|
||||
"src/engine/EventListener.h"
|
||||
"src/engine/Island.h"
|
||||
|
|
@ -234,6 +235,7 @@ SET (REACTPHYSICS3D_SOURCES
|
|||
"src/systems/SolveBallAndSocketJointSystem.cpp"
|
||||
"src/systems/SolveFixedJointSystem.cpp"
|
||||
"src/systems/SolveHingeJointSystem.cpp"
|
||||
"src/systems/SolveSliderJointSystem.cpp"
|
||||
"src/engine/DynamicsWorld.cpp"
|
||||
"src/engine/Island.cpp"
|
||||
"src/engine/Material.cpp"
|
||||
|
|
|
|||
|
|
@ -224,6 +224,7 @@ class RigidBody : public CollisionBody {
|
|||
friend class SolveBallAndSocketJointSystem;
|
||||
friend class SolveFixedJointSystem;
|
||||
friend class SolveHingeJointSystem;
|
||||
friend class SolveSliderJointSystem;
|
||||
};
|
||||
|
||||
// Return a reference to the material properties of the rigid body
|
||||
|
|
|
|||
|
|
@ -340,6 +340,7 @@ class RigidBodyComponents : public Components {
|
|||
friend class SolveBallAndSocketJointSystem;
|
||||
friend class SolveFixedJointSystem;
|
||||
friend class SolveHingeJointSystem;
|
||||
friend class SolveSliderJointSystem;
|
||||
friend class DynamicsSystem;
|
||||
friend class BallAndSocketJoint;
|
||||
friend class FixedJoint;
|
||||
|
|
|
|||
|
|
@ -456,6 +456,7 @@ class SliderJointComponents : public Components {
|
|||
// -------------------- Friendship -------------------- //
|
||||
|
||||
friend class BroadPhaseSystem;
|
||||
friend class SolveSliderJointSystem;
|
||||
};
|
||||
|
||||
// Return a pointer to a given joint
|
||||
|
|
|
|||
|
|
@ -101,31 +101,6 @@ class BallAndSocketJoint : public Joint {
|
|||
|
||||
/// Deleted assignment operator
|
||||
BallAndSocketJoint& operator=(const BallAndSocketJoint& constraint) = delete;
|
||||
|
||||
/// 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 {
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
// Return the number of bytes used by the joint
|
||||
|
|
|
|||
|
|
@ -56,26 +56,6 @@ FixedJoint::FixedJoint(Entity entity, DynamicsWorld &world, const FixedJointInfo
|
|||
mWorld.mFixedJointsComponents.setInitOrientationDifferenceInv(mEntity, transform2.getOrientation().getInverse() * transform1.getOrientation());
|
||||
}
|
||||
|
||||
// Initialize before solving the constraint
|
||||
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
}
|
||||
|
||||
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
void FixedJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
}
|
||||
|
||||
// Solve the velocity constraint
|
||||
void FixedJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
}
|
||||
|
||||
// Solve the position constraint (for position error correction)
|
||||
void FixedJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
}
|
||||
|
||||
// Return a string representation
|
||||
std::string FixedJoint::to_string() const {
|
||||
return "FixedJoint{ localAnchorPointBody1=" + mWorld.mFixedJointsComponents.getLocalAnchorPointBody1(mEntity).to_string() +
|
||||
|
|
|
|||
|
|
@ -73,22 +73,6 @@ class FixedJoint : public Joint {
|
|||
/// 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 :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
|
|||
|
|
@ -61,27 +61,6 @@ HingeJoint::HingeJoint(Entity entity, DynamicsWorld &world, const HingeJointInfo
|
|||
mWorld.mHingeJointsComponents.setInitOrientationDifferenceInv(mEntity, initOrientationDifferenceInv);
|
||||
}
|
||||
|
||||
// Initialize before solving the constraint
|
||||
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
}
|
||||
|
||||
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
void HingeJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
}
|
||||
|
||||
// Solve the velocity constraint
|
||||
void HingeJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
}
|
||||
|
||||
// Solve the position constraint (for position error correction)
|
||||
void HingeJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
}
|
||||
|
||||
|
||||
// Enable/Disable the limits of the joint
|
||||
/**
|
||||
* @param isLimitEnabled True if you want to enable the limits of the joint and
|
||||
|
|
|
|||
|
|
@ -155,18 +155,6 @@ class HingeJoint : public Joint {
|
|||
/// Return the number of bytes used by the joint
|
||||
virtual size_t getSizeInBytes() const override;
|
||||
|
||||
/// Initialize before solving the constraint
|
||||
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) override;
|
||||
|
||||
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
virtual void warmstart(const ConstraintSolverData& constraintSolverData) override;
|
||||
|
||||
/// Solve the velocity constraint
|
||||
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) override;
|
||||
|
||||
/// Solve the position constraint (for position error correction)
|
||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) override;
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
|
|||
|
|
@ -106,22 +106,6 @@ class Joint {
|
|||
/// Return the number of bytes used by the joint
|
||||
virtual size_t getSizeInBytes() const = 0;
|
||||
|
||||
/// Initialize before solving the joint
|
||||
// TODO : REMOVE THIS
|
||||
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) = 0;
|
||||
|
||||
/// Warm start the joint (apply the previous impulse at the beginning of the step)
|
||||
// TODO : REMOVE THIS
|
||||
virtual void warmstart(const ConstraintSolverData& constraintSolverData) = 0;
|
||||
|
||||
/// Solve the velocity constraint
|
||||
// TODO : REMOVE THIS
|
||||
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) = 0;
|
||||
|
||||
/// Solve the position constraint
|
||||
// TODO : REMOVE THIS
|
||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0;
|
||||
|
||||
/// Awake the two bodies of the joint
|
||||
void awakeBodies() const;
|
||||
|
||||
|
|
|
|||
|
|
@ -70,727 +70,6 @@ SliderJoint::SliderJoint(Entity entity, DynamicsWorld &world, const SliderJointI
|
|||
mWorld.mSliderJointsComponents.setSliderAxisBody1(mEntity, sliderAxisBody1);
|
||||
}
|
||||
|
||||
// Initialize before solving the constraint
|
||||
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
|
||||
const 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 = mWorld.mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
const Quaternion& orientationBody2 = mWorld.mTransformComponents.getTransform(body2Entity).getOrientation();
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mWorld.mSliderJointsComponents.setI1(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity));
|
||||
mWorld.mSliderJointsComponents.setI2(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity));
|
||||
|
||||
// Vector from body center to the anchor point
|
||||
mWorld.mSliderJointsComponents.setR1(mEntity, orientationBody1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity));
|
||||
mWorld.mSliderJointsComponents.setR2(mEntity, orientationBody2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity));
|
||||
|
||||
const Vector3& r1 = mWorld.mSliderJointsComponents.getR1(mEntity);
|
||||
const Vector3& r2 = mWorld.mSliderJointsComponents.getR2(mEntity);
|
||||
|
||||
// Compute the vector u (difference between anchor points)
|
||||
const Vector3 u = x2 + r2 - x1 - r1;
|
||||
|
||||
// Compute the two orthogonal vectors to the slider axis in world-space
|
||||
Vector3 sliderAxisWorld = orientationBody1 * mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity);
|
||||
sliderAxisWorld.normalize();
|
||||
mWorld.mSliderJointsComponents.setSliderAxisWorld(mEntity, sliderAxisWorld);
|
||||
mWorld.mSliderJointsComponents.setN1(mEntity, sliderAxisWorld.getOneUnitOrthogonalVector());
|
||||
const Vector3& n1 = mWorld.mSliderJointsComponents.getN1(mEntity);
|
||||
mWorld.mSliderJointsComponents.setN2(mEntity, sliderAxisWorld.cross(n1));
|
||||
const Vector3& n2 = mWorld.mSliderJointsComponents.getN2(mEntity);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal uDotSliderAxis = u.dot(sliderAxisWorld);
|
||||
decimal lowerLimitError = uDotSliderAxis - mWorld.mSliderJointsComponents.getLowerLimit(mEntity);
|
||||
decimal upperLimitError = mWorld.mSliderJointsComponents.getUpperLimit(mEntity) - uDotSliderAxis;
|
||||
bool oldIsLowerLimitViolated = mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity);
|
||||
bool isLowerLimitViolated = lowerLimitError <= 0;
|
||||
mWorld.mSliderJointsComponents.setIsLowerLimitViolated(mEntity, isLowerLimitViolated);
|
||||
if (isLowerLimitViolated != oldIsLowerLimitViolated) {
|
||||
mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
|
||||
}
|
||||
bool oldIsUpperLimitViolated = mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity);
|
||||
bool isUpperLimitViolated = upperLimitError <= 0;
|
||||
mWorld.mSliderJointsComponents.setIsUpperLimitViolated(mEntity, isUpperLimitViolated);
|
||||
if (isUpperLimitViolated != oldIsUpperLimitViolated) {
|
||||
mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
|
||||
}
|
||||
|
||||
// Compute the cross products used in the Jacobians
|
||||
mWorld.mSliderJointsComponents.setR2CrossN1(mEntity, r2.cross(n1));
|
||||
mWorld.mSliderJointsComponents.setR2CrossN2(mEntity, r2.cross(n2));
|
||||
mWorld.mSliderJointsComponents.setR2CrossSliderAxis(mEntity, r2.cross(sliderAxisWorld));
|
||||
const Vector3 r1PlusU = r1 + u;
|
||||
mWorld.mSliderJointsComponents.setR1PlusUCrossN1(mEntity, r1PlusU.cross(n1));
|
||||
mWorld.mSliderJointsComponents.setR1PlusUCrossN2(mEntity, r1PlusU.cross(n2));
|
||||
mWorld.mSliderJointsComponents.setR1PlusUCrossSliderAxis(mEntity, r1PlusU.cross(sliderAxisWorld));
|
||||
|
||||
const Vector3& r2CrossN1 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity);
|
||||
const Vector3& r2CrossN2 = mWorld.mSliderJointsComponents.getR2CrossN2(mEntity);
|
||||
const Vector3& r1PlusUCrossN1 = mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity);
|
||||
const Vector3& r1PlusUCrossN2 = mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity);
|
||||
const Vector3& r2CrossSliderAxis = mWorld.mSliderJointsComponents.getR2CrossSliderAxis(mEntity);
|
||||
const Vector3& r1PlusUCrossSliderAxis = mWorld.mSliderJointsComponents.getR1PlusUCrossSliderAxis(mEntity);
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity);
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||
// constraints (2x2 matrix)
|
||||
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
const decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1;
|
||||
Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2;
|
||||
Vector3 I2R2CrossN1 = i2 * r2CrossN1;
|
||||
Vector3 I2R2CrossN2 = i2 * r2CrossN2;
|
||||
const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN1.dot(I2R2CrossN1);
|
||||
const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN1.dot(I2R2CrossN2);
|
||||
const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN2.dot(I2R2CrossN1);
|
||||
const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN2.dot(I2R2CrossN2);
|
||||
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||
Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixTranslation(mEntity, matrixKTranslation.getInverse());
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the translation constraint
|
||||
Vector2& biasTranslation = mWorld.mSliderJointsComponents.getBiasTranslation(mEntity);
|
||||
biasTranslation.setToZero();
|
||||
decimal biasFactor = (BETA / constraintSolverData.timeStep);
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
biasTranslation.x = u.dot(n1);
|
||||
biasTranslation.y = u.dot(n2);
|
||||
biasTranslation *= biasFactor;
|
||||
mWorld.mSliderJointsComponents.setBiasTranslation(mEntity, biasTranslation);
|
||||
}
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||
// contraints (3x3 matrix)
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, i1 + i2);
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity).getInverse());
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the rotation constraint
|
||||
Vector3& biasRotation = mWorld.mSliderJointsComponents.getBiasRotation(mEntity);
|
||||
biasRotation.setToZero();
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
const Quaternion qError = orientationBody2 * mWorld.mSliderJointsComponents.getInitOrientationDifferenceInv(mEntity) * orientationBody1.getInverse();
|
||||
mWorld.mSliderJointsComponents.setBiasRotation(mEntity, biasFactor * decimal(2.0) * qError.getVectorV());
|
||||
}
|
||||
|
||||
// If the limits are enabled
|
||||
if (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity) && (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity) ||
|
||||
mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity))) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||
decimal inverseMassMatrixLimit = sumInverseMass +
|
||||
r1PlusUCrossSliderAxis.dot(i1 * r1PlusUCrossSliderAxis) +
|
||||
r2CrossSliderAxis.dot(i2 * r2CrossSliderAxis);
|
||||
inverseMassMatrixLimit = (inverseMassMatrixLimit > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixLimit : decimal(0.0);
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixLimit(mEntity, inverseMassMatrixLimit);
|
||||
|
||||
// Compute the bias "b" of the lower limit constraint
|
||||
mWorld.mSliderJointsComponents.setBLowerLimit(mEntity, decimal(0.0));
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mWorld.mSliderJointsComponents.setBLowerLimit(mEntity, biasFactor * lowerLimitError);
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the upper limit constraint
|
||||
mWorld.mSliderJointsComponents.setBUpperLimit(mEntity, decimal(0.0));
|
||||
if (mWorld.mJointsComponents.getPositionCorrectionTechnique(mEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mWorld.mSliderJointsComponents.setBUpperLimit(mEntity, biasFactor * upperLimitError);
|
||||
}
|
||||
}
|
||||
|
||||
// If the motor is enabled
|
||||
if (mWorld.mSliderJointsComponents.getIsMotorEnabled(mEntity)) {
|
||||
|
||||
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
|
||||
decimal inverseMassMatrixMotor = sumInverseMass;
|
||||
inverseMassMatrixMotor = (inverseMassMatrixMotor > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixMotor : decimal(0.0);
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixMotor(mEntity, inverseMassMatrixMotor);
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!constraintSolverData.isWarmStartingActive) {
|
||||
|
||||
// Reset all the accumulated impulses
|
||||
Vector2& impulseTranslation = mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity);
|
||||
Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity);
|
||||
impulseTranslation.setToZero();
|
||||
impulseRotation.setToZero();
|
||||
mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, decimal(0.0));
|
||||
mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, decimal(0.0));
|
||||
mWorld.mSliderJointsComponents.setImpulseMotor(mEntity, decimal(0.0));
|
||||
}
|
||||
}
|
||||
|
||||
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
|
||||
const 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 and inverse inertia tensors of the bodies
|
||||
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
const Vector3& n1 = mWorld.mSliderJointsComponents.getN1(mEntity);
|
||||
const Vector3& n2 = mWorld.mSliderJointsComponents.getN2(mEntity);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
|
||||
decimal impulseLimits = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) - mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity);
|
||||
Vector3 linearImpulseLimits = impulseLimits * mWorld.mSliderJointsComponents.getSliderAxisWorld(mEntity);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
|
||||
Vector3 impulseMotor = mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) * mWorld.mSliderJointsComponents.getSliderAxisWorld(mEntity);
|
||||
|
||||
const Vector2& impulseTranslation = mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity);
|
||||
const Vector3& impulseRotation = mWorld.mSliderJointsComponents.getImpulseRotation(mEntity);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
Vector3 linearImpulseBody1 = -n1 * impulseTranslation.x - n2 * impulseTranslation.y;
|
||||
Vector3 angularImpulseBody1 = -mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity) * impulseTranslation.x -
|
||||
mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity) * impulseTranslation.y;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 += -impulseRotation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
|
||||
linearImpulseBody1 += linearImpulseLimits;
|
||||
angularImpulseBody1 += impulseLimits * mWorld.mSliderJointsComponents.getR1PlusUCrossSliderAxis(mEntity);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
|
||||
linearImpulseBody1 += impulseMotor;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mWorld.mSliderJointsComponents.getI1(mEntity) * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
|
||||
Vector3 linearImpulseBody2 = n1 * impulseTranslation.x + n2 * impulseTranslation.y;
|
||||
Vector3 angularImpulseBody2 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity) * impulseTranslation.x +
|
||||
mWorld.mSliderJointsComponents.getR2CrossN2(mEntity) * impulseTranslation.y;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
angularImpulseBody2 += impulseRotation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
|
||||
linearImpulseBody2 += -linearImpulseLimits;
|
||||
angularImpulseBody2 += -impulseLimits * mWorld.mSliderJointsComponents.getR2CrossSliderAxis(mEntity);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 2
|
||||
linearImpulseBody2 += -impulseMotor;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += mWorld.mSliderJointsComponents.getI2(mEntity) * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// Solve the velocity constraint
|
||||
void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
|
||||
const 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];
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity);
|
||||
|
||||
const Vector3& n1 = mWorld.mSliderJointsComponents.getN1(mEntity);
|
||||
const Vector3& n2 = mWorld.mSliderJointsComponents.getN2(mEntity);
|
||||
|
||||
const Vector3& r2CrossN1 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity);
|
||||
const Vector3& r2CrossN2 = mWorld.mSliderJointsComponents.getR2CrossN2(mEntity);
|
||||
const Vector3& r1PlusUCrossN1 = mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity);
|
||||
const Vector3& r1PlusUCrossN2 = mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity);
|
||||
const Vector3& r2CrossSliderAxis = mWorld.mSliderJointsComponents.getR2CrossSliderAxis(mEntity);
|
||||
const Vector3& r1PlusUCrossSliderAxis = mWorld.mSliderJointsComponents.getR1PlusUCrossSliderAxis(mEntity);
|
||||
|
||||
// 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& sliderAxisWorld = mWorld.mSliderJointsComponents.getSliderAxisWorld(mEntity);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 2 translation constraints
|
||||
const decimal el1 = -n1.dot(v1) - w1.dot(r1PlusUCrossN1) +
|
||||
n1.dot(v2) + w2.dot(r2CrossN1);
|
||||
const decimal el2 = -n2.dot(v1) - w1.dot(r1PlusUCrossN2) +
|
||||
n2.dot(v2) + w2.dot(r2CrossN2);
|
||||
const Vector2 JvTranslation(el1, el2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
Vector2 deltaLambda = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity) * (-JvTranslation - mWorld.mSliderJointsComponents.getBiasTranslation(mEntity));
|
||||
mWorld.mSliderJointsComponents.setImpulseTranslation(mEntity, deltaLambda + mWorld.mSliderJointsComponents.getImpulseTranslation(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * deltaLambda.x - n2 * deltaLambda.y;
|
||||
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * deltaLambda.x -
|
||||
r1PlusUCrossN2 * deltaLambda.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
|
||||
const Vector3 linearImpulseBody2 = n1 * deltaLambda.x + n2 * deltaLambda.y;
|
||||
Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 3 rotation constraints
|
||||
const Vector3 JvRotation = w2 - w1;
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector3 deltaLambda2 = mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity) *
|
||||
(-JvRotation - mWorld.mSliderJointsComponents.getBiasRotation(mEntity));
|
||||
mWorld.mSliderJointsComponents.setImpulseRotation(mEntity, deltaLambda2 + mWorld.mSliderJointsComponents.getImpulseRotation(mEntity));
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 = -deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body to body 1
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
angularImpulseBody2 = deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
const decimal inverseMassMatrixLimit = mWorld.mSliderJointsComponents.getInverseMassMatrixLimit(mEntity);
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity)) {
|
||||
|
||||
// Compute J*v for the lower limit constraint
|
||||
const decimal JvLowerLimit = sliderAxisWorld.dot(v2) + r2CrossSliderAxis.dot(w2) -
|
||||
sliderAxisWorld.dot(v1) - r1PlusUCrossSliderAxis.dot(w1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal deltaLambdaLower = inverseMassMatrixLimit * (-JvLowerLimit - mWorld.mSliderJointsComponents.getBLowerLimit(mEntity));
|
||||
decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity);
|
||||
mWorld.mSliderJointsComponents.setImpulseLowerLimit(mEntity, std::max(mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity) + deltaLambdaLower, decimal(0.0)));
|
||||
deltaLambdaLower = mWorld.mSliderJointsComponents.getImpulseLowerLimit(mEntity) - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambdaLower * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaLower * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = deltaLambdaLower * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaLower * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
// Compute J*v for the upper limit constraint
|
||||
const decimal JvUpperLimit = sliderAxisWorld.dot(v1) + r1PlusUCrossSliderAxis.dot(w1)
|
||||
- sliderAxisWorld.dot(v2) - r2CrossSliderAxis.dot(w2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal deltaLambdaUpper = inverseMassMatrixLimit * (-JvUpperLimit -mWorld.mSliderJointsComponents.getBUpperLimit(mEntity));
|
||||
decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity);
|
||||
mWorld.mSliderJointsComponents.setImpulseUpperLimit(mEntity, std::max(mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) + deltaLambdaUpper, decimal(0.0)));
|
||||
deltaLambdaUpper = mWorld.mSliderJointsComponents.getImpulseUpperLimit(mEntity) - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = deltaLambdaUpper * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody1 = deltaLambdaUpper * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Motor --------------- //
|
||||
|
||||
if (mWorld.mSliderJointsComponents.getIsMotorEnabled(mEntity)) {
|
||||
|
||||
// Compute J*v for the motor
|
||||
const decimal JvMotor = sliderAxisWorld.dot(v1) - sliderAxisWorld.dot(v2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the motor
|
||||
const decimal maxMotorImpulse = mWorld.mSliderJointsComponents.getMaxMotorForce(mEntity) * constraintSolverData.timeStep;
|
||||
decimal deltaLambdaMotor = mWorld.mSliderJointsComponents.getInverseMassMatrixMotor(mEntity) * (-JvMotor - mWorld.mSliderJointsComponents.getMotorSpeed(mEntity));
|
||||
decimal lambdaTemp = mWorld.mSliderJointsComponents.getImpulseMotor(mEntity);
|
||||
mWorld.mSliderJointsComponents.setImpulseMotor(mEntity, clamp(mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse));
|
||||
deltaLambdaMotor = mWorld.mSliderJointsComponents.getImpulseMotor(mEntity) - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 1
|
||||
const Vector3 linearImpulseBody1 = deltaLambdaMotor * sliderAxisWorld;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 2
|
||||
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * sliderAxisWorld;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the position constraint (for position error correction)
|
||||
void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||
|
||||
// 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 entities
|
||||
const Entity body1Entity = mWorld.mJointsComponents.getBody1Entity(mEntity);
|
||||
const 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
|
||||
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
|
||||
const decimal inverseMassBody1 = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal inverseMassBody2 = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
|
||||
// Recompute the inertia tensor of bodies
|
||||
mWorld.mSliderJointsComponents.setI1(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity));
|
||||
mWorld.mSliderJointsComponents.setI2(mEntity, RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity));
|
||||
|
||||
// Vector from body center to the anchor point
|
||||
mWorld.mSliderJointsComponents.setR1(mEntity, q1 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody1(mEntity));
|
||||
mWorld.mSliderJointsComponents.setR2(mEntity, q2 * mWorld.mSliderJointsComponents.getLocalAnchorPointBody2(mEntity));
|
||||
|
||||
const Vector3& r1 = mWorld.mSliderJointsComponents.getR1(mEntity);
|
||||
const Vector3& r2 = mWorld.mSliderJointsComponents.getR2(mEntity);
|
||||
|
||||
const Vector3& n1 = mWorld.mSliderJointsComponents.getN1(mEntity);
|
||||
const Vector3& n2 = mWorld.mSliderJointsComponents.getN2(mEntity);
|
||||
|
||||
// Compute the vector u (difference between anchor points)
|
||||
const Vector3 u = x2 + r2 - x1 - r1;
|
||||
|
||||
// Compute the two orthogonal vectors to the slider axis in world-space
|
||||
Vector3 sliderAxisWorld = q1 * mWorld.mSliderJointsComponents.getSliderAxisBody1(mEntity);
|
||||
sliderAxisWorld.normalize();
|
||||
mWorld.mSliderJointsComponents.setSliderAxisWorld(mEntity, sliderAxisWorld);
|
||||
mWorld.mSliderJointsComponents.setN1(mEntity, sliderAxisWorld.getOneUnitOrthogonalVector());
|
||||
mWorld.mSliderJointsComponents.setN2(mEntity, sliderAxisWorld.cross(n1));
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal uDotSliderAxis = u.dot(sliderAxisWorld);
|
||||
decimal lowerLimitError = uDotSliderAxis - mWorld.mSliderJointsComponents.getLowerLimit(mEntity);
|
||||
decimal upperLimitError = mWorld.mSliderJointsComponents.getUpperLimit(mEntity) - uDotSliderAxis;
|
||||
mWorld.mSliderJointsComponents.setIsLowerLimitViolated(mEntity, lowerLimitError <= 0);
|
||||
mWorld.mSliderJointsComponents.setIsUpperLimitViolated(mEntity, upperLimitError <= 0);
|
||||
|
||||
// Compute the cross products used in the Jacobians
|
||||
mWorld.mSliderJointsComponents.setR2CrossN1(mEntity, r2.cross(n1));
|
||||
mWorld.mSliderJointsComponents.setR2CrossN2(mEntity, r2.cross(n2));
|
||||
mWorld.mSliderJointsComponents.setR2CrossSliderAxis(mEntity, r2.cross(sliderAxisWorld));
|
||||
const Vector3 r1PlusU = r1 + u;
|
||||
mWorld.mSliderJointsComponents.setR1PlusUCrossN1(mEntity, r1PlusU.cross(n1));
|
||||
mWorld.mSliderJointsComponents.setR1PlusUCrossN2(mEntity, r1PlusU.cross(n2));
|
||||
mWorld.mSliderJointsComponents.setR1PlusUCrossSliderAxis(mEntity, r1PlusU.cross(sliderAxisWorld));
|
||||
|
||||
const Vector3& r2CrossN1 = mWorld.mSliderJointsComponents.getR2CrossN1(mEntity);
|
||||
const Vector3& r2CrossN2 = mWorld.mSliderJointsComponents.getR2CrossN2(mEntity);
|
||||
const Vector3& r1PlusUCrossN1 = mWorld.mSliderJointsComponents.getR1PlusUCrossN1(mEntity);
|
||||
const Vector3& r1PlusUCrossN2 = mWorld.mSliderJointsComponents.getR1PlusUCrossN2(mEntity);
|
||||
const Vector3& r2CrossSliderAxis = mWorld.mSliderJointsComponents.getR2CrossSliderAxis(mEntity);
|
||||
const Vector3& r1PlusUCrossSliderAxis = mWorld.mSliderJointsComponents.getR1PlusUCrossSliderAxis(mEntity);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
const Matrix3x3& i1 = mWorld.mSliderJointsComponents.getI1(mEntity);
|
||||
const Matrix3x3& i2 = mWorld.mSliderJointsComponents.getI2(mEntity);
|
||||
|
||||
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||
// constraints (2x2 matrix)
|
||||
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1;
|
||||
Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2;
|
||||
Vector3 I2R2CrossN1 = i2 * r2CrossN1;
|
||||
Vector3 I2R2CrossN2 = i2 * r2CrossN2;
|
||||
const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN1.dot(I2R2CrossN1);
|
||||
const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN1.dot(I2R2CrossN2);
|
||||
const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN2.dot(I2R2CrossN1);
|
||||
const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN2.dot(I2R2CrossN2);
|
||||
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||
Matrix2x2& inverseMassMatrixTranslation = mWorld.mSliderJointsComponents.getInverseMassMatrixTranslation(mEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixTranslation(mEntity, matrixKTranslation.getInverse());
|
||||
}
|
||||
|
||||
// Compute the position error for the 2 translation constraints
|
||||
const Vector2 translationError(u.dot(n1), u.dot(n2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
Vector2 lambdaTranslation = inverseMassMatrixTranslation * (-translationError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;
|
||||
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x -
|
||||
r1PlusUCrossN2 * lambdaTranslation.y;
|
||||
|
||||
// Apply the impulse to the 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 P=J^T * lambda for the 2 translation constraints of body 2
|
||||
const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y;
|
||||
Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
|
||||
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)
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, i1 + i2);
|
||||
if (mWorld.mRigidBodyComponents.getBodyType(body1Entity) == BodyType::DYNAMIC ||
|
||||
mWorld.mRigidBodyComponents.getBodyType(body2Entity) == BodyType::DYNAMIC) {
|
||||
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixRotation(mEntity, mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity).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.mSliderJointsComponents.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 = mWorld.mSliderJointsComponents.getInverseMassMatrixRotation(mEntity) * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 = -lambdaRotation;
|
||||
|
||||
// Apply the impulse to the 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 impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
angularImpulseBody2 = lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mWorld.mSliderJointsComponents.getIsLimitEnabled(mEntity)) {
|
||||
|
||||
if (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity) || mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||
const decimal body1MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal body2MassInverse = constraintSolverData.rigidBodyComponents.getMassInverse(body2Entity);
|
||||
decimal inverseMassMatrixLimit = body1MassInverse + body2MassInverse +
|
||||
r1PlusUCrossSliderAxis.dot(i1 * r1PlusUCrossSliderAxis) +
|
||||
r2CrossSliderAxis.dot(i2 * r2CrossSliderAxis);
|
||||
inverseMassMatrixLimit = (inverseMassMatrixLimit > decimal(0.0)) ?
|
||||
decimal(1.0) / inverseMassMatrixLimit : decimal(0.0);
|
||||
mWorld.mSliderJointsComponents.setInverseMassMatrixLimit(mEntity, inverseMassMatrixLimit);
|
||||
}
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mWorld.mSliderJointsComponents.getIsLowerLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal lambdaLowerLimit = mWorld.mSliderJointsComponents.getInverseMassMatrixLimit(mEntity) * (-lowerLimitError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
const 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 P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = lambdaLowerLimit * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody2 = lambdaLowerLimit * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
|
||||
const Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mWorld.mSliderJointsComponents.getIsUpperLimitViolated(mEntity)) {
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal lambdaUpperLimit = mWorld.mSliderJointsComponents.getInverseMassMatrixLimit(mEntity) * (-upperLimitError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = lambdaUpperLimit * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody1 = lambdaUpperLimit * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
const 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 P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
|
||||
const Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
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);
|
||||
}
|
||||
|
||||
// Enable/Disable the limits of the joint
|
||||
/**
|
||||
* @param isLimitEnabled True if you want to enable the joint limits and false
|
||||
|
|
|
|||
|
|
@ -158,18 +158,6 @@ class SliderJoint : public Joint {
|
|||
/// Return the number of bytes used by the joint
|
||||
virtual size_t getSizeInBytes() const override;
|
||||
|
||||
/// Initialize before solving the constraint
|
||||
virtual void initBeforeSolve(const ConstraintSolverData& constraintSolverData) override;
|
||||
|
||||
/// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
virtual void warmstart(const ConstraintSolverData& constraintSolverData) override;
|
||||
|
||||
/// Solve the velocity constraint
|
||||
virtual void solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) override;
|
||||
|
||||
/// Solve the position constraint (for position error correction)
|
||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) override;
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
|
|||
|
|
@ -53,7 +53,8 @@ DynamicsWorld::DynamicsWorld(const Vector3& gravity, const WorldSettings& worldS
|
|||
mContactSolverSystem(mMemoryManager, *this, mIslands, mCollisionBodyComponents, mRigidBodyComponents,
|
||||
mProxyShapesComponents, mConfig),
|
||||
mConstraintSolverSystem(*this, mIslands, mRigidBodyComponents, mTransformComponents, mJointsComponents,
|
||||
mBallAndSocketJointsComponents, mFixedJointsComponents, mHingeJointsComponents),
|
||||
mBallAndSocketJointsComponents, mFixedJointsComponents, mHingeJointsComponents,
|
||||
mSliderJointsComponents),
|
||||
mDynamicsSystem(*this, mRigidBodyComponents, mTransformComponents, mIsGravityEnabled, mGravity),
|
||||
mNbVelocitySolverIterations(mConfig.defaultVelocitySolverNbIterations),
|
||||
mNbPositionSolverIterations(mConfig.defaultPositionSolverNbIterations),
|
||||
|
|
|
|||
|
|
@ -38,12 +38,14 @@ ConstraintSolverSystem::ConstraintSolverSystem(DynamicsWorld& world, Islands& is
|
|||
JointComponents& jointComponents,
|
||||
BallAndSocketJointComponents& ballAndSocketJointComponents,
|
||||
FixedJointComponents& fixedJointComponents,
|
||||
HingeJointComponents& hingeJointComponents)
|
||||
HingeJointComponents& hingeJointComponents,
|
||||
SliderJointComponents& sliderJointComponents)
|
||||
: mIsWarmStartingActive(true), mIslands(islands),
|
||||
mConstraintSolverData(rigidBodyComponents, jointComponents),
|
||||
mSolveBallAndSocketJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, ballAndSocketJointComponents),
|
||||
mSolveFixedJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, fixedJointComponents),
|
||||
mSolveHingeJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, hingeJointComponents),
|
||||
mSolveSliderJointSystem(world, rigidBodyComponents, transformComponents, jointComponents, sliderJointComponents),
|
||||
mJointComponents(jointComponents), mBallAndSocketJointComponents(ballAndSocketJointComponents),
|
||||
mFixedJointComponents(fixedJointComponents), mHingeJointComponents(hingeJointComponents) {
|
||||
|
||||
|
|
@ -73,40 +75,19 @@ void ConstraintSolverSystem::initialize(decimal dt) {
|
|||
mSolveFixedJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
||||
mSolveHingeJointSystem.setTimeStep(dt);
|
||||
mSolveHingeJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
||||
mSolveSliderJointSystem.setTimeStep(dt);
|
||||
mSolveSliderJointSystem.setIsWarmStartingActive(mIsWarmStartingActive);
|
||||
|
||||
mSolveBallAndSocketJointSystem.initBeforeSolve();
|
||||
mSolveFixedJointSystem.initBeforeSolve();
|
||||
mSolveHingeJointSystem.initBeforeSolve();
|
||||
mSolveSliderJointSystem.initBeforeSolve();
|
||||
|
||||
if (mIsWarmStartingActive) {
|
||||
mSolveBallAndSocketJointSystem.warmstart();
|
||||
mSolveFixedJointSystem.warmstart();
|
||||
mSolveHingeJointSystem.warmstart();
|
||||
}
|
||||
|
||||
// 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) ||
|
||||
mFixedJointComponents.hasComponent(jointEntity) ||
|
||||
mHingeJointComponents.hasComponent(jointEntity)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const Entity body1 = mConstraintSolverData.jointComponents.mBody1Entities[i];
|
||||
const Entity body2 = mConstraintSolverData.jointComponents.mBody2Entities[i];
|
||||
assert(!mConstraintSolverData.rigidBodyComponents.getIsEntityDisabled(body1));
|
||||
assert(!mConstraintSolverData.rigidBodyComponents.getIsEntityDisabled(body2));
|
||||
|
||||
// Initialize the constraint before solving it
|
||||
mJointComponents.mJoints[i]->initBeforeSolve(mConstraintSolverData);
|
||||
|
||||
// Warm-start the constraint if warm-starting is enabled
|
||||
if (mIsWarmStartingActive) {
|
||||
mConstraintSolverData.jointComponents.mJoints[i]->warmstart(mConstraintSolverData);
|
||||
}
|
||||
mSolveSliderJointSystem.warmstart();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -118,21 +99,7 @@ void ConstraintSolverSystem::solveVelocityConstraints() {
|
|||
mSolveBallAndSocketJointSystem.solveVelocityConstraint();
|
||||
mSolveFixedJointSystem.solveVelocityConstraint();
|
||||
mSolveHingeJointSystem.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) ||
|
||||
mFixedJointComponents.hasComponent(jointEntity) ||
|
||||
mHingeJointComponents.hasComponent(jointEntity)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Solve the constraint
|
||||
mConstraintSolverData.jointComponents.mJoints[i]->solveVelocityConstraint(mConstraintSolverData);
|
||||
}
|
||||
mSolveSliderJointSystem.solveVelocityConstraint();
|
||||
}
|
||||
|
||||
// Solve the position constraints
|
||||
|
|
@ -143,19 +110,5 @@ void ConstraintSolverSystem::solvePositionConstraints() {
|
|||
mSolveBallAndSocketJointSystem.solvePositionConstraint();
|
||||
mSolveFixedJointSystem.solvePositionConstraint();
|
||||
mSolveHingeJointSystem.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) ||
|
||||
mFixedJointComponents.hasComponent(jointEntity) ||
|
||||
mHingeJointComponents.hasComponent(jointEntity)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Solve the constraint
|
||||
mConstraintSolverData.jointComponents.mJoints[i]->solvePositionConstraint(mConstraintSolverData);
|
||||
}
|
||||
mSolveSliderJointSystem.solvePositionConstraint();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@
|
|||
#include "systems/SolveBallAndSocketJointSystem.h"
|
||||
#include "systems/SolveFixedJointSystem.h"
|
||||
#include "systems/SolveHingeJointSystem.h"
|
||||
#include "systems/SolveSliderJointSystem.h"
|
||||
|
||||
namespace reactphysics3d {
|
||||
|
||||
|
|
@ -169,6 +170,9 @@ class ConstraintSolverSystem {
|
|||
/// Solver for the HingeJoint constraints
|
||||
SolveHingeJointSystem mSolveHingeJointSystem;
|
||||
|
||||
/// Solver for the SliderJoint constraints
|
||||
SolveSliderJointSystem mSolveSliderJointSystem;
|
||||
|
||||
// TODO : Delete this
|
||||
JointComponents& mJointComponents;
|
||||
|
||||
|
|
@ -196,7 +200,8 @@ class ConstraintSolverSystem {
|
|||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
BallAndSocketJointComponents& ballAndSocketJointComponents,
|
||||
FixedJointComponents& fixedJointComponents, HingeJointComponents &hingeJointComponents);
|
||||
FixedJointComponents& fixedJointComponents, HingeJointComponents &hingeJointComponents,
|
||||
SliderJointComponents& sliderJointComponents);
|
||||
|
||||
/// Destructor
|
||||
~ConstraintSolverSystem() = default;
|
||||
|
|
@ -232,6 +237,8 @@ inline void ConstraintSolverSystem::setProfiler(Profiler* profiler) {
|
|||
mProfiler = profiler;
|
||||
mSolveBallAndSocketJointSystem.setProfiler(profiler);
|
||||
mSolveFixedJointSystem.setProfiler(profiler);
|
||||
mSolveHingeJointSystem.setProfiler(profiler);
|
||||
mSolveSliderJointSystem.setProfiler(profiler);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -56,6 +56,9 @@ void SolveBallAndSocketJointSystem::initBeforeSolve() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mBallAndSocketJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mBallAndSocketJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
|
|
|
|||
|
|
@ -56,6 +56,9 @@ void SolveFixedJointSystem::initBeforeSolve() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mFixedJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mFixedJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
|
|
|
|||
|
|
@ -56,6 +56,9 @@ void SolveHingeJointSystem::initBeforeSolve() {
|
|||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mHingeJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mHingeJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
|
|
|
|||
962
src/systems/SolveSliderJointSystem.cpp
Normal file
962
src/systems/SolveSliderJointSystem.cpp
Normal file
|
|
@ -0,0 +1,962 @@
|
|||
/********************************************************************************
|
||||
* 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/SolveSliderJointSystem.h"
|
||||
#include "engine/DynamicsWorld.h"
|
||||
#include "body/RigidBody.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Static variables definition
|
||||
const decimal SolveSliderJointSystem::BETA = decimal(0.2);
|
||||
|
||||
// Constructor
|
||||
SolveSliderJointSystem::SolveSliderJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
SliderJointComponents& sliderJointComponents)
|
||||
:mWorld(world), mRigidBodyComponents(rigidBodyComponents), mTransformComponents(transformComponents),
|
||||
mJointComponents(jointComponents), mSliderJointComponents(sliderJointComponents),
|
||||
mTimeStep(0), mIsWarmStartingActive(true) {
|
||||
|
||||
}
|
||||
|
||||
// Initialize before solving the constraint
|
||||
void SolveSliderJointSystem::initBeforeSolve() {
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body1Entity));
|
||||
assert(!mRigidBodyComponents.getIsEntityDisabled(body2Entity));
|
||||
|
||||
// Get the inertia tensor of bodies
|
||||
mSliderJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mSliderJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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();
|
||||
|
||||
// Vector from body center to the anchor point
|
||||
mSliderJointComponents.mR1[i] = orientationBody1 * mSliderJointComponents.mLocalAnchorPointBody1[i];
|
||||
mSliderJointComponents.mR2[i] = orientationBody2 * mSliderJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
|
||||
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Quaternion& orientationBody1 = mTransformComponents.getTransform(body1Entity).getOrientation();
|
||||
|
||||
// Compute the two orthogonal vectors to the slider axis in world-space
|
||||
mSliderJointComponents.mSliderAxisWorld[i] = orientationBody1 * mSliderJointComponents.mSliderAxisBody1[i];
|
||||
mSliderJointComponents.mSliderAxisWorld[i].normalize();
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
mSliderJointComponents.mN1[i] = mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector();
|
||||
mSliderJointComponents.mN2[i] = mSliderJointComponents.mSliderAxisWorld[i].cross(mSliderJointComponents.mN1[i]);
|
||||
}
|
||||
|
||||
const decimal biasFactor = (BETA / mTimeStep);
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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);
|
||||
|
||||
const Vector3& x1 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody1];
|
||||
const Vector3& x2 = mRigidBodyComponents.mCentersOfMassWorld[componentIndexBody2];
|
||||
|
||||
const Vector3& r1 = mSliderJointComponents.mR1[i];
|
||||
const Vector3& r2 = mSliderJointComponents.mR2[i];
|
||||
|
||||
// Compute the vector u (difference between anchor points)
|
||||
const Vector3 u = x2 + r2 - x1 - r1;
|
||||
|
||||
// Compute the cross products used in the Jacobians
|
||||
const Vector3 r1PlusU = mSliderJointComponents.mR1[i] + u;
|
||||
mSliderJointComponents.mR1PlusUCrossN1[i] = r1PlusU.cross(mSliderJointComponents.mN1[i]);
|
||||
mSliderJointComponents.mR1PlusUCrossN2[i] = r1PlusU.cross(mSliderJointComponents.mN2[i]);
|
||||
mSliderJointComponents.mR1PlusUCrossSliderAxis[i] = r1PlusU.cross(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal uDotSliderAxis = u.dot(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
decimal lowerLimitError = uDotSliderAxis - mSliderJointComponents.mLowerLimit[i];
|
||||
decimal upperLimitError = mSliderJointComponents.mUpperLimit[i] - uDotSliderAxis;
|
||||
bool oldIsLowerLimitViolated = mSliderJointComponents.mIsLowerLimitViolated[i];
|
||||
mSliderJointComponents.mIsLowerLimitViolated[i] = lowerLimitError <= 0;
|
||||
if (mSliderJointComponents.mIsLowerLimitViolated[i] != oldIsLowerLimitViolated) {
|
||||
mSliderJointComponents.mImpulseLowerLimit[i] = decimal(0.0);
|
||||
}
|
||||
bool oldIsUpperLimitViolated = mSliderJointComponents.mIsUpperLimitViolated[i];
|
||||
mSliderJointComponents.mIsUpperLimitViolated[i] = upperLimitError <= 0;
|
||||
if (mSliderJointComponents.mIsUpperLimitViolated[i] != oldIsUpperLimitViolated) {
|
||||
mSliderJointComponents.mImpulseUpperLimit[i] = decimal(0.0);
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the translation constraint
|
||||
mSliderJointComponents.mBiasTranslation[i].setToZero();
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mSliderJointComponents.mBiasTranslation[i].x = u.dot(mSliderJointComponents.mN1[i]);
|
||||
mSliderJointComponents.mBiasTranslation[i].y = u.dot(mSliderJointComponents.mN2[i]);
|
||||
mSliderJointComponents.mBiasTranslation[i] *= biasFactor;
|
||||
}
|
||||
|
||||
// If the limits are enabled
|
||||
if (mSliderJointComponents.mIsLimitEnabled[i] && (mSliderJointComponents.mIsLowerLimitViolated[i] ||
|
||||
mSliderJointComponents.mIsUpperLimitViolated[i])) {
|
||||
|
||||
const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i];
|
||||
const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i];
|
||||
|
||||
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
const decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||
mSliderJointComponents.mInverseMassMatrixLimit[i] = sumInverseMass +
|
||||
r1PlusUCrossSliderAxis.dot(mSliderJointComponents.mI1[i] * r1PlusUCrossSliderAxis) +
|
||||
r2CrossSliderAxis.dot(mSliderJointComponents.mI2[i] * r2CrossSliderAxis);
|
||||
mSliderJointComponents.mInverseMassMatrixLimit[i] = (mSliderJointComponents.mInverseMassMatrixLimit[i] > decimal(0.0)) ?
|
||||
decimal(1.0) / mSliderJointComponents.mInverseMassMatrixLimit[i] : decimal(0.0);
|
||||
|
||||
// Compute the bias "b" of the lower limit constraint
|
||||
mSliderJointComponents.mBLowerLimit[i] = decimal(0.0);
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mSliderJointComponents.mBLowerLimit[i] = biasFactor * lowerLimitError;
|
||||
}
|
||||
|
||||
// Compute the bias "b" of the upper limit constraint
|
||||
mSliderJointComponents.mBUpperLimit[i] = decimal(0.0);
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
mSliderJointComponents.mBUpperLimit[i] = biasFactor * upperLimitError;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
// Compute the cross products used in the Jacobians
|
||||
mSliderJointComponents.mR2CrossN1[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mN1[i]);
|
||||
mSliderJointComponents.mR2CrossN2[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mN2[i]);
|
||||
mSliderJointComponents.mR2CrossSliderAxis[i] = mSliderJointComponents.mR2[i].cross(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.mJointEntities[i];
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i];
|
||||
const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i];
|
||||
const Vector3& r1PlusUCrossN1 = mSliderJointComponents.mR1PlusUCrossN1[i];
|
||||
const Vector3& r1PlusUCrossN2 = mSliderJointComponents.mR1PlusUCrossN2[i];
|
||||
|
||||
const Matrix3x3& i1 = mSliderJointComponents.mI1[i];
|
||||
const Matrix3x3& i2 = mSliderJointComponents.mI2[i];
|
||||
|
||||
const uint32 componentIndexBody1 = mRigidBodyComponents.getEntityIndex(body1Entity);
|
||||
const uint32 componentIndexBody2 = mRigidBodyComponents.getEntityIndex(body2Entity);
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||
// constraints (2x2 matrix)
|
||||
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
const decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1;
|
||||
Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2;
|
||||
Vector3 I2R2CrossN1 = i2 * r2CrossN1;
|
||||
Vector3 I2R2CrossN2 = i2 * r2CrossN2;
|
||||
const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN1.dot(I2R2CrossN1);
|
||||
const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN1.dot(I2R2CrossN2);
|
||||
const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN2.dot(I2R2CrossN1);
|
||||
const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN2.dot(I2R2CrossN2);
|
||||
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||
mSliderJointComponents.mInverseMassMatrixTranslation[i].setToZero();
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.mInverseMassMatrixTranslation[i] = matrixKTranslation.getInverse();
|
||||
}
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||
// contraints (3x3 matrix)
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = i1 + i2;
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC ||
|
||||
mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mInverseMassMatrixRotation[i].getInverse();
|
||||
}
|
||||
}
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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 bias "b" of the rotation constraint
|
||||
mSliderJointComponents.mBiasRotation[i].setToZero();
|
||||
if (mJointComponents.getPositionCorrectionTechnique(jointEntity) == JointsPositionCorrectionTechnique::BAUMGARTE_JOINTS) {
|
||||
const Quaternion qError = orientationBody2 * mSliderJointComponents.mInitOrientationDifferenceInv[i] * orientationBody1.getInverse();
|
||||
mSliderJointComponents.mBiasRotation[i] = biasFactor * decimal(2.0) * qError.getVectorV();
|
||||
}
|
||||
|
||||
// If the motor is enabled
|
||||
if (mSliderJointComponents.mIsMotorEnabled[i]) {
|
||||
|
||||
const decimal body1MassInverse = mRigidBodyComponents.getMassInverse(body1Entity);
|
||||
const decimal body2MassInverse = mRigidBodyComponents.getMassInverse(body2Entity);
|
||||
const decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
|
||||
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
|
||||
mSliderJointComponents.mInverseMassMatrixMotor[i] = sumInverseMass;
|
||||
mSliderJointComponents.mInverseMassMatrixMotor[i] = (mSliderJointComponents.mInverseMassMatrixMotor[i] > decimal(0.0)) ?
|
||||
decimal(1.0) / mSliderJointComponents.mInverseMassMatrixMotor[i] : decimal(0.0);
|
||||
}
|
||||
}
|
||||
|
||||
// If warm-starting is not enabled
|
||||
if (!mIsWarmStartingActive) {
|
||||
|
||||
// For each joint
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
// Reset all the accumulated impulses
|
||||
mSliderJointComponents.mImpulseTranslation[i].setToZero();
|
||||
mSliderJointComponents.mImpulseRotation[i].setToZero();
|
||||
mSliderJointComponents.mImpulseLowerLimit[i] = decimal(0.0);
|
||||
mSliderJointComponents.mImpulseUpperLimit[i] = decimal(0.0);
|
||||
mSliderJointComponents.mImpulseMotor[i] = decimal(0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||
void SolveSliderJointSystem::warmstart() {
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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 and inverse inertia tensors of the bodies
|
||||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
const Vector3& n1 = mSliderJointComponents.mN1[i];
|
||||
const Vector3& n2 = mSliderJointComponents.mN2[i];
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
|
||||
decimal impulseLimits = mSliderJointComponents.mImpulseUpperLimit[i] - mSliderJointComponents.mImpulseLowerLimit[i];
|
||||
Vector3 linearImpulseLimits = impulseLimits * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
|
||||
Vector3 impulseMotor = mSliderJointComponents.mImpulseMotor[i] * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
|
||||
const Vector2& impulseTranslation = mSliderJointComponents.mImpulseTranslation[i];
|
||||
const Vector3& impulseRotation = mSliderJointComponents.mImpulseRotation[i];
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
Vector3 linearImpulseBody1 = -n1 * impulseTranslation.x - n2 * impulseTranslation.y;
|
||||
Vector3 angularImpulseBody1 = -mSliderJointComponents.mR1PlusUCrossN1[i] * impulseTranslation.x -
|
||||
mSliderJointComponents.mR1PlusUCrossN2[i] * impulseTranslation.y;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
angularImpulseBody1 += -impulseRotation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 1
|
||||
linearImpulseBody1 += linearImpulseLimits;
|
||||
angularImpulseBody1 += impulseLimits * mSliderJointComponents.mR1PlusUCrossSliderAxis[i];
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 1
|
||||
linearImpulseBody1 += impulseMotor;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
|
||||
Vector3 linearImpulseBody2 = n1 * impulseTranslation.x + n2 * impulseTranslation.y;
|
||||
Vector3 angularImpulseBody2 = mSliderJointComponents.mR2CrossN1[i] * impulseTranslation.x +
|
||||
mSliderJointComponents.mR2CrossN2[i] * impulseTranslation.y;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
angularImpulseBody2 += impulseRotation;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints of body 2
|
||||
linearImpulseBody2 += -linearImpulseLimits;
|
||||
angularImpulseBody2 += -impulseLimits * mSliderJointComponents.mR2CrossSliderAxis[i];
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor constraint of body 2
|
||||
linearImpulseBody2 += -impulseMotor;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the velocity constraint
|
||||
void SolveSliderJointSystem::solveVelocityConstraint() {
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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];
|
||||
|
||||
const Matrix3x3& i1 = mSliderJointComponents.mI1[i];
|
||||
const Matrix3x3& i2 = mSliderJointComponents.mI2[i];
|
||||
|
||||
const Vector3& n1 = mSliderJointComponents.mN1[i];
|
||||
const Vector3& n2 = mSliderJointComponents.mN2[i];
|
||||
|
||||
const Vector3& r2CrossN1 = mSliderJointComponents.mR2CrossN1[i];
|
||||
const Vector3& r2CrossN2 = mSliderJointComponents.mR2CrossN2[i];
|
||||
const Vector3& r1PlusUCrossN1 = mSliderJointComponents.mR1PlusUCrossN1[i];
|
||||
const Vector3& r1PlusUCrossN2 = mSliderJointComponents.mR1PlusUCrossN2[i];
|
||||
|
||||
// Get the inverse mass and inverse inertia tensors of the bodies
|
||||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
// Compute J*v for the 2 translation constraints
|
||||
const decimal el1 = -n1.dot(v1) - w1.dot(r1PlusUCrossN1) +
|
||||
n1.dot(v2) + w2.dot(r2CrossN1);
|
||||
const decimal el2 = -n2.dot(v1) - w1.dot(r1PlusUCrossN2) +
|
||||
n2.dot(v2) + w2.dot(r2CrossN2);
|
||||
const Vector2 JvTranslation(el1, el2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
Vector2 deltaLambda = mSliderJointComponents.mInverseMassMatrixTranslation[i] * (-JvTranslation - mSliderJointComponents.mBiasTranslation[i]);
|
||||
mSliderJointComponents.mImpulseTranslation[i] += deltaLambda;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * deltaLambda.x - n2 * deltaLambda.y;
|
||||
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * deltaLambda.x -
|
||||
r1PlusUCrossN2 * deltaLambda.y;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += i1 * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 2
|
||||
const Vector3 linearImpulseBody2 = n1 * deltaLambda.x + n2 * deltaLambda.y;
|
||||
Vector3 angularImpulseBody2 = r2CrossN1 * deltaLambda.x + r2CrossN2 * deltaLambda.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += i2 * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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);
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
|
||||
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
|
||||
|
||||
// Compute J*v for the 3 rotation constraints
|
||||
const Vector3 JvRotation = w2 - w1;
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 3 rotation constraints
|
||||
Vector3 deltaLambda2 = mSliderJointComponents.mInverseMassMatrixRotation[i] *
|
||||
(-JvRotation - mSliderJointComponents.getBiasRotation(jointEntity));
|
||||
mSliderJointComponents.mImpulseRotation[i] += deltaLambda2;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body to body 1
|
||||
w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = deltaLambda2;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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);
|
||||
|
||||
Vector3& v1 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody1];
|
||||
Vector3& v2 = mRigidBodyComponents.mConstrainedLinearVelocities[componentIndexBody2];
|
||||
|
||||
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
|
||||
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
|
||||
|
||||
decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i];
|
||||
const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i];
|
||||
|
||||
const Vector3& sliderAxisWorld = mSliderJointComponents.mSliderAxisWorld[i];
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mSliderJointComponents.mIsLimitEnabled[i]) {
|
||||
|
||||
const decimal inverseMassMatrixLimit = mSliderJointComponents.mInverseMassMatrixLimit[i];
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mSliderJointComponents.mIsLowerLimitViolated[i]) {
|
||||
|
||||
// Compute J*v for the lower limit constraint
|
||||
const decimal JvLowerLimit = sliderAxisWorld.dot(v2) + r2CrossSliderAxis.dot(w2) -
|
||||
sliderAxisWorld.dot(v1) - r1PlusUCrossSliderAxis.dot(w1);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal deltaLambdaLower = inverseMassMatrixLimit * (-JvLowerLimit - mSliderJointComponents.mBLowerLimit[i]);
|
||||
decimal lambdaTemp = mSliderJointComponents.mImpulseLowerLimit[i];
|
||||
mSliderJointComponents.mImpulseLowerLimit[i] = std::max(mSliderJointComponents.mImpulseLowerLimit[i] + deltaLambdaLower, decimal(0.0));
|
||||
deltaLambdaLower = mSliderJointComponents.mImpulseLowerLimit[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = -deltaLambdaLower * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody1 = -deltaLambdaLower * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = deltaLambdaLower * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody2 = deltaLambdaLower * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mSliderJointComponents.mIsUpperLimitViolated[i]) {
|
||||
|
||||
// Compute J*v for the upper limit constraint
|
||||
const decimal JvUpperLimit = sliderAxisWorld.dot(v1) + r1PlusUCrossSliderAxis.dot(w1)
|
||||
- sliderAxisWorld.dot(v2) - r2CrossSliderAxis.dot(w2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal deltaLambdaUpper = inverseMassMatrixLimit * (-JvUpperLimit -mSliderJointComponents.mBUpperLimit[i]);
|
||||
decimal lambdaTemp = mSliderJointComponents.mImpulseUpperLimit[i];
|
||||
mSliderJointComponents.mImpulseUpperLimit[i] = std::max(mSliderJointComponents.mImpulseUpperLimit[i] + deltaLambdaUpper, decimal(0.0));
|
||||
deltaLambdaUpper = mSliderJointComponents.mImpulseUpperLimit[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = deltaLambdaUpper * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody1 = deltaLambdaUpper * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
w1 += mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = -deltaLambdaUpper * sliderAxisWorld;
|
||||
const Vector3 angularImpulseBody2 = -deltaLambdaUpper * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
w2 += mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
}
|
||||
}
|
||||
|
||||
// --------------- Motor --------------- //
|
||||
|
||||
if (mSliderJointComponents.mIsMotorEnabled[i]) {
|
||||
|
||||
// Compute J*v for the motor
|
||||
const decimal JvMotor = sliderAxisWorld.dot(v1) - sliderAxisWorld.dot(v2);
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the motor
|
||||
const decimal maxMotorImpulse = mSliderJointComponents.mMaxMotorForce[i] * mTimeStep;
|
||||
decimal deltaLambdaMotor = mSliderJointComponents.mInverseMassMatrixMotor[i] * (-JvMotor - mSliderJointComponents.mMotorSpeed[i]);
|
||||
decimal lambdaTemp = mSliderJointComponents.mImpulseMotor[i];
|
||||
mSliderJointComponents.mImpulseMotor[i] = clamp(mSliderJointComponents.mImpulseMotor[i] + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
|
||||
deltaLambdaMotor = mSliderJointComponents.mImpulseMotor[i] - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 1
|
||||
const Vector3 linearImpulseBody1 = deltaLambdaMotor * sliderAxisWorld;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the motor of body 2
|
||||
const Vector3 linearImpulseBody2 = -deltaLambdaMotor * sliderAxisWorld;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Solve the position constraint (for position error correction)
|
||||
void SolveSliderJointSystem::solvePositionConstraint() {
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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) return;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
// Recompute the inertia tensor of bodies
|
||||
mSliderJointComponents.mI1[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body1Entity);
|
||||
mSliderJointComponents.mI2[i] = RigidBody::getInertiaTensorInverseWorld(mWorld, body2Entity);
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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) return;
|
||||
|
||||
// Get the bodies entities
|
||||
const Entity body1Entity = mJointComponents.getBody1Entity(jointEntity);
|
||||
const Entity body2Entity = mJointComponents.getBody2Entity(jointEntity);
|
||||
|
||||
const Quaternion& q1 = mRigidBodyComponents.getConstrainedOrientation(body1Entity);
|
||||
const Quaternion& q2 = mRigidBodyComponents.getConstrainedOrientation(body2Entity);
|
||||
|
||||
// Vector from body center to the anchor point
|
||||
mSliderJointComponents.mR1[i] = q1 * mSliderJointComponents.mLocalAnchorPointBody1[i];
|
||||
mSliderJointComponents.mR2[i] = q2 * mSliderJointComponents.mLocalAnchorPointBody2[i];
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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) return;
|
||||
|
||||
// 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
|
||||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
const Vector3& r1 = mSliderJointComponents.getR1(jointEntity);
|
||||
const Vector3& r2 = mSliderJointComponents.getR2(jointEntity);
|
||||
|
||||
const Vector3& n1 = mSliderJointComponents.getN1(jointEntity);
|
||||
const Vector3& n2 = mSliderJointComponents.getN2(jointEntity);
|
||||
|
||||
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
|
||||
// Compute the vector u (difference between anchor points)
|
||||
const Vector3 u = x2 + r2 - x1 - r1;
|
||||
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
// Compute the two orthogonal vectors to the slider axis in world-space
|
||||
mSliderJointComponents.mSliderAxisWorld[i] = q1 * mSliderJointComponents.mSliderAxisBody1[i];
|
||||
mSliderJointComponents.mSliderAxisWorld[i].normalize();
|
||||
mSliderJointComponents.setN1(jointEntity, mSliderJointComponents.mSliderAxisWorld[i].getOneUnitOrthogonalVector());
|
||||
mSliderJointComponents.setN2(jointEntity, mSliderJointComponents.mSliderAxisWorld[i].cross(n1));
|
||||
|
||||
// Check if the limit constraints are violated or not
|
||||
decimal uDotSliderAxis = u.dot(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
decimal lowerLimitError = uDotSliderAxis - mSliderJointComponents.getLowerLimit(jointEntity);
|
||||
decimal upperLimitError = mSliderJointComponents.getUpperLimit(jointEntity) - uDotSliderAxis;
|
||||
mSliderJointComponents.setIsLowerLimitViolated(jointEntity, lowerLimitError <= 0);
|
||||
mSliderJointComponents.setIsUpperLimitViolated(jointEntity, upperLimitError <= 0);
|
||||
|
||||
// Compute the cross products used in the Jacobians
|
||||
mSliderJointComponents.setR2CrossN1(jointEntity, r2.cross(n1));
|
||||
mSliderJointComponents.setR2CrossN2(jointEntity, r2.cross(n2));
|
||||
mSliderJointComponents.setR2CrossSliderAxis(jointEntity, r2.cross(mSliderJointComponents.mSliderAxisWorld[i]));
|
||||
const Vector3 r1PlusU = r1 + u;
|
||||
mSliderJointComponents.setR1PlusUCrossN1(jointEntity, r1PlusU.cross(n1));
|
||||
mSliderJointComponents.setR1PlusUCrossN2(jointEntity, r1PlusU.cross(n2));
|
||||
mSliderJointComponents.setR1PlusUCrossSliderAxis(jointEntity, r1PlusU.cross(mSliderJointComponents.mSliderAxisWorld[i]));
|
||||
|
||||
const Vector3& r2CrossN1 = mSliderJointComponents.getR2CrossN1(jointEntity);
|
||||
const Vector3& r2CrossN2 = mSliderJointComponents.getR2CrossN2(jointEntity);
|
||||
const Vector3& r1PlusUCrossN1 = mSliderJointComponents.getR1PlusUCrossN1(jointEntity);
|
||||
const Vector3& r1PlusUCrossN2 = mSliderJointComponents.getR1PlusUCrossN2(jointEntity);
|
||||
|
||||
// --------------- Translation Constraints --------------- //
|
||||
|
||||
const Matrix3x3& i1 = mSliderJointComponents.getI1(jointEntity);
|
||||
const Matrix3x3& i2 = mSliderJointComponents.getI2(jointEntity);
|
||||
|
||||
// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
|
||||
// constraints (2x2 matrix)
|
||||
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
decimal sumInverseMass = body1MassInverse + body2MassInverse;
|
||||
Vector3 I1R1PlusUCrossN1 = i1 * r1PlusUCrossN1;
|
||||
Vector3 I1R1PlusUCrossN2 = i1 * r1PlusUCrossN2;
|
||||
Vector3 I2R2CrossN1 = i2 * r2CrossN1;
|
||||
Vector3 I2R2CrossN2 = i2 * r2CrossN2;
|
||||
const decimal el11 = sumInverseMass + r1PlusUCrossN1.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN1.dot(I2R2CrossN1);
|
||||
const decimal el12 = r1PlusUCrossN1.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN1.dot(I2R2CrossN2);
|
||||
const decimal el21 = r1PlusUCrossN2.dot(I1R1PlusUCrossN1) +
|
||||
r2CrossN2.dot(I2R2CrossN1);
|
||||
const decimal el22 = sumInverseMass + r1PlusUCrossN2.dot(I1R1PlusUCrossN2) +
|
||||
r2CrossN2.dot(I2R2CrossN2);
|
||||
Matrix2x2 matrixKTranslation(el11, el12, el21, el22);
|
||||
Matrix2x2& inverseMassMatrixTranslation = mSliderJointComponents.getInverseMassMatrixTranslation(jointEntity);
|
||||
inverseMassMatrixTranslation.setToZero();
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.setInverseMassMatrixTranslation(jointEntity, matrixKTranslation.getInverse());
|
||||
}
|
||||
|
||||
// Compute the position error for the 2 translation constraints
|
||||
const Vector2 translationError(u.dot(n1), u.dot(n2));
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the 2 translation constraints
|
||||
Vector2 lambdaTranslation = inverseMassMatrixTranslation * (-translationError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 2 translation constraints of body 1
|
||||
const Vector3 linearImpulseBody1 = -n1 * lambdaTranslation.x - n2 * lambdaTranslation.y;
|
||||
Vector3 angularImpulseBody1 = -r1PlusUCrossN1 * lambdaTranslation.x -
|
||||
r1PlusUCrossN2 * lambdaTranslation.y;
|
||||
|
||||
// Apply the impulse to the 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 P=J^T * lambda for the 2 translation constraints of body 2
|
||||
const Vector3 linearImpulseBody2 = n1 * lambdaTranslation.x + n2 * lambdaTranslation.y;
|
||||
Vector3 angularImpulseBody2 = r2CrossN1 * lambdaTranslation.x + r2CrossN2 * lambdaTranslation.y;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
|
||||
Vector3 w2 = i2 * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// For each joint component
|
||||
for (uint32 i=0; i < mSliderJointComponents.getNbEnabledComponents(); i++) {
|
||||
|
||||
const Entity jointEntity = mSliderJointComponents.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) return;
|
||||
|
||||
// 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);
|
||||
|
||||
Vector3& x1 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody1];
|
||||
Vector3& x2 = mRigidBodyComponents.mConstrainedPositions[componentIndexBody2];
|
||||
|
||||
Quaternion& q1 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody1];
|
||||
Quaternion& q2 = mRigidBodyComponents.mConstrainedOrientations[componentIndexBody2];
|
||||
|
||||
// Get the velocities
|
||||
Vector3& w1 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody1];
|
||||
Vector3& w2 = mRigidBodyComponents.mConstrainedAngularVelocities[componentIndexBody2];
|
||||
|
||||
// --------------- Rotation Constraints --------------- //
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
|
||||
// contraints (3x3 matrix)
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.mI1[i] + mSliderJointComponents.mI2[i];
|
||||
if (mRigidBodyComponents.mBodyTypes[componentIndexBody1] == BodyType::DYNAMIC || mRigidBodyComponents.mBodyTypes[componentIndexBody2] == BodyType::DYNAMIC) {
|
||||
|
||||
mSliderJointComponents.mInverseMassMatrixRotation[i] = mSliderJointComponents.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 * mSliderJointComponents.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 = mSliderJointComponents.mInverseMassMatrixRotation[i] * (-errorRotation);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 1
|
||||
Vector3 angularImpulseBody1 = -lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
w1 = mSliderJointComponents.mI1[i] * angularImpulseBody1;
|
||||
|
||||
// Update the body position/orientation of body 1
|
||||
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
|
||||
q1.normalize();
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the 3 rotation constraints of body 2
|
||||
Vector3 angularImpulseBody2 = lambdaRotation;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mSliderJointComponents.mIsLimitEnabled[i]) {
|
||||
|
||||
const Vector3& r2CrossSliderAxis = mSliderJointComponents.mR2CrossSliderAxis[i];
|
||||
const Vector3& r1PlusUCrossSliderAxis = mSliderJointComponents.mR1PlusUCrossSliderAxis[i];
|
||||
|
||||
if (mSliderJointComponents.mIsLowerLimitViolated[i] || mSliderJointComponents.mIsUpperLimitViolated[i]) {
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
|
||||
const decimal body1MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal body2MassInverse = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
mSliderJointComponents.mInverseMassMatrixLimit[i] = body1MassInverse + body2MassInverse +
|
||||
r1PlusUCrossSliderAxis.dot(mSliderJointComponents.mI1[i] * r1PlusUCrossSliderAxis) +
|
||||
r2CrossSliderAxis.dot(mSliderJointComponents.mI2[i] * r2CrossSliderAxis);
|
||||
mSliderJointComponents.mInverseMassMatrixLimit[i] = (mSliderJointComponents.mInverseMassMatrixLimit[i] > decimal(0.0)) ?
|
||||
decimal(1.0) / mSliderJointComponents.mInverseMassMatrixLimit[i] : decimal(0.0);
|
||||
}
|
||||
|
||||
const decimal inverseMassBody1 = mRigidBodyComponents.mInverseMasses[componentIndexBody1];
|
||||
const decimal inverseMassBody2 = mRigidBodyComponents.mInverseMasses[componentIndexBody2];
|
||||
|
||||
// If the lower limit is violated
|
||||
if (mSliderJointComponents.mIsLowerLimitViolated[i]) {
|
||||
|
||||
const Vector3& r1 = mSliderJointComponents.mR1[i];
|
||||
const Vector3& r2 = mSliderJointComponents.mR2[i];
|
||||
const Vector3 u = x2 + r2 - x1 - r1;
|
||||
decimal uDotSliderAxis = u.dot(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
decimal lowerLimitError = uDotSliderAxis - mSliderJointComponents.mLowerLimit[i];
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the lower limit constraint
|
||||
decimal lambdaLowerLimit = mSliderJointComponents.mInverseMassMatrixLimit[i] * (-lowerLimitError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the lower limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = -lambdaLowerLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 angularImpulseBody1 = -lambdaLowerLimit * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
const Vector3 w1 = mSliderJointComponents.mI1[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 P=J^T * lambda for the lower limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = lambdaLowerLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 angularImpulseBody2 = lambdaLowerLimit * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
|
||||
const Vector3 w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
|
||||
// If the upper limit is violated
|
||||
if (mSliderJointComponents.mIsUpperLimitViolated[i]) {
|
||||
|
||||
const Vector3& r1 = mSliderJointComponents.mR1[i];
|
||||
const Vector3& r2 = mSliderJointComponents.mR2[i];
|
||||
const Vector3 u = x2 + r2 - x1 - r1;
|
||||
decimal uDotSliderAxis = u.dot(mSliderJointComponents.mSliderAxisWorld[i]);
|
||||
decimal upperLimitError = mSliderJointComponents.mUpperLimit[i] - uDotSliderAxis;
|
||||
|
||||
// Compute the Lagrange multiplier lambda for the upper limit constraint
|
||||
decimal lambdaUpperLimit = mSliderJointComponents.mInverseMassMatrixLimit[i] * (-upperLimitError);
|
||||
|
||||
// Compute the impulse P=J^T * lambda for the upper limit constraint of body 1
|
||||
const Vector3 linearImpulseBody1 = lambdaUpperLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 angularImpulseBody1 = lambdaUpperLimit * r1PlusUCrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 1
|
||||
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
|
||||
const Vector3 w1 = mSliderJointComponents.mI1[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 P=J^T * lambda for the upper limit constraint of body 2
|
||||
const Vector3 linearImpulseBody2 = -lambdaUpperLimit * mSliderJointComponents.mSliderAxisWorld[i];
|
||||
const Vector3 angularImpulseBody2 = -lambdaUpperLimit * r2CrossSliderAxis;
|
||||
|
||||
// Apply the impulse to the body 2
|
||||
const Vector3 v2 = inverseMassBody2 * linearImpulseBody2;
|
||||
const Vector3 w2 = mSliderJointComponents.mI2[i] * angularImpulseBody2;
|
||||
|
||||
// Update the body position/orientation of body 2
|
||||
x2 += v2;
|
||||
q2 += Quaternion(0, w2) * q2 * decimal(0.5);
|
||||
q2.normalize();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
146
src/systems/SolveSliderJointSystem.h
Normal file
146
src/systems/SolveSliderJointSystem.h
Normal file
|
|
@ -0,0 +1,146 @@
|
|||
/********************************************************************************
|
||||
* 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_SLIDER_JOINT_SYSTEM_H
|
||||
#define REACTPHYSICS3D_SOLVE_SLIDER_JOINT_SYSTEM_H
|
||||
|
||||
// Libraries
|
||||
#include "utils/Profiler.h"
|
||||
#include "components/RigidBodyComponents.h"
|
||||
#include "components/JointComponents.h"
|
||||
#include "components/SliderJointComponents.h"
|
||||
#include "components/TransformComponents.h"
|
||||
|
||||
namespace reactphysics3d {
|
||||
|
||||
class DynamicsWorld;
|
||||
|
||||
// Class SolveSliderJointSystem
|
||||
/**
|
||||
* This class is responsible to solve the SliderJoint constraints
|
||||
*/
|
||||
class SolveSliderJointSystem {
|
||||
|
||||
private :
|
||||
|
||||
// -------------------- Constants -------------------- //
|
||||
|
||||
// Beta value for the bias factor of position correction
|
||||
static const decimal BETA;
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// Physics world
|
||||
DynamicsWorld& mWorld;
|
||||
|
||||
/// Reference to the rigid body components
|
||||
RigidBodyComponents& mRigidBodyComponents;
|
||||
|
||||
/// Reference to transform components
|
||||
TransformComponents& mTransformComponents;
|
||||
|
||||
/// Reference to the joint components
|
||||
JointComponents& mJointComponents;
|
||||
|
||||
/// Reference to the slider joint components
|
||||
SliderJointComponents& mSliderJointComponents;
|
||||
|
||||
/// 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
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
public :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
/// Constructor
|
||||
SolveSliderJointSystem(DynamicsWorld& world, RigidBodyComponents& rigidBodyComponents,
|
||||
TransformComponents& transformComponents,
|
||||
JointComponents& jointComponents,
|
||||
SliderJointComponents& sliderJointComponents);
|
||||
|
||||
/// Destructor
|
||||
~SolveSliderJointSystem() = 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 SolveSliderJointSystem::setProfiler(Profiler* profiler) {
|
||||
mProfiler = profiler;
|
||||
}
|
||||
|
||||
// Set the time step
|
||||
inline void SolveSliderJointSystem::setTimeStep(decimal timeStep) {
|
||||
assert(timeStep > decimal(0.0));
|
||||
mTimeStep = timeStep;
|
||||
}
|
||||
|
||||
// Set to true to enable warm starting
|
||||
inline void SolveSliderJointSystem::setIsWarmStartingActive(bool isWarmStartingActive) {
|
||||
mIsWarmStartingActive = isWarmStartingActive;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue
Block a user