polygon_path_finder.cpp 16 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597
  1. /*************************************************************************/
  2. /* polygon_path_finder.cpp */
  3. /*************************************************************************/
  4. /* This file is part of: */
  5. /* GODOT ENGINE */
  6. /* https://godotengine.org */
  7. /*************************************************************************/
  8. /* Copyright (c) 2007-2019 Juan Linietsky, Ariel Manzur. */
  9. /* Copyright (c) 2014-2019 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. #include "polygon_path_finder.h"
  31. #include "core/math/geometry.h"
  32. bool PolygonPathFinder::_is_point_inside(const Vector2 &p_point) const {
  33. int crosses = 0;
  34. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  35. const Edge &e = E->get();
  36. Vector2 a = points[e.points[0]].pos;
  37. Vector2 b = points[e.points[1]].pos;
  38. if (Geometry::segment_intersects_segment_2d(a, b, p_point, outside_point, NULL)) {
  39. crosses++;
  40. }
  41. }
  42. return crosses & 1;
  43. }
  44. void PolygonPathFinder::setup(const Vector<Vector2> &p_points, const Vector<int> &p_connections) {
  45. ERR_FAIL_COND(p_connections.size() & 1);
  46. points.clear();
  47. edges.clear();
  48. //insert points
  49. int point_count = p_points.size();
  50. points.resize(point_count + 2);
  51. bounds = Rect2();
  52. for (int i = 0; i < p_points.size(); i++) {
  53. points.write[i].pos = p_points[i];
  54. points.write[i].penalty = 0;
  55. outside_point.x = i == 0 ? p_points[0].x : (MAX(p_points[i].x, outside_point.x));
  56. outside_point.y = i == 0 ? p_points[0].y : (MAX(p_points[i].y, outside_point.y));
  57. if (i == 0) {
  58. bounds.position = points[i].pos;
  59. } else {
  60. bounds.expand_to(points[i].pos);
  61. }
  62. }
  63. outside_point.x += 20.451 + Math::randf() * 10.2039;
  64. outside_point.y += 21.193 + Math::randf() * 12.5412;
  65. //insert edges (which are also connetions)
  66. for (int i = 0; i < p_connections.size(); i += 2) {
  67. Edge e(p_connections[i], p_connections[i + 1]);
  68. ERR_FAIL_INDEX(e.points[0], point_count);
  69. ERR_FAIL_INDEX(e.points[1], point_count);
  70. points.write[p_connections[i]].connections.insert(p_connections[i + 1]);
  71. points.write[p_connections[i + 1]].connections.insert(p_connections[i]);
  72. edges.insert(e);
  73. }
  74. //fill the remaining connections based on visibility
  75. for (int i = 0; i < point_count; i++) {
  76. for (int j = i + 1; j < point_count; j++) {
  77. if (edges.has(Edge(i, j)))
  78. continue; //if in edge ignore
  79. Vector2 from = points[i].pos;
  80. Vector2 to = points[j].pos;
  81. if (!_is_point_inside(from * 0.5 + to * 0.5)) //connection between points in inside space
  82. continue;
  83. bool valid = true;
  84. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  85. const Edge &e = E->get();
  86. if (e.points[0] == i || e.points[1] == i || e.points[0] == j || e.points[1] == j)
  87. continue;
  88. Vector2 a = points[e.points[0]].pos;
  89. Vector2 b = points[e.points[1]].pos;
  90. if (Geometry::segment_intersects_segment_2d(a, b, from, to, NULL)) {
  91. valid = false;
  92. break;
  93. }
  94. }
  95. if (valid) {
  96. points.write[i].connections.insert(j);
  97. points.write[j].connections.insert(i);
  98. }
  99. }
  100. }
  101. }
  102. Vector<Vector2> PolygonPathFinder::find_path(const Vector2 &p_from, const Vector2 &p_to) {
  103. Vector<Vector2> path;
  104. Vector2 from = p_from;
  105. Vector2 to = p_to;
  106. Edge ignore_from_edge(-1, -1);
  107. Edge ignore_to_edge(-1, -1);
  108. if (!_is_point_inside(from)) {
  109. float closest_dist = 1e20;
  110. Vector2 closest_point;
  111. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  112. const Edge &e = E->get();
  113. Vector2 seg[2] = {
  114. points[e.points[0]].pos,
  115. points[e.points[1]].pos
  116. };
  117. Vector2 closest = Geometry::get_closest_point_to_segment_2d(from, seg);
  118. float d = from.distance_squared_to(closest);
  119. if (d < closest_dist) {
  120. ignore_from_edge = E->get();
  121. closest_dist = d;
  122. closest_point = closest;
  123. }
  124. }
  125. from = closest_point;
  126. };
  127. if (!_is_point_inside(to)) {
  128. float closest_dist = 1e20;
  129. Vector2 closest_point;
  130. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  131. const Edge &e = E->get();
  132. Vector2 seg[2] = {
  133. points[e.points[0]].pos,
  134. points[e.points[1]].pos
  135. };
  136. Vector2 closest = Geometry::get_closest_point_to_segment_2d(to, seg);
  137. float d = to.distance_squared_to(closest);
  138. if (d < closest_dist) {
  139. ignore_to_edge = E->get();
  140. closest_dist = d;
  141. closest_point = closest;
  142. }
  143. }
  144. to = closest_point;
  145. };
  146. //test direct connection
  147. {
  148. bool can_see_eachother = true;
  149. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  150. const Edge &e = E->get();
  151. if (e.points[0] == ignore_from_edge.points[0] && e.points[1] == ignore_from_edge.points[1])
  152. continue;
  153. if (e.points[0] == ignore_to_edge.points[0] && e.points[1] == ignore_to_edge.points[1])
  154. continue;
  155. Vector2 a = points[e.points[0]].pos;
  156. Vector2 b = points[e.points[1]].pos;
  157. if (Geometry::segment_intersects_segment_2d(a, b, from, to, NULL)) {
  158. can_see_eachother = false;
  159. break;
  160. }
  161. }
  162. if (can_see_eachother) {
  163. path.push_back(from);
  164. path.push_back(to);
  165. return path;
  166. }
  167. }
  168. //add to graph
  169. int aidx = points.size() - 2;
  170. int bidx = points.size() - 1;
  171. points.write[aidx].pos = from;
  172. points.write[bidx].pos = to;
  173. points.write[aidx].distance = 0;
  174. points.write[bidx].distance = 0;
  175. points.write[aidx].prev = -1;
  176. points.write[bidx].prev = -1;
  177. points.write[aidx].penalty = 0;
  178. points.write[bidx].penalty = 0;
  179. for (int i = 0; i < points.size() - 2; i++) {
  180. bool valid_a = true;
  181. bool valid_b = true;
  182. points.write[i].prev = -1;
  183. points.write[i].distance = 0;
  184. if (!_is_point_inside(from * 0.5 + points[i].pos * 0.5)) {
  185. valid_a = false;
  186. }
  187. if (!_is_point_inside(to * 0.5 + points[i].pos * 0.5)) {
  188. valid_b = false;
  189. }
  190. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  191. const Edge &e = E->get();
  192. if (e.points[0] == i || e.points[1] == i)
  193. continue;
  194. Vector2 a = points[e.points[0]].pos;
  195. Vector2 b = points[e.points[1]].pos;
  196. if (valid_a) {
  197. if (e.points[0] != ignore_from_edge.points[1] &&
  198. e.points[1] != ignore_from_edge.points[1] &&
  199. e.points[0] != ignore_from_edge.points[0] &&
  200. e.points[1] != ignore_from_edge.points[0]) {
  201. if (Geometry::segment_intersects_segment_2d(a, b, from, points[i].pos, NULL)) {
  202. valid_a = false;
  203. }
  204. }
  205. }
  206. if (valid_b) {
  207. if (e.points[0] != ignore_to_edge.points[1] &&
  208. e.points[1] != ignore_to_edge.points[1] &&
  209. e.points[0] != ignore_to_edge.points[0] &&
  210. e.points[1] != ignore_to_edge.points[0]) {
  211. if (Geometry::segment_intersects_segment_2d(a, b, to, points[i].pos, NULL)) {
  212. valid_b = false;
  213. }
  214. }
  215. }
  216. if (!valid_a && !valid_b)
  217. break;
  218. }
  219. if (valid_a) {
  220. points.write[i].connections.insert(aidx);
  221. points.write[aidx].connections.insert(i);
  222. }
  223. if (valid_b) {
  224. points.write[i].connections.insert(bidx);
  225. points.write[bidx].connections.insert(i);
  226. }
  227. }
  228. //solve graph
  229. Set<int> open_list;
  230. points.write[aidx].distance = 0;
  231. points.write[aidx].prev = aidx;
  232. for (Set<int>::Element *E = points[aidx].connections.front(); E; E = E->next()) {
  233. open_list.insert(E->get());
  234. points.write[E->get()].distance = from.distance_to(points[E->get()].pos);
  235. points.write[E->get()].prev = aidx;
  236. }
  237. bool found_route = false;
  238. while (true) {
  239. if (open_list.size() == 0) {
  240. printf("open list empty\n");
  241. break;
  242. }
  243. //check open list
  244. int least_cost_point = -1;
  245. float least_cost = 1e30;
  246. //this could be faster (cache previous results)
  247. for (Set<int>::Element *E = open_list.front(); E; E = E->next()) {
  248. const Point &p = points[E->get()];
  249. float cost = p.distance;
  250. cost += p.pos.distance_to(to);
  251. cost += p.penalty;
  252. if (cost < least_cost) {
  253. least_cost_point = E->get();
  254. least_cost = cost;
  255. }
  256. }
  257. const Point &np = points[least_cost_point];
  258. //open the neighbours for search
  259. for (Set<int>::Element *E = np.connections.front(); E; E = E->next()) {
  260. Point &p = points.write[E->get()];
  261. float distance = np.pos.distance_to(p.pos) + np.distance;
  262. if (p.prev != -1) {
  263. //oh this was visited already, can we win the cost?
  264. if (p.distance > distance) {
  265. p.prev = least_cost_point; //reasign previous
  266. p.distance = distance;
  267. }
  268. } else {
  269. //add to open neighbours
  270. p.prev = least_cost_point;
  271. p.distance = distance;
  272. open_list.insert(E->get());
  273. if (E->get() == bidx) {
  274. //oh my reached end! stop algorithm
  275. found_route = true;
  276. break;
  277. }
  278. }
  279. }
  280. if (found_route)
  281. break;
  282. open_list.erase(least_cost_point);
  283. }
  284. if (found_route) {
  285. int at = bidx;
  286. path.push_back(points[at].pos);
  287. do {
  288. at = points[at].prev;
  289. path.push_back(points[at].pos);
  290. } while (at != aidx);
  291. path.invert();
  292. }
  293. for (int i = 0; i < points.size() - 2; i++) {
  294. points.write[i].connections.erase(aidx);
  295. points.write[i].connections.erase(bidx);
  296. points.write[i].prev = -1;
  297. points.write[i].distance = 0;
  298. }
  299. points.write[aidx].connections.clear();
  300. points.write[aidx].prev = -1;
  301. points.write[aidx].distance = 0;
  302. points.write[bidx].connections.clear();
  303. points.write[bidx].prev = -1;
  304. points.write[bidx].distance = 0;
  305. return path;
  306. }
  307. void PolygonPathFinder::_set_data(const Dictionary &p_data) {
  308. ERR_FAIL_COND(!p_data.has("points"));
  309. ERR_FAIL_COND(!p_data.has("connections"));
  310. ERR_FAIL_COND(!p_data.has("segments"));
  311. ERR_FAIL_COND(!p_data.has("bounds"));
  312. PoolVector<Vector2> p = p_data["points"];
  313. Array c = p_data["connections"];
  314. ERR_FAIL_COND(c.size() != p.size());
  315. if (c.size())
  316. return;
  317. int pc = p.size();
  318. points.resize(pc + 2);
  319. PoolVector<Vector2>::Read pr = p.read();
  320. for (int i = 0; i < pc; i++) {
  321. points.write[i].pos = pr[i];
  322. PoolVector<int> con = c[i];
  323. PoolVector<int>::Read cr = con.read();
  324. int cc = con.size();
  325. for (int j = 0; j < cc; j++) {
  326. points.write[i].connections.insert(cr[j]);
  327. }
  328. }
  329. if (p_data.has("penalties")) {
  330. PoolVector<float> penalties = p_data["penalties"];
  331. if (penalties.size() == pc) {
  332. PoolVector<float>::Read pr = penalties.read();
  333. for (int i = 0; i < pc; i++) {
  334. points.write[i].penalty = pr[i];
  335. }
  336. }
  337. }
  338. PoolVector<int> segs = p_data["segments"];
  339. int sc = segs.size();
  340. ERR_FAIL_COND(sc & 1);
  341. PoolVector<int>::Read sr = segs.read();
  342. for (int i = 0; i < sc; i += 2) {
  343. Edge e(sr[i], sr[i + 1]);
  344. edges.insert(e);
  345. }
  346. bounds = p_data["bounds"];
  347. }
  348. Dictionary PolygonPathFinder::_get_data() const {
  349. Dictionary d;
  350. PoolVector<Vector2> p;
  351. PoolVector<int> ind;
  352. Array connections;
  353. p.resize(points.size() - 2);
  354. connections.resize(points.size() - 2);
  355. ind.resize(edges.size() * 2);
  356. PoolVector<float> penalties;
  357. penalties.resize(points.size() - 2);
  358. {
  359. PoolVector<Vector2>::Write wp = p.write();
  360. PoolVector<float>::Write pw = penalties.write();
  361. for (int i = 0; i < points.size() - 2; i++) {
  362. wp[i] = points[i].pos;
  363. pw[i] = points[i].penalty;
  364. PoolVector<int> c;
  365. c.resize(points[i].connections.size());
  366. {
  367. PoolVector<int>::Write cw = c.write();
  368. int idx = 0;
  369. for (Set<int>::Element *E = points[i].connections.front(); E; E = E->next()) {
  370. cw[idx++] = E->get();
  371. }
  372. }
  373. connections[i] = c;
  374. }
  375. }
  376. {
  377. PoolVector<int>::Write iw = ind.write();
  378. int idx = 0;
  379. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  380. iw[idx++] = E->get().points[0];
  381. iw[idx++] = E->get().points[1];
  382. }
  383. }
  384. d["bounds"] = bounds;
  385. d["points"] = p;
  386. d["penalties"] = penalties;
  387. d["connections"] = connections;
  388. d["segments"] = ind;
  389. return d;
  390. }
  391. bool PolygonPathFinder::is_point_inside(const Vector2 &p_point) const {
  392. return _is_point_inside(p_point);
  393. }
  394. Vector2 PolygonPathFinder::get_closest_point(const Vector2 &p_point) const {
  395. float closest_dist = 1e20;
  396. Vector2 closest_point;
  397. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  398. const Edge &e = E->get();
  399. Vector2 seg[2] = {
  400. points[e.points[0]].pos,
  401. points[e.points[1]].pos
  402. };
  403. Vector2 closest = Geometry::get_closest_point_to_segment_2d(p_point, seg);
  404. float d = p_point.distance_squared_to(closest);
  405. if (d < closest_dist) {
  406. closest_dist = d;
  407. closest_point = closest;
  408. }
  409. }
  410. ERR_FAIL_COND_V(closest_dist == 1e20, Vector2());
  411. return closest_point;
  412. }
  413. Vector<Vector2> PolygonPathFinder::get_intersections(const Vector2 &p_from, const Vector2 &p_to) const {
  414. Vector<Vector2> inters;
  415. for (Set<Edge>::Element *E = edges.front(); E; E = E->next()) {
  416. Vector2 a = points[E->get().points[0]].pos;
  417. Vector2 b = points[E->get().points[1]].pos;
  418. Vector2 res;
  419. if (Geometry::segment_intersects_segment_2d(a, b, p_from, p_to, &res)) {
  420. inters.push_back(res);
  421. }
  422. }
  423. return inters;
  424. }
  425. Rect2 PolygonPathFinder::get_bounds() const {
  426. return bounds;
  427. }
  428. void PolygonPathFinder::set_point_penalty(int p_point, float p_penalty) {
  429. ERR_FAIL_INDEX(p_point, points.size() - 2);
  430. points.write[p_point].penalty = p_penalty;
  431. }
  432. float PolygonPathFinder::get_point_penalty(int p_point) const {
  433. ERR_FAIL_INDEX_V(p_point, points.size() - 2, 0);
  434. return points[p_point].penalty;
  435. }
  436. void PolygonPathFinder::_bind_methods() {
  437. ClassDB::bind_method(D_METHOD("setup", "points", "connections"), &PolygonPathFinder::setup);
  438. ClassDB::bind_method(D_METHOD("find_path", "from", "to"), &PolygonPathFinder::find_path);
  439. ClassDB::bind_method(D_METHOD("get_intersections", "from", "to"), &PolygonPathFinder::get_intersections);
  440. ClassDB::bind_method(D_METHOD("get_closest_point", "point"), &PolygonPathFinder::get_closest_point);
  441. ClassDB::bind_method(D_METHOD("is_point_inside", "point"), &PolygonPathFinder::is_point_inside);
  442. ClassDB::bind_method(D_METHOD("set_point_penalty", "idx", "penalty"), &PolygonPathFinder::set_point_penalty);
  443. ClassDB::bind_method(D_METHOD("get_point_penalty", "idx"), &PolygonPathFinder::get_point_penalty);
  444. ClassDB::bind_method(D_METHOD("get_bounds"), &PolygonPathFinder::get_bounds);
  445. ClassDB::bind_method(D_METHOD("_set_data"), &PolygonPathFinder::_set_data);
  446. ClassDB::bind_method(D_METHOD("_get_data"), &PolygonPathFinder::_get_data);
  447. ADD_PROPERTY(PropertyInfo(Variant::DICTIONARY, "data", PROPERTY_HINT_NONE, "", PROPERTY_USAGE_NOEDITOR | PROPERTY_USAGE_INTERNAL), "_set_data", "_get_data");
  448. }
  449. PolygonPathFinder::PolygonPathFinder() {
  450. }