/// <summary>
/// Execute Expanding Polytope Algorithm (EPA).
/// </summary>
/// <param name="objectA">Object a.</param>
/// <param name="objectB">Object b.</param>
/// <param name="startTriangles">Start triangles.</param>
public EPAOutput Execute(
VertexProperties[] vertexObjA,
VertexProperties[] vertexObjB,
List <SupportTriangle> startTriangles,
Vector3d centroid)
{
EngineCollisionPoint epaCollisionPoint = ExecuteEngine(
vertexObjA,
vertexObjB,
startTriangles,
centroid);
var distance = Vector3d.Length(epaCollisionPoint.Dist);
var collisionPoint = new CollisionPoint(
epaCollisionPoint.A,
epaCollisionPoint.B,
epaCollisionPoint.Normal,
distance,
true);
return(new EPAOutput(
distance,
collisionPoint));
}
public static void GetVertexFromMinkowsky(
SupportTriangle triangle,
VertexProperties[] vertexShape1,
VertexProperties[] vertexShape2,
ref EngineCollisionPoint collisionPoint)
{
Vector3d a1 = vertexShape1[triangle.A.a].Vertex;
Vector3d ba1 = vertexShape1[triangle.B.a].Vertex - a1;
Vector3d ca1 = vertexShape1[triangle.C.a].Vertex - a1;
Vector3d a2 = vertexShape2[triangle.A.b].Vertex;
Vector3d ba2 = vertexShape2[triangle.B.b].Vertex - a2;
Vector3d ca2 = vertexShape2[triangle.C.b].Vertex - a2;
collisionPoint.SetA(
new VertexProperties(a1 + (ba1 * triangle.S) + (ca1 * triangle.T),
new int?[] { vertexShape1[triangle.A.a].ID, vertexShape1[triangle.B.a].ID, vertexShape1[triangle.C.a].ID }));
collisionPoint.SetB(
new VertexProperties(a2 + (ba2 * triangle.S) + (ca2 * triangle.T),
new int?[] { vertexShape2[triangle.A.b].ID, vertexShape2[triangle.B.b].ID, vertexShape2[triangle.C.b].ID }));
}
/// <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);
}