public List<CollisionPoint> GetManifoldPoints(
SimulationObject objectA,
SimulationObject objectB,
CollisionPoint collisionPoint)
{
List<Vector3> collisionA = getNearestPoint (
objectA,
collisionPoint.CollisionPointA,
collisionPoint.CollisionNormal);
List<Vector3> collisionB = getNearestPoint (
objectB,
collisionPoint.CollisionPointB,
collisionPoint.CollisionNormal);
List<CollisionPoint> collisionPointsList = findCollisionPoints (
collisionA.ToArray (),
collisionB.ToArray (),
collisionPoint.CollisionNormal,
collisionPoint);
collisionA.Clear ();
collisionB.Clear ();
return collisionPointsList;
}
public GJKOutput (
double collisionDistance,
CollisionPoint collisionPoint,
Vector3 collisionNormal,
Vector3 centroid,
bool intersection,
List<SupportTriangle> supportTriangles)
{
CollisionDistance = collisionDistance;
CollisionPoint = collisionPoint;
CollisionNormal = collisionNormal;
Centroid = centroid;
Intersection = intersection;
SupportTriangles = supportTriangles;
}
/// <summary>
/// Detects the collision.
/// </summary>
/// <returns>The collision.</returns>
/// <param name="objectA">Object a.</param>
/// <param name="objectB">Object b.</param>
public GJKOutput Execute(
SimulationObject objectA,
SimulationObject objectB,
int geometryIndexA,
int geometryIndexB)
{
var collisionPoint = new CollisionPoint();
var collisionNormal = new Vector3();
var supportTriangles = new List<SupportTriangle>();
var centroid = new Vector3();
bool isIntersection = false;
double collisionDistance = ExecuteGJKAlgorithm (
objectA,
objectB,
geometryIndexA,
geometryIndexB,
ref collisionNormal,
ref collisionPoint,
ref supportTriangles,
ref centroid,
ref isIntersection);
return new GJKOutput (
collisionDistance,
collisionPoint,
collisionNormal,
centroid,
isIntersection,
supportTriangles);
}
/// <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>
/// 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(
SimulationObject objectA,
SimulationObject objectB,
int geometryIndexA,
int geometryIndexB,
List<SupportTriangle> startTriangles,
Vector3 centroid)
{
EngineCollisionPoint epaCollisionPoint = ExecuteEngine (
objectA,
objectB,
geometryIndexA,
geometryIndexB,
startTriangles,
centroid);
var collisionPoint = new CollisionPoint (
epaCollisionPoint.a,
epaCollisionPoint.b,
epaCollisionPoint.normal);
return new EPAOutput (
Vector3.Length (epaCollisionPoint.dist),
collisionPoint);
}
private List<CollisionPoint> findCollisionPoints(
Vector3[] ca,
Vector3[] cb,
Vector3 normal,
CollisionPoint cp)
{
var result = new List<CollisionPoint> ();
if (ca.Length == 2 && cb.Length == 2)
{
CollisionPoint? collisionP = TestLineIntersection (
ca [0],
ca [1],
cb [0],
cb [1],
cp.CollisionNormal);
if (collisionP != null)
result.Add (collisionP.Value);
}
else if (ca.Length > 2 && cb.Length == 2)
{
ca = GeometryUtilities.TurnVectorClockWise (
ca,
normal);
result.AddRange(TestPointIsOnPlane (
ca,
cb,
cp,
normal));
if (result.Count < ca.Length) {
for (int i = 0; i < ca.Length; i++) {
CollisionPoint? collisionP = TestLineIntersection (
ca [i],
ca [(i + 1) % ca.Length],
cb [0],
cb [1],
cp.CollisionNormal);
if (collisionP != null)
result.Add (collisionP.Value);
}
}
}
else if (ca.Length == 2 && cb.Length > 2)
{
cb = GeometryUtilities.TurnVectorClockWise (
cb,
normal);
result.AddRange(TestPointIsOnPlane (
ca,
cb,
cp,
normal));
if (result.Count < cb.Length) {
for (int i = 0; i < cb.Length; i++) {
CollisionPoint? collisionP = TestLineIntersection (
ca [0],
ca [1],
cb [i],
cb [(i + 1) % cb.Length],
cp.CollisionNormal);
if (collisionP != null)
result.Add (collisionP.Value);
}
}
}
else if (ca.Length > 2 && cb.Length > 2)
{
ca = GeometryUtilities.TurnVectorClockWise (
ca,
normal);
cb = GeometryUtilities.TurnVectorClockWise (
cb,
normal);
result.AddRange(TestPointIsOnPlane (
ca,
cb,
cp,
normal));
for (int i = 0; i < ca.Length; i++) {
for (int j = 0; j < cb.Length; j++) {
CollisionPoint? collisionP = TestLineIntersection (
ca [i],
ca [(i + 1) % ca.Length],
cb [j],
cb [(j + 1) % cb.Length],
cp.CollisionNormal);
if (collisionP != null)
result.Add (collisionP.Value);
}
}
}
result = PruneCollisionPoints (result);
if (result.Count < 1)
result.Add(cp);
return result;
}
/// <summary>
/// Tests the point is on plane.
/// </summary>
/// <returns>The point is on plane.</returns>
/// <param name="ca">Ca.</param>
/// <param name="cb">Cb.</param>
/// <param name="initPoint">Init point.</param>
/// <param name="na">Na.</param>
private List<CollisionPoint> TestPointIsOnPlane(
Vector3[] ca,
Vector3[] cb,
CollisionPoint initPoint,
Vector3 na)
{
var result = new List<CollisionPoint>();
if (cb.Length > 2)
{
for (int i = 0; i < ca.Length; i++)
{
Vector3 project = ca[i] -
(na * (Vector3.Dot(na, ca[i]) +
Vector3.Dot(na * -1.0, initPoint.CollisionPointB)));
double angle = GeometryUtilities.TestPointInsidePolygon(
cb,
project,
na,
initPoint.CollisionPointB);
//Inserito il minore per gestire problemi di approssimazione
if (angle + ManifoldStabilizeValue >= 2.0 * Math.PI)
{
var cp = new CollisionPoint(
ca[i],
project,
na);
result.Add(cp);
}
}
}
if (ca.Length > 2)
{
for (int i = 0; i < cb.Length; i++)
{
Vector3 project = cb[i] -
(na * (Vector3.Dot(na, cb[i]) +
Vector3.Dot(na * -1.0, initPoint.CollisionPointA)));
double angle = GeometryUtilities.TestPointInsidePolygon(
ca,
project,
na,
initPoint.CollisionPointA);
if (angle + ManifoldStabilizeValue >= 2.0 * Math.PI)
{
var cp = new CollisionPoint(
project,
cb[i],
na);
result.Add(cp);
}
}
}
return result;
}
