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