2020-01-10 19:22:34 +08:00
/*************************************************************************/
2020-02-11 21:01:43 +08:00
/* nav_map.cpp */
2020-01-10 19:22:34 +08:00
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
2022-01-04 04:27:34 +08:00
/* Copyright (c) 2007-2022 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2022 Godot Engine contributors (cf. AUTHORS.md). */
2020-01-10 19:22:34 +08:00
/* */
/* 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"
2020-05-12 23:01:17 +08:00
2020-01-10 19:22:34 +08:00
# include <algorithm>
2021-03-15 19:45:28 +08:00
# define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
2020-01-10 19:22:34 +08:00
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 ;
}
2021-03-09 03:56:33 +08:00
Vector < Vector3 > NavMap : : get_path ( Vector3 p_origin , Vector3 p_destination , bool p_optimize , uint32_t p_layers ) const {
2021-03-15 19:45:28 +08:00
// Find the start poly and the end poly on this map.
2020-04-02 07:20:12 +08:00
const gd : : Polygon * begin_poly = nullptr ;
const gd : : Polygon * end_poly = nullptr ;
2020-01-10 19:22:34 +08:00
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 ] ;
2021-03-09 03:56:33 +08:00
// Only consider the polygon if it in a region with compatible layers.
if ( ( p_layers & p . owner - > get_layers ( ) ) = = 0 ) {
continue ;
}
2022-02-13 23:07:01 +08:00
// 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 ) ;
2021-03-15 19:45:28 +08:00
Vector3 point = face . get_closest_point_to ( p_origin ) ;
float distance_to_point = point . distance_to ( p_origin ) ;
if ( distance_to_point < begin_d ) {
begin_d = distance_to_point ;
2020-01-10 19:22:34 +08:00
begin_poly = & p ;
2021-03-15 19:45:28 +08:00
begin_point = point ;
2020-01-10 19:22:34 +08:00
}
2021-03-15 19:45:28 +08:00
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 ;
2020-01-10 19:22:34 +08:00
end_poly = & p ;
2021-03-15 19:45:28 +08:00
end_point = point ;
2020-01-10 19:22:34 +08:00
}
}
}
2021-05-20 18:07:26 +08:00
// Check for trivial cases
2020-01-10 19:22:34 +08:00
if ( ! begin_poly | | ! end_poly ) {
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 ;
}
2021-03-15 19:45:28 +08:00
// List of all reachable navigation polys.
2020-01-10 19:22:34 +08:00
std : : vector < gd : : NavigationPoly > navigation_polys ;
navigation_polys . reserve ( polygons . size ( ) * 0.75 ) ;
2021-03-15 19:45:28 +08:00
// 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 ) ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
// List of polygon IDs to visit.
List < uint32_t > to_visit ;
to_visit . push_back ( 0 ) ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
// This is an implementation of the A* algorithm.
int least_cost_id = 0 ;
bool found_route = false ;
2020-01-10 19:22:34 +08:00
2020-04-02 07:20:12 +08:00
const gd : : Polygon * reachable_end = nullptr ;
2020-01-10 19:22:34 +08:00
float reachable_d = 1e30 ;
bool is_reachable = true ;
2021-03-15 19:45:28 +08:00
while ( true ) {
gd : : NavigationPoly * least_cost_poly = & navigation_polys [ least_cost_id ] ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
for ( size_t i = 0 ; i < least_cost_poly - > poly - > edges . size ( ) ; i + + ) {
const gd : : Edge & edge = least_cost_poly - > poly - > edges [ i ] ;
// 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 ] ;
2021-03-09 03:56:33 +08:00
2021-03-15 19:45:28 +08:00
// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
if ( ( p_layers & connection . polygon - > owner - > get_layers ( ) ) = = 0 ) {
2020-01-10 19:22:34 +08:00
continue ;
2020-05-14 22:41:43 +08:00
}
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
Vector3 pathway [ 2 ] = { connection . pathway_start , connection . pathway_end } ;
const Vector3 new_entry = Geometry3D : : get_closest_point_to_segment ( least_cost_poly - > entry , pathway ) ;
2020-01-10 19:22:34 +08:00
const float new_distance = least_cost_poly - > entry . distance_to ( new_entry ) + least_cost_poly - > traveled_distance ;
2021-04-25 22:40:58 +08:00
const std : : vector < gd : : NavigationPoly > : : iterator it = std : : find (
2020-01-10 19:22:34 +08:00
navigation_polys . begin ( ) ,
navigation_polys . end ( ) ,
2021-03-15 19:45:28 +08:00
gd : : NavigationPoly ( connection . polygon ) ) ;
2020-01-10 19:22:34 +08:00
if ( it ! = navigation_polys . end ( ) ) {
2021-03-15 19:45:28 +08:00
// Polygon already visited, check if we can reduce the travel cost.
if ( new_distance < it - > traveled_distance ) {
it - > back_navigation_poly_id = least_cost_id ;
it - > back_navigation_edge = connection . edge ;
it - > back_navigation_edge_pathway_start = connection . pathway_start ;
it - > back_navigation_edge_pathway_end = connection . pathway_end ;
2020-01-10 19:22:34 +08:00
it - > traveled_distance = new_distance ;
it - > entry = new_entry ;
}
} else {
2021-03-15 19:45:28 +08:00
// Add the neighbour 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 neighbour polygon to the polygons to visit.
to_visit . push_back ( navigation_polys . size ( ) - 1 ) ;
2020-01-10 19:22:34 +08:00
}
}
}
2021-03-15 19:45:28 +08:00
// Removes the least cost polygon from the list of polygons to visit so we can advance.
to_visit . erase ( least_cost_id ) ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
// 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
2020-01-10 19:22:34 +08:00
ERR_BREAK_MSG ( is_reachable = = false , " It's not expect to not find the most reachable polygons " ) ;
is_reachable = false ;
2020-04-02 07:20:12 +08:00
if ( reachable_end = = nullptr ) {
2020-01-10 19:22:34 +08:00
// 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 + + ) {
2022-02-13 23:07:01 +08:00
Face3 f ( end_poly - > points [ 0 ] . pos , end_poly - > points [ point_id - 1 ] . pos , end_poly - > points [ point_id ] . pos ) ;
2020-01-10 19:22:34 +08:00
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 ) ;
2021-03-15 19:45:28 +08:00
to_visit . clear ( ) ;
to_visit . push_back ( 0 ) ;
2020-01-10 19:22:34 +08:00
2020-04-02 07:20:12 +08:00
reachable_end = nullptr ;
2020-01-10 19:22:34 +08:00
continue ;
}
2021-03-15 19:45:28 +08:00
// Find the polygon with the minimum cost from the list of polygons to visit.
2020-01-10 19:22:34 +08:00
least_cost_id = - 1 ;
float least_cost = 1e30 ;
2021-03-15 19:45:28 +08:00
for ( List < uint32_t > : : Element * element = to_visit . front ( ) ; element ! = nullptr ; element = element - > next ( ) ) {
2020-01-10 19:22:34 +08:00
gd : : NavigationPoly * np = & navigation_polys [ element - > get ( ) ] ;
float cost = np - > traveled_distance ;
cost + = np - > entry . distance_to ( end_point ) ;
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 ) {
found_route = true ;
break ;
}
}
2021-03-15 19:45:28 +08:00
// If we did not find a route, return an empty path.
if ( ! found_route ) {
return Vector < Vector3 > ( ) ;
}
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
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 apex_point = end_point ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
gd : : NavigationPoly * left_poly = apex_poly ;
Vector3 left_portal = apex_point ;
gd : : NavigationPoly * right_poly = apex_poly ;
Vector3 right_portal = apex_point ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
gd : : NavigationPoly * p = apex_poly ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
path . push_back ( end_point ) ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
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 ) ;
}
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
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 ) ;
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 ) ;
skip = true ;
2020-01-10 19:22:34 +08:00
}
2021-03-15 19:45:28 +08:00
}
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
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 ;
2020-05-14 22:41:43 +08:00
} else {
2021-03-15 19:45:28 +08:00
clip_path ( navigation_polys , path , apex_poly , left_portal , left_poly ) ;
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 ) ;
2020-05-14 22:41:43 +08:00
}
2020-01-10 19:22:34 +08:00
}
2021-03-15 19:45:28 +08:00
// 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 ;
2020-05-14 22:41:43 +08:00
}
2021-03-15 19:45:28 +08:00
}
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
// 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 ) ;
}
2020-01-10 19:22:34 +08:00
2021-03-14 15:21:32 +08:00
path . reverse ( ) ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
} else {
path . push_back ( end_point ) ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
// Add mid points
int np_id = least_cost_id ;
2022-04-06 05:24:03 +08:00
while ( np_id ! = - 1 & & navigation_polys [ np_id ] . back_navigation_poly_id ! = - 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 ) ;
2021-03-15 19:45:28 +08:00
np_id = navigation_polys [ np_id ] . back_navigation_poly_id ;
2020-01-10 19:22:34 +08:00
}
2022-04-06 05:24:03 +08:00
path . push_back ( begin_point ) ;
2021-03-14 15:21:32 +08:00
path . reverse ( ) ;
2020-01-10 19:22:34 +08:00
}
2021-03-15 19:45:28 +08:00
return path ;
2020-01-10 19:22:34 +08:00
}
2020-02-19 00:08:34 +08:00
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 ;
for ( size_t i ( 0 ) ; i < polygons . size ( ) ; i + + ) {
const gd : : Polygon & p = polygons [ i ] ;
2022-02-13 23:07:01 +08:00
// For each face check the distance to the segment
2020-02-19 00:08:34 +08:00
for ( size_t point_id = 2 ; point_id < p . points . size ( ) ; point_id + = 1 ) {
2022-02-13 23:07:01 +08:00
const Face3 f ( p . points [ 0 ] . pos , p . points [ point_id - 1 ] . pos , p . points [ point_id ] . pos ) ;
2020-02-19 00:08:34 +08:00
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 ;
2020-05-26 01:20:45 +08:00
Geometry3D : : get_closest_points_between_segments (
2020-02-19 00:08:34 +08:00
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 {
2022-02-13 23:07:01 +08:00
gd : : ClosestPointQueryResult cp = get_closest_point_info ( p_point ) ;
return cp . point ;
2020-02-19 00:08:34 +08:00
}
Vector3 NavMap : : get_closest_point_normal ( const Vector3 & p_point ) const {
2022-02-13 23:07:01 +08:00
gd : : ClosestPointQueryResult cp = get_closest_point_info ( p_point ) ;
return cp . normal ;
2020-02-19 00:08:34 +08:00
}
RID NavMap : : get_closest_point_owner ( const Vector3 & p_point ) const {
2022-02-13 23:07:01 +08:00
gd : : ClosestPointQueryResult cp = get_closest_point_info ( p_point ) ;
return cp . owner ;
}
2020-02-19 00:08:34 +08:00
2022-02-13 23:07:01 +08:00
gd : : ClosestPointQueryResult NavMap : : get_closest_point_info ( const Vector3 & p_point ) const {
gd : : ClosestPointQueryResult result ;
real_t closest_point_ds = 1e20 ;
2020-02-19 00:08:34 +08:00
for ( size_t i ( 0 ) ; i < polygons . size ( ) ; i + + ) {
const gd : : Polygon & p = polygons [ i ] ;
2022-02-13 23:07:01 +08:00
// For each face check the distance to the point
2020-02-19 00:08:34 +08:00
for ( size_t point_id = 2 ; point_id < p . points . size ( ) ; point_id + = 1 ) {
2022-02-13 23:07:01 +08:00
const Face3 f ( p . points [ 0 ] . pos , p . points [ point_id - 1 ] . pos , p . points [ point_id ] . pos ) ;
2020-02-19 00:08:34 +08:00
const Vector3 inters = f . get_closest_point_to ( p_point ) ;
2022-02-13 23:07:01 +08:00
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 ;
2020-02-19 00:08:34 +08:00
}
}
}
2022-02-13 23:07:01 +08:00
return result ;
2020-02-19 00:08:34 +08:00
}
2020-01-10 19:22:34 +08:00
void NavMap : : add_region ( NavRegion * p_region ) {
regions . push_back ( p_region ) ;
regenerate_links = true ;
}
void NavMap : : remove_region ( NavRegion * p_region ) {
2021-04-25 22:40:58 +08:00
const std : : vector < NavRegion * > : : iterator it = std : : find ( regions . begin ( ) , regions . end ( ) , p_region ) ;
2020-02-25 01:09:14 +08:00
if ( it ! = regions . end ( ) ) {
regions . erase ( it ) ;
regenerate_links = true ;
}
2020-01-10 19:22:34 +08:00
}
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 ) ;
2021-04-25 22:40:58 +08:00
const std : : vector < RvoAgent * > : : iterator it = std : : find ( agents . begin ( ) , agents . end ( ) , agent ) ;
2020-01-10 19:22:34 +08:00
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 ) {
2021-04-25 22:40:58 +08:00
const std : : vector < RvoAgent * > : : iterator it = std : : find ( controlled_agents . begin ( ) , controlled_agents . end ( ) , agent ) ;
2020-01-10 19:22:34 +08:00
if ( it ! = controlled_agents . end ( ) ) {
controlled_agents . erase ( it ) ;
}
}
void NavMap : : sync ( ) {
2021-03-15 19:45:28 +08:00
// Check if we need to update the links.
2020-01-10 19:22:34 +08:00
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 ) {
2021-03-15 19:45:28 +08:00
// Remove regions connections.
for ( size_t r ( 0 ) ; r < regions . size ( ) ; r + + ) {
regions [ r ] - > get_connections ( ) . clear ( ) ;
}
// Resize the polygon count.
2020-01-10 19:22:34 +08:00
int count = 0 ;
for ( size_t r ( 0 ) ; r < regions . size ( ) ; r + + ) {
count + = regions [ r ] - > get_polygons ( ) . size ( ) ;
}
polygons . resize ( count ) ;
2021-03-15 19:45:28 +08:00
// Copy all region polygons in the map.
count = 0 ;
2020-01-10 19:22:34 +08:00
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 ( ) ;
}
2021-03-15 19:45:28 +08:00
// Group all edges per key.
Map < gd : : EdgeKey , Vector < gd : : Edge : : Connection > > connections ;
2020-01-10 19:22:34 +08:00
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 ) ;
2021-03-15 19:45:28 +08:00
Map < gd : : EdgeKey , Vector < gd : : Edge : : Connection > > : : Element * connection = connections . find ( ek ) ;
2020-01-10 19:22:34 +08:00
if ( ! connection ) {
2021-03-15 19:45:28 +08:00
connections [ ek ] = Vector < gd : : Edge : : Connection > ( ) ;
}
if ( connections [ ek ] . size ( ) < = 1 ) {
// Add the polygon/edge tuple to this key.
gd : : Edge : : Connection new_connection ;
new_connection . polygon = & poly ;
new_connection . edge = p ;
new_connection . pathway_start = poly . points [ p ] . pos ;
new_connection . pathway_end = poly . points [ next_point ] . pos ;
connections [ ek ] . push_back ( new_connection ) ;
2020-01-10 19:22:34 +08:00
} else {
// The edge is already connected with another edge, skip.
2021-03-09 03:56:33 +08:00
ERR_PRINT ( " Attempted to merge a navigation mesh triangle edge with another already-merged edge. This happens when the current `cell_size` is different from the one used to generate the navigation mesh. This will cause navigation problem. " ) ;
2020-01-10 19:22:34 +08:00
}
}
}
2021-03-15 19:45:28 +08:00
Vector < gd : : Edge : : Connection > free_edges ;
2021-08-10 04:13:42 +08:00
for ( KeyValue < gd : : EdgeKey , Vector < gd : : Edge : : Connection > > & E : connections ) {
if ( E . value . size ( ) = = 2 ) {
2021-03-15 19:45:28 +08:00
// Connect edge that are shared in different polygons.
2021-08-10 04:13:42 +08:00
gd : : Edge : : Connection & c1 = E . value . write [ 0 ] ;
gd : : Edge : : Connection & c2 = E . value . write [ 1 ] ;
2021-03-15 19:45:28 +08:00
c1 . polygon - > edges [ c1 . edge ] . connections . push_back ( c2 ) ;
c2 . polygon - > edges [ c2 . edge ] . connections . push_back ( c1 ) ;
// Note: The pathway_start/end are full for those connection and do not need to be modified.
} else {
2021-08-10 04:13:42 +08:00
CRASH_COND_MSG ( E . value . size ( ) ! = 1 , vformat ( " Number of connection != 1. Found: %d " , E . value . size ( ) ) ) ;
free_edges . push_back ( E . value [ 0 ] ) ;
2020-01-10 19:22:34 +08:00
}
}
// 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.
2021-03-15 19:45:28 +08:00
for ( int i = 0 ; i < free_edges . size ( ) ; i + + ) {
const gd : : Edge : : Connection & free_edge = free_edges [ i ] ;
Vector3 edge_p1 = free_edge . polygon - > points [ free_edge . edge ] . pos ;
Vector3 edge_p2 = free_edge . polygon - > points [ ( free_edge . edge + 1 ) % free_edge . polygon - > points . size ( ) ] . pos ;
for ( int j = 0 ; j < free_edges . size ( ) ; j + + ) {
const gd : : Edge : : Connection & other_edge = free_edges [ j ] ;
if ( i = = j | | free_edge . polygon - > owner = = other_edge . polygon - > owner ) {
continue ;
}
Vector3 other_edge_p1 = other_edge . polygon - > points [ other_edge . edge ] . pos ;
Vector3 other_edge_p2 = other_edge . polygon - > points [ ( other_edge . edge + 1 ) % other_edge . polygon - > points . size ( ) ] . pos ;
// Compute the projection of the opposite edge on the current one
Vector3 edge_vector = edge_p2 - edge_p1 ;
float projected_p1_ratio = edge_vector . dot ( other_edge_p1 - edge_p1 ) / ( edge_vector . length_squared ( ) ) ;
float projected_p2_ratio = edge_vector . dot ( other_edge_p2 - edge_p1 ) / ( edge_vector . length_squared ( ) ) ;
if ( ( projected_p1_ratio < 0.0 & & projected_p2_ratio < 0.0 ) | | ( projected_p1_ratio > 1.0 & & projected_p2_ratio > 1.0 ) ) {
2020-01-10 19:22:34 +08:00
continue ;
}
2021-03-15 19:45:28 +08:00
// Check if the two edges are close to each other enough and compute a pathway between the two regions.
Vector3 self1 = edge_vector * CLAMP ( projected_p1_ratio , 0.0 , 1.0 ) + edge_p1 ;
Vector3 other1 ;
if ( projected_p1_ratio > = 0.0 & & projected_p1_ratio < = 1.0 ) {
other1 = other_edge_p1 ;
} else {
other1 = other_edge_p1 . lerp ( other_edge_p2 , ( 1.0 - projected_p1_ratio ) / ( projected_p2_ratio - projected_p1_ratio ) ) ;
2020-01-10 19:22:34 +08:00
}
2021-09-29 11:51:34 +08:00
if ( other1 . distance_to ( self1 ) > edge_connection_margin ) {
2021-03-15 19:45:28 +08:00
continue ;
}
Vector3 self2 = edge_vector * CLAMP ( projected_p2_ratio , 0.0 , 1.0 ) + edge_p1 ;
Vector3 other2 ;
if ( projected_p2_ratio > = 0.0 & & projected_p2_ratio < = 1.0 ) {
other2 = other_edge_p2 ;
} else {
other2 = other_edge_p1 . lerp ( other_edge_p2 , ( 0.0 - projected_p1_ratio ) / ( projected_p2_ratio - projected_p1_ratio ) ) ;
}
2021-09-29 11:51:34 +08:00
if ( other2 . distance_to ( self2 ) > edge_connection_margin ) {
2021-03-15 19:45:28 +08:00
continue ;
}
// The edges can now be connected.
gd : : Edge : : Connection new_connection = other_edge ;
new_connection . pathway_start = ( self1 + other1 ) / 2.0 ;
new_connection . pathway_end = ( self2 + other2 ) / 2.0 ;
free_edge . polygon - > edges [ free_edge . edge ] . connections . push_back ( new_connection ) ;
// Add the connection to the region_connection map.
free_edge . polygon - > owner - > get_connections ( ) . push_back ( new_connection ) ;
2020-01-10 19:22:34 +08:00
}
}
2021-03-15 19:45:28 +08:00
// Update the update ID.
2020-12-09 02:58:49 +08:00
map_update_id = ( map_update_id + 1 ) % 9999999 ;
2020-01-10 19:22:34 +08:00
}
2021-03-15 19:45:28 +08:00
// Update agents tree.
2020-01-10 19:22:34 +08:00
if ( agents_dirty ) {
std : : vector < RVO : : Agent * > raw_agents ;
raw_agents . reserve ( agents . size ( ) ) ;
2020-05-14 22:41:43 +08:00
for ( size_t i ( 0 ) ; i < agents . size ( ) ; i + + ) {
2020-01-10 19:22:34 +08:00
raw_agents . push_back ( agents [ i ] - > get_agent ( ) ) ;
2020-05-14 22:41:43 +08:00
}
2020-01-10 19:22:34 +08:00
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 ] ;
2021-07-17 01:19:55 +08:00
if ( from . is_equal_approx ( p_to_point ) ) {
2020-01-10 19:22:34 +08:00
return ;
2020-05-14 22:41:43 +08:00
}
2020-01-10 19:22:34 +08:00
Plane cut_plane ;
cut_plane . normal = ( from - p_to_point ) . cross ( up ) ;
2020-05-14 22:41:43 +08:00
if ( cut_plane . normal = = Vector3 ( ) ) {
2020-01-10 19:22:34 +08:00
return ;
2020-05-14 22:41:43 +08:00
}
2020-01-10 19:22:34 +08:00
cut_plane . normal . normalize ( ) ;
2020-05-10 22:47:11 +08:00
cut_plane . d = cut_plane . normal . dot ( from ) ;
2020-01-10 19:22:34 +08:00
while ( from_poly ! = p_to_poly ) {
2021-03-15 19:45:28 +08:00
Vector3 pathway_start = from_poly - > back_navigation_edge_pathway_start ;
Vector3 pathway_end = from_poly - > back_navigation_edge_pathway_end ;
2020-01-10 19:22:34 +08:00
2021-03-15 19:45:28 +08:00
ERR_FAIL_COND ( from_poly - > back_navigation_poly_id = = - 1 ) ;
from_poly = & p_navigation_polys [ from_poly - > back_navigation_poly_id ] ;
2020-01-10 19:22:34 +08:00
2021-07-17 01:19:55 +08:00
if ( ! pathway_start . is_equal_approx ( pathway_end ) ) {
2020-01-10 19:22:34 +08:00
Vector3 inters ;
2021-03-15 19:45:28 +08:00
if ( cut_plane . intersects_segment ( pathway_start , pathway_end , & inters ) ) {
2021-07-17 01:19:55 +08:00
if ( ! inters . is_equal_approx ( p_to_point ) & & ! inters . is_equal_approx ( path [ path . size ( ) - 1 ] ) ) {
2020-01-10 19:22:34 +08:00
path . push_back ( inters ) ;
}
}
}
}
}