1 /*************************************************************************/
2 /* navigation2d.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 "navigation2d.h"
31
32 #define USE_ENTRY_POINT
33
_navpoly_link(int p_id)34 void Navigation2D::_navpoly_link(int p_id) {
35
36 ERR_FAIL_COND(!navpoly_map.has(p_id));
37 NavMesh &nm = navpoly_map[p_id];
38 ERR_FAIL_COND(nm.linked);
39
40 DVector<Vector2> vertices = nm.navpoly->get_vertices();
41 int len = vertices.size();
42 if (len == 0)
43 return;
44
45 DVector<Vector2>::Read r = vertices.read();
46
47 for (int i = 0; i < nm.navpoly->get_polygon_count(); i++) {
48
49 //build
50
51 List<Polygon>::Element *P = nm.polygons.push_back(Polygon());
52 Polygon &p = P->get();
53 p.owner = &nm;
54
55 Vector<int> poly = nm.navpoly->get_polygon(i);
56 int plen = poly.size();
57 const int *indices = poly.ptr();
58 bool valid = true;
59 p.edges.resize(plen);
60
61 Vector2 center;
62 float sum = 0;
63
64 for (int j = 0; j < plen; j++) {
65
66 int idx = indices[j];
67 if (idx < 0 || idx >= len) {
68 valid = false;
69 break;
70 }
71
72 Polygon::Edge e;
73 Vector2 ep = nm.xform.xform(r[idx]);
74 center += ep;
75 e.point = _get_point(ep);
76 p.edges[j] = e;
77
78 int idxn = indices[(j + 1) % plen];
79 if (idxn < 0 || idxn >= len) {
80 valid = false;
81 break;
82 }
83
84 Vector2 epn = nm.xform.xform(r[idxn]);
85
86 sum += (epn.x - ep.x) * (epn.y + ep.y);
87 }
88
89 p.clockwise = sum > 0;
90
91 if (!valid) {
92 nm.polygons.pop_back();
93 ERR_CONTINUE(!valid);
94 continue;
95 }
96
97 p.center = center / plen;
98
99 //connect
100
101 for (int j = 0; j < plen; j++) {
102
103 int next = (j + 1) % plen;
104 EdgeKey ek(p.edges[j].point, p.edges[next].point);
105
106 Map<EdgeKey, Connection>::Element *C = connections.find(ek);
107 if (!C) {
108
109 Connection c;
110 c.A = &p;
111 c.A_edge = j;
112 c.B = NULL;
113 c.B_edge = -1;
114 connections[ek] = c;
115 } else {
116
117 if (C->get().B != NULL) {
118 ConnectionPending pending;
119 pending.polygon = &p;
120 pending.edge = j;
121 p.edges[j].P = C->get().pending.push_back(pending);
122 continue;
123 //print_line(String()+_get_vertex(ek.a)+" -> "+_get_vertex(ek.b));
124 }
125
126 C->get().B = &p;
127 C->get().B_edge = j;
128 C->get().A->edges[C->get().A_edge].C = &p;
129 C->get().A->edges[C->get().A_edge].C_edge = j;
130 p.edges[j].C = C->get().A;
131 p.edges[j].C_edge = C->get().A_edge;
132 //connection successful.
133 }
134 }
135 }
136
137 nm.linked = true;
138 }
139
_navpoly_unlink(int p_id)140 void Navigation2D::_navpoly_unlink(int p_id) {
141
142 ERR_FAIL_COND(!navpoly_map.has(p_id));
143 NavMesh &nm = navpoly_map[p_id];
144 ERR_FAIL_COND(!nm.linked);
145
146 //print_line("UNLINK");
147
148 for (List<Polygon>::Element *E = nm.polygons.front(); E; E = E->next()) {
149
150 Polygon &p = E->get();
151
152 int ec = p.edges.size();
153 Polygon::Edge *edges = p.edges.ptr();
154
155 for (int i = 0; i < ec; i++) {
156 int next = (i + 1) % ec;
157
158 EdgeKey ek(edges[i].point, edges[next].point);
159 Map<EdgeKey, Connection>::Element *C = connections.find(ek);
160 ERR_CONTINUE(!C);
161
162 if (edges[i].P) {
163 C->get().pending.erase(edges[i].P);
164 edges[i].P = NULL;
165
166 } else if (C->get().B) {
167 //disconnect
168
169 C->get().B->edges[C->get().B_edge].C = NULL;
170 C->get().B->edges[C->get().B_edge].C_edge = -1;
171 C->get().A->edges[C->get().A_edge].C = NULL;
172 C->get().A->edges[C->get().A_edge].C_edge = -1;
173
174 if (C->get().A == &E->get()) {
175
176 C->get().A = C->get().B;
177 C->get().A_edge = C->get().B_edge;
178 }
179 C->get().B = NULL;
180 C->get().B_edge = -1;
181
182 if (C->get().pending.size()) {
183 //reconnect if something is pending
184 ConnectionPending cp = C->get().pending.front()->get();
185 C->get().pending.pop_front();
186
187 C->get().B = cp.polygon;
188 C->get().B_edge = cp.edge;
189 C->get().A->edges[C->get().A_edge].C = cp.polygon;
190 C->get().A->edges[C->get().A_edge].C_edge = cp.edge;
191 cp.polygon->edges[cp.edge].C = C->get().A;
192 cp.polygon->edges[cp.edge].C_edge = C->get().A_edge;
193 cp.polygon->edges[cp.edge].P = NULL;
194 }
195
196 } else {
197 connections.erase(C);
198 //erase
199 }
200 }
201 }
202
203 nm.polygons.clear();
204
205 nm.linked = false;
206 }
207
navpoly_create(const Ref<NavigationPolygon> & p_mesh,const Matrix32 & p_xform,Object * p_owner)208 int Navigation2D::navpoly_create(const Ref<NavigationPolygon> &p_mesh, const Matrix32 &p_xform, Object *p_owner) {
209
210 int id = last_id++;
211 NavMesh nm;
212 nm.linked = false;
213 nm.navpoly = p_mesh;
214 nm.xform = p_xform;
215 nm.owner = p_owner;
216 navpoly_map[id] = nm;
217
218 _navpoly_link(id);
219
220 return id;
221 }
222
navpoly_set_transform(int p_id,const Matrix32 & p_xform)223 void Navigation2D::navpoly_set_transform(int p_id, const Matrix32 &p_xform) {
224
225 ERR_FAIL_COND(!navpoly_map.has(p_id));
226 NavMesh &nm = navpoly_map[p_id];
227 if (nm.xform == p_xform)
228 return; //bleh
229 _navpoly_unlink(p_id);
230 nm.xform = p_xform;
231 _navpoly_link(p_id);
232 }
navpoly_remove(int p_id)233 void Navigation2D::navpoly_remove(int p_id) {
234
235 ERR_FAIL_COND(!navpoly_map.has(p_id));
236 _navpoly_unlink(p_id);
237 navpoly_map.erase(p_id);
238 }
239 #if 0
240 void Navigation2D::_clip_path(Vector<Vector2>& path, Polygon *from_poly, const Vector2& p_to_point, Polygon* p_to_poly) {
241
242 Vector2 from = path[path.size()-1];
243
244 if (from.distance_to(p_to_point)<CMP_EPSILON)
245 return;
246 Plane cut_plane;
247 cut_plane.normal = (from-p_to_point).cross(up);
248 if (cut_plane.normal==Vector2())
249 return;
250 cut_plane.normal.normalize();
251 cut_plane.d = cut_plane.normal.dot(from);
252
253
254 while(from_poly!=p_to_poly) {
255
256 int pe = from_poly->prev_edge;
257 Vector2 a = _get_vertex(from_poly->edges[pe].point);
258 Vector2 b = _get_vertex(from_poly->edges[(pe+1)%from_poly->edges.size()].point);
259
260 from_poly=from_poly->edges[pe].C;
261 ERR_FAIL_COND(!from_poly);
262
263 if (a.distance_to(b)>CMP_EPSILON) {
264
265 Vector2 inters;
266 if (cut_plane.intersects_segment(a,b,&inters)) {
267 if (inters.distance_to(p_to_point)>CMP_EPSILON && inters.distance_to(path[path.size()-1])>CMP_EPSILON) {
268 path.push_back(inters);
269 }
270 }
271 }
272 }
273 }
274 #endif
275
get_simple_path(const Vector2 & p_start,const Vector2 & p_end,bool p_optimize)276 Vector<Vector2> Navigation2D::get_simple_path(const Vector2 &p_start, const Vector2 &p_end, bool p_optimize) {
277
278 Polygon *begin_poly = NULL;
279 Polygon *end_poly = NULL;
280 Vector2 begin_point;
281 Vector2 end_point;
282 float begin_d = 1e20;
283 float end_d = 1e20;
284
285 //look for point inside triangle
286
287 for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
288
289 if (!E->get().linked)
290 continue;
291 for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
292
293 Polygon &p = F->get();
294 if (begin_d || end_d) {
295 for (int i = 2; i < p.edges.size(); i++) {
296
297 if (begin_d > 0) {
298
299 if (Geometry::is_point_in_triangle(p_start, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
300
301 begin_poly = &p;
302 begin_point = p_start;
303 begin_d = 0;
304 if (end_d == 0)
305 break;
306 }
307 }
308
309 if (end_d > 0) {
310
311 if (Geometry::is_point_in_triangle(p_end, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
312
313 end_poly = &p;
314 end_point = p_end;
315 end_d = 0;
316 if (begin_d == 0)
317 break;
318 }
319 }
320 }
321 }
322
323 p.prev_edge = -1;
324 }
325 }
326
327 //start or end not inside triangle.. look for closest segment :|
328 if (begin_d || end_d) {
329 for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
330
331 if (!E->get().linked)
332 continue;
333 for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
334
335 Polygon &p = F->get();
336 int es = p.edges.size();
337 for (int i = 0; i < es; i++) {
338
339 Vector2 edge[2] = {
340 _get_vertex(p.edges[i].point),
341 _get_vertex(p.edges[(i + 1) % es].point)
342 };
343
344 if (begin_d > 0) {
345 Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_start, edge);
346 float d = spoint.distance_to(p_start);
347 if (d < begin_d) {
348 begin_poly = &p;
349 begin_point = spoint;
350 begin_d = d;
351 }
352 }
353
354 if (end_d > 0) {
355 Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_end, edge);
356 float d = spoint.distance_to(p_end);
357 if (d < end_d) {
358 end_poly = &p;
359 end_point = spoint;
360 end_d = d;
361 }
362 }
363 }
364 }
365 }
366 }
367
368 if (!begin_poly || !end_poly) {
369
370 return Vector<Vector2>(); //no path
371 }
372
373 if (begin_poly == end_poly) {
374
375 Vector<Vector2> path;
376 path.resize(2);
377 path[0] = begin_point;
378 path[1] = end_point;
379 //print_line("Direct Path");
380 return path;
381 }
382
383 bool found_route = false;
384
385 List<Polygon *> open_list;
386
387 begin_poly->entry = p_start;
388
389 for (int i = 0; i < begin_poly->edges.size(); i++) {
390
391 if (begin_poly->edges[i].C) {
392
393 begin_poly->edges[i].C->prev_edge = begin_poly->edges[i].C_edge;
394 #ifdef USE_ENTRY_POINT
395 Vector2 edge[2] = {
396 _get_vertex(begin_poly->edges[i].point),
397 _get_vertex(begin_poly->edges[(i + 1) % begin_poly->edges.size()].point)
398 };
399
400 Vector2 entry = Geometry::get_closest_point_to_segment_2d(begin_poly->entry, edge);
401 begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
402 begin_poly->edges[i].C->entry = entry;
403 #else
404 begin_poly->edges[i].C->distance = begin_poly->center.distance_to(begin_poly->edges[i].C->center);
405 #endif
406 open_list.push_back(begin_poly->edges[i].C);
407
408 if (begin_poly->edges[i].C == end_poly) {
409 found_route = true;
410 }
411 }
412 }
413
414 while (!found_route) {
415
416 if (open_list.size() == 0) {
417 // print_line("NOU OPEN LIST");
418 break;
419 }
420 //check open list
421
422 List<Polygon *>::Element *least_cost_poly = NULL;
423 float least_cost = 1e30;
424
425 //this could be faster (cache previous results)
426 for (List<Polygon *>::Element *E = open_list.front(); E; E = E->next()) {
427
428 Polygon *p = E->get();
429
430 float cost = p->distance;
431 cost += p->center.distance_to(end_point);
432
433 if (cost < least_cost) {
434
435 least_cost_poly = E;
436 least_cost = cost;
437 }
438 }
439
440 Polygon *p = least_cost_poly->get();
441 //open the neighbours for search
442 int es = p->edges.size();
443
444 for (int i = 0; i < es; i++) {
445
446 Polygon::Edge &e = p->edges[i];
447
448 if (!e.C)
449 continue;
450
451 #ifdef USE_ENTRY_POINT
452 Vector2 edge[2] = {
453 _get_vertex(p->edges[i].point),
454 _get_vertex(p->edges[(i + 1) % es].point)
455 };
456
457 Vector2 edge_entry = Geometry::get_closest_point_to_segment_2d(p->entry, edge);
458 float distance = p->entry.distance_to(edge_entry) + p->distance;
459
460 #else
461
462 float distance = p->center.distance_to(e.C->center) + p->distance;
463
464 #endif
465
466 if (e.C->prev_edge != -1) {
467 //oh this was visited already, can we win the cost?
468
469 if (e.C->distance > distance) {
470
471 e.C->prev_edge = e.C_edge;
472 e.C->distance = distance;
473 #ifdef USE_ENTRY_POINT
474 e.C->entry = edge_entry;
475 #endif
476 }
477 } else {
478 //add to open neighbours
479
480 e.C->prev_edge = e.C_edge;
481 e.C->distance = distance;
482 #ifdef USE_ENTRY_POINT
483 e.C->entry = edge_entry;
484 #endif
485
486 open_list.push_back(e.C);
487
488 if (e.C == end_poly) {
489 //oh my reached end! stop algorithm
490 found_route = true;
491 break;
492 }
493 }
494 }
495
496 if (found_route)
497 break;
498
499 open_list.erase(least_cost_poly);
500 }
501 #if 0
502 debug path
503 {
504 Polygon *p=end_poly;
505 int idx=0;
506
507 while(true) {
508 int prev = p->prev_edge;
509 int prev_n = (p->prev_edge+1)%p->edges.size();
510 Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point))*0.5;
511 String points;
512 for(int i=0;i<p->edges.size();i++) {
513 if (i>0)
514 points+=", ";
515 points+=_get_vertex(p->edges[i].point);
516 }
517 //print_line("poly "+itos(idx++)+" - "+points);
518 p = p->edges[prev].C;
519 if (p==begin_poly)
520 break;
521 }
522 }
523 #endif
524 if (found_route) {
525
526 Vector<Vector2> path;
527
528 if (p_optimize) {
529 //string pulling
530
531 Vector2 apex_point = end_point;
532 Vector2 portal_left = apex_point;
533 Vector2 portal_right = apex_point;
534 Polygon *left_poly = end_poly;
535 Polygon *right_poly = end_poly;
536 Polygon *p = end_poly;
537 path.push_back(end_point);
538
539 while (p) {
540
541 Vector2 left;
542 Vector2 right;
543
544 //#define CLOCK_TANGENT(m_a,m_b,m_c) ( ((m_a)-(m_c)).cross((m_a)-(m_b)) )
545 #define CLOCK_TANGENT(m_a, m_b, m_c) ((((m_a).x - (m_c).x) * ((m_b).y - (m_c).y) - ((m_b).x - (m_c).x) * ((m_a).y - (m_c).y)))
546
547 if (p == begin_poly) {
548 left = begin_point;
549 right = begin_point;
550 } else {
551 int prev = p->prev_edge;
552 int prev_n = (p->prev_edge + 1) % p->edges.size();
553 left = _get_vertex(p->edges[prev].point);
554 right = _get_vertex(p->edges[prev_n].point);
555
556 if (p->clockwise) {
557 SWAP(left, right);
558 }
559 /*if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5) < 0){
560 SWAP(left,right);
561 }*/
562 }
563
564 bool skip = false;
565
566 /*
567 print_line("-----\nAPEX: "+(apex_point-end_point));
568 print_line("LEFT:");
569 print_line("\tPortal: "+(portal_left-end_point));
570 print_line("\tPoint: "+(left-end_point));
571 print_line("\tLeft Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_left,left)));
572 print_line("\tLeft Distance: "+rtos(portal_left.distance_squared_to(apex_point)));
573 print_line("\tLeft Test: "+rtos(CLOCK_TANGENT(apex_point,left,portal_right)));
574 print_line("RIGHT:");
575 print_line("\tPortal: "+(portal_right-end_point));
576 print_line("\tPoint: "+(right-end_point));
577 print_line("\tRight Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_right,right)));
578 print_line("\tRight Distance: "+rtos(portal_right.distance_squared_to(apex_point)));
579 print_line("\tRight Test: "+rtos(CLOCK_TANGENT(apex_point,right,portal_left)));
580 */
581
582 if (CLOCK_TANGENT(apex_point, portal_left, left) >= 0) {
583 //process
584 if (portal_left.distance_squared_to(apex_point) < CMP_EPSILON || CLOCK_TANGENT(apex_point, left, portal_right) > 0) {
585 left_poly = p;
586 portal_left = left;
587 //print_line("***ADVANCE LEFT");
588 } else {
589
590 apex_point = portal_right;
591 p = right_poly;
592 left_poly = p;
593 portal_left = apex_point;
594 portal_right = apex_point;
595 if (path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
596 path.push_back(apex_point);
597 skip = true;
598 //print_line("addpoint left");
599 //print_line("***CLIP LEFT");
600 }
601 }
602
603 if (!skip && CLOCK_TANGENT(apex_point, portal_right, right) <= 0) {
604 //process
605 if (portal_right.distance_squared_to(apex_point) < CMP_EPSILON || CLOCK_TANGENT(apex_point, right, portal_left) < 0) {
606 right_poly = p;
607 portal_right = right;
608 //print_line("***ADVANCE RIGHT");
609 } else {
610
611 apex_point = portal_left;
612 p = left_poly;
613 right_poly = p;
614 portal_right = apex_point;
615 portal_left = apex_point;
616 if (path[path.size() - 1].distance_to(apex_point) > CMP_EPSILON)
617 path.push_back(apex_point);
618 //print_line("addpoint right");
619 //print_line("***CLIP RIGHT");
620 }
621 }
622
623 if (p != begin_poly)
624 p = p->edges[p->prev_edge].C;
625 else
626 p = NULL;
627 }
628
629 if (path[path.size() - 1].distance_to(begin_point) > CMP_EPSILON)
630 path.push_back(begin_point);
631
632 path.invert();
633
634 } else {
635 //midpoints
636 Polygon *p = end_poly;
637
638 path.push_back(end_point);
639 while (true) {
640 int prev = p->prev_edge;
641 int prev_n = (p->prev_edge + 1) % p->edges.size();
642 Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point)) * 0.5;
643 path.push_back(point);
644 p = p->edges[prev].C;
645 if (p == begin_poly)
646 break;
647 }
648
649 if (path[path.size() - 1].distance_to(begin_point) > CMP_EPSILON)
650 path.push_back(begin_point);
651
652 path.invert();
653 }
654
655 return path;
656 }
657
658 return Vector<Vector2>();
659 }
660
get_closest_point(const Vector2 & p_point)661 Vector2 Navigation2D::get_closest_point(const Vector2 &p_point) {
662
663 Vector2 closest_point = Vector2();
664 float closest_point_d = 1e20;
665
666 for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
667
668 if (!E->get().linked)
669 continue;
670 for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
671
672 Polygon &p = F->get();
673 for (int i = 2; i < p.edges.size(); i++) {
674
675 if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
676
677 return p_point; //inside triangle, nothing else to discuss
678 }
679 }
680 }
681 }
682
683 for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
684
685 if (!E->get().linked)
686 continue;
687 for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
688
689 Polygon &p = F->get();
690 int es = p.edges.size();
691 for (int i = 0; i < es; i++) {
692
693 Vector2 edge[2] = {
694 _get_vertex(p.edges[i].point),
695 _get_vertex(p.edges[(i + 1) % es].point)
696 };
697
698 Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
699 float d = spoint.distance_squared_to(p_point);
700 if (d < closest_point_d) {
701
702 closest_point = spoint;
703 closest_point_d = d;
704 }
705 }
706 }
707 }
708
709 return closest_point;
710 }
711
get_closest_point_owner(const Vector2 & p_point)712 Object *Navigation2D::get_closest_point_owner(const Vector2 &p_point) {
713
714 Object *owner = NULL;
715 Vector2 closest_point = Vector2();
716 float closest_point_d = 1e20;
717
718 for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
719
720 if (!E->get().linked)
721 continue;
722 for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
723
724 Polygon &p = F->get();
725 for (int i = 2; i < p.edges.size(); i++) {
726
727 if (Geometry::is_point_in_triangle(p_point, _get_vertex(p.edges[0].point), _get_vertex(p.edges[i - 1].point), _get_vertex(p.edges[i].point))) {
728
729 E->get().owner;
730 }
731 }
732 }
733 }
734
735 for (Map<int, NavMesh>::Element *E = navpoly_map.front(); E; E = E->next()) {
736
737 if (!E->get().linked)
738 continue;
739 for (List<Polygon>::Element *F = E->get().polygons.front(); F; F = F->next()) {
740
741 Polygon &p = F->get();
742 int es = p.edges.size();
743 for (int i = 0; i < es; i++) {
744
745 Vector2 edge[2] = {
746 _get_vertex(p.edges[i].point),
747 _get_vertex(p.edges[(i + 1) % es].point)
748 };
749
750 Vector2 spoint = Geometry::get_closest_point_to_segment_2d(p_point, edge);
751 float d = spoint.distance_squared_to(p_point);
752 if (d < closest_point_d) {
753
754 closest_point = spoint;
755 closest_point_d = d;
756 owner = E->get().owner;
757 }
758 }
759 }
760 }
761
762 return owner;
763 }
764
_bind_methods()765 void Navigation2D::_bind_methods() {
766
767 ObjectTypeDB::bind_method(_MD("navpoly_create", "mesh:NavigationPolygon", "xform", "owner"), &Navigation2D::navpoly_create, DEFVAL(Variant()));
768 ObjectTypeDB::bind_method(_MD("navpoly_set_transform", "id", "xform"), &Navigation2D::navpoly_set_transform);
769 ObjectTypeDB::bind_method(_MD("navpoly_remove", "id"), &Navigation2D::navpoly_remove);
770
771 ObjectTypeDB::bind_method(_MD("get_simple_path", "start", "end", "optimize"), &Navigation2D::get_simple_path, DEFVAL(true));
772 ObjectTypeDB::bind_method(_MD("get_closest_point", "to_point"), &Navigation2D::get_closest_point);
773 ObjectTypeDB::bind_method(_MD("get_closest_point_owner", "to_point"), &Navigation2D::get_closest_point_owner);
774 }
775
Navigation2D()776 Navigation2D::Navigation2D() {
777
778 ERR_FAIL_COND(sizeof(Point) != 8);
779 cell_size = 1; // one pixel
780 last_id = 1;
781 }
782