New Navigation & Pathfinding support for 2D

-Added Navigation & NavigationPolygon nodes
-Added corresponding visual editor
-New pathfinding algorithm is modern and fast!
-Similar API to 3D Pathfinding (more coherent)

core/bind/core_bind.cpp

@@ -838,6 +838,12 @@ Variant _Geometry::segment_intersects_triangle( const Vector3& p_from, const Vec
 		return Variant();
 
 }
+
+bool _Geometry::point_is_inside_triangle(const Vector2& s, const Vector2& a, const Vector2& b, const Vector2& c) const {
+
+	return Geometry::is_point_in_triangle(s,a,b,c);
+}
+
 DVector<Vector3> _Geometry::segment_intersects_sphere( const Vector3& p_from, const Vector3& p_to, const Vector3& p_sphere_pos,real_t p_sphere_radius) {
 
 	DVector<Vector3> r;
@@ -938,6 +944,7 @@ void _Geometry::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("segment_intersects_sphere","from","to","spos","sradius"),&_Geometry::segment_intersects_sphere);
 	ObjectTypeDB::bind_method(_MD("segment_intersects_cylinder","from","to","height","radius"),&_Geometry::segment_intersects_cylinder);
 	ObjectTypeDB::bind_method(_MD("segment_intersects_convex","from","to","planes"),&_Geometry::segment_intersects_convex);
+	ObjectTypeDB::bind_method(_MD("point_is_inside_triangle","point","a","b","c"),&_Geometry::point_is_inside_triangle);
 
 	ObjectTypeDB::bind_method(_MD("triangulate_polygon","polygon"),&_Geometry::triangulate_polygon);
 

core/bind/core_bind.h

@@ -248,6 +248,8 @@ public:
 	Vector3 get_closest_point_to_segment(const Vector3& p_point, const Vector3& p_a,const Vector3& p_b);
 	Variant ray_intersects_triangle( const Vector3& p_from, const Vector3& p_dir, const Vector3& p_v0,const Vector3& p_v1,const Vector3& p_v2);
 	Variant segment_intersects_triangle( const Vector3& p_from, const Vector3& p_to, const Vector3& p_v0,const Vector3& p_v1,const Vector3& p_v2);
+	bool point_is_inside_triangle(const Vector2& s, const Vector2& a, const Vector2& b, const Vector2& c) const;
+
 	DVector<Vector3> segment_intersects_sphere( const Vector3& p_from, const Vector3& p_to, const Vector3& p_sphere_pos,real_t p_sphere_radius);
 	DVector<Vector3> segment_intersects_cylinder( const Vector3& p_from, const Vector3& p_to, float p_height,float p_radius);
 	DVector<Vector3> segment_intersects_convex(const Vector3& p_from, const Vector3& p_to,const Vector<Plane>& p_planes);

core/dvector.h

@@ -262,6 +262,23 @@ public:
 			w[bs+i]=r[i];
 	}
 
+
+	Error insert(int p_pos,const T& p_val) {
+
+		int s=size();
+		ERR_FAIL_INDEX_V(p_pos,s+1,ERR_INVALID_PARAMETER);
+		resize(s+1);
+		{
+			Write w = write();
+			for (int i=s;i>p_pos;i--)
+				w[i]=w[i-1];
+			w[p_pos]=p_val;
+		}
+
+		return OK;
+	}
+
+
 	bool is_locked() const { return mem.is_locked(); }
 	
 	inline const T operator[](int p_index) const;

core/math/geometry.h

@@ -511,6 +511,20 @@ public:
 		else
 			return p_segment[0]+n*d; // inside
 	}
