I’m making an attempt to implement the GJK algorithm by following this lecture. For probably the most half, it’s working, however generally 1 of the two closest factors is inaccurate. Listed here are two examples:
EXAMPLE 1:
The closest factors are accurately calculated.
EXAMPLE 2:
As you’ll be able to see, the closet level on the sq.’s closest level ought to have been the decrease proper nook and never the higher left nook, and the L-shape’s closet level ought to have been the higher proper nook and never the middle.
I’ve been making an attempt to debug the code for days now, so a recent pair of eyes is perhaps useful. If I did one thing mistaken, please clarify why; I actually need to perceive how this all works!
Right here is my code:
public struct Vertex
{
public float Middle { get; }
public Vector Level { get; }
public Vector Point1 { get; }
public Vector Point2 { get; }
public Vertex(float middle, Vector level, Vector point1, Vector point2)
{
Middle = middle;
Level = level;
Point1 = point1;
Point2 = point2;
}
public Vertex(float middle, Vertex vertex)
{
Middle = middle;
Level = vertex.Level;
Point1 = vertex.Point1;
Point2 = vertex.Point2;
}
}
public static class Simplex
{
personal static Vector ClosestPoint(Vertex[] simplex)
{
swap (simplex.Size)
{
case 1:
{
return simplex[0].Level;
}
case 2:
{
return ClosestPoint(simplex[0].Level, simplex[1].Level);
}
case 3:
{
var closetPoint = ClosestPoint(simplex[0].Level, simplex[1].Level);
var shortestDistance = closetPoint.MagnitudeSquared();
var factors = new[]
{
ClosestPoint(simplex[1].Level, simplex[2].Level),
ClosestPoint(simplex[2].Level, simplex[0].Level)
};
for (var index = 0; index < factors.Size; index++)
{
var distance = factors[index].MagnitudeSquared();
if (distance.IsGreaterThanOrEqualTo(shortestDistance)) proceed;
closetPoint = factors[index];
shortestDistance = distance;
}
return closetPoint;
}
default:
{
throw new IndexOutOfRangeException($"The depend is {simplex.Size}, which is out of vary for this operation.");
}
}
}
personal static Vector ClosestPoint(Vector begin, Vector finish)
{
var edge = finish - begin;
var distance = (-start).DotProduct(edge) / edge.MagnitudeSquared();
if (distance.IsLessThanZero())
{
return begin;
}
if (distance.IsGreaterThan(1.0f))
{
return finish;
}
return begin + edge * distance;
}
public static Level[] Resolve(Polygon polygon, Polygon different)
{
var divisor = 1.0f;
var simplex = new[] {new Vertex(1.0f, (Vector) different.First() - polygon.First(), polygon.First(), different.First())};
for (var iteration = 0; iteration < 20; iteration++)
{
swap (simplex.Size)
{
case 1:
{
break;
}
case 2:
{
simplex = OneSimplex(simplex, out divisor);
break;
}
case 3:
{
simplex = TwoSimplex(simplex, out divisor);
break;
}
default:
{
throw new IndexOutOfRangeException($"The depend is {simplex.Size}, which is out of vary for this operation.");
}
}
if (simplex.Size == 3)
{
break;
}
var course = -ClosestPoint(simplex);
if (course.DotProduct(course).IsZero())
{
break;
}
var help = SupportPoint(course, polygon, different, out var point1, out var point2);
if (simplex.Any(vertex => vertex.Level == help))
{
break;
}
var newSimplex = new Vertex[simplex.Length + 1];
for (var index = 0; index < simplex.Size; index++)
{
newSimplex[index] = simplex[index];
}
newSimplex[simplex.Length] = new Vertex(1.0f, help, point1, point2);
simplex = newSimplex;
}
swap (simplex.Size)
{
case 1:
{
return new Level[] {simplex[0].Point1, simplex[0].Point2};
}
case 2:
{
var scalar = 1.0f / divisor;
return new Level[]
{
simplex[0].Point1 * (scalar * simplex[0].Middle) + simplex[1].Point1 * (scalar * simplex[1].Middle),
simplex[0].Point2 * (scalar * simplex[0].Middle) + simplex[1].Point2 * (scalar * simplex[1].Middle)
};
}
case 3:
{
var scalar = 1.0f / divisor;
return new Level[]
{
simplex[0].Point1 * (scalar * simplex[0].Middle) +
simplex[1].Point1 * (scalar * simplex[1].Middle) +
simplex[2].Point1 * (scalar * simplex[2].Middle)
};
}
default:
{
throw new IndexOutOfRangeException($"The depend is {simplex.Size}, which is out of vary for this operation.");
}
}
}
personal static Vertex[] OneSimplex(Vertex[] simplex, out float divisor)
{
var v = (-simplex[0].Level).DotProduct(simplex[1].Level - simplex[0].Level);
if (v.IsLessThanZero())
{
divisor = 1.0f;
return new[] {new Vertex(1.0f, simplex[0])};
}
var u = (-simplex[1].Level).DotProduct(simplex[0].Level - simplex[1].Level);
if (u.IsLessThanZero())
{
divisor = 1.0f;
return new[] {new Vertex(1.0f, simplex[1])};
}
var edge = simplex[1].Level - simplex[0].Level;
divisor = edge.DotProduct(edge);
return new[] {new Vertex(u, simplex[0]), new Vertex(v, simplex[1])};
}
personal static Vertex[] TwoSimplex(Vertex[] simplex, out float divisor)
{
var uAb = (-simplex[1].Level).DotProduct(simplex[0].Level - simplex[1].Level);
var vAb = (-simplex[0].Level).DotProduct(simplex[1].Level - simplex[0].Level);
var uBc = (-simplex[2].Level).DotProduct(simplex[1].Level - simplex[2].Level);
var vBc = (-simplex[1].Level).DotProduct(simplex[2].Level - simplex[1].Level);
var uCa = (-simplex[0].Level).DotProduct(simplex[2].Level - simplex[0].Level);
var vCa = (-simplex[2].Level).DotProduct(simplex[0].Level - simplex[2].Level);
if (vAb.IsLessThanOrEqualToZero() && uCa.IsLessThanOrEqualToZero())
{
divisor = 1.0f;
return new[] {new Vertex(1.0f, simplex[0])};
}
if (uAb.IsLessThanOrEqualToZero() && vBc.IsLessThanOrEqualToZero())
{
divisor = 1.0f;
return new[] {new Vertex(1.0f, simplex[1])};
}
if (uBc.IsLessThanOrEqualToZero() && vCa.IsLessThanOrEqualToZero())
{
divisor = 1.0f;
return new[] {new Vertex(1.0f, simplex[2])};
}
var space = (simplex[1].Level - simplex[0].Level).CrossProduct(simplex[2].Level - simplex[0].Level);
var uAbc = (simplex[1].Level).CrossProduct(simplex[2].Level);
var vAbc = (simplex[2].Level).CrossProduct(simplex[0].Level);
var wAbc = (simplex[0].Level).CrossProduct(simplex[1].Level);
if (uAb.IsGreaterThanZero() && vAb.IsGreaterThanZero() && (wAbc * space).IsLessThanOrEqualToZero())
{
var edge = simplex[1].Level - simplex[0].Level;
divisor = edge.DotProduct(edge);
return new[] {new Vertex(uAb, simplex[0]), new Vertex(vAb, simplex[1])};
}
if (uBc.IsGreaterThanZero() && vBc.IsGreaterThanZero() && (uAbc * space).IsLessThanOrEqualToZero())
{
var edge = simplex[2].Level - simplex[1].Level;
divisor = edge.DotProduct(edge);
return new[] {new Vertex(uBc, simplex[1]), new Vertex(vBc, simplex[2])};
}
if (uCa.IsGreaterThanZero() && vCa.IsGreaterThanZero() && (vAbc * space).IsLessThanOrEqualToZero())
{
var edge = simplex[0].Level - simplex[2].Level;
divisor = edge.DotProduct(edge);
return new[] {new Vertex(uCa, simplex[2]), new Vertex(vCa, simplex[0])};
}
divisor = space;
return new[] {new Vertex(uAbc, simplex[0]), new Vertex(vAbc, simplex[1]), new Vertex(wAbc, simplex[2])};
}
personal static Vector SupportPoint(Vector course, Polygon polygon)
{
var supportPoint = polygon.First();
var supportValue = course.DotProduct(supportPoint);
for (var index = 1; index < polygon.Depend; index++)
{
var worth = course.DotProduct(polygon[index]);
if (worth.IsLessThanOrEqualTo(supportValue)) proceed;
supportPoint = polygon[index];
supportValue = worth;
}
return supportPoint;
}
personal static Vector SupportPoint(Vector course, Polygon polygon, Polygon different, out Vector point1, out Vector point2)
{
point1 = SupportPoint(-direction, polygon);
point2 = SupportPoint(course, different);
return point2 - point1;
}
}
UPDATE:
Out of curiosity, I downloaded the supply code and transformed it from C++ to C#, so in addition to minor syntax adjustments it was precisely the identical. To my shock the bug was taking place with it as properly. Does this algorithm not work on concave polygons?
UPDATE 2:
I simply observed it doesn’t work 100% of the time with the sq. and the triangle, so the bug is just not restricted to concave polygons.
UPDATE 3:
There was a bug with my sq. initialization, forgot to set the purpose depend to 4 so it solely ever took the primary level. Now that it could correctly iterate by the factors it not is a matter. The L-shape nonetheless has an issue. After researching the algorithm a bit extra, the Minkowski distinction creates a convex hull, and due to this fact is not going to work on concave shapes just like the L. Wanting on the picture now it ought to have been apparent as a result of the purple line ends proper the place the convex hull can be. With all that being stated, is there a unique algorithm that I can use or will I simply need to iterate over every edge and discover the closest factors that means?
