/// <summary> /// Ritorna la distanza se distanza 0.0f allora vi รจ intersezione o compenetrazione tra gli oggetti, /// se distanza 0.0 allora i due oggetti non collidono /// </summary> /// <returns>The GJK algorithm.</returns> /// <param name="shape1">Shape1.</param> /// <param name="shape2">Shape2.</param> /// <param name="cp">Cp.</param> /// <param name="isIntersection">If set to <c>true</c> is itersection.</param> private double ExecuteGJKAlgorithm( VertexProperties[] vertexShape1, VertexProperties[] vertexShape2, ref Vector3d collisionNormal, ref CollisionPoint cp, ref List <SupportTriangle> triangles, ref Vector3d centroid, ref bool isIntersection) { double minDistance = double.MaxValue; int minTriangleIndex = -1; var result = new EngineCollisionPoint(); var oldDirection = new Vector3d(); var simplex = new Simplex(); //Primo punto del simplex simplex.Support.Add(GetFarthestPoint(vertexShape1, vertexShape2, vertexShape2.Length / 2)); //Secondo punto del simplex Vector3d direction = Vector3d.Normalize(simplex.Support[0].s * -1.0); if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { return(-1.0); } //Terzo punto del simplex direction = Vector3d.Normalize(GetDirectionOnSimplex2(simplex)); if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { return(-1.0); } //Quarto punto del simplex direction = Vector3d.Normalize(GeometryUtils.CalculateTriangleNormal( simplex.Support[0].s, simplex.Support[1].s, simplex.Support[2].s)); if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, -1.0 * direction)); } //Costruisco il poliedro centroid = Helper.SetStartTriangle( ref triangles, simplex.Support.ToArray()); //Verifico che l'origine sia contenuta nel poliedro if (Helper.IsInConvexPoly(origin, triangles)) { isIntersection = true; return(-1.0); } Vector3d triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex); result.SetDist(triangleDistance); result.SetNormal(Vector3d.Normalize(triangleDistance)); Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], vertexShape1, vertexShape2, ref result); minDistance = triangleDistance.Length(); for (int i = 0; i < MaxIterations; i++) { direction = -1.0 * triangleDistance.Normalize(); if (Vector3d.Length(direction) < constTolerance) { direction = origin - centroid; } if (direction == oldDirection) { break; } oldDirection = direction; if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { for (int j = 0; j < triangles.Count; j++) { direction = triangles[j].Normal; if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, direction))) { if (simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(vertexShape1, vertexShape2, -1.0 * direction))) { break; } continue; } break; } } triangles = Helper.AddPointToConvexPolygon(triangles, simplex.Support[simplex.Support.Count - 1], centroid); //Verifico che l'origine sia contenuta nel poliedro if (Helper.IsInConvexPoly(origin, triangles)) { isIntersection = true; return(-1.0); } triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex); double mod = triangleDistance.Length(); if (mod < minDistance) { result.SetDist(triangleDistance); result.SetNormal(triangles[minTriangleIndex].Normal); Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], vertexShape1, vertexShape2, ref result); minDistance = mod; } } collisionNormal = -1.0 * result.Normal; cp = new CollisionPoint( result.A, result.B, collisionNormal, 0.0, false); return(minDistance); }
/// <summary> /// Executes the EPA engine. /// </summary> /// <param name="shape1">Shape1.</param> /// <param name="shape2">Shape2.</param> /// <param name="startPoint">Start point.</param> private EngineCollisionPoint ExecuteEngine( VertexProperties[] vertexShapeA, VertexProperties[] vertexShapeB, List <SupportTriangle> triangles, Vector3d centroid) { var epaCollisionPoint = new EngineCollisionPoint(); double s = 0.0; double t = 0.0; var direction = new Vector3d(); var oldDirection = new Vector3d(); var vDistance = new Vector3d(); SupportTriangle epaBuffer; if (triangles.Count > 0) { for (int k = 0; k < MaxIterations; k++) { double minDistance = double.MaxValue; for (int i = 0; i < triangles.Count; i++) { epaBuffer = triangles [i]; if (!GeometryUtils.TestCollinearity( epaBuffer.A.s, epaBuffer.B.s, epaBuffer.C.s)) { vDistance = GeometryUtils.GetPointTriangleIntersection( epaBuffer.A.s, epaBuffer.B.s, epaBuffer.C.s, origin, ref s, ref t).Value; epaBuffer.SetValueS(s); epaBuffer.SetValueT(t); } else { continue; } triangles [i] = epaBuffer; double distance = Vector3d.Length(vDistance); if (distance < minDistance) { minDistance = distance; direction = vDistance; epaCollisionPoint.SetDist(vDistance); epaCollisionPoint.SetNormal(triangles[i].Normal); Helper.GetVertexFromMinkowsky( triangles[i], vertexShapeA, vertexShapeB, ref epaCollisionPoint); } } //L'origine risiede su uno dei bordi del triangolo if (Vector3d.Length(direction) < constTolerance) { direction = origin - centroid; } if (direction == oldDirection) { break; } Support vt = Helper.GetMinkowskiFarthestPoint( vertexShapeA, vertexShapeB, direction.Normalize()); triangles = Helper.AddPointToConvexPolygon( triangles, vt, centroid); oldDirection = direction; } } triangles.Clear(); return(epaCollisionPoint); }