+
+	static bool is_point_in_triangle(const Vector2& s, const Vector2& a, const Vector2& b, const Vector2& c)
+	{
+	    int as_x = s.x-a.x;
+	    int as_y = s.y-a.y;
+
+	    bool s_ab = (b.x-a.x)*as_y-(b.y-a.y)*as_x > 0;
+
+	    if((c.x-a.x)*as_y-(c.y-a.y)*as_x > 0 == s_ab) return false;
+
+	    if((c.x-b.x)*(s.y-b.y)-(c.y-b.y)*(s.x-b.x) > 0 != s_ab) return false;
+
+	    return true;
+	}
 	static Vector2 get_closest_point_to_segment_uncapped_2d(const Vector2& p_point, const Vector2 *p_segment) {
 
 		Vector2 p=p_point-p_segment[0];

core/math/triangulator.cpp

@@ -0,0 +1,1543 @@
+//Copyright (C) 2011 by Ivan Fratric
+//
+//Permission is hereby granted, free of charge, to any person obtaining a copy
+//of this software and associated documentation files (the "Software"), to deal
+//in the Software without restriction, including without limitation the rights
+//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//copies of the Software, and to permit persons to whom the Software is
+//furnished to do so, subject to the following conditions:
+//
+//The above copyright notice and this permission notice shall be included in
+//all copies or substantial portions of the Software.
+//
+//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//THE SOFTWARE.
+
+
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <algorithm>
+#include "triangulator.h"
+using namespace std;
+
+#define TRIANGULATOR_VERTEXTYPE_REGULAR 0
+#define TRIANGULATOR_VERTEXTYPE_START 1
+#define TRIANGULATOR_VERTEXTYPE_END 2
+#define TRIANGULATOR_VERTEXTYPE_SPLIT 3
+#define TRIANGULATOR_VERTEXTYPE_MERGE 4
+
+TriangulatorPoly::TriangulatorPoly() {
+	hole = false;
+	numpoints = 0;
+	points = NULL;
+}
+
+TriangulatorPoly::~TriangulatorPoly() {
+	if(points) delete [] points;
+}
+
+void TriangulatorPoly::Clear() {
+	if(points) delete [] points;
+	hole = false;
+	numpoints = 0;
+	points = NULL;
+}
+
+void TriangulatorPoly::Init(long numpoints) {
+	Clear();
+	this->numpoints = numpoints;
+	points = new Vector2[numpoints];
+}
+
+void TriangulatorPoly::Triangle(Vector2 &p1, Vector2 &p2, Vector2 &p3) {
+	Init(3);
+	points[0] = p1;
+	points[1] = p2;
+	points[2] = p3;
+}
+
+TriangulatorPoly::TriangulatorPoly(const TriangulatorPoly &src) {
+	hole = src.hole;
+	numpoints = src.numpoints;
+	points = new Vector2[numpoints];
+	memcpy(points, src.points, numpoints*sizeof(Vector2));
+}
+
+TriangulatorPoly& TriangulatorPoly::operator=(const TriangulatorPoly &src) {
+	Clear();
+	hole = src.hole;
+	numpoints = src.numpoints;
+	points = new Vector2[numpoints];
+	memcpy(points, src.points, numpoints*sizeof(Vector2));
+	return *this;
+}
+
+int TriangulatorPoly::GetOrientation() {
+	long i1,i2;
+	real_t area = 0;
+	for(i1=0; i1<numpoints; i1++) {
+		i2 = i1+1;
+		if(i2 == numpoints) i2 = 0;
+		area += points[i1].x * points[i2].y - points[i1].y * points[i2].x;
+	}
+	if(area>0) return TRIANGULATOR_CCW;
+	if(area<0) return TRIANGULATOR_CW;
+	return 0;
+}
+
+void TriangulatorPoly::SetOrientation(int orientation) {
+	int polyorientation = GetOrientation();
+	if(polyorientation&&(polyorientation!=orientation)) {
+		Invert();
+	}
+}
+
+void TriangulatorPoly::Invert() {
+	long i;
+	Vector2 *invpoints;
+
+	invpoints = new Vector2[numpoints];
+	for(i=0;i<numpoints;i++) {
+		invpoints[i] = points[numpoints-i-1];
+	}
+
+	delete [] points;
+	points = invpoints;
+}
+
+Vector2 TriangulatorPartition::Normalize(const Vector2 &p) {
+	Vector2 r;
+	real_t n = sqrt(p.x*p.x + p.y*p.y);
+	if(n!=0) {
+		r = p/n;
+	} else {
+		r.x = 0;
+		r.y = 0;
+	}
+	return r;
+}
+
+real_t TriangulatorPartition::Distance(const Vector2 &p1, const Vector2 &p2) {
+	real_t dx,dy;
+	dx = p2.x - p1.x;
+	dy = p2.y - p1.y;
+	return(sqrt(dx*dx + dy*dy));
+}
+
+//checks if two lines intersect
+int TriangulatorPartition::Intersects(Vector2 &p11, Vector2 &p12, Vector2 &p21, Vector2 &p22) {
+	if((p11.x == p21.x)&&(p11.y == p21.y)) return 0;
+	if((p11.x == p22.x)&&(p11.y == p22.y)) return 0;
+	if((p12.x == p21.x)&&(p12.y == p21.y)) return 0;
+	if((p12.x == p22.x)&&(p12.y == p22.y)) return 0;
+
+	Vector2 v1ort,v2ort,v;
+	real_t dot11,dot12,dot21,dot22;
+
+	v1ort.x = p12.y-p11.y;
+	v1ort.y = p11.x-p12.x;
+
+	v2ort.x = p22.y-p21.y;
+	v2ort.y = p21.x-p22.x;
+
+	v = p21-p11;
+	dot21 = v.x*v1ort.x + v.y*v1ort.y;
+	v = p22-p11;
+	dot22 = v.x*v1ort.x + v.y*v1ort.y;
+
+	v = p11-p21;
+	dot11 = v.x*v2ort.x + v.y*v2ort.y;
+	v = p12-p21;
+	dot12 = v.x*v2ort.x + v.y*v2ort.y;
+
+	if(dot11*dot12>0) return 0;
+	if(dot21*dot22>0) return 0;
+
+	return 1;
+}
+
+//removes holes from inpolys by merging them with non-holes
+int TriangulatorPartition::RemoveHoles(list<TriangulatorPoly> *inpolys, list<TriangulatorPoly> *outpolys) {
+	list<TriangulatorPoly> polys;
+	list<TriangulatorPoly>::iterator holeiter,polyiter,iter,iter2;
+	long i,i2,holepointindex,polypointindex;
+	Vector2 holepoint,polypoint,bestpolypoint;
+	Vector2 linep1,linep2;
+	Vector2 v1,v2;
+	TriangulatorPoly newpoly;
+	bool hasholes;
+	bool pointvisible;
+	bool pointfound;
+
+	//check for trivial case (no holes)
+	hasholes = false;
+	for(iter = inpolys->begin(); iter!=inpolys->end(); iter++) {
+		if(iter->IsHole()) {
+			hasholes = true;
+			break;
+		}
+	}
+	if(!hasholes) {
+		for(iter = inpolys->begin(); iter!=inpolys->end(); iter++) {
+			outpolys->push_back(*iter);
+		}
+		return 1;
+	}
+
+	polys = *inpolys;
+
+	while(1) {
+		//find the hole point with the largest x
+		hasholes = false;
+		for(iter = polys.begin(); iter!=polys.end(); iter++) {
+			if(!iter->IsHole()) continue;
+
+			if(!hasholes) {
+				hasholes = true;
+				holeiter = iter;
+				holepointindex = 0;
+			}
+
+			for(i=0; i < iter->GetNumPoints(); i++) {
+				if(iter->GetPoint(i).x > holeiter->GetPoint(holepointindex).x) {
+					holeiter = iter;
+					holepointindex = i;
+				}
+			}
+		}
+		if(!hasholes) break;
+		holepoint = holeiter->GetPoint(holepointindex);
+
+		pointfound = false;
+		for(iter = polys.begin(); iter!=polys.end(); iter++) {
+			if(iter->IsHole()) continue;
+			for(i=0; i < iter->GetNumPoints(); i++) {
+				if(iter->GetPoint(i).x <= holepoint.x) continue;
+				if(!InCone(iter->GetPoint((i+iter->GetNumPoints()-1)%(iter->GetNumPoints())),
+					   iter->GetPoint(i),
+					   iter->GetPoint((i+1)%(iter->GetNumPoints())),
+					   holepoint))
+					continue;
+				polypoint = iter->GetPoint(i);
+				if(pointfound) {
+					v1 = Normalize(polypoint-holepoint);
+					v2 = Normalize(bestpolypoint-holepoint);
+					if(v2.x > v1.x) continue;
+				}
+				pointvisible = true;
+				for(iter2 = polys.begin(); iter2!=polys.end(); iter2++) {
+					if(iter2->IsHole()) continue;
+					for(i2=0; i2 < iter2->GetNumPoints(); i2++) {
+						linep1 = iter2->GetPoint(i2);
+						linep2 = iter2->GetPoint((i2+1)%(iter2->GetNumPoints()));
+						if(Intersects(holepoint,polypoint,linep1,linep2)) {
+							pointvisible = false;
+							break;
+						}
+					}
+					if(!pointvisible) break;
+				}
+				if(pointvisible) {
+					pointfound = true;
+					bestpolypoint = polypoint;
+					polyiter = iter;
+					polypointindex = i;
+				}
+			}
+		}
+
+		if(!pointfound) return 0;
+
+		newpoly.Init(holeiter->GetNumPoints() + polyiter->GetNumPoints() + 2);
+		i2 = 0;
+		for(i=0;i<=polypointindex;i++) {
+			newpoly[i2] = polyiter->GetPoint(i);
+			i2++;
+		}
+		for(i=0;i<=holeiter->GetNumPoints();i++) {
+			newpoly[i2] = holeiter->GetPoint((i+holepointindex)%holeiter->GetNumPoints());
+			i2++;
+		}
+		for(i=polypointindex;i<polyiter->GetNumPoints();i++) {
+			newpoly[i2] = polyiter->GetPoint(i);
+			i2++;
+		}
+
+		polys.erase(holeiter);
+		polys.erase(polyiter);
+		polys.push_back(newpoly);
+	}
+
+	for(iter = polys.begin(); iter!=polys.end(); iter++) {
+		outpolys->push_back(*iter);
+	}
+
+	return 1;
+}
+
+bool TriangulatorPartition::IsConvex(Vector2& p1, Vector2& p2, Vector2& p3) {
+	real_t tmp;
+	tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y);
+	if(tmp>0) return 1;
+	else return 0;
+}
+
+bool TriangulatorPartition::IsReflex(Vector2& p1, Vector2& p2, Vector2& p3) {
+	real_t tmp;
+	tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y);
+	if(tmp<0) return 1;
+	else return 0;
+}
+
+bool TriangulatorPartition::IsInside(Vector2& p1, Vector2& p2, Vector2& p3, Vector2 &p) {
+	if(IsConvex(p1,p,p2)) return false;
+	if(IsConvex(p2,p,p3)) return false;
+	if(IsConvex(p3,p,p1)) return false;
+	return true;
+}
+
+bool TriangulatorPartition::InCone(Vector2 &p1, Vector2 &p2, Vector2 &p3, Vector2 &p) {
+	bool convex;
+
+	convex = IsConvex(p1,p2,p3);
+
+	if(convex) {
+		if(!IsConvex(p1,p2,p)) return false;
+		if(!IsConvex(p2,p3,p)) return false;
+		return true;
+	} else {
+		if(IsConvex(p1,p2,p)) return true;
+		if(IsConvex(p2,p3,p)) return true;
+		return false;
+	}
+}
+
+bool TriangulatorPartition::InCone(PartitionVertex *v, Vector2 &p) {
+	Vector2 p1,p2,p3;
+
+	p1 = v->previous->p;
+	p2 = v->p;
+	p3 = v->next->p;
+
+	return InCone(p1,p2,p3,p);
+}
+
+void TriangulatorPartition::UpdateVertexReflexity(PartitionVertex *v) {
+	PartitionVertex *v1,*v3;
+	v1 = v->previous;
+	v3 = v->next;
+	v->isConvex = !IsReflex(v1->p,v->p,v3->p);
+}
+
+void TriangulatorPartition::UpdateVertex(PartitionVertex *v, PartitionVertex *vertices, long numvertices) {
+	long i;
+	PartitionVertex *v1,*v3;
+	Vector2 vec1,vec3;
+
+	v1 = v->previous;
+	v3 = v->next;
+
+	v->isConvex = IsConvex(v1->p,v->p,v3->p);
+
+	vec1 = Normalize(v1->p - v->p);
+	vec3 = Normalize(v3->p - v->p);
+	v->angle = vec1.x*vec3.x + vec1.y*vec3.y;
+
+	if(v->isConvex) {
+		v->isEar = true;
+		for(i=0;i<numvertices;i++) {
+			if((vertices[i].p.x==v->p.x)&&(vertices[i].p.y==v->p.y)) continue;
+			if((vertices[i].p.x==v1->p.x)&&(vertices[i].p.y==v1->p.y)) continue;
+			if((vertices[i].p.x==v3->p.x)&&(vertices[i].p.y==v3->p.y)) continue;
+			if(IsInside(v1->p,v->p,v3->p,vertices[i].p)) {
+				v->isEar = false;
+				break;
+			}
+		}
+	} else {
+		v->isEar = false;
+	}
+}
+
+//triangulation by ear removal
+int TriangulatorPartition::Triangulate_EC(TriangulatorPoly *poly, list<TriangulatorPoly> *triangles) {
+	long numvertices;
+	PartitionVertex *vertices;
+	PartitionVertex *ear;
+	TriangulatorPoly triangle;
+	long i,j;
+	bool earfound;
+
+	if(poly->GetNumPoints() < 3) return 0;
+	if(poly->GetNumPoints() == 3) {
+		triangles->push_back(*poly);
+		return 1;
+	}
+
+	numvertices = poly->GetNumPoints();
+
+	vertices = new PartitionVertex[numvertices];
+	for(i=0;i<numvertices;i++) {
+		vertices[i].isActive = true;
+		vertices[i].p = poly->GetPoint(i);
+		if(i==(numvertices-1)) vertices[i].next=&(vertices[0]);
+		else vertices[i].next=&(vertices[i+1]);
+		if(i==0) vertices[i].previous = &(vertices[numvertices-1]);
+		else vertices[i].previous = &(vertices[i-1]);
+	}
+	for(i=0;i<numvertices;i++) {
+		UpdateVertex(&vertices[i],vertices,numvertices);
+	}
+
+	for(i=0;i<numvertices-3;i++) {
+		earfound = false;
+		//find the most extruded ear
+		for(j=0;j<numvertices;j++) {
+			if(!vertices[j].isActive) continue;
+			if(!vertices[j].isEar) continue;
+			if(!earfound) {
+				earfound = true;
+				ear = &(vertices[j]);
+			} else {
+				if(vertices[j].angle > ear->angle) {
+					ear = &(vertices[j]);
+				}
+			}
+		}
+		if(!earfound) {
+			delete [] vertices;
+			return 0;
+		}
+
+		triangle.Triangle(ear->previous->p,ear->p,ear->next->p);
+		triangles->push_back(triangle);
+
+		ear->isActive = false;
+		ear->previous->next = ear->next;
+		ear->next->previous = ear->previous;
+
+		if(i==numvertices-4) break;
+
+		UpdateVertex(ear->previous,vertices,numvertices);
+		UpdateVertex(ear->next,vertices,numvertices);
+	}
+	for(i=0;i<numvertices;i++) {
+		if(vertices[i].isActive) {
+			triangle.Triangle(vertices[i].previous->p,vertices[i].p,vertices[i].next->p);
+			triangles->push_back(triangle);
+			break;
+		}
+	}
+
+	delete [] vertices;
+
+	return 1;
+}
+
+int TriangulatorPartition::Triangulate_EC(list<TriangulatorPoly> *inpolys, list<TriangulatorPoly> *triangles) {
+	list<TriangulatorPoly> outpolys;
+	list<TriangulatorPoly>::iterator iter;
+
+	if(!RemoveHoles(inpolys,&outpolys)) return 0;
+	for(iter=outpolys.begin();iter!=outpolys.end();iter++) {
+		if(!Triangulate_EC(&(*iter),triangles)) return 0;
+	}
+	return 1;
+}
+
+int TriangulatorPartition::ConvexPartition_HM(TriangulatorPoly *poly, list<TriangulatorPoly> *parts) {
+	list<TriangulatorPoly> triangles;
+	list<TriangulatorPoly>::iterator iter1,iter2;
+	TriangulatorPoly *poly1,*poly2;
+	TriangulatorPoly newpoly;
+	Vector2 d1,d2,p1,p2,p3;
+	long i11,i12,i21,i22,i13,i23,j,k;
+	bool isdiagonal;
+	long numreflex;
+
+	//check if the poly is already convex
+	numreflex = 0;
+	for(i11=0;i11<poly->GetNumPoints();i11++) {
+		if(i11==0) i12 = poly->GetNumPoints()-1;
+		else i12=i11-1;
+		if(i11==(poly->GetNumPoints()-1)) i13=0;
+		else i13=i11+1;
+		if(IsReflex(poly->GetPoint(i12),poly->GetPoint(i11),poly->GetPoint(i13))) {
+			numreflex = 1;
+			break;
+		}
+	}
+	if(numreflex == 0) {
+		parts->push_back(*poly);
+		return 1;
+	}
+
+	if(!Triangulate_EC(poly,&triangles)) return 0;
+
+	for(iter1 = triangles.begin(); iter1 != triangles.end(); iter1++) {
+		poly1 = &(*iter1);
+		for(i11=0;i11<poly1->GetNumPoints();i11++) {
+			d1 = poly1->GetPoint(i11);
+			i12 = (i11+1)%(poly1->GetNumPoints());
+			d2 = poly1->GetPoint(i12);
+
+			isdiagonal = false;
+			for(iter2 = iter1; iter2 != triangles.end(); iter2++) {
+				if(iter1 == iter2) continue;
+				poly2 = &(*iter2);
+
+				for(i21=0;i21<poly2->GetNumPoints();i21++) {
+					if((d2.x != poly2->GetPoint(i21).x)||(d2.y != poly2->GetPoint(i21).y)) continue;
+					i22 = (i21+1)%(poly2->GetNumPoints());
+					if((d1.x != poly2->GetPoint(i22).x)||(d1.y != poly2->GetPoint(i22).y)) continue;
+					isdiagonal = true;
+					break;
+				}
+				if(isdiagonal) break;
+			}
+
+			if(!isdiagonal) continue;
+
+			p2 = poly1->GetPoint(i11);
+			if(i11 == 0) i13 = poly1->GetNumPoints()-1;
+			else i13 = i11-1;
+			p1 = poly1->GetPoint(i13);
+			if(i22 == (poly2->GetNumPoints()-1)) i23 = 0;
+			else i23 = i22+1;
+			p3 = poly2->GetPoint(i23);
+
+			if(!IsConvex(p1,p2,p3)) continue;
+
+			p2 = poly1->GetPoint(i12);
+			if(i12 == (poly1->GetNumPoints()-1)) i13 = 0;
+			else i13 = i12+1;
+			p3 = poly1->GetPoint(i13);
+			if(i21 == 0) i23 = poly2->GetNumPoints()-1;
+			else i23 = i21-1;
+			p1 = poly2->GetPoint(i23);
+
+			if(!IsConvex(p1,p2,p3)) continue;
+
+			newpoly.Init(poly1->GetNumPoints()+poly2->GetNumPoints()-2);
+			k = 0;
+			for(j=i12;j!=i11;j=(j+1)%(poly1->GetNumPoints())) {
+				newpoly[k] = poly1->GetPoint(j);
+				k++;
+			}
+			for(j=i22;j!=i21;j=(j+1)%(poly2->GetNumPoints())) {
+				newpoly[k] = poly2->GetPoint(j);
+				k++;
+			}
+
+			triangles.erase(iter2);
+			*iter1 = newpoly;
+			poly1 = &(*iter1);
+			i11 = -1;
+
+			continue;
+		}
+	}
+
+	for(iter1 = triangles.begin(); iter1 != triangles.end(); iter1++) {
+		parts->push_back(*iter1);
+	}
+
+	return 1;
+}
+
+int TriangulatorPartition::ConvexPartition_HM(list<TriangulatorPoly> *inpolys, list<TriangulatorPoly> *parts) {
+	list<TriangulatorPoly> outpolys;
+	list<TriangulatorPoly>::iterator iter;
+
+	if(!RemoveHoles(inpolys,&outpolys)) return 0;
+	for(iter=outpolys.begin();iter!=outpolys.end();iter++) {
+		if(!ConvexPartition_HM(&(*iter),parts)) return 0;
+	}
+	return 1;
+}
+
+//minimum-weight polygon triangulation by dynamic programming
+//O(n^3) time complexity
+//O(n^2) space complexity
+int TriangulatorPartition::Triangulate_OPT(TriangulatorPoly *poly, list<TriangulatorPoly> *triangles) {
+	long i,j,k,gap,n;
+	DPState **dpstates;
+	Vector2 p1,p2,p3,p4;
+	long bestvertex;
+	real_t weight,minweight,d1,d2;
+	Diagonal diagonal,newdiagonal;
+	list<Diagonal> diagonals;
+	TriangulatorPoly triangle;
+	int ret = 1;
+
+	n = poly->GetNumPoints();
+	dpstates = new DPState *[n];
+	for(i=1;i<n;i++) {
+		dpstates[i] = new DPState[i];
+	}
+
+	//init states and visibility
+	for(i=0;i<(n-1);i++) {
+		p1 = poly->GetPoint(i);
+		for(j=i+1;j<n;j++) {
+			dpstates[j][i].visible = true;
+			dpstates[j][i].weight = 0;
+			dpstates[j][i].bestvertex = -1;
+			if(j!=(i+1)) {
+				p2 = poly->GetPoint(j);
+
+				//visibility check
+				if(i==0) p3 = poly->GetPoint(n-1);
+				else p3 = poly->GetPoint(i-1);
+				if(i==(n-1)) p4 = poly->GetPoint(0);
+				else p4 = poly->GetPoint(i+1);
+				if(!InCone(p3,p1,p4,p2)) {
+					dpstates[j][i].visible = false;
+					continue;
+				}
+
+				if(j==0) p3 = poly->GetPoint(n-1);
+				else p3 = poly->GetPoint(j-1);
+				if(j==(n-1)) p4 = poly->GetPoint(0);
+				else p4 = poly->GetPoint(j+1);
+				if(!InCone(p3,p2,p4,p1)) {
+					dpstates[j][i].visible = false;
+					continue;
+				}
+
+				for(k=0;k<n;k++) {
+					p3 = poly->GetPoint(k);
+					if(k==(n-1)) p4 = poly->GetPoint(0);
+					else p4 = poly->GetPoint(k+1);
+					if(Intersects(p1,p2,p3,p4)) {
+						dpstates[j][i].visible = false;
+						break;
+					}
+				}
+			}
+		}
+	}
+	dpstates[n-1][0].visible = true;
+	dpstates[n-1][0].weight = 0;
+	dpstates[n-1][0].bestvertex = -1;
+
+	for(gap = 2; gap<n; gap++) {
+		for(i=0; i<(n-gap); i++) {
+			j = i+gap;
+			if(!dpstates[j][i].visible) continue;
+			bestvertex = -1;
+			for(k=(i+1);k<j;k++) {
+				if(!dpstates[k][i].visible) continue;
+				if(!dpstates[j][k].visible) continue;
+
+				if(k<=(i+1)) d1=0;
+				else d1 = Distance(poly->GetPoint(i),poly->GetPoint(k));
+				if(j<=(k+1)) d2=0;
+				else d2 = Distance(poly->GetPoint(k),poly->GetPoint(j));
+
+				weight = dpstates[k][i].weight + dpstates[j][k].weight + d1 + d2;
+
+				if((bestvertex == -1)||(weight<minweight)) {
+					bestvertex = k;
+					minweight = weight;
+				}
+			}
+			if(bestvertex == -1) {
+				for(i=1;i<n;i++) {
+					delete [] dpstates[i];
+				}
+				delete [] dpstates;
+
+				return 0;
+			}
+
+			dpstates[j][i].bestvertex = bestvertex;
+			dpstates[j][i].weight = minweight;
+		}
+	}
+
+	newdiagonal.index1 = 0;
+	newdiagonal.index2 = n-1;
+	diagonals.push_back(newdiagonal);
+	while(!diagonals.empty()) {
+		diagonal = *(diagonals.begin());
+		diagonals.pop_front();
+		bestvertex = dpstates[diagonal.index2][diagonal.index1].bestvertex;
+		if(bestvertex == -1) {
+			ret = 0;
+			break;
+		}
+		triangle.Triangle(poly->GetPoint(diagonal.index1),poly->GetPoint(bestvertex),poly->GetPoint(diagonal.index2));
+		triangles->push_back(triangle);
+		if(bestvertex > (diagonal.index1+1)) {
+			newdiagonal.index1 = diagonal.index1;
+			newdiagonal.index2 = bestvertex;
+			diagonals.push_back(newdiagonal);
+		}
+		if(diagonal.index2 > (bestvertex+1)) {
+			newdiagonal.index1 = bestvertex;
+			newdiagonal.index2 = diagonal.index2;
+			diagonals.push_back(newdiagonal);
+		}
+	}
+
+	for(i=1;i<n;i++) {
+		delete [] dpstates[i];
+	}
+	delete [] dpstates;
+
+	return ret;
+}
+
+void TriangulatorPartition::UpdateState(long a, long b, long w, long i, long j, DPState2 **dpstates) {
+	Diagonal newdiagonal;
+	list<Diagonal> *pairs;
+	long w2;
+
+	w2 = dpstates[a][b].weight;
+	if(w>w2) return;
+
+	pairs = &(dpstates[a][b].pairs);
+	newdiagonal.index1 = i;
+	newdiagonal.index2 = j;
+
+	if(w<w2) {
+		pairs->clear();
+		pairs->push_front(newdiagonal);
+		dpstates[a][b].weight = w;
+	} else {
+		if((!pairs->empty())&&(i <= pairs->begin()->index1)) return;
+		while((!pairs->empty())&&(pairs->begin()->index2 >= j)) pairs->pop_front();
+		pairs->push_front(newdiagonal);
+	}
+}
+
+void TriangulatorPartition::TypeA(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates) {
+	list<Diagonal> *pairs;
+	list<Diagonal>::iterator iter,lastiter;
+	long top;
+	long w;
+
+	if(!dpstates[i][j].visible) return;
+	top = j;
+	w = dpstates[i][j].weight;
+	if(k-j > 1) {
+		if (!dpstates[j][k].visible) return;
+		w += dpstates[j][k].weight + 1;
+	}
+	if(j-i > 1) {
+		pairs = &(dpstates[i][j].pairs);
+		iter = pairs->end();
+		lastiter = pairs->end();
+		while(iter!=pairs->begin()) {
+			iter--;
+			if(!IsReflex(vertices[iter->index2].p,vertices[j].p,vertices[k].p)) lastiter = iter;
+			else break;
+		}
+		if(lastiter == pairs->end()) w++;
+		else {
+			if(IsReflex(vertices[k].p,vertices[i].p,vertices[lastiter->index1].p)) w++;
+			else top = lastiter->index1;
+		}
+	}
+	UpdateState(i,k,w,top,j,dpstates);
+}
+
+void TriangulatorPartition::TypeB(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates) {
+	list<Diagonal> *pairs;
+	list<Diagonal>::iterator iter,lastiter;
+	long top;
+	long w;
+
+	if(!dpstates[j][k].visible) return;
+	top = j;
+	w = dpstates[j][k].weight;
+
+	if (j-i > 1) {
+		if (!dpstates[i][j].visible) return;
+		w += dpstates[i][j].weight + 1;
+	}
+	if (k-j > 1) {
+		pairs = &(dpstates[j][k].pairs);
+
+		iter = pairs->begin();
+		if((!pairs->empty())&&(!IsReflex(vertices[i].p,vertices[j].p,vertices[iter->index1].p))) {
+			lastiter = iter;
+			while(iter!=pairs->end()) {
+				if(!IsReflex(vertices[i].p,vertices[j].p,vertices[iter->index1].p)) {
+					lastiter = iter;
+					iter++;
+				}
+				else break;
+			}
+			if(IsReflex(vertices[lastiter->index2].p,vertices[k].p,vertices[i].p)) w++;
+			else top = lastiter->index2;
+		} else w++;
+	}
+	UpdateState(i,k,w,j,top,dpstates);
+}
+
+int TriangulatorPartition::ConvexPartition_OPT(TriangulatorPoly *poly, list<TriangulatorPoly> *parts) {
+	Vector2 p1,p2,p3,p4;
+	PartitionVertex *vertices;
+	DPState2 **dpstates;
+	long i,j,k,n,gap;
+	list<Diagonal> diagonals,diagonals2;
+	Diagonal diagonal,newdiagonal;
+	list<Diagonal> *pairs,*pairs2;
+	list<Diagonal>::iterator iter,iter2;
+	int ret;
+	TriangulatorPoly newpoly;
+	list<long> indices;
+	list<long>::iterator iiter;
+	bool ijreal,jkreal;
+
+	n = poly->GetNumPoints();
+	vertices = new PartitionVertex[n];
+
+	dpstates = new DPState2 *[n];
+	for(i=0;i<n;i++) {
+		dpstates[i] = new DPState2[n];
+	}
+
+	//init vertex information
+	for(i=0;i<n;i++) {
+		vertices[i].p = poly->GetPoint(i);
+		vertices[i].isActive = true;
+		if(i==0) vertices[i].previous = &(vertices[n-1]);
+		else vertices[i].previous = &(vertices[i-1]);
+		if(i==(poly->GetNumPoints()-1)) vertices[i].next = &(vertices[0]);
+		else vertices[i].next = &(vertices[i+1]);
+	}
+	for(i=1;i<n;i++) {
+		UpdateVertexReflexity(&(vertices[i]));
+	}
+
+	//init states and visibility
+	for(i=0;i<(n-1);i++) {
+		p1 = poly->GetPoint(i);
+		for(j=i+1;j<n;j++) {
+			dpstates[i][j].visible = true;
+			if(j==i+1) {
+				dpstates[i][j].weight = 0;
+			} else {
+				dpstates[i][j].weight = 2147483647;
+			}
+			if(j!=(i+1)) {
+				p2 = poly->GetPoint(j);
+
+				//visibility check
+				if(!InCone(&vertices[i],p2)) {
+					dpstates[i][j].visible = false;
+					continue;
+				}
+				if(!InCone(&vertices[j],p1)) {
+					dpstates[i][j].visible = false;
+					continue;
+				}
+
+				for(k=0;k<n;k++) {
+					p3 = poly->GetPoint(k);
+					if(k==(n-1)) p4 = poly->GetPoint(0);
+					else p4 = poly->GetPoint(k+1);
+					if(Intersects(p1,p2,p3,p4)) {
+						dpstates[i][j].visible = false;
+						break;
+					}
+				}
+			}
+		}
+	}
+	for(i=0;i<(n-2);i++) {
+		j = i+2;
+		if(dpstates[i][j].visible) {
+			dpstates[i][j].weight = 0;
+			newdiagonal.index1 = i+1;
+			newdiagonal.index2 = i+1;
+			dpstates[i][j].pairs.push_back(newdiagonal);
+		}
+	}
+
+	dpstates[0][n-1].visible = true;
+	vertices[0].isConvex = false; //by convention
+
+	for(gap=3; gap<n; gap++) {
+		for(i=0;i<n-gap;i++) {
+			if(vertices[i].isConvex) continue;
+			k = i+gap;
+			if(dpstates[i][k].visible) {
+				if(!vertices[k].isConvex) {
+					for(j=i+1;j<k;j++) TypeA(i,j,k,vertices,dpstates);
+				} else {
+					for(j=i+1;j<(k-1);j++) {
+						if(vertices[j].isConvex) continue;
+						TypeA(i,j,k,vertices,dpstates);
+					}
+					TypeA(i,k-1,k,vertices,dpstates);
+				}
+			}
+		}
+		for(k=gap;k<n;k++) {
+			if(vertices[k].isConvex) continue;
+			i = k-gap;
+			if((vertices[i].isConvex)&&(dpstates[i][k].visible)) {
+				TypeB(i,i+1,k,vertices,dpstates);
+				for(j=i+2;j<k;j++) {
+					if(vertices[j].isConvex) continue;
+					TypeB(i,j,k,vertices,dpstates);
+				}
+			}
+		}
+	}
+
+
+	//recover solution
+	ret = 1;
+	newdiagonal.index1 = 0;
+	newdiagonal.index2 = n-1;
+	diagonals.push_front(newdiagonal);
+	while(!diagonals.empty()) {
+		diagonal = *(diagonals.begin());
+		diagonals.pop_front();
+		if((diagonal.index2 - diagonal.index1) <=1) continue;
+		pairs = &(dpstates[diagonal.index1][diagonal.index2].pairs);
+		if(pairs->empty()) {
+			ret = 0;
+			break;
+		}
+		if(!vertices[diagonal.index1].isConvex) {
+			iter = pairs->end();
+			iter--;
+			j = iter->index2;
+			newdiagonal.index1 = j;
+			newdiagonal.index2 = diagonal.index2;
+			diagonals.push_front(newdiagonal);
+			if((j - diagonal.index1)>1) {
+				if(iter->index1 != iter->index2) {
+					pairs2 = &(dpstates[diagonal.index1][j].pairs);
+					while(1) {
+						if(pairs2->empty()) {
+							ret = 0;
+							break;
+						}
+						iter2 = pairs2->end();
+						iter2--;
+						if(iter->index1 != iter2->index1) pairs2->pop_back();
+						else break;
+					}
+					if(ret == 0) break;
+				}
+				newdiagonal.index1 = diagonal.index1;
+				newdiagonal.index2 = j;
+				diagonals.push_front(newdiagonal);
+			}
+		} else {
+			iter = pairs->begin();
+			j = iter->index1;
+			newdiagonal.index1 = diagonal.index1;
+			newdiagonal.index2 = j;
+			diagonals.push_front(newdiagonal);
+			if((diagonal.index2 - j) > 1) {
+				if(iter->index1 != iter->index2) {
+					pairs2 = &(dpstates[j][diagonal.index2].pairs);
+					while(1) {
+						if(pairs2->empty()) {
+							ret = 0;
+							break;
+						}
+						iter2 = pairs2->begin();
+						if(iter->index2 != iter2->index2) pairs2->pop_front();
+						else break;
+					}
+					if(ret == 0) break;
+				}
+				newdiagonal.index1 = j;
+				newdiagonal.index2 = diagonal.index2;
+				diagonals.push_front(newdiagonal);
+			}
+		}
+	}
+
+	if(ret == 0) {
+		for(i=0;i<n;i++) {
+			delete [] dpstates[i];
+		}
+		delete [] dpstates;
+		delete [] vertices;
+
+		return ret;
+	}
+
+	newdiagonal.index1 = 0;
+	newdiagonal.index2 = n-1;
+	diagonals.push_front(newdiagonal);
+	while(!diagonals.empty()) {
+		diagonal = *(diagonals.begin());
+		diagonals.pop_front();
+		if((diagonal.index2 - diagonal.index1) <= 1) continue;
+
+		indices.clear();
+		diagonals2.clear();
+		indices.push_back(diagonal.index1);
+		indices.push_back(diagonal.index2);
+		diagonals2.push_front(diagonal);
+
+		while(!diagonals2.empty()) {
+			diagonal = *(diagonals2.begin());
+			diagonals2.pop_front();
+			if((diagonal.index2 - diagonal.index1) <= 1) continue;
+			ijreal = true;
+			jkreal = true;
+			pairs = &(dpstates[diagonal.index1][diagonal.index2].pairs);
+			if(!vertices[diagonal.index1].isConvex) {
+				iter = pairs->end();
+				iter--;
+				j = iter->index2;
+				if(iter->index1 != iter->index2) ijreal = false;
+			} else {
+				iter = pairs->begin();
+				j = iter->index1;
+				if(iter->index1 != iter->index2) jkreal = false;
+			}
+
+			newdiagonal.index1 = diagonal.index1;
+			newdiagonal.index2 = j;
+			if(ijreal) {
+				diagonals.push_back(newdiagonal);
+			} else {
+				diagonals2.push_back(newdiagonal);
+			}
+
+			newdiagonal.index1 = j;
+			newdiagonal.index2 = diagonal.index2;
+			if(jkreal) {
+				diagonals.push_back(newdiagonal);
+			} else {
+				diagonals2.push_back(newdiagonal);
+			}
+
+			indices.push_back(j);
+		}
+
+		indices.sort();
+		newpoly.Init((long)indices.size());
+		k=0;
+		for(iiter = indices.begin();iiter!=indices.end();iiter++) {
+			newpoly[k] = vertices[*iiter].p;
+			k++;
+		}
+		parts->push_back(newpoly);
+	}
+
+	for(i=0;i<n;i++) {
+		delete [] dpstates[i];
+	}
+	delete [] dpstates;
+	delete [] vertices;
+
+	return ret;
+}
+
+//triangulates a set of polygons by first partitioning them into monotone polygons
+//O(n*log(n)) time complexity, O(n) space complexity
+//the algorithm used here is outlined in the book
+//"Computational Geometry: Algorithms and Applications"
+//by Mark de Berg, Otfried Cheong, Marc van Kreveld and Mark Overmars
+int TriangulatorPartition::MonotonePartition(list<TriangulatorPoly> *inpolys, list<TriangulatorPoly> *monotonePolys) {
+	list<TriangulatorPoly>::iterator iter;
+	MonotoneVertex *vertices;
+	long i,numvertices,vindex,vindex2,newnumvertices,maxnumvertices;
+	long polystartindex, polyendindex;
+	TriangulatorPoly *poly;
+	MonotoneVertex *v,*v2,*vprev,*vnext;
+	ScanLineEdge newedge;
+	bool error = false;
+
+	numvertices = 0;
+	for(iter = inpolys->begin(); iter != inpolys->end(); iter++) {
+		numvertices += iter->GetNumPoints();
+	}
+
+	maxnumvertices = numvertices*3;
+	vertices = new MonotoneVertex[maxnumvertices];
+	newnumvertices = numvertices;
+
+	polystartindex = 0;
+	for(iter = inpolys->begin(); iter != inpolys->end(); iter++) {
+		poly = &(*iter);
+		polyendindex = polystartindex + poly->GetNumPoints()-1;
+		for(i=0;i<poly->GetNumPoints();i++) {
+			vertices[i+polystartindex].p = poly->GetPoint(i);
+			if(i==0) vertices[i+polystartindex].previous = polyendindex;
+			else vertices[i+polystartindex].previous = i+polystartindex-1;
+			if(i==(poly->GetNumPoints()-1)) vertices[i+polystartindex].next = polystartindex;
+			else vertices[i+polystartindex].next = i+polystartindex+1;
+		}
+		polystartindex = polyendindex+1;
+	}
+
+	//construct the priority queue
+	long *priority = new long [numvertices];
+	for(i=0;i<numvertices;i++) priority[i] = i;
+	std::sort(priority,&(priority[numvertices]),VertexSorter(vertices));
+
+	//determine vertex types
+	char *vertextypes = new char[maxnumvertices];
+	for(i=0;i<numvertices;i++) {
+		v = &(vertices[i]);
+		vprev = &(vertices[v->previous]);
+		vnext = &(vertices[v->next]);
+
+		if(Below(vprev->p,v->p)&&Below(vnext->p,v->p)) {
+			if(IsConvex(vnext->p,vprev->p,v->p)) {
+				vertextypes[i] = TRIANGULATOR_VERTEXTYPE_START;
+			} else {
+				vertextypes[i] = TRIANGULATOR_VERTEXTYPE_SPLIT;
+			}
+		} else if(Below(v->p,vprev->p)&&Below(v->p,vnext->p)) {
+			if(IsConvex(vnext->p,vprev->p,v->p))
+			{
+				vertextypes[i] = TRIANGULATOR_VERTEXTYPE_END;
+			} else {
+				vertextypes[i] = TRIANGULATOR_VERTEXTYPE_MERGE;
+			}
+		} else {
+			vertextypes[i] = TRIANGULATOR_VERTEXTYPE_REGULAR;
+		}
+	}
+
+	//helpers
+	long *helpers = new long[maxnumvertices];
+
+	//binary search tree that holds edges intersecting the scanline
+	//note that while set doesn't actually have to be implemented as a tree
+	//complexity requirements for operations are the same as for the balanced binary search tree
+	set<ScanLineEdge> edgeTree;
+	//store iterators to the edge tree elements
+	//this makes deleting existing edges much faster
+	set<ScanLineEdge>::iterator *edgeTreeIterators,edgeIter;
+	edgeTreeIterators = new set<ScanLineEdge>::iterator[maxnumvertices];
+	pair<set<ScanLineEdge>::iterator,bool> edgeTreeRet;
+	for(i = 0; i<numvertices; i++) edgeTreeIterators[i] = edgeTree.end();
+
+	//for each vertex
+	for(i=0;i<numvertices;i++) {
+		vindex = priority[i];
+		v = &(vertices[vindex]);
+		vindex2 = vindex;
+		v2 = v;
+
+		//depending on the vertex type, do the appropriate action
+		//comments in the following sections are copied from "Computational Geometry: Algorithms and Applications"
+		switch(vertextypes[vindex]) {
+			case TRIANGULATOR_VERTEXTYPE_START:
+				//Insert ei in T and set helper(ei) to vi.
+				newedge.p1 = v->p;
+				newedge.p2 = vertices[v->next].p;
+				newedge.index = vindex;
+				edgeTreeRet = edgeTree.insert(newedge);
+				edgeTreeIterators[vindex] = edgeTreeRet.first;
+				helpers[vindex] = vindex;
+				break;
+
+			case TRIANGULATOR_VERTEXTYPE_END:
+				//if helper(ei-1) is a merge vertex
+				if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
+					//Insert the diagonal connecting vi to helper(ei-1) in D.
+					AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
+							vertextypes, edgeTreeIterators, &edgeTree, helpers);
+				}
+				//Delete ei-1 from T
+				edgeTree.erase(edgeTreeIterators[v->previous]);
+				break;
+
+			case TRIANGULATOR_VERTEXTYPE_SPLIT:
+				//Search in T to find the edge e j directly left of vi.
+				newedge.p1 = v->p;
+				newedge.p2 = v->p;
+				edgeIter = edgeTree.lower_bound(newedge);
+				if(edgeIter == edgeTree.begin()) {
+					error = true;
+					break;
+				}
+				edgeIter--;
+				//Insert the diagonal connecting vi to helper(ej) in D.
+				AddDiagonal(vertices,&newnumvertices,vindex,helpers[edgeIter->index],
+						vertextypes, edgeTreeIterators, &edgeTree, helpers);
+				vindex2 = newnumvertices-2;
+				v2 = &(vertices[vindex2]);
+				//helper(e j)�vi
+				helpers[edgeIter->index] = vindex;
+				//Insert ei in T and set helper(ei) to vi.
+				newedge.p1 = v2->p;
+				newedge.p2 = vertices[v2->next].p;
+				newedge.index = vindex2;
+				edgeTreeRet = edgeTree.insert(newedge);
+				edgeTreeIterators[vindex2] = edgeTreeRet.first;
+				helpers[vindex2] = vindex2;
+				break;
+
+			case TRIANGULATOR_VERTEXTYPE_MERGE:
+				//if helper(ei-1) is a merge vertex
+				if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
+					//Insert the diagonal connecting vi to helper(ei-1) in D.
+					AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
+							vertextypes, edgeTreeIterators, &edgeTree, helpers);
+					vindex2 = newnumvertices-2;
+					v2 = &(vertices[vindex2]);
+				}
+				//Delete ei-1 from T.
+				edgeTree.erase(edgeTreeIterators[v->previous]);
+				//Search in T to find the edge e j directly left of vi.
+				newedge.p1 = v->p;
+				newedge.p2 = v->p;
+				edgeIter = edgeTree.lower_bound(newedge);
+				if(edgeIter == edgeTree.begin()) {
+					error = true;
+					break;
+				}
+				edgeIter--;
+				//if helper(ej) is a merge vertex
+				if(vertextypes[helpers[edgeIter->index]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
+					//Insert the diagonal connecting vi to helper(e j) in D.
+					AddDiagonal(vertices,&newnumvertices,vindex2,helpers[edgeIter->index],
+							vertextypes, edgeTreeIterators, &edgeTree, helpers);
+				}
+				//helper(e j)�vi
+				helpers[edgeIter->index] = vindex2;
+				break;
+
+			case TRIANGULATOR_VERTEXTYPE_REGULAR:
+				//if the interior of P lies to the right of vi
+				if(Below(v->p,vertices[v->previous].p)) {
+					//if helper(ei-1) is a merge vertex
+					if(vertextypes[helpers[v->previous]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
+						//Insert the diagonal connecting vi to helper(ei-1) in D.
+						AddDiagonal(vertices,&newnumvertices,vindex,helpers[v->previous],
+								vertextypes, edgeTreeIterators, &edgeTree, helpers);
+						vindex2 = newnumvertices-2;
+						v2 = &(vertices[vindex2]);
+					}
+					//Delete ei-1 from T.
+					edgeTree.erase(edgeTreeIterators[v->previous]);
+					//Insert ei in T and set helper(ei) to vi.
+					newedge.p1 = v2->p;
+					newedge.p2 = vertices[v2->next].p;
+					newedge.index = vindex2;
+					edgeTreeRet = edgeTree.insert(newedge);
+					edgeTreeIterators[vindex2] = edgeTreeRet.first;
+					helpers[vindex2] = vindex;
+				} else {
+					//Search in T to find the edge ej directly left of vi.
+					newedge.p1 = v->p;
+					newedge.p2 = v->p;
+					edgeIter = edgeTree.lower_bound(newedge);
+					if(edgeIter == edgeTree.begin()) {
+						error = true;
+						break;
+					}
+					edgeIter--;
+					//if helper(ej) is a merge vertex
+					if(vertextypes[helpers[edgeIter->index]]==TRIANGULATOR_VERTEXTYPE_MERGE) {
+						//Insert the diagonal connecting vi to helper(e j) in D.
+						AddDiagonal(vertices,&newnumvertices,vindex,helpers[edgeIter->index],
+								vertextypes, edgeTreeIterators, &edgeTree, helpers);
+					}
+					//helper(e j)�vi
+					helpers[edgeIter->index] = vindex;
+				}
+				break;
+		}
+
+		if(error) break;
+	}
+
+	char *used = new char[newnumvertices];
+	memset(used,0,newnumvertices*sizeof(char));
+
+	if(!error) {
+		//return result
+		long size;
+		TriangulatorPoly mpoly;
+		for(i=0;i<newnumvertices;i++) {
+			if(used[i]) continue;
+			v = &(vertices[i]);
+			vnext = &(vertices[v->next]);
+			size = 1;
+			while(vnext!=v) {
+				vnext = &(vertices[vnext->next]);
+				size++;
+			}
+			mpoly.Init(size);
+			v = &(vertices[i]);
+			mpoly[0] = v->p;
+			vnext = &(vertices[v->next]);
+			size = 1;
+			used[i] = 1;
+			used[v->next] = 1;
+			while(vnext!=v) {
+				mpoly[size] = vnext->p;
+				used[vnext->next] = 1;
+				vnext = &(vertices[vnext->next]);
+				size++;
+			}
+			monotonePolys->push_back(mpoly);
+		}
+	}
+
+	//cleanup
+	delete [] vertices;
+	delete [] priority;
+	delete [] vertextypes;
+	delete [] edgeTreeIterators;
+	delete [] helpers;
+	delete [] used;
+
+	if(error) {
+		return 0;
+	} else {
+		return 1;
+	}
+}
+
+//adds a diagonal to the doubly-connected list of vertices
+void TriangulatorPartition::AddDiagonal(MonotoneVertex *vertices, long *numvertices, long index1, long index2,
+					char *vertextypes, set<ScanLineEdge>::iterator *edgeTreeIterators,
+					set<ScanLineEdge> *edgeTree, long *helpers)
+{
+	long newindex1,newindex2;
+
+	newindex1 = *numvertices;
+	(*numvertices)++;
+	newindex2 = *numvertices;
+	(*numvertices)++;
+
+	vertices[newindex1].p = vertices[index1].p;
+	vertices[newindex2].p = vertices[index2].p;
+
+	vertices[newindex2].next = vertices[index2].next;
+	vertices[newindex1].next = vertices[index1].next;
+
+	vertices[vertices[index2].next].previous = newindex2;
+	vertices[vertices[index1].next].previous = newindex1;
+
+	vertices[index1].next = newindex2;
+	vertices[newindex2].previous = index1;
+
+	vertices[index2].next = newindex1;
+	vertices[newindex1].previous = index2;
+
+	//update all relevant structures
+	vertextypes[newindex1] = vertextypes[index1];
+	edgeTreeIterators[newindex1] = edgeTreeIterators[index1];
+	helpers[newindex1] = helpers[index1];
+	if(edgeTreeIterators[newindex1] != edgeTree->end())
+		edgeTreeIterators[newindex1]->index = newindex1;
+	vertextypes[newindex2] = vertextypes[index2];
+	edgeTreeIterators[newindex2] = edgeTreeIterators[index2];
+	helpers[newindex2] = helpers[index2];
+	if(edgeTreeIterators[newindex2] != edgeTree->end())
+		edgeTreeIterators[newindex2]->index = newindex2;
+}
+
+bool TriangulatorPartition::Below(Vector2 &p1, Vector2 &p2) {
+	if(p1.y < p2.y) return true;
+	else if(p1.y == p2.y) {
+		if(p1.x < p2.x) return true;
+	}
+	return false;
+}
+
+//sorts in the falling order of y values, if y is equal, x is used instead
+bool TriangulatorPartition::VertexSorter::operator() (long index1, long index2) {
+	if(vertices[index1].p.y > vertices[index2].p.y) return true;
+	else if(vertices[index1].p.y == vertices[index2].p.y) {
+		if(vertices[index1].p.x > vertices[index2].p.x) return true;
+	}
+	return false;
+}
+
+bool TriangulatorPartition::ScanLineEdge::IsConvex(const Vector2& p1, const Vector2& p2, const Vector2& p3) const {
+	real_t tmp;
+	tmp = (p3.y-p1.y)*(p2.x-p1.x)-(p3.x-p1.x)*(p2.y-p1.y);
+	if(tmp>0) return 1;
+	else return 0;
+}
+
+bool TriangulatorPartition::ScanLineEdge::operator < (const ScanLineEdge & other) const {
+	if(other.p1.y == other.p2.y) {
+		if(p1.y == p2.y) {
+			if(p1.y < other.p1.y) return true;
+			else return false;
+		}
+		if(IsConvex(p1,p2,other.p1)) return true;
+		else return false;
+	} else if(p1.y == p2.y) {
+		if(IsConvex(other.p1,other.p2,p1)) return false;
+		else return true;
+	} else if(p1.y < other.p1.y) {
+		if(IsConvex(other.p1,other.p2,p1)) return false;
+		else return true;
+	} else {
+		if(IsConvex(p1,p2,other.p1)) return true;
+		else return false;
+	}
+}
+
+//triangulates monotone polygon
+//O(n) time, O(n) space complexity
+int TriangulatorPartition::TriangulateMonotone(TriangulatorPoly *inPoly, list<TriangulatorPoly> *triangles) {
+	long i,i2,j,topindex,bottomindex,leftindex,rightindex,vindex;
+	Vector2 *points;
+	long numpoints;
+	TriangulatorPoly triangle;
+
+	numpoints = inPoly->GetNumPoints();
+	points = inPoly->GetPoints();
+
+	//trivial calses
+	if(numpoints < 3) return 0;
+	if(numpoints == 3) {
+		triangles->push_back(*inPoly);
+	}
+
+	topindex = 0; bottomindex=0;
+	for(i=1;i<numpoints;i++) {
+		if(Below(points[i],points[bottomindex])) bottomindex = i;
+		if(Below(points[topindex],points[i])) topindex = i;
+	}
+
+	//check if the poly is really monotone
+	i = topindex;
+	while(i!=bottomindex) {
+		i2 = i+1; if(i2>=numpoints) i2 = 0;
+		if(!Below(points[i2],points[i])) return 0;
+		i = i2;
+	}
+	i = bottomindex;
+	while(i!=topindex) {
+		i2 = i+1; if(i2>=numpoints) i2 = 0;
+		if(!Below(points[i],points[i2])) return 0;
+		i = i2;
+	}
+
+	char *vertextypes = new char[numpoints];
+	long *priority = new long[numpoints];
+
+	//merge left and right vertex chains
+	priority[0] = topindex;
+	vertextypes[topindex] = 0;
+	leftindex = topindex+1; if(leftindex>=numpoints) leftindex = 0;
+	rightindex = topindex-1; if(rightindex<0) rightindex = numpoints-1;
+	for(i=1;i<(numpoints-1);i++) {
+		if(leftindex==bottomindex) {
+			priority[i] = rightindex;
+			rightindex--; if(rightindex<0) rightindex = numpoints-1;
+			vertextypes[priority[i]] = -1;
+		} else if(rightindex==bottomindex) {
+			priority[i] = leftindex;
+			leftindex++;  if(leftindex>=numpoints) leftindex = 0;
+			vertextypes[priority[i]] = 1;
+		} else {
+			if(Below(points[leftindex],points[rightindex])) {
+				priority[i] = rightindex;
+				rightindex--; if(rightindex<0) rightindex = numpoints-1;
+				vertextypes[priority[i]] = -1;
+			} else {
+				priority[i] = leftindex;
+				leftindex++;  if(leftindex>=numpoints) leftindex = 0;
+				vertextypes[priority[i]] = 1;
+			}
+		}
+	}
+	priority[i] = bottomindex;
+	vertextypes[bottomindex] = 0;
+
+	long *stack = new long[numpoints];
+	long stackptr = 0;
+
+	stack[0] = priority[0];
+	stack[1] = priority[1];
+	stackptr = 2;
+
+	//for each vertex from top to bottom trim as many triangles as possible
+	for(i=2;i<(numpoints-1);i++) {
+		vindex = priority[i];
+		if(vertextypes[vindex]!=vertextypes[stack[stackptr-1]]) {
+			for(j=0;j<(stackptr-1);j++) {
+				if(vertextypes[vindex]==1) {
+					triangle.Triangle(points[stack[j+1]],points[stack[j]],points[vindex]);
+				} else {
+					triangle.Triangle(points[stack[j]],points[stack[j+1]],points[vindex]);
+				}
+				triangles->push_back(triangle);
+			}
+			stack[0] = priority[i-1];
+			stack[1] = priority[i];
+			stackptr = 2;
+		} else {
+			stackptr--;
+			while(stackptr>0) {
+				if(vertextypes[vindex]==1) {
+					if(IsConvex(points[vindex],points[stack[stackptr-1]],points[stack[stackptr]])) {
+						triangle.Triangle(points[vindex],points[stack[stackptr-1]],points[stack[stackptr]]);
+						triangles->push_back(triangle);
+						stackptr--;
+					} else {
+						break;
+					}
+				} else {
+					if(IsConvex(points[vindex],points[stack[stackptr]],points[stack[stackptr-1]])) {
+						triangle.Triangle(points[vindex],points[stack[stackptr]],points[stack[stackptr-1]]);
+						triangles->push_back(triangle);
+						stackptr--;
+					} else {
+						break;
+					}
+				}
+			}
+			stackptr++;
+			stack[stackptr] = vindex;
+			stackptr++;
+		}
+	}
+	vindex = priority[i];
+	for(j=0;j<(stackptr-1);j++) {
+		if(vertextypes[stack[j+1]]==1) {
+			triangle.Triangle(points[stack[j]],points[stack[j+1]],points[vindex]);
+		} else {
+			triangle.Triangle(points[stack[j+1]],points[stack[j]],points[vindex]);
+		}
+		triangles->push_back(triangle);
+	}
+
+	delete [] priority;
+	delete [] vertextypes;
+	delete [] stack;
+
+	return 1;
+}
+
+int TriangulatorPartition::Triangulate_MONO(list<TriangulatorPoly> *inpolys, list<TriangulatorPoly> *triangles) {
+	list<TriangulatorPoly> monotone;
+	list<TriangulatorPoly>::iterator iter;
+
+	if(!MonotonePartition(inpolys,&monotone)) return 0;
+	for(iter = monotone.begin(); iter!=monotone.end();iter++) {
+		if(!TriangulateMonotone(&(*iter),triangles)) return 0;
+	}
+	return 1;
+}
+
+int TriangulatorPartition::Triangulate_MONO(TriangulatorPoly *poly, list<TriangulatorPoly> *triangles) {
+	list<TriangulatorPoly> polys;
+	polys.push_back(*poly);
+
+	return Triangulate_MONO(&polys, triangles);
+}

