public void AddCollisionPoint(CollisionPoint point)
{
var lst = CollisionPoints.ToList();
lst.Add(point);
CollisionPoints = lst.ToArray();
}
/// <summary>
/// Detects the collision.
/// </summary>
/// <returns>The collision.</returns>
/// <param name="objectA">Object a.</param>
/// <param name="objectB">Object b.</param>
public GJKOutput Execute(
VertexProperties[] vertexObjA,
VertexProperties[] vertexObjB)
{
var collisionPoint = new CollisionPoint();
var collisionNormal = new Vector3d();
var supportTriangles = new List <SupportTriangle>();
var centroid = new Vector3d();
bool isIntersection = false;
//TODO refactoring
double collisionDistance = ExecuteGJKAlgorithm(
vertexObjA,
vertexObjB,
ref collisionNormal,
ref collisionPoint,
ref supportTriangles,
ref centroid,
ref isIntersection);
return(new GJKOutput(
collisionDistance,
collisionPoint,
collisionNormal,
centroid,
isIntersection,
supportTriangles));
}
public List <CollisionPoint> GetManifoldPoints(
Vector3d[] vertexObjA,
Vector3d[] vertexObjB,
CollisionPoint collisionPoint)
{
if (ManifoldPointNumber > 1)
{
List <Vector3d> collisionA = GetNearestPoint(
vertexObjA,
collisionPoint.CollisionPointA.Vertex,
collisionPoint.CollisionNormal);
List <Vector3d> collisionB = GetNearestPoint(
vertexObjB,
collisionPoint.CollisionPointB.Vertex,
collisionPoint.CollisionNormal);
List <CollisionPoint> collisionPointsList = FindCollisionPoints(
collisionA.ToArray(),
collisionB.ToArray(),
collisionPoint);
collisionA.Clear();
collisionB.Clear();
SetCollisionNormal(collisionPointsList, collisionPoint.CollisionNormal);
return(collisionPointsList);
}
return(null);
}
public CollisionPointBaseStructure(
CollisionPoint collisionPoint,
CollisionPoint[] collisionPoints)
{
CollisionPoint = collisionPoint;
CollisionPoints = collisionPoints;
}
private CollisionPoint TestEdgesIntersection(
Vector3d p1,
Vector3d p2,
Vector3d p3,
Vector3d p4,
CollisionPoint point)
{
var a = new Vector3d();
var b = new Vector3d();
double mua = 0.0;
double mub = 0.0;
if (GeometryUtils.TestEdgesIntersect(
p1,
p2,
p3,
p4,
ref a,
ref b,
ref mua,
ref mub))
{
if (!(mua < 0.0 || mua > 1.0 || mub < 0.0 || mub > 1.0))
{
return(new CollisionPoint(
new VertexProperties(a),
new VertexProperties(b),
point.CollisionNormal,
0.0,
false));
}
}
return(null);
}
/// <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 GJKOutput(
double collisionDistance,
CollisionPoint collisionPoint,
Vector3d collisionNormal,
Vector3d centroid,
bool intersection,
List <SupportTriangle> supportTriangles)
{
CollisionDistance = collisionDistance;
CollisionPoint = collisionPoint;
CollisionNormal = collisionNormal;
Centroid = centroid;
Intersection = intersection;
SupportTriangles = 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(
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);
}
private List <CollisionPoint> ExtractFourCollisionPoints(
List <CollisionPoint> cpList,
Vector3d normal)
{
if (cpList.Count > 4)
{
var result = new List <CollisionPoint>();
//Point 1
Vector3d A = cpList[0].CollisionPointA.Vertex;
result.Add(cpList[0]);
cpList.RemoveAt(0);
//Point 2
double maxDist = double.MinValue;
CollisionPoint farthestPoint = null;
int index = -1;
for (int i = 0; i < cpList.Count; i++)
{
double dist = (cpList[i].CollisionPointA.Vertex - A).Length();
if (dist > maxDist)
{
maxDist = dist;
farthestPoint = cpList[i];
index = i;
}
}
Vector3d B = farthestPoint.CollisionPointA.Vertex;
result.Add(farthestPoint);
cpList.RemoveAt(index);
//Point 3
double maxArea = double.MinValue;
CollisionPoint third = null;
index = -1;
for (int i = 0; i < cpList.Count; i++)
{
double area = CalculateArea(A, B, cpList[i].CollisionPointA.Vertex, normal);
if (area > maxArea)
{
maxArea = area;
third = cpList[i];
index = i;
}
}
Vector3d C = third.CollisionPointA.Vertex;
result.Add(third);
cpList.RemoveAt(index);
//Point 4
CollisionPoint fourth = null;
maxArea = double.MinValue;
for (int i = 0; i < cpList.Count; i++)
{
// A-B-D
double area = CalculateArea(A, B, cpList[i].CollisionPointA.Vertex, normal);
if (area > maxArea)
{
maxArea = area;
fourth = cpList[i];
}
// A-C-D
area = CalculateArea(A, C, cpList[i].CollisionPointA.Vertex, normal);
if (area > maxArea)
{
maxArea = area;
fourth = cpList[i];
}
// B-C-D
area = CalculateArea(B, C, cpList[i].CollisionPointA.Vertex, normal);
if (area > maxArea)
{
maxArea = area;
fourth = cpList[i];
}
}
result.Add(fourth);
return(result);
}
return(cpList);
}
/// <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(
Vector3d[] ca,
Vector3d[] cb,
CollisionPoint initPoint,
Vector3d na)
{
var result = new List <CollisionPoint>();
if (cb.Length > 2)
{
for (int i = 0; i < ca.Length; i++)
{
Vector3d project = ca[i] -
(na * (Vector3d.Dot(na, ca[i]) +
Vector3d.Dot(na * -1.0, initPoint.CollisionPointB.Vertex)));
double angle = GeometryUtils.TestPointInsidePolygon(
cb,
project,
na,
initPoint.CollisionPointB.Vertex);
//Inserito il minore per gestire problemi di approssimazione
if (angle + ManifoldStabilizeValue >= ConstValues.PI2)
{
var cp = new CollisionPoint(
new VertexProperties(ca[i]),
new VertexProperties(project),
na,
0.0,
false);
result.Add(cp);
}
}
}
if (ca.Length > 2)
{
for (int i = 0; i < cb.Length; i++)
{
Vector3d project = cb[i] -
(na * (Vector3d.Dot(na, cb[i]) +
Vector3d.Dot(na * -1.0, initPoint.CollisionPointA.Vertex)));
double angle = GeometryUtils.TestPointInsidePolygon(
ca,
project,
na,
initPoint.CollisionPointA.Vertex);
if (angle + ManifoldStabilizeValue >= ConstValues.PI2)
{
var cp = new CollisionPoint(
new VertexProperties(project),
new VertexProperties(cb[i]),
na,
0.0,
false);
result.Add(cp);
}
}
}
return(result);
}
private List <CollisionPoint> FindCollisionPoints(
Vector3d[] ca,
Vector3d[] cb,
CollisionPoint cp)
{
var result = new List <CollisionPoint> ();
if (ca.Length == 2 && cb.Length == 2)
{
CollisionPoint collisionP = TestEdgesIntersection(
ca [0],
ca [1],
cb [0],
cb [1],
cp);
if (collisionP != null)
{
result.Add(collisionP);
}
}
else if (ca.Length > 2 && cb.Length == 2)
{
ca = GeometryUtils.TurnVectorClockWise(
ca,
cp.CollisionNormal);
result.AddRange(TestPointIsOnPlane(
ca,
cb,
cp,
cp.CollisionNormal));
if (result.Count < ca.Length)
{
for (int i = 0; i < ca.Length; i++)
{
CollisionPoint collisionP = TestEdgesIntersection(
ca [i],
ca [(i + 1) % ca.Length],
cb [0],
cb [1],
cp);
if (collisionP != null)
{
result.Add(collisionP);
}
}
}
}
else if (ca.Length == 2 && cb.Length > 2)
{
cb = GeometryUtils.TurnVectorClockWise(
cb,
cp.CollisionNormal);
result.AddRange(TestPointIsOnPlane(
ca,
cb,
cp,
cp.CollisionNormal));
if (result.Count < cb.Length)
{
for (int i = 0; i < cb.Length; i++)
{
CollisionPoint collisionP = TestEdgesIntersection(
ca [0],
ca [1],
cb [i],
cb [(i + 1) % cb.Length],
cp);
if (collisionP != null)
{
result.Add(collisionP);
}
}
}
}
else if (ca.Length > 2 && cb.Length > 2)
{
ca = GeometryUtils.TurnVectorClockWise(
ca,
cp.CollisionNormal);
cb = GeometryUtils.TurnVectorClockWise(
cb,
cp.CollisionNormal);
result.AddRange(TestPointIsOnPlane(
ca,
cb,
cp,
cp.CollisionNormal));
for (int i = 0; i < ca.Length; i++)
{
for (int j = 0; j < cb.Length; j++)
{
CollisionPoint collisionP = TestEdgesIntersection(
ca [i],
ca [(i + 1) % ca.Length],
cb [j],
cb [(j + 1) % cb.Length],
cp);
if (collisionP != null)
{
result.Add(collisionP);
}
}
}
}
else
{
result.Add(cp);
}
if (ManifoldPointNumber == 4 && result.Count > 4)
{
result = ExtractFourCollisionPoints(result, cp.CollisionNormal);
}
else
{
result = ExtractCollisionPoints(result, cp.CollisionNormal);
}
return(result);
}
