Finish the implementation of the slider joint
This commit is contained in:
parent
61562b3560
commit
9c0844cf1b
|
@ -29,8 +29,11 @@
|
||||||
|
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
|
||||||
|
// Static variables definition
|
||||||
|
const decimal BallAndSocketJoint::BETA = 0.2;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo)
|
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
|
||||||
: Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
|
: Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
|
||||||
|
|
||||||
// Compute the local-space anchor point for each body
|
// Compute the local-space anchor point for each body
|
||||||
|
@ -51,6 +54,8 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
||||||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
|
const Vector3& x1 = mBody1->getTransform().getPosition();
|
||||||
|
const Vector3& x2 = mBody2->getTransform().getPosition();
|
||||||
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
||||||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||||
|
|
||||||
|
@ -89,6 +94,20 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
||||||
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
|
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
|
||||||
mInverseMassMatrix = massMatrix.getInverse();
|
mInverseMassMatrix = massMatrix.getInverse();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Compute the bias "b" of the constraint
|
||||||
|
mBiasVector.setToZero();
|
||||||
|
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
||||||
|
decimal biasFactor = (BETA / constraintSolverData.timeStep);
|
||||||
|
mBiasVector = biasFactor * (x2 + mR2World - x1 - mR1World);
|
||||||
|
}
|
||||||
|
|
||||||
|
// If warm-starting is not enabled
|
||||||
|
if (!constraintSolverData.isWarmStartingActive) {
|
||||||
|
|
||||||
|
// Reset the accumulated impulse
|
||||||
|
mImpulse.setToZero();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
// Warm start the constraint (apply the previous impulse at the beginning of the step)
|
||||||
|
@ -126,10 +145,6 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
|
||||||
// Solve the velocity constraint
|
// Solve the velocity constraint
|
||||||
void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
|
void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||||
|
|
||||||
// Get the body positions
|
|
||||||
const Vector3& x1 = mBody1->getTransform().getPosition();
|
|
||||||
const Vector3& x2 = mBody2->getTransform().getPosition();
|
|
||||||
|
|
||||||
// Get the velocities
|
// Get the velocities
|
||||||
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
|
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
|
||||||
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
|
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
|
||||||
|
@ -143,18 +158,10 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
|
||||||
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||||
|
|
||||||
// Compute J*v
|
// Compute J*v
|
||||||
const Vector3 Jv = -v1 + mR1World.cross(w1) + v2 - mR2World.cross(w2);
|
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
|
||||||
|
|
||||||
// Compute the bias "b" of the constraint
|
|
||||||
Vector3 b(0, 0, 0);
|
|
||||||
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
|
||||||
decimal beta = decimal(0.2); // TODO : Use a constant here
|
|
||||||
decimal biasFactor = (beta / constraintSolverData.timeStep);
|
|
||||||
b = biasFactor * (x2 + mR2World - x1 - mR1World);
|
|
||||||
}
|
|
||||||
|
|
||||||
// Compute the Lagrange multiplier lambda
|
// Compute the Lagrange multiplier lambda
|
||||||
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b);
|
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
|
||||||
mImpulse += deltaLambda;
|
mImpulse += deltaLambda;
|
||||||
|
|
||||||
// Compute the impulse P=J^T * lambda
|
// Compute the impulse P=J^T * lambda
|
||||||
|
|
|
@ -62,6 +62,11 @@ class BallAndSocketJoint : public Constraint {
|
||||||
|
|
||||||
private :
|
private :
|
||||||
|
|
||||||
|
// -------------------- Constants -------------------- //
|
||||||
|
|
||||||
|
// Beta value for the bias factor of position correction
|
||||||
|
static const decimal BETA;
|
||||||
|
|
||||||
// -------------------- Attributes -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Anchor point of body 1 (in local-space coordinates of body 1)
|
/// Anchor point of body 1 (in local-space coordinates of body 1)
|
||||||
|
@ -76,6 +81,9 @@ class BallAndSocketJoint : public Constraint {
|
||||||
/// Vector from center of body 2 to anchor point in world-space
|
/// Vector from center of body 2 to anchor point in world-space
|
||||||
Vector3 mR2World;
|
Vector3 mR2World;
|
||||||
|
|
||||||
|
/// Bias vector for the constraint
|
||||||
|
Vector3 mBiasVector;
|
||||||
|
|
||||||
/// Skew-Symmetric matrix for cross product with vector mR1World
|
/// Skew-Symmetric matrix for cross product with vector mR1World
|
||||||
Matrix3x3 mSkewSymmetricMatrixR1World;
|
Matrix3x3 mSkewSymmetricMatrixR1World;
|
||||||
|
|
||||||
|
|
|
@ -33,9 +33,9 @@
|
||||||
|
|
||||||
// ReactPhysics3D namespace
|
// ReactPhysics3D namespace
|
||||||
namespace reactphysics3d {
|
namespace reactphysics3d {
|
||||||
|
|
||||||
// Enumeration for the type of a constraint
|
// Enumeration for the type of a constraint
|
||||||
enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT};
|
enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT};
|
||||||
|
|
||||||
// Class declarations
|
// Class declarations
|
||||||
struct ConstraintSolverData;
|
struct ConstraintSolverData;
|
||||||
|
|
|
@ -28,15 +28,21 @@
|
||||||
|
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
|
||||||
|
// Static variables definition
|
||||||
|
const decimal SliderJoint::BETA = decimal(0.2);
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
|
SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
|
||||||
: Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
|
: Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
|
||||||
mImpulseLowerLimit(0), mImpulseUpperLimit(0),
|
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
|
||||||
mIsLimitsActive(jointInfo.isLimitsActive), mLowerLimit(jointInfo.lowerLimit),
|
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
|
||||||
mUpperLimit(jointInfo.upperLimit) {
|
mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit),
|
||||||
|
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
|
||||||
|
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce){
|
||||||
|
|
||||||
assert(mUpperLimit >= 0.0);
|
assert(mUpperLimit >= 0.0);
|
||||||
assert(mLowerLimit <= 0.0);
|
assert(mLowerLimit <= 0.0);
|
||||||
|
assert(mMaxMotorForce >= 0.0);
|
||||||
|
|
||||||
// Compute the local-space anchor point for each body
|
// Compute the local-space anchor point for each body
|
||||||
const Transform& transform1 = mBody1->getTransform();
|
const Transform& transform1 = mBody1->getTransform();
|
||||||
|
@ -63,7 +69,7 @@ SliderJoint::~SliderJoint() {
|
||||||
// Initialize before solving the constraint
|
// Initialize before solving the constraint
|
||||||
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
|
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
|
||||||
|
|
||||||
// Initialize the bodies index in the velocity array
|
// Initialize the bodies index in the veloc ity array
|
||||||
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
|
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
|
||||||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||||
|
|
||||||
|
@ -94,8 +100,16 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
||||||
decimal uDotSliderAxis = u.dot(mSliderAxisWorld);
|
decimal uDotSliderAxis = u.dot(mSliderAxisWorld);
|
||||||
decimal lowerLimitError = uDotSliderAxis - mLowerLimit;
|
decimal lowerLimitError = uDotSliderAxis - mLowerLimit;
|
||||||
decimal upperLimitError = mUpperLimit - uDotSliderAxis;
|
decimal upperLimitError = mUpperLimit - uDotSliderAxis;
|
||||||
mIsLowerLimitViolated = (lowerLimitError <= 0);
|
bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
|
||||||
mIsUpperLimitViolated = (upperLimitError <= 0);
|
mIsLowerLimitViolated = lowerLimitError <= 0;
|
||||||
|
if (mIsLowerLimitViolated != oldIsLowerLimitViolated) {
|
||||||
|
mImpulseLowerLimit = 0.0;
|
||||||
|
}
|
||||||
|
bool oldIsUpperLimitViolated = mIsUpperLimitViolated;
|
||||||
|
mIsUpperLimitViolated = upperLimitError <= 0;
|
||||||
|
if (mIsUpperLimitViolated != oldIsUpperLimitViolated) {
|
||||||
|
mImpulseUpperLimit = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
// Compute the cross products used in the Jacobians
|
// Compute the cross products used in the Jacobians
|
||||||
mR2CrossN1 = mR2.cross(mN1);
|
mR2CrossN1 = mR2.cross(mN1);
|
||||||
|
@ -139,8 +153,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
||||||
|
|
||||||
// Compute the bias "b" of the translation constraint
|
// Compute the bias "b" of the translation constraint
|
||||||
mBTranslation.setToZero();
|
mBTranslation.setToZero();
|
||||||
decimal beta = decimal(0.2); // TODO : Use a constant here
|
decimal biasFactor = (BETA / constraintSolverData.timeStep);
|
||||||
decimal biasFactor = (beta / constraintSolverData.timeStep);
|
|
||||||
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
||||||
mBTranslation.x = u.dot(mN1);
|
mBTranslation.x = u.dot(mN1);
|
||||||
mBTranslation.y = u.dot(mN2);
|
mBTranslation.y = u.dot(mN2);
|
||||||
|
@ -170,21 +183,54 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
||||||
mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
|
mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix)
|
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
|
||||||
mInverseMassMatrixLimit = sumInverseMass +
|
|
||||||
mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis) +
|
|
||||||
mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis);
|
|
||||||
|
|
||||||
// Compute the bias "b" of the lower limit constraint
|
// Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix)
|
||||||
mBLowerLimit = 0.0;
|
mInverseMassMatrixLimit = 0.0;
|
||||||
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
if (mBody1->getIsMotionEnabled()) {
|
||||||
mBLowerLimit = biasFactor * lowerLimitError;
|
mInverseMassMatrixLimit += mBody1->getMassInverse() +
|
||||||
|
mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis);
|
||||||
|
}
|
||||||
|
if (mBody2->getIsMotionEnabled()) {
|
||||||
|
mInverseMassMatrixLimit += mBody2->getMassInverse() +
|
||||||
|
mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis);
|
||||||
|
}
|
||||||
|
mInverseMassMatrixLimit = (mInverseMassMatrixLimit > 0.0) ?
|
||||||
|
decimal(1.0) / mInverseMassMatrixLimit : decimal(0.0);
|
||||||
|
|
||||||
|
// Compute the bias "b" of the lower limit constraint
|
||||||
|
mBLowerLimit = 0.0;
|
||||||
|
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
||||||
|
mBLowerLimit = biasFactor * lowerLimitError;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the bias "b" of the upper limit constraint
|
||||||
|
mBUpperLimit = 0.0;
|
||||||
|
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
||||||
|
mBUpperLimit = biasFactor * upperLimitError;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the bias "b" of the upper limit constraint
|
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
|
||||||
mBUpperLimit = 0.0;
|
mInverseMassMatrixMotor = 0.0;
|
||||||
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
if (mBody1->getIsMotionEnabled()) {
|
||||||
mBUpperLimit = biasFactor * upperLimitError;
|
mInverseMassMatrixMotor += mBody1->getMassInverse();
|
||||||
|
}
|
||||||
|
if (mBody2->getIsMotionEnabled()) {
|
||||||
|
mInverseMassMatrixMotor += mBody2->getMassInverse();
|
||||||
|
}
|
||||||
|
mInverseMassMatrixMotor = (mInverseMassMatrixMotor > 0.0) ?
|
||||||
|
decimal(1.0) / mInverseMassMatrixMotor : decimal(0.0);
|
||||||
|
|
||||||
|
// If warm-starting is not enabled
|
||||||
|
if (!constraintSolverData.isWarmStartingActive) {
|
||||||
|
|
||||||
|
// Reset all the accumulated impulses
|
||||||
|
mImpulseTranslation.setToZero();
|
||||||
|
mImpulseRotation.setToZero();
|
||||||
|
mImpulseLowerLimit = 0.0;
|
||||||
|
mImpulseUpperLimit = 0.0;
|
||||||
|
mImpulseMotor = 0.0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -215,6 +261,19 @@ void SliderJoint::warmstart(const ConstraintSolverData& constraintSolverData) {
|
||||||
angularImpulseBody1 += -mImpulseRotation;
|
angularImpulseBody1 += -mImpulseRotation;
|
||||||
angularImpulseBody2 += mImpulseRotation;
|
angularImpulseBody2 += mImpulseRotation;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda for the lower and upper limits constraints
|
||||||
|
decimal impulseLimits = mImpulseUpperLimit - mImpulseLowerLimit;
|
||||||
|
Vector3 linearImpulseLimits = impulseLimits * mSliderAxisWorld;
|
||||||
|
linearImpulseBody1 += linearImpulseLimits;
|
||||||
|
angularImpulseBody1 += impulseLimits * mR1PlusUCrossSliderAxis;
|
||||||
|
linearImpulseBody2 += -linearImpulseLimits;
|
||||||
|
angularImpulseBody2 += -impulseLimits * mR2CrossSliderAxis;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda for the motor constraint
|
||||||
|
Vector3 impulseMotor = mImpulseMotor * mSliderAxisWorld;
|
||||||
|
linearImpulseBody1 += impulseMotor;
|
||||||
|
linearImpulseBody2 += -impulseMotor;
|
||||||
|
|
||||||
// Apply the impulse to the bodies of the joint
|
// Apply the impulse to the bodies of the joint
|
||||||
if (mBody1->getIsMotionEnabled()) {
|
if (mBody1->getIsMotionEnabled()) {
|
||||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||||
|
@ -293,7 +352,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
|
||||||
|
|
||||||
// --------------- Limits Constraints --------------- //
|
// --------------- Limits Constraints --------------- //
|
||||||
|
|
||||||
if (mIsLimitsActive) {
|
if (mIsLimitEnabled) {
|
||||||
|
|
||||||
// If the lower limit is violated
|
// If the lower limit is violated
|
||||||
if (mIsLowerLimitViolated) {
|
if (mIsLowerLimitViolated) {
|
||||||
|
@ -355,9 +414,118 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// --------------- Motor --------------- //
|
||||||
|
|
||||||
|
if (mIsMotorEnabled) {
|
||||||
|
|
||||||
|
// Compute J*v for the motor
|
||||||
|
const decimal JvMotor = mSliderAxisWorld.dot(v1) - mSliderAxisWorld.dot(v2);
|
||||||
|
|
||||||
|
// Compute the Lagrange multiplier lambda for the motor
|
||||||
|
const decimal maxMotorImpulse = mMaxMotorForce * constraintSolverData.timeStep;
|
||||||
|
decimal deltaLambdaMotor = mInverseMassMatrixMotor * (-JvMotor -mMotorSpeed);
|
||||||
|
decimal lambdaTemp = mImpulseMotor;
|
||||||
|
mImpulseMotor = clamp(mImpulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
|
||||||
|
deltaLambdaMotor = mImpulseMotor - lambdaTemp;
|
||||||
|
|
||||||
|
// Compute the impulse P=J^T * lambda for the motor
|
||||||
|
const Vector3 linearImpulseBody1 = deltaLambdaMotor * mSliderAxisWorld;
|
||||||
|
const Vector3 linearImpulseBody2 = -linearImpulseBody1;
|
||||||
|
|
||||||
|
// Apply the impulse to the bodies of the joint
|
||||||
|
if (mBody1->getIsMotionEnabled()) {
|
||||||
|
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||||
|
}
|
||||||
|
if (mBody2->getIsMotionEnabled()) {
|
||||||
|
v2 += inverseMassBody2 * linearImpulseBody2;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Solve the position constraint
|
// Solve the position constraint
|
||||||
void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
|
void SliderJoint::solvePositionConstraint(const ConstraintSolverData& constraintSolverData) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Enable/Disable the limits of the joint
|
||||||
|
void SliderJoint::enableLimit(bool isLimitEnabled) {
|
||||||
|
|
||||||
|
if (isLimitEnabled != mIsLimitEnabled) {
|
||||||
|
|
||||||
|
mIsLimitEnabled = isLimitEnabled;
|
||||||
|
|
||||||
|
// Reset the limits
|
||||||
|
resetLimits();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Enable/Disable the motor of the joint
|
||||||
|
void SliderJoint::enableMotor(bool isMotorEnabled) {
|
||||||
|
|
||||||
|
mIsMotorEnabled = isMotorEnabled;
|
||||||
|
mImpulseMotor = 0.0;
|
||||||
|
|
||||||
|
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the lower limit
|
||||||
|
void SliderJoint::setLowerLimit(decimal lowerLimit) {
|
||||||
|
|
||||||
|
assert(lowerLimit <= mUpperLimit);
|
||||||
|
|
||||||
|
if (lowerLimit != mLowerLimit) {
|
||||||
|
|
||||||
|
mLowerLimit = lowerLimit;
|
||||||
|
|
||||||
|
// Reset the limits
|
||||||
|
resetLimits();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the upper limit
|
||||||
|
void SliderJoint::setUpperLimit(decimal upperLimit) {
|
||||||
|
|
||||||
|
assert(mLowerLimit <= upperLimit);
|
||||||
|
|
||||||
|
if (upperLimit != mUpperLimit) {
|
||||||
|
|
||||||
|
mUpperLimit = upperLimit;
|
||||||
|
|
||||||
|
// Reset the limits
|
||||||
|
resetLimits();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reset the limits
|
||||||
|
void SliderJoint::resetLimits() {
|
||||||
|
|
||||||
|
// Reset the accumulated impulses for the limits
|
||||||
|
mImpulseLowerLimit = 0.0;
|
||||||
|
mImpulseUpperLimit = 0.0;
|
||||||
|
|
||||||
|
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the motor speed
|
||||||
|
void SliderJoint::setMotorSpeed(decimal motorSpeed) {
|
||||||
|
|
||||||
|
if (motorSpeed != mMotorSpeed) {
|
||||||
|
|
||||||
|
mMotorSpeed = motorSpeed;
|
||||||
|
|
||||||
|
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Set the maximum motor force
|
||||||
|
void SliderJoint::setMaxMotorForce(decimal maxMotorForce) {
|
||||||
|
|
||||||
|
if (maxMotorForce != mMaxMotorForce) {
|
||||||
|
|
||||||
|
assert(mMaxMotorForce >= 0.0);
|
||||||
|
mMaxMotorForce = maxMotorForce;
|
||||||
|
|
||||||
|
// TODO : Wake up the bodies of the joint here when sleeping is implemented
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -49,8 +49,11 @@ struct SliderJointInfo : public ConstraintInfo {
|
||||||
/// Slider axis (in world-space coordinates)
|
/// Slider axis (in world-space coordinates)
|
||||||
Vector3 sliderAxisWorldSpace;
|
Vector3 sliderAxisWorldSpace;
|
||||||
|
|
||||||
/// True if the slider limits are active
|
/// True if the slider limits are enabled
|
||||||
bool isLimitsActive;
|
bool isLimitEnabled;
|
||||||
|
|
||||||
|
/// True if the slider motor is enabled
|
||||||
|
bool isMotorEnabled;
|
||||||
|
|
||||||
/// Lower limit
|
/// Lower limit
|
||||||
decimal lowerLimit;
|
decimal lowerLimit;
|
||||||
|
@ -58,16 +61,23 @@ struct SliderJointInfo : public ConstraintInfo {
|
||||||
/// Upper limit
|
/// Upper limit
|
||||||
decimal upperLimit;
|
decimal upperLimit;
|
||||||
|
|
||||||
/// Constructor without limits
|
/// Motor speed
|
||||||
|
decimal motorSpeed;
|
||||||
|
|
||||||
|
/// Maximum motor force (in Newton) that can be applied to reach to desired motor speed
|
||||||
|
decimal maxMotorForce;
|
||||||
|
|
||||||
|
/// Constructor without limits and without motor
|
||||||
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
|
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
|
||||||
const Vector3& initAnchorPointWorldSpace,
|
const Vector3& initAnchorPointWorldSpace,
|
||||||
const Vector3& initSliderAxisWorldSpace)
|
const Vector3& initSliderAxisWorldSpace)
|
||||||
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
||||||
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||||
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
||||||
isLimitsActive(false), lowerLimit(-1.0), upperLimit(1.0) {}
|
isLimitEnabled(false), isMotorEnabled(false), lowerLimit(-1.0),
|
||||||
|
upperLimit(1.0), motorSpeed(0), maxMotorForce(0) {}
|
||||||
|
|
||||||
/// Constructor with limits
|
/// Constructor with limits and no motor
|
||||||
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
|
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
|
||||||
const Vector3& initAnchorPointWorldSpace,
|
const Vector3& initAnchorPointWorldSpace,
|
||||||
const Vector3& initSliderAxisWorldSpace,
|
const Vector3& initSliderAxisWorldSpace,
|
||||||
|
@ -75,8 +85,21 @@ struct SliderJointInfo : public ConstraintInfo {
|
||||||
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
||||||
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||||
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
||||||
isLimitsActive(true), lowerLimit(initLowerLimit),
|
isLimitEnabled(true), isMotorEnabled(false), lowerLimit(initLowerLimit),
|
||||||
upperLimit(initUpperLimit) {}
|
upperLimit(initUpperLimit), motorSpeed(0), maxMotorForce(0) {}
|
||||||
|
|
||||||
|
/// Constructor with limits and motor
|
||||||
|
SliderJointInfo(RigidBody* rigidBody1, RigidBody* rigidBody2,
|
||||||
|
const Vector3& initAnchorPointWorldSpace,
|
||||||
|
const Vector3& initSliderAxisWorldSpace,
|
||||||
|
decimal initLowerLimit, decimal initUpperLimit,
|
||||||
|
decimal initMotorSpeed, decimal initMaxMotorForce)
|
||||||
|
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
||||||
|
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||||
|
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
||||||
|
isLimitEnabled(true), isMotorEnabled(true), lowerLimit(initLowerLimit),
|
||||||
|
upperLimit(initUpperLimit), motorSpeed(initMotorSpeed),
|
||||||
|
maxMotorForce(initMaxMotorForce) {}
|
||||||
};
|
};
|
||||||
|
|
||||||
// Class SliderJoint
|
// Class SliderJoint
|
||||||
|
@ -87,6 +110,11 @@ class SliderJoint : public Constraint {
|
||||||
|
|
||||||
private :
|
private :
|
||||||
|
|
||||||
|
// -------------------- Constants -------------------- //
|
||||||
|
|
||||||
|
// Beta value for the position correction bias factor
|
||||||
|
static const decimal BETA;
|
||||||
|
|
||||||
// -------------------- Attributes -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Anchor point of body 1 (in local-space coordinates of body 1)
|
/// Anchor point of body 1 (in local-space coordinates of body 1)
|
||||||
|
@ -152,6 +180,9 @@ class SliderJoint : public Constraint {
|
||||||
/// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
|
/// Inverse of mass matrix K=JM^-1J^t for the upper and lower limit constraints (1x1 matrix)
|
||||||
decimal mInverseMassMatrixLimit;
|
decimal mInverseMassMatrixLimit;
|
||||||
|
|
||||||
|
/// Inverse of mass matrix K=JM^-1J^t for the motor
|
||||||
|
decimal mInverseMassMatrixMotor;
|
||||||
|
|
||||||
/// Impulse for the 2 translation constraints
|
/// Impulse for the 2 translation constraints
|
||||||
Vector2 mImpulseTranslation;
|
Vector2 mImpulseTranslation;
|
||||||
|
|
||||||
|
@ -164,8 +195,14 @@ class SliderJoint : public Constraint {
|
||||||
/// Impulse for the upper limit constraint
|
/// Impulse for the upper limit constraint
|
||||||
decimal mImpulseUpperLimit;
|
decimal mImpulseUpperLimit;
|
||||||
|
|
||||||
/// True if the slider limits are active
|
/// Impulse for the motor
|
||||||
bool mIsLimitsActive;
|
decimal mImpulseMotor;
|
||||||
|
|
||||||
|
/// True if the slider limits are enabled
|
||||||
|
bool mIsLimitEnabled;
|
||||||
|
|
||||||
|
/// True if the motor of the joint in enabled
|
||||||
|
bool mIsMotorEnabled;
|
||||||
|
|
||||||
/// Slider axis in world-space coordinates
|
/// Slider axis in world-space coordinates
|
||||||
Vector3 mSliderAxisWorld;
|
Vector3 mSliderAxisWorld;
|
||||||
|
@ -182,6 +219,17 @@ class SliderJoint : public Constraint {
|
||||||
/// True if the upper limit is violated
|
/// True if the upper limit is violated
|
||||||
bool mIsUpperLimitViolated;
|
bool mIsUpperLimitViolated;
|
||||||
|
|
||||||
|
/// Motor speed
|
||||||
|
decimal mMotorSpeed;
|
||||||
|
|
||||||
|
/// Maximum motor force (in Newton) that can be applied to reach to desired motor speed
|
||||||
|
decimal mMaxMotorForce;
|
||||||
|
|
||||||
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
|
/// Reset the limits
|
||||||
|
void resetLimits();
|
||||||
|
|
||||||
public :
|
public :
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -192,6 +240,45 @@ class SliderJoint : public Constraint {
|
||||||
/// Destructor
|
/// Destructor
|
||||||
virtual ~SliderJoint();
|
virtual ~SliderJoint();
|
||||||
|
|
||||||
|
/// Return true if the limits or the joint are enabled
|
||||||
|
bool isLimitEnabled() const;
|
||||||
|
|
||||||
|
/// Return true if the motor of the joint is enabled
|
||||||
|
bool isMotorEnabled() const;
|
||||||
|
|
||||||
|
/// Enable/Disable the limits of the joint
|
||||||
|
void enableLimit(bool isLimitEnabled);
|
||||||
|
|
||||||
|
/// Enable/Disable the motor of the joint
|
||||||
|
void enableMotor(bool isMotorEnabled);
|
||||||
|
|
||||||
|
/// Return the lower limit
|
||||||
|
decimal getLowerLimit() const;
|
||||||
|
|
||||||
|
/// Set the lower limit
|
||||||
|
void setLowerLimit(decimal lowerLimit);
|
||||||
|
|
||||||
|
/// Return the upper limit
|
||||||
|
decimal getUpperLimit() const;
|
||||||
|
|
||||||
|
/// Set the upper limit
|
||||||
|
void setUpperLimit(decimal upperLimit);
|
||||||
|
|
||||||
|
/// Return the motor speed
|
||||||
|
decimal getMotorSpeed() const;
|
||||||
|
|
||||||
|
/// Set the motor speed
|
||||||
|
void setMotorSpeed(decimal motorSpeed);
|
||||||
|
|
||||||
|
/// Return the maximum motor force
|
||||||
|
decimal getMaxMotorForce() const;
|
||||||
|
|
||||||
|
/// Set the maximum motor force
|
||||||
|
void setMaxMotorForce(decimal maxMotorForce);
|
||||||
|
|
||||||
|
/// Return the intensity of the current force applied for the joint motor
|
||||||
|
decimal getMotorForce(decimal timeStep) const;
|
||||||
|
|
||||||
/// Return the number of bytes used by the joint
|
/// Return the number of bytes used by the joint
|
||||||
virtual size_t getSizeInBytes() const;
|
virtual size_t getSizeInBytes() const;
|
||||||
|
|
||||||
|
@ -208,6 +295,41 @@ class SliderJoint : public Constraint {
|
||||||
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
|
virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData);
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// Return true if the limits or the joint are enabled
|
||||||
|
inline bool SliderJoint::isLimitEnabled() const {
|
||||||
|
return mIsLimitEnabled;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return true if the motor of the joint is enabled
|
||||||
|
inline bool SliderJoint::isMotorEnabled() const {
|
||||||
|
return mIsMotorEnabled;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the lower limit
|
||||||
|
inline decimal SliderJoint::getLowerLimit() const {
|
||||||
|
return mLowerLimit;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the upper limit
|
||||||
|
inline decimal SliderJoint::getUpperLimit() const {
|
||||||
|
return mUpperLimit;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the motor speed
|
||||||
|
inline decimal SliderJoint::getMotorSpeed() const {
|
||||||
|
return mMotorSpeed;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the maximum motor force
|
||||||
|
inline decimal SliderJoint::getMaxMotorForce() const {
|
||||||
|
return mMaxMotorForce;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return the intensity of the current force applied for the joint motor
|
||||||
|
inline decimal SliderJoint::getMotorForce(decimal timeStep) const {
|
||||||
|
return mImpulseMotor / timeStep;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the number of bytes used by the joint
|
// Return the number of bytes used by the joint
|
||||||
inline size_t SliderJoint::getSizeInBytes() const {
|
inline size_t SliderJoint::getSizeInBytes() const {
|
||||||
return sizeof(SliderJoint);
|
return sizeof(SliderJoint);
|
||||||
|
|
Loading…
Reference in New Issue
Block a user