From ef6e1252aeb7e15a72d4df1e3a2335b539d57b95 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Pedro=20J=2E=20Est=C3=A9banez?= <pedrojrulez@gmail.com>
Date: Thu, 17 Aug 2017 01:27:30 +0200
Subject: [PATCH] Guarantee start & end points are returned by Navigation2D

So start and end points are unconditionally added to the returned point list, but first and last middle points are checked against them to avoid duplicates.

`CMP_EPSILON` was already providing a guarantee, but knowing you will get exactly the endpoints you provided is even better.
---
 scene/2d/navigation2d.cpp | 26 ++++++++++++++------------
 1 file changed, 14 insertions(+), 12 deletions(-)

diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp
index 383236b4ca5..455a57822a4 100644
--- a/scene/2d/navigation2d.cpp
+++ b/scene/2d/navigation2d.cpp
@@ -534,7 +534,6 @@ debug path
 			Polygon *left_poly = end_poly;
 			Polygon *right_poly = end_poly;
 			Polygon *p = end_poly;
-			path.push_back(end_point);
 
 			while (p) {
 
@@ -592,7 +591,7 @@ debug path
 						left_poly = p;
 						portal_left = apex_point;
 						portal_right = apex_point;
-						if (path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
+						if (!path.size() || path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
 							path.push_back(apex_point);
 						skip = true;
 						//print_line("addpoint left");
@@ -613,7 +612,7 @@ debug path
 						right_poly = p;
 						portal_right = apex_point;
 						portal_left = apex_point;
-						if (path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
+						if (!path.size() || path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
 							path.push_back(apex_point);
 						//print_line("addpoint right");
 						//print_line("***CLIP RIGHT");
@@ -626,16 +625,10 @@ debug path
 					p = NULL;
 			}
 
-			if (path[path.size() - 1].distance_to(begin_point) > CMP_EPSILON)
-				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();
@@ -645,11 +638,20 @@ debug path
 				if (p == begin_poly)
 					break;
 			}
+		}
 
-			if (path[path.size() - 1].distance_to(begin_point) > CMP_EPSILON)
-				path.push_back(begin_point);
+		if (!path.size() || path[path.size() - 1].distance_squared_to(begin_point) > CMP_EPSILON) {
+			path.push_back(begin_point); // Add the begin point
+		} else {
+			path[path.size() - 1] = begin_point; // Replace first midpoint by the exact begin point
+		}
 
-			path.invert();
+		path.invert();
+
+		if (path.size() <= 1 || path[path.size() - 1].distance_squared_to(end_point) > CMP_EPSILON) {
+			path.push_back(end_point); // Add the end point
+		} else {
+			path[path.size() - 1] = end_point; // Replace last midpoint by the exact end point
 		}
 
 		return path;