public static Boolean isCollision(CollisionPoly poly1, CollisionPoly poly2)
{
Vector2 direction = new Vector2();
List <Vector2> simplex = new List <Vector2>();
while (true)
{
switch (simplex.Count)
{
case 0:
//direction in the direction of separation
direction = Vector2.Subtract(poly1.center(), poly2.center());
break;
case 1:
//direction opposite direction of separation
direction = Vector2.Multiply(direction, -1);
break;
case 2:
//direction orthogonal to the line segment generated by first two points and also
direction = tripleCrossProd(Vector2.Subtract(simplex[1], simplex[0]), Vector2.Multiply(simplex[0], -1), Vector2.Subtract(simplex[1], simplex[0]));
break;
case 3:
Vector2 side1 = Vector2.Subtract(simplex[1], simplex[2]);
Vector2 side2 = Vector2.Subtract(simplex[0], simplex[2]);
//vector orthogonal to the 2nd line segment generated (vertex 2 -> vertex 1)
Vector2 orthogonal1 = tripleCrossProd(side2, side1, side1);
//vector orthogonal to third line segment generated (vertex 3 -> vertex 1)
Vector2 orthogonal2 = tripleCrossProd(side1, side2, side2);
if (Vector2.Dot(orthogonal1, Vector2.Multiply(simplex[2], -1)) > 0)
{
direction = orthogonal1;
simplex.RemoveAt(0);
}
else if (Vector2.Dot(orthogonal2, Vector2.Multiply(simplex[2], -1)) > 0)
{
direction = orthogonal2;
simplex.RemoveAt(1);
}
else
{
return(true);
}
break;
}
Vector2 nextVert = Vector2.Subtract(poly1.supportFunction(direction), poly2.supportFunction(Vector2.Multiply(direction, -1)));
if (Vector2.Dot(direction, nextVert) > 0)
{
simplex.Add(nextVert);
}
else
{
return(false);
}
}
}
public static Vector2 calculateIntersection(CollisionPoly poly1, CollisionPoly poly2, Vector2?velocity)
{
//Constructs basic simplex
Vector2 direction = Vector2.Subtract(poly1.center(), poly2.center());
List <Vector2> simplex = new List <Vector2>();
simplex.Add(Vector2.Subtract(poly1.supportFunction(direction), poly2.supportFunction(Vector2.Multiply(direction, -1))));
direction = Vector2.Multiply(direction, -1);
simplex.Add(Vector2.Subtract(poly1.supportFunction(direction), poly2.supportFunction(Vector2.Multiply(direction, -1))));
direction = tripleCrossProd(Vector2.Subtract(simplex[1], simplex[0]), Vector2.Multiply(simplex[0], -1), Vector2.Subtract(simplex[1], simplex[0]));
simplex.Add(Vector2.Subtract(poly1.supportFunction(direction), poly2.supportFunction(Vector2.Multiply(direction, -1))));
//Calculates distance inside/outside object
if (velocity.HasValue)
{
while (true)
{
Edge closestEdge = closestEdgeAlongVector(simplex, (Vector2)velocity);
Vector2 support = Vector2.Subtract(poly1.supportFunction(closestEdge.normal), poly2.supportFunction(Vector2.Negate(closestEdge.normal)));
if (Math.Abs(Vector2.Dot(support, closestEdge.normal) - closestEdge.dist) <= 0.0001)
{
return(closestEdge.toOrigin);
}
else
{
simplex.Insert(closestEdge.index, support);
}
}
}
else
{
while (true)
{
Edge closestEdge = closestEdgeToOrigin(simplex);
Vector2 support = Vector2.Subtract(poly1.supportFunction(closestEdge.normal), poly2.supportFunction(Vector2.Negate(closestEdge.normal)));
if (Math.Abs(Vector2.Dot(support, closestEdge.normal) - closestEdge.dist) <= 0.00001)
{
return(Vector2.Multiply(closestEdge.normal, Vector2.Dot(support, closestEdge.normal)));
}
else
{
simplex.Insert(closestEdge.index, support);
}
}
}
}