/*************************************************************************/ /* nav_map.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* https://godotengine.org */ /*************************************************************************/ /* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */ /* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */ /* */ /* Permission is hereby granted, free of charge, to any person obtaining */ /* a copy of this software and associated documentation files (the */ /* "Software"), to deal in the Software without restriction, including */ /* without limitation the rights to use, copy, modify, merge, publish, */ /* distribute, sublicense, and/or sell copies of the Software, and to */ /* permit persons to whom the Software is furnished to do so, subject to */ /* the following conditions: */ /* */ /* The above copyright notice and this permission notice shall be */ /* included in all copies or substantial portions of the Software. */ /* */ /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /*************************************************************************/ #include "nav_map.h" #include "core/os/threaded_array_processor.h" #include "nav_region.h" #include "rvo_agent.h" #include /** @author AndreaCatania */ #define USE_ENTRY_POINT NavMap::NavMap() : up(0, 1, 0), cell_size(0.3), edge_connection_margin(5.0), regenerate_polygons(true), regenerate_links(true), agents_dirty(false), deltatime(0.0), map_update_id(0) {} void NavMap::set_up(Vector3 p_up) { up = p_up; regenerate_polygons = true; } void NavMap::set_cell_size(float p_cell_size) { cell_size = p_cell_size; regenerate_polygons = true; } void NavMap::set_edge_connection_margin(float p_edge_connection_margin) { edge_connection_margin = p_edge_connection_margin; regenerate_links = true; } gd::PointKey NavMap::get_point_key(const Vector3 &p_pos) const { const int x = int(Math::floor(p_pos.x / cell_size)); const int y = int(Math::floor(p_pos.y / cell_size)); const int z = int(Math::floor(p_pos.z / cell_size)); gd::PointKey p; p.key = 0; p.x = x; p.y = y; p.z = z; return p; } Vector NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const { const gd::Polygon *begin_poly = NULL; const gd::Polygon *end_poly = NULL; Vector3 begin_point; Vector3 end_point; float begin_d = 1e20; float end_d = 1e20; // Find the initial poly and the end poly on this map. for (size_t i(0); i < polygons.size(); i++) { 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); float dpoint = spoint.distance_to(p_origin); if (dpoint < begin_d) { begin_d = dpoint; begin_poly = &p; begin_point = spoint; } spoint = f.get_closest_point_to(p_destination); dpoint = spoint.distance_to(p_destination); if (dpoint < end_d) { end_d = dpoint; end_poly = &p; end_point = spoint; } } } if (!begin_poly || !end_poly) { // No path return Vector(); } if (begin_poly == end_poly) { Vector path; path.resize(2); path.write[0] = begin_point; path.write[1] = end_point; return path; } std::vector navigation_polys; navigation_polys.reserve(polygons.size() * 0.75); // The elements indices in the `navigation_polys`. int least_cost_id(-1); List open_list; bool found_route = false; navigation_polys.push_back(gd::NavigationPoly(begin_poly)); { least_cost_id = 0; gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; least_cost_poly->self_id = least_cost_id; least_cost_poly->entry = begin_point; } open_list.push_back(0); const gd::Polygon *reachable_end = NULL; float reachable_d = 1e30; bool is_reachable = true; while (found_route == false) { { // Takes the current least_cost_poly neighbors and compute the traveled_distance of each for (size_t i = 0; i < navigation_polys[least_cost_id].poly->edges.size(); i++) { gd::NavigationPoly *least_cost_poly = &navigation_polys[least_cost_id]; const gd::Edge &edge = least_cost_poly->poly->edges[i]; if (!edge.other_polygon) continue; #ifdef USE_ENTRY_POINT Vector3 edge_line[2] = { least_cost_poly->poly->points[i].pos, least_cost_poly->poly->points[(i + 1) % least_cost_poly->poly->points.size()].pos }; const Vector3 new_entry = Geometry::get_closest_point_to_segment(least_cost_poly->entry, edge_line); const float new_distance = least_cost_poly->entry.distance_to(new_entry) + least_cost_poly->traveled_distance; #else const float new_distance = least_cost_poly->poly->center.distance_to(edge.other_polygon->center) + least_cost_poly->traveled_distance; #endif auto it = std::find( navigation_polys.begin(), navigation_polys.end(), gd::NavigationPoly(edge.other_polygon)); if (it != navigation_polys.end()) { // Oh this was visited already, can we win the cost? if (it->traveled_distance > new_distance) { it->prev_navigation_poly_id = least_cost_id; it->back_navigation_edge = edge.other_edge; it->traveled_distance = new_distance; #ifdef USE_ENTRY_POINT it->entry = new_entry; #endif } } else { // Add to open neighbours navigation_polys.push_back(gd::NavigationPoly(edge.other_polygon)); gd::NavigationPoly *np = &navigation_polys[navigation_polys.size() - 1]; np->self_id = navigation_polys.size() - 1; np->prev_navigation_poly_id = least_cost_id; np->back_navigation_edge = edge.other_edge; np->traveled_distance = new_distance; #ifdef USE_ENTRY_POINT np->entry = new_entry; #endif open_list.push_back(navigation_polys.size() - 1); } } } // Removes the least cost polygon from the open list so we can advance. open_list.erase(least_cost_id); if (open_list.size() == 0) { // When the open list is empty at this point the End Polygon is not reachable // so 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 == NULL) { // 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 = 1e20; for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) { Face3 f(end_poly->points[point_id - 2].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos); Vector3 spoint = f.get_closest_point_to(p_destination); float dpoint = spoint.distance_to(p_destination); if (dpoint < end_d) { end_point = spoint; end_d = dpoint; } } // Reset open and navigation_polys gd::NavigationPoly np = navigation_polys[0]; navigation_polys.clear(); navigation_polys.push_back(np); open_list.clear(); open_list.push_back(0); reachable_end = NULL; continue; } // Now take the new least_cost_poly from the open list. least_cost_id = -1; float least_cost = 1e30; for (auto element = open_list.front(); element != NULL; element = element->next()) { gd::NavigationPoly *np = &navigation_polys[element->get()]; float cost = np->traveled_distance; #ifdef USE_ENTRY_POINT cost += np->entry.distance_to(end_point); #else cost += np->poly->center.distance_to(end_point); #endif if (cost < least_cost) { least_cost_id = np->self_id; least_cost = cost; } } // Stores the further reachable end polygon, in case our goal is not reachable. if (is_reachable) { float 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; } } ERR_BREAK(least_cost_id == -1); // Check if we reached the end if (navigation_polys[least_cost_id].poly == end_poly) { // Yep, done!! found_route = true; break; } } if (found_route) { Vector path; if (p_optimize) { // String pulling gd::NavigationPoly *apex_poly = &navigation_polys[least_cost_id]; Vector3 apex_point = end_point; Vector3 portal_left = apex_point; Vector3 portal_right = apex_point; gd::NavigationPoly *left_poly = apex_poly; gd::NavigationPoly *right_poly = apex_poly; gd::NavigationPoly *p = apex_poly; path.push_back(end_point); while (p) { Vector3 left; Vector3 right; #define CLOCK_TANGENT(m_a, m_b, m_c) (((m_a) - (m_c)).cross((m_a) - (m_b))) if (p->poly == begin_poly) { left = begin_point; right = begin_point; } else { int prev = p->back_navigation_edge; int prev_n = (p->back_navigation_edge + 1) % p->poly->points.size(); left = p->poly->points[prev].pos; right = p->poly->points[prev_n].pos; //if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5).dot(up) < 0){ if (p->poly->clockwise) { SWAP(left, right); } } bool skip = false; if (CLOCK_TANGENT(apex_point, portal_left, left).dot(up) >= 0) { //process if (portal_left == apex_point || CLOCK_TANGENT(apex_point, left, portal_right).dot(up) > 0) { left_poly = p; portal_left = left; } else { clip_path(navigation_polys, path, apex_poly, portal_right, right_poly); apex_point = portal_right; p = right_poly; left_poly = p; apex_poly = p; portal_left = apex_point; portal_right = apex_point; path.push_back(apex_point); skip = true; } } if (!skip && CLOCK_TANGENT(apex_point, portal_right, right).dot(up) <= 0) { //process if (portal_right == apex_point || CLOCK_TANGENT(apex_point, right, portal_left).dot(up) < 0) { right_poly = p; portal_right = right; } else { clip_path(navigation_polys, path, apex_poly, portal_left, left_poly); apex_point = portal_left; p = left_poly; right_poly = p; apex_poly = p; portal_right = apex_point; portal_left = apex_point; path.push_back(apex_point); } } if (p->prev_navigation_poly_id != -1) p = &navigation_polys[p->prev_navigation_poly_id]; else // The end p = NULL; } if (path[path.size() - 1] != begin_point) path.push_back(begin_point); path.invert(); } else { path.push_back(end_point); // Add mid points int np_id = least_cost_id; while (np_id != -1) { #ifdef USE_ENTRY_POINT Vector3 point = navigation_polys[np_id].entry; #else 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; #endif path.push_back(point); np_id = navigation_polys[np_id].prev_navigation_poly_id; } path.invert(); } return path; } return Vector(); } Vector3 NavMap::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const { bool use_collision = p_use_collision; Vector3 closest_point; real_t closest_point_d = 1e20; // Find the initial poly and the end poly on this map. for (size_t i(0); i < polygons.size(); i++) { const gd::Polygon &p = polygons[i]; // For each point cast a face and 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[point_id - 2].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 = closest_point_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 (use_collision == false) { for (size_t point_id = 0; point_id < p.points.size(); point_id += 1) { Vector3 a, b; Geometry::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; } Vector3 NavMap::get_closest_point(const Vector3 &p_point) const { // TODO this is really not optimal, please redesign the API to directly return all this data Vector3 closest_point; real_t closest_point_d = 1e20; // Find the initial poly and the end poly on this map. for (size_t i(0); i < polygons.size(); i++) { const gd::Polygon &p = polygons[i]; // For each point cast a face and 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[point_id - 2].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 d = inters.distance_to(p_point); if (d < closest_point_d) { closest_point = inters; closest_point_d = d; } } } return closest_point; } Vector3 NavMap::get_closest_point_normal(const Vector3 &p_point) const { // TODO this is really not optimal, please redesign the API to directly return all this data Vector3 closest_point; Vector3 closest_point_normal; real_t closest_point_d = 1e20; // Find the initial poly and the end poly on this map. for (size_t i(0); i < polygons.size(); i++) { const gd::Polygon &p = polygons[i]; // For each point cast a face and 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[point_id - 2].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 d = inters.distance_to(p_point); if (d < closest_point_d) { closest_point = inters; closest_point_normal = f.get_plane().normal; closest_point_d = d; } } } return closest_point_normal; } RID NavMap::get_closest_point_owner(const Vector3 &p_point) const { // TODO this is really not optimal, please redesign the API to directly return all this data Vector3 closest_point; RID closest_point_owner; real_t closest_point_d = 1e20; // Find the initial poly and the end poly on this map. for (size_t i(0); i < polygons.size(); i++) { const gd::Polygon &p = polygons[i]; // For each point cast a face and 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[point_id - 2].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 d = inters.distance_to(p_point); if (d < closest_point_d) { closest_point = inters; closest_point_owner = p.owner->get_self(); closest_point_d = d; } } } return closest_point_owner; } void NavMap::add_region(NavRegion *p_region) { regions.push_back(p_region); regenerate_links = true; } void NavMap::remove_region(NavRegion *p_region) { std::vector::iterator it = std::find(regions.begin(), regions.end(), p_region); if (it != regions.end()) { regions.erase(it); regenerate_links = true; } } bool NavMap::has_agent(RvoAgent *agent) const { return std::find(agents.begin(), agents.end(), agent) != agents.end(); } void NavMap::add_agent(RvoAgent *agent) { if (!has_agent(agent)) { agents.push_back(agent); agents_dirty = true; } } void NavMap::remove_agent(RvoAgent *agent) { remove_agent_as_controlled(agent); auto it = std::find(agents.begin(), agents.end(), agent); if (it != agents.end()) { agents.erase(it); agents_dirty = true; } } void NavMap::set_agent_as_controlled(RvoAgent *agent) { const bool exist = std::find(controlled_agents.begin(), controlled_agents.end(), agent) != controlled_agents.end(); if (!exist) { ERR_FAIL_COND(!has_agent(agent)); controlled_agents.push_back(agent); } } void NavMap::remove_agent_as_controlled(RvoAgent *agent) { auto it = std::find(controlled_agents.begin(), controlled_agents.end(), agent); if (it != controlled_agents.end()) { controlled_agents.erase(it); } } void NavMap::sync() { if (regenerate_polygons) { for (size_t r(0); r < regions.size(); r++) { regions[r]->scratch_polygons(); } regenerate_links = true; } for (size_t r(0); r < regions.size(); r++) { if (regions[r]->sync()) { regenerate_links = true; } } if (regenerate_links) { // Copy all region polygons in the map. int count = 0; for (size_t r(0); r < regions.size(); r++) { count += regions[r]->get_polygons().size(); } polygons.resize(count); count = 0; for (size_t r(0); r < regions.size(); r++) { std::copy( regions[r]->get_polygons().data(), regions[r]->get_polygons().data() + regions[r]->get_polygons().size(), polygons.begin() + count); count += regions[r]->get_polygons().size(); } // Connects the `Edges` of all the `Polygons` of all `Regions` each other. Map connections; for (size_t poly_id(0); poly_id < polygons.size(); poly_id++) { gd::Polygon &poly(polygons[poly_id]); for (size_t p(0); p < poly.points.size(); p++) { int next_point = (p + 1) % poly.points.size(); gd::EdgeKey ek(poly.points[p].key, poly.points[next_point].key); Map::Element *connection = connections.find(ek); if (!connection) { // Nothing yet gd::Connection c; c.A = &poly; c.A_edge = p; c.B = NULL; c.B_edge = -1; connections[ek] = c; } else if (connection->get().B == NULL) { CRASH_COND(connection->get().A == NULL); // Unreachable // Connect the two Polygons by this edge connection->get().B = &poly; connection->get().B_edge = p; connection->get().A->edges[connection->get().A_edge].this_edge = connection->get().A_edge; connection->get().A->edges[connection->get().A_edge].other_polygon = connection->get().B; connection->get().A->edges[connection->get().A_edge].other_edge = connection->get().B_edge; connection->get().B->edges[connection->get().B_edge].this_edge = connection->get().B_edge; connection->get().B->edges[connection->get().B_edge].other_polygon = connection->get().A; connection->get().B->edges[connection->get().B_edge].other_edge = connection->get().A_edge; } else { // The edge is already connected with another edge, skip. ERR_PRINT("Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the Navigation's `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem."); } } } // Takes all the free edges. std::vector free_edges; free_edges.reserve(connections.size()); for (auto connection_element = connections.front(); connection_element; connection_element = connection_element->next()) { if (connection_element->get().B == NULL) { CRASH_COND(connection_element->get().A == NULL); // Unreachable CRASH_COND(connection_element->get().A_edge < 0); // Unreachable // This is a free edge uint32_t id(free_edges.size()); free_edges.push_back(gd::FreeEdge()); free_edges[id].is_free = true; free_edges[id].poly = connection_element->get().A; free_edges[id].edge_id = connection_element->get().A_edge; uint32_t point_0(free_edges[id].edge_id); uint32_t point_1((free_edges[id].edge_id + 1) % free_edges[id].poly->points.size()); Vector3 pos_0 = free_edges[id].poly->points[point_0].pos; Vector3 pos_1 = free_edges[id].poly->points[point_1].pos; Vector3 relative = pos_1 - pos_0; free_edges[id].edge_center = (pos_0 + pos_1) / 2.0; free_edges[id].edge_dir = relative.normalized(); free_edges[id].edge_len_squared = relative.length_squared(); } } const float ecm_squared(edge_connection_margin * edge_connection_margin); #define LEN_TOLERANCE 0.1 #define DIR_TOLERANCE 0.9 // In front of tolerance #define IFO_TOLERANCE 0.5 // Find the compatible near edges. // // Note: // Considering that the edges must be compatible (for obvious reasons) // to be connected, create new polygons to remove that small gap is // not really useful and would result in wasteful computation during // connection, integration and path finding. for (size_t i(0); i < free_edges.size(); i++) { if (!free_edges[i].is_free) { continue; } gd::FreeEdge &edge = free_edges[i]; for (size_t y(0); y < free_edges.size(); y++) { gd::FreeEdge &other_edge = free_edges[y]; if (i == y || !other_edge.is_free || edge.poly->owner == other_edge.poly->owner) { continue; } Vector3 rel_centers = other_edge.edge_center - edge.edge_center; if (ecm_squared > rel_centers.length_squared() // Are enough closer? && ABS(edge.edge_len_squared - other_edge.edge_len_squared) < LEN_TOLERANCE // Are the same length? && ABS(edge.edge_dir.dot(other_edge.edge_dir)) > DIR_TOLERANCE // Are aligned? && ABS(rel_centers.normalized().dot(edge.edge_dir)) < IFO_TOLERANCE // Are one in front the other? ) { // The edges can be connected edge.is_free = false; other_edge.is_free = false; edge.poly->edges[edge.edge_id].this_edge = edge.edge_id; edge.poly->edges[edge.edge_id].other_edge = other_edge.edge_id; edge.poly->edges[edge.edge_id].other_polygon = other_edge.poly; other_edge.poly->edges[other_edge.edge_id].this_edge = other_edge.edge_id; other_edge.poly->edges[other_edge.edge_id].other_edge = edge.edge_id; other_edge.poly->edges[other_edge.edge_id].other_polygon = edge.poly; } } } } if (regenerate_links) { map_update_id = map_update_id + 1 % 9999999; } if (agents_dirty) { std::vector raw_agents; raw_agents.reserve(agents.size()); for (size_t i(0); i < agents.size(); i++) raw_agents.push_back(agents[i]->get_agent()); rvo.buildAgentTree(raw_agents); } regenerate_polygons = false; regenerate_links = false; agents_dirty = false; } void NavMap::compute_single_step(uint32_t index, RvoAgent **agent) { (*(agent + index))->get_agent()->computeNeighbors(&rvo); (*(agent + index))->get_agent()->computeNewVelocity(deltatime); } void NavMap::step(real_t p_deltatime) { deltatime = p_deltatime; if (controlled_agents.size() > 0) { thread_process_array( controlled_agents.size(), this, &NavMap::compute_single_step, controlled_agents.data()); } } void NavMap::dispatch_callbacks() { for (int i(0); i < static_cast(controlled_agents.size()); i++) { controlled_agents[i]->dispatch_callback(); } } void NavMap::clip_path(const std::vector &p_navigation_polys, Vector &path, const gd::NavigationPoly *from_poly, const Vector3 &p_to_point, const gd::NavigationPoly *p_to_poly) const { Vector3 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 == Vector3()) return; cut_plane.normal.normalize(); cut_plane.d = cut_plane.normal.dot(from); while (from_poly != p_to_poly) { int back_nav_edge = from_poly->back_navigation_edge; Vector3 a = from_poly->poly->points[back_nav_edge].pos; Vector3 b = from_poly->poly->points[(back_nav_edge + 1) % from_poly->poly->points.size()].pos; ERR_FAIL_COND(from_poly->prev_navigation_poly_id == -1); from_poly = &p_navigation_polys[from_poly->prev_navigation_poly_id]; if (a.distance_to(b) > CMP_EPSILON) { Vector3 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); } } } } }