/* (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#solveVelocityConstraints(org.dyn4j.dynamics.Step, org.dyn4j.dynamics.Settings)
*/
@Override
public void solveVelocityConstraints(Step step, Settings settings) {
Mass m1 = this.body1.getMass();
Mass m2 = this.body2.getMass();
double invI1 = m1.getInverseInertia();
double invI2 = m2.getInverseInertia();
// check if the limit needs to be applied (if we are at one of the limits
// then we ignore the ratio)
if (this.limitState != LimitState.INACTIVE) {
// solve the angular constraint
// get the relative velocity
double C = this.body1.getAngularVelocity() - this.body2.getAngularVelocity();
// get the impulse required to obtain the speed
double impulse = this.invK * -C;
if (this.limitState == LimitState.EQUAL) {
this.impulse += impulse;
} else if (this.limitState == LimitState.AT_LOWER) {
double newImpulse = this.impulse + impulse;
if (newImpulse < 0.0) {
impulse = -this.impulse;
this.impulse = 0.0;
}
} else if (this.limitState == LimitState.AT_UPPER) {
double newImpulse = this.impulse + impulse;
if (newImpulse > 0.0) {
impulse = -this.impulse;
this.impulse = 0.0;
}
}
// apply the impulse
this.body1.setAngularVelocity(this.body1.getAngularVelocity() + invI1 * impulse);
this.body2.setAngularVelocity(this.body2.getAngularVelocity() - invI2 * impulse);
} else if (this.ratio != 1.0) {
// the limit is inactive and the ratio is not one
// get the relative velocity
double C = this.body1.getAngularVelocity() - this.ratio * this.body2.getAngularVelocity();
// get the impulse required to obtain the speed
double impulse = this.invK * -C;
// apply the impulse
this.body1.setAngularVelocity(this.body1.getAngularVelocity() + invI1 * impulse);
this.body2.setAngularVelocity(this.body2.getAngularVelocity() - invI2 * impulse * this.ratio);
}
}
/* (non-Javadoc)
* @see org.dyn4j.dynamics.joint.Joint#solveVelocityConstraints(org.dyn4j.dynamics.Step, org.dyn4j.dynamics.Settings)
*/
@Override
public void solveVelocityConstraints(Step step, Settings settings) {
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();
// compute r1 and r2
Vector2 r1 = t1.getTransformedR(this.body1.getLocalCenter().to(this.localAnchor1));
Vector2 r2 = t2.getTransformedR(this.body2.getLocalCenter().to(this.localAnchor2));
// compute the relative velocity
Vector2 v1 = body1.getLinearVelocity().sum(r1.cross(body1.getAngularVelocity()));
Vector2 v2 = body2.getLinearVelocity().sum(r2.cross(body2.getAngularVelocity()));
// compute Jv
double Jv = n.dot(v1.difference(v2));
// compute lambda (the magnitude of the impulse)
double j = -this.invK * (Jv + this.bias + this.gamma * this.impulse);
this.impulse += j;
// apply the impulse
Vector2 J = n.product(j);
body1.getLinearVelocity().add(J.product(invM1));
body1.setAngularVelocity(body1.getAngularVelocity() + invI1 * r1.cross(J));
body2.getLinearVelocity().subtract(J.product(invM2));
body2.setAngularVelocity(body2.getAngularVelocity() - invI2 * r2.cross(J));
}
/* (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;
}
/* (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));
}
/* (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 linearTolerance = settings.getLinearTolerance();
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();
// compute the normal
Vector2 r1 = t1.getTransformedR(this.body1.getLocalCenter().to(this.localAnchor1));
Vector2 r2 = t2.getTransformedR(this.body2.getLocalCenter().to(this.localAnchor2));
this.n = r1.sum(this.body1.getWorldCenter()).subtract(r2.sum(this.body2.getWorldCenter()));
// get the current length
double length = this.n.getMagnitude();
// check for the tolerance
if (length < linearTolerance) {
this.n.zero();
} else {
// normalize it
this.n.multiply(1.0 / length);
}
// compute K inverse
double cr1n = r1.cross(this.n);
double cr2n = r2.cross(this.n);
double invMass = invM1 + invI1 * cr1n * cr1n;
invMass += invM2 + invI2 * cr2n * cr2n;
// check for zero before inverting
this.invK = invMass <= Epsilon.E ? 0.0 : 1.0 / invMass;
// see if we need to compute spring damping
if (this.frequency > 0.0) {
double dt = step.getDeltaTime();
// get the current compression/extension of the spring
double x = length - this.distance;
// compute the natural frequency; f = w / (2 * pi) -> w = 2 * pi * f
double w = Geometry.TWO_PI * this.frequency;
// compute the damping coefficient; dRatio = d / (2 * m * w) -> d = 2 * m * w * dRatio
double d = 2.0 * this.invK * this.dampingRatio * w;
// compute the spring constant; w = sqrt(k / m) -> k = m * w * w
double k = this.invK * w * w;
// compute gamma = CMF = 1 / (hk + d)
this.gamma = dt * (d + dt * k);
// check for zero before inverting
this.gamma = this.gamma <= Epsilon.E ? 0.0 : 1.0 / this.gamma;
// compute the bias = x * ERP where ERP = hk / (hk + d)
this.bias = x * dt * k * this.gamma;
// compute the effective mass
invMass += this.gamma;
// check for zero before inverting
this.invK = invMass <= Epsilon.E ? 0.0 : 1.0 / invMass;
} else {
this.gamma = 0.0;
this.bias = 0.0;
}
// warm start
impulse *= step.getDeltaTimeRatio();
Vector2 J = n.product(impulse);
body1.getLinearVelocity().add(J.product(invM1));
body1.setAngularVelocity(body1.getAngularVelocity() + invI1 * r1.cross(J));
body2.getLinearVelocity().subtract(J.product(invM2));
body2.setAngularVelocity(body2.getAngularVelocity() - invI2 * r2.cross(J));
}
