2023-01-05 13:25:55 +01:00
/**************************************************************************/
/* a_star.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. */
/**************************************************************************/
2018-01-05 00:50:27 +01:00
2016-09-13 23:17:18 +02:00
# include "a_star.h"
2018-09-11 18:13:45 +02:00
2020-05-25 19:20:45 +02:00
# include "core/math/geometry_3d.h"
2020-11-07 23:33:38 +01:00
# include "core/object/script_language.h"
2016-09-13 23:17:18 +02:00
2022-06-16 10:56:12 +02:00
int64_t AStar3D : : get_available_point_id ( ) const {
2019-08-16 00:22:52 +02:00
if ( points . has ( last_free_id ) ) {
2022-06-16 10:56:12 +02:00
int64_t cur_new_id = last_free_id + 1 ;
2019-08-16 00:22:52 +02:00
while ( points . has ( cur_new_id ) ) {
cur_new_id + + ;
}
2022-06-16 10:56:12 +02:00
const_cast < int64_t & > ( last_free_id ) = cur_new_id ;
2019-08-16 00:22:52 +02:00
}
return last_free_id ;
2016-09-13 23:17:18 +02:00
}
2022-06-16 10:56:12 +02:00
void AStar3D : : add_point ( int64_t p_id , const Vector3 & p_pos , real_t p_weight_scale ) {
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( p_id < 0 , vformat ( " Can't add a point with negative id: %d. " , p_id ) ) ;
2022-05-06 02:33:10 +02:00
ERR_FAIL_COND_MSG ( p_weight_scale < 0.0 , vformat ( " Can't add a point with weight scale less than 0.0: %f. " , p_weight_scale ) ) ;
2017-10-27 19:19:01 +02:00
2019-08-16 00:22:52 +02:00
Point * found_pt ;
bool p_exists = points . lookup ( p_id , found_pt ) ;
if ( ! p_exists ) {
2016-09-13 23:17:18 +02:00
Point * pt = memnew ( Point ) ;
pt - > id = p_id ;
pt - > pos = p_pos ;
pt - > weight_scale = p_weight_scale ;
2020-04-02 01:20:12 +02:00
pt - > prev_point = nullptr ;
2019-05-16 20:09:47 +02:00
pt - > open_pass = 0 ;
pt - > closed_pass = 0 ;
2019-03-29 09:10:57 +01:00
pt - > enabled = true ;
2019-08-16 00:22:52 +02:00
points . set ( p_id , pt ) ;
2016-09-13 23:17:18 +02:00
} else {
2019-08-16 00:22:52 +02:00
found_pt - > pos = p_pos ;
found_pt - > weight_scale = p_weight_scale ;
2016-09-13 23:17:18 +02:00
}
}
2022-06-16 10:56:12 +02:00
Vector3 AStar3D : : get_point_position ( int64_t p_id ) const {
2019-08-16 00:22:52 +02:00
Point * p ;
bool p_exists = points . lookup ( p_id , p ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! p_exists , Vector3 ( ) , vformat ( " Can't get point's position. Point with id: %d doesn't exist. " , p_id ) ) ;
2016-09-13 23:17:18 +02:00
2019-08-16 00:22:52 +02:00
return p - > pos ;
2016-09-13 23:17:18 +02:00
}
2017-10-27 19:19:01 +02:00
2022-06-16 10:56:12 +02:00
void AStar3D : : set_point_position ( int64_t p_id , const Vector3 & p_pos ) {
2019-08-16 00:22:52 +02:00
Point * p ;
bool p_exists = points . lookup ( p_id , p ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( ! p_exists , vformat ( " Can't set point's position. Point with id: %d doesn't exist. " , p_id ) ) ;
2017-10-27 19:19:01 +02:00
2019-08-16 00:22:52 +02:00
p - > pos = p_pos ;
2017-10-27 19:19:01 +02:00
}
2022-06-16 10:56:12 +02:00
real_t AStar3D : : get_point_weight_scale ( int64_t p_id ) const {
2019-08-16 00:22:52 +02:00
Point * p ;
bool p_exists = points . lookup ( p_id , p ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! p_exists , 0 , vformat ( " Can't get point's weight scale. Point with id: %d doesn't exist. " , p_id ) ) ;
2016-09-13 23:17:18 +02:00
2019-08-16 00:22:52 +02:00
return p - > weight_scale ;
2016-09-13 23:17:18 +02:00
}
2017-10-27 19:19:01 +02:00
2022-06-16 10:56:12 +02:00
void AStar3D : : set_point_weight_scale ( int64_t p_id , real_t p_weight_scale ) {
2019-08-16 00:22:52 +02:00
Point * p ;
bool p_exists = points . lookup ( p_id , p ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( ! p_exists , vformat ( " Can't set point's weight scale. Point with id: %d doesn't exist. " , p_id ) ) ;
2022-05-06 02:33:10 +02:00
ERR_FAIL_COND_MSG ( p_weight_scale < 0.0 , vformat ( " Can't set point's weight scale less than 0.0: %f. " , p_weight_scale ) ) ;
2017-10-27 19:19:01 +02:00
2019-08-16 00:22:52 +02:00
p - > weight_scale = p_weight_scale ;
2017-10-27 19:19:01 +02:00
}
2022-06-16 10:56:12 +02:00
void AStar3D : : remove_point ( int64_t p_id ) {
2019-08-16 00:22:52 +02:00
Point * p ;
bool p_exists = points . lookup ( p_id , p ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( ! p_exists , vformat ( " Can't remove point. Point with id: %d doesn't exist. " , p_id ) ) ;
2016-09-13 23:17:18 +02:00
2022-06-16 10:56:12 +02:00
for ( OAHashMap < int64_t , Point * > : : Iterator it = p - > neighbours . iter ( ) ; it . valid ; it = p - > neighbours . next_iter ( it ) ) {
2019-08-16 00:22:52 +02:00
Segment s ( p_id , ( * it . key ) ) ;
2019-06-04 21:39:37 +02:00
segments . erase ( s ) ;
2019-08-16 00:22:52 +02:00
( * it . value ) - > neighbours . remove ( p - > id ) ;
( * it . value ) - > unlinked_neighbours . remove ( p - > id ) ;
2019-06-04 21:39:37 +02:00
}
2022-06-16 10:56:12 +02:00
for ( OAHashMap < int64_t , Point * > : : Iterator it = p - > unlinked_neighbours . iter ( ) ; it . valid ; it = p - > unlinked_neighbours . next_iter ( it ) ) {
2019-08-16 00:22:52 +02:00
Segment s ( p_id , ( * it . key ) ) ;
2019-06-04 21:39:37 +02:00
segments . erase ( s ) ;
2019-08-16 00:22:52 +02:00
( * it . value ) - > neighbours . remove ( p - > id ) ;
( * it . value ) - > unlinked_neighbours . remove ( p - > id ) ;
2016-09-13 23:17:18 +02:00
}
memdelete ( p ) ;
2019-08-16 00:22:52 +02:00
points . remove ( p_id ) ;
last_free_id = p_id ;
2016-09-13 23:17:18 +02:00
}
2022-06-16 10:56:12 +02:00
void AStar3D : : connect_points ( int64_t p_id , int64_t p_with_id , bool bidirectional ) {
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( p_id = = p_with_id , vformat ( " Can't connect point with id: %d to itself. " , p_id ) ) ;
2016-09-13 23:17:18 +02:00
2019-08-16 00:22:52 +02:00
Point * a ;
bool from_exists = points . lookup ( p_id , a ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( ! from_exists , vformat ( " Can't connect points. Point with id: %d doesn't exist. " , p_id ) ) ;
2017-05-19 13:16:45 +02:00
2019-08-16 00:22:52 +02:00
Point * b ;
bool to_exists = points . lookup ( p_with_id , b ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( ! to_exists , vformat ( " Can't connect points. Point with id: %d doesn't exist. " , p_with_id ) ) ;
2019-08-16 00:22:52 +02:00
a - > neighbours . set ( b - > id , b ) ;
if ( bidirectional ) {
b - > neighbours . set ( a - > id , a ) ;
} else {
b - > unlinked_neighbours . set ( a - > id , a ) ;
}
2016-09-13 23:17:18 +02:00
Segment s ( p_id , p_with_id ) ;
2020-05-14 16:41:43 +02:00
if ( bidirectional ) {
2019-07-15 19:01:50 +02:00
s . direction = Segment : : BIDIRECTIONAL ;
2020-05-14 16:41:43 +02:00
}
2019-07-15 19:01:50 +02:00
2022-05-19 17:00:06 +02:00
HashSet < Segment , Segment > : : Iterator element = segments . find ( s ) ;
if ( element ) {
s . direction | = element - > direction ;
2019-07-15 19:01:50 +02:00
if ( s . direction = = Segment : : BIDIRECTIONAL ) {
// Both are neighbours of each other now
a - > unlinked_neighbours . remove ( b - > id ) ;
b - > unlinked_neighbours . remove ( a - > id ) ;
}
2022-05-19 17:00:06 +02:00
segments . remove ( element ) ;
2016-09-13 23:17:18 +02:00
}
segments . insert ( s ) ;
}
2019-08-16 00:22:52 +02:00
2022-06-16 10:56:12 +02:00
void AStar3D : : disconnect_points ( int64_t p_id , int64_t p_with_id , bool bidirectional ) {
2019-08-16 00:22:52 +02:00
Point * a ;
bool a_exists = points . lookup ( p_id , a ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( ! a_exists , vformat ( " Can't disconnect points. Point with id: %d doesn't exist. " , p_id ) ) ;
2019-08-16 00:22:52 +02:00
Point * b ;
bool b_exists = points . lookup ( p_with_id , b ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( ! b_exists , vformat ( " Can't disconnect points. Point with id: %d doesn't exist. " , p_with_id ) ) ;
2019-08-16 00:22:52 +02:00
2019-07-15 19:01:50 +02:00
Segment s ( p_id , p_with_id ) ;
2022-06-16 10:56:12 +02:00
int remove_direction = bidirectional ? ( int ) Segment : : BIDIRECTIONAL : ( int ) s . direction ;
2019-07-15 19:01:50 +02:00
2022-05-19 17:00:06 +02:00
HashSet < Segment , Segment > : : Iterator element = segments . find ( s ) ;
if ( element ) {
2019-07-15 19:01:50 +02:00
// s is the new segment
// Erase the directions to be removed
2022-05-19 17:00:06 +02:00
s . direction = ( element - > direction & ~ remove_direction ) ;
2019-07-13 05:22:12 +02:00
a - > neighbours . remove ( b - > id ) ;
2019-07-15 19:01:50 +02:00
if ( bidirectional ) {
b - > neighbours . remove ( a - > id ) ;
2022-05-19 17:00:06 +02:00
if ( element - > direction ! = Segment : : BIDIRECTIONAL ) {
2019-07-15 19:01:50 +02:00
a - > unlinked_neighbours . remove ( b - > id ) ;
b - > unlinked_neighbours . remove ( a - > id ) ;
}
} else {
2020-05-14 16:41:43 +02:00
if ( s . direction = = Segment : : NONE ) {
2019-07-15 19:01:50 +02:00
b - > unlinked_neighbours . remove ( a - > id ) ;
2020-05-14 16:41:43 +02:00
} else {
2019-07-15 19:01:50 +02:00
a - > unlinked_neighbours . set ( b - > id , b ) ;
2020-05-14 16:41:43 +02:00
}
2019-07-15 19:01:50 +02:00
}
2019-07-13 05:22:12 +02:00
2022-05-19 17:00:06 +02:00
segments . remove ( element ) ;
2020-05-14 16:41:43 +02:00
if ( s . direction ! = Segment : : NONE ) {
2019-07-15 19:01:50 +02:00
segments . insert ( s ) ;
2020-05-14 16:41:43 +02:00
}
2019-07-13 05:22:12 +02:00
}
2016-09-13 23:17:18 +02:00
}
2017-07-11 16:04:41 +02:00
2022-06-16 10:56:12 +02:00
bool AStar3D : : has_point ( int64_t p_id ) const {
2017-07-11 16:04:41 +02:00
return points . has ( p_id ) ;
}
2022-08-05 03:41:48 +02:00
PackedInt64Array AStar3D : : get_point_ids ( ) {
PackedInt64Array point_list ;
2017-09-07 16:11:48 +02:00
2022-06-16 10:56:12 +02:00
for ( OAHashMap < int64_t , Point * > : : Iterator it = points . iter ( ) ; it . valid ; it = points . next_iter ( it ) ) {
2019-08-16 00:22:52 +02:00
point_list . push_back ( * ( it . key ) ) ;
2017-09-07 16:11:48 +02:00
}
return point_list ;
}
2022-06-16 10:56:12 +02:00
Vector < int64_t > AStar3D : : get_point_connections ( int64_t p_id ) {
2019-08-16 00:22:52 +02:00
Point * p ;
bool p_exists = points . lookup ( p_id , p ) ;
2022-06-16 10:56:12 +02:00
ERR_FAIL_COND_V_MSG ( ! p_exists , Vector < int64_t > ( ) , vformat ( " Can't get point's connections. Point with id: %d doesn't exist. " , p_id ) ) ;
2017-11-02 20:42:58 +01:00
2022-06-16 10:56:12 +02:00
Vector < int64_t > point_list ;
2017-11-02 20:42:58 +01:00
2022-06-16 10:56:12 +02:00
for ( OAHashMap < int64_t , Point * > : : Iterator it = p - > neighbours . iter ( ) ; it . valid ; it = p - > neighbours . next_iter ( it ) ) {
2019-08-16 00:22:52 +02:00
point_list . push_back ( ( * it . key ) ) ;
2017-11-02 20:42:58 +01:00
}
return point_list ;
}
2022-06-16 10:56:12 +02:00
bool AStar3D : : are_points_connected ( int64_t p_id , int64_t p_with_id , bool bidirectional ) const {
2016-09-13 23:17:18 +02:00
Segment s ( p_id , p_with_id ) ;
2022-05-19 17:00:06 +02:00
const HashSet < Segment , Segment > : : Iterator element = segments . find ( s ) ;
2019-07-15 19:01:50 +02:00
2022-05-19 17:00:06 +02:00
return element & &
( bidirectional | | ( element - > direction & s . direction ) = = s . direction ) ;
2016-09-13 23:17:18 +02:00
}
2022-03-20 15:20:32 +01:00
void AStar3D : : clear ( ) {
2019-08-16 00:22:52 +02:00
last_free_id = 0 ;
2022-06-16 10:56:12 +02:00
for ( OAHashMap < int64_t , Point * > : : Iterator it = points . iter ( ) ; it . valid ; it = points . next_iter ( it ) ) {
2019-08-16 00:22:52 +02:00
memdelete ( * ( it . value ) ) ;
2016-09-13 23:17:18 +02:00
}
segments . clear ( ) ;
points . clear ( ) ;
}
2022-06-16 10:56:12 +02:00
int64_t AStar3D : : get_point_count ( ) const {
2019-08-25 21:30:52 +02:00
return points . get_num_elements ( ) ;
}
2022-06-16 10:56:12 +02:00
int64_t AStar3D : : get_point_capacity ( ) const {
2019-08-25 21:30:52 +02:00
return points . get_capacity ( ) ;
}
2022-06-16 10:56:12 +02:00
void AStar3D : : reserve_space ( int64_t p_num_nodes ) {
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( p_num_nodes < = 0 , vformat ( " New capacity must be greater than 0, new was: %d. " , p_num_nodes ) ) ;
ERR_FAIL_COND_MSG ( ( uint32_t ) p_num_nodes < points . get_capacity ( ) , vformat ( " New capacity must be greater than current capacity: %d, new was: %d. " , points . get_capacity ( ) , p_num_nodes ) ) ;
2019-08-25 21:30:52 +02:00
points . reserve ( p_num_nodes ) ;
}
2022-06-16 10:56:12 +02:00
int64_t AStar3D : : get_closest_point ( const Vector3 & p_point , bool p_include_disabled ) const {
int64_t closest_id = - 1 ;
2017-01-14 21:35:39 +01:00
real_t closest_dist = 1e20 ;
2016-09-13 23:17:18 +02:00
2022-06-16 10:56:12 +02:00
for ( OAHashMap < int64_t , Point * > : : Iterator it = points . iter ( ) ; it . valid ; it = points . next_iter ( it ) ) {
2020-05-14 16:41:43 +02:00
if ( ! p_include_disabled & & ! ( * it . value ) - > enabled ) {
2019-09-04 19:04:48 +02:00
continue ; // Disabled points should not be considered.
2020-05-14 16:41:43 +02:00
}
2016-09-13 23:17:18 +02:00
2020-06-09 10:26:21 +02:00
// Keep the closest point's ID, and in case of multiple closest IDs,
// the smallest one (makes it deterministic).
2019-08-16 00:22:52 +02:00
real_t d = p_point . distance_squared_to ( ( * it . value ) - > pos ) ;
2022-06-16 10:56:12 +02:00
int64_t id = * ( it . key ) ;
2020-06-09 10:26:21 +02:00
if ( d < = closest_dist ) {
if ( d = = closest_dist & & id > closest_id ) { // Keep lowest ID.
continue ;
}
2016-09-13 23:17:18 +02:00
closest_dist = d ;
2020-06-09 10:26:21 +02:00
closest_id = id ;
2016-09-13 23:17:18 +02:00
}
}
return closest_id ;
}
2017-10-27 19:19:01 +02:00
2022-03-20 15:20:32 +01:00
Vector3 AStar3D : : get_closest_position_in_segment ( const Vector3 & p_point ) const {
2019-08-16 00:22:52 +02:00
real_t closest_dist = 1e20 ;
2016-09-13 23:17:18 +02:00
Vector3 closest_point ;
2022-05-19 01:43:40 +02:00
for ( const Segment & E : segments ) {
2019-07-15 19:01:50 +02:00
Point * from_point = nullptr , * to_point = nullptr ;
2022-06-16 10:56:12 +02:00
points . lookup ( E . key . first , from_point ) ;
points . lookup ( E . key . second , to_point ) ;
2019-07-15 19:01:50 +02:00
if ( ! ( from_point - > enabled & & to_point - > enabled ) ) {
2019-06-27 05:19:52 +02:00
continue ;
}
2016-09-13 23:17:18 +02:00
Vector3 segment [ 2 ] = {
2019-07-15 19:01:50 +02:00
from_point - > pos ,
to_point - > pos ,
2016-09-13 23:17:18 +02:00
} ;
2020-05-25 19:20:45 +02:00
Vector3 p = Geometry3D : : get_closest_point_to_segment ( p_point , segment ) ;
2017-01-14 21:35:39 +01:00
real_t d = p_point . distance_squared_to ( p ) ;
2020-06-09 10:26:21 +02:00
if ( d < closest_dist ) {
2016-09-13 23:17:18 +02:00
closest_point = p ;
closest_dist = d ;
}
}
return closest_point ;
}
2022-03-20 15:20:32 +01:00
bool AStar3D : : _solve ( Point * begin_point , Point * end_point ) {
2016-09-13 23:17:18 +02:00
pass + + ;
2020-05-14 16:41:43 +02:00
if ( ! end_point - > enabled ) {
2019-08-16 00:22:52 +02:00
return false ;
2020-05-14 16:41:43 +02:00
}
2019-03-29 09:10:57 +01:00
2016-09-13 23:17:18 +02:00
bool found_route = false ;
2019-05-16 20:09:47 +02:00
Vector < Point * > open_list ;
SortArray < Point * , SortPoints > sorter ;
2019-03-29 09:10:57 +01:00
2019-05-16 20:09:47 +02:00
begin_point - > g_score = 0 ;
begin_point - > f_score = _estimate_cost ( begin_point - > id , end_point - > id ) ;
open_list . push_back ( begin_point ) ;
2016-09-13 23:17:18 +02:00
2020-12-15 13:04:21 +01:00
while ( ! open_list . is_empty ( ) ) {
2019-05-16 20:09:47 +02:00
Point * p = open_list [ 0 ] ; // The currently processed point
2016-09-13 23:17:18 +02:00
2018-08-27 18:58:22 +02:00
if ( p = = end_point ) {
found_route = true ;
break ;
}
2016-09-13 23:17:18 +02:00
2019-05-16 20:09:47 +02:00
sorter . pop_heap ( 0 , open_list . size ( ) , open_list . ptrw ( ) ) ; // Remove the current point from the open list
2021-07-04 00:17:03 +02:00
open_list . remove_at ( open_list . size ( ) - 1 ) ;
2019-05-16 20:09:47 +02:00
p - > closed_pass = pass ; // Mark the point as closed
2022-06-16 10:56:12 +02:00
for ( OAHashMap < int64_t , Point * > : : Iterator it = p - > neighbours . iter ( ) ; it . valid ; it = p - > neighbours . next_iter ( it ) ) {
2019-08-16 00:22:52 +02:00
Point * e = * ( it . value ) ; // The neighbour point
2016-09-13 23:17:18 +02:00
2019-08-16 00:22:52 +02:00
if ( ! e - > enabled | | e - > closed_pass = = pass ) {
2019-03-29 09:10:57 +01:00
continue ;
2019-08-16 00:22:52 +02:00
}
2019-03-29 09:10:57 +01:00
2019-05-16 20:09:47 +02:00
real_t tentative_g_score = p - > g_score + _compute_cost ( p - > id , e - > id ) * e - > weight_scale ;
bool new_point = false ;
2016-09-13 23:17:18 +02:00
2019-08-16 00:22:52 +02:00
if ( e - > open_pass ! = pass ) { // The point wasn't inside the open list.
2019-05-16 20:09:47 +02:00
e - > open_pass = pass ;
open_list . push_back ( e ) ;
new_point = true ;
2019-08-16 00:22:52 +02:00
} else if ( tentative_g_score > = e - > g_score ) { // The new path is worse than the previous.
2019-05-16 20:09:47 +02:00
continue ;
2016-09-13 23:17:18 +02:00
}
2019-05-16 20:09:47 +02:00
e - > prev_point = p ;
e - > g_score = tentative_g_score ;
e - > f_score = e - > g_score + _estimate_cost ( e - > id , end_point - > id ) ;
2016-09-13 23:17:18 +02:00
2019-08-16 00:22:52 +02:00
if ( new_point ) { // The position of the new points is already known.
2019-05-16 20:09:47 +02:00
sorter . push_heap ( 0 , open_list . size ( ) - 1 , 0 , e , open_list . ptrw ( ) ) ;
2019-08-16 00:22:52 +02:00
} else {
2019-05-16 20:09:47 +02:00
sorter . push_heap ( 0 , open_list . find ( e ) , 0 , e , open_list . ptrw ( ) ) ;
2019-08-16 00:22:52 +02:00
}
2019-05-16 20:09:47 +02:00
}
2016-09-13 23:17:18 +02:00
}
return found_route ;
}
2022-06-16 10:56:12 +02:00
real_t AStar3D : : _estimate_cost ( int64_t p_from_id , int64_t p_to_id ) {
2021-08-22 03:52:44 +02:00
real_t scost ;
if ( GDVIRTUAL_CALL ( _estimate_cost , p_from_id , p_to_id , scost ) ) {
return scost ;
2020-05-14 16:41:43 +02:00
}
2017-03-25 10:44:41 +01:00
2019-08-16 00:22:52 +02:00
Point * from_point ;
bool from_exists = points . lookup ( p_from_id , from_point ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! from_exists , 0 , vformat ( " Can't estimate cost. Point with id: %d doesn't exist. " , p_from_id ) ) ;
2019-08-16 00:22:52 +02:00
Point * to_point ;
bool to_exists = points . lookup ( p_to_id , to_point ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! to_exists , 0 , vformat ( " Can't estimate cost. Point with id: %d doesn't exist. " , p_to_id ) ) ;
2019-08-16 00:22:52 +02:00
return from_point - > pos . distance_to ( to_point - > pos ) ;
2017-03-25 10:44:41 +01:00
}
2022-06-16 10:56:12 +02:00
real_t AStar3D : : _compute_cost ( int64_t p_from_id , int64_t p_to_id ) {
2021-08-22 03:52:44 +02:00
real_t scost ;
if ( GDVIRTUAL_CALL ( _compute_cost , p_from_id , p_to_id , scost ) ) {
return scost ;
2020-05-14 16:41:43 +02:00
}
2017-03-25 10:44:41 +01:00
2019-08-16 00:22:52 +02:00
Point * from_point ;
bool from_exists = points . lookup ( p_from_id , from_point ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! from_exists , 0 , vformat ( " Can't compute cost. Point with id: %d doesn't exist. " , p_from_id ) ) ;
2019-08-16 00:22:52 +02:00
Point * to_point ;
bool to_exists = points . lookup ( p_to_id , to_point ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! to_exists , 0 , vformat ( " Can't compute cost. Point with id: %d doesn't exist. " , p_to_id ) ) ;
2019-08-16 00:22:52 +02:00
return from_point - > pos . distance_to ( to_point - > pos ) ;
2017-03-25 10:44:41 +01:00
}
2022-06-16 10:56:12 +02:00
Vector < Vector3 > AStar3D : : get_point_path ( int64_t p_from_id , int64_t p_to_id ) {
2019-08-16 00:22:52 +02:00
Point * a ;
bool from_exists = points . lookup ( p_from_id , a ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! from_exists , Vector < Vector3 > ( ) , vformat ( " Can't get point path. Point with id: %d doesn't exist. " , p_from_id ) ) ;
2016-09-13 23:17:18 +02:00
2019-08-16 00:22:52 +02:00
Point * b ;
bool to_exists = points . lookup ( p_to_id , b ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! to_exists , Vector < Vector3 > ( ) , vformat ( " Can't get point path. Point with id: %d doesn't exist. " , p_to_id ) ) ;
2016-09-13 23:17:18 +02:00
if ( a = = b ) {
2020-02-17 22:06:54 +01:00
Vector < Vector3 > ret ;
2016-09-13 23:17:18 +02:00
ret . push_back ( a - > pos ) ;
return ret ;
}
Point * begin_point = a ;
Point * end_point = b ;
bool found_route = _solve ( begin_point , end_point ) ;
2020-05-14 16:41:43 +02:00
if ( ! found_route ) {
2020-02-17 22:06:54 +01:00
return Vector < Vector3 > ( ) ;
2020-05-14 16:41:43 +02:00
}
2016-09-13 23:17:18 +02:00
Point * p = end_point ;
2022-06-16 10:56:12 +02:00
int64_t pc = 1 ; // Begin point
2016-09-13 23:17:18 +02:00
while ( p ! = begin_point ) {
pc + + ;
p = p - > prev_point ;
}
2020-02-17 22:06:54 +01:00
Vector < Vector3 > path ;
2016-09-13 23:17:18 +02:00
path . resize ( pc ) ;
{
2020-02-17 22:06:54 +01:00
Vector3 * w = path . ptrw ( ) ;
2016-09-13 23:17:18 +02:00
2019-02-12 21:10:08 +01:00
Point * p2 = end_point ;
2022-06-16 10:56:12 +02:00
int64_t idx = pc - 1 ;
2019-02-12 21:10:08 +01:00
while ( p2 ! = begin_point ) {
w [ idx - - ] = p2 - > pos ;
p2 = p2 - > prev_point ;
2016-09-13 23:17:18 +02:00
}
2019-02-12 21:10:08 +01:00
w [ 0 ] = p2 - > pos ; // Assign first
2016-09-13 23:17:18 +02:00
}
return path ;
}
2022-06-16 10:56:12 +02:00
Vector < int64_t > AStar3D : : get_id_path ( int64_t p_from_id , int64_t p_to_id ) {
2019-08-16 00:22:52 +02:00
Point * a ;
bool from_exists = points . lookup ( p_from_id , a ) ;
2022-06-16 10:56:12 +02:00
ERR_FAIL_COND_V_MSG ( ! from_exists , Vector < int64_t > ( ) , vformat ( " Can't get id path. Point with id: %d doesn't exist. " , p_from_id ) ) ;
2016-09-13 23:17:18 +02:00
2019-08-16 00:22:52 +02:00
Point * b ;
bool to_exists = points . lookup ( p_to_id , b ) ;
2022-06-16 10:56:12 +02:00
ERR_FAIL_COND_V_MSG ( ! to_exists , Vector < int64_t > ( ) , vformat ( " Can't get id path. Point with id: %d doesn't exist. " , p_to_id ) ) ;
2016-09-13 23:17:18 +02:00
if ( a = = b ) {
2022-06-16 10:56:12 +02:00
Vector < int64_t > ret ;
2016-09-13 23:17:18 +02:00
ret . push_back ( a - > id ) ;
return ret ;
}
Point * begin_point = a ;
Point * end_point = b ;
bool found_route = _solve ( begin_point , end_point ) ;
2020-05-14 16:41:43 +02:00
if ( ! found_route ) {
2022-06-16 10:56:12 +02:00
return Vector < int64_t > ( ) ;
2020-05-14 16:41:43 +02:00
}
2016-09-13 23:17:18 +02:00
Point * p = end_point ;
2022-06-16 10:56:12 +02:00
int64_t pc = 1 ; // Begin point
2016-09-13 23:17:18 +02:00
while ( p ! = begin_point ) {
pc + + ;
p = p - > prev_point ;
}
2022-06-16 10:56:12 +02:00
Vector < int64_t > path ;
2016-09-13 23:17:18 +02:00
path . resize ( pc ) ;
{
2022-06-16 10:56:12 +02:00
int64_t * w = path . ptrw ( ) ;
2016-09-13 23:17:18 +02:00
p = end_point ;
2022-06-16 10:56:12 +02:00
int64_t idx = pc - 1 ;
2016-09-13 23:17:18 +02:00
while ( p ! = begin_point ) {
w [ idx - - ] = p - > id ;
p = p - > prev_point ;
}
2017-10-27 19:19:01 +02:00
w [ 0 ] = p - > id ; // Assign first
2016-09-13 23:17:18 +02:00
}
return path ;
}
2022-06-16 10:56:12 +02:00
void AStar3D : : set_point_disabled ( int64_t p_id , bool p_disabled ) {
2019-08-16 00:22:52 +02:00
Point * p ;
bool p_exists = points . lookup ( p_id , p ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_MSG ( ! p_exists , vformat ( " Can't set if point is disabled. Point with id: %d doesn't exist. " , p_id ) ) ;
2019-06-18 06:53:41 +02:00
2019-08-16 00:22:52 +02:00
p - > enabled = ! p_disabled ;
2019-03-29 09:10:57 +01:00
}
2022-06-16 10:56:12 +02:00
bool AStar3D : : is_point_disabled ( int64_t p_id ) const {
2019-08-16 00:22:52 +02:00
Point * p ;
bool p_exists = points . lookup ( p_id , p ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! p_exists , false , vformat ( " Can't get if point is disabled. Point with id: %d doesn't exist. " , p_id ) ) ;
2019-06-18 06:53:41 +02:00
2019-08-16 00:22:52 +02:00
return ! p - > enabled ;
2019-03-29 09:10:57 +01:00
}
2022-03-20 15:20:32 +01:00
void AStar3D : : _bind_methods ( ) {
ClassDB : : bind_method ( D_METHOD ( " get_available_point_id " ) , & AStar3D : : get_available_point_id ) ;
ClassDB : : bind_method ( D_METHOD ( " add_point " , " id " , " position " , " weight_scale " ) , & AStar3D : : add_point , DEFVAL ( 1.0 ) ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_position " , " id " ) , & AStar3D : : get_point_position ) ;
ClassDB : : bind_method ( D_METHOD ( " set_point_position " , " id " , " position " ) , & AStar3D : : set_point_position ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_weight_scale " , " id " ) , & AStar3D : : get_point_weight_scale ) ;
ClassDB : : bind_method ( D_METHOD ( " set_point_weight_scale " , " id " , " weight_scale " ) , & AStar3D : : set_point_weight_scale ) ;
ClassDB : : bind_method ( D_METHOD ( " remove_point " , " id " ) , & AStar3D : : remove_point ) ;
ClassDB : : bind_method ( D_METHOD ( " has_point " , " id " ) , & AStar3D : : has_point ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_connections " , " id " ) , & AStar3D : : get_point_connections ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_ids " ) , & AStar3D : : get_point_ids ) ;
2016-09-13 23:17:18 +02:00
2022-03-20 15:20:32 +01:00
ClassDB : : bind_method ( D_METHOD ( " set_point_disabled " , " id " , " disabled " ) , & AStar3D : : set_point_disabled , DEFVAL ( true ) ) ;
ClassDB : : bind_method ( D_METHOD ( " is_point_disabled " , " id " ) , & AStar3D : : is_point_disabled ) ;
2019-03-29 09:10:57 +01:00
2022-03-20 15:20:32 +01:00
ClassDB : : bind_method ( D_METHOD ( " connect_points " , " id " , " to_id " , " bidirectional " ) , & AStar3D : : connect_points , DEFVAL ( true ) ) ;
ClassDB : : bind_method ( D_METHOD ( " disconnect_points " , " id " , " to_id " , " bidirectional " ) , & AStar3D : : disconnect_points , DEFVAL ( true ) ) ;
ClassDB : : bind_method ( D_METHOD ( " are_points_connected " , " id " , " to_id " , " bidirectional " ) , & AStar3D : : are_points_connected , DEFVAL ( true ) ) ;
2016-09-13 23:17:18 +02:00
2022-03-20 15:20:32 +01:00
ClassDB : : bind_method ( D_METHOD ( " get_point_count " ) , & AStar3D : : get_point_count ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_capacity " ) , & AStar3D : : get_point_capacity ) ;
ClassDB : : bind_method ( D_METHOD ( " reserve_space " , " num_nodes " ) , & AStar3D : : reserve_space ) ;
ClassDB : : bind_method ( D_METHOD ( " clear " ) , & AStar3D : : clear ) ;
2016-09-13 23:17:18 +02:00
2022-03-20 15:20:32 +01:00
ClassDB : : bind_method ( D_METHOD ( " get_closest_point " , " to_position " , " include_disabled " ) , & AStar3D : : get_closest_point , DEFVAL ( false ) ) ;
ClassDB : : bind_method ( D_METHOD ( " get_closest_position_in_segment " , " to_position " ) , & AStar3D : : get_closest_position_in_segment ) ;
2016-09-13 23:17:18 +02:00
2022-03-20 15:20:32 +01:00
ClassDB : : bind_method ( D_METHOD ( " get_point_path " , " from_id " , " to_id " ) , & AStar3D : : get_point_path ) ;
ClassDB : : bind_method ( D_METHOD ( " get_id_path " , " from_id " , " to_id " ) , & AStar3D : : get_id_path ) ;
2017-03-25 10:44:41 +01:00
2021-08-22 03:52:44 +02:00
GDVIRTUAL_BIND ( _estimate_cost , " from_id " , " to_id " )
GDVIRTUAL_BIND ( _compute_cost , " from_id " , " to_id " )
2016-09-13 23:17:18 +02:00
}
2022-03-20 15:20:32 +01:00
AStar3D : : ~ AStar3D ( ) {
2017-10-23 00:37:02 +02:00
clear ( ) ;
2016-09-13 23:17:18 +02:00
}
2019-03-19 17:15:59 +01:00
/////////////////////////////////////////////////////////////
2022-06-16 10:56:12 +02:00
int64_t AStar2D : : get_available_point_id ( ) const {
2019-03-19 17:15:59 +01:00
return astar . get_available_point_id ( ) ;
}
2022-06-16 10:56:12 +02:00
void AStar2D : : add_point ( int64_t p_id , const Vector2 & p_pos , real_t p_weight_scale ) {
2019-03-19 17:15:59 +01:00
astar . add_point ( p_id , Vector3 ( p_pos . x , p_pos . y , 0 ) , p_weight_scale ) ;
}
2022-06-16 10:56:12 +02:00
Vector2 AStar2D : : get_point_position ( int64_t p_id ) const {
2019-03-19 17:15:59 +01:00
Vector3 p = astar . get_point_position ( p_id ) ;
return Vector2 ( p . x , p . y ) ;
}
2022-06-16 10:56:12 +02:00
void AStar2D : : set_point_position ( int64_t p_id , const Vector2 & p_pos ) {
2019-03-19 17:15:59 +01:00
astar . set_point_position ( p_id , Vector3 ( p_pos . x , p_pos . y , 0 ) ) ;
}
2022-06-16 10:56:12 +02:00
real_t AStar2D : : get_point_weight_scale ( int64_t p_id ) const {
2019-03-19 17:15:59 +01:00
return astar . get_point_weight_scale ( p_id ) ;
}
2022-06-16 10:56:12 +02:00
void AStar2D : : set_point_weight_scale ( int64_t p_id , real_t p_weight_scale ) {
2019-03-19 17:15:59 +01:00
astar . set_point_weight_scale ( p_id , p_weight_scale ) ;
}
2022-06-16 10:56:12 +02:00
void AStar2D : : remove_point ( int64_t p_id ) {
2019-03-19 17:15:59 +01:00
astar . remove_point ( p_id ) ;
}
2022-06-16 10:56:12 +02:00
bool AStar2D : : has_point ( int64_t p_id ) const {
2019-03-19 17:15:59 +01:00
return astar . has_point ( p_id ) ;
}
2022-06-16 10:56:12 +02:00
Vector < int64_t > AStar2D : : get_point_connections ( int64_t p_id ) {
2019-03-19 17:15:59 +01:00
return astar . get_point_connections ( p_id ) ;
}
2022-08-05 03:41:48 +02:00
PackedInt64Array AStar2D : : get_point_ids ( ) {
2020-05-10 02:34:50 +02:00
return astar . get_point_ids ( ) ;
2019-03-19 17:15:59 +01:00
}
2022-06-16 10:56:12 +02:00
void AStar2D : : set_point_disabled ( int64_t p_id , bool p_disabled ) {
2019-03-19 17:15:59 +01:00
astar . set_point_disabled ( p_id , p_disabled ) ;
}
2022-06-16 10:56:12 +02:00
bool AStar2D : : is_point_disabled ( int64_t p_id ) const {
2019-03-19 17:15:59 +01:00
return astar . is_point_disabled ( p_id ) ;
}
2022-06-16 10:56:12 +02:00
void AStar2D : : connect_points ( int64_t p_id , int64_t p_with_id , bool p_bidirectional ) {
2019-03-19 17:15:59 +01:00
astar . connect_points ( p_id , p_with_id , p_bidirectional ) ;
}
2022-06-16 10:56:12 +02:00
void AStar2D : : disconnect_points ( int64_t p_id , int64_t p_with_id , bool p_bidirectional ) {
2022-03-01 23:01:32 +01:00
astar . disconnect_points ( p_id , p_with_id , p_bidirectional ) ;
2019-03-19 17:15:59 +01:00
}
2022-06-16 10:56:12 +02:00
bool AStar2D : : are_points_connected ( int64_t p_id , int64_t p_with_id , bool p_bidirectional ) const {
2022-03-01 23:01:32 +01:00
return astar . are_points_connected ( p_id , p_with_id , p_bidirectional ) ;
2019-03-19 17:15:59 +01:00
}
2022-06-16 10:56:12 +02:00
int64_t AStar2D : : get_point_count ( ) const {
2019-08-25 21:30:52 +02:00
return astar . get_point_count ( ) ;
}
2022-06-16 10:56:12 +02:00
int64_t AStar2D : : get_point_capacity ( ) const {
2019-08-25 21:30:52 +02:00
return astar . get_point_capacity ( ) ;
}
2019-03-19 17:15:59 +01:00
void AStar2D : : clear ( ) {
astar . clear ( ) ;
}
2022-06-16 10:56:12 +02:00
void AStar2D : : reserve_space ( int64_t p_num_nodes ) {
2019-08-25 21:30:52 +02:00
astar . reserve_space ( p_num_nodes ) ;
}
2022-06-16 10:56:12 +02:00
int64_t AStar2D : : get_closest_point ( const Vector2 & p_point , bool p_include_disabled ) const {
2019-09-04 19:04:48 +02:00
return astar . get_closest_point ( Vector3 ( p_point . x , p_point . y , 0 ) , p_include_disabled ) ;
2019-03-19 17:15:59 +01:00
}
Vector2 AStar2D : : get_closest_position_in_segment ( const Vector2 & p_point ) const {
Vector3 p = astar . get_closest_position_in_segment ( Vector3 ( p_point . x , p_point . y , 0 ) ) ;
return Vector2 ( p . x , p . y ) ;
}
2022-06-16 10:56:12 +02:00
real_t AStar2D : : _estimate_cost ( int64_t p_from_id , int64_t p_to_id ) {
2021-08-22 03:52:44 +02:00
real_t scost ;
if ( GDVIRTUAL_CALL ( _estimate_cost , p_from_id , p_to_id , scost ) ) {
return scost ;
2020-05-14 16:41:43 +02:00
}
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * from_point ;
2020-03-14 08:22:34 +01:00
bool from_exists = astar . points . lookup ( p_from_id , from_point ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! from_exists , 0 , vformat ( " Can't estimate cost. Point with id: %d doesn't exist. " , p_from_id ) ) ;
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * to_point ;
2020-03-14 08:22:34 +01:00
bool to_exists = astar . points . lookup ( p_to_id , to_point ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! to_exists , 0 , vformat ( " Can't estimate cost. Point with id: %d doesn't exist. " , p_to_id ) ) ;
2020-03-14 08:22:34 +01:00
return from_point - > pos . distance_to ( to_point - > pos ) ;
}
2022-06-16 10:56:12 +02:00
real_t AStar2D : : _compute_cost ( int64_t p_from_id , int64_t p_to_id ) {
2021-08-22 03:52:44 +02:00
real_t scost ;
if ( GDVIRTUAL_CALL ( _compute_cost , p_from_id , p_to_id , scost ) ) {
return scost ;
2020-05-14 16:41:43 +02:00
}
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * from_point ;
2020-03-14 08:22:34 +01:00
bool from_exists = astar . points . lookup ( p_from_id , from_point ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! from_exists , 0 , vformat ( " Can't compute cost. Point with id: %d doesn't exist. " , p_from_id ) ) ;
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * to_point ;
2020-03-14 08:22:34 +01:00
bool to_exists = astar . points . lookup ( p_to_id , to_point ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! to_exists , 0 , vformat ( " Can't compute cost. Point with id: %d doesn't exist. " , p_to_id ) ) ;
2020-03-14 08:22:34 +01:00
return from_point - > pos . distance_to ( to_point - > pos ) ;
}
2022-06-16 10:56:12 +02:00
Vector < Vector2 > AStar2D : : get_point_path ( int64_t p_from_id , int64_t p_to_id ) {
2022-03-20 15:20:32 +01:00
AStar3D : : Point * a ;
2020-03-14 08:22:34 +01:00
bool from_exists = astar . points . lookup ( p_from_id , a ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! from_exists , Vector < Vector2 > ( ) , vformat ( " Can't get point path. Point with id: %d doesn't exist. " , p_from_id ) ) ;
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * b ;
2020-03-14 08:22:34 +01:00
bool to_exists = astar . points . lookup ( p_to_id , b ) ;
2021-09-03 00:06:23 +02:00
ERR_FAIL_COND_V_MSG ( ! to_exists , Vector < Vector2 > ( ) , vformat ( " Can't get point path. Point with id: %d doesn't exist. " , p_to_id ) ) ;
2020-03-14 08:22:34 +01:00
if ( a = = b ) {
2022-01-11 16:27:39 +01:00
Vector < Vector2 > ret = { Vector2 ( a - > pos . x , a - > pos . y ) } ;
2020-03-14 08:22:34 +01:00
return ret ;
}
2022-03-20 15:20:32 +01:00
AStar3D : : Point * begin_point = a ;
AStar3D : : Point * end_point = b ;
2020-03-14 08:22:34 +01:00
bool found_route = _solve ( begin_point , end_point ) ;
2020-05-14 16:41:43 +02:00
if ( ! found_route ) {
2020-03-14 08:22:34 +01:00
return Vector < Vector2 > ( ) ;
2020-05-14 16:41:43 +02:00
}
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * p = end_point ;
2022-06-16 10:56:12 +02:00
int64_t pc = 1 ; // Begin point
2020-03-14 08:22:34 +01:00
while ( p ! = begin_point ) {
pc + + ;
p = p - > prev_point ;
}
Vector < Vector2 > path ;
path . resize ( pc ) ;
2019-03-19 17:15:59 +01:00
{
2020-02-17 22:06:54 +01:00
Vector2 * w = path . ptrw ( ) ;
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * p2 = end_point ;
2022-06-16 10:56:12 +02:00
int64_t idx = pc - 1 ;
2020-03-14 08:22:34 +01:00
while ( p2 ! = begin_point ) {
w [ idx - - ] = Vector2 ( p2 - > pos . x , p2 - > pos . y ) ;
p2 = p2 - > prev_point ;
2019-03-19 17:15:59 +01:00
}
2020-03-14 08:22:34 +01:00
w [ 0 ] = Vector2 ( p2 - > pos . x , p2 - > pos . y ) ; // Assign first
2019-03-19 17:15:59 +01:00
}
2020-03-14 08:22:34 +01:00
2019-03-19 17:15:59 +01:00
return path ;
}
2022-06-16 10:56:12 +02:00
Vector < int64_t > AStar2D : : get_id_path ( int64_t p_from_id , int64_t p_to_id ) {
2022-03-20 15:20:32 +01:00
AStar3D : : Point * a ;
2020-03-14 08:22:34 +01:00
bool from_exists = astar . points . lookup ( p_from_id , a ) ;
2022-06-16 10:56:12 +02:00
ERR_FAIL_COND_V_MSG ( ! from_exists , Vector < int64_t > ( ) , vformat ( " Can't get id path. Point with id: %d doesn't exist. " , p_from_id ) ) ;
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * b ;
2020-03-14 08:22:34 +01:00
bool to_exists = astar . points . lookup ( p_to_id , b ) ;
2022-06-16 10:56:12 +02:00
ERR_FAIL_COND_V_MSG ( ! to_exists , Vector < int64_t > ( ) , vformat ( " Can't get id path. Point with id: %d doesn't exist. " , p_to_id ) ) ;
2020-03-14 08:22:34 +01:00
if ( a = = b ) {
2022-06-16 10:56:12 +02:00
Vector < int64_t > ret ;
2020-03-14 08:22:34 +01:00
ret . push_back ( a - > id ) ;
return ret ;
}
2022-03-20 15:20:32 +01:00
AStar3D : : Point * begin_point = a ;
AStar3D : : Point * end_point = b ;
2020-03-14 08:22:34 +01:00
bool found_route = _solve ( begin_point , end_point ) ;
2020-05-14 16:41:43 +02:00
if ( ! found_route ) {
2022-06-16 10:56:12 +02:00
return Vector < int64_t > ( ) ;
2020-05-14 16:41:43 +02:00
}
2020-03-14 08:22:34 +01:00
2022-03-20 15:20:32 +01:00
AStar3D : : Point * p = end_point ;
2022-06-16 10:56:12 +02:00
int64_t pc = 1 ; // Begin point
2020-03-14 08:22:34 +01:00
while ( p ! = begin_point ) {
pc + + ;
p = p - > prev_point ;
}
2022-06-16 10:56:12 +02:00
Vector < int64_t > path ;
2020-03-14 08:22:34 +01:00
path . resize ( pc ) ;
{
2022-06-16 10:56:12 +02:00
int64_t * w = path . ptrw ( ) ;
2020-03-14 08:22:34 +01:00
p = end_point ;
2022-06-16 10:56:12 +02:00
int64_t idx = pc - 1 ;
2020-03-14 08:22:34 +01:00
while ( p ! = begin_point ) {
w [ idx - - ] = p - > id ;
p = p - > prev_point ;
}
w [ 0 ] = p - > id ; // Assign first
}
return path ;
}
2022-03-20 15:20:32 +01:00
bool AStar2D : : _solve ( AStar3D : : Point * begin_point , AStar3D : : Point * end_point ) {
2020-03-14 08:22:34 +01:00
astar . pass + + ;
2020-05-14 16:41:43 +02:00
if ( ! end_point - > enabled ) {
2020-03-14 08:22:34 +01:00
return false ;
2020-05-14 16:41:43 +02:00
}
2020-03-14 08:22:34 +01:00
bool found_route = false ;
2022-03-20 15:20:32 +01:00
Vector < AStar3D : : Point * > open_list ;
SortArray < AStar3D : : Point * , AStar3D : : SortPoints > sorter ;
2020-03-14 08:22:34 +01:00
begin_point - > g_score = 0 ;
begin_point - > f_score = _estimate_cost ( begin_point - > id , end_point - > id ) ;
open_list . push_back ( begin_point ) ;
2020-12-15 13:04:21 +01:00
while ( ! open_list . is_empty ( ) ) {
2022-03-20 15:20:32 +01:00
AStar3D : : Point * p = open_list [ 0 ] ; // The currently processed point
2020-03-14 08:22:34 +01:00
if ( p = = end_point ) {
found_route = true ;
break ;
}
sorter . pop_heap ( 0 , open_list . size ( ) , open_list . ptrw ( ) ) ; // Remove the current point from the open list
2021-07-04 00:17:03 +02:00
open_list . remove_at ( open_list . size ( ) - 1 ) ;
2020-03-14 08:22:34 +01:00
p - > closed_pass = astar . pass ; // Mark the point as closed
2022-06-16 10:56:12 +02:00
for ( OAHashMap < int64_t , AStar3D : : Point * > : : Iterator it = p - > neighbours . iter ( ) ; it . valid ; it = p - > neighbours . next_iter ( it ) ) {
2022-03-20 15:20:32 +01:00
AStar3D : : Point * e = * ( it . value ) ; // The neighbour point
2020-03-14 08:22:34 +01:00
if ( ! e - > enabled | | e - > closed_pass = = astar . pass ) {
continue ;
}
real_t tentative_g_score = p - > g_score + _compute_cost ( p - > id , e - > id ) * e - > weight_scale ;
bool new_point = false ;
if ( e - > open_pass ! = astar . pass ) { // The point wasn't inside the open list.
e - > open_pass = astar . pass ;
open_list . push_back ( e ) ;
new_point = true ;
} else if ( tentative_g_score > = e - > g_score ) { // The new path is worse than the previous.
continue ;
}
e - > prev_point = p ;
e - > g_score = tentative_g_score ;
e - > f_score = e - > g_score + _estimate_cost ( e - > id , end_point - > id ) ;
if ( new_point ) { // The position of the new points is already known.
sorter . push_heap ( 0 , open_list . size ( ) - 1 , 0 , e , open_list . ptrw ( ) ) ;
} else {
sorter . push_heap ( 0 , open_list . find ( e ) , 0 , e , open_list . ptrw ( ) ) ;
}
}
}
return found_route ;
2019-03-19 17:15:59 +01:00
}
void AStar2D : : _bind_methods ( ) {
ClassDB : : bind_method ( D_METHOD ( " get_available_point_id " ) , & AStar2D : : get_available_point_id ) ;
ClassDB : : bind_method ( D_METHOD ( " add_point " , " id " , " position " , " weight_scale " ) , & AStar2D : : add_point , DEFVAL ( 1.0 ) ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_position " , " id " ) , & AStar2D : : get_point_position ) ;
ClassDB : : bind_method ( D_METHOD ( " set_point_position " , " id " , " position " ) , & AStar2D : : set_point_position ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_weight_scale " , " id " ) , & AStar2D : : get_point_weight_scale ) ;
ClassDB : : bind_method ( D_METHOD ( " set_point_weight_scale " , " id " , " weight_scale " ) , & AStar2D : : set_point_weight_scale ) ;
ClassDB : : bind_method ( D_METHOD ( " remove_point " , " id " ) , & AStar2D : : remove_point ) ;
ClassDB : : bind_method ( D_METHOD ( " has_point " , " id " ) , & AStar2D : : has_point ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_connections " , " id " ) , & AStar2D : : get_point_connections ) ;
2020-05-10 02:34:50 +02:00
ClassDB : : bind_method ( D_METHOD ( " get_point_ids " ) , & AStar2D : : get_point_ids ) ;
2019-03-19 17:15:59 +01:00
ClassDB : : bind_method ( D_METHOD ( " set_point_disabled " , " id " , " disabled " ) , & AStar2D : : set_point_disabled , DEFVAL ( true ) ) ;
ClassDB : : bind_method ( D_METHOD ( " is_point_disabled " , " id " ) , & AStar2D : : is_point_disabled ) ;
ClassDB : : bind_method ( D_METHOD ( " connect_points " , " id " , " to_id " , " bidirectional " ) , & AStar2D : : connect_points , DEFVAL ( true ) ) ;
2022-03-01 23:01:32 +01:00
ClassDB : : bind_method ( D_METHOD ( " disconnect_points " , " id " , " to_id " , " bidirectional " ) , & AStar2D : : disconnect_points , DEFVAL ( true ) ) ;
ClassDB : : bind_method ( D_METHOD ( " are_points_connected " , " id " , " to_id " , " bidirectional " ) , & AStar2D : : are_points_connected , DEFVAL ( true ) ) ;
2019-03-19 17:15:59 +01:00
2019-08-25 21:30:52 +02:00
ClassDB : : bind_method ( D_METHOD ( " get_point_count " ) , & AStar2D : : get_point_count ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_capacity " ) , & AStar2D : : get_point_capacity ) ;
ClassDB : : bind_method ( D_METHOD ( " reserve_space " , " num_nodes " ) , & AStar2D : : reserve_space ) ;
2019-03-19 17:15:59 +01:00
ClassDB : : bind_method ( D_METHOD ( " clear " ) , & AStar2D : : clear ) ;
2019-09-04 19:04:48 +02:00
ClassDB : : bind_method ( D_METHOD ( " get_closest_point " , " to_position " , " include_disabled " ) , & AStar2D : : get_closest_point , DEFVAL ( false ) ) ;
2019-03-19 17:15:59 +01:00
ClassDB : : bind_method ( D_METHOD ( " get_closest_position_in_segment " , " to_position " ) , & AStar2D : : get_closest_position_in_segment ) ;
ClassDB : : bind_method ( D_METHOD ( " get_point_path " , " from_id " , " to_id " ) , & AStar2D : : get_point_path ) ;
ClassDB : : bind_method ( D_METHOD ( " get_id_path " , " from_id " , " to_id " ) , & AStar2D : : get_id_path ) ;
2020-03-14 08:22:34 +01:00
2021-08-22 03:52:44 +02:00
GDVIRTUAL_BIND ( _estimate_cost , " from_id " , " to_id " )
GDVIRTUAL_BIND ( _compute_cost , " from_id " , " to_id " )
2019-03-19 17:15:59 +01:00
}