/* (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#solvePositionConstraints(org.dyn4j.dynamics.Step, org.dyn4j.dynamics.Settings)
*/
@Override
public boolean solvePositionConstraints(Step step, Settings settings) {
// check if this is a spring damper
if (this.frequency > 0.0) {
// don't solve position constraints for spring damper
return true;
}
double linearTolerance = settings.getLinearTolerance();
double maxLinearCorrection = settings.getMaximumLinearCorrection();
Transform t1 = body1.getTransform();
Transform t2 = body2.getTransform();
Mass m1 = body1.getMass();
Mass m2 = body2.getMass();
double invM1 = m1.getInverseMass();
double invM2 = m2.getInverseMass();
double invI1 = m1.getInverseInertia();
double invI2 = m2.getInverseInertia();
Vector2 c1 = body1.getWorldCenter();
Vector2 c2 = body2.getWorldCenter();
// recompute n since it may have changed after integration
Vector2 r1 = t1.getTransformedR(this.body1.getLocalCenter().to(this.localAnchor1));
Vector2 r2 = t2.getTransformedR(this.body2.getLocalCenter().to(this.localAnchor2));
n = r1.sum(body1.getWorldCenter()).subtract(r2.sum(body2.getWorldCenter()));
// solve the position constraint
double l = n.normalize();
double C = l - this.distance;
C = Interval.clamp(C, -maxLinearCorrection, maxLinearCorrection);
double impulse = -this.invK * C;
Vector2 J = n.product(impulse);
// translate and rotate the objects
body1.translate(J.product(invM1));
body1.rotate(invI1 * r1.cross(J), c1);
body2.translate(J.product(-invM2));
body2.rotate(-invI2 * r2.cross(J), c2);
return Math.abs(C) < linearTolerance;
}