internal static void ResetClearance(Edge *s) { var perp = Math.PerpCcw(s->Dest->Point - s->Org->Point); var o = s->Org->Point; var d = s->Dest->Point; ResetClearance(o, d, s, perp); ResetClearance(d, o, s->Sym, perp); }
void Update() { var a = A.position.xz(); var b = B.position.xz(); var c = C.position.xz(); var s = S.position.xz(); var g = G.position.xz(); DebugUtil.DrawCircle(a, b, c, Color.black); DebugUtil.DrawLine(A.position.xz(), B.position.xz(), Color.black); DebugUtil.DrawLine(B.position.xz(), C.position.xz(), Color.black); DebugUtil.DrawLine(C.position.xz(), A.position.xz(), Color.black); DebugUtil.DrawLine(S.position.xz(), G.position.xz()); DebugUtil.DrawCircle(S.position.xz(), R); DebugUtil.DrawCircle(G.position.xz(), R); foreach (var edge in GetEdges()) { DebugUtil.DrawLine(edge.Item1, edge.Item2, Color.red); } FindPath(); void FindPath() { var sg = g - s; var perp = Math.PerpCcw(sg); var pd = math.normalize(perp) * 2 * R; var sp = s + perp; var gp = g + perp; double2 bl = default; double2 tl = default; double2 br = default; double2 tr = default; var topFound = false; var bottomFound = false; CheckVertex(a); CheckVertex(b); CheckVertex(c); if (!bottomFound) { bl = IntersectTri(false, s, sp); br = IntersectTri(false, g, gp); } if (!topFound) { tl = IntersectTri(true, s, sp); tr = IntersectTri(true, g, gp); } void CheckVertex(double2 v) { if (GeometricPredicates.Orient2DFast(s, sp, v) <= 0 && GeometricPredicates.Orient2DFast(g, gp, v) >= 0) { if (GeometricPredicates.Orient2DFast(s, g, v) > 0) { tl = Math.ProjectLine(s, sp, v + pd); tr = Math.ProjectLine(g, gp, v + pd); topFound = true; } else { bl = Math.ProjectLine(s, sp, v - pd); br = Math.ProjectLine(g, gp, v - pd); bottomFound = true; } } } double2 IntersectTri(bool up, double2 l0, double2 l1) { if (IntersectLineSeg(l0, l1, a, b, out var r)) { var orient = GeometricPredicates.Orient2DFast(s, g, r); if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, b - a) < 0 : math.dot(sg, b - a) > 0)) { return(up ? r + pd : r - pd); } } if (IntersectLineSeg(l0, l1, b, c, out r)) { var orient = GeometricPredicates.Orient2DFast(s, g, r); if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, c - b) < 0 : math.dot(sg, c - b) > 0)) { return(up ? r + pd : r - pd); } } if (IntersectLineSeg(l0, l1, c, a, out r)) { var orient = GeometricPredicates.Orient2DFast(s, g, r); if ((up ? orient > 0 : orient < 0) || orient == 0 && (up ? math.dot(sg, a - c) < 0 : math.dot(sg, a - c) > 0)) { return(up ? r + pd : r - pd); } } throw new BreakDebuggerException(); } // DebugUtil.DrawLine(bl, br); // DebugUtil.DrawLine(br, tr); // DebugUtil.DrawLine(tr, tl); // DebugUtil.DrawLine(tl, bl); } }