1/**************************************************************************/
2/* polygon_path_finder.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
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#include "polygon_path_finder.h"
32#include "core/math/geometry_2d.h"
33
34bool PolygonPathFinder::_is_point_inside(const Vector2 &p_point) const {
35 int crosses = 0;
36
37 for (const Edge &E : edges) {
38 const Edge &e = E;
39
40 Vector2 a = points[e.points[0]].pos;
41 Vector2 b = points[e.points[1]].pos;
42
43 if (Geometry2D::segment_intersects_segment(a, b, p_point, outside_point, nullptr)) {
44 crosses++;
45 }
46 }
47
48 return crosses & 1;
49}
50
51void PolygonPathFinder::setup(const Vector<Vector2> &p_points, const Vector<int> &p_connections) {
52 ERR_FAIL_COND(p_connections.size() & 1);
53
54 points.clear();
55 edges.clear();
56
57 //insert points
58
59 int point_count = p_points.size();
60 points.resize(point_count + 2);
61 bounds = Rect2();
62
63 for (int i = 0; i < p_points.size(); i++) {
64 points.write[i].pos = p_points[i];
65 points.write[i].penalty = 0;
66
67 outside_point.x = i == 0 ? p_points[0].x : (MAX(p_points[i].x, outside_point.x));
68 outside_point.y = i == 0 ? p_points[0].y : (MAX(p_points[i].y, outside_point.y));
69
70 if (i == 0) {
71 bounds.position = points[i].pos;
72 } else {
73 bounds.expand_to(points[i].pos);
74 }
75 }
76
77 outside_point.x += 20.451 + Math::randf() * 10.2039;
78 outside_point.y += 21.193 + Math::randf() * 12.5412;
79
80 //insert edges (which are also connections)
81
82 for (int i = 0; i < p_connections.size(); i += 2) {
83 Edge e(p_connections[i], p_connections[i + 1]);
84 ERR_FAIL_INDEX(e.points[0], point_count);
85 ERR_FAIL_INDEX(e.points[1], point_count);
86 points.write[p_connections[i]].connections.insert(p_connections[i + 1]);
87 points.write[p_connections[i + 1]].connections.insert(p_connections[i]);
88 edges.insert(e);
89 }
90
91 //fill the remaining connections based on visibility
92
93 for (int i = 0; i < point_count; i++) {
94 for (int j = i + 1; j < point_count; j++) {
95 if (edges.has(Edge(i, j))) {
96 continue; //if in edge ignore
97 }
98
99 Vector2 from = points[i].pos;
100 Vector2 to = points[j].pos;
101
102 if (!_is_point_inside(from * 0.5 + to * 0.5)) { //connection between points in inside space
103 continue;
104 }
105
106 bool valid = true;
107
108 for (const Edge &E : edges) {
109 const Edge &e = E;
110 if (e.points[0] == i || e.points[1] == i || e.points[0] == j || e.points[1] == j) {
111 continue;
112 }
113
114 Vector2 a = points[e.points[0]].pos;
115 Vector2 b = points[e.points[1]].pos;
116
117 if (Geometry2D::segment_intersects_segment(a, b, from, to, nullptr)) {
118 valid = false;
119 break;
120 }
121 }
122
123 if (valid) {
124 points.write[i].connections.insert(j);
125 points.write[j].connections.insert(i);
126 }
127 }
128 }
129}
130
131Vector<Vector2> PolygonPathFinder::find_path(const Vector2 &p_from, const Vector2 &p_to) {
132 Vector<Vector2> path;
133
134 Vector2 from = p_from;
135 Vector2 to = p_to;
136 Edge ignore_from_edge(-1, -1);
137 Edge ignore_to_edge(-1, -1);
138
139 if (!_is_point_inside(from)) {
140 float closest_dist = 1e20f;
141 Vector2 closest_point;
142
143 for (const Edge &E : edges) {
144 const Edge &e = E;
145 Vector2 seg[2] = {
146 points[e.points[0]].pos,
147 points[e.points[1]].pos
148 };
149
150 Vector2 closest = Geometry2D::get_closest_point_to_segment(from, seg);
151 float d = from.distance_squared_to(closest);
152
153 if (d < closest_dist) {
154 ignore_from_edge = E;
155 closest_dist = d;
156 closest_point = closest;
157 }
158 }
159
160 from = closest_point;
161 };
162
163 if (!_is_point_inside(to)) {
164 float closest_dist = 1e20f;
165 Vector2 closest_point;
166
167 for (const Edge &E : edges) {
168 const Edge &e = E;
169 Vector2 seg[2] = {
170 points[e.points[0]].pos,
171 points[e.points[1]].pos
172 };
173
174 Vector2 closest = Geometry2D::get_closest_point_to_segment(to, seg);
175 float d = to.distance_squared_to(closest);
176
177 if (d < closest_dist) {
178 ignore_to_edge = E;
179 closest_dist = d;
180 closest_point = closest;
181 }
182 }
183
184 to = closest_point;
185 };
186
187 //test direct connection
188 {
189 bool can_see_eachother = true;
190
191 for (const Edge &E : edges) {
192 const Edge &e = E;
193 if (e.points[0] == ignore_from_edge.points[0] && e.points[1] == ignore_from_edge.points[1]) {
194 continue;
195 }
196 if (e.points[0] == ignore_to_edge.points[0] && e.points[1] == ignore_to_edge.points[1]) {
197 continue;
198 }
199
200 Vector2 a = points[e.points[0]].pos;
201 Vector2 b = points[e.points[1]].pos;
202
203 if (Geometry2D::segment_intersects_segment(a, b, from, to, nullptr)) {
204 can_see_eachother = false;
205 break;
206 }
207 }
208
209 if (can_see_eachother) {
210 path.push_back(from);
211 path.push_back(to);
212 return path;
213 }
214 }
215
216 //add to graph
217
218 int aidx = points.size() - 2;
219 int bidx = points.size() - 1;
220 points.write[aidx].pos = from;
221 points.write[bidx].pos = to;
222 points.write[aidx].distance = 0;
223 points.write[bidx].distance = 0;
224 points.write[aidx].prev = -1;
225 points.write[bidx].prev = -1;
226 points.write[aidx].penalty = 0;
227 points.write[bidx].penalty = 0;
228
229 for (int i = 0; i < points.size() - 2; i++) {
230 bool valid_a = true;
231 bool valid_b = true;
232 points.write[i].prev = -1;
233 points.write[i].distance = 0;
234
235 if (!_is_point_inside(from * 0.5 + points[i].pos * 0.5)) {
236 valid_a = false;
237 }
238
239 if (!_is_point_inside(to * 0.5 + points[i].pos * 0.5)) {
240 valid_b = false;
241 }
242
243 for (const Edge &E : edges) {
244 const Edge &e = E;
245
246 if (e.points[0] == i || e.points[1] == i) {
247 continue;
248 }
249
250 Vector2 a = points[e.points[0]].pos;
251 Vector2 b = points[e.points[1]].pos;
252
253 if (valid_a) {
254 if (e.points[0] != ignore_from_edge.points[1] &&
255 e.points[1] != ignore_from_edge.points[1] &&
256 e.points[0] != ignore_from_edge.points[0] &&
257 e.points[1] != ignore_from_edge.points[0]) {
258 if (Geometry2D::segment_intersects_segment(a, b, from, points[i].pos, nullptr)) {
259 valid_a = false;
260 }
261 }
262 }
263
264 if (valid_b) {
265 if (e.points[0] != ignore_to_edge.points[1] &&
266 e.points[1] != ignore_to_edge.points[1] &&
267 e.points[0] != ignore_to_edge.points[0] &&
268 e.points[1] != ignore_to_edge.points[0]) {
269 if (Geometry2D::segment_intersects_segment(a, b, to, points[i].pos, nullptr)) {
270 valid_b = false;
271 }
272 }
273 }
274
275 if (!valid_a && !valid_b) {
276 break;
277 }
278 }
279
280 if (valid_a) {
281 points.write[i].connections.insert(aidx);
282 points.write[aidx].connections.insert(i);
283 }
284
285 if (valid_b) {
286 points.write[i].connections.insert(bidx);
287 points.write[bidx].connections.insert(i);
288 }
289 }
290 //solve graph
291
292 HashSet<int> open_list;
293
294 points.write[aidx].distance = 0;
295 points.write[aidx].prev = aidx;
296 for (const int &E : points[aidx].connections) {
297 open_list.insert(E);
298 points.write[E].distance = from.distance_to(points[E].pos);
299 points.write[E].prev = aidx;
300 }
301
302 bool found_route = false;
303
304 while (true) {
305 if (open_list.size() == 0) {
306 print_verbose("Open list empty.");
307 break;
308 }
309 //check open list
310
311 int least_cost_point = -1;
312 float least_cost = 1e30;
313
314 //this could be faster (cache previous results)
315 for (const int &E : open_list) {
316 const Point &p = points[E];
317 float cost = p.distance;
318 cost += p.pos.distance_to(to);
319 cost += p.penalty;
320
321 if (cost < least_cost) {
322 least_cost_point = E;
323 least_cost = cost;
324 }
325 }
326
327 const Point &np = points[least_cost_point];
328 //open the neighbors for search
329
330 for (const int &E : np.connections) {
331 Point &p = points.write[E];
332 float distance = np.pos.distance_to(p.pos) + np.distance;
333
334 if (p.prev != -1) {
335 //oh this was visited already, can we win the cost?
336
337 if (p.distance > distance) {
338 p.prev = least_cost_point; //reassign previous
339 p.distance = distance;
340 }
341 } else {
342 //add to open neighbors
343
344 p.prev = least_cost_point;
345 p.distance = distance;
346 open_list.insert(E);
347
348 if (E == bidx) {
349 //oh my reached end! stop algorithm
350 found_route = true;
351 break;
352 }
353 }
354 }
355
356 if (found_route) {
357 break;
358 }
359
360 open_list.erase(least_cost_point);
361 }
362
363 if (found_route) {
364 int at = bidx;
365 path.push_back(points[at].pos);
366 do {
367 at = points[at].prev;
368 path.push_back(points[at].pos);
369 } while (at != aidx);
370
371 path.reverse();
372 }
373
374 for (int i = 0; i < points.size() - 2; i++) {
375 points.write[i].connections.erase(aidx);
376 points.write[i].connections.erase(bidx);
377 points.write[i].prev = -1;
378 points.write[i].distance = 0;
379 }
380
381 points.write[aidx].connections.clear();
382 points.write[aidx].prev = -1;
383 points.write[aidx].distance = 0;
384 points.write[bidx].connections.clear();
385 points.write[bidx].prev = -1;
386 points.write[bidx].distance = 0;
387
388 return path;
389}
390
391void PolygonPathFinder::_set_data(const Dictionary &p_data) {
392 ERR_FAIL_COND(!p_data.has("points"));
393 ERR_FAIL_COND(!p_data.has("connections"));
394 ERR_FAIL_COND(!p_data.has("segments"));
395 ERR_FAIL_COND(!p_data.has("bounds"));
396
397 Vector<Vector2> p = p_data["points"];
398 Array c = p_data["connections"];
399
400 ERR_FAIL_COND(c.size() != p.size());
401 if (c.size()) {
402 return;
403 }
404
405 int pc = p.size();
406 points.resize(pc + 2);
407
408 const Vector2 *pr = p.ptr();
409 for (int i = 0; i < pc; i++) {
410 points.write[i].pos = pr[i];
411 Vector<int> con = c[i];
412 const int *cr = con.ptr();
413 int cc = con.size();
414 for (int j = 0; j < cc; j++) {
415 points.write[i].connections.insert(cr[j]);
416 }
417 }
418
419 if (p_data.has("penalties")) {
420 Vector<real_t> penalties = p_data["penalties"];
421 if (penalties.size() == pc) {
422 const real_t *pr2 = penalties.ptr();
423 for (int i = 0; i < pc; i++) {
424 points.write[i].penalty = pr2[i];
425 }
426 }
427 }
428
429 Vector<int> segs = p_data["segments"];
430 int sc = segs.size();
431 ERR_FAIL_COND(sc & 1);
432 const int *sr = segs.ptr();
433 for (int i = 0; i < sc; i += 2) {
434 Edge e(sr[i], sr[i + 1]);
435 edges.insert(e);
436 }
437 bounds = p_data["bounds"];
438}
439
440Dictionary PolygonPathFinder::_get_data() const {
441 Dictionary d;
442 Vector<Vector2> p;
443 Vector<int> ind;
444 Array path_connections;
445 p.resize(MAX(0, points.size() - 2));
446 path_connections.resize(MAX(0, points.size() - 2));
447 ind.resize(edges.size() * 2);
448 Vector<real_t> penalties;
449 penalties.resize(MAX(0, points.size() - 2));
450 {
451 Vector2 *wp = p.ptrw();
452 real_t *pw = penalties.ptrw();
453
454 for (int i = 0; i < points.size() - 2; i++) {
455 wp[i] = points[i].pos;
456 pw[i] = points[i].penalty;
457 Vector<int> c;
458 c.resize(points[i].connections.size());
459 {
460 int *cw = c.ptrw();
461 int idx = 0;
462 for (const int &E : points[i].connections) {
463 cw[idx++] = E;
464 }
465 }
466 path_connections[i] = c;
467 }
468 }
469 {
470 int *iw = ind.ptrw();
471 int idx = 0;
472 for (const Edge &E : edges) {
473 iw[idx++] = E.points[0];
474 iw[idx++] = E.points[1];
475 }
476 }
477
478 d["bounds"] = bounds;
479 d["points"] = p;
480 d["penalties"] = penalties;
481 d["connections"] = path_connections;
482 d["segments"] = ind;
483
484 return d;
485}
486
487bool PolygonPathFinder::is_point_inside(const Vector2 &p_point) const {
488 return _is_point_inside(p_point);
489}
490
491Vector2 PolygonPathFinder::get_closest_point(const Vector2 &p_point) const {
492 float closest_dist = 1e20f;
493 Vector2 closest_point;
494
495 for (const Edge &E : edges) {
496 const Edge &e = E;
497 Vector2 seg[2] = {
498 points[e.points[0]].pos,
499 points[e.points[1]].pos
500 };
501
502 Vector2 closest = Geometry2D::get_closest_point_to_segment(p_point, seg);
503 float d = p_point.distance_squared_to(closest);
504
505 if (d < closest_dist) {
506 closest_dist = d;
507 closest_point = closest;
508 }
509 }
510
511 ERR_FAIL_COND_V(Math::is_equal_approx(closest_dist, 1e20f), Vector2());
512
513 return closest_point;
514}
515
516Vector<Vector2> PolygonPathFinder::get_intersections(const Vector2 &p_from, const Vector2 &p_to) const {
517 Vector<Vector2> inters;
518
519 for (const Edge &E : edges) {
520 Vector2 a = points[E.points[0]].pos;
521 Vector2 b = points[E.points[1]].pos;
522
523 Vector2 res;
524 if (Geometry2D::segment_intersects_segment(a, b, p_from, p_to, &res)) {
525 inters.push_back(res);
526 }
527 }
528
529 return inters;
530}
531
532Rect2 PolygonPathFinder::get_bounds() const {
533 return bounds;
534}
535
536void PolygonPathFinder::set_point_penalty(int p_point, float p_penalty) {
537 ERR_FAIL_INDEX(p_point, points.size() - 2);
538 points.write[p_point].penalty = p_penalty;
539}
540
541float PolygonPathFinder::get_point_penalty(int p_point) const {
542 ERR_FAIL_INDEX_V(p_point, points.size() - 2, 0);
543 return points[p_point].penalty;
544}
545
546void PolygonPathFinder::_bind_methods() {
547 ClassDB::bind_method(D_METHOD("setup", "points", "connections"), &PolygonPathFinder::setup);
548 ClassDB::bind_method(D_METHOD("find_path", "from", "to"), &PolygonPathFinder::find_path);
549 ClassDB::bind_method(D_METHOD("get_intersections", "from", "to"), &PolygonPathFinder::get_intersections);
550 ClassDB::bind_method(D_METHOD("get_closest_point", "point"), &PolygonPathFinder::get_closest_point);
551 ClassDB::bind_method(D_METHOD("is_point_inside", "point"), &PolygonPathFinder::is_point_inside);
552 ClassDB::bind_method(D_METHOD("set_point_penalty", "idx", "penalty"), &PolygonPathFinder::set_point_penalty);
553 ClassDB::bind_method(D_METHOD("get_point_penalty", "idx"), &PolygonPathFinder::get_point_penalty);
554
555 ClassDB::bind_method(D_METHOD("get_bounds"), &PolygonPathFinder::get_bounds);
556 ClassDB::bind_method(D_METHOD("_set_data", "data"), &PolygonPathFinder::_set_data);
557 ClassDB::bind_method(D_METHOD("_get_data"), &PolygonPathFinder::_get_data);
558
559 ADD_PROPERTY(PropertyInfo(Variant::DICTIONARY, "data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NO_EDITOR | PROPERTY_USAGE_INTERNAL), "_set_data", "_get_data");
560}
561
562PolygonPathFinder::PolygonPathFinder() {
563}
564