2024-08-30 21:52:29 +08:00
/**************************************************************************/
/* nav_mesh_queries_3d.cpp */
/**************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/**************************************************************************/
/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
/* */
/* 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. */
/**************************************************************************/
# ifndef _3D_DISABLED
# include "nav_mesh_queries_3d.h"
# include "../nav_base.h"
# include "core/math/geometry_3d.h"
# define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
# define APPEND_METADATA(poly) \
if ( r_path_types ) { \
r_path_types - > push_back ( poly - > owner - > get_type ( ) ) ; \
} \
if ( r_path_rids ) { \
r_path_rids - > push_back ( poly - > owner - > get_self ( ) ) ; \
} \
if ( r_path_owners ) { \
r_path_owners - > push_back ( poly - > owner - > get_owner_id ( ) ) ; \
}
Vector3 NavMeshQueries3D : : polygons_get_random_point ( const LocalVector < gd : : Polygon > & p_polygons , uint32_t p_navigation_layers , bool p_uniformly ) {
const LocalVector < gd : : Polygon > & region_polygons = p_polygons ;
if ( region_polygons . is_empty ( ) ) {
return Vector3 ( ) ;
}
if ( p_uniformly ) {
real_t accumulated_area = 0 ;
RBMap < real_t , uint32_t > region_area_map ;
for ( uint32_t rp_index = 0 ; rp_index < region_polygons . size ( ) ; rp_index + + ) {
const gd : : Polygon & region_polygon = region_polygons [ rp_index ] ;
real_t polyon_area = region_polygon . surface_area ;
if ( polyon_area = = 0.0 ) {
continue ;
}
region_area_map [ accumulated_area ] = rp_index ;
accumulated_area + = polyon_area ;
}
if ( region_area_map . is_empty ( ) | | accumulated_area = = 0 ) {
// All polygons have no real surface / no area.
return Vector3 ( ) ;
}
real_t region_area_map_pos = Math : : random ( real_t ( 0 ) , accumulated_area ) ;
RBMap < real_t , uint32_t > : : Iterator region_E = region_area_map . find_closest ( region_area_map_pos ) ;
ERR_FAIL_COND_V ( ! region_E , Vector3 ( ) ) ;
uint32_t rrp_polygon_index = region_E - > value ;
ERR_FAIL_UNSIGNED_INDEX_V ( rrp_polygon_index , region_polygons . size ( ) , Vector3 ( ) ) ;
const gd : : Polygon & rr_polygon = region_polygons [ rrp_polygon_index ] ;
real_t accumulated_polygon_area = 0 ;
RBMap < real_t , uint32_t > polygon_area_map ;
for ( uint32_t rpp_index = 2 ; rpp_index < rr_polygon . points . size ( ) ; rpp_index + + ) {
real_t face_area = Face3 ( rr_polygon . points [ 0 ] . pos , rr_polygon . points [ rpp_index - 1 ] . pos , rr_polygon . points [ rpp_index ] . pos ) . get_area ( ) ;
if ( face_area = = 0.0 ) {
continue ;
}
polygon_area_map [ accumulated_polygon_area ] = rpp_index ;
accumulated_polygon_area + = face_area ;
}
if ( polygon_area_map . is_empty ( ) | | accumulated_polygon_area = = 0 ) {
// All faces have no real surface / no area.
return Vector3 ( ) ;
}
real_t polygon_area_map_pos = Math : : random ( real_t ( 0 ) , accumulated_polygon_area ) ;
RBMap < real_t , uint32_t > : : Iterator polygon_E = polygon_area_map . find_closest ( polygon_area_map_pos ) ;
ERR_FAIL_COND_V ( ! polygon_E , Vector3 ( ) ) ;
uint32_t rrp_face_index = polygon_E - > value ;
ERR_FAIL_UNSIGNED_INDEX_V ( rrp_face_index , rr_polygon . points . size ( ) , Vector3 ( ) ) ;
const Face3 face ( rr_polygon . points [ 0 ] . pos , rr_polygon . points [ rrp_face_index - 1 ] . pos , rr_polygon . points [ rrp_face_index ] . pos ) ;
Vector3 face_random_position = face . get_random_point_inside ( ) ;
return face_random_position ;
} else {
uint32_t rrp_polygon_index = Math : : random ( int ( 0 ) , region_polygons . size ( ) - 1 ) ;
const gd : : Polygon & rr_polygon = region_polygons [ rrp_polygon_index ] ;
uint32_t rrp_face_index = Math : : random ( int ( 2 ) , rr_polygon . points . size ( ) - 1 ) ;
const Face3 face ( rr_polygon . points [ 0 ] . pos , rr_polygon . points [ rrp_face_index - 1 ] . pos , rr_polygon . points [ rrp_face_index ] . pos ) ;
Vector3 face_random_position = face . get_random_point_inside ( ) ;
return face_random_position ;
}
}
Vector < Vector3 > NavMeshQueries3D : : polygons_get_path ( const LocalVector < gd : : Polygon > & p_polygons , Vector3 p_origin , Vector3 p_destination , bool p_optimize , uint32_t p_navigation_layers , Vector < int32_t > * r_path_types , TypedArray < RID > * r_path_rids , Vector < int64_t > * r_path_owners , const Vector3 & p_map_up , uint32_t p_link_polygons_size ) {
// Clear metadata outputs.
if ( r_path_types ) {
r_path_types - > clear ( ) ;
}
if ( r_path_rids ) {
r_path_rids - > clear ( ) ;
}
if ( r_path_owners ) {
r_path_owners - > clear ( ) ;
}
// Find the start poly and the end poly on this map.
const gd : : Polygon * begin_poly = nullptr ;
const gd : : Polygon * end_poly = nullptr ;
Vector3 begin_point ;
Vector3 end_point ;
real_t begin_d = FLT_MAX ;
real_t end_d = FLT_MAX ;
// Find the initial poly and the end poly on this map.
for ( const gd : : Polygon & p : p_polygons ) {
// Only consider the polygon if it in a region with compatible layers.
if ( ( p_navigation_layers & p . owner - > get_navigation_layers ( ) ) = = 0 ) {
continue ;
}
// For each face check the distance between the origin/destination
for ( size_t point_id = 2 ; point_id < p . points . size ( ) ; point_id + + ) {
const Face3 face ( p . points [ 0 ] . pos , p . points [ point_id - 1 ] . pos , p . points [ point_id ] . pos ) ;
Vector3 point = face . get_closest_point_to ( p_origin ) ;
real_t distance_to_point = point . distance_to ( p_origin ) ;
if ( distance_to_point < begin_d ) {
begin_d = distance_to_point ;
begin_poly = & p ;
begin_point = point ;
}
point = face . get_closest_point_to ( p_destination ) ;
distance_to_point = point . distance_to ( p_destination ) ;
if ( distance_to_point < end_d ) {
end_d = distance_to_point ;
end_poly = & p ;
end_point = point ;
}
}
}
// Check for trivial cases
if ( ! begin_poly | | ! end_poly ) {
return Vector < Vector3 > ( ) ;
}
if ( begin_poly = = end_poly ) {
if ( r_path_types ) {
r_path_types - > resize ( 2 ) ;
r_path_types - > write [ 0 ] = begin_poly - > owner - > get_type ( ) ;
r_path_types - > write [ 1 ] = end_poly - > owner - > get_type ( ) ;
}
if ( r_path_rids ) {
r_path_rids - > resize ( 2 ) ;
( * r_path_rids ) [ 0 ] = begin_poly - > owner - > get_self ( ) ;
( * r_path_rids ) [ 1 ] = end_poly - > owner - > get_self ( ) ;
}
if ( r_path_owners ) {
r_path_owners - > resize ( 2 ) ;
r_path_owners - > write [ 0 ] = begin_poly - > owner - > get_owner_id ( ) ;
r_path_owners - > write [ 1 ] = end_poly - > owner - > get_owner_id ( ) ;
}
Vector < Vector3 > path ;
path . resize ( 2 ) ;
path . write [ 0 ] = begin_point ;
path . write [ 1 ] = end_point ;
return path ;
}
// List of all reachable navigation polys.
LocalVector < gd : : NavigationPoly > navigation_polys ;
navigation_polys . resize ( p_polygons . size ( ) + p_link_polygons_size ) ;
// Initialize the matching navigation polygon.
gd : : NavigationPoly & begin_navigation_poly = navigation_polys [ begin_poly - > id ] ;
begin_navigation_poly . poly = begin_poly ;
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 ;
// Heap of polygons to travel next.
gd : : Heap < gd : : NavigationPoly * , gd : : NavPolyTravelCostGreaterThan , gd : : NavPolyHeapIndexer >
traversable_polys ;
traversable_polys . reserve ( p_polygons . size ( ) * 0.25 ) ;
// This is an implementation of the A* algorithm.
int least_cost_id = begin_poly - > id ;
int prev_least_cost_id = - 1 ;
bool found_route = false ;
const gd : : Polygon * reachable_end = nullptr ;
real_t distance_to_reachable_end = FLT_MAX ;
bool is_reachable = true ;
while ( true ) {
// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
for ( const gd : : Edge & edge : navigation_polys [ least_cost_id ] . poly - > edges ) {
// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
2024-11-06 05:10:53 +08:00
for ( uint32_t connection_index = 0 ; connection_index < edge . connections . size ( ) ; connection_index + + ) {
2024-08-30 21:52:29 +08:00
const gd : : Edge : : Connection & connection = edge . connections [ connection_index ] ;
// Only consider the connection to another polygon if this polygon is in a region with compatible layers.
if ( ( p_navigation_layers & connection . polygon - > owner - > get_navigation_layers ( ) ) = = 0 ) {
continue ;
}
const gd : : NavigationPoly & least_cost_poly = navigation_polys [ least_cost_id ] ;
real_t poly_enter_cost = 0.0 ;
real_t poly_travel_cost = least_cost_poly . poly - > owner - > get_travel_cost ( ) ;
if ( prev_least_cost_id ! = - 1 & & navigation_polys [ prev_least_cost_id ] . poly - > owner - > get_self ( ) ! = least_cost_poly . poly - > owner - > get_self ( ) ) {
poly_enter_cost = least_cost_poly . poly - > owner - > get_enter_cost ( ) ;
}
prev_least_cost_id = least_cost_id ;
Vector3 pathway [ 2 ] = { connection . pathway_start , connection . pathway_end } ;
const Vector3 new_entry = Geometry3D : : get_closest_point_to_segment ( least_cost_poly . entry , pathway ) ;
const real_t new_traveled_distance = least_cost_poly . entry . distance_to ( new_entry ) * poly_travel_cost + poly_enter_cost + least_cost_poly . traveled_distance ;
// Check if the neighbor polygon has already been processed.
gd : : NavigationPoly & neighbor_poly = navigation_polys [ connection . polygon - > id ] ;
if ( neighbor_poly . poly ! = nullptr ) {
// If the neighbor polygon hasn't been traversed yet and the new path leading to
// it is shorter, update the polygon.
if ( neighbor_poly . traversable_poly_index < traversable_polys . size ( ) & &
new_traveled_distance < neighbor_poly . traveled_distance ) {
neighbor_poly . back_navigation_poly_id = least_cost_id ;
neighbor_poly . back_navigation_edge = connection . edge ;
neighbor_poly . back_navigation_edge_pathway_start = connection . pathway_start ;
neighbor_poly . back_navigation_edge_pathway_end = connection . pathway_end ;
neighbor_poly . traveled_distance = new_traveled_distance ;
neighbor_poly . distance_to_destination =
new_entry . distance_to ( end_point ) *
neighbor_poly . poly - > owner - > get_travel_cost ( ) ;
neighbor_poly . entry = new_entry ;
// Update the priority of the polygon in the heap.
traversable_polys . shift ( neighbor_poly . traversable_poly_index ) ;
}
} else {
// Initialize the matching navigation polygon.
neighbor_poly . poly = connection . polygon ;
neighbor_poly . back_navigation_poly_id = least_cost_id ;
neighbor_poly . back_navigation_edge = connection . edge ;
neighbor_poly . back_navigation_edge_pathway_start = connection . pathway_start ;
neighbor_poly . back_navigation_edge_pathway_end = connection . pathway_end ;
neighbor_poly . traveled_distance = new_traveled_distance ;
neighbor_poly . distance_to_destination =
new_entry . distance_to ( end_point ) *
neighbor_poly . poly - > owner - > get_travel_cost ( ) ;
neighbor_poly . entry = new_entry ;
// Add the polygon to the heap of polygons to traverse next.
traversable_polys . push ( & neighbor_poly ) ;
}
}
}
// When the heap of traversable polygons is empty at this point it means the end polygon is
// unreachable.
if ( traversable_polys . is_empty ( ) ) {
// Thus use the further reachable polygon
ERR_BREAK_MSG ( is_reachable = = false , " It's not expect to not find the most reachable polygons " ) ;
is_reachable = false ;
if ( reachable_end = = nullptr ) {
// The path is not found and there is not a way out.
break ;
}
// Set as end point the furthest reachable point.
end_poly = reachable_end ;
end_d = FLT_MAX ;
for ( size_t point_id = 2 ; point_id < end_poly - > points . size ( ) ; point_id + + ) {
Face3 f ( end_poly - > points [ 0 ] . pos , end_poly - > points [ point_id - 1 ] . pos , end_poly - > points [ point_id ] . pos ) ;
Vector3 spoint = f . get_closest_point_to ( p_destination ) ;
real_t dpoint = spoint . distance_to ( p_destination ) ;
if ( dpoint < end_d ) {
end_point = spoint ;
end_d = dpoint ;
}
}
// Search all faces of start polygon as well.
bool closest_point_on_start_poly = false ;
for ( size_t point_id = 2 ; point_id < begin_poly - > points . size ( ) ; point_id + + ) {
Face3 f ( begin_poly - > points [ 0 ] . pos , begin_poly - > points [ point_id - 1 ] . pos , begin_poly - > points [ point_id ] . pos ) ;
Vector3 spoint = f . get_closest_point_to ( p_destination ) ;
real_t dpoint = spoint . distance_to ( p_destination ) ;
if ( dpoint < end_d ) {
end_point = spoint ;
end_d = dpoint ;
closest_point_on_start_poly = true ;
}
}
if ( closest_point_on_start_poly ) {
// No point to run PostProcessing when start and end convex polygon is the same.
if ( r_path_types ) {
r_path_types - > resize ( 2 ) ;
r_path_types - > write [ 0 ] = begin_poly - > owner - > get_type ( ) ;
r_path_types - > write [ 1 ] = begin_poly - > owner - > get_type ( ) ;
}
if ( r_path_rids ) {
r_path_rids - > resize ( 2 ) ;
( * r_path_rids ) [ 0 ] = begin_poly - > owner - > get_self ( ) ;
( * r_path_rids ) [ 1 ] = begin_poly - > owner - > get_self ( ) ;
}
if ( r_path_owners ) {
r_path_owners - > resize ( 2 ) ;
r_path_owners - > write [ 0 ] = begin_poly - > owner - > get_owner_id ( ) ;
r_path_owners - > write [ 1 ] = begin_poly - > owner - > get_owner_id ( ) ;
}
Vector < Vector3 > path ;
path . resize ( 2 ) ;
path . write [ 0 ] = begin_point ;
path . write [ 1 ] = end_point ;
return path ;
}
for ( gd : : NavigationPoly & nav_poly : navigation_polys ) {
nav_poly . poly = nullptr ;
}
navigation_polys [ begin_poly - > id ] . poly = begin_poly ;
least_cost_id = begin_poly - > id ;
prev_least_cost_id = - 1 ;
reachable_end = nullptr ;
continue ;
}
// Pop the polygon with the lowest travel cost from the heap of traversable polygons.
least_cost_id = traversable_polys . pop ( ) - > poly - > id ;
// Store the farthest reachable end polygon in case our goal is not reachable.
if ( is_reachable ) {
real_t distance = navigation_polys [ least_cost_id ] . entry . distance_to ( p_destination ) ;
if ( distance_to_reachable_end > distance ) {
distance_to_reachable_end = distance ;
reachable_end = navigation_polys [ least_cost_id ] . poly ;
}
}
// Check if we reached the end
if ( navigation_polys [ least_cost_id ] . poly = = end_poly ) {
found_route = true ;
break ;
}
}
// We did not find a route but we have both a start polygon and an end polygon at this point.
// Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon.
if ( ! found_route ) {
end_d = FLT_MAX ;
// Search all faces of the start polygon for the closest point to our target position.
for ( size_t point_id = 2 ; point_id < begin_poly - > points . size ( ) ; point_id + + ) {
Face3 f ( begin_poly - > points [ 0 ] . pos , begin_poly - > points [ point_id - 1 ] . pos , begin_poly - > points [ point_id ] . pos ) ;
Vector3 spoint = f . get_closest_point_to ( p_destination ) ;
real_t dpoint = spoint . distance_to ( p_destination ) ;
if ( dpoint < end_d ) {
end_point = spoint ;
end_d = dpoint ;
}
}
if ( r_path_types ) {
r_path_types - > resize ( 2 ) ;
r_path_types - > write [ 0 ] = begin_poly - > owner - > get_type ( ) ;
r_path_types - > write [ 1 ] = begin_poly - > owner - > get_type ( ) ;
}
if ( r_path_rids ) {
r_path_rids - > resize ( 2 ) ;
( * r_path_rids ) [ 0 ] = begin_poly - > owner - > get_self ( ) ;
( * r_path_rids ) [ 1 ] = begin_poly - > owner - > get_self ( ) ;
}
if ( r_path_owners ) {
r_path_owners - > resize ( 2 ) ;
r_path_owners - > write [ 0 ] = begin_poly - > owner - > get_owner_id ( ) ;
r_path_owners - > write [ 1 ] = begin_poly - > owner - > get_owner_id ( ) ;
}
Vector < Vector3 > path ;
path . resize ( 2 ) ;
path . write [ 0 ] = begin_point ;
path . write [ 1 ] = end_point ;
return path ;
}
Vector < Vector3 > path ;
// Optimize the path.
if ( p_optimize ) {
// Set the apex poly/point to the end point
gd : : NavigationPoly * apex_poly = & navigation_polys [ least_cost_id ] ;
Vector3 back_pathway [ 2 ] = { apex_poly - > back_navigation_edge_pathway_start , apex_poly - > back_navigation_edge_pathway_end } ;
const Vector3 back_edge_closest_point = Geometry3D : : get_closest_point_to_segment ( end_point , back_pathway ) ;
if ( end_point . is_equal_approx ( back_edge_closest_point ) ) {
// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
if ( apex_poly - > back_navigation_poly_id ! = - 1 ) {
apex_poly = & navigation_polys [ apex_poly - > back_navigation_poly_id ] ;
}
}
Vector3 apex_point = end_point ;
gd : : NavigationPoly * left_poly = apex_poly ;
Vector3 left_portal = apex_point ;
gd : : NavigationPoly * right_poly = apex_poly ;
Vector3 right_portal = apex_point ;
gd : : NavigationPoly * p = apex_poly ;
path . push_back ( end_point ) ;
APPEND_METADATA ( end_poly ) ;
while ( p ) {
// Set left and right points of the pathway between polygons.
Vector3 left = p - > back_navigation_edge_pathway_start ;
Vector3 right = p - > back_navigation_edge_pathway_end ;
if ( THREE_POINTS_CROSS_PRODUCT ( apex_point , left , right ) . dot ( p_map_up ) < 0 ) {
SWAP ( left , right ) ;
}
bool skip = false ;
if ( THREE_POINTS_CROSS_PRODUCT ( apex_point , left_portal , left ) . dot ( p_map_up ) > = 0 ) {
//process
if ( left_portal = = apex_point | | THREE_POINTS_CROSS_PRODUCT ( apex_point , left , right_portal ) . dot ( p_map_up ) > 0 ) {
left_poly = p ;
left_portal = left ;
} else {
clip_path ( navigation_polys , path , apex_poly , right_portal , right_poly , r_path_types , r_path_rids , r_path_owners , p_map_up ) ;
apex_point = right_portal ;
p = right_poly ;
left_poly = p ;
apex_poly = p ;
left_portal = apex_point ;
right_portal = apex_point ;
path . push_back ( apex_point ) ;
APPEND_METADATA ( apex_poly - > poly ) ;
skip = true ;
}
}
if ( ! skip & & THREE_POINTS_CROSS_PRODUCT ( apex_point , right_portal , right ) . dot ( p_map_up ) < = 0 ) {
//process
if ( right_portal = = apex_point | | THREE_POINTS_CROSS_PRODUCT ( apex_point , right , left_portal ) . dot ( p_map_up ) < 0 ) {
right_poly = p ;
right_portal = right ;
} else {
clip_path ( navigation_polys , path , apex_poly , left_portal , left_poly , r_path_types , r_path_rids , r_path_owners , p_map_up ) ;
apex_point = left_portal ;
p = left_poly ;
right_poly = p ;
apex_poly = p ;
right_portal = apex_point ;
left_portal = apex_point ;
path . push_back ( apex_point ) ;
APPEND_METADATA ( apex_poly - > poly ) ;
}
}
// Go to the previous polygon.
if ( p - > back_navigation_poly_id ! = - 1 ) {
p = & navigation_polys [ p - > back_navigation_poly_id ] ;
} else {
// The end
p = nullptr ;
}
}
// If the last point is not the begin point, add it to the list.
if ( path [ path . size ( ) - 1 ] ! = begin_point ) {
path . push_back ( begin_point ) ;
APPEND_METADATA ( begin_poly ) ;
}
path . reverse ( ) ;
if ( r_path_types ) {
r_path_types - > reverse ( ) ;
}
if ( r_path_rids ) {
r_path_rids - > reverse ( ) ;
}
if ( r_path_owners ) {
r_path_owners - > reverse ( ) ;
}
} else {
path . push_back ( end_point ) ;
APPEND_METADATA ( end_poly ) ;
// Add mid points
int np_id = least_cost_id ;
while ( np_id ! = - 1 & & navigation_polys [ np_id ] . back_navigation_poly_id ! = - 1 ) {
if ( navigation_polys [ np_id ] . back_navigation_edge ! = - 1 ) {
int prev = navigation_polys [ np_id ] . back_navigation_edge ;
int prev_n = ( navigation_polys [ np_id ] . back_navigation_edge + 1 ) % navigation_polys [ np_id ] . poly - > points . size ( ) ;
Vector3 point = ( navigation_polys [ np_id ] . poly - > points [ prev ] . pos + navigation_polys [ np_id ] . poly - > points [ prev_n ] . pos ) * 0.5 ;
path . push_back ( point ) ;
APPEND_METADATA ( navigation_polys [ np_id ] . poly ) ;
} else {
path . push_back ( navigation_polys [ np_id ] . entry ) ;
APPEND_METADATA ( navigation_polys [ np_id ] . poly ) ;
}
np_id = navigation_polys [ np_id ] . back_navigation_poly_id ;
}
path . push_back ( begin_point ) ;
APPEND_METADATA ( begin_poly ) ;
path . reverse ( ) ;
if ( r_path_types ) {
r_path_types - > reverse ( ) ;
}
if ( r_path_rids ) {
r_path_rids - > reverse ( ) ;
}
if ( r_path_owners ) {
r_path_owners - > reverse ( ) ;
}
}
// Ensure post conditions (path arrays MUST match in size).
CRASH_COND ( r_path_types & & path . size ( ) ! = r_path_types - > size ( ) ) ;
CRASH_COND ( r_path_rids & & path . size ( ) ! = r_path_rids - > size ( ) ) ;
CRASH_COND ( r_path_owners & & path . size ( ) ! = r_path_owners - > size ( ) ) ;
return path ;
}
Vector3 NavMeshQueries3D : : polygons_get_closest_point_to_segment ( const LocalVector < gd : : Polygon > & p_polygons , const Vector3 & p_from , const Vector3 & p_to , const bool p_use_collision ) {
bool use_collision = p_use_collision ;
Vector3 closest_point ;
real_t closest_point_distance = FLT_MAX ;
for ( const gd : : Polygon & polygon : p_polygons ) {
// For each face check the distance to the segment.
for ( size_t point_id = 2 ; point_id < polygon . points . size ( ) ; point_id + = 1 ) {
const Face3 face ( polygon . points [ 0 ] . pos , polygon . points [ point_id - 1 ] . pos , polygon . points [ point_id ] . pos ) ;
Vector3 intersection_point ;
if ( face . intersects_segment ( p_from , p_to , & intersection_point ) ) {
const real_t d = p_from . distance_to ( intersection_point ) ;
if ( ! use_collision ) {
closest_point = intersection_point ;
use_collision = true ;
closest_point_distance = d ;
} else if ( closest_point_distance > d ) {
closest_point = intersection_point ;
closest_point_distance = d ;
}
}
// If segment does not itersect face, check the distance from segment's endpoints.
else if ( ! use_collision ) {
const Vector3 p_from_closest = face . get_closest_point_to ( p_from ) ;
const real_t d_p_from = p_from . distance_to ( p_from_closest ) ;
if ( closest_point_distance > d_p_from ) {
closest_point = p_from_closest ;
closest_point_distance = d_p_from ;
}
const Vector3 p_to_closest = face . get_closest_point_to ( p_to ) ;
const real_t d_p_to = p_to . distance_to ( p_to_closest ) ;
if ( closest_point_distance > d_p_to ) {
closest_point = p_to_closest ;
closest_point_distance = d_p_to ;
}
}
}
// Finally, check for a case when shortest distance is between some point located on a face's edge and some point located on a line segment.
if ( ! use_collision ) {
for ( size_t point_id = 0 ; point_id < polygon . points . size ( ) ; point_id + = 1 ) {
Vector3 a , b ;
Geometry3D : : get_closest_points_between_segments (
p_from ,
p_to ,
polygon . points [ point_id ] . pos ,
polygon . points [ ( point_id + 1 ) % polygon . points . size ( ) ] . pos ,
a ,
b ) ;
const real_t d = a . distance_to ( b ) ;
if ( d < closest_point_distance ) {
closest_point_distance = d ;
closest_point = b ;
}
}
}
}
return closest_point ;
}
Vector3 NavMeshQueries3D : : polygons_get_closest_point ( const LocalVector < gd : : Polygon > & p_polygons , const Vector3 & p_point ) {
gd : : ClosestPointQueryResult cp = polygons_get_closest_point_info ( p_polygons , p_point ) ;
return cp . point ;
}
Vector3 NavMeshQueries3D : : polygons_get_closest_point_normal ( const LocalVector < gd : : Polygon > & p_polygons , const Vector3 & p_point ) {
gd : : ClosestPointQueryResult cp = polygons_get_closest_point_info ( p_polygons , p_point ) ;
return cp . normal ;
}
gd : : ClosestPointQueryResult NavMeshQueries3D : : polygons_get_closest_point_info ( const LocalVector < gd : : Polygon > & p_polygons , const Vector3 & p_point ) {
gd : : ClosestPointQueryResult result ;
real_t closest_point_distance_squared = FLT_MAX ;
for ( const gd : : Polygon & polygon : p_polygons ) {
for ( size_t point_id = 2 ; point_id < polygon . points . size ( ) ; point_id + = 1 ) {
const Face3 face ( polygon . points [ 0 ] . pos , polygon . points [ point_id - 1 ] . pos , polygon . points [ point_id ] . pos ) ;
const Vector3 closest_point_on_face = face . get_closest_point_to ( p_point ) ;
const real_t distance_squared_to_point = closest_point_on_face . distance_squared_to ( p_point ) ;
if ( distance_squared_to_point < closest_point_distance_squared ) {
result . point = closest_point_on_face ;
result . normal = face . get_plane ( ) . normal ;
result . owner = polygon . owner - > get_self ( ) ;
closest_point_distance_squared = distance_squared_to_point ;
}
}
}
return result ;
}
RID NavMeshQueries3D : : polygons_get_closest_point_owner ( const LocalVector < gd : : Polygon > & p_polygons , const Vector3 & p_point ) {
gd : : ClosestPointQueryResult cp = polygons_get_closest_point_info ( p_polygons , p_point ) ;
return cp . owner ;
}
void NavMeshQueries3D : : clip_path ( const LocalVector < gd : : NavigationPoly > & p_navigation_polys , Vector < Vector3 > & path , const gd : : NavigationPoly * from_poly , const Vector3 & p_to_point , const gd : : NavigationPoly * p_to_poly , Vector < int32_t > * r_path_types , TypedArray < RID > * r_path_rids , Vector < int64_t > * r_path_owners , const Vector3 & p_map_up ) {
Vector3 from = path [ path . size ( ) - 1 ] ;
if ( from . is_equal_approx ( p_to_point ) ) {
return ;
}
Plane cut_plane ;
cut_plane . normal = ( from - p_to_point ) . cross ( p_map_up ) ;
if ( cut_plane . normal = = Vector3 ( ) ) {
return ;
}
cut_plane . normal . normalize ( ) ;
cut_plane . d = cut_plane . normal . dot ( from ) ;
while ( from_poly ! = p_to_poly ) {
Vector3 pathway_start = from_poly - > back_navigation_edge_pathway_start ;
Vector3 pathway_end = from_poly - > back_navigation_edge_pathway_end ;
ERR_FAIL_COND ( from_poly - > back_navigation_poly_id = = - 1 ) ;
from_poly = & p_navigation_polys [ from_poly - > back_navigation_poly_id ] ;
if ( ! pathway_start . is_equal_approx ( pathway_end ) ) {
Vector3 inters ;
if ( cut_plane . intersects_segment ( pathway_start , pathway_end , & inters ) ) {
if ( ! inters . is_equal_approx ( p_to_point ) & & ! inters . is_equal_approx ( path [ path . size ( ) - 1 ] ) ) {
path . push_back ( inters ) ;
APPEND_METADATA ( from_poly - > poly ) ;
}
}
}
}
}
# endif // _3D_DISABLED