diff options
Diffstat (limited to 'modules/gdnavigation/nav_map.cpp')
| -rw-r--r-- | modules/gdnavigation/nav_map.cpp | 780 |
1 files changed, 0 insertions, 780 deletions
diff --git a/modules/gdnavigation/nav_map.cpp b/modules/gdnavigation/nav_map.cpp deleted file mode 100644 index 2646a4cc0c..0000000000 --- a/modules/gdnavigation/nav_map.cpp +++ /dev/null @@ -1,780 +0,0 @@ -/*************************************************************************/ -/* nav_map.cpp */ -/*************************************************************************/ -/* This file is part of: */ -/* GODOT ENGINE */ -/* https://godotengine.org */ -/*************************************************************************/ -/* Copyright (c) 2007-2021 Juan Linietsky, Ariel Manzur. */ -/* Copyright (c) 2014-2021 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 <algorithm> - -/** - @author AndreaCatania -*/ - -#define USE_ENTRY_POINT - -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<Vector3> NavMap::get_path(Vector3 p_origin, Vector3 p_destination, bool p_optimize) const { - const gd::Polygon *begin_poly = nullptr; - const gd::Polygon *end_poly = nullptr; - 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<Vector3>(); - } - - if (begin_poly == end_poly) { - Vector<Vector3> path; - path.resize(2); - path.write[0] = begin_point; - path.write[1] = end_point; - return path; - } - - std::vector<gd::NavigationPoly> navigation_polys; - navigation_polys.reserve(polygons.size() * 0.75); - - // The elements indices in the `navigation_polys`. - int least_cost_id(-1); - List<uint32_t> 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 = nullptr; - 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 = Geometry3D::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 == 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 = 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 = nullptr; - - 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 != nullptr; 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<Vector3> 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 (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 = nullptr; - } - } - - 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>(); -} - -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; - - 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; -} - -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<NavRegion *>::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<gd::EdgeKey, gd::Connection> 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<gd::EdgeKey, gd::Connection>::Element *connection = connections.find(ek); - if (!connection) { - // Nothing yet - gd::Connection c; - c.A = &poly; - c.A_edge = p; - c.B = nullptr; - c.B_edge = -1; - connections[ek] = c; - - } else if (connection->get().B == nullptr) { - CRASH_COND(connection->get().A == nullptr); // 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 Navigation3D'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<gd::FreeEdge> 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 == nullptr) { - CRASH_COND(connection_element->get().A == nullptr); // 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_TOLLERANCE 0.1 -#define DIR_TOLLERANCE 0.9 - // In front of tolerance -#define IFO_TOLLERANCE 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_TOLLERANCE // Are the same length? - && ABS(edge.edge_dir.dot(other_edge.edge_dir)) > DIR_TOLLERANCE // Are aligned? - && ABS(rel_centers.normalized().dot(edge.edge_dir)) < IFO_TOLLERANCE // 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<RVO::Agent *> 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<int>(controlled_agents.size()); i++) { - controlled_agents[i]->dispatch_callback(); - } -} - -void NavMap::clip_path(const std::vector<gd::NavigationPoly> &p_navigation_polys, Vector<Vector3> &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); - } - } - } - } -} |