float biasFactor = 0.005f;
float biasImpulse = 0.0f;
float RA = body1.getRotation() + rotateA;
float RB = body1.getRotation() + rotateB;
Vector2f VA = new Vector2f((float) Math.cos(RA), (float) Math.sin(RA));
Vector2f VB = new Vector2f((float) Math.cos(RB), (float) Math.sin(RB));
Matrix2f rot1 = new Matrix2f(body1.getRotation());
Matrix2f rot2 = new Matrix2f(body2.getRotation());
Vector2f r1 = MathUtil.mul(rot1, anchor1);
Vector2f r2 = MathUtil.mul(rot2, anchor2);
Vector2f p1 = new Vector2f(body1.getPosition());
p1.add(r1);
Vector2f p2 = new Vector2f(body2.getPosition());
p2.add(r2);
dp = new Vector2f(p2);
dp.sub(p1);
dlength2 = dp.lengthSquared();
ndp = new Vector2f(dp);
ndp.normalise();
R = new Vector2f(r1);
R.add(dp);
// System.out.println(accumulateImpulse);
Vector2f relativeVelocity = new Vector2f(body2.getVelocity());
relativeVelocity.add(MathUtil.cross(r2, body2.getAngularVelocity()));
relativeVelocity.sub(body1.getVelocity());
relativeVelocity.sub(MathUtil.cross(r1, body1.getAngularVelocity()));
/*
* Matrix2f tr1 = new Matrix2f(-body1.getRotation()); relativeVelocity =
* MathUtil.mul(tr1, relativeVelocity);
* relativeVelocity.add(MathUtil.mul(tr1, dp));
*/
// relativeVelocity.add(MathUtil.cross(dp,body1.getAngularVelocity()));
n = new Vector2f(-ndp.y, ndp.x);
Vector2f v1 = new Vector2f(n);
v1.scale(-body2.getInvMass() - body1.getInvMass());
Vector2f v2 = MathUtil.cross(MathUtil.cross(r2, n), r2);
v2.scale(-body2.getInvI());
Vector2f v3 = MathUtil.cross(MathUtil.cross(R, n), r1);
v3.scale(-body1.getInvI());
Vector2f K1 = new Vector2f(v1);
K1.add(v2);
K1.add(v3);
K = MathUtil.cross(dp, K1) / dlength2 - MathUtil.cross(R, n)
* body1.getInvI();
restituteAngular = -restitution
* (MathUtil.cross(dp, relativeVelocity) / dlength2 - body1
.getAngularVelocity());
if (MathUtil.cross(ndp, VA) > 0) {
// collide on A side
if (bounceSide != BOUNCE_LOWER)
accumulateImpulse = 0;
biasImpulse = biasFactor * MathUtil.cross(ndp, VA) * invDT;
bounceSide = BOUNCE_LOWER;
if (restituteAngular < 0) {
restituteAngular = 0;
}
} else if (MathUtil.cross(VB, ndp) > 0) {
// collide on B side
if (bounceSide != BOUNCE_HIGHER)
accumulateImpulse = 0;
biasImpulse = -biasFactor * MathUtil.cross(VB, ndp) * invDT;
bounceSide = BOUNCE_HIGHER;
if (restituteAngular > 0) {
restituteAngular = 0;
}
} else {
accumulateImpulse = 0;
biasImpulse = 0.0f;
bounceSide = BOUNCE_NONE;
}
restituteAngular += biasImpulse;
Vector2f impulse = new Vector2f(n);
impulse.scale(accumulateImpulse);
if (!body1.isStatic()) {
Vector2f accum1 = new Vector2f(impulse);
accum1.scale(body1.getInvMass());
body1.adjustVelocity(accum1);
body1.adjustAngularVelocity((body1.getInvI() * MathUtil.cross(R,
impulse)));
}
if (!body2.isStatic()) {
Vector2f accum2 = new Vector2f(impulse);
accum2.scale(-body2.getInvMass());
body2.adjustVelocity(accum2);
body2.adjustAngularVelocity(-(body2.getInvI() * MathUtil.cross(r2,
impulse)));
}
}