I’m making an attempt to implement the GJK algorithm following this convention. For essentially the most half, it’s working, however generally 1 of the two closest factors is wrong. Listed here are two examples:
Instance 1:
The closest factors are calculated accurately.
Instance 2:
As you may see, the closet level on the closest level of the sq. ought to have been the decrease proper nook and never the higher left nook, and the closet level of the shape l ought to have been the higher proper nook and never the middle.
I’ve been making an attempt to purify the code for days, so a brand new pair of eyes may very well be helpful. If I did one thing unsuitable, clarify why; I actually wish to perceive how all this works!
Right here is my code:
public struct Vertex
{
public float Heart { get; }
public Vector Level { get; }
public Vector Point1 { get; }
public Vector Point2 { get; }
public Vertex(float heart, Vector level, Vector point1, Vector point2)
{
Heart = heart;
Level = level;
Point1 = point1;
Point2 = point2;
}
public Vertex(float heart, Vertex vertex)
{
Heart = heart;
Level = vertex.Level;
Point1 = vertex.Point1;
Point2 = vertex.Point2;
}
}
public static class Simplex
{
non-public static Vector ClosestPoint(Vertex() simplex)
{
change (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 rely is {simplex.Size}, which is out of vary for this operation.");
}
}
}
non-public 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() Remedy(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++)
{
change (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 rely is {simplex.Size}, which is out of vary for this operation.");
}
}
if (simplex.Size == 3)
{
break;
}
var route = -ClosestPoint(simplex);
if (route.DotProduct(route).IsZero())
{
break;
}
var assist = SupportPoint(route, polygon, different, out var point1, out var point2);
if (simplex.Any(vertex => vertex.Level == assist))
{
break;
}
var newSimplex = new Vertex(simplex.Size + 1);
for (var index = 0; index < simplex.Size; index++)
{
newSimplex(index) = simplex(index);
}
newSimplex(simplex.Size) = new Vertex(1.0f, assist, point1, point2);
simplex = newSimplex;
}
change (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).Heart) + simplex(1).Point1 * (scalar * simplex(1).Heart),
simplex(0).Point2 * (scalar * simplex(0).Heart) + simplex(1).Point2 * (scalar * simplex(1).Heart)
};
}
case 3:
{
var scalar = 1.0f / divisor;
return new Level()
{
simplex(0).Point1 * (scalar * simplex(0).Heart) +
simplex(1).Point1 * (scalar * simplex(1).Heart) +
simplex(2).Point1 * (scalar * simplex(2).Heart)
};
}
default:
{
throw new IndexOutOfRangeException($"The rely is {simplex.Size}, which is out of vary for this operation.");
}
}
}
non-public 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))};
}
non-public 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))};
}
non-public static Vector SupportPoint(Vector route, Polygon polygon)
{
var supportPoint = polygon.First();
var supportValue = route.DotProduct(supportPoint);
for (var index = 1; index < polygon.Depend; index++)
{
var worth = route.DotProduct(polygon(index));
if (worth.IsLessThanOrEqualTo(supportValue)) proceed;
supportPoint = polygon(index);
supportValue = worth;
}
return supportPoint;
}
non-public static Vector SupportPoint(Vector route, Polygon polygon, Polygon different, out Vector point1, out Vector point2)
{
point1 = SupportPoint(-direction, polygon);
point2 = SupportPoint(route, different);
return point2 - point1;
}
}
UPDATE:
Out of curiosity, I downloaded the supply code and transformed it from C ++ to C#, so along with minor syntax adjustments it was precisely the identical. To my shock, the error was additionally occurring with him. Does this algorithm work in concave polygons?
Replace 2:
I simply observed that 100% of the time doesn’t work with the sq. and the triangle, so the error will not be restricted to the concave polygons.
Replace 3:
There was an error with my sq. initialization, I forgot to ascertain the factors rely in 4, so it solely took the primary level. Now that it may well correctly objects by way of the factors, it’s now not an issue. The type of l nonetheless has an issue. After investigating the algorithm somewhat extra, the distinction in Minkowski creates a convex helmet and, due to this fact, it is not going to work in concave shapes reminiscent of L. Wanting on the picture now ought to have been apparent as a result of the purple line ends simply the place the convex helmet could be. Mentioned all this, is there a distinct algorithm that I can use or will I’ve to iterate on every edge and discover the closest factors that method?
