if (collider == null || c == null) {
return;
}
Vector3 contact = new Vector3(0.0f);
Vector3 displacement = new Vector3(0.0f);
Vector3 normal = new Vector3(0.0f);
boolean b = collider.colliding(c, contact, displacement, normal);
if (b) {
//ent.transform.position = ent.transform.position.plus(displacement);
boolean both = true;
RigidBody rb2 = c.ent.rigidBody();
float moiA, moiB;
if (rb2 == null) {
rb2 = new RigidBody();
rb2.mass = Float.POSITIVE_INFINITY;
both = false;
}
Vector3 pos1 = contact.minus(ent.transform.position);
Vector3 pos2 = contact.minus(c.ent.transform.position);
Vector3 va = velocity.plus(Vector3.cross(angularVelocity, pos1));
Vector3 vb = rb2.velocity.plus(Vector3.cross(rb2.angularVelocity, pos2));
if (rb2.ent == null) {
ent.transform.position = ent.transform.position.plus(displacement);
} else {
Vector3 vt = va.plus(vb);
float magt = vt.mag();
if (magt == 0.0f) {
ent.transform.position = ent.transform.position.plus(displacement);
} else {
float f1 = va.mag() / magt;
float f2 = 1.0f - f1;
ent.transform.position = ent.transform.position.plus(displacement.times(f1));
rb2.ent.transform.position = rb2.ent.transform.position.minus(displacement.times(f2));
}
}
Vector3 vr = va.minus(vb);
// Normal impulse
float num = -Vector3.dot(vr.times(1 + coefficientOfRestitution), normal);
float f = 1.0f / mass + 1.0f / rb2.mass;
Vector3 n2 = normal.times(f);
float denom1 = Vector3.dot(normal, n2);
moiA = momentOfInertia(Vector3.cross(normal, pos1).normalize());
if (both) {
moiB = rb2.momentOfInertia(Vector3.cross(normal, pos2).normalize());
} else {
moiB = Float.POSITIVE_INFINITY;
}
Vector3 cross1 = Vector3.cross(Vector3.divide(Vector3.cross(pos1, normal), moiA), pos1);
Vector3 cross2 = Vector3.cross(Vector3.divide(Vector3.cross(pos2, normal), moiB), pos2);
Vector3 cross = cross1.plus(cross2);
float denom2 = Vector3.dot(normal, cross);
float denom = denom1 + denom2;
float impulse = num / denom;
Vector3 impulseN = normal.times(impulse);
impel(impulseN, pos1);
rb2.impel(impulseN.negate(), pos2);
// Frictional impulse
Vector3 tangent = Vector3.cross(Vector3.cross(vr, normal), normal).normalize();
if (tangent.mag2() == 0.0f) {
return;
}
num = -Vector3.dot(vr.times(coefficientOfFriction), tangent);
Vector3 t2 = tangent.times(f);
denom1 = Vector3.dot(tangent, t2);
moiA = momentOfInertia(Vector3.cross(tangent, pos1).normalize());
if (both) {
moiB = rb2.momentOfInertia(Vector3.cross(tangent, pos2).normalize());
} else {
moiB = Float.POSITIVE_INFINITY;
}
cross1 = Vector3.cross(Vector3.divide(Vector3.cross(pos1, tangent), moiA), pos1);
cross2 = Vector3.cross(Vector3.divide(Vector3.cross(pos2, tangent), moiB), pos2);
cross = cross1.plus(cross2);
denom2 = Vector3.dot(tangent, cross);
denom = denom1 + denom2;
float friction = num / denom;
Vector3 frictionT = tangent.times(friction);
impel(frictionT, pos1);
rb2.impel(frictionT.negate(), pos2);
//Vector3 friction = Vector3.cross(Vector3.cross(impulse, normal), normal).times(coefficientOfFriction);
//impel(friction, pos1);
}
}