Edge *RemoveVertex(Vertex *vert)
{
Assert.IsTrue(vert->Edge != null);
Assert.IsTrue(vert->Edge->Org == vert);
var remaining = vert->Edge->LNext;
InfiniteLoopDetection.Reset();
while (true)
{
InfiniteLoopDetection.Register(1000, "RemoveVertex");
var e = vert->Edge;
if (e == null)
{
break;
}
RemoveEdge(e);
}
V.Remove((IntPtr)vert);
Assert.IsTrue(vert->Edge == null);
_qt.Remove(vert);
var delPos = vert->SeqPos;
((Vertex *)_verticesSeq[_verticesSeq.Length - 1])->SeqPos = delPos;
_verticesSeq.RemoveAtSwapBack(delPos);
_vertices.Recycle(vert);
return(remaining);
}
void LocalRefinement()
{
var verts = V.GetEnumerator();
while (verts.MoveNext())
{
_refinementQueue.PushBack(verts.Current);
}
InfiniteLoopDetection.Reset();
while (_refinementQueue.Count > 0)
{
InfiniteLoopDetection.Register(1000, "LocalRefinement");
var v = (Vertex *)_refinementQueue.PopFront();
var e = v->GetEdgeEnumerator();
while (e.MoveNext())
{
if (TriDisturbed(e.Current, out var vRef) || TravsDisturbed(e.Current, out vRef))
{
// todo are we adding duplicates here?
_refinementQueue.PushBack((IntPtr)v);
_refinementQueue.PushBack((IntPtr)vRef);
break;
}
}
}
}
/// <summary>
/// Returns an edge for which the specified point is contained within it's left face. If the point lies
/// on an edge this edge is returned. If the point lies on a vertex an arbitrary edge with identical origin is returned.
/// </summary>
/// <param name="collinear">True when the specified point lies on the returned edge</param>
public Edge *FindTriangleContainingPoint(float2 p, out bool collinear)
{
var e = FindClosestVertex(p)->Edge;
Assert.IsTrue(e != null);
InfiniteLoopDetection.Reset();
while (true)
{
InfiniteLoopDetection.Register(1000, "FindTriangleContainingPoint");
Edge *collinearEdge = null;
var orient = Math.TriArea(e->Org->Point, e->Dest->Point, p);
if (orient == 0)
{
collinearEdge = e;
}
else if (orient < 0)
{
e = e->Sym;
continue;
}
orient = Math.TriArea(e->ONext->Org->Point, e->ONext->Dest->Point, p);
if (orient == 0)
{
collinearEdge = e->ONext;
}
else if (orient > 0)
{
e = e->ONext;
continue;
}
orient = Math.TriArea(e->DPrev->Org->Point, e->DPrev->Dest->Point, p);
if (orient == 0)
{
collinear = true;
return(e->DPrev);
}
if (orient > 0)
{
e = e->DPrev;
continue;
}
if (collinearEdge != null)
{
collinear = true;
return(collinearEdge);
}
collinear = false;
return(e);
}
}
Vertex *CreatePRef(float2 p, Edge *e)
{
var stepSize = _e / 2;
var po = e->Org->Point - p;
var pd = e->Dest->Point - p;
var dir = math.normalize(e->Dest->Point - e->Org->Point);
var lpo = math.length(po) - _e;
var lpd = math.length(pd) - _e;
var offset = 0f;
InfiniteLoopDetection.Reset();
while (true)
{
InfiniteLoopDetection.Register(1000, "CreatePRef");
offset += stepSize;
if (offset >= lpo)
{
return(e->Org);
}
if (offset >= lpd)
{
return(e->Dest);
}
var pplus = p + offset * dir;
var pointPresent = TryGetPoint(pplus, e, out var vertex);
if (vertex != null)
{
return(vertex);
}
if (!pointPresent && SplitIsRobust(pplus, e))
{
return(InsertPointInEdge(pplus, e));
}
var pmin = p - offset * dir;
pointPresent = TryGetPoint(pmin, e, out vertex);
if (vertex != null)
{
return(vertex);
}
if (!pointPresent && SplitIsRobust(pmin, e))
{
return(InsertPointInEdge(pmin, e));
}
}
}
void InsertSegmentNoConstraints(Vertex *a, Vertex *b, Entity id)
{
var c = GetConnection(a, b);
if (c != null)
{
C.TryAdd((IntPtr)c);
if (!c->IsConstrainedBy(id))
{
c->AddConstraint(id);
}
ResetClearance(c);
return;
}
var e = GetLeftEdge(a, b->Point);
InfiniteLoopDetection.Reset();
while (e->Dest != b)
{
InfiniteLoopDetection.Register(1000, "InsertSegmentNoConstraints");
var d = Math.TriArea(a->Point, b->Point, e->Dest->Point);
var next = d > 0 ? e->RPrev : e->ONext;
if (d < 0)
{
Assert.IsTrue(!e->Constrained);
RemoveEdge(e);
}
else if (d == 0 && e->Dest != a)
{
var t = e->Dest;
Connect(a, t, id);
a = t;
}
e = next;
}
Connect(a, b, id);
}
static Edge *GetLeftEdge(Vertex *a, float2 p)
{
var result = a->Edge;
var o = result->Org->Point;
InfiniteLoopDetection.Reset();
while (!Math.Ccw(o, p, result->Dest->Point))
{
InfiniteLoopDetection.Register(1000, "GetLeftEdge 0");
result = result->ONext;
}
InfiniteLoopDetection.Reset();
while (Math.Ccw(o, p, result->OPrev->Dest->Point))
{
InfiniteLoopDetection.Register(1000, "GetLeftEdge 1");
result = result->OPrev;
}
return(result);
}
bool TravsDisturbed(Edge *edge, out Vertex *vRef)
{
var v = edge->Org->Point;
var check = edge->LNext->Sym;
InfiniteLoopDetection.Reset();
while (true)
{
InfiniteLoopDetection.Register(1000, "TravsDisturbed 0");
if (check->Constrained)
{
break;
}
if (!Math.ProjectSeg(check->Org->Point, check->Dest->Point, v, out _))
{
check = check->OPrev->Sym;
if (check->Constrained || !Math.ProjectSeg(check->Org->Point, check->Dest->Point, v, out _))
{
break;
}
}
if (RhsDisturbed(check, out vRef))
{
return(true);
}
check = check->DPrev;
}
check = edge->LNext;
InfiniteLoopDetection.Reset();
while (true)
{
InfiniteLoopDetection.Register(1000, "TravsDisturbed 1");
if (check->Constrained)
{
break;
}
if (!Math.ProjectSeg(check->Org->Point, check->Dest->Point, v, out _))
{
check = check->ONext->Sym;
if (check->Constrained || !Math.ProjectSeg(check->Org->Point, check->Dest->Point, v, out _))
{
break;
}
}
if (LhsDisturbed(check, out vRef))
{
return(true);
}
check = check->DNext;
}
vRef = default;
return(false);
}
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
});
}
Vertex *InsertPoint(float2 p)
{
var closest = _qt.FindClosest(p);
if (math.lengthsq(closest->Point - p) <= _e * _e)
{
return(closest);
}
var e = closest->Edge;
Assert.IsTrue(e != null);
InfiniteLoopDetection.Reset();
while (true)
{
InfiniteLoopDetection.Register(1000, "InsertPoint");
Edge *inEdge = null;
var orient = Math.TriArea(e->Org->Point, e->Dest->Point, p);
if (orient == 0)
{
inEdge = e;
}
if (orient < 0)
{
e = e->Sym;
continue;
}
orient = Math.TriArea(e->ONext->Org->Point, e->ONext->Dest->Point, p);
if (orient == 0)
{
inEdge = e->ONext;
}
if (orient > 0)
{
e = e->ONext;
continue;
}
orient = Math.TriArea(e->DPrev->Org->Point, e->DPrev->Dest->Point, p);
if (orient == 0)
{
inEdge = e->DPrev;
}
if (orient > 0)
{
e = e->DPrev;
continue;
}
if (inEdge != null)
{
Assert.IsTrue(SplitIsRobust(p, inEdge));
return(InsertPointInEdge(p, inEdge));
}
return(InsertPointInFace(p, e));
}
}
// http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.61.3862&rep=rep1&type=pdf
// TriangulatePseudopolygonDelaunay()
void RetriangulateFace(Edge *edge)
{
Assert.IsTrue(edge != null);
Assert.IsTrue(edge != edge->LNext->LNext);
if (edge->LNext->LNext->LNext == edge)
{
NewTriangle(edge);
return;
}
InfiniteLoopDetection.Reset();
while (!Math.Ccw(edge->Org->Point, edge->Dest->Point, edge->LNext->Dest->Point))
{
InfiniteLoopDetection.Register(1000, "RetriangulateFace 0");
edge = edge->LNext;
}
var c = edge->LNext;
var e = c;
InfiniteLoopDetection.Reset();
while (true)
{
InfiniteLoopDetection.Register(1000, "RetriangulateFace 1");
e = e->LNext;
if (e->LNext == edge)
{
break;
}
if (Math.CircumcircleContains(edge->Org->Point, edge->Dest->Point, c->Dest->Point, e->Dest->Point))
{
c = e;
}
}
Assert.IsTrue(c != edge);
var connected = false;
if (c->LNext->LNext != edge)
{
V.TryAdd((IntPtr)edge->LPrev->Dest);
V.TryAdd((IntPtr)c->LNext->Org);
var b = Connect(edge->LPrev, c->LNext);
RetriangulateFace(b);
connected = true;
}
if (c != edge->LNext)
{
V.TryAdd((IntPtr)c->Dest);
V.TryAdd((IntPtr)edge->LNext->Org);
var a = Connect(c, edge->LNext);
RetriangulateFace(a);
connected = true;
}
if (connected)
{
NewTriangle(edge);
}
}