Better heuristic for the shortest path algorithm for navigation2D and navigation.

Better heuristic for the shortest path algorithm for navigation2D and navigation.
It now will use the shortest distance to the polygon as cost instead of the distance to the center.

3 files changed, 65 insertions, 4 deletions

diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp
index 9eec8e6cc3..1b789bab9d 100644
--- a/scene/2d/navigation2d.cpp
+++ b/scene/2d/navigation2d.cpp
@@ -388,10 +388,34 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 			Polygon *p = E->get();
 
 			float cost = p->distance;
-			cost += p->center.distance_to(end_point);
 
-			if (cost < least_cost) {
+#ifdef USE_ENTRY_POINT
+			int es = p->edges.size();
 
+			float shortest_distance = 1e30;
+
+			for (int i = 0; i < es; i++) {
+				Polygon::Edge &e = p->edges.write[i];
+
+				if (!e.C)
+					continue;
+
+				Vector2 edge[2] = {
+					_get_vertex(p->edges[i].point),
+					_get_vertex(p->edges[(i + 1) % es].point)
+				};
+
+				Vector2 edge_point = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
+				float dist = p->entry.distance_to(edge_point);
+				if (dist < shortest_distance)
+					shortest_distance = dist;
+			}
+
+			cost += shortest_distance;
+#else
+			cost += p->center.distance_to(end_point);
+#endif
+			if (cost < least_cost) {
 				least_cost_poly = E;
 				least_cost = cost;
 			}
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index 8d84d2408c..54f74c2df3 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -30,6 +30,8 @@
 
 #include "navigation.h"
 
+#define USE_ENTRY_POINT
+
 void Navigation::_navmesh_link(int p_id) {
 
 	ERR_FAIL_COND(!navmesh_map.has(p_id));
@@ -331,7 +333,18 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 		if (begin_poly->edges[i].C) {
 
 			begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
+#ifdef USE_ENTRY_POINT
+			Vector3 edge[2] = {
+				_get_vertex(begin_poly->edges[i].point),
+				_get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
+			};
+
+			Vector3 entry = Geometry::get_closest_point_to_segment(begin_poly->entry, edge);
+			begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
+			begin_poly->edges[i].C->entry = entry;
+#else
 			begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
+#endif
 			open_list.push_back(begin_poly->edges[i].C);
 
 			if (begin_poly->edges[i].C == end_poly) {
@@ -356,10 +369,33 @@ Vector<Vector3> Navigation::get_simple_path(const Vector3 &p_start, const Vector
 			Polygon *p = E->get();
 
 			float cost = p->distance;
-			cost += p->center.distance_to(end_point);
+#ifdef USE_ENTRY_POINT
+			int es = p->edges.size();
 
-			if (cost < least_cost) {
+			float shortest_distance = 1e30;
+
+			for (int i = 0; i < es; i++) {
+				Polygon::Edge &e = p->edges.write[i];
+
+				if (!e.C)
+					continue;
 
+				Vector3 edge[2] = {
+					_get_vertex(p->edges[i].point),
+					_get_vertex(p->edges[(i + 1) % es].point)
+				};
+
+				Vector3 edge_point = Geometry::get_closest_point_to_segment(p->entry, edge);
+				float dist = p->entry.distance_to(edge_point);
+				if (dist < shortest_distance)
+					shortest_distance = dist;
+			}
+
+			cost += shortest_distance;
+#else
+			cost += p->center.distance_to(end_point);
+#endif
+			if (cost < least_cost) {
 				least_cost_poly = E;
 				least_cost = cost;
 			}
diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h
index 5a501039c8..8f200997cd 100644
--- a/scene/3d/navigation.h
+++ b/scene/3d/navigation.h
@@ -94,6 +94,7 @@ class Navigation : public Spatial {
 		Vector<Edge> edges;
 
 		Vector3 center;
+		Vector3 entry;
 
 		float distance;
 		int prev_edge;