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); }
private static bool SanityCheck(Vertices vertices) { if (vertices.Count < 3) { return(false); } if (vertices.GetArea() < 0.00001f) { return(false); } for (int i = 0; i < vertices.Count; ++i) { int i1 = i; int i2 = i + 1 < vertices.Count ? i + 1 : 0; FVector2 edge = vertices[i2] - vertices[i1]; if (edge.LengthSquared() < Box2D.Settings.b2_epsilon * Box2D.Settings.b2_epsilon) { return(false); } } for (int i = 0; i < vertices.Count; ++i) { int i1 = i; int i2 = i + 1 < vertices.Count ? i + 1 : 0; FVector2 edge = vertices[i2] - vertices[i1]; for (int j = 0; j < vertices.Count; ++j) { // Don't check vertices on the current edge. if (j == i1 || j == i2) { continue; } FVector2 r = vertices[j] - vertices[i1]; // Your polygon is non-convex (it has an indentation) or // has colinear edges. float s = edge.X * r.Y - edge.Y * r.X; if (s < 0.0f) { return(false); } } } return(true); }