core/math/triangulator.h

@@ -0,0 +1,309 @@
+//Copyright (C) 2011 by Ivan Fratric
+//
+//Permission is hereby granted, free of charge, to any person obtaining a copy
+//of this software and associated documentation files (the "Software"), to deal
+//in the Software without restriction, including without limitation the rights
+//to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//copies of the Software, and to permit persons to whom the Software is
+//furnished to do so, subject to the following conditions:
+//
+//The above copyright notice and this permission notice shall be included in
+//all copies or substantial portions of the Software.
+//
+//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//THE SOFTWARE.
+
+#ifndef TRIANGULATOR_H
+#define TRIANGULATOR_H
+
+#include "math_2d.h"
+#include <list>
+#include <set>
+
+//2D point structure
+
+
+#define TRIANGULATOR_CCW 1
+#define TRIANGULATOR_CW -1
+//Polygon implemented as an array of points with a 'hole' flag
+class TriangulatorPoly {
+protected:
+
+
+
+	Vector2 *points;
+	long numpoints;
+	bool hole;
+
+public:
+
+	//constructors/destructors
+	TriangulatorPoly();
+	~TriangulatorPoly();
+
+	TriangulatorPoly(const TriangulatorPoly &src);
+	TriangulatorPoly& operator=(const TriangulatorPoly &src);
+
+	//getters and setters
+	long GetNumPoints() {
+		return numpoints;
+	}
+
+	bool IsHole() {
+		return hole;
+	}
+
+	void SetHole(bool hole) {
+		this->hole = hole;
+	}
+
+	Vector2 &GetPoint(long i) {
+		return points[i];
+	}
+
+	Vector2 *GetPoints() {
+		return points;
+	}
+
+	Vector2& operator[] (int i) {
+		return points[i];
+	}
+
+	//clears the polygon points
+	void Clear();
+
+	//inits the polygon with numpoints vertices
+	void Init(long numpoints);
+
+	//creates a triangle with points p1,p2,p3
+	void Triangle(Vector2 &p1, Vector2 &p2, Vector2 &p3);
+
+	//inverts the orfer of vertices
+	void Invert();
+
+	//returns the orientation of the polygon
+	//possible values:
+	//   Triangulator_CCW : polygon vertices are in counter-clockwise order
+	//   Triangulator_CW : polygon vertices are in clockwise order
+	//       0 : the polygon has no (measurable) area
+	int GetOrientation();
+
+	//sets the polygon orientation
+	//orientation can be
+	//   Triangulator_CCW : sets vertices in counter-clockwise order
+	//   Triangulator_CW : sets vertices in clockwise order
+	void SetOrientation(int orientation);
+};
+
+class TriangulatorPartition {
+protected:
+	struct PartitionVertex {
+		bool isActive;
+		bool isConvex;
+		bool isEar;
+
+		Vector2 p;
+		real_t angle;
+		PartitionVertex *previous;
+		PartitionVertex *next;
+	};
+
+	struct MonotoneVertex {
+		Vector2 p;
+		long previous;
+		long next;
+	};
+
+	class VertexSorter{
+		MonotoneVertex *vertices;
+	public:
+		VertexSorter(MonotoneVertex *v) : vertices(v) {}
+		bool operator() (long index1, long index2);
+	};
+
+	struct Diagonal {
+		long index1;
+		long index2;
+	};
+
+	//dynamic programming state for minimum-weight triangulation
+	struct DPState {
+		bool visible;
+		real_t weight;
+		long bestvertex;
+	};
+
+	//dynamic programming state for convex partitioning
+	struct DPState2 {
+		bool visible;
+		long weight;
+		std::list<Diagonal> pairs;
+	};
+
+	//edge that intersects the scanline
+	struct ScanLineEdge {
+		mutable long index;
+		Vector2 p1;
+		Vector2 p2;
+
+		//determines if the edge is to the left of another edge
+		bool operator< (const ScanLineEdge & other) const;
+
+		bool IsConvex(const Vector2& p1, const Vector2& p2, const Vector2& p3) const;
+	};
+
+	//standard helper functions
+	bool IsConvex(Vector2& p1, Vector2& p2, Vector2& p3);
+	bool IsReflex(Vector2& p1, Vector2& p2, Vector2& p3);
+	bool IsInside(Vector2& p1, Vector2& p2, Vector2& p3, Vector2 &p);
+
+	bool InCone(Vector2 &p1, Vector2 &p2, Vector2 &p3, Vector2 &p);
+	bool InCone(PartitionVertex *v, Vector2 &p);
+
+	int Intersects(Vector2 &p11, Vector2 &p12, Vector2 &p21, Vector2 &p22);
+
+	Vector2 Normalize(const Vector2 &p);
+	real_t Distance(const Vector2 &p1, const Vector2 &p2);
+
+	//helper functions for Triangulate_EC
+	void UpdateVertexReflexity(PartitionVertex *v);
+	void UpdateVertex(PartitionVertex *v,PartitionVertex *vertices, long numvertices);
+
+	//helper functions for ConvexPartition_OPT
+	void UpdateState(long a, long b, long w, long i, long j, DPState2 **dpstates);
+	void TypeA(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates);
+	void TypeB(long i, long j, long k, PartitionVertex *vertices, DPState2 **dpstates);
+
+	//helper functions for MonotonePartition
+	bool Below(Vector2 &p1, Vector2 &p2);
+	void AddDiagonal(MonotoneVertex *vertices, long *numvertices, long index1, long index2,
+			 char *vertextypes, std::set<ScanLineEdge>::iterator *edgeTreeIterators,
+			 std::set<ScanLineEdge> *edgeTree, long *helpers);
+
+	//triangulates a monotone polygon, used in Triangulate_MONO
+	int TriangulateMonotone(TriangulatorPoly *inPoly, std::list<TriangulatorPoly> *triangles);
+
+public:
+
+	//simple heuristic procedure for removing holes from a list of polygons
+	//works by creating a diagonal from the rightmost hole vertex to some visible vertex
+	//time complexity: O(h*(n^2)), h is the number of holes, n is the number of vertices
+	//space complexity: O(n)
+	//params:
+	//   inpolys : a list of polygons that can contain holes
+	//             vertices of all non-hole polys have to be in counter-clockwise order
+	//             vertices of all hole polys have to be in clockwise order
+	//   outpolys : a list of polygons without holes
+	//returns 1 on success, 0 on failure
+	int RemoveHoles(std::list<TriangulatorPoly> *inpolys, std::list<TriangulatorPoly> *outpolys);
+
+	//triangulates a polygon by ear clipping
+	//time complexity O(n^2), n is the number of vertices
+	//space complexity: O(n)
+	//params:
+	//   poly : an input polygon to be triangulated
+	//          vertices have to be in counter-clockwise order
+	//   triangles : a list of triangles (result)
+	//returns 1 on success, 0 on failure
+	int Triangulate_EC(TriangulatorPoly *poly, std::list<TriangulatorPoly> *triangles);
+
+	//triangulates a list of polygons that may contain holes by ear clipping algorithm
+	//first calls RemoveHoles to get rid of the holes, and then Triangulate_EC for each resulting polygon
+	//time complexity: O(h*(n^2)), h is the number of holes, n is the number of vertices
+	//space complexity: O(n)
+	//params:
+	//   inpolys : a list of polygons to be triangulated (can contain holes)
+	//             vertices of all non-hole polys have to be in counter-clockwise order
+	//             vertices of all hole polys have to be in clockwise order
+	//   triangles : a list of triangles (result)
+	//returns 1 on success, 0 on failure
+	int Triangulate_EC(std::list<TriangulatorPoly> *inpolys, std::list<TriangulatorPoly> *triangles);
+
+	//creates an optimal polygon triangulation in terms of minimal edge length
+	//time complexity: O(n^3), n is the number of vertices
+	//space complexity: O(n^2)
+	//params:
+	//   poly : an input polygon to be triangulated
+	//          vertices have to be in counter-clockwise order
+	//   triangles : a list of triangles (result)
+	//returns 1 on success, 0 on failure
+	int Triangulate_OPT(TriangulatorPoly *poly, std::list<TriangulatorPoly> *triangles);
+
+	//triangulates a polygons by firstly partitioning it into monotone polygons
+	//time complexity: O(n*log(n)), n is the number of vertices
+	//space complexity: O(n)
+	//params:
+	//   poly : an input polygon to be triangulated
+	//          vertices have to be in counter-clockwise order
+	//   triangles : a list of triangles (result)
+	//returns 1 on success, 0 on failure
+	int Triangulate_MONO(TriangulatorPoly *poly, std::list<TriangulatorPoly> *triangles);
+
+	//triangulates a list of polygons by firstly partitioning them into monotone polygons
+	//time complexity: O(n*log(n)), n is the number of vertices
+	//space complexity: O(n)
+	//params:
+	//   inpolys : a list of polygons to be triangulated (can contain holes)
+	//             vertices of all non-hole polys have to be in counter-clockwise order
+	//             vertices of all hole polys have to be in clockwise order
+	//   triangles : a list of triangles (result)
+	//returns 1 on success, 0 on failure
+	int Triangulate_MONO(std::list<TriangulatorPoly> *inpolys, std::list<TriangulatorPoly> *triangles);
+
+	//creates a monotone partition of a list of polygons that can contain holes
+	//time complexity: O(n*log(n)), n is the number of vertices
+	//space complexity: O(n)
+	//params:
+	//   inpolys : a list of polygons to be triangulated (can contain holes)
+	//             vertices of all non-hole polys have to be in counter-clockwise order
+	//             vertices of all hole polys have to be in clockwise order
+	//   monotonePolys : a list of monotone polygons (result)
+	//returns 1 on success, 0 on failure
+	int MonotonePartition(std::list<TriangulatorPoly> *inpolys, std::list<TriangulatorPoly> *monotonePolys);
+
+	//partitions a polygon into convex polygons by using Hertel-Mehlhorn algorithm
+	//the algorithm gives at most four times the number of parts as the optimal algorithm
+	//however, in practice it works much better than that and often gives optimal partition
+	//uses triangulation obtained by ear clipping as intermediate result
+	//time complexity O(n^2), n is the number of vertices
+	//space complexity: O(n)
+	//params:
+	//   poly : an input polygon to be partitioned
+	//          vertices have to be in counter-clockwise order
+	//   parts : resulting list of convex polygons
+	//returns 1 on success, 0 on failure
+	int ConvexPartition_HM(TriangulatorPoly *poly, std::list<TriangulatorPoly> *parts);
+
+	//partitions a list of polygons into convex parts by using Hertel-Mehlhorn algorithm
+	//the algorithm gives at most four times the number of parts as the optimal algorithm
+	//however, in practice it works much better than that and often gives optimal partition
+	//uses triangulation obtained by ear clipping as intermediate result
+	//time complexity O(n^2), n is the number of vertices
+	//space complexity: O(n)
+	//params:
+	//   inpolys : an input list of polygons to be partitioned
+	//             vertices of all non-hole polys have to be in counter-clockwise order
+	//             vertices of all hole polys have to be in clockwise order
+	//   parts : resulting list of convex polygons
+	//returns 1 on success, 0 on failure
+	int ConvexPartition_HM(std::list<TriangulatorPoly> *inpolys, std::list<TriangulatorPoly> *parts);
+
+	//optimal convex partitioning (in terms of number of resulting convex polygons)
+	//using the Keil-Snoeyink algorithm
+	//M. Keil, J. Snoeyink, "On the time bound for convex decomposition of simple polygons", 1998
+	//time complexity O(n^3), n is the number of vertices
+	//space complexity: O(n^3)
+	//   poly : an input polygon to be partitioned
+	//          vertices have to be in counter-clockwise order
+	//   parts : resulting list of convex polygons
+	//returns 1 on success, 0 on failure
+	int ConvexPartition_OPT(TriangulatorPoly *poly, std::list<TriangulatorPoly> *parts);
+};
+
+
+#endif

