public void Update3(out Vec3 searchDirection) { var ba = B - A; var ca = C - A; Vec3 norm; Vec3.Cross(ref ba, ref ca, out norm); var origin = -A; Scalar dotRes; Vec3 crossRes; Vec3.Cross(ref ba, ref norm, out crossRes); Vec3.Dot(ref crossRes, ref origin, out dotRes); if (dotRes > 0) // origin outside the triangle, BA closest. { Dimension = 2; C = A; Vec3.Cross(ref ba, ref origin, out crossRes); Vec3.Cross(ref crossRes, ref ba, out searchDirection); return; } Vec3.Cross(ref norm, ref ca, out crossRes); Vec3.Dot(ref crossRes, ref origin, out dotRes); if (dotRes > 0) // origin outside the triangle, CA closest { Dimension = 2; B = A; Vec3.Cross(ref ca, ref origin, out crossRes); Vec3.Cross(ref crossRes, ref ca, out searchDirection); return; } Dimension = 3; Vec3.Dot(ref norm, ref origin, out dotRes); if (dotRes < 0) // origin below the triangle. { // reverse winding order, BCD = BAC D = C; C = A; searchDirection = -norm; return; } D = C; C = B; B = A; searchDirection = norm; }
public bool Update4(out Vec3 searchDirection) { var abc = Vec3.Cross(B - A, C - A); var origin = -A; Scalar dotRes; Vec3.Dot(ref abc, ref origin, out dotRes); if (dotRes > 0) // outside, ABC closest { Dimension = 3; // BCD = BCA D = A; searchDirection = abc; return(false); } var acd = Vec3.Cross(C - A, D - A); Vec3.Dot(ref acd, ref origin, out dotRes); if (dotRes > 0) // outside, ACD closest { Dimension = 3; // BCD = ACD B = A; searchDirection = acd; return(false); } var adb = Vec3.Cross(D - A, B - A); Vec3.Dot(ref adb, ref origin, out dotRes); if (dotRes > 0) // outside, ADB closest { Dimension = 3; // BCD = BAD C = A; searchDirection = adb; return(false); } // skip checking BCD dot origin, we know it's on the inside w.r.t it searchDirection = Vec3.Zero; return(true); }
public static bool Intersects <T1, T2>(ref T1 shape1, ref T2 shape2, int iterations = 64) where T1 : struct, ConvexShape where T2 : struct, ConvexShape { Vec3 searchDirection; Scalar dotResult; var simplex = new Simplex(); // Initialize the simplex to a line, with searchDirection for next point. { searchDirection = shape2.Center - shape1.Center; if (Vec3.IsZero(searchDirection)) { searchDirection = Vec3.UnitX; } // Initial point simplex.C = shape2.FarthestInDirection(searchDirection) - shape1.FarthestInDirection(-searchDirection); searchDirection = -simplex.C; simplex.Dimension++; // Second point, crossing origin simplex.B = shape2.FarthestInDirection(searchDirection) - shape1.FarthestInDirection(-searchDirection); simplex.Dimension++; // Check if we made progress Vec3.Dot(ref simplex.B, ref searchDirection, out dotResult); if (dotResult < 0) { return(false); } var bc = simplex.C - simplex.B; Vec3.Cross(ref bc, ref simplex.B, out searchDirection); Vec3.Cross(ref bc, ref searchDirection, out searchDirection); if (Vec3.IsZero(searchDirection)) { // create perp vector to BC if (Math.Abs(bc.Y + bc.Z) > 1e-5 || Math.Abs(bc.X) > 1e-5) { searchDirection = new Vector3D(-(bc.Y + bc.Z), bc.X, bc.X); } else { searchDirection = new Vector3D(bc.Z, bc.Z, -(bc.X + bc.Y)); } } } for (var i = 0; i < iterations; i++) { simplex.A = shape2.FarthestInDirection(searchDirection) - shape1.FarthestInDirection(-searchDirection); simplex.Dimension++; // Check if we made progress Vec3.Dot(ref simplex.A, ref searchDirection, out dotResult); if (dotResult < 0) { return(false); } if (simplex.Dimension == 3) { simplex.Update3(out searchDirection); } else { // assert Dimension == 4 if (simplex.Update4(out searchDirection)) { return(true); } } } return(false); }