/* (non-Javadoc)
* @see org.dyn4j.dynamics.joint.Joint#solvePositionConstraints(org.dyn4j.dynamics.Step, org.dyn4j.dynamics.Settings)
*/
@Override
public boolean solvePositionConstraints(Step step, Settings settings) {
// check if the constraint needs to be applied
if (this.limitState != LimitState.INACTIVE) {
double angularTolerance = settings.getAngularTolerance();
double maxAngularCorrection = settings.getMaximumAngularCorrection();
Mass m1 = this.body1.getMass();
Mass m2 = this.body2.getMass();
double invI1 = m1.getInverseInertia();
double invI2 = m2.getInverseInertia();
// get the current angle between the bodies
double angle = this.getRelativeRotation();
double impulse = 0.0;
double angularError = 0.0;
// check the limit state
if (this.limitState == LimitState.EQUAL) {
// if the limits are equal then clamp the impulse to maintain
// the constraint between the maximum
double j =
Interval.clamp(angle - this.lowerLimit, -maxAngularCorrection, maxAngularCorrection);
impulse = -j * this.invK;
angularError = Math.abs(j);
} else if (this.limitState == LimitState.AT_LOWER) {
// if the joint is at the lower limit then clamp only the lower value
double j = angle - this.lowerLimit;
angularError = -j;
j = Interval.clamp(j + angularTolerance, -maxAngularCorrection, 0.0);
impulse = -j * this.invK;
} else if (this.limitState == LimitState.AT_UPPER) {
// if the joint is at the upper limit then clamp only the upper value
double j = angle - this.upperLimit;
angularError = j;
j = Interval.clamp(j - angularTolerance, 0.0, maxAngularCorrection);
impulse = -j * this.invK;
}
// apply the corrective impulses to the bodies
this.body1.rotateAboutCenter(invI1 * impulse);
this.body2.rotateAboutCenter(-invI2 * impulse);
return angularError <= angularTolerance;
} else {
return true;
}
}
/* (non-Javadoc)
* @see org.dyn4j.dynamics.joint.Joint#initializeConstraints(org.dyn4j.dynamics.Step, org.dyn4j.dynamics.Settings)
*/
@Override
public void initializeConstraints(Step step, Settings settings) {
double angularTolerance = settings.getAngularTolerance();
Mass m1 = this.body1.getMass();
Mass m2 = this.body2.getMass();
double invI1 = m1.getInverseInertia();
double invI2 = m2.getInverseInertia();
// check if the limits are enabled
if (this.limitEnabled) {
// compute the current angle
double angle = this.getRelativeRotation();
// if they are enabled check if they are equal
if (Math.abs(this.upperLimit - this.lowerLimit) < 2.0 * angularTolerance) {
// if so then set the state to equal
this.limitState = LimitState.EQUAL;
} else {
// make sure we have valid settings
if (this.upperLimit > this.lowerLimit) {
// check against the max and min distances
if (angle >= this.upperLimit) {
// is the limit already at the upper limit
if (this.limitState != LimitState.AT_UPPER) {
this.impulse = 0;
}
// set the state to at upper
this.limitState = LimitState.AT_UPPER;
} else if (angle <= this.lowerLimit) {
// is the limit already at the lower limit
if (this.limitState != LimitState.AT_LOWER) {
this.impulse = 0;
}
// set the state to at lower
this.limitState = LimitState.AT_LOWER;
} else {
// set the state to inactive
this.limitState = LimitState.INACTIVE;
this.impulse = 0;
}
}
}
} else {
// neither is enabled so no constraint needed at this time
this.limitState = LimitState.INACTIVE;
this.impulse = 0;
}
// compute the mass
if (this.limitState == LimitState.INACTIVE) {
// compute the angular mass including the ratio
this.invK = invI1 + this.ratio * this.ratio * invI2;
} else {
// compute the angular mass normally
this.invK = invI1 + invI2;
}
if (this.invK > Epsilon.E) {
this.invK = 1.0 / this.invK;
}
// account for variable time step
this.impulse *= step.getDeltaTimeRatio();
// warm start
this.body1.setAngularVelocity(this.body1.getAngularVelocity() + invI1 * this.impulse);
// we only want to apply the ratio to the impulse if the limits are not active. When the
// limits are active we effectively disable the ratio
this.body2.setAngularVelocity(
this.body2.getAngularVelocity()
- invI2 * this.impulse * (this.limitState == LimitState.INACTIVE ? this.ratio : 1.0));
}