From 0b5c694b7497861a8b432d142d5758ce843559bb Mon Sep 17 00:00:00 2001
From: DualMatrix <piet.goris@gmail.com>
Date: Thu, 13 Sep 2018 21:11:33 +0200
Subject: 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.
---
 scene/3d/navigation.cpp | 40 ++++++++++++++++++++++++++++++++++++++--
 scene/3d/navigation.h   |  1 +
 2 files changed, 39 insertions(+), 2 deletions(-)

(limited to 'scene/3d')

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;
-- 
cgit v1.2.3