diff options
| -rw-r--r-- | scene/2d/navigation2d.cpp | 26 |
1 files changed, 14 insertions, 12 deletions
diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp index 383236b4ca..455a57822a 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; |