*/
@Override
public void preStep(float invDT) {
// calculate the spring's vector (pointing from body1 to body2) and length
spring = new Vector2f(body2.getPosition());
spring.add(r2);
spring.sub(body1.getPosition());
spring.sub(r1);
springLength = spring.length();
// the spring vector needs to be normalized for applyImpulse as well!
spring.normalise();
// calculate the spring's forces
// note that although theoretically invDT could never be 0
// but here it can
float springConst;
if ( springLength < minSpringSize || springLength > maxSpringSize ) {
// Pre-compute anchors, mass matrix, and bias.
Matrix2f rot1 = new Matrix2f(body1.getRotation());
Matrix2f rot2 = new Matrix2f(body2.getRotation());
r1 = MathUtil.mul(rot1,localAnchor1);
r2 = MathUtil.mul(rot2,localAnchor2);
// the mass normal or 'k'
float rn1 = r1.dot(spring);
float rn2 = r2.dot(spring);
float kNormal = body1.getInvMass() + body2.getInvMass();
kNormal += body1.getInvI() * (r1.dot(r1) - rn1 * rn1) + body2.getInvI() * (r2.dot(r2) - rn2 * rn2);
massNormal = 1 / kNormal;
// The spring is broken so apply force to correct it
// note that we use biased velocities for this
float springImpulse =
invDT != 0 ? brokenSpringConst * (springLength - springSize) / invDT : 0;
Vector2f impulse = MathUtil.scale(spring, springImpulse);
body1.adjustBiasedVelocity(MathUtil.scale(impulse, body1.getInvMass()));
body1.adjustBiasedAngularVelocity((body1.getInvI() * MathUtil.cross(r1, impulse)));
body2.adjustBiasedVelocity(MathUtil.scale(impulse, -body2.getInvMass()));
body2.adjustBiasedAngularVelocity(-(body2.getInvI() * MathUtil.cross(r2, impulse)));
isBroken = true;
return;
} else if ( springLength < springSize ) {
springConst = compressedSpringConst;
isBroken = false;
} else { // if ( springLength >= springSize )
springConst = stretchedSpringConst;
isBroken = false;
}
float springImpulse =
invDT != 0 ? springConst * (springLength - springSize) / invDT : 0;
// apply the spring's forces
Vector2f impulse = MathUtil.scale(spring, springImpulse);
body1.adjustVelocity(MathUtil.scale(impulse, body1.getInvMass()));
body1.adjustAngularVelocity((body1.getInvI() * MathUtil.cross(r1, impulse)));
body2.adjustVelocity(MathUtil.scale(impulse, -body2.getInvMass()));
body2.adjustAngularVelocity(-(body2.getInvI() * MathUtil.cross(r2, impulse)));