diff options
Diffstat (limited to 'modules/navigation/nav_map.cpp')
| -rw-r--r-- | modules/navigation/nav_map.cpp | 617 |
1 files changed, 22 insertions, 595 deletions
diff --git a/modules/navigation/nav_map.cpp b/modules/navigation/nav_map.cpp index 0c91e8dea3..dd77e81b45 100644 --- a/modules/navigation/nav_map.cpp +++ b/modules/navigation/nav_map.cpp @@ -35,25 +35,13 @@ #include "nav_obstacle.h" #include "nav_region.h" +#include "3d/nav_mesh_queries_3d.h" + #include "core/config/project_settings.h" #include "core/object/worker_thread_pool.h" #include <Obstacle2d.h> -#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) - -// Helper macro -#define APPEND_METADATA(poly) \ - if (r_path_types) { \ - r_path_types->push_back(poly->owner->get_type()); \ - } \ - if (r_path_rids) { \ - r_path_rids->push_back(poly->owner->get_self()); \ - } \ - if (r_path_owners) { \ - r_path_owners->push_back(poly->owner->get_owner_id()); \ - } - #ifdef DEBUG_ENABLED #define NAVMAP_ITERATION_ZERO_ERROR_MSG() \ ERR_PRINT_ONCE("NavigationServer navigation map query failed because it was made before first map synchronization.\n\ @@ -142,462 +130,9 @@ Vector<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p return Vector<Vector3>(); } - // Clear metadata outputs. - if (r_path_types) { - r_path_types->clear(); - } - if (r_path_rids) { - r_path_rids->clear(); - } - if (r_path_owners) { - r_path_owners->clear(); - } - - // Find the start poly and the end poly on this map. - const gd::Polygon *begin_poly = nullptr; - const gd::Polygon *end_poly = nullptr; - Vector3 begin_point; - Vector3 end_point; - real_t begin_d = FLT_MAX; - real_t end_d = FLT_MAX; - // Find the initial poly and the end poly on this map. - for (const gd::Polygon &p : polygons) { - // Only consider the polygon if it in a region with compatible layers. - if ((p_navigation_layers & p.owner->get_navigation_layers()) == 0) { - continue; - } - - // For each face check the distance between the origin/destination - for (size_t point_id = 2; point_id < p.points.size(); point_id++) { - const Face3 face(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - - Vector3 point = face.get_closest_point_to(p_origin); - real_t distance_to_point = point.distance_to(p_origin); - if (distance_to_point < begin_d) { - begin_d = distance_to_point; - begin_poly = &p; - begin_point = point; - } - - point = face.get_closest_point_to(p_destination); - distance_to_point = point.distance_to(p_destination); - if (distance_to_point < end_d) { - end_d = distance_to_point; - end_poly = &p; - end_point = point; - } - } - } - - // Check for trivial cases - if (!begin_poly || !end_poly) { - return Vector<Vector3>(); - } - if (begin_poly == end_poly) { - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = end_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = end_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = end_poly->owner->get_owner_id(); - } - - Vector<Vector3> path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - // List of all reachable navigation polys. - LocalVector<gd::NavigationPoly> navigation_polys; - navigation_polys.reserve(polygons.size() * 0.75); - - // Add the start polygon to the reachable navigation polygons. - gd::NavigationPoly begin_navigation_poly = gd::NavigationPoly(begin_poly); - begin_navigation_poly.self_id = 0; - begin_navigation_poly.entry = begin_point; - begin_navigation_poly.back_navigation_edge_pathway_start = begin_point; - begin_navigation_poly.back_navigation_edge_pathway_end = begin_point; - navigation_polys.push_back(begin_navigation_poly); - - // List of polygon IDs to visit. - List<uint32_t> to_visit; - to_visit.push_back(0); - - // This is an implementation of the A* algorithm. - int least_cost_id = 0; - int prev_least_cost_id = -1; - bool found_route = false; - - const gd::Polygon *reachable_end = nullptr; - real_t reachable_d = FLT_MAX; - bool is_reachable = true; - - while (true) { - // Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance. - for (const gd::Edge &edge : navigation_polys[least_cost_id].poly->edges) { - // Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon. - for (int connection_index = 0; connection_index < edge.connections.size(); connection_index++) { - const gd::Edge::Connection &connection = edge.connections[connection_index]; - - // Only consider the connection to another polygon if this polygon is in a region with compatible layers. - if ((p_navigation_layers & connection.polygon->owner->get_navigation_layers()) == 0) { - continue; - } - - const gd::NavigationPoly &least_cost_poly = navigation_polys[least_cost_id]; - real_t poly_enter_cost = 0.0; - real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost(); - - if (prev_least_cost_id != -1 && (navigation_polys[prev_least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self())) { - poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost(); - } - prev_least_cost_id = least_cost_id; - - Vector3 pathway[2] = { connection.pathway_start, connection.pathway_end }; - const Vector3 new_entry = Geometry3D::get_closest_point_to_segment(least_cost_poly.entry, pathway); - const real_t new_distance = (least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost) + poly_enter_cost + least_cost_poly.traveled_distance; - - int64_t already_visited_polygon_index = navigation_polys.find(gd::NavigationPoly(connection.polygon)); - - if (already_visited_polygon_index != -1) { - // Polygon already visited, check if we can reduce the travel cost. - gd::NavigationPoly &avp = navigation_polys[already_visited_polygon_index]; - if (new_distance < avp.traveled_distance) { - avp.back_navigation_poly_id = least_cost_id; - avp.back_navigation_edge = connection.edge; - avp.back_navigation_edge_pathway_start = connection.pathway_start; - avp.back_navigation_edge_pathway_end = connection.pathway_end; - avp.traveled_distance = new_distance; - avp.entry = new_entry; - } - } else { - // Add the neighbor polygon to the reachable ones. - gd::NavigationPoly new_navigation_poly = gd::NavigationPoly(connection.polygon); - new_navigation_poly.self_id = navigation_polys.size(); - new_navigation_poly.back_navigation_poly_id = least_cost_id; - new_navigation_poly.back_navigation_edge = connection.edge; - new_navigation_poly.back_navigation_edge_pathway_start = connection.pathway_start; - new_navigation_poly.back_navigation_edge_pathway_end = connection.pathway_end; - new_navigation_poly.traveled_distance = new_distance; - new_navigation_poly.entry = new_entry; - navigation_polys.push_back(new_navigation_poly); - - // Add the neighbor polygon to the polygons to visit. - to_visit.push_back(navigation_polys.size() - 1); - } - } - } - - // Removes the least cost polygon from the list of polygons to visit so we can advance. - to_visit.erase(least_cost_id); - - // When the list of polygons to visit is empty at this point it means the End Polygon is not reachable - if (to_visit.size() == 0) { - // Thus use the further reachable polygon - ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons"); - is_reachable = false; - if (reachable_end == nullptr) { - // The path is not found and there is not a way out. - break; - } - - // Set as end point the furthest reachable point. - end_poly = reachable_end; - end_d = FLT_MAX; - for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) { - Face3 f(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - if (dpoint < end_d) { - end_point = spoint; - end_d = dpoint; - } - } - - // Search all faces of start polygon as well. - bool closest_point_on_start_poly = false; - for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { - Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - if (dpoint < end_d) { - end_point = spoint; - end_d = dpoint; - closest_point_on_start_poly = true; - } - } - - if (closest_point_on_start_poly) { - // No point to run PostProcessing when start and end convex polygon is the same. - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = begin_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = begin_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = begin_poly->owner->get_owner_id(); - } - - Vector<Vector3> path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - // Reset open and navigation_polys - gd::NavigationPoly np = navigation_polys[0]; - navigation_polys.clear(); - navigation_polys.push_back(np); - to_visit.clear(); - to_visit.push_back(0); - least_cost_id = 0; - prev_least_cost_id = -1; - - reachable_end = nullptr; - - continue; - } - - // Find the polygon with the minimum cost from the list of polygons to visit. - least_cost_id = -1; - real_t least_cost = FLT_MAX; - for (List<uint32_t>::Element *element = to_visit.front(); element != nullptr; element = element->next()) { - gd::NavigationPoly *np = &navigation_polys[element->get()]; - real_t cost = np->traveled_distance; - cost += (np->entry.distance_to(end_point) * np->poly->owner->get_travel_cost()); - if (cost < least_cost) { - least_cost_id = np->self_id; - least_cost = cost; - } - } - - ERR_BREAK(least_cost_id == -1); - - // Stores the further reachable end polygon, in case our goal is not reachable. - if (is_reachable) { - real_t d = navigation_polys[least_cost_id].entry.distance_to(p_destination); - if (reachable_d > d) { - reachable_d = d; - reachable_end = navigation_polys[least_cost_id].poly; - } - } - - // Check if we reached the end - if (navigation_polys[least_cost_id].poly == end_poly) { - found_route = true; - break; - } - } - - // We did not find a route but we have both a start polygon and an end polygon at this point. - // Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon. - if (!found_route) { - end_d = FLT_MAX; - // Search all faces of the start polygon for the closest point to our target position. - for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) { - Face3 f(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos); - Vector3 spoint = f.get_closest_point_to(p_destination); - real_t dpoint = spoint.distance_to(p_destination); - if (dpoint < end_d) { - end_point = spoint; - end_d = dpoint; - } - } - - if (r_path_types) { - r_path_types->resize(2); - r_path_types->write[0] = begin_poly->owner->get_type(); - r_path_types->write[1] = begin_poly->owner->get_type(); - } - - if (r_path_rids) { - r_path_rids->resize(2); - (*r_path_rids)[0] = begin_poly->owner->get_self(); - (*r_path_rids)[1] = begin_poly->owner->get_self(); - } - - if (r_path_owners) { - r_path_owners->resize(2); - r_path_owners->write[0] = begin_poly->owner->get_owner_id(); - r_path_owners->write[1] = begin_poly->owner->get_owner_id(); - } - - Vector<Vector3> path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - Vector<Vector3> path; - // Optimize the path. - if (p_optimize) { - // Set the apex poly/point to the end point - gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; - - Vector3 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end }; - const Vector3 back_edge_closest_point = Geometry3D::get_closest_point_to_segment(end_point, back_pathway); - if (end_point.is_equal_approx(back_edge_closest_point)) { - // The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing. - // At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon. - if (apex_poly->back_navigation_poly_id != -1) { - apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id]; - } - } - - Vector3 apex_point = end_point; - - gd::NavigationPoly *left_poly = apex_poly; - Vector3 left_portal = apex_point; - gd::NavigationPoly *right_poly = apex_poly; - Vector3 right_portal = apex_point; - - gd::NavigationPoly *p = apex_poly; - - path.push_back(end_point); - APPEND_METADATA(end_poly); - - while (p) { - // Set left and right points of the pathway between polygons. - Vector3 left = p->back_navigation_edge_pathway_start; - Vector3 right = p->back_navigation_edge_pathway_end; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right).dot(up) < 0) { - SWAP(left, right); - } - - bool skip = false; - if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left).dot(up) >= 0) { - //process - if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal).dot(up) > 0) { - left_poly = p; - left_portal = left; - } else { - clip_path(navigation_polys, path, apex_poly, right_portal, right_poly, r_path_types, r_path_rids, r_path_owners); - - apex_point = right_portal; - p = right_poly; - left_poly = p; - apex_poly = p; - left_portal = apex_point; - right_portal = apex_point; - - path.push_back(apex_point); - APPEND_METADATA(apex_poly->poly); - skip = true; - } - } - - if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right).dot(up) <= 0) { - //process - if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal).dot(up) < 0) { - right_poly = p; - right_portal = right; - } else { - clip_path(navigation_polys, path, apex_poly, left_portal, left_poly, r_path_types, r_path_rids, r_path_owners); - - apex_point = left_portal; - p = left_poly; - right_poly = p; - apex_poly = p; - right_portal = apex_point; - left_portal = apex_point; - - path.push_back(apex_point); - APPEND_METADATA(apex_poly->poly); - } - } - - // Go to the previous polygon. - if (p->back_navigation_poly_id != -1) { - p = &navigation_polys[p->back_navigation_poly_id]; - } else { - // The end - p = nullptr; - } - } - - // If the last point is not the begin point, add it to the list. - if (path[path.size() - 1] != begin_point) { - path.push_back(begin_point); - APPEND_METADATA(begin_poly); - } - - path.reverse(); - if (r_path_types) { - r_path_types->reverse(); - } - if (r_path_rids) { - r_path_rids->reverse(); - } - if (r_path_owners) { - r_path_owners->reverse(); - } - - } else { - path.push_back(end_point); - APPEND_METADATA(end_poly); - - // Add mid points - int np_id = least_cost_id; - while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) { - if (navigation_polys[np_id].back_navigation_edge != -1) { - int prev = navigation_polys[np_id].back_navigation_edge; - int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size(); - Vector3 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5; - - path.push_back(point); - APPEND_METADATA(navigation_polys[np_id].poly); - } else { - path.push_back(navigation_polys[np_id].entry); - APPEND_METADATA(navigation_polys[np_id].poly); - } - - np_id = navigation_polys[np_id].back_navigation_poly_id; - } - - path.push_back(begin_point); - APPEND_METADATA(begin_poly); - - path.reverse(); - if (r_path_types) { - r_path_types->reverse(); - } - if (r_path_rids) { - r_path_rids->reverse(); - } - if (r_path_owners) { - r_path_owners->reverse(); - } - } - - // Ensure post conditions (path arrays MUST match in size). - CRASH_COND(r_path_types && path.size() != r_path_types->size()); - CRASH_COND(r_path_rids && path.size() != r_path_rids->size()); - CRASH_COND(r_path_owners && path.size() != r_path_owners->size()); - - return path; + return NavMeshQueries3D::polygons_get_path( + polygons, p_origin, p_destination, p_optimize, p_navigation_layers, + r_path_types, r_path_rids, r_path_owners, up, link_polygons.size()); } Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { @@ -607,66 +142,7 @@ Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector return Vector3(); } - bool use_collision = p_use_collision; - Vector3 closest_point; - real_t closest_point_d = FLT_MAX; - - for (const gd::Polygon &p : polygons) { - // For each face check the distance to the segment - for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { - const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - Vector3 inters; - if (f.intersects_segment(p_from, p_to, &inters)) { - const real_t d = p_from.distance_to(inters); - if (use_collision == false) { - closest_point = inters; - use_collision = true; - closest_point_d = d; - } else if (closest_point_d > d) { - closest_point = inters; - closest_point_d = d; - } - } - // If segment does not itersect face, check the distance from segment's endpoints. - else if (!use_collision) { - const Vector3 p_from_closest = f.get_closest_point_to(p_from); - const real_t d_p_from = p_from.distance_to(p_from_closest); - if (closest_point_d > d_p_from) { - closest_point = p_from_closest; - closest_point_d = d_p_from; - } - - const Vector3 p_to_closest = f.get_closest_point_to(p_to); - const real_t d_p_to = p_to.distance_to(p_to_closest); - if (closest_point_d > d_p_to) { - closest_point = p_to_closest; - closest_point_d = d_p_to; - } - } - } - // Finally, check for a case when shortest distance is between some point located on a face's edge and some point located on a line segment. - if (!use_collision) { - for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) { - Vector3 a, b; - - Geometry3D::get_closest_points_between_segments( - p_from, - p_to, - p.points[point_id].pos, - p.points[(point_id + 1) % p.points.size()].pos, - a, - b); - - const real_t d = a.distance_to(b); - if (d < closest_point_d) { - closest_point_d = d; - closest_point = b; - } - } - } - } - - return closest_point; + return NavMeshQueries3D::polygons_get_closest_point_to_segment(polygons, p_from, p_to, p_use_collision); } Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { @@ -675,8 +151,8 @@ Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.point; + + return NavMeshQueries3D::polygons_get_closest_point(polygons, p_point); } Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { @@ -685,8 +161,8 @@ Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return Vector3(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.normal; + + return NavMeshQueries3D::polygons_get_closest_point_normal(polygons, p_point); } RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { @@ -695,32 +171,14 @@ RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { NAVMAP_ITERATION_ZERO_ERROR_MSG(); return RID(); } - gd::ClosestPointQueryResult cp = get_closest_point_info(p_point); - return cp.owner; + + return NavMeshQueries3D::polygons_get_closest_point_owner(polygons, p_point); } gd::ClosestPointQueryResult NavMap::get_closest_point_info(const Vector3 &p_point) const { RWLockRead read_lock(map_rwlock); - gd::ClosestPointQueryResult result; - real_t closest_point_ds = FLT_MAX; - - for (const gd::Polygon &p : polygons) { - // For each face check the distance to the point - for (size_t point_id = 2; point_id < p.points.size(); point_id += 1) { - const Face3 f(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos); - const Vector3 inters = f.get_closest_point_to(p_point); - const real_t ds = inters.distance_squared_to(p_point); - if (ds < closest_point_ds) { - result.point = inters; - result.normal = f.get_plane().normal; - result.owner = p.owner->get_self(); - closest_point_ds = ds; - } - } - } - - return result; + return NavMeshQueries3D::polygons_get_closest_point_info(polygons, p_point); } void NavMap::add_region(NavRegion *p_region) { @@ -943,29 +401,30 @@ void NavMap::sync() { } // Resize the polygon count. - int count = 0; + int polygon_count = 0; for (const NavRegion *region : regions) { if (!region->get_enabled()) { continue; } - count += region->get_polygons().size(); + polygon_count += region->get_polygons().size(); } - polygons.resize(count); + polygons.resize(polygon_count); // Copy all region polygons in the map. - count = 0; + polygon_count = 0; for (const NavRegion *region : regions) { if (!region->get_enabled()) { continue; } const LocalVector<gd::Polygon> &polygons_source = region->get_polygons(); for (uint32_t n = 0; n < polygons_source.size(); n++) { - polygons[count + n] = polygons_source[n]; + polygons[polygon_count] = polygons_source[n]; + polygons[polygon_count].id = polygon_count; + polygon_count++; } - count += region->get_polygons().size(); } - _new_pm_polygon_count = polygons.size(); + _new_pm_polygon_count = polygon_count; // Group all edges per key. HashMap<gd::EdgeKey, Vector<gd::Edge::Connection>, gd::EdgeKey> connections; @@ -1136,6 +595,7 @@ void NavMap::sync() { // If we have both a start and end point, then create a synthetic polygon to route through. if (closest_start_polygon && closest_end_polygon) { gd::Polygon &new_polygon = link_polygons[link_poly_idx++]; + new_polygon.id = polygon_count++; new_polygon.owner = link; new_polygon.edges.clear(); @@ -1391,39 +851,6 @@ void NavMap::dispatch_callbacks() { } } -void NavMap::clip_path(const LocalVector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly, Vector<int32_t> *r_path_types, TypedArray<RID> *r_path_rids, Vector<int64_t> *r_path_owners) const { - Vector3 from = path[path.size() - 1]; - - if (from.is_equal_approx(p_to_point)) { - return; - } - Plane cut_plane; - cut_plane.normal = (from - p_to_point).cross(up); - if (cut_plane.normal == Vector3()) { - return; - } - cut_plane.normal.normalize(); - cut_plane.d = cut_plane.normal.dot(from); - - while (from_poly != p_to_poly) { - Vector3 pathway_start = from_poly->back_navigation_edge_pathway_start; - Vector3 pathway_end = from_poly->back_navigation_edge_pathway_end; - - ERR_FAIL_COND(from_poly->back_navigation_poly_id == -1); - from_poly = &p_navigation_polys[from_poly->back_navigation_poly_id]; - - if (!pathway_start.is_equal_approx(pathway_end)) { - Vector3 inters; - if (cut_plane.intersects_segment(pathway_start, pathway_end, &inters)) { - if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(path[path.size() - 1])) { - path.push_back(inters); - APPEND_METADATA(from_poly->poly); - } - } - } - } -} - void NavMap::_update_merge_rasterizer_cell_dimensions() { merge_rasterizer_cell_size = cell_size * merge_rasterizer_cell_scale; merge_rasterizer_cell_height = cell_height * merge_rasterizer_cell_scale; |