Added pathfinding (a*) and string pulling

README.md

@@ -24,6 +24,30 @@ The library works with Haxe 2 and 3, and has been tested on OpenFl's flash, html
     haxe -cp ../src -main me.nerik.poly2trihx.NekoDemo -neko build/nekoDemo.n
     neko build/nekoDemo.n
 
+**How to use it :**
+
+```haxe
+//Imports
+import org.poly2tri.VisiblePolygon;
+import org.poly2tri.Point;
+import org.poly2tri.utils.TriangleCell;
+import org.poly2tri.utils.AStar;
+import org.poly2tri.utils.Funnel;
+
+//...
+//Triangulation
+var vp:VisiblePolygon = new VisiblePolygon();
+vp.addPolyline( poly ); //poly and hole = Array<org.poly2tri.Point>
+vp.addPolyline( hole );
+
+vp.performTriangulationOnce();
+var graph:Array<TriangleCell> = vp.getGraph();
+
+//...
+//Pathfinding and string pulling
+var channel:Array<TriangleCell> = AStar.find(startTriangle,endTriangle,graph); //startTriangle and endTriangle = org.poly2tri.TriangleCell
+var path:Array<Point> = Funnel.stringPull(startPoint,endPoint, channel); //startPoint and endPoint = org.poly2tri.Point
+```
 
 ## License
 

src/org/poly2tri/Edge.hx

@@ -37,6 +37,10 @@ class Edge
 		q.edge_list.push( this );
 	}
 
+	public function hasPoint(point:Point):Bool {
+			return p.equals(point) || q.equals(point);
+	}
+
 
 
 	public function toString() 

src/org/poly2tri/Triangle.hx

@@ -110,6 +110,35 @@ class Triangle
 		}
 	}
 
+	static public function getNotCommonVertex(t1:Triangle, t2:Triangle):Point {
+		return t1.points[Triangle.getNotCommonVertexIndex(t1, t2)];
+	}
+
+
+	static public function getNotCommonVertexIndex(t1:Triangle, t2:Triangle):Int {
+			var sum:Int = 0, index:Int = -1;
+			if (!t2.containsPoint(t1.points[0])) { index = 0; sum++; }
+			if (!t2.containsPoint(t1.points[1])) { index = 1; sum++; }
+			if (!t2.containsPoint(t1.points[2])) { index = 2; sum++; }
+			if (sum != 1) {
+				throw "Triangle::Triangles not adjacent";
+			}
+			return index;
+	}
+
+	static public function getCommonEdge(t1:Triangle, t2:Triangle):Edge {
+		var commonIndexes:Array<Point> = new Array<Point>();
+		for(point in t1.points) {
+			if (t2.containsPoint(point)) {				
+				commonIndexes.push(point);
+			}
+		}
+		if (commonIndexes.length != 2) {
+			throw "Triangle::Triangles not adjacent";		
+		};
+		return new Edge(commonIndexes[0], commonIndexes[1]);
+	}
+
 
 
 	// Optimized?
@@ -125,12 +154,6 @@ class Triangle
 		}
 
 		throw "Triangle::Point not in triangle";
-		// for (var n:uint = 0; n < 3; n++, no++) {
-		// 	while (no < 0) no += 3;
-		// 	while (no > 2) no -= 3;
-		// 	if (p.equals(this.points[n])) return no;
-		// }
-		// throw(new Error("Point not in triangle"));
 	}
 
 

src/org/poly2tri/VisiblePolygon.hx

@@ -1,5 +1,7 @@
 package org.poly2tri;
 
+import org.poly2tri.Point;
+import org.poly2tri.utils.TriangleCell;
 
 class VisiblePolygon
 {
@@ -44,6 +46,17 @@ class VisiblePolygon
 		sweep.triangulate();
 	}
 
