private void GetEpaVertexFromMinkowsky( SupportTriangle triangle, SimulationObject shape1, SimulationObject shape2, ref EngineCollisionPoint epaCollisionPoint) { Vector3 a1 = Helper.GetVertexPosition(shape1, triangle.a.a); Vector3 ba1 = Helper.GetVertexPosition(shape1, triangle.b.a) - a1; Vector3 ca1 = Helper.GetVertexPosition(shape1, triangle.c.a) - a1; Vector3 a2 = Helper.GetVertexPosition(shape2, triangle.a.b); Vector3 ba2 = Helper.GetVertexPosition(shape2, triangle.b.b) - a2; Vector3 ca2 = Helper.GetVertexPosition(shape2, triangle.c.b) - a2; epaCollisionPoint.SetA (a1 + (ba1 * triangle.s) + (ca1 * triangle.t)); epaCollisionPoint.SetB (a2 + (ba2 * triangle.s) + (ca2 * triangle.t)); }
/// <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( SimulationObject shape1, SimulationObject shape2, int geometryIndexA, int geometryIndexB, ref Vector3 collisionNormal, ref CollisionPoint cp, ref List<SupportTriangle> triangles, ref Vector3 centroid, ref bool isIntersection) { double minDistance = double.MaxValue; int minTriangleIndex = -1; var result = new EngineCollisionPoint(); var oldDirection = new Vector3(); var simplex = new Simplex(); //Primo punto del simplex simplex.Support.Add(GetFarthestPoint(shape1, shape2)); //Secondo punto del simplex Vector3 direction = Vector3.Normalize(simplex.Support[0].s * -1.0); if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction))) return -1.0; //Terzo punto del simplex direction = Vector3.Normalize(GetDirectionOnSimplex2(simplex)); if(!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction))) return -1.0; //Quarto punto del simplex direction = Vector3.Normalize(GeometryUtilities.CalculateNormal( simplex.Support[0].s, simplex.Support[1].s, simplex.Support[2].s)); if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction))) simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, -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; } Vector3 triangleDistance = GetMinDistance(ref triangles, origin, ref minTriangleIndex); result.SetDist(triangleDistance); result.SetNormal(Vector3.Normalize(triangleDistance)); Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], shape1, shape2, ref result); minDistance = triangleDistance.Length(); for (int i = 0; i < MaxIterations; i++) { direction = -1.0 * triangleDistance.Normalize(); if (Vector3.Length(direction) < constTolerance) { direction = origin - centroid; } if (direction == oldDirection) break; oldDirection = direction; if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction))) { for (int j = 0; j < triangles.Count; j++) { direction = triangles[j].normal; if (!simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, direction))) { if (simplex.AddSupport(Helper.GetMinkowskiFarthestPoint(shape1, shape2, geometryIndexA, geometryIndexB, -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(Vector3.Normalize(triangleDistance)); Helper.GetVertexFromMinkowsky(triangles[minTriangleIndex], shape1, shape2, ref result); minDistance = mod; } } collisionNormal = -1.0 * result.normal; cp = new CollisionPoint( result.a, result.b, collisionNormal); 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( SimulationObject shape1, SimulationObject shape2, int geometryIndexA, int geometryIndexB, List<SupportTriangle> triangles, Vector3 centroid) { var epaCollisionPoint = new EngineCollisionPoint(); double s = 0.0; double t = 0.0; var direction = new Vector3 (); var oldDirection = new Vector3 (); var vDistance = new Vector3 (); 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 (!GeometryUtilities.TestCollinearity ( epaBuffer.a.s, epaBuffer.b.s, epaBuffer.c.s)) { vDistance = GeometryUtilities.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 = Vector3.Length (vDistance); if (distance < minDistance) { minDistance = distance; direction = vDistance; epaCollisionPoint.SetDist (vDistance); epaCollisionPoint.SetNormal (Vector3.Normalize (vDistance)); GetEpaVertexFromMinkowsky( triangles[i], shape1, shape2, ref epaCollisionPoint); } } //L'origine risiede su uno dei bordi del triangolo if (Vector3.Length(direction) < constTolerance) { direction = origin - centroid; } if (direction == oldDirection) break; Support vt = Helper.GetMinkowskiFarthestPoint( shape1, shape2, geometryIndexA, geometryIndexB, direction.Normalize()); triangles = Helper.AddPointToConvexPolygon ( triangles, vt, centroid); oldDirection = direction; } } triangles.Clear (); return epaCollisionPoint; }