private static float transformToAxis(Box box, Vector3 axis)
{
return box.HalfSizes.X * TrickyMath.Abs(Vector3.Dot(axis, box.getAxis(0)))
+ box.HalfSizes.Y * TrickyMath.Abs(Vector3.Dot(axis, box.getAxis(1)))
+ box.HalfSizes.Z * TrickyMath.Abs(Vector3.Dot(axis, box.getAxis(2)));
}
//TODO - write unit tests!!!!!!
public static int boxAndPoint(Box box, Vector3 pointWorld, CollisionData data)
{
Vector3 pointLocal = Vector3.Transform(pointWorld, Matrix.Invert(box.Body.World * box.OffsetMatrix));
Vector3 halfSizes = box.HalfSizes;
Vector3 absPoint = new Vector3(
TrickyMath.Abs(pointLocal.X),
TrickyMath.Abs(pointLocal.Y),
TrickyMath.Abs(pointLocal.Z)
);
Vector3 check = halfSizes - absPoint;
if (check.X < 0 || check.Y < 0 || check.Z < 0)
{
return 0;
}
Vector3 depth = new Vector3(
TrickyMath.Abs(absPoint.X - halfSizes.X),
TrickyMath.Abs(absPoint.Y - halfSizes.Y),
TrickyMath.Abs(absPoint.Z - halfSizes.Z)
);
depth = new Vector3(
depth.X != 0f ? depth.X : float.MaxValue,
depth.Y != 0f ? depth.Y : float.MaxValue,
depth.Z != 0f ? depth.Z : float.MaxValue
);
if (depth.X <= depth.Y && depth.X <= depth.Z)
{
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
Vector3 boxAxis = box.getAxis(0);
contact.Normal = boxAxis * (pointLocal.X > 0 ? -1f : 1f);
contact.Point = pointWorld;
contact.Penetration = TrickyMath.Abs(absPoint.X - halfSizes.X);
contact.Bodies[0] = box.Body;
contact.Bodies[1] = null;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
else if (depth.Y <= depth.X && depth.Y <= depth.Z)
{
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
Vector3 boxAxis = box.getAxis(1);
contact.Normal = boxAxis * (pointLocal.Y > 0 ? -1f : 1f);
contact.Point = pointWorld;
contact.Penetration = TrickyMath.Abs(absPoint.Y - halfSizes.Y);
contact.Bodies[0] = box.Body;
contact.Bodies[1] = null;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
else if (depth.Z <= depth.X && depth.Z <= depth.Y)
{
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
Vector3 boxAxis = box.getAxis(2);
contact.Normal = boxAxis * (pointLocal.Z > 0 ? -1f : 1f);
contact.Point = pointWorld;
contact.Penetration = TrickyMath.Abs(absPoint.Z - halfSizes.Z);
contact.Bodies[0] = box.Body;
contact.Bodies[1] = null;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
else
{
return 0;
}
}
private static void fillPointFaceBoxBox(Box one, Box two, Vector3 toCenter, CollisionData data, int best, float penetration)
{
Contact contact = new Contact();
Vector3 normal = one.getAxis(best);
if (Vector3.Dot(normal, toCenter) > 0)
{
normal *= -1f;
}
Vector3 vertex = two.HalfSizes;
float oneDot = Vector3.Dot(two.getAxis(0), normal);
if (oneDot < 0)
{
vertex *= new Vector3(-1f, 0f, 0f);
}
float twoDot = Vector3.Dot(two.getAxis(1), normal);
if (twoDot < 0)
{
vertex *= new Vector3(0f, -1f, 0f);
}
float threeDot = Vector3.Dot(two.getAxis(2), normal);
if (threeDot < 0)
{
vertex *= new Vector3(0f, 0f, -1f);
}
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Point_Face;
contact.Normal = normal;
contact.Penetration = penetration;
contact.Point = Vector3.Transform(vertex, two.Transform);
contact.Bodies[0] = one.Body;
contact.Bodies[1] = two.Body;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
}
public static int boxAndBox(Box one, Box two, CollisionData data)
{
Vector3 toCenter = two.Body.Position - one.Body.Position;
float pen = float.MaxValue;
int best = int.MaxValue;
if (!tryAxis(one, two, one.getAxis(0), toCenter, 0, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(1), toCenter, 1, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(2), toCenter, 2, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(0), toCenter, 3, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(1), toCenter, 4, ref pen, ref best)) return 0;
if (!tryAxis(one, two, one.getAxis(2), toCenter, 5, ref pen, ref best)) return 0;
int bestSingleAxis = best;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(0)), toCenter, 6, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(1)), toCenter, 7, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(0), two.getAxis(2)), toCenter, 8, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(0)), toCenter, 9, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(1)), toCenter, 10, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(1), two.getAxis(2)), toCenter, 11, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(0)), toCenter, 12, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(1)), toCenter, 13, ref pen, ref best)) return 0;
if (!tryAxis(one, two, TrickyMath.VectorProduct(one.getAxis(2), two.getAxis(2)), toCenter, 14, ref pen, ref best)) return 0;
if (best == int.MaxValue)
{
throw new Exception("Found no good axis");
}
if (best < 3)
{
fillPointFaceBoxBox(one, two, toCenter, data, best, pen);
return 1;
}
else if (best < 6)
{
// We've got a vertex of box one on a face of box two.
// We use the same algorithm as above, but swap around
// one and two (and therefore also the vector between their
// centres).
fillPointFaceBoxBox(two, one, toCenter * -1.0f, data, best - 3, pen);
return 1;
}
else
{
best -= 6; //move this back to talk about a cardinal axis
int oneAxisIndex = best / 3;
int twoAxisIndex = best % 3;
Vector3 oneAxis = one.getAxis(oneAxisIndex);
Vector3 twoAxis = two.getAxis(twoAxisIndex);
Vector3 axis = TrickyMath.VectorProduct(oneAxis, twoAxis);
axis.Normalize();
if (Vector3.Dot(axis, toCenter) > 0)
{
axis *= -1f;
}
float[] ptOnOneEdge = TrickyMath.Vector3ToFloatArray(one.HalfSizes);
float[] ptOnTwoEdge = TrickyMath.Vector3ToFloatArray(two.HalfSizes);
for (int i = 0; i < 3; i++)
{
if (i == oneAxisIndex)
{
ptOnOneEdge[i] = 0f;
}
else if (Vector3.Dot(one.getAxis(i), axis) > 0)
{
ptOnOneEdge[i] *= -1f;
}
if (i == twoAxisIndex)
{
ptOnTwoEdge[i] = 0;
}
else if (Vector3.Dot(two.getAxis(i), axis) < 0)
{
ptOnTwoEdge[i] *= -1f;
}
}
Vector3 vertexOnOneEdge = Vector3.Transform(
TrickyMath.FloatArrayToVector3(ptOnOneEdge),
one.Transform
);
Vector3 vertexOnTwoEdge = Vector3.Transform(
TrickyMath.FloatArrayToVector3(ptOnTwoEdge),
two.Transform
);
float[] oneHalfSizes = TrickyMath.Vector3ToFloatArray(one.HalfSizes);
float[] twoHalfSizes = TrickyMath.Vector3ToFloatArray(two.HalfSizes);
Vector3 vertex = contactPoint(
vertexOnOneEdge, oneAxis, oneHalfSizes[oneAxisIndex],
vertexOnTwoEdge, twoAxis, twoHalfSizes[twoAxisIndex],
bestSingleAxis > 2
);
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnBox_Edge_Edge;
contact.Penetration = pen;
contact.Normal = axis;
contact.Point = vertex;
contact.Bodies[0] = one.Body;
contact.Bodies[1] = two.Body;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
return 0;
}
