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