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; } } } } }