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)

1 files changed, 137 insertions, 0 deletions

diff --git a/scene/2d/navigation2d.h b/scene/2d/navigation2d.h
new file mode 100644
index 0000000000..1fca80dc5c
--- /dev/null
+++ b/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