demos/2d/navpoly/engine.cfg

@@ -0,0 +1,4 @@
+[application]
+
+name="Navigation Polygon (2D)"
+main_scene="res://navigation.scn"

demos/2d/navpoly/navigation.gd

@@ -0,0 +1,63 @@
+
+extends Navigation2D
+
+# member variables here, example:
+# var a=2
+# var b="textvar"
+var begin=Vector2()
+var end=Vector2()
+var path=[]
+
+const SPEED=200.0
+
+func _process(delta):
+
+
+	if (path.size()>1):
+	
+		var to_walk = delta*SPEED
+		while(to_walk>0 and path.size()>=2):
+			var pfrom = path[path.size()-1]
+			var pto = path[path.size()-2]
+			var d = pfrom.distance_to(pto)
+			if (d<=to_walk):
+				path.remove(path.size()-1)
+				to_walk-=d
+			else:
+				path[path.size()-1] = pfrom.linear_interpolate(pto,to_walk/d)
+				to_walk=0
+				
+		var atpos = path[path.size()-1]	
+		get_node("agent").set_pos(atpos)
+		
+		if (path.size()<2):
+			path=[]
+			set_process(false)
+				
+	else:
+		set_process(false)
+
+
+
+func _update_path():
+
+	var p = get_simple_path(begin,end,true)
+	path=Array(p) # Vector2array to complex to use, convert to regular array
+	path.invert()
+	
+	set_process(true)
+
+
+func _input(ev):
+	if (ev.type==InputEvent.MOUSE_BUTTON and ev.pressed and ev.button_index==1):
+		begin=get_node("agent").get_pos()
+		#mouse to local navigatio cooards
+		end=ev.pos - get_pos()
+		_update_path()
+
+func _ready():
+	# Initialization here
+	set_process_input(true)
+	pass
+
+

