public static ClosestInfo GetClosestVertex(World world, Vector2 p, float selectionRadius = 6f)
{
var minDistance = float.MaxValue;
var info = new ClosestInfo();
var selection2 = selectionRadius * selectionRadius;
for (var i = 0; i < world.bodies.Count; ++i)
{
var b = world.bodies[i];
for (var vIndex = 0; vIndex < b.vertices.Length; ++vIndex)
{
var dist = MathUtils.SquareDistance(b.vertices[vIndex].position, p);
if (dist < selection2 && dist < minDistance)
{
minDistance = dist;
info.v = b.vertices[vIndex];
info.body = b;
}
}
}
if (info.body != null)
{
for (var i = 0; i < info.body.constraints.Length; ++i)
{
var pin = info.body.constraints[i] as PinConstraint;
if (pin != null && pin.v == info.v)
{
info.pin = pin;
}
}
}
return(info);
}
public void TestHarness() // fly the route and debug the closestto.. keep this for testing
{
for (double percent = -10; percent < 110; percent += 0.1)
{
ISystem cursys = PosAlongRoute(percent, 100);
System.Diagnostics.Debug.WriteLine(Environment.NewLine + "Sys {0} {1} {2} {3}", cursys.X, cursys.Y, cursys.Z, cursys.Name);
ClosestInfo closest = ClosestTo(cursys);
if (closest != null)
{
System.Diagnostics.Debug.WriteLine("Next {0} {1} {2} {3}, index {4} dev {5} dist to wp {6} cumul left {7}", closest.system?.X, closest.system?.Y, closest.system?.Z, closest.system?.Name, closest.waypoint, closest.deviation, closest.disttowaypoint, closest.cumulativewpdist);
}
}
}
public static int Compare(ClosestInfo x, ClosestInfo y)
{
return(y.dist.CompareTo(x.dist)); // 使Sort排序时倒序
///return x.dist.CompareTo(y.dist);
}
