void CheckCollisions()
{
// Check for any collisions
for (int i = 0; i < m_rigidBodies.Count; i++)
{
RigidBody_c b1 = m_rigidBodies[i];
for (int j = i + 1; j < m_rigidBodies.Count; j++)
{
RigidBody_c b2 = m_rigidBodies[j];
float cullingRadius = b1.maxRadius + b2.maxRadius;
if ((b1.body.x - b2.body.x).LengthSquared() > cullingRadius * cullingRadius)
{
continue;
}
//if (b1.body.inv_m==0.0f && b1.body.inv_m==0.0f) continue;
Vector3 n = Vector3.Zero;
Vector3 p1 = Vector3.Zero;
Vector3 p2 = Vector3.Zero;
bool haveHit =
Intersection_c.HasIntersection(b1.collideModel, b1.body.q, b1.body.x - MyMath.Rotate(b1.body.q, b1.body.com),
b2.collideModel, b2.body.q, b2.body.x - MyMath.Rotate(b2.body.q, b2.body.com),
out n,
out p1,
out p2);
if (haveHit)
{
Arbiter_c arb = ArbiterContainer_c.FindArbiter(ref b1, ref b2);
// Find the support points
Vector3 s1 = Collision.TransformSupportVert(b1.collideModel, b1.body.q, b1.body.x - MyMath.Rotate(b1.body.q, b1.body.com), -n);
Vector3 s2 = Collision.TransformSupportVert(b2.collideModel, b2.body.q, b2.body.x - MyMath.Rotate(b2.body.q, b2.body.com), n);
Vector3 pp1 = (s1 - p1) * Vector3.Dot(n, n) + p1;
Vector3 pp2 = (s2 - p2) * Vector3.Dot(n, n) + p2;
arb.AddContact(pp1, pp2, n);
}
}
}
}
public ArbiterItem_c(ArbiterKey_c key, Arbiter_c arbiter)
{
this.key = key;
this.arbiter = arbiter;
}
