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); }