List <Vector2> StringPullingAlgorithm(List <int> triPath, Vector2 startPos, Vector2 targetPos)
{
if (triPath.Count == 1)
{
return(new List <Vector2> {
startPos, targetPos
});
}
List <Vector2> edgePortals = new List <Vector2>();
Vector2Int e = triPairToEdgeMap[new Vector2Int(triPath[0], triPath[1])];
Vector2 v1 = mesh.vertices[e[0]];
Vector2 v2 = mesh.vertices[e[1]];
if (VecMath.Det(v1 - startPos, v2 - startPos) >= 0)
{
//counter clockwise rotation
edgePortals.Add(v1);
edgePortals.Add(v2);
}
else
{
//clockwise rotation
edgePortals.Add(v2);
edgePortals.Add(v1);
}
//Make sure that all edge portals are oriented in the same way
for (int i = 2; i < triPath.Count; i++)
{
int tri1 = triPath[i - 1];
int tri2 = triPath[i];
e = triPairToEdgeMap[new Vector2Int(tri1, tri2)];
v1 = mesh.vertices[e[0]];
v2 = mesh.vertices[e[1]];
if (v1 == edgePortals[edgePortals.Count - 2] ||
v2 == edgePortals[edgePortals.Count - 1])
{
edgePortals.Add(v1);
edgePortals.Add(v2);
}
else
{
edgePortals.Add(v2);
edgePortals.Add(v1);
}
}
edgePortals.Add(targetPos);
edgePortals.Add(targetPos);
//for(int i = 0; i < edgePortals.Count; i+=2)
//{
// Debug.DrawLine(transform.TransformPoint(edgePortals[i]),
// transform.TransformPoint(edgePortals[i + 1]), Color.magenta, 0.0f, false);
//}
//Run Simple Stupid Funnel Algorithm.
List <Vector2> breadCrumbs = new List <Vector2> {
startPos
};
Vector2 funnelL = edgePortals[0] - breadCrumbs[breadCrumbs.Count - 1];
Vector2 funnelR = edgePortals[1] - breadCrumbs[breadCrumbs.Count - 1];
//Left funnel refers to left from our point of view looking down on navmesh
int t = 0;
int epL = 2;
int epR = 2;
int ep = 2;
while (ep < edgePortals.Count && t < 1000)
{
Vector2 newFunnelL = edgePortals[ep] - breadCrumbs[breadCrumbs.Count - 1];
if (VecMath.Det(funnelL, newFunnelL) >= 0)
{
if (edgePortals[ep] == edgePortals[epL] || VecMath.Det(newFunnelL, funnelR) >= 0)
{
funnelL = newFunnelL;
epL = ep;
}
else
{
breadCrumbs.Add(breadCrumbs[breadCrumbs.Count - 1] + funnelR);
ep = epR;
epL = ep;
epR = ep;
funnelL = edgePortals[epL] - breadCrumbs[breadCrumbs.Count - 1];
funnelR = edgePortals[epR + 1] - breadCrumbs[breadCrumbs.Count - 1];
continue;
}
}
Vector2 newFunnelR = edgePortals[ep + 1] - breadCrumbs[breadCrumbs.Count - 1];
if (VecMath.Det(funnelR, newFunnelR) <= 0)
{
if (edgePortals[ep + 1] == edgePortals[epR + 1] || VecMath.Det(funnelL, newFunnelR) >= 0)
{
funnelR = newFunnelR;
epR = ep;
}
else
{
breadCrumbs.Add(breadCrumbs[breadCrumbs.Count - 1] + funnelL);
ep = epL;
epL = ep;
epR = ep;
funnelL = edgePortals[epL] - breadCrumbs[breadCrumbs.Count - 1];
funnelR = edgePortals[epR + 1] - breadCrumbs[breadCrumbs.Count - 1];
continue;
}
}
ep += 2;
t += 1;
}
if (t == 100)
{
Debug.LogError("FUNNEL ALG FAILED");
}
breadCrumbs.Add(targetPos);
for (int i = 1; i < breadCrumbs.Count; i++)
{
Debug.DrawLine(transform.TransformPoint(breadCrumbs[i - 1]),
transform.TransformPoint(breadCrumbs[i]), Color.green, 0.0f, false);
}
return(breadCrumbs);
}
private int GetSensorIndex(Vector3 forwardDir, Vector3 dir)
{
var angle = Mathf.Repeat(VecMath.SignedAngle(forwardDir, dir, Vector3.up), 360f);
return(Mathf.RoundToInt(angle / this.step));
}
private Triangle[] Triangulate(List <Vertex> verts)
{
//Randomize vertices for faster average triangulation
Algorithm.Shuffle <Vertex>(ref verts);
//Construct circle enclosing all the vertices
Vector2 centroid = Vector2.zero;
foreach (Vertex vert in verts)
{
centroid += vert.p;
}
centroid /= verts.Count;
float r = 0.0f;
foreach (Vertex vert in verts)
{
r = Mathf.Max(r, Vector2.Distance(vert.p, centroid));
}
//Add some padding to avoid floating point errors later on
r *= 1.2f;
Vector2 pi0 = new Vector2(centroid.x, centroid.y - 2 * r);
Vector2 pi1 = new Vector2(centroid.x + r * Mathf.Sqrt(3), centroid.y + r);
Vector2 pi2 = new Vector2(centroid.x - r * Mathf.Sqrt(3), centroid.y + r);
Debug.DrawLine(pi0, pi1, Color.cyan, 5.0f, false);
Debug.DrawLine(pi1, pi2, Color.cyan, 5.0f, false);
Debug.DrawLine(pi2, pi0, Color.cyan, 5.0f, false);
Vertex vi0 = new Vertex(pi0, -1);
Vertex vi1 = new Vertex(pi1, -1);
Vertex vi2 = new Vertex(pi2, -1);
//Imaginary triangle
HalfEdge e01Imag = new HalfEdge(vi0);
HalfEdge e12Imag = new HalfEdge(vi1);
HalfEdge e20Imag = new HalfEdge(vi2);
Triangle treeRoot = new Triangle(e01Imag, e12Imag, e20Imag);
//Perform Delaunay Triangulation
for (int i = 0; i < verts.Count; i++)
{
Vertex v = verts[i];
Triangle containingTri = treeRoot.FindContainingTriangle(v.p);
//Check for potential degenerate case when point lies on edge of triangle
HalfEdge e01 = containingTri.edge;
HalfEdge e12 = e01.next;
HalfEdge e20 = e12.next;
Vector3 uvw = Geometry.ToBarycentricCoordinates(e01.origin.p, e12.origin.p, e20.origin.p, v.p);
HalfEdge[] edges;
if (uvw[0] < VecMath.epsilon)
{
edges = e12.InsertVertex(v);
}
else if (uvw[1] < VecMath.epsilon)
{
edges = e20.InsertVertex(v);
}
else if (uvw[2] < VecMath.epsilon)
{
edges = e01.InsertVertex(v);
}
else
{
edges = containingTri.InsertVertex(v);
}
//Flip triangles that don't satisfy delaunay property
HashSet <HalfEdge> sptSet = new HashSet <HalfEdge>();
LinkedList <HalfEdge> frontier = new LinkedList <HalfEdge>();
foreach (HalfEdge edge in edges)
{
frontier.AddLast(edge);
}
LinkedListNode <HalfEdge> curNode;
int t = 0;
while (frontier.Count > 0 && t < 100)
{
t += 1;
curNode = frontier.First;
frontier.RemoveFirst();
HalfEdge ab = curNode.Value;
HalfEdge ba = ab.twin;
if (!sptSet.Contains(ab) && ba != null)
{
HalfEdge bc = ab.next;
HalfEdge ca = bc.next;
HalfEdge ad = ba.next;
HalfEdge db = ad.next;
//Check if Delaunay Property is violated
if (Geometry.IsInCircumscribedCircle(ab.origin.p, bc.origin.p, ca.origin.p, db.origin.p))
{
//Flip triangle
HalfEdge dc = new HalfEdge(db.origin);
HalfEdge cd = new HalfEdge(ca.origin);
HalfEdge.SetTwins(dc, cd);
Triangle adc = new Triangle(ad, dc, ca);
Triangle bcd = new Triangle(bc, cd, db);
//Remove old edges from vertices
ab.origin.RemoveOutgoingEdge(ab);
ba.origin.RemoveOutgoingEdge(ba);
//Add new edges from flip to vertices
cd.origin.AddOutgoingEdge(cd);
dc.origin.AddOutgoingEdge(dc);
ab.incidentTriangle.children = new List <Triangle> {
adc, bcd
};
ba.incidentTriangle.children = new List <Triangle> {
adc, bcd
};
frontier.AddLast(ad);
frontier.AddLast(db);
}
sptSet.Add(ab);
sptSet.Add(ba);
}
}
}
//Insert constrained edges
foreach (ConstrainedVertex[] segments in constrainedVerts)
{
HashSet <HalfEdge> holeBounds = new HashSet <HalfEdge>();
HalfEdge holeEdge = null;
bool isHole = segments.Length > 2;
int n = isHole ? segments.Length : segments.Length - 1;
for (int i = 0; i < n; i++)
{
Vertex v1 = segments[i % segments.Length];
Vertex v2 = segments[(i + 1) % segments.Length];
Debug.DrawLine(v1.p, v2.p, Color.magenta, 5.0f, false);
Vector2 dir = v2.p - v1.p;
HalfEdge eStart = v1.GetOutgoingEdgeClockwiseFrom(dir);
HalfEdge eEnd = v2.GetOutgoingEdgeClockwiseFrom(-dir);
List <HalfEdge> edgePortals = new List <HalfEdge>();
HalfEdge intersected = eStart.next.twin;
Vertex v = v1;
int t = 0;
while (v != v2 && t < 100)
{
t += 1;
edgePortals.Add(intersected);
v = intersected.prev.origin;
Vector2 newDir = v.p - v1.p;
if (VecMath.Det(dir, newDir) >= 0)
{
intersected = intersected.next.twin;
}
else
{
intersected = intersected.prev.twin;
}
}
List <HalfEdge> forwardEdgePortals = new List <HalfEdge>();
forwardEdgePortals.Add(eStart);
for (int j = 0; j < edgePortals.Count; j++)
{
forwardEdgePortals.Add(edgePortals[j]);
}
List <HalfEdge> backwardEdgePortals = new List <HalfEdge>();
backwardEdgePortals.Add(eEnd);
for (int j = edgePortals.Count - 1; j >= 0; j--)
{
backwardEdgePortals.Add(edgePortals[j].twin);
}
int sL = 0;
HalfEdge eConstrainedL = PolygonTriangulation(ref sL, forwardEdgePortals);
holeBounds.Add(eConstrainedL);
int sR = 0;
HalfEdge eConstrainedR = PolygonTriangulation(ref sR, backwardEdgePortals);
holeEdge = eConstrainedR;
HalfEdge.SetTwins(eConstrainedL, eConstrainedR);
foreach (HalfEdge e in edgePortals)
{
Vector2 ep1 = e.origin.p;
Vector2 ep2 = e.next.origin.p;
Debug.DrawLine(ep1, ep2, Color.green, 5.0f, false);
}
//v1.DrawOutgoingEdges();
//Debug.DrawLine(eStart.origin.p, eStart.next.origin.p, Color.red, 5.0f, false);
}
if (isHole && holeEdge != null)
{
//We have a hole. Hide all triangles that are in hole
CreateHole(holeEdge, holeBounds);
}
}
//Generate Triangle list
List <Triangle> leafs = new List <Triangle>();
HashSet <Triangle> visited = new HashSet <Triangle>();
int count = 0;
treeRoot.GetRealLeafs(ref leafs, ref visited, ref count, 0);
Debug.Log(count);
foreach (Triangle leaf in leafs)
{
Vector3 p0 = leaf.edge.origin.p;
Vector3 p1 = leaf.edge.next.origin.p;
Vector3 p2 = leaf.edge.next.next.origin.p;
Debug.DrawLine(p0, p1, Color.cyan, 5.0f, false);
Debug.DrawLine(p1, p2, Color.cyan, 5.0f, false);
Debug.DrawLine(p2, p0, Color.cyan, 5.0f, false);
}
return(leafs.ToArray());
}
