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

@@ -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;