/// <summary>
/// Scales the line segment between (0,0) and the current point to a set length.
/// </summary>
/// <param name="vec">Self</param>
/// <param name="amount">The length to scale the vector to</param>
/// <returns>The normalized vector</returns>
public static Vector2f Normalized(this Vector2f vec, float amount = 1)
{
if (vec.IsZero())
{
return(vec);
}
double val = 1.0 / vec.Length();
var x = vec.X * val;
var y = vec.Y * val;
x *= amount;
y *= amount;
vec.X = (float)x;
vec.Y = (float)y;
return(vec);
}
private static bool OverlapsGjk(IShape shapeA, IShape shapeB, ref List <Vector2f> simplex)
{
var direction = new Vector2f();
int vertexCount = 0;
for (int i = 0; i < MaxGjkIterations; i++)
{
switch (vertexCount)
{
case 0:
direction = shapeB.GetPosition() - shapeA.GetPosition();
if (direction.IsZero())
{
direction.X = 1;
}
break;
case 1:
direction *= -1;
break;
case 2:
{
var xy = simplex[0] - simplex[1];
var xo = simplex[1] * -1;
direction = GetTripleProduct(xy, xo, xy);
if (direction.Magnitude() < float.Epsilon)
{
direction = xy.PerendicularCounterClockwise();
}
break;
}
case 3:
{
// Testing whether simplex contains origin..
var xy = simplex[1] - simplex[2];
var xz = simplex[0] - simplex[2];
var xo = simplex[2] * -1.0f;
// Origin outside simplex to right of xz, remove y and reset search direction.
var xzPerp = GetTripleProduct(xy, xz, xz);
if (xzPerp.Dot(xo) >= 0.0f)
{
simplex[1] = simplex[2];
direction = xzPerp;
vertexCount--;
simplex.Remove(simplex.Last());
break;
}
// Origin outside simplex to left of xy, remove z and reset search direction.
var xyPerp = GetTripleProduct(xz, xy, xy);
if (xyPerp.Dot(xo) >= 0.0f)
{
simplex[0] = simplex[1];
simplex[1] = simplex[2];
direction = xyPerp;
vertexCount--;
simplex.Remove(simplex.Last());
break;
}
// Origin inside simplex, overlap found!
return(true);
}
default:
break;
}
// Calculate support vertex in search direction.
var support = GetSupportVertex(shapeA, shapeB, direction);
// Support vertex did not pass origin, no overlap!
if (support.Dot(direction) <= GjkEpsilon)
{
return(false);
}
// Add support vertex to simplex.
simplex.Add(support);
vertexCount++;
}
return(false);
}