From 752fbc7f4e5bb1e277321d33f292833c743dc7d6 Mon Sep 17 00:00:00 2001 From: Dardan Bujupaj Date: Wed, 19 Jan 2022 23:39:43 +0100 Subject: [PATCH] Fix indexing of polygon points when building path. --- modules/navigation/nav_map.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 7e9502283b1..59bf1a74a45 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -92,9 +92,13 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p const gd::Polygon &p = polygons[i]; // For each point cast a face and check the distance between the origin/destination - for (size_t point_id = 2; point_id < p.points.size(); point_id++) { - Face3 f(p.points[point_id - 2].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_origin); + for (size_t point_id = 0; point_id < p.points.size(); point_id++) { + const Vector3 p1 = p.points[point_id].pos; + const Vector3 p2 = p.points[(point_id + 1) % p.points.size()].pos; + const Vector3 p3 = p.points[(point_id + 2) % p.points.size()].pos; + const Face3 face(p1, p2, p3); + + Vector3 spoint = face.get_closest_point_to(p_origin); float dpoint = spoint.distance_to(p_origin); if (dpoint < begin_d) { begin_d = dpoint; @@ -102,7 +106,7 @@ Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p begin_point = spoint; } - spoint = f.get_closest_point_to(p_destination); + spoint = face.get_closest_point_to(p_destination); dpoint = spoint.distance_to(p_destination); if (dpoint < end_d) { end_d = dpoint;