scene/2d/navigation2d.cpp

@@ -0,0 +1,623 @@
+#include "navigation2d.h"
+
+void Navigation2D::_navpoly_link(int p_id) {
+
+	ERR_FAIL_COND(!navpoly_map.has(p_id));
+	NavMesh &nm=navpoly_map[p_id];
+	ERR_FAIL_COND(nm.linked);
+
+	print_line("LINK");
+
+	DVector<Vector2> vertices=nm.navpoly->get_vertices();
+	int len = vertices.size();
+	if (len==0)
+		return;
+
+	DVector<Vector2>::Read r=vertices.read();
+
+	for(int i=0;i<nm.navpoly->get_polygon_count();i++) {
+
+		//build
+
+		List<Polygon>::Element *P=nm.polygons.push_back(Polygon());
+		Polygon &p=P->get();
+		p.owner=&nm;
+
+		Vector<int> poly = nm.navpoly->get_polygon(i);
+		int plen=poly.size();
+		const int *indices=poly.ptr();
+		bool valid=true;
+		p.edges.resize(plen);
+
+		Vector2 center;
+
+		for(int j=0;j<plen;j++) {
+
+			int idx = indices[j];
+			if (idx<0 || idx>=len) {
+				valid=false;
+				break;
+			}
+
+			Polygon::Edge e;
+			Vector2 ep=nm.xform.xform(r[idx]);
+			center+=ep;
+			e.point=_get_point(ep);
+			p.edges[j]=e;
+		}
+
+		if (!valid) {
+			nm.polygons.pop_back();
+			ERR_CONTINUE(!valid);
+			continue;
+		}
+
+		p.center=center/plen;
+
+		//connect
+
+		for(int j=0;j<plen;j++) {
+
+			int next = (j+1)%plen;
+			EdgeKey ek(p.edges[j].point,p.edges[next].point);
+
+			Map<EdgeKey,Connection>::Element *C=connections.find(ek);
+			if (!C) {
+
+				Connection c;
+				c.A=&p;
+				c.A_edge=j;
+				c.B=NULL;
+				c.B_edge=-1;
+				connections[ek]=c;
+			} else {
+
+				if (C->get().B!=NULL) {
+					print_line(String()+_get_vertex(ek.a)+" -> "+_get_vertex(ek.b));
+				}
+				ERR_CONTINUE(C->get().B!=NULL); //wut
+
+				C->get().B=&p;
+				C->get().B_edge=j;
+				C->get().A->edges[C->get().A_edge].C=&p;
+				C->get().A->edges[C->get().A_edge].C_edge=j;;
+				p.edges[j].C=C->get().A;
+				p.edges[j].C_edge=C->get().A_edge;
+				//connection successful.
+			}
+		}
+	}
+
+	nm.linked=true;
+
+}
+
+
+void Navigation2D::_navpoly_unlink(int p_id) {
+
+	ERR_FAIL_COND(!navpoly_map.has(p_id));
+	NavMesh &nm=navpoly_map[p_id];
+	ERR_FAIL_COND(!nm.linked);
+
+	print_line("UNLINK");
+
+	for (List<Polygon>::Element *E=nm.polygons.front();E;E=E->next()) {
+
+
+		Polygon &p=E->get();
+
+		int ec = p.edges.size();
+		Polygon::Edge *edges=p.edges.ptr();
+
+		for(int i=0;i<ec;i++) {
+			int next = (i+1)%ec;
+
+			EdgeKey ek(edges[i].point,edges[next].point);
+			Map<EdgeKey,Connection>::Element *C=connections.find(ek);
+			ERR_CONTINUE(!C);
+			if (C->get().B) {
+				//disconnect
+
+				C->get().B->edges[C->get().B_edge].C=NULL;
+				C->get().B->edges[C->get().B_edge].C_edge=-1;
+				C->get().A->edges[C->get().A_edge].C=NULL;
+				C->get().A->edges[C->get().A_edge].C_edge=-1;
+
+				if (C->get().A==&E->get()) {
+
+					C->get().A=C->get().B;
+					C->get().A_edge=C->get().B_edge;
+				}
+				C->get().B=NULL;
+				C->get().B_edge=-1;
+
+			} else {
+				connections.erase(C);
+				//erase
+			}
+		}
+	}
+
+	nm.polygons.clear();
+
+	nm.linked=false;
+
+
+}
+
+
+int Navigation2D::navpoly_create(const Ref<NavigationPolygon>& p_mesh, const Matrix32& p_xform, Object *p_owner) {
+
+	int id = last_id++;
+	NavMesh nm;
+	nm.linked=false;
+	nm.navpoly=p_mesh;
+	nm.xform=p_xform;
+	nm.owner=p_owner;
+	navpoly_map[id]=nm;
+
+	_navpoly_link(id);
+
+	return id;
+}
+
+void Navigation2D::navpoly_set_transform(int p_id, const Matrix32& p_xform){
+
+	ERR_FAIL_COND(!navpoly_map.has(p_id));
+	NavMesh &nm=navpoly_map[p_id];
+	if (nm.xform==p_xform)
+		return; //bleh
+	_navpoly_unlink(p_id);
+	nm.xform=p_xform;
+	_navpoly_link(p_id);
+
+
+
+}
+void Navigation2D::navpoly_remove(int p_id){
+
+	ERR_FAIL_COND(!navpoly_map.has(p_id));
+	_navpoly_unlink(p_id);
+	navpoly_map.erase(p_id);
+
+}
+#if 0
+void Navigation2D::_clip_path(Vector<Vector2>& path, Polygon *from_poly, const Vector2& p_to_point, Polygon* p_to_poly) {
+
+	Vector2 from = path[path.size()-1];
+
+	if (from.distance_to(p_to_point)<CMP_EPSILON)
+		return;
+	Plane cut_plane;
+	cut_plane.normal = (from-p_to_point).cross(up);
+	if (cut_plane.normal==Vector2())
+		return;
+	cut_plane.normal.normalize();
+	cut_plane.d = cut_plane.normal.dot(from);
+
+
+	while(from_poly!=p_to_poly) {
+
+		int pe = from_poly->prev_edge;
+		Vector2 a = _get_vertex(from_poly->edges[pe].point);
+		Vector2 b = _get_vertex(from_poly->edges[(pe+1)%from_poly->edges.size()].point);
+
+		from_poly=from_poly->edges[pe].C;
+		ERR_FAIL_COND(!from_poly);
+
+		if (a.distance_to(b)>CMP_EPSILON) {
+
+			Vector2 inters;
+			if (cut_plane.intersects_segment(a,b,&inters)) {
+				if (inters.distance_to(p_to_point)>CMP_EPSILON && inters.distance_to(path[path.size()-1])>CMP_EPSILON) {
+					path.push_back(inters);
+				}
+			}
+		}
+	}
+}
+#endif
+
+Vector<Vector2> Navigation2D::get_simple_path(const Vector2& p_start, const Vector2& p_end, bool p_optimize) {
+
+
+	Polygon *begin_poly=NULL;
+	Polygon *end_poly=NULL;
+	Vector2 begin_point;
+	Vector2 end_point;
+	float begin_d=1e20;
+	float end_d=1e20;
+
+	//look for point inside triangle
+
+	for (Map<int,NavMesh>::Element*E=navpoly_map.front();E;E=E->next()) {
+
+		if (!E->get().linked)
+			continue;
+		for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) {
+
+
+			Polygon &p=F->get();
+			if (begin_d || end_d) {
+				for(int i=2;i<p.edges.size();i++) {
+
+					if (begin_d>0) {
+
+						if (Geometry::is_point_in_triangle(p_start,_get_vertex(p.edges[0].point),_get_vertex(p.edges[i-1].point),_get_vertex(p.edges[i].point))) {
+
+							begin_poly=&p;
+							begin_point=p_start;
+							begin_d=0;
+							if (end_d==0)
+								break;
+
+						}
+					}
+
+					if (end_d>0) {
+
+						if (Geometry::is_point_in_triangle(p_end,_get_vertex(p.edges[0].point),_get_vertex(p.edges[i-1].point),_get_vertex(p.edges[i].point))) {
+
+							end_poly=&p;
+							end_point=p_end;
+							end_d=0;
+							if (begin_d==0)
+								break;
+						}
+					}
+
+				}
+			}
+
+			p.prev_edge=-1;
+		}
+	}
+
+	//start or end not inside triangle.. look for closest segment :|
+	if (begin_d || end_d) {
+		for (Map<int,NavMesh>::Element*E=navpoly_map.front();E;E=E->next()) {
+
+			if (!E->get().linked)
+				continue;
+			for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) {
+
+				Polygon &p=F->get();
+				int es = p.edges.size();
+				for(int i=0;i<es;i++) {
+
+					Vector2 edge[2]={
+						_get_vertex(p.edges[i].point),
+						_get_vertex(p.edges[(i+1)%es].point)
+					};
+
+
+					if (begin_d>0) {
+						Vector2 spoint=Geometry::get_closest_point_to_segment_2d(p_start,edge);
+						float d = spoint.distance_to(p_start);
+						if (d<begin_d) {
+							begin_poly=&p;
+							begin_point=spoint;
+							begin_d=d;
+						}
+					}
+
+					if (end_d>0) {
+						Vector2 spoint=Geometry::get_closest_point_to_segment_2d(p_end,edge);
+						float d = spoint.distance_to(p_end);
+						if (d<end_d) {
+							end_poly=&p;
+							end_point=spoint;
+							end_d=d;
+						}
+					}
+				}
+			}
+		}
+	}
+
+	if (!begin_poly || !end_poly) {
+
+		//print_line("No Path Path");
+		return Vector<Vector2>(); //no path
+	}
+
+	if (begin_poly==end_poly) {
+
+		Vector<Vector2> path;
+		path.resize(2);
+		path[0]=begin_point;
+		path[1]=end_point;
+		//print_line("Direct Path");
+		return path;
+	}
+
+
+	bool found_route=false;
+
+	List<Polygon*> open_list;
+
+	for(int i=0;i<begin_poly->edges.size();i++) {
+
+		if (begin_poly->edges[i].C) {
+
+			begin_poly->edges[i].C->prev_edge=begin_poly->edges[i].C_edge;
+			begin_poly->edges[i].C->distance=begin_poly->center.distance_to(begin_poly->edges[i].C->center);
+			open_list.push_back(begin_poly->edges[i].C);
+
+			if (begin_poly->edges[i].C==end_poly) {
+				found_route=true;
+			}
+		}
+	}
+
+
+	while(!found_route) {
+
+		if (open_list.size()==0) {
+		//	print_line("NOU OPEN LIST");
+			break;
+		}
+		//check open list
+
+		List<Polygon*>::Element *least_cost_poly=NULL;
+		float least_cost=1e30;
+
+		//this could be faster (cache previous results)
+		for (List<Polygon*>::Element *E=open_list.front();E;E=E->next()) {
+
+			Polygon *p=E->get();
+
+
+			float cost=p->distance;
+			cost+=p->center.distance_to(end_point);
+
+			if (cost<least_cost) {
+
+				least_cost_poly=E;
+				least_cost=cost;
+			}
+		}
+
+
+		Polygon *p=least_cost_poly->get();
+		//open the neighbours for search
+
+		for(int i=0;i<p->edges.size();i++) {
+
+
+			Polygon::Edge &e=p->edges[i];
+
+			if (!e.C)
+				continue;
+
+			float distance = p->center.distance_to(e.C->center) + p->distance;
+
+			if (e.C->prev_edge!=-1) {
+				//oh this was visited already, can we win the cost?
+
+				if (e.C->distance>distance) {
+
+					e.C->prev_edge=e.C_edge;
+					e.C->distance=distance;
+				}
+			} else {
+				//add to open neighbours
+
+				e.C->prev_edge=e.C_edge;
+				e.C->distance=distance;
+				open_list.push_back(e.C);
+
+				if (e.C==end_poly) {
+					//oh my reached end! stop algorithm
+					found_route=true;
+					break;
+
+				}
+
+			}
+		}
+
+		if (found_route)
+			break;
+
+		open_list.erase(least_cost_poly);
+	}
+
+	if (found_route) {
+
+		Vector<Vector2> path;
+
+		if (p_optimize) {
+			//string pulling
+
+			Polygon *apex_poly=end_poly;
+			Vector2 apex_point=end_point;
+			Vector2 portal_left=apex_point;
+			Vector2 portal_right=apex_point;
+			Polygon *left_poly=end_poly;
+			Polygon *right_poly=end_poly;
+			Polygon *p=end_poly;
+			path.push_back(end_point);
+
+			while(p) {
+
+				Vector2 left;
+				Vector2 right;
+
+//#define CLOCK_TANGENT(m_a,m_b,m_c) ( ((m_a)-(m_c)).cross((m_a)-(m_b)) )
+#define CLOCK_TANGENT(m_a,m_b,m_c) ((((m_a).x - (m_c).x) * ((m_b).y - (m_c).y) - ((m_b).x - (m_c).x) * ((m_a).y - (m_c).y)))
+
+				if (p==begin_poly) {
+					left=begin_point;
+					right=begin_point;
+				} else {
+					int prev = p->prev_edge;
+					int prev_n = (p->prev_edge+1)%p->edges.size();
+					left = _get_vertex(p->edges[prev].point);
+					right = _get_vertex(p->edges[prev_n].point);
+
+					if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5) < 0){
+						SWAP(left,right);
+					}
+				}
+
+				bool skip=false;
+
+
+				if (CLOCK_TANGENT(apex_point,portal_left,left) >= 0){
+					//process
+					if (portal_left==apex_point || CLOCK_TANGENT(apex_point,left,portal_right) > 0) {
+						left_poly=p;
+						portal_left=left;
+					} else {
+
+						//_clip_path(path,apex_poly,portal_right,right_poly);
+
+						apex_point=portal_right;
+						p=right_poly;
+						left_poly=p;
+						apex_poly=p;
+						portal_left=apex_point;
+						portal_right=apex_point;
+						path.push_back(apex_point);
+						skip=true;
+					}
+				}
+
+				if (!skip && CLOCK_TANGENT(apex_point,portal_right,right) <= 0){
+					//process
+					if (portal_right==apex_point || CLOCK_TANGENT(apex_point,right,portal_left) < 0) {
+						right_poly=p;
+						portal_right=right;
+					} else {
+
+						//_clip_path(path,apex_poly,portal_left,left_poly);
+
+						apex_point=portal_left;
+						p=left_poly;
+						right_poly=p;
+						apex_poly=p;
+						portal_right=apex_point;
+						portal_left=apex_point;
+						path.push_back(apex_point);
+					}
+				}
+
+				if (p!=begin_poly)
+					p=p->edges[p->prev_edge].C;
+				else
+					p=NULL;
+
+			}
+
+			if (path[path.size()-1]!=begin_point)
+				path.push_back(begin_point);
+
+			path.invert();
+
+
+
+
+		} else {
+			//midpoints
+			Polygon *p=end_poly;
+
+			path.push_back(end_point);
+			while(true) {
+				int prev = p->prev_edge;
+				int prev_n = (p->prev_edge+1)%p->edges.size();
+				Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point))*0.5;
+				path.push_back(point);
+				p = p->edges[prev].C;
+				if (p==begin_poly)
+					break;
+			}
+
+			path.push_back(begin_point);
+
+
+			path.invert();;
+		}
+
+		return path;
+	}
+
+
+	return Vector<Vector2>();
+
+}
+
+
+Vector2 Navigation2D::get_closest_point(const Vector2& p_point) {
+
+	Vector2 closest_point=Vector2();
+	float closest_point_d=1e20;
+
+	for (Map<int,NavMesh>::Element*E=navpoly_map.front();E;E=E->next()) {
+
+		if (!E->get().linked)
+			continue;
+		for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) {
+
+			Polygon &p=F->get();
+			for(int i=2;i<p.edges.size();i++) {
+
+				if (Geometry::is_point_in_triangle(p_point,_get_vertex(p.edges[0].point),_get_vertex(p.edges[i-1].point),_get_vertex(p.edges[i].point))) {
+
+					return p_point; //inside triangle, nothing else to discuss
+				}
+
+			}
+		}
+	}
+
+	for (Map<int,NavMesh>::Element*E=navpoly_map.front();E;E=E->next()) {
+
+		if (!E->get().linked)
+			continue;
+		for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) {
+
+			Polygon &p=F->get();
+			int es = p.edges.size();
+			for(int i=0;i<es;i++) {
+
+				Vector2 edge[2]={
+					_get_vertex(p.edges[i].point),
+					_get_vertex(p.edges[(i+1)%es].point)
+				};
+
+
+				Vector2 spoint=Geometry::get_closest_point_to_segment_2d(p_point,edge);
+				float d = spoint.distance_squared_to(p_point);
+				if (d<closest_point_d) {
+
+					closest_point=spoint;
+					closest_point_d=d;
+				}
+			}
+		}
+	}
+
+	return closest_point;
+
+}
+
+
+void Navigation2D::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("navpoly_create","mesh:NavigationPolygon","xform","owner"),&Navigation2D::navpoly_create,DEFVAL(Variant()));
+	ObjectTypeDB::bind_method(_MD("navpoly_set_transform","id","xform"),&Navigation2D::navpoly_set_transform);
+	ObjectTypeDB::bind_method(_MD("navpoly_remove","id"),&Navigation2D::navpoly_remove);
+
+	ObjectTypeDB::bind_method(_MD("get_simple_path","start","end","optimize"),&Navigation2D::get_simple_path,DEFVAL(true));
+	ObjectTypeDB::bind_method(_MD("get_closest_point","to_point"),&Navigation2D::get_closest_point);
+
+}
+
+Navigation2D::Navigation2D() {
+
+	ERR_FAIL_COND( sizeof(Point)!=8 );
+	cell_size=1; // one pixel
+	last_id=1;
+
+}

