internal static float transformToAxis(CollisionBox box, Vector3 axis)
{
return
box.HalfSize.X * MathHelper.Abs(Vector3.Dot(axis, box.GetAxis(0))) +
box.HalfSize.Y * MathHelper.Abs(Vector3.Dot(axis, box.GetAxis(1))) +
box.HalfSize.Z * MathHelper.Abs(Vector3.Dot(axis, box.GetAxis(2)));
}
public static bool BoxAndHalfSpace(CollisionBox box, CollisionPlane plane)
{
float projectedRadius = transformToAxis(box, plane.Normal);
float boxDistance = Vector3.Dot(plane.Normal, box.GetAxis(3)) - projectedRadius;
return boxDistance <= plane.D;
}
public static bool BoxAndBox(CollisionBox one, CollisionBox two)
{
Vector3 toCenter = two.GetAxis(3) - one.GetAxis(3);
return (
// Check on box one's axes first
overlapOnAxis(one, two, one.GetAxis(0), toCenter) &&
overlapOnAxis(one, two,one.GetAxis(1), toCenter) &&
overlapOnAxis(one, two,one.GetAxis(2), toCenter) &&
// And on two's
overlapOnAxis(one, two,two.GetAxis(0), toCenter) &&
overlapOnAxis(one, two,two.GetAxis(1), toCenter) &&
overlapOnAxis(one, two,two.GetAxis(2), toCenter) &&
// Now on the cross products
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(0), two.GetAxis(0)), toCenter) &&
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(0), two.GetAxis(1)), toCenter) &&
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(0), two.GetAxis(2)), toCenter) &&
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(1), two.GetAxis(0)), toCenter) &&
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(1), two.GetAxis(1)), toCenter) &&
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(1), two.GetAxis(2)), toCenter) &&
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(2), two.GetAxis(0)), toCenter) &&
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(2), two.GetAxis(1)), toCenter) &&
overlapOnAxis(one, two,Vector3.Cross(one.GetAxis(2), two.GetAxis(2)), toCenter)
);
}
static void fillPointFaceBoxBox(CollisionBox one, CollisionBox two, Vector3 toCentre, CollisionData data, int best, float pen)
{
Contact contact = new Contact();
Vector3 normal = one.GetAxis(best);
if (Vector3.Dot(one.GetAxis(best), toCentre) > 0)
{
normal = normal * -1.0f;
}
Vector3 vertex = two.HalfSize;
if (Vector3.Dot(two.GetAxis(0), normal) < 0) vertex.X = -vertex.X;
if (Vector3.Dot(two.GetAxis(1), normal) < 0) vertex.Y = -vertex.Y;
if (Vector3.Dot(two.GetAxis(2), normal) < 0) vertex.Z = -vertex.Z;
contact.ContactNormal = normal;
contact.Penetration = pen;
contact.ContactPoint = Vector3.Transform(vertex, two.Transform);
contact.SetData(one.Body, two.Body, data.Friction, data.Restitution);
data.Contacts.Add(contact);
}
public static int BoxAndPoint(CollisionBox box, Vector3 point, ref CollisionData data)
{
Vector3 relPt = box.Transform.TransformInverse(point);
Vector3 normal = new Vector3();
float min_depth = box.HalfSize.X - MathHelper.Abs(relPt.X);
if (min_depth < 0) return 0;
normal = box.GetAxis(0) * ((relPt.X < 0) ? -1 : 1);
float depth = box.HalfSize.Y - MathHelper.Abs(relPt.Y);
if (depth < 0) return 0;
else if (depth < min_depth)
{
min_depth = depth;
normal = box.GetAxis(1) * ((relPt.Y < 0) ? -1 : 1);
}
depth = box.HalfSize.Z - MathHelper.Abs(relPt.Z);
if (depth < 0) return 0;
else if (depth < min_depth)
{
min_depth = depth;
normal = box.GetAxis(2) * ((relPt.Z < 0) ? -1 : 1);
}
Contact contact = new Contact();
contact.ContactNormal = normal;
contact.ContactPoint = point;
contact.Penetration = min_depth;
contact.SetData(box.Body, null, data.Friction, data.Restitution);
data.Contacts.Add(contact);
data.AddContacts(1);
return 1;
}
public static int BoxAndBox(CollisionBox one, CollisionBox two, ref CollisionData data)
{
Vector3 toCenter = two.GetAxis(3) - one.GetAxis(3);
float pen = float.MaxValue;
int best = 0xffffff;
CHECK_OVERLAP(one.GetAxis(0), 0, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(one.GetAxis(1), 1, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(one.GetAxis(2), 2, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(one.GetAxis(0), 3, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(one.GetAxis(1), 4, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(one.GetAxis(2), 5, one, two, toCenter, ref pen, ref best);
int bestSingleAxis = best;
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(0), two.GetAxis(0)), 6, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(0), two.GetAxis(1)), 7, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(0), two.GetAxis(2)), 8, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(1), two.GetAxis(0)), 9, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(1), two.GetAxis(1)), 10, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(1), two.GetAxis(2)), 11, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(2), two.GetAxis(0)), 12, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(2), two.GetAxis(1)), 13, one, two, toCenter, ref pen, ref best);
CHECK_OVERLAP(Vector3.Cross(one.GetAxis(2), two.GetAxis(2)), 14, one, two, toCenter, ref pen, ref best);
if (best == 0xffffff) throw new Exception("same, same");
if (best < 3)
{
fillPointFaceBoxBox(one, two, toCenter, data, best, pen);
data.AddContacts(1);
}
else if (best < 6)
{
fillPointFaceBoxBox(two, one, toCenter * -1.0f, data, best - 3, pen);
data.AddContacts(1);
return 1;
}
else
{
best -= 6;
int oneAxisIndex = best / 3;
int twoAxisIndex = best % 3;
Vector3 oneAxis = one.GetAxis(oneAxisIndex);
Vector3 twoAxis = two.GetAxis(twoAxisIndex);
Vector3 axis = Vector3.Cross(oneAxis, twoAxis);
axis.Normalize();
if (Vector3.Dot(axis, toCenter) > 0) axis = axis * -1.0f;
Vector3 ptOnOneEdge = one.HalfSize;
Vector3 ptOnTwoEdge = two.HalfSize;
for (int i = 0; i < 3; i++)
{
if (i == oneAxisIndex) ptOnOneEdge[i] = 0;
else if (Vector3.Dot(one.GetAxis(i), axis) > 0) ptOnOneEdge[i] = -ptOnOneEdge[i];
if (i == twoAxisIndex) ptOnTwoEdge[i] = 0;
else if (Vector3.Dot(two.GetAxis(i), axis) < 0) ptOnTwoEdge[i] = -ptOnTwoEdge[i];
}
ptOnOneEdge = Vector3.Transform(ptOnOneEdge, one.Transform);
ptOnTwoEdge = Vector3.Transform(ptOnTwoEdge, two.Transform);
Vector3 vertex = contactPoint(
ptOnOneEdge, oneAxis, one.HalfSize[oneAxisIndex],
ptOnTwoEdge, twoAxis, two.HalfSize[twoAxisIndex],
bestSingleAxis > 2
);
Contact contact = new Contact();
contact.Penetration = pen;
contact.ContactNormal = axis;
contact.ContactPoint = vertex;
contact.SetData(one.Body, two.Body,
data.Friction, data.Restitution);
data.Contacts.Add(contact);
data.AddContacts(1);
return 1;
}
return 0;
}