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.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 <reduzio@gmail.com>
+*/
+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