scene/2d/navigation2d.h

@@ -0,0 +1,137 @@
+#ifndef NAVIGATION_2D_H
+#define NAVIGATION_2D_H
+
+#include "scene/2d/node_2d.h"
+#include "scene/2d/navigation_polygon.h"
+
+class Navigation2D : public Node2D {
+
+	OBJ_TYPE( Navigation2D, Node2D);
+
+
+	union Point {
+
+		struct {
+			int64_t x:32;
+			int64_t y:32;
+		};
+
+		uint64_t key;
+		bool operator<(const Point& p_key) const { return key < p_key.key; }
+	};
+
+
+	struct EdgeKey {
+
+		Point a;
+		Point b;
+
+		bool operator<(const EdgeKey& p_key) const {
+			return (a.key==p_key.a.key)?(b.key<p_key.b.key):(a.key<p_key.a.key);
+		};
+
+		EdgeKey(const Point& p_a=Point(),const Point& p_b=Point()) {
+			a=p_a;
+			b=p_b;
+			if (a.key > b.key) {
+				SWAP(a,b);
+			}
+		}
+	};
+
+
+	struct NavMesh;
+
+
+	struct Polygon {
+
+		struct Edge {
+			Point point;
+			Polygon *C; //connection
+			int C_edge;
+			Edge() { C=NULL; C_edge=-1; }
+		};
+
+		Vector<Edge> edges;
+
+		Vector2 center;
+
+		float distance;
+		int prev_edge;
+
+		NavMesh *owner;
+	};
+
+
+	struct Connection {
+
+		Polygon *A;
+		int A_edge;
+		Polygon *B;
+		int B_edge;
+		Connection() { A=NULL; B=NULL; A_edge=-1; B_edge=-1;}
+	};
+
+	Map<EdgeKey,Connection> connections;
+
+
+	struct NavMesh {
+
+		Object *owner;
+		Matrix32 xform;
+		bool linked;
+		Ref<NavigationPolygon> navpoly;
+		List<Polygon> polygons;
+
+	};
+
+
+
+	_FORCE_INLINE_ Point _get_point(const Vector2& p_pos) const {
+
+		int x = int(Math::floor(p_pos.x/cell_size));
+		int y = int(Math::floor(p_pos.y/cell_size));
+
+		Point p;
+		p.key=0;
+		p.x=x;
+		p.y=y;
+		return p;
+
+	}
+
+	_FORCE_INLINE_ Vector2 _get_vertex(const Point& p_point) const {
+
+		return Vector2(p_point.x,p_point.y)*cell_size;
+	}
+
+
+
+	void _navpoly_link(int p_id);
+	void _navpoly_unlink(int p_id);
+
+	float cell_size;
+	Map<int,NavMesh> navpoly_map;
+	int last_id;
+#if 0
+	void _clip_path(Vector<Vector2>& path,Polygon *from_poly, const Vector2& p_to_point, Polygon* p_to_poly);
+#endif
+protected:
+
+	static void _bind_methods();
+
+public:
+
+	//API should be as dynamic as possible
+	int navpoly_create(const Ref<NavigationPolygon>& p_mesh,const Matrix32& p_xform,Object* p_owner=NULL);
+	void navpoly_set_transform(int p_id, const Matrix32& p_xform);
+	void navpoly_remove(int p_id);
+
+	Vector<Vector2> get_simple_path(const Vector2& p_start, const Vector2& p_end,bool p_optimize=true);
+	Vector2 get_closest_point(const Vector2& p_point);
+
+	Navigation2D();
+};
+
+
+#endif // Navigation2D2D_H

scene/2d/navigation_polygon.cpp

@@ -0,0 +1,450 @@
+#include "navigation_polygon.h"
+#include "navigation2d.h"
+#include "triangulator.h"
+#include "core_string_names.h"
+
+void NavigationPolygon::set_vertices(const DVector<Vector2>& p_vertices) {
+
+	vertices=p_vertices;
+}
+
+DVector<Vector2> NavigationPolygon::get_vertices() const{
+
+	return vertices;
+}
+
+
+void NavigationPolygon::_set_polygons(const Array& p_array) {
+
+	polygons.resize(p_array.size());
+	for(int i=0;i<p_array.size();i++) {
+		polygons[i].indices=p_array[i];
+	}
+}
+
+Array NavigationPolygon::_get_polygons() const {
+
+	Array ret;
+	ret.resize(polygons.size());
+	for(int i=0;i<ret.size();i++) {
+		ret[i]=polygons[i].indices;
+	}
+
+	return ret;
+}
+
+void NavigationPolygon::_set_outlines(const Array& p_array) {
+
+	outlines.resize(p_array.size());
+	for(int i=0;i<p_array.size();i++) {
+		outlines[i]=p_array[i];
+	}
+}
+
+Array NavigationPolygon::_get_outlines() const {
+
+	Array ret;
+	ret.resize(outlines.size());
+	for(int i=0;i<ret.size();i++) {
+		ret[i]=outlines[i];
+	}
+
+	return ret;
+}
+
+
+void NavigationPolygon::add_polygon(const Vector<int>& p_polygon){
+
+	Polygon polygon;
+	polygon.indices=p_polygon;
+	polygons.push_back(polygon);
+
+}
+
+void NavigationPolygon::add_outline_at_index(const DVector<Vector2>& p_outline,int p_index) {
+
+	outlines.insert(p_index,p_outline);
+}
+
+int NavigationPolygon::get_polygon_count() const{
+
+	return polygons.size();
+}
+Vector<int> NavigationPolygon::get_polygon(int p_idx){
+
+	ERR_FAIL_INDEX_V(p_idx,polygons.size(),Vector<int>());
+	return polygons[p_idx].indices;
+}
+void NavigationPolygon::clear_polygons(){
+
+	polygons.clear();
+}
+
+void NavigationPolygon::add_outline(const DVector<Vector2>& p_outline) {
+
+	outlines.push_back(p_outline);
+}
+
+int NavigationPolygon::get_outline_count() const{
+
+	return outlines.size();
+}
+
+void NavigationPolygon::set_outline(int p_idx,const DVector<Vector2>& p_outline) {
+	ERR_FAIL_INDEX(p_idx,outlines.size());
+	outlines[p_idx]=p_outline;
+}
+
+void NavigationPolygon::remove_outline(int p_idx) {
+
+	ERR_FAIL_INDEX(p_idx,outlines.size());
+	outlines.remove(p_idx);
+
+}
+
+DVector<Vector2> NavigationPolygon::get_outline(int p_idx) const {
+	ERR_FAIL_INDEX_V(p_idx,outlines.size(),DVector<Vector2>());
+	return outlines[p_idx];
+}
+
+void NavigationPolygon::clear_outlines(){
+
+	outlines.clear();;
+}
+void NavigationPolygon::make_polygons_from_outlines(){
+
+	std::list<TriangulatorPoly> in_poly,out_poly;
+
+	Vector2 outside_point(-1e10,-1e10);
+
+	for(int i=0;i<outlines.size();i++) {
+
+		DVector<Vector2> ol = outlines[i];
+		int olsize = ol.size();
+		if (olsize<3)
+			continue;
+		DVector<Vector2>::Read r=ol.read();
+		for(int j=0;j<olsize;j++) {
+			outside_point.x = MAX( r[j].x, outside_point.x );
+			outside_point.y = MAX( r[j].y, outside_point.y );
+		}
+
+	}
+
+	outside_point+=Vector2(0.7239784,0.819238); //avoid precision issues
+
+
+
+	for(int i=0;i<outlines.size();i++) {
+
+		DVector<Vector2> ol = outlines[i];
+		int olsize = ol.size();
+		if (olsize<3)
+			continue;
+		DVector<Vector2>::Read r=ol.read();
+
+		int interscount=0;
+		//test if this is an outer outline
+		for(int k=0;k<outlines.size();k++) {
+
+			if (i==k)
+				continue; //no self intersect
+
+			DVector<Vector2> ol2 = outlines[k];
+			int olsize2 = ol2.size();
+			if (olsize2<3)
+				continue;
+			DVector<Vector2>::Read r2=ol2.read();
+
+			for(int l=0;l<olsize2;l++) {
+
+				if (Geometry::segment_intersects_segment_2d(r[0],outside_point,r2[l],r2[(l+1)%olsize2],NULL)) {
+					interscount++;
+				}
+			}
+
+		}
+
+		bool outer = (interscount%2)==0;
+
+		TriangulatorPoly tp;
+		tp.Init(olsize);
+		for(int j=0;j<olsize;j++) {
+			tp[j]=r[j];
+		}
+
+		if (outer)
+			tp.SetOrientation(TRIANGULATOR_CCW);
+		else {
+			tp.SetOrientation(TRIANGULATOR_CW);
+			tp.SetHole(true);
+		}
+
+		in_poly.push_back(tp);
+	}
+
+
+	TriangulatorPartition tpart;
+	if (tpart.ConvexPartition_HM(&in_poly,&out_poly)==0) { //failed!
+		print_line("convex partition failed!");
+		return;
+	}
+
+	polygons.clear();
+	vertices.resize(0);
+
+	Map<Vector2,int> points;
+	for(std::list<TriangulatorPoly>::iterator I = out_poly.begin();I!=out_poly.end();I++) {
+
+		TriangulatorPoly& tp = *I;
+
+		struct Polygon p;
+
+		for(int i=0;i<tp.GetNumPoints();i++) {
+
+			Map<Vector2,int>::Element *E=points.find(tp[i]);
+			if (!E) {
+				E=points.insert(tp[i],vertices.size());
+				vertices.push_back(tp[i]);
+			}
+			p.indices.push_back(E->get());
+		}
+
+		polygons.push_back(p);
+	}
+
+	emit_signal(CoreStringNames::get_singleton()->changed);
+}
+
+
+void NavigationPolygon::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("set_vertices","vertices"),&NavigationPolygon::set_vertices);
+	ObjectTypeDB::bind_method(_MD("get_vertices"),&NavigationPolygon::get_vertices);
+
+	ObjectTypeDB::bind_method(_MD("add_polygon","polygon"),&NavigationPolygon::add_polygon);
+	ObjectTypeDB::bind_method(_MD("get_polygon_count"),&NavigationPolygon::get_polygon_count);
+	ObjectTypeDB::bind_method(_MD("get_polygon","idx"),&NavigationPolygon::get_polygon);
+	ObjectTypeDB::bind_method(_MD("clear_polygons"),&NavigationPolygon::clear_polygons);
+
+	ObjectTypeDB::bind_method(_MD("add_outline","outline"),&NavigationPolygon::add_outline);
+	ObjectTypeDB::bind_method(_MD("add_outline_at_index","outline","index"),&NavigationPolygon::add_outline_at_index);
+	ObjectTypeDB::bind_method(_MD("get_outline_count"),&NavigationPolygon::get_outline_count);
+	ObjectTypeDB::bind_method(_MD("set_outline","idx","outline"),&NavigationPolygon::set_outline);
+	ObjectTypeDB::bind_method(_MD("get_outline","idx"),&NavigationPolygon::get_outline);
+	ObjectTypeDB::bind_method(_MD("remove_outline","idx"),&NavigationPolygon::remove_outline);
+	ObjectTypeDB::bind_method(_MD("clear_outlines"),&NavigationPolygon::clear_outlines);
+	ObjectTypeDB::bind_method(_MD("make_polygons_from_outlines"),&NavigationPolygon::make_polygons_from_outlines);
+
+	ObjectTypeDB::bind_method(_MD("_set_polygons","polygons"),&NavigationPolygon::_set_polygons);
+	ObjectTypeDB::bind_method(_MD("_get_polygons"),&NavigationPolygon::_get_polygons);
+
+	ObjectTypeDB::bind_method(_MD("_set_outlines","outlines"),&NavigationPolygon::_set_outlines);
+	ObjectTypeDB::bind_method(_MD("_get_outlines"),&NavigationPolygon::_get_outlines);
+
+	ADD_PROPERTY(PropertyInfo(Variant::VECTOR3_ARRAY,"vertices",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_NOEDITOR),_SCS("set_vertices"),_SCS("get_vertices"));
+	ADD_PROPERTY(PropertyInfo(Variant::ARRAY,"polygons",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_NOEDITOR),_SCS("_set_polygons"),_SCS("_get_polygons"));
+	ADD_PROPERTY(PropertyInfo(Variant::ARRAY,"outlines",PROPERTY_HINT_NONE,"",PROPERTY_USAGE_NOEDITOR),_SCS("_set_outlines"),_SCS("_get_outlines"));
+}
+
+NavigationPolygon::NavigationPolygon() {
+
+
+}
+
+void NavigationPolygonInstance::set_enabled(bool p_enabled) {
+
+	if (enabled==p_enabled)
+		return;
+	enabled=p_enabled;
+
+	if (!is_inside_tree())
+		return;
+
+	if (!enabled) {
+
+		if (nav_id!=-1) {
+			navigation->navpoly_remove(nav_id);
+			nav_id=-1;
+		}
+	} else {
+
+		if (navigation) {
+
+			if (navpoly.is_valid()) {
+
+				nav_id = navigation->navpoly_create(navpoly,get_relative_transform(navigation),this);
+			}
+		}
+
+	}
+
+	if (get_tree()->is_editor_hint())
+		update();
+
+//	update_gizmo();
+}
+
+bool NavigationPolygonInstance::is_enabled() const {
+
+
+	return enabled;
+}
+
+
+/////////////////////////////
+
+
+void NavigationPolygonInstance::_notification(int p_what) {
+
+
+	switch(p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+
+			Node2D *c=this;
+			while(c) {
+
+				navigation=c->cast_to<Navigation2D>();
+				if (navigation) {
+
+					if (enabled && navpoly.is_valid()) {
+
+						nav_id = navigation->navpoly_create(navpoly,get_relative_transform(navigation),this);
+					}
+					break;
+				}
+
+				c=c->get_parent()->cast_to<Node2D>();
+			}
+
+		} break;
+		case NOTIFICATION_TRANSFORM_CHANGED: {
+
+			if (navigation && nav_id!=-1) {
+				navigation->navpoly_set_transform(nav_id,get_relative_transform(navigation));
+			}
+
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+
+			if (navigation) {
+
+				if (nav_id!=-1) {
+					navigation->navpoly_remove(nav_id);
+					nav_id=-1;
+				}
+			}
+			navigation=NULL;
+		} break;
+		case NOTIFICATION_DRAW: {
+
+			if (is_inside_tree() && get_tree()->is_editor_hint() && navpoly.is_valid()) {
+
+				DVector<Vector2> verts=navpoly->get_vertices();
+				int vsize = verts.size();
+				if (vsize<3)
+					return;
+
+
+				Color color;
+				if (enabled) {
+					color=Color(0.1,0.8,1.0,0.4);
+				} else {
+					color=Color(1.0,0.8,0.1,0.4);
+				}
+				Vector<Color> colors;
+				Vector<Vector2> vertices;
+				vertices.resize(vsize);
+				colors.resize(vsize);
+				{
+					DVector<Vector2>::Read vr = verts.read();
+					for(int i=0;i<vsize;i++) {
+						vertices[i]=vr[i];
+						colors[i]=color;
+					}
+				}
+
+				Vector<int> indices;
+
+
+				for(int i=0;i<navpoly->get_polygon_count();i++) {
+					Vector<int> polygon = navpoly->get_polygon(i);
+
+					for(int j=2;j<polygon.size();j++) {
+
+						int kofs[3]={0,j-1,j};
+						for(int k=0;k<3;k++) {
+
+							int idx = polygon[ kofs[k] ];
+							ERR_FAIL_INDEX(idx,vsize);
+							indices.push_back(idx);
+						}
+					}
+				}
+				VS::get_singleton()->canvas_item_add_triangle_array(get_canvas_item(),indices,vertices,colors);
+
+			}
+		} break;
+
+	}
+}
+
+
+void NavigationPolygonInstance::set_navigation_polygon(const Ref<NavigationPolygon>& p_navpoly) {
+
+	if (p_navpoly==navpoly)
+		return;
+
+	if (navigation && nav_id!=-1) {
+		navigation->navpoly_remove(nav_id);
+		nav_id=-1;
+	}
+	if (navpoly.is_valid()) {
+		navpoly->disconnect(CoreStringNames::get_singleton()->changed,this,"_navpoly_changed");
+	}
+	navpoly=p_navpoly;
+
+	if (navpoly.is_valid()) {
+		navpoly->connect(CoreStringNames::get_singleton()->changed,this,"_navpoly_changed");
+	}
+
+	if (navigation && navpoly.is_valid() && enabled) {
+		nav_id = navigation->navpoly_create(navpoly,get_relative_transform(navigation),this);
+	}
+	//update_gizmo();
+	_change_notify("navpoly");
+
+}
+
+Ref<NavigationPolygon> NavigationPolygonInstance::get_navigation_polygon() const{
+
+	return navpoly;
+}
+
+void NavigationPolygonInstance::_navpoly_changed() {
+
+	if (is_inside_tree() && get_tree()->is_editor_hint())
+		update();
+}
+
+void NavigationPolygonInstance::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("set_navigation_polygon","navpoly"),&NavigationPolygonInstance::set_navigation_polygon);
+	ObjectTypeDB::bind_method(_MD("get_navigation_polygon"),&NavigationPolygonInstance::get_navigation_polygon);
+
+	ObjectTypeDB::bind_method(_MD("set_enabled","enabled"),&NavigationPolygonInstance::set_enabled);
+	ObjectTypeDB::bind_method(_MD("is_enabled"),&NavigationPolygonInstance::is_enabled);
+
+	ObjectTypeDB::bind_method(_MD("_navpoly_changed"),&NavigationPolygonInstance::_navpoly_changed);
+
+	ADD_PROPERTY( PropertyInfo(Variant::OBJECT,"navpoly",PROPERTY_HINT_RESOURCE_TYPE,"NavigationPolygon"),_SCS("set_navigation_polygon"),_SCS("get_navigation_polygon"));
+	ADD_PROPERTY( PropertyInfo(Variant::BOOL,"enabled"),_SCS("set_enabled"),_SCS("is_enabled"));
+}
+
+NavigationPolygonInstance::NavigationPolygonInstance() {
+
+	navigation=NULL;
+	nav_id=-1;
+	enabled=true;
+
+}

