1 /*************************************************************************/ 2 /* a_star.h */ 3 /*************************************************************************/ 4 /* This file is part of: */ 5 /* GODOT ENGINE */ 6 /* https://godotengine.org */ 7 /*************************************************************************/ 8 /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */ 9 /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */ 10 /* */ 11 /* Permission is hereby granted, free of charge, to any person obtaining */ 12 /* a copy of this software and associated documentation files (the */ 13 /* "Software"), to deal in the Software without restriction, including */ 14 /* without limitation the rights to use, copy, modify, merge, publish, */ 15 /* distribute, sublicense, and/or sell copies of the Software, and to */ 16 /* permit persons to whom the Software is furnished to do so, subject to */ 17 /* the following conditions: */ 18 /* */ 19 /* The above copyright notice and this permission notice shall be */ 20 /* included in all copies or substantial portions of the Software. */ 21 /* */ 22 /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ 23 /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ 24 /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ 25 /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ 26 /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ 27 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ 28 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ 29 /*************************************************************************/ 30 31 #ifndef ASTAR_H 32 #define ASTAR_H 33 34 #include "core/oa_hash_map.h" 35 #include "core/reference.h" 36 37 /** 38 A* pathfinding algorithm 39 40 @author Juan Linietsky <reduzio@gmail.com> 41 */ 42 43 class AStar : public Reference { 44 45 GDCLASS(AStar, Reference); 46 friend class AStar2D; 47 48 struct Point { 49 PointPoint50 Point() : 51 neighbours(4u), 52 unlinked_neighbours(4u) {} 53 54 int id; 55 Vector3 pos; 56 real_t weight_scale; 57 bool enabled; 58 59 OAHashMap<int, Point *> neighbours; 60 OAHashMap<int, Point *> unlinked_neighbours; 61 62 // Used for pathfinding. 63 Point *prev_point; 64 real_t g_score; 65 real_t f_score; 66 uint64_t open_pass; 67 uint64_t closed_pass; 68 }; 69 70 struct SortPoints { operatorSortPoints71 _FORCE_INLINE_ bool operator()(const Point *A, const Point *B) const { // Returns true when the Point A is worse than Point B. 72 if (A->f_score > B->f_score) { 73 return true; 74 } else if (A->f_score < B->f_score) { 75 return false; 76 } else { 77 return A->g_score < B->g_score; // If the f_costs are the same then prioritize the points that are further away from the start. 78 } 79 } 80 }; 81 82 struct Segment { 83 union { 84 struct { 85 int32_t u; 86 int32_t v; 87 }; 88 uint64_t key; 89 }; 90 91 enum { 92 NONE = 0, 93 FORWARD = 1, 94 BACKWARD = 2, 95 BIDIRECTIONAL = FORWARD | BACKWARD 96 }; 97 unsigned char direction; 98 99 bool operator<(const Segment &p_s) const { return key < p_s.key; } SegmentSegment100 Segment() { 101 key = 0; 102 direction = NONE; 103 } SegmentSegment104 Segment(int p_from, int p_to) { 105 if (p_from < p_to) { 106 u = p_from; 107 v = p_to; 108 direction = FORWARD; 109 } else { 110 u = p_to; 111 v = p_from; 112 direction = BACKWARD; 113 } 114 } 115 }; 116 117 int last_free_id; 118 uint64_t pass; 119 120 OAHashMap<int, Point *> points; 121 Set<Segment> segments; 122 123 bool _solve(Point *begin_point, Point *end_point); 124 125 protected: 126 static void _bind_methods(); 127 128 virtual real_t _estimate_cost(int p_from_id, int p_to_id); 129 virtual real_t _compute_cost(int p_from_id, int p_to_id); 130 131 public: 132 int get_available_point_id() const; 133 134 void add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale = 1); 135 Vector3 get_point_position(int p_id) const; 136 void set_point_position(int p_id, const Vector3 &p_pos); 137 real_t get_point_weight_scale(int p_id) const; 138 void set_point_weight_scale(int p_id, real_t p_weight_scale); 139 void remove_point(int p_id); 140 bool has_point(int p_id) const; 141 PoolVector<int> get_point_connections(int p_id); 142 Array get_points(); 143 144 void set_point_disabled(int p_id, bool p_disabled = true); 145 bool is_point_disabled(int p_id) const; 146 147 void connect_points(int p_id, int p_with_id, bool bidirectional = true); 148 void disconnect_points(int p_id, int p_with_id, bool bidirectional = true); 149 bool are_points_connected(int p_id, int p_with_id, bool bidirectional = true) const; 150 151 int get_point_count() const; 152 int get_point_capacity() const; 153 void reserve_space(int p_num_nodes); 154 void clear(); 155 156 int get_closest_point(const Vector3 &p_point, bool p_include_disabled = false) const; 157 Vector3 get_closest_position_in_segment(const Vector3 &p_point) const; 158 159 PoolVector<Vector3> get_point_path(int p_from_id, int p_to_id); 160 PoolVector<int> get_id_path(int p_from_id, int p_to_id); 161 162 AStar(); 163 ~AStar(); 164 }; 165 166 class AStar2D : public Reference { 167 GDCLASS(AStar2D, Reference); 168 AStar astar; 169 170 bool _solve(AStar::Point *begin_point, AStar::Point *end_point); 171 172 protected: 173 static void _bind_methods(); 174 175 virtual real_t _estimate_cost(int p_from_id, int p_to_id); 176 virtual real_t _compute_cost(int p_from_id, int p_to_id); 177 178 public: 179 int get_available_point_id() const; 180 181 void add_point(int p_id, const Vector2 &p_pos, real_t p_weight_scale = 1); 182 Vector2 get_point_position(int p_id) const; 183 void set_point_position(int p_id, const Vector2 &p_pos); 184 real_t get_point_weight_scale(int p_id) const; 185 void set_point_weight_scale(int p_id, real_t p_weight_scale); 186 void remove_point(int p_id); 187 bool has_point(int p_id) const; 188 PoolVector<int> get_point_connections(int p_id); 189 Array get_points(); 190 191 void set_point_disabled(int p_id, bool p_disabled = true); 192 bool is_point_disabled(int p_id) const; 193 194 void connect_points(int p_id, int p_with_id, bool p_bidirectional = true); 195 void disconnect_points(int p_id, int p_with_id); 196 bool are_points_connected(int p_id, int p_with_id) const; 197 198 int get_point_count() const; 199 int get_point_capacity() const; 200 void reserve_space(int p_num_nodes); 201 void clear(); 202 203 int get_closest_point(const Vector2 &p_point, bool p_include_disabled = false) const; 204 Vector2 get_closest_position_in_segment(const Vector2 &p_point) const; 205 206 PoolVector<Vector2> get_point_path(int p_from_id, int p_to_id); 207 PoolVector<int> get_id_path(int p_from_id, int p_to_id); 208 209 AStar2D(); 210 ~AStar2D(); 211 }; 212 213 #endif // ASTAR_H 214