+	//get graph of triangles with neighbours and all
+	public function getGraph():Array<TriangleCell> {
+		var graph:Array<TriangleCell> = new Array<TriangleCell>();
+		for (t in sweepContext.triangles){
+			var triangle:TriangleCell = new TriangleCell(t);
+			triangle.setNeighbours(graph);
+			graph.push(triangle);
+		}
+		return graph;
+	}
+
 	//returns vertices in a 3D engine-friendly, XYZ format
 	public function getVerticesAndTriangles() 
 	{

src/org/poly2tri/utils/AStar.hx

@@ -0,0 +1,94 @@
+package org.poly2tri.utils;
+
+import org.poly2tri.utils.TriangleCell;
+
+
+/**
+ * ...
+ * @author LunaFromTheMoon
+ */
+
+
+class AStar
+{
+
+	private function new() 
+	{
+
+	}
+
+	private static function getPath(last:TriangleCell):Array<TriangleCell>	{
+		//trace the path by going backwards from goal node to start node
+		var path:Array<TriangleCell> = [last];
+		var node:TriangleCell = last.prevCell;
+		while (node != null)
+		{
+			path.unshift(node);
+			node = node.prevCell;
+		}
+		return path;
+	}
+
+	private static function cleanGraph(map:Array<TriangleCell>) {
+		for (node in map) {
+			node.prevCell = null;
+			node.marked = false;
+			node.distance = 0;
+		}
+	}
+
+	private static function sortByHeuristic(nodes:Array<TriangleCell>, last:TriangleCell):Array<TriangleCell> {
+		nodes.sort( function(a:TriangleCell, b:TriangleCell):Int {
+				var aF:Float = a.distance + a.heuristicTo(last);
+				var bF:Float = b.distance + b.heuristicTo(last);
+				if (aF < bF) return -1;
+				if (aF > bF) return 1;
+				return 0;
+			} );
+		return nodes;
+	}
+
+	/**
+	 *      This function does very little.
+	 *      @param map The graph
+	 *      @param startNode The first node in the path
+	 *      @param goalNode The last node in the path
+	 *      @return The path if found, else null
+	 **/
+	public static function find(startNode:TriangleCell, goalNode:TriangleCell,map:Array<TriangleCell>):Array<TriangleCell> {
+		cleanGraph(map);
+		var queue:Array<TriangleCell> = new Array<TriangleCell>();
+		queue.push(startNode);
+		while (!(queue.length == 0)) {
+			//obtain the first node (with smallest aproximate distance to goal)
+			var currentNode:TriangleCell = queue[0];
+			queue.splice(0, 1);
+			if (!currentNode.marked) {
+				currentNode.marked = true;
+				//check if we're in goal
+				if (currentNode == goalNode) {
+					return getPath(goalNode);
+				}
+				//try to get there through the neighbours
+				for (next in currentNode.neighbours) {
+					//process only new nodes
+					if (!next.marked) {
+						var distance = currentNode.distance + (currentNode.distanceTo(next) * currentNode.weightTo(next));
+						if (next.prevCell == null || distance < next.distance) {
+							//node not in queue, or in queue but with longer distance							
+							next.prevCell = currentNode;
+							next.distance = distance;							
+						}
+						//if not in queue, add
+						if (queue.indexOf(next) == -1) {
+							queue.push(next);
+						}
+					}
+				}
+				//sort queue by aproximate shortest distance to goal node
+				queue = sortByHeuristic(queue, goalNode);
+			}
+		}
+		return null;
+	}
+}

src/org/poly2tri/utils/Funnel.hx

@@ -0,0 +1,146 @@
+package org.poly2tri.utils;
+
+import org.poly2tri.Point;
+import org.poly2tri.utils.PointUtils;
+import org.poly2tri.utils.TriangleCell;
+
+/**
+ * ...
+ * @author LunaFromTheMoon
+ */
+class Funnel
+{
+
+	private function new() 
+	{
+
+	}
+
+	public static function channelToPortals(startPoint:Point, endPoint:Point, channel:Array<TriangleCell>):Array<Portal> {
+		var portals:Array<Portal> = new Array<Portal>();
+		portals.push(new Portal(startPoint, startPoint));
+		if (channel.length >= 2) {
+			var firstTriangle:Triangle = channel[0].triangle;
+			var secondTriangle:Triangle = channel[1].triangle;
+			var lastTriangle:Triangle  = channel[channel.length - 1].triangle;
+			var startVertex:Point = Triangle.getNotCommonVertex(firstTriangle, secondTriangle);
+			var vertexCW0:Point = startVertex;
+			var vertexCCW0:Point = startVertex;
+			for (n in 0...(channel.length-1)) {
+				var triangleCurrent:Triangle = channel[n + 0].triangle;
+				var triangleNext:Triangle    = channel[n + 1].triangle;
+				var commonEdge:Edge  = Triangle.getCommonEdge(triangleCurrent, triangleNext);
+				var vertexCW1:Point  = triangleCurrent.pointCW (vertexCW0 );
+				var vertexCCW1:Point = triangleCurrent.pointCCW(vertexCCW0);
+				if (!commonEdge.hasPoint(vertexCW0)) {
+						vertexCW0 = vertexCW1;
+				}
+				if (!commonEdge.hasPoint(vertexCCW0)) {
+						vertexCCW0 = vertexCCW1;
+				}
+				portals.push(new Portal(vertexCW0, vertexCCW0));
+			}
+		}	   
+		portals.push(new Portal(endPoint, endPoint));
+		return portals;
+	}
+
+	public static function stringPull(startPoint:Point, endPoint:Point, channel:Array<TriangleCell>):Array<Point> {
+		var pts:Array<Point> = new Array<Point>();
+		// Init scan state			
+		var portals:Array<Portal> = channelToPortals(startPoint, endPoint, channel);
+		var portalApex:Point = portals[0].left;
+		var portalLeft:Point  = portals[0].left;
+		var portalRight:Point = portals[0].right;
+		var apexIndex:Int = 0, leftIndex:Int = 0, rightIndex:Int = 0;
+
+		// Add start point.
+		pts.push(portalApex);
+		var i:Int = 1;
+		while (i < portals.length) {
+			var left:Point  = portals[i].left;
+			var right:Point = portals[i].right;
+			// Update right vertex.
+			if (PointUtils.triarea2(portalApex, portalRight, right) <= 0.0) {				
+				if (PointUtils.vequal(portalApex, portalRight) || PointUtils.triarea2(portalApex, portalLeft, right) > 0.0) {
+					// Tighten the funnel.
+					portalRight = right;
+					rightIndex = i;
+				} else {
+					// Right over left, insert left to path and restart scan from portal left point.
+					pts.push(portalLeft);
+					// Make current left the new apex.
+					portalApex = portalLeft;
+					apexIndex = getNextPortalIndex(portals,leftIndex,portalApex);
+					// Reset portal
+					portalLeft = portalApex;
+					portalRight = portalApex;
+					leftIndex = apexIndex;
+					rightIndex = apexIndex;
+					// Restart scan
+					i = apexIndex;
+					continue;
+				}
+			}
+
+			// Update left vertex.
+			if (PointUtils.triarea2(portalApex, portalLeft, left) >= 0.0) {
+				if (PointUtils.vequal(portalApex, portalLeft) || PointUtils.triarea2(portalApex, portalRight, left) < 0.0) {
+					// Tighten the funnel.
+					portalLeft = left;
+					leftIndex = i;
+				} else {
+					// Left over right, insert right to path and restart scan from portal right point.
+					pts.push(portalRight);
+					// Make current right the new apex.
+					portalApex = portalRight;
+					apexIndex = getNextPortalIndex(portals,rightIndex,portalApex);
+					// Reset portal
+					portalLeft = portalApex;
+					portalRight = portalApex;
+					leftIndex = apexIndex;
+					rightIndex = apexIndex;
+					// Restart scan
+					i = apexIndex;
+					continue;
+				}
+			}	
+			i++;
+		}
+		if ((pts.length == 0) || (!PointUtils.vequal(pts[pts.length - 1], portals[portals.length - 1].left))) {
+			// Append last point to path.
+			pts.push(portals[portals.length - 1].left);
+		}
+		return pts;
+	}
+
+	//search next portal that doesn't contain the apex
+	private static function getNextPortalIndex(portals:Array<Portal>, apexIndex:Int, portalApex:Point):Int {
+		var i:Int = apexIndex;
+		while (i < portals.length && portals[i].contains(portalApex)) {
+			i++;
+		}
+		return i;
+	}
+}
+
+
+
+private class Portal {
+	public var left:Point;
+	public var right:Point;
+
+	public function new(left:Point, right:Point) 
+	{
+		this.left = left;
+		this.right = right;
+	}
+
+	public function toString() {
+		return this.left.toString() + ".." + this.right.toString();
+	}
+
+	public function contains(point:Point) {
+		return this.left.equals(point) || this.right.equals(point);
+	}
+}

src/org/poly2tri/utils/PointUtils.hx

@@ -0,0 +1,35 @@
+package org.poly2tri.utils;
+
+import org.poly2tri.Point;
+
+/**
+ * ...
+ * @author LunaFromTheMoon
+ */
+class PointUtils
+{
+
+	private function new() 
+	{
+
+	}
+
+	static public function vdistsqr(a:Point, b:Point):Float {
+		var x:Float = b.x - a.x;
+		var y:Float = b.y - a.y;
+		return Math.sqrt(x * x + y * y);
+	}
+
+	static public function vequal(a:Point, b:Point):Bool {
+		return vdistsqr(a, b) < (0.001 * 0.001);
+	}
+
+	static public function triarea2(a:Point, b:Point, c:Point):Float {
+		var ax:Float = b.x - a.x;
+		var ay:Float = b.y - a.y;
+		var bx:Float = c.x - a.x;
+		var by:Float = c.y - a.y;
+		return bx * ay - ax * by;
+	}
+
+}