scene/2d/navigation_polygon.h

@@ -0,0 +1,84 @@
+#ifndef NAVIGATION_POLYGON_H
+#define NAVIGATION_POLYGON_H
+
+#include "scene/2d/node_2d.h"
+
+
+class NavigationPolygon : public Resource  {
+
+	OBJ_TYPE( NavigationPolygon, Resource );
+
+	DVector<Vector2> vertices;
+	struct Polygon {
+		Vector<int> indices;
+	};
+	Vector<Polygon> polygons;
+	Vector< DVector<Vector2> > outlines;
+
+protected:
+
+	static void _bind_methods();
+
+	void _set_polygons(const Array& p_array);
+	Array _get_polygons() const;
+
+	void _set_outlines(const Array& p_array);
+	Array _get_outlines() const;
+
+public:
+
+
+
+	void set_vertices(const DVector<Vector2>& p_vertices);
+	DVector<Vector2> get_vertices() const;
+
+	void add_polygon(const Vector<int>& p_polygon);
+	int get_polygon_count() const;
+
+	void add_outline(const DVector<Vector2>& p_outline);
+	void add_outline_at_index(const DVector<Vector2>& p_outline,int p_index);
+	void set_outline(int p_idx,const DVector<Vector2>& p_outline);
+	DVector<Vector2> get_outline(int p_idx) const;
+	void remove_outline(int p_idx);
+	int get_outline_count() const;
+
+	void clear_outlines();
+	void make_polygons_from_outlines();
+
+	Vector<int> get_polygon(int p_idx);
+	void clear_polygons();
+
+	NavigationPolygon();
+};
+
+
+class Navigation2D;
+
+class NavigationPolygonInstance : public Node2D {
+
+	OBJ_TYPE(NavigationPolygonInstance,Node2D);
+
+	bool enabled;
+	int nav_id;
+	Navigation2D *navigation;
+	Ref<NavigationPolygon> navpoly;
+
+	void _navpoly_changed();
+
+protected:
+
+	void _notification(int p_what);
+	static void _bind_methods();
+public:
+
+	void set_enabled(bool p_enabled);
+	bool is_enabled() const;
+
+	void set_navigation_polygon(const Ref<NavigationPolygon>& p_navpoly);
+	Ref<NavigationPolygon> get_navigation_polygon() const;
+
+	NavigationPolygonInstance();
+};
+
+
+#endif // NAVIGATIONPOLYGON_H

scene/2d/node_2d.cpp

@@ -317,6 +317,18 @@ int Node2D::get_z() const{
 	return z;
 }
 
+Matrix32 Node2D::get_relative_transform(const Node *p_parent) const {
+
+	if (p_parent==this)
+		return Matrix32();
+
+	Node2D *parent_2d = get_parent()->cast_to<Node2D>();
+	ERR_FAIL_COND_V(!parent_2d,Matrix32());
+	if (p_parent==parent_2d)
+		return get_transform();
+	else
+		return parent_2d->get_relative_transform(p_parent) * get_transform();
+}
 
 void Node2D::_bind_methods() {
 
@@ -351,6 +363,8 @@ void Node2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("edit_set_pivot"),&Node2D::edit_set_pivot);
 
+	ObjectTypeDB::bind_method(_MD("get_relative_transform"),&Node2D::get_relative_transform);
+
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2,"transform/pos"),_SCS("set_pos"),_SCS("get_pos"));
 	ADD_PROPERTY(PropertyInfo(Variant::REAL,"transform/rot",PROPERTY_HINT_RANGE,"-1440,1440,0.1"),_SCS("_set_rotd"),_SCS("_get_rotd"));
 	ADD_PROPERTY(PropertyInfo(Variant::VECTOR2,"transform/scale"),_SCS("set_scale"),_SCS("get_scale"));

scene/2d/node_2d.h

@@ -93,6 +93,9 @@ public:
 	void set_z_as_relative(bool p_enabled);
 	bool is_z_relative() const;
 
+	Matrix32 get_relative_transform(const Node *p_parent) const;
+
+
 	Matrix32 get_transform() const;
 
 	Node2D();

scene/gui/dialogs.cpp

