private static void SupportMapTransformed(ISupportMappable2 support,
ref MatrixD orientation, ref Vector3D position, ref Vector3D direction, out Vector3D result)
{
// THIS IS *THE* HIGH FREQUENCY CODE OF THE COLLLISION PART OF THE ENGINE
result.X = ((direction.X * orientation.M11) + (direction.Y * orientation.M12)) + (direction.Z * orientation.M13);
result.Y = ((direction.X * orientation.M21) + (direction.Y * orientation.M22)) + (direction.Z * orientation.M23);
result.Z = ((direction.X * orientation.M31) + (direction.Y * orientation.M32)) + (direction.Z * orientation.M33);
support.SupportMapping(ref result, out result);
double x = ((result.X * orientation.M11) + (result.Y * orientation.M21)) + (result.Z * orientation.M31);
double y = ((result.X * orientation.M12) + (result.Y * orientation.M22)) + (result.Z * orientation.M32);
double z = ((result.X * orientation.M13) + (result.Y * orientation.M23)) + (result.Z * orientation.M33);
result.X = position.X + x;
result.Y = position.Y + y;
result.Z = position.Z + z;
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable2 support1, ISupportMappable2 support2, ref MatrixD orientation1,
ref MatrixD orientation2, ref Vector3D position1, ref Vector3D position2,
out Vector3D point, out Vector3D normal, out double penetration)
{
// Used variables
Vector3D temp1, temp2;
Vector3D v01, v02, v0;
Vector3D v11, v12, v1;
Vector3D v21, v22, v2;
Vector3D v31, v32, v3;
Vector3D v41, v42, v4;
Vector3D mn;
// Initialization of the output
point = normal = Vector3D.Zero;
penetration = 0.0f;
//Vector3 right = Vector3.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
Vector3D.Transform(ref v01, ref orientation1, out v01);
Vector3D.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
Vector3D.Transform(ref v02, ref orientation2, out v02);
Vector3D.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
Vector3D.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.LengthSquared() < MathHelper.EPSILON * MathHelper.EPSILON)
{
v0 = new Vector3D(0.00001f, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
Vector3D.Negate(ref v0, out normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
Vector3D.Subtract(ref v12, ref v11, out v1);
if (Vector3D.Dot(v1, normal) <= 0.0f)
{
return(false);
}
// v2 = support perpendicular to v1,v0
Vector3D.Cross(ref v1, ref v0, out normal);
if (normal.LengthSquared() < MathHelper.EPSILON * MathHelper.EPSILON)
{
Vector3D.Subtract(ref v1, ref v0, out normal);
normal.Normalize();
point = v11;
Vector3D.Add(ref point, ref v12, out point);
Vector3D.Multiply(ref point, 0.5f, out point);
Vector3D.Subtract(ref v12, ref v11, out temp1);
penetration = Vector3D.Dot(temp1, normal);
//point = v11;
//point2 = v12;
return(true);
}
Vector3D.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
Vector3D.Subtract(ref v22, ref v21, out v2);
if (Vector3D.Dot(v2, normal) <= 0.0f)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
Vector3D.Subtract(ref v1, ref v0, out temp1);
Vector3D.Subtract(ref v2, ref v0, out temp2);
Vector3D.Cross(ref temp1, ref temp2, out normal);
double dist = Vector3D.Dot(normal, v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > 0.0f)
{
Swap(ref v1, ref v2);
Swap(ref v11, ref v21);
Swap(ref v12, ref v22);
Vector3D.Negate(ref normal, out normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations)
{
return(false);
}
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
Vector3D.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
Vector3D.Subtract(ref v32, ref v31, out v3);
if (Vector3D.Dot(v3, normal) <= 0.0f)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
Vector3D.Cross(ref v1, ref v3, out temp1);
if (Vector3D.Dot(temp1, v0) < 0.0f)
{
v2 = v3;
v21 = v31;
v22 = v32;
Vector3D.Subtract(ref v1, ref v0, out temp1);
Vector3D.Subtract(ref v3, ref v0, out temp2);
Vector3D.Cross(ref temp1, ref temp2, out normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
Vector3D.Cross(ref v3, ref v2, out temp1);
if (Vector3D.Dot(temp1, v0) < 0.0f)
{
v1 = v3;
v11 = v31;
v12 = v32;
Vector3D.Subtract(ref v3, ref v0, out temp1);
Vector3D.Subtract(ref v2, ref v0, out temp2);
Vector3D.Cross(ref temp1, ref temp2, out normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
// Compute normal of the wedge face
Vector3D.Subtract(ref v2, ref v1, out temp1);
Vector3D.Subtract(ref v3, ref v1, out temp2);
Vector3D.Cross(ref temp1, ref temp2, out normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.LengthSquared() < MathHelper.EPSILON * MathHelper.EPSILON)
{
return(true);
}
normal.Normalize();
// Compute distance from origin to wedge face
double d = Vector3D.Dot(normal, v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
Vector3D.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
Vector3D.Subtract(ref v42, ref v41, out v4);
Vector3D.Subtract(ref v4, ref v3, out temp1);
double delta = Vector3D.Dot(temp1, normal);
penetration = Vector3D.Dot(v4, normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations)
{
if (hit)
{
Vector3D.Cross(ref v1, ref v2, out temp1);
double b0 = Vector3D.Dot(temp1, v3);
Vector3D.Cross(ref v3, ref v2, out temp1);
double b1 = Vector3D.Dot(temp1, v0);
Vector3D.Cross(ref v0, ref v1, out temp1);
double b2 = Vector3D.Dot(temp1, v3);
Vector3D.Cross(ref v2, ref v1, out temp1);
double b3 = Vector3D.Dot(temp1, v0);
double sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
Vector3D.Cross(ref v2, ref v3, out temp1);
b1 = Vector3D.Dot(temp1, normal);
Vector3D.Cross(ref v3, ref v1, out temp1);
b2 = Vector3D.Dot(temp1, normal);
Vector3D.Cross(ref v1, ref v2, out temp1);
b3 = Vector3D.Dot(temp1, normal);
sum = b1 + b2 + b3;
}
double inv = 1.0f / sum;
Vector3D.Multiply(ref v01, b0, out point);
Vector3D.Multiply(ref v11, b1, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v21, b2, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v31, b3, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v02, b0, out temp2);
Vector3D.Add(ref temp2, ref point, out point);
Vector3D.Multiply(ref v12, b1, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v22, b2, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v32, b3, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref point, inv * 0.5f, out point);
}
// Compute the barycentric coordinates of the origin
return(hit);
}
//// Compute the tetrahedron dividing face (v4,v0,v1)
//Vector3.Cross(ref v4, ref v1, out temp1);
//double d1 = Vector3.Dot(ref temp1, ref v0);
//// Compute the tetrahedron dividing face (v4,v0,v2)
//Vector3.Cross(ref v4, ref v2, out temp1);
//double d2 = Vector3.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
Vector3D.Cross(ref v4, ref v0, out temp1);
double dot = Vector3D.Dot(temp1, v1);
if (dot >= 0.0f)
{
dot = Vector3D.Dot(temp1, v2);
if (dot >= 0.0f)
{
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
dot = Vector3D.Dot(temp1, v3);
if (dot >= 0.0f)
{
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
