1 files changed, 16 insertions, 72 deletions

diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp
index 383236b4ca..74d835dfb2 100644
--- a/scene/2d/navigation2d.cpp
+++ b/scene/2d/navigation2d.cpp
@@ -3,7 +3,7 @@
 /*************************************************************************/
 /*                       This file is part of:                           */
 /*                           GODOT ENGINE                                */
-/*                    http://www.godotengine.org                         */
+/*                      https://godotengine.org                          */
 /*************************************************************************/
 /* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur.                 */
 /* Copyright (c) 2014-2017 Godot Engine contributors (cf. AUTHORS.md)    */
@@ -236,42 +236,6 @@ void Navigation2D::navpoly_remove(int p_id) {
 	_navpoly_unlink(p_id);
 	navpoly_map.erase(p_id);
 }
-#if 0
-void Navigation2D::_clip_path(Vector<Vector2>& path, Polygon *from_poly, const Vector2& p_to_point, Polygon* p_to_poly) {
-
-	Vector2 from = path[path.size()-1];
-
-	if (from.distance_to(p_to_point)<CMP_EPSILON)
-		return;
-	Plane cut_plane;
-	cut_plane.normal = (from-p_to_point).cross(up);
-	if (cut_plane.normal==Vector2())
-		return;
-	cut_plane.normal.normalize();
-	cut_plane.d = cut_plane.normal.dot(from);
-
-
-	while(from_poly!=p_to_poly) {
-
-		int pe = from_poly->prev_edge;
-		Vector2 a = _get_vertex(from_poly->edges[pe].point);
-		Vector2 b = _get_vertex(from_poly->edges[(pe+1)%from_poly->edges.size()].point);
-
-		from_poly=from_poly->edges[pe].C;
-		ERR_FAIL_COND(!from_poly);
-
-		if (a.distance_to(b)>CMP_EPSILON) {
-
-			Vector2 inters;
-			if (cut_plane.intersects_segment(a,b,&inters)) {
-				if (inters.distance_to(p_to_point)>CMP_EPSILON && inters.distance_to(path[path.size()-1])>CMP_EPSILON) {
-					path.push_back(inters);
-				}
-			}
-		}
-	}
-}
-#endif
 
 Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize) {
 
@@ -498,29 +462,7 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vect
 
 		open_list.erase(least_cost_poly);
 	}
-#if 0
-debug path
-	{
-		       Polygon *p=end_poly;
-		       int idx=0;
-
-		       while(true) {
-			   int prev = p->prev_edge;
-			   int prev_n = (p->prev_edge+1)%p->edges.size();
-			   Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point))*0.5;
-			   String points;
-			   for(int i=0;i<p->edges.size();i++) {
-				   if (i>0)
-					   points+=", ";
-				   points+=_get_vertex(p->edges[i].point);
-			   }
-			   //print_line("poly "+itos(idx++)+" - "+points);
-			   p = p->edges[prev].C;
-			   if (p==begin_poly)
-			       break;
-		       }
-		   }
-#endif
+
 	if (found_route) {
 
 		Vector<Vector2> path;
@@ -534,7 +476,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 +533,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 +554,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 +567,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 +580,20 @@ debug path
 				if (p == begin_poly)
 					break;
 			}
+		}
+
+		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
+		}
 
-			if (path[path.size() - 1].distance_to(begin_point) > CMP_EPSILON)
-				path.push_back(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;