Finish the implementation of the slider joint
This commit is contained in:
Daniel Chappuis
parent
61562b3560
commit
9c0844cf1b
|
|
@ -29,8 +29,11 @@
|
|||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Static variables definition
|
||||
const decimal BallAndSocketJoint::BETA = 0.2;
|
||||
|
||||
// Constructor
|
||||
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo &jointInfo)
|
||||
BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
|
||||
: Constraint(jointInfo), mImpulse(Vector3(0, 0, 0)) {
|
||||
|
||||
// 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;
|
||||
|
||||
// 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& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||
|
||||
|
|
@ -89,6 +94,20 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
|
|||
if (mBody1->getIsMotionEnabled() || mBody2->getIsMotionEnabled()) {
|
||||
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)
|
||||
|
|
@ -126,10 +145,6 @@ void BallAndSocketJoint::warmstart(const ConstraintSolverData& constraintSolverD
|
|||
// Solve the velocity constraint
|
||||
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
|
||||
Vector3& v1 = constraintSolverData.linearVelocities[mIndexBody1];
|
||||
Vector3& v2 = constraintSolverData.linearVelocities[mIndexBody2];
|
||||
|
|
@ -143,18 +158,10 @@ void BallAndSocketJoint::solveVelocityConstraint(const ConstraintSolverData& con
|
|||
Matrix3x3 I2 = mBody2->getInertiaTensorInverseWorld();
|
||||
|
||||
// Compute J*v
|
||||
const Vector3 Jv = -v1 + mR1World.cross(w1) + v2 - mR2World.cross(w2);
|
||||
|
||||
// Compute the bias "b" of the constraint
|
||||
Vector3 b(0, 0, 0);
|
||||
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
||||
decimal beta = decimal(0.2); // TODO : Use a constant here
|
||||
decimal biasFactor = (beta / constraintSolverData.timeStep);
|
||||
b = biasFactor * (x2 + mR2World - x1 - mR1World);
|
||||
}
|
||||
const Vector3 Jv = v2 + w2.cross(mR2World) - v1 - w1.cross(mR1World);
|
||||
|
||||
// Compute the Lagrange multiplier lambda
|
||||
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - b);
|
||||
const Vector3 deltaLambda = mInverseMassMatrix * (-Jv - mBiasVector);
|
||||
mImpulse += deltaLambda;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
|
|
|
|||
|
|
@ -62,6 +62,11 @@ class BallAndSocketJoint : public Constraint {
|
|||
|
||||
private :
|
||||
|
||||
// -------------------- Constants -------------------- //
|
||||
|
||||
// Beta value for the bias factor of position correction
|
||||
static const decimal BETA;
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// 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
|
||||
Vector3 mR2World;
|
||||
|
||||
/// Bias vector for the constraint
|
||||
Vector3 mBiasVector;
|
||||
|
||||
/// Skew-Symmetric matrix for cross product with vector mR1World
|
||||
Matrix3x3 mSkewSymmetricMatrixR1World;
|
||||
|
||||
|
|
|
|||
|
|
@ -33,9 +33,9 @@
|
|||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
|
||||
// Enumeration for the type of a constraint
|
||||
enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT};
|
||||
enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT};
|
||||
|
||||
// Class declarations
|
||||
struct ConstraintSolverData;
|
||||
|
|
|
|||
|
|
@ -28,15 +28,21 @@
|
|||
|
||||
using namespace reactphysics3d;
|
||||
|
||||
// Static variables definition
|
||||
const decimal SliderJoint::BETA = decimal(0.2);
|
||||
|
||||
// Constructor
|
||||
SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
|
||||
: Constraint(jointInfo), mImpulseTranslation(0, 0), mImpulseRotation(0, 0, 0),
|
||||
mImpulseLowerLimit(0), mImpulseUpperLimit(0),
|
||||
mIsLimitsActive(jointInfo.isLimitsActive), mLowerLimit(jointInfo.lowerLimit),
|
||||
mUpperLimit(jointInfo.upperLimit) {
|
||||
mImpulseLowerLimit(0), mImpulseUpperLimit(0), mImpulseMotor(0),
|
||||
mIsLimitEnabled(jointInfo.isLimitEnabled), mIsMotorEnabled(jointInfo.isMotorEnabled),
|
||||
mLowerLimit(jointInfo.lowerLimit), mUpperLimit(jointInfo.upperLimit),
|
||||
mIsLowerLimitViolated(false), mIsUpperLimitViolated(false),
|
||||
mMotorSpeed(jointInfo.motorSpeed), mMaxMotorForce(jointInfo.maxMotorForce){
|
||||
|
||||
assert(mUpperLimit >= 0.0);
|
||||
assert(mLowerLimit <= 0.0);
|
||||
assert(mMaxMotorForce >= 0.0);
|
||||
|
||||
// Compute the local-space anchor point for each body
|
||||
const Transform& transform1 = mBody1->getTransform();
|
||||
|
|
@ -63,7 +69,7 @@ SliderJoint::~SliderJoint() {
|
|||
// Initialize before solving the constraint
|
||||
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;
|
||||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||
|
||||
|
|
@ -94,8 +100,16 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
decimal uDotSliderAxis = u.dot(mSliderAxisWorld);
|
||||
decimal lowerLimitError = uDotSliderAxis - mLowerLimit;
|
||||
decimal upperLimitError = mUpperLimit - uDotSliderAxis;
|
||||
mIsLowerLimitViolated = (lowerLimitError <= 0);
|
||||
mIsUpperLimitViolated = (upperLimitError <= 0);
|
||||
bool oldIsLowerLimitViolated = mIsLowerLimitViolated;
|
||||
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
|
||||
mR2CrossN1 = mR2.cross(mN1);
|
||||
|
|
@ -139,8 +153,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
|
||||
// Compute the bias "b" of the translation constraint
|
||||
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) {
|
||||
mBTranslation.x = u.dot(mN1);
|
||||
mBTranslation.y = u.dot(mN2);
|
||||
|
|
@ -170,21 +183,54 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
|||
mBRotation = biasFactor * decimal(2.0) * qError.getVectorV();
|
||||
}
|
||||
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix)
|
||||
mInverseMassMatrixLimit = sumInverseMass +
|
||||
mR1PlusUCrossSliderAxis.dot(I1 * mR1PlusUCrossSliderAxis) +
|
||||
mR2CrossSliderAxis.dot(I2 * mR2CrossSliderAxis);
|
||||
if (mIsLimitEnabled && (mIsLowerLimitViolated || mIsUpperLimitViolated)) {
|
||||
|
||||
// Compute the bias "b" of the lower limit constraint
|
||||
mBLowerLimit = 0.0;
|
||||
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
||||
mBLowerLimit = biasFactor * lowerLimitError;
|
||||
// Compute the inverse of the mass matrix K=JM^-1J^t for the lower limit (1x1 matrix)
|
||||
mInverseMassMatrixLimit = 0.0;
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
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
|
||||
mBUpperLimit = 0.0;
|
||||
if (mPositionCorrectionTechnique == BAUMGARTE_JOINTS) {
|
||||
mBUpperLimit = biasFactor * upperLimitError;
|
||||
// Compute the inverse of mass matrix K=JM^-1J^t for the motor (1x1 matrix)
|
||||
mInverseMassMatrixMotor = 0.0;
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
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;
|
||||
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
|
||||
if (mBody1->getIsMotionEnabled()) {
|
||||
v1 += inverseMassBody1 * linearImpulseBody1;
|
||||
|
|
@ -293,7 +352,7 @@ void SliderJoint::solveVelocityConstraint(const ConstraintSolverData& constraint
|
|||
|
||||
// --------------- Limits Constraints --------------- //
|
||||
|
||||
if (mIsLimitsActive) {
|
||||
if (mIsLimitEnabled) {
|
||||
|
||||
// If the lower limit is violated
|
||||
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
|
||||
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)
|
||||
Vector3 sliderAxisWorldSpace;
|
||||
|
||||
/// True if the slider limits are active
|
||||
bool isLimitsActive;
|
||||
/// True if the slider limits are enabled
|
||||
bool isLimitEnabled;
|
||||
|
||||
/// True if the slider motor is enabled
|
||||
bool isMotorEnabled;
|
||||
|
||||
/// Lower limit
|
||||
decimal lowerLimit;
|
||||
|
|
@ -58,16 +61,23 @@ struct SliderJointInfo : public ConstraintInfo {
|
|||
/// Upper limit
|
||||
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,
|
||||
const Vector3& initAnchorPointWorldSpace,
|
||||
const Vector3& initSliderAxisWorldSpace)
|
||||
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
||||
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||
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,
|
||||
const Vector3& initAnchorPointWorldSpace,
|
||||
const Vector3& initSliderAxisWorldSpace,
|
||||
|
|
@ -75,8 +85,21 @@ struct SliderJointInfo : public ConstraintInfo {
|
|||
: ConstraintInfo(rigidBody1, rigidBody2, SLIDERJOINT),
|
||||
anchorPointWorldSpace(initAnchorPointWorldSpace),
|
||||
sliderAxisWorldSpace(initSliderAxisWorldSpace),
|
||||
isLimitsActive(true), lowerLimit(initLowerLimit),
|
||||
upperLimit(initUpperLimit) {}
|
||||
isLimitEnabled(true), isMotorEnabled(false), lowerLimit(initLowerLimit),
|
||||
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
|
||||
|
|
@ -87,6 +110,11 @@ class SliderJoint : public Constraint {
|
|||
|
||||
private :
|
||||
|
||||
// -------------------- Constants -------------------- //
|
||||
|
||||
// Beta value for the position correction bias factor
|
||||
static const decimal BETA;
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
/// 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)
|
||||
decimal mInverseMassMatrixLimit;
|
||||
|
||||
/// Inverse of mass matrix K=JM^-1J^t for the motor
|
||||
decimal mInverseMassMatrixMotor;
|
||||
|
||||
/// Impulse for the 2 translation constraints
|
||||
Vector2 mImpulseTranslation;
|
||||
|
||||
|
|
@ -164,8 +195,14 @@ class SliderJoint : public Constraint {
|
|||
/// Impulse for the upper limit constraint
|
||||
decimal mImpulseUpperLimit;
|
||||
|
||||
/// True if the slider limits are active
|
||||
bool mIsLimitsActive;
|
||||
/// Impulse for the motor
|
||||
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
|
||||
Vector3 mSliderAxisWorld;
|
||||
|
|
@ -182,6 +219,17 @@ class SliderJoint : public Constraint {
|
|||
/// True if the upper limit is violated
|
||||
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 :
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
|
@ -192,6 +240,45 @@ class SliderJoint : public Constraint {
|
|||
/// Destructor
|
||||
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
|
||||
virtual size_t getSizeInBytes() const;
|
||||
|
||||
|
|
@ -208,6 +295,41 @@ class SliderJoint : public Constraint {
|
|||
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
|
||||
inline size_t SliderJoint::getSizeInBytes() const {
|
||||
return sizeof(SliderJoint);
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user