void CheckTraversal(float2 a, Edge *exit, bool lhs, Edge *s)
{
var b = exit->Org->Point;
var c = exit->Dest->Point;
if (!Math.ProjectSeg(a, c, b, out var bac))
{
return;
}
var mirror = b + (c - 2 * bac + a);
var lba = math.lengthsq(a - b);
var lbc = math.lengthsq(c - b);
var clearance = math.sqrt(math.min(lba, lbc));
var s1 = s->Org->Point;
var s2 = s->Dest->Point;
if (Math.IntersectSegCircle(s1, s2, b, clearance) < 2 && Math.IntersectSegCircle(s1, s2, mirror, clearance) < 2)
{
return;
}
if (Math.ProjectSeg(s1, s2, b, out var bi))
{
var bib = b - bi;
var lbib = math.length(bib);
clearance = math.min(clearance, lbib);
var r1 = bi + bib / lbib * clearance;
var acp = Math.PerpCw(c - a);
var s12 = s2 - s1;
var s12p = Math.PerpCw(s12);
var r2 = Math.Angle(acp, s12p) < 0
? Math.IntersectLineLine(r1, r1 + s12, c, c + acp)
: Math.IntersectLineLine(r1, r1 + s12, c, c + s12p);
if (TryGetDisturbance(lhs ? exit->Sym : exit, r1, r2, c, out var e, out _, out var vert, lhs))
{
var v = vert->Point;
var vi = Math.ProjectLine(s1, s2, v);
var lvs = math.length(vi - v);
var lve = math.length(e - v);
if (lvs < lve && lvs < clearance)
{
V.TryAdd((IntPtr)vert);
return;
}
}
}
Propagate(s, lhs ? exit : exit->Sym);
}
Point GetNextPoint(Vertex *a, Vertex *b, float2 start, float2 end)
{
InfiniteLoopDetection.Reset();
var e = GetLeftEdge(a, b->Point);
while (e->Dest != b)
{
InfiniteLoopDetection.Register(1000, "GetNextPoint");
var d = Math.TriArea(a->Point, b->Point, e->Dest->Point);
if (d < 0 && e->Constrained)
{
var p = (float2)Math.IntersectLineSegClamped(start, end, e->Org->Point, e->Dest->Point);
var pointExists = TryGetPoint(p, e, out var v);
if (v != null)
{
if (_insertedPoints.Length > 1)
{
var prev = _insertedPoints[_insertedPoints.Length - 1].Vertex;
if (prev == v || e->Org == prev || e->Dest == prev)
{
continue;
}
}
if (_insertedPoints.Length > 2)
{
var prev = _insertedPoints[_insertedPoints.Length - 2].Vertex;
if (prev == v || e->Org == prev || e->Dest == prev)
{
continue;
}
}
return(new Point
{
Vertex = v,
FoundExisting = true,
P = p
});
}
if (pointExists || !SplitIsRobust(p, e))
{
var pRef = CreatePRef(p, e);
if (_insertedPoints.Length > 1 && _insertedPoints[_insertedPoints.Length - 1].Vertex == pRef)
{
continue;
}
var point = new Point
{
Vertex = pRef,
Modified = true,
P = p
};
var proj = (float2)Math.ProjectLine(a->Point, b->Point, point.Vertex->Point);
var pproj = proj - p;
if (math.dot(b - a, pproj) < 0)
{
point.Before = proj;
point.After = p;
}
else
{
point.Before = p;
point.After = proj;
}
return(point);
}
var vert = InsertPointInEdge(p, e);
return(new Point
{
Vertex = vert,
P = p
});
}
e = d > 0 ? e->RPrev : e->ONext;
}
return(new Point
{
Vertex = b,
P = b->Point
});
}
bool CheckTraversal(float2 a, float2 b, float2 c, Edge *exit, bool lhs, out Disturbance disturbance)
{
if (!Math.ProjectSeg(a, c, b, out var bac))
{
disturbance = default;
return(false);
}
var mirror = b + (c - 2 * bac + a);
var lba = math.lengthsq(a - b);
var lbc = math.lengthsq(c - b);
var clearance = math.sqrt(math.min(lba, lbc));
var constraint = TryGetConstraint(exit, clearance, b, mirror, lhs);
if (constraint == null || constraint->RefineFailed)
{
disturbance = default;
return(false);
}
var s1 = constraint->Org->Point;
var s2 = constraint->Dest->Point;
if (!Math.ProjectSeg(s1, s2, b, out var bi))
{
disturbance = default;
return(false);
}
// The paper says R is delimited by a line parallel to s passing by b.
// When cl(a, b, c) == dist(b, a) and dist(b, a) < dist(b, s) this allows
// for vertices in R violating definition 3.4: dist(v, s) < cl(a, b, c).
// R will be delimited by a line parallel to s at a distance of cl(a, b, c)
// Technially r1 should be at the intersection point with bc, but as bc
// is enclosed by the Delaunay circle of triangle abc this is irrelevant.
// R is triangle r1, r2, c
var bib = b - bi;
var lbib = math.length(bib);
clearance = math.min(clearance, lbib);
var r1 = bi + bib / lbib * clearance;
var acp = Math.PerpCw(c - a);
var s12 = s2 - s1;
var s12p = Math.PerpCw(s12);
var r2 = Math.Angle(acp, s12p) < 0
? Math.IntersectLineLine(r1, r1 + s12, c, c + acp)
: Math.IntersectLineLine(r1, r1 + s12, c, c + s12p);
if (TryGetDisturbance(lhs ? exit->Sym : exit, r1, r2, c, out var e, out var u, out var vert, lhs))
{
var v = vert->Point;
var vi = Math.ProjectLine(s1, s2, v);
var lvs = math.length(vi - v);
var lve = math.length(e - v);
if (lvs < lve && lvs < clearance)
{
Math.CircleFromPoints(u, v, e, out var centre, out var radius);
var t = Math.IntersectLineCircle(s1, s2, centre, radius, out var x1, out var x2);
Assert.IsTrue(t == 2);
var pRef = (x1 + x2) / 2;
if (ValidateRefinement(ref pRef, constraint))
{
disturbance = new Disturbance(vert, pRef, constraint);
return(true);
}
constraint->RefineFailed = true;
}
}
disturbance = default;
return(false);
}
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);
}
}
