protected override void Test(NarrowPhase test, Part a, Part b)
{
int thread = TaskManager.CurrentThreadIndex;
_a[thread] = a;
_b[thread] = b;
RigidBody ba = ((BodySkin)a.Owner).Owner, bb = ((BodySkin)b.Owner).Owner;
if (ba.IsFast || bb.IsFast)
{
var delta = Vector3.Zero;
if (ba.IsFast)
{
Vector3.Add(ref delta, ref ba.Velocity.Linear, out delta);
}
if (bb.IsFast)
{
Vector3.Subtract(ref delta, ref bb.Velocity.Linear, out delta);
}
Vector3.Multiply(ref delta, bb.Manager.TimeStep, out delta);
test.SweptTest(this, a, b, delta);
}
else
{
test.OverlapTest(this, a, b);
}
var current = _contact[thread];
if (current != null)
{
var points = current.Points;
var normal = current.Normal;
// normalize contact normal if required
if (Math.Abs(normal.LengthSquared() - 1f) >= Constants.Epsilon)
{
normal.Normalize();
current.Normal = normal;
}
for (int i = 0; i < current.Count; i++)
{
// transform points to old position
current.BodyA.Skin.UndoTransform(ref points[i].OffsetA);
current.BodyB.Skin.UndoTransform(ref points[i].OffsetB);
// calculate penetration depth of each point
float ax, bx;
Vector3.Dot(ref normal, ref points[i].OffsetA, out ax);
Vector3.Dot(ref normal, ref points[i].OffsetB, out bx);
points[i].Depth = bx - ax;
// convert world points to world offsets
Vector3.Subtract(ref points[i].OffsetA, ref current.BodyA.World.Position, out points[i].OffsetA);
Vector3.Subtract(ref points[i].OffsetB, ref current.BodyB.World.Position, out points[i].OffsetB);
}
_items[thread].Add(current);
_contact[thread] = null;
}
}
