/// <summary>
/// Checks for colision between <paramref name="A"/> and <paramref name="B"/>
/// </summary>
/// <param name="A">Convex object</param>
/// <param name="B">Convex object</param>
/// <param name="W">Initial simplex</param>
public GJKOutput ProximityGJK(Polyline A, Polyline B, Simplex W)
{
Vector v = new Vector(1, 0);
GJKOutput result;
double delta = 0;
Vector w = new Vector(A.points.First().X, A.points.First().Y);
Simplex old = new Simplex();
int i = 0;
bool start = true;
while (Vector.Multiply(v, v) - delta > 0.1 || (v.X == 0 && v.Y == 0))
{
i++;
if (!start)
{
v = ClosestPoint(W);
}
Vector supportA = SupportHC(A, v);
Vector supportB = SupportHC(B, -v);
w = supportA - supportB;
ShapePoint sp = new ShapePoint();
sp.S1 = supportA;
sp.S2 = supportB;
sp.MinkowskiPoint = w;
W = BestSimplex(W, sp);
if (W.count == 3 || (w.X == 0 && w.Y == 0) || (i > 10000 && old == W))
{
Pair <Vector, double> epa = computeEPA(A, B, W);
result.Collision = true;
result.Distance = epa.Second;
result.closestFeatures = new Pair <Vector, Vector>();
result.touchingVector = new Vector();
result.touchingVectorPoints = new Pair <Vector, Vector>();
return(result);
}
if (Vector.Multiply(v, w) > 0 && !start)
{
delta = Math.Max(delta, ((Vector.Multiply(v, w) * Vector.Multiply(v, w)) / Vector.Multiply(v, v)));
}
start = false;
old = W;
}
result.Collision = false;
result.Distance = v.Length;
result.closestFeatures = getClosestFeatures(W);
result.touchingVector = w;
result.touchingVectorPoints = getTouchingVectorPoints(W, w);
return(result);
}
public Simplex()
{
A = new ShapePoint();
B = new ShapePoint();
C = new ShapePoint();
}
/// <summary>
/// Form new simplex and test in which external Voronoi region the origin lies.
/// </summary>
/// <param name="W">Simplex</param>
/// <param name="w">New point in CSO surface</param>
/// <returns>New smallest simplex <paramref name="W"/> containing <paramref name="w"/> and closest point to origin.</returns>
public Simplex BestSimplex(Simplex W, ShapePoint w)
{
Simplex result = new Simplex();
switch (W.count)
{
case 0: {
result.A = w;
result.count = 1;
break;
}
case 1: {
Simplex line = new Simplex();
line.A = w;
line.B = W.A;
line.count = 2;
Vector closest = ClosestToSegment(new Vector(0, 0), w.MinkowskiPoint, W.A.MinkowskiPoint);
if (closest == w.MinkowskiPoint)
{
result.A = w;
result.count = 1;
}
else
{
result.A = W.A;
result.B = w;
result.count = 2;
}
break;
}
case 2: {
Vector d = new Vector(0, 0) - w.MinkowskiPoint; // d.negate(); AO = O - A; O = origin (0, 0)
Vector e1 = W.A.MinkowskiPoint - w.MinkowskiPoint; // AB = B - A
Vector e2 = W.B.MinkowskiPoint - w.MinkowskiPoint;
Vector3 e1_3D = new Vector3((float)e1.X, (float)e1.Y, 0);
Vector3 e2_3D = new Vector3((float)e2.X, (float)e2.Y, 0);
Vector3 u1 = Vector3.Cross(e1_3D, Vector3.Cross(e1_3D, e2_3D));
Vector3 v1 = Vector3.Cross(Vector3.Cross(e1_3D, e2_3D), e2_3D);
if (Vector.Multiply(d, e1) < 0 && Vector.Multiply(d, e2) < 0)
{
result.A = w;
result.count = 1;
}
else if (Vector.Multiply(d, e1) > 0 && (d.X * u1.X + d.Y * u1.Y + 0 * u1.Z) > 0)
{
result.A = W.A;
result.B = w;
result.count = 2;
}
else if (Vector.Multiply(d, e2) > 0 && (d.X * v1.X + d.Y * v1.Y + 0 * v1.Z) > 0)
{
result.A = W.B;
result.B = w;
result.count = 2;
}
else if ((d.X * u1.X + d.Y * u1.Y + 0 * u1.Z < 0) && (d.X * v1.X + d.Y * v1.Y + 0 * v1.Z < 0))
{
result.A = W.A;
result.B = W.B;
result.C = w;
result.count = 3;
}
else
{
result = W;
}
break;
}
}
return(result);
}
