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