db81928e08
- `vk_enum_string_helper.h` is a generated file taken from the SDK (Vulkan-ValidationLayers). - `vk_mem_alloc.h` is a library from GPUOpen: https://github.com/GPUOpen-LibrariesAndSDKs/VulkanMemoryAllocator
665 lines
20 KiB
C++
665 lines
20 KiB
C++
/*************************************************************************/
|
|
/* nav_map.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* https://godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
|
|
/* Copyright (c) 2014-2020 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
|
|
|
|
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<Vector3> 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<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 = 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<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 (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>();
|
|
}
|
|
|
|
void NavMap::add_region(NavRegion *p_region) {
|
|
regions.push_back(p_region);
|
|
regenerate_links = true;
|
|
}
|
|
|
|
void NavMap::remove_region(NavRegion *p_region) {
|
|
regions.push_back(p_region);
|
|
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 = 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.
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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 == 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_TOLLERANCE 0.1
|
|
#define DIR_TOLLERANCE 0.9
|
|
// In front of tollerance
|
|
#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 alligned?
|
|
&& 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);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|