public void CalculateN()
{
if (!IsCollision || N != Vector2.Zero)
{
return;
}
float tolerance = 3f;
float lastDist = float.MaxValue;
while (true)
{
Simplex.Edge e = simplex.FindClosestEdge();
MinkowskiDiff p = MinkowskiDiff.Support(Object1, Object2, e.Normal);
float d = p.S.Dot(e.Normal);
if (d - e.Distance < tolerance || lastDist - d + e.Distance < 1f)
{
N = e.Normal;
Depth = d;
return;
}
simplex.Add(p, e.Index + 1);
lastDist = d - e.Distance;
}
}
public void Add(MinkowskiDiff Value, int index)
{
index = (count + index) % count;
for (int i = count - 1; i >= index; i--)
points[i + 1] = points[i];
points[index] = Value;
count++;
}
public void Add(MinkowskiDiff value, int index)
{
index = (count + index) % count;
for (int i = count - 1; i >= index; i--)
{
points[i + 1] = points[i];
}
points[index] = value;
count++;
}
public void Add(MinkowskiDiff Value)
{
points[count++] = Value;
}
public void Add(MinkowskiDiff value)
{
points[count++] = value;
}
public bool Check()
{
if (IsCollision)
{
return(true);
}
if (Object1 == Object2)
{
return(false);
}
Vector2 axis = new Vector2(1, 1);
simplex.Add(MinkowskiDiff.Support(Object1, Object2, axis));
simplex.Add(MinkowskiDiff.Support(Object1, Object2, -axis));
axis = (simplex[1].S - simplex[0].S).Perpendicular();
if (Orient(simplex[0].S, simplex[1].S) < 0)
{
axis = -axis;
}
for (int b = 0; b < 20; b++)
{
simplex.Add(MinkowskiDiff.Support(Object1, Object2, axis));
if (simplex[2].S.Dot(axis) <= 0)
{
return(false);
}
else if (simplex.ContainsOrigin(ref axis))
{
float do1 = simplex[0].S.LengthSquared();
float do2 = simplex[1].S.LengthSquared();
float do3 = simplex[2].S.LengthSquared();
MinkowskiDiff A, B;
if (do1 > do2 && do1 > do3)
{
A = simplex[1];
B = simplex[2];
}
else if (do2 > do1 && do2 > do3)
{
A = simplex[0];
B = simplex[2];
}
else
{
A = simplex[0];
B = simplex[1];
}
IsCollision = true;
if (A.S == B.S)
{
Point1 = A.S1;
Point2 = A.S2;
}
Vector2 L = B.S - A.S;
float l2 = -L.Dot(A.S) / L.Dot(L);
float l1 = 1 - l2;
if (l1 < 0)
{
Point1 = B.S1;
Point2 = B.S2;
return(true);
}
if (l2 < 0)
{
Point1 = A.S1;
Point2 = A.S2;
return(true);
}
Point1 = l1 * A.S1 + l2 * B.S1;
Point2 = l1 * A.S2 + l2 * B.S2;
return(true);
}
}
return(false);
}
