public static float DistanceBetweenPointAndLineSegment(ref FVector2 point, ref FVector2 lineEndPoint1, ref FVector2 lineEndPoint2) { FVector2 v = FVector2.Subtract(lineEndPoint2, lineEndPoint1); FVector2 w = FVector2.Subtract(point, lineEndPoint1); float c1 = FVector2.Dot(w, v); if (c1 <= 0) { return(DistanceBetweenPointAndPoint(ref point, ref lineEndPoint1)); } float c2 = FVector2.Dot(v, v); if (c2 <= c1) { return(DistanceBetweenPointAndPoint(ref point, ref lineEndPoint2)); } float b = c1 / c2; FVector2 pointOnLine = FVector2.Add(lineEndPoint1, FVector2.Multiply(v, b)); return(DistanceBetweenPointAndPoint(ref point, ref pointOnLine)); }
public PolyNode GetRightestConnection(PolyNode incoming) { if (NConnected == 0) { Debug.Assert(false); // This means the connection graph is inconsistent } if (NConnected == 1) { //b2Assert(false); // Because of the possibility of collapsing nearby points, // we may end up with "spider legs" dangling off of a region. // The correct behavior here is to turn around. return(incoming); } FVector2 inDir = Position - incoming.Position; float inLength = inDir.Length(); inDir.Normalize(); Debug.Assert(inLength > Box2D.Settings.b2_epsilon); PolyNode result = null; for (int i = 0; i < NConnected; ++i) { if (Connected[i] == incoming) { continue; } FVector2 testDir = Connected[i].Position - Position; float testLengthSqr = testDir.LengthSquared(); testDir.Normalize(); Debug.Assert(testLengthSqr >= Box2D.Settings.b2_epsilon * Box2D.Settings.b2_epsilon); float myCos = FVector2.Dot(inDir, testDir); float mySin = MathUtils.Cross(inDir, testDir); if (result != null) { FVector2 resultDir = result.Position - Position; resultDir.Normalize(); float resCos = FVector2.Dot(inDir, resultDir); float resSin = MathUtils.Cross(inDir, resultDir); if (IsRighter(mySin, myCos, resSin, resCos)) { result = Connected[i]; } } else { result = Connected[i]; } } Debug.Assert(result != null); return(result); }