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