@@ -328,8 +328,8 @@ AcceptDialog::AcceptDialog() {
 	label->set_anchor(MARGIN_RIGHT,ANCHOR_END);
 	label->set_anchor(MARGIN_BOTTOM,ANCHOR_END);
 	label->set_begin( Point2( margin, margin) );
-	label->set_end( Point2( margin, button_margin) );
-	label->set_autowrap(true);
+	label->set_end( Point2( margin, button_margin+10) );
+	//label->set_autowrap(true);
 	add_child(label);
 
 	hbc = memnew( HBoxContainer );

scene/gui/popup.cpp

@@ -94,6 +94,8 @@ void Popup::popup_centered_minsize(const Size2& p_minsize) {
 		Control *c=get_child(i)->cast_to<Control>();
 		if (!c)
 			continue;
+		if (c->is_hidden())
+			continue;
 
 		Size2 minsize = c->get_combined_minimum_size();
 
@@ -114,6 +116,8 @@ void Popup::popup_centered_minsize(const Size2& p_minsize) {
 
 		}
 
+		print_line(String(c->get_type())+": "+minsize);
+
 		total_minsize.width = MAX( total_minsize.width, minsize.width );
 		total_minsize.height = MAX( total_minsize.height, minsize.height );
 	}

scene/register_scene_types.cpp

@@ -102,6 +102,7 @@
 #include "scene/2d/screen_button.h"
 #include "scene/2d/remote_transform_2d.h"
 #include "scene/2d/y_sort.h"
+#include "scene/2d/navigation2d.h"
 
 #include "scene/2d/position_2d.h"
 #include "scene/2d/tile_map.h"
@@ -575,6 +576,10 @@ void register_scene_types() {
 	ObjectTypeDB::register_type<Path2D>();
 	ObjectTypeDB::register_type<PathFollow2D>();
 
+	ObjectTypeDB::register_type<Navigation2D>();
+	ObjectTypeDB::register_type<NavigationPolygon>();
+	ObjectTypeDB::register_type<NavigationPolygonInstance>();
+
 	OS::get_singleton()->yield(); //may take time to init
 
 	ObjectTypeDB::register_type<PackedScene>();

tools/editor/editor_node.cpp

@@ -89,6 +89,7 @@
 #include "plugins/animation_player_editor_plugin.h"
 #include "plugins/baked_light_editor_plugin.h"
 #include "plugins/polygon_2d_editor_plugin.h"
+#include "plugins/navigation_polygon_editor_plugin.h"
 // end
 #include "tools/editor/io_plugins/editor_texture_import_plugin.h"
 #include "tools/editor/io_plugins/editor_scene_import_plugin.h"
@@ -3260,6 +3261,11 @@ Error EditorNode::export_platform(const String& p_platform, const String& p_path
 	return OK;
 }
 
+void EditorNode::show_warning(const String& p_text) {
+
+	warning->set_text(p_text);
+	warning->popup_centered_minsize();
+}
 
 
 EditorNode::EditorNode() {
@@ -3970,6 +3976,8 @@ EditorNode::EditorNode() {
 	logo->set_pos(Point2(20,20));
 	logo->set_texture(gui_base->get_icon("Logo","EditorIcons") );
 
+	warning = memnew( AcceptDialog );
+	add_child(warning);
 
 
 
@@ -4107,6 +4115,7 @@ EditorNode::EditorNode() {
 	add_editor_plugin( memnew( PathEditorPlugin(this) ) );
 	add_editor_plugin( memnew( BakedLightEditorPlugin(this) ) );
 	add_editor_plugin( memnew( Polygon2DEditorPlugin(this) ) );
+	add_editor_plugin( memnew( NavigationPolygonEditorPlugin(this) ) );
 
 	for(int i=0;i<EditorPlugins::get_plugin_count();i++)
 		add_editor_plugin( EditorPlugins::create(i,this) );

tools/editor/editor_node.h

@@ -232,6 +232,7 @@ class EditorNode : public Node {
 	ConfirmationDialog *open_recent_confirmation;
 	AcceptDialog *accept;
 	AcceptDialog *about;
+	AcceptDialog *warning;
 
 	//OptimizedPresetsDialog *optimized_presets;
 	EditorSettingsDialog *settings_config_dialog;
@@ -484,6 +485,9 @@ public:
 	Ref<Theme> get_editor_theme() const { return theme; }
 
 
+	void show_warning(const String& p_text);
+
+
 	Error export_platform(const String& p_platform, const String& p_path, bool p_debug,const String& p_password,bool p_quit_after=false);
 
 	static void register_editor_types();

tools/editor/plugins/navigation_polygon_editor_plugin.cpp

@@ -0,0 +1,547 @@
+#include "navigation_polygon_editor_plugin.h"
+
+#include "canvas_item_editor_plugin.h"
+#include "os/file_access.h"
+#include "tools/editor/editor_settings.h"
+
+void NavigationPolygonEditor::_notification(int p_what) {
+
+	switch(p_what) {
+
+		case NOTIFICATION_READY: {
+
+			button_create->set_icon( get_icon("Edit","EditorIcons"));
+			button_edit->set_icon( get_icon("MovePoint","EditorIcons"));
+			button_edit->set_pressed(true);
+			get_tree()->connect("node_removed",this,"_node_removed");
+			create_nav->connect("confirmed",this,"_create_nav");
+
+		} break;
+		case NOTIFICATION_FIXED_PROCESS: {
+
+
+		} break;
+	}
+
+}
+void NavigationPolygonEditor::_node_removed(Node *p_node) {
+
+	if(p_node==node) {
+		node=NULL;
+		hide();
+		canvas_item_editor->get_viewport_control()->update();
+	}
+
+}
+
+void NavigationPolygonEditor::_create_nav()  {
+
+	undo_redo->create_action("Create Navigation Polygon");
+	undo_redo->add_do_method(node,"set_navigation_polygon",Ref<NavigationPolygon>(memnew( NavigationPolygon)));
+	undo_redo->add_undo_method(node,"set_navigation_polygon",Variant(REF()));
+	undo_redo->commit_action();
+}
+
+Vector2 NavigationPolygonEditor::snap_point(const Vector2& p_point) const {
+
+	if (canvas_item_editor->is_snap_active()) {
+
+		return p_point.snapped(Vector2(1,1)*canvas_item_editor->get_snap());
+
+	} else {
+		return p_point;
+	}
+}
+
+void NavigationPolygonEditor::_menu_option(int p_option) {
+
+	switch(p_option) {
+
+		case MODE_CREATE: {
+
+			mode=MODE_CREATE;
+			button_create->set_pressed(true);
+			button_edit->set_pressed(false);
+		} break;
+		case MODE_EDIT: {
+
+			mode=MODE_EDIT;
+			button_create->set_pressed(false);
+			button_edit->set_pressed(true);
+		} break;
+
+	}
+}
+
+void NavigationPolygonEditor::_wip_close() {
+
+
+	if (wip.size()>=3) {
+
+		undo_redo->create_action("Create Poly");
+		undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"remove_outline",node->get_navigation_polygon()->get_outline_count());
+		undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"add_outline",wip);
+		undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
+		undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
+		undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
+		undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
+		undo_redo->commit_action();
+		mode=MODE_EDIT;
+		button_edit->set_pressed(true);
+		button_create->set_pressed(false);
+	}
+
+	wip.clear();
+	wip_active=false;
+	edited_point=-1;
+}
+
+bool NavigationPolygonEditor::forward_input_event(const InputEvent& p_event) {
+
+
+	if (!node)
+		return false;
+
+	if (node->get_navigation_polygon().is_null()) {
+		if (p_event.type==InputEvent::MOUSE_BUTTON && p_event.mouse_button.button_index==1 && p_event.mouse_button.pressed) {
+			create_nav->set_text("No NavigationPolygon resource on this node.\nCreate and assign one?");
+			create_nav->popup_centered_minsize();
+		}
+		return false;
+	}
+
+
+	switch(p_event.type) {
+
+		case InputEvent::MOUSE_BUTTON: {
+
+			const InputEventMouseButton &mb=p_event.mouse_button;
+
+			Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
+
+
+			Vector2 gpoint = Point2(mb.x,mb.y);
+			Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
+			cpoint=snap_point(cpoint);
+			cpoint = node->get_global_transform().affine_inverse().xform(cpoint);
+
+
+
+			//first check if a point is to be added (segment split)
+			real_t grab_treshold=EDITOR_DEF("poly_editor/point_grab_radius",8);
+
+			switch(mode) {
+
+
+				case MODE_CREATE: {
+
+					if (mb.button_index==BUTTON_LEFT && mb.pressed) {
+
+
+						if (!wip_active) {
+
+							wip.clear();
+							wip.push_back( cpoint );
+							wip_active=true;
+							edited_point_pos=cpoint;
+							edited_outline=-1;
+							canvas_item_editor->get_viewport_control()->update();
+							edited_point=1;
+							return true;
+						} else {
+
+
+							if (wip.size()>1 && xform.xform(wip[0]).distance_to(gpoint)<grab_treshold) {
+								//wip closed
+								_wip_close();
+
+								return true;
+							} else {
+
+								wip.push_back( cpoint );
+								edited_point=wip.size();
+								canvas_item_editor->get_viewport_control()->update();
+								return true;
+
+								//add wip point
+							}
+						}
+					} else if (mb.button_index==BUTTON_RIGHT && mb.pressed && wip_active) {
+						_wip_close();
+					}
+
+
+
+				} break;
+
+				case MODE_EDIT: {
+
+					if (mb.button_index==BUTTON_LEFT) {
+						if (mb.pressed) {
+
+							if (mb.mod.control) {
+
+
+								//search edges
+								int closest_outline=-1;
+								int closest_idx=-1;
+								Vector2 closest_pos;
+								real_t closest_dist=1e10;
+
+								for(int j=0;j<node->get_navigation_polygon()->get_outline_count();j++) {
+
+
+									DVector<Vector2> points=node->get_navigation_polygon()->get_outline(j);
+
+									int pc=points.size();
+									DVector<Vector2>::Read poly=points.read();
+
+									for(int i=0;i<pc;i++) {
+
+										Vector2 points[2] ={ xform.xform(poly[i]),
+											xform.xform(poly[(i+1)%pc]) };
+
+										Vector2 cp = Geometry::get_closest_point_to_segment_2d(gpoint,points);
+										if (cp.distance_squared_to(points[0])<CMP_EPSILON2 || cp.distance_squared_to(points[1])<CMP_EPSILON2)
+											continue; //not valid to reuse point
+
+										real_t d = cp.distance_to(gpoint);
+										if (d<closest_dist && d<grab_treshold) {
+											closest_dist=d;
+											closest_outline=j;
+											closest_pos=cp;
+											closest_idx=i;
+										}
+
+
+									}
+								}
+
+								if (closest_idx>=0) {
+
+									pre_move_edit=node->get_navigation_polygon()->get_outline(closest_outline);
+									DVector<Point2> poly = pre_move_edit;
+									poly.insert(closest_idx+1,xform.affine_inverse().xform(closest_pos));
+									edited_point=closest_idx+1;
+									edited_outline=closest_outline;
+									edited_point_pos=xform.affine_inverse().xform(closest_pos);
+									node->get_navigation_polygon()->set_outline(closest_outline,poly);
+									canvas_item_editor->get_viewport_control()->update();
+									return true;
+								}
+							} else {
+
+								//look for points to move
+								int closest_outline=-1;
+								int closest_idx=-1;
+								Vector2 closest_pos;
+								real_t closest_dist=1e10;
+
+								for(int j=0;j<node->get_navigation_polygon()->get_outline_count();j++) {
+
+
+									DVector<Vector2> points=node->get_navigation_polygon()->get_outline(j);
+
+									int pc=points.size();
+									DVector<Vector2>::Read poly=points.read();
+
+									for(int i=0;i<pc;i++) {
+
+
+										Vector2 cp =xform.xform(poly[i]);
+
+										real_t d = cp.distance_to(gpoint);
+										if (d<closest_dist && d<grab_treshold) {
+											closest_dist=d;
+											closest_pos=cp;
+											closest_outline=j;
+											closest_idx=i;
+										}
+									}
+								}
+
+								if (closest_idx>=0) {
+
+									pre_move_edit=node->get_navigation_polygon()->get_outline(closest_outline);
+									edited_point=closest_idx;
+									edited_outline=closest_outline;
+									edited_point_pos=xform.affine_inverse().xform(closest_pos);
+									canvas_item_editor->get_viewport_control()->update();
+									return true;
+								}
+							}
+						} else {
+
+							if (edited_point!=-1) {
+
+								//apply
+
+								DVector<Vector2> poly = node->get_navigation_polygon()->get_outline(edited_outline);
+								ERR_FAIL_INDEX_V(edited_point,poly.size(),false);
+								poly.set(edited_point,edited_point_pos);
+								undo_redo->create_action("Edit Poly");
+								undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"set_outline",edited_outline,poly);
+								undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"set_outline",edited_outline,pre_move_edit);
+								undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
+								undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
+								undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
+								undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
+								undo_redo->commit_action();
+
+								edited_point=-1;
+								return true;
+							}
+						}
+					} if (mb.button_index==BUTTON_RIGHT && mb.pressed && edited_point==-1) {
+
+						int closest_outline=-1;
+						int closest_idx=-1;
+						Vector2 closest_pos;
+						real_t closest_dist=1e10;
+
+						for(int j=0;j<node->get_navigation_polygon()->get_outline_count();j++) {
+
+
+							DVector<Vector2> points=node->get_navigation_polygon()->get_outline(j);
+
+							int pc=points.size();
+							DVector<Vector2>::Read poly=points.read();
+
+							for(int i=0;i<pc;i++) {
+
+
+								Vector2 cp =xform.xform(poly[i]);
+
+								real_t d = cp.distance_to(gpoint);
+								if (d<closest_dist && d<grab_treshold) {
+									closest_dist=d;
+									closest_pos=cp;
+									closest_outline=j;
+									closest_idx=i;
+								}
+							}
+						}
+
+						if (closest_idx>=0) {
+
+
+							DVector<Vector2> poly = node->get_navigation_polygon()->get_outline(closest_outline);
+
+							if (poly.size()>3) {
+								undo_redo->create_action("Edit Poly (Remove Point)");
+								undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"set_outline",closest_outline,poly);
+								poly.remove(closest_idx);
+								undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"set_outline",closest_outline,poly);
+								undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
+								undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
+								undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
+								undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
+								undo_redo->commit_action();
+							} else {
+
+								undo_redo->create_action("Remove Poly And Point");
+								undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"add_outline_at_index",poly,closest_outline);
+								poly.remove(closest_idx);
+								undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"remove_outline",closest_outline);
+								undo_redo->add_do_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
+								undo_redo->add_undo_method(node->get_navigation_polygon().ptr(),"make_polygons_from_outlines");
+								undo_redo->add_do_method(canvas_item_editor->get_viewport_control(),"update");
+								undo_redo->add_undo_method(canvas_item_editor->get_viewport_control(),"update");
+								undo_redo->commit_action();
+
+							}
+							return true;
+						}
+					}
+
+
+
+				} break;
+			}
+
+
+
+		} break;
+		case InputEvent::MOUSE_MOTION: {
+
+			const InputEventMouseMotion &mm=p_event.mouse_motion;
+
+			if (edited_point!=-1 && (wip_active || mm.button_mask&BUTTON_MASK_LEFT)) {
+
+				Vector2 gpoint = Point2(mm.x,mm.y);
+				Vector2 cpoint = canvas_item_editor->get_canvas_transform().affine_inverse().xform(gpoint);
+				cpoint=snap_point(cpoint);
+				edited_point_pos = node->get_global_transform().affine_inverse().xform(cpoint);
+
+				canvas_item_editor->get_viewport_control()->update();
+
+			}
+
+		} break;
+	}
+
+	return false;
+}
+void NavigationPolygonEditor::_canvas_draw() {
+
+	if (!node)
+		return;
+
+	Control *vpc = canvas_item_editor->get_viewport_control();
+	if (node->get_navigation_polygon().is_null())
+			return;
+
+	Matrix32 xform = canvas_item_editor->get_canvas_transform() * node->get_global_transform();
+	Ref<Texture> handle= get_icon("EditorHandle","EditorIcons");
+
+
+
+	for(int j=-1;j<node->get_navigation_polygon()->get_outline_count();j++)	{
+		Vector<Vector2> poly;
+
+		if (wip_active && j==edited_outline) {
+			poly=wip;
+		} else {
+			if (j==-1)
+				continue;
+			poly = Variant(node->get_navigation_polygon()->get_outline(j));
+		}
+
+		int len = poly.size();
+
+		for(int i=0;i<poly.size();i++) {
+
+
+			Vector2 p,p2;
+			p = (j==edited_outline && i==edited_point) ? edited_point_pos : poly[i];
+			if (j==edited_outline && ((wip_active && i==poly.size()-1) || (((i+1)%poly.size())==edited_point)))
+				p2=edited_point_pos;
+			else
+				p2 = poly[(i+1)%poly.size()];
+
+			Vector2 point = xform.xform(p);
+			Vector2 next_point = xform.xform(p2);
+
+			Color col=Color(1,0.3,0.1,0.8);
+			vpc->draw_line(point,next_point,col,2);
+			vpc->draw_texture(handle,point-handle->get_size()*0.5);
+		}
+	}
+}
+
+
+
+void NavigationPolygonEditor::edit(Node *p_collision_polygon) {
+
+	if (!canvas_item_editor) {
+		canvas_item_editor=CanvasItemEditor::get_singleton();
+	}
+
+	if (p_collision_polygon) {
+
+		node=p_collision_polygon->cast_to<NavigationPolygonInstance>();
+		if (!canvas_item_editor->get_viewport_control()->is_connected("draw",this,"_canvas_draw"))
+			canvas_item_editor->get_viewport_control()->connect("draw",this,"_canvas_draw");
+		wip.clear();
+		wip_active=false;
+		edited_point=-1;
+
+	} else {
+		node=NULL;
+
+		if (canvas_item_editor->get_viewport_control()->is_connected("draw",this,"_canvas_draw"))
+			canvas_item_editor->get_viewport_control()->disconnect("draw",this,"_canvas_draw");
+
+	}
+
+}
+
+void NavigationPolygonEditor::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("_menu_option"),&NavigationPolygonEditor::_menu_option);
+	ObjectTypeDB::bind_method(_MD("_canvas_draw"),&NavigationPolygonEditor::_canvas_draw);
+	ObjectTypeDB::bind_method(_MD("_node_removed"),&NavigationPolygonEditor::_node_removed);
+	ObjectTypeDB::bind_method(_MD("_create_nav"),&NavigationPolygonEditor::_create_nav);
+
+}
+
+NavigationPolygonEditor::NavigationPolygonEditor(EditorNode *p_editor) {
+
+	canvas_item_editor=NULL;
+	editor=p_editor;
+	undo_redo = editor->get_undo_redo();
+
+	add_child( memnew( VSeparator ));
+	button_create = memnew( ToolButton );
+	add_child(button_create);
+	button_create->connect("pressed",this,"_menu_option",varray(MODE_CREATE));
+	button_create->set_toggle_mode(true);
+	button_create->set_tooltip("Create a new polygon from scratch");
+
+	button_edit = memnew( ToolButton );
+	add_child(button_edit);
+	button_edit->connect("pressed",this,"_menu_option",varray(MODE_EDIT));
+	button_edit->set_toggle_mode(true);
+	button_edit->set_tooltip("Edit existing polygon:\nLMB: Move Point.\nCtrl+LMB: Split Segment.\nRMB: Erase Point.");
+	create_nav = memnew( ConfirmationDialog );
+	add_child(create_nav);
+	create_nav->get_ok()->set_text("Create");
+
+
+	//add_constant_override("separation",0);
+
+#if 0
+	options = memnew( MenuButton );
+	add_child(options);
+	options->set_area_as_parent_rect();
+	options->set_text("Polygon");
+	//options->get_popup()->add_item("Parse BBCODE",PARSE_BBCODE);
+	options->get_popup()->connect("item_pressed", this,"_menu_option");
+#endif
+
+	mode = MODE_EDIT;
+	wip_active=false;
+	edited_outline=-1;
+
+}
+
+
+void NavigationPolygonEditorPlugin::edit(Object *p_object) {
+
+	collision_polygon_editor->edit(p_object->cast_to<Node>());
+}
+
+bool NavigationPolygonEditorPlugin::handles(Object *p_object) const {
+
+	return p_object->is_type("NavigationPolygonInstance");
+}
+
+void NavigationPolygonEditorPlugin::make_visible(bool p_visible) {
+
+	if (p_visible) {
+		collision_polygon_editor->show();
+	} else {
+
+		collision_polygon_editor->hide();
+		collision_polygon_editor->edit(NULL);
+	}
+
+}
+
+NavigationPolygonEditorPlugin::NavigationPolygonEditorPlugin(EditorNode *p_node) {
+
+	editor=p_node;
+	collision_polygon_editor = memnew( NavigationPolygonEditor(p_node) );
+	CanvasItemEditor::get_singleton()->add_control_to_menu_panel(collision_polygon_editor);
+
+	collision_polygon_editor->hide();
+
+
+
+}
+
+
+NavigationPolygonEditorPlugin::~NavigationPolygonEditorPlugin()
+{
+}
+

tools/editor/plugins/navigation_polygon_editor_plugin.h

@@ -0,0 +1,91 @@
+#ifndef NAVIGATIONPOLYGONEDITORPLUGIN_H
+#define NAVIGATIONPOLYGONEDITORPLUGIN_H
+
+
+
+#include "tools/editor/editor_plugin.h"
+#include "tools/editor/editor_node.h"
+#include "scene/2d/navigation_polygon.h"
+#include "scene/gui/tool_button.h"
+#include "scene/gui/button_group.h"
+
+/**
+	@author Juan Linietsky <[email protected]>
+*/
+class CanvasItemEditor;
+
+class NavigationPolygonEditor : public HBoxContainer {
+
+	OBJ_TYPE(NavigationPolygonEditor, HBoxContainer );
+
+	UndoRedo *undo_redo;
+	enum Mode {
+
+		MODE_CREATE,
+		MODE_EDIT,
+
+	};
+
+	Mode mode;
+
+	ToolButton *button_create;
+	ToolButton *button_edit;
+
+	ConfirmationDialog *create_nav;
+
+	CanvasItemEditor *canvas_item_editor;
+	EditorNode *editor;
+	Panel *panel;
+	NavigationPolygonInstance *node;
+	MenuButton *options;
+
+	int edited_outline;
+	int edited_point;
+	Vector2 edited_point_pos;
+	DVector<Vector2> pre_move_edit;
+	Vector<Vector2> wip;
+	bool wip_active;
+
+
+	void _wip_close();
+	void _canvas_draw();
+	void _create_nav();
+
+	void _menu_option(int p_option);
+
+protected:
+	void _notification(int p_what);
+	void _node_removed(Node *p_node);
+	static void _bind_methods();
+public:
+
+	Vector2 snap_point(const Vector2& p_point) const;
+	bool forward_input_event(const InputEvent& p_event);
+	void edit(Node *p_collision_polygon);
+	NavigationPolygonEditor(EditorNode *p_editor);
+};
+
+class NavigationPolygonEditorPlugin : public EditorPlugin {
+
+	OBJ_TYPE( NavigationPolygonEditorPlugin, EditorPlugin );
+
+	NavigationPolygonEditor *collision_polygon_editor;
+	EditorNode *editor;
+
+public:
+
+	virtual bool forward_input_event(const InputEvent& p_event) { return collision_polygon_editor->forward_input_event(p_event); }
+
+	virtual String get_name() const { return "NavigationPolygonInstance"; }
+	bool has_main_screen() const { return false; }
+	virtual void edit(Object *p_node);
+	virtual bool handles(Object *p_node) const;
+	virtual void make_visible(bool p_visible);
+
+	NavigationPolygonEditorPlugin(EditorNode *p_node);
+	~NavigationPolygonEditorPlugin();
+
+};
+
+
+#endif // NAVIGATIONPOLYGONEDITORPLUGIN_H