From 7648088fcacb1603ecfaab34903b605029810d8a Mon Sep 17 00:00:00 2001
From: Juan Linietsky <reduzio@gmail.com>
Date: Tue, 21 Apr 2015 16:47:49 -0300
Subject: [PATCH] ability to get closest owner to point, for navigation and
 navigation2d

---
 scene/2d/navigation2d.cpp | 57 +++++++++++++++++++++++++++++++++++++++
 scene/2d/navigation2d.h   |  1 +
 scene/3d/navigation.cpp   | 34 ++++++++++++++++++++++-
 scene/3d/navigation.h     |  3 ++-
 4 files changed, 93 insertions(+), 2 deletions(-)

diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp
index 46af68444ad..e4d10cad123 100644
--- a/scene/2d/navigation2d.cpp
+++ b/scene/2d/navigation2d.cpp
@@ -639,6 +639,62 @@ Vector2 Navigation2D::get_closest_point(const Vector2& p_point) {
 
 }
 
+Object* Navigation2D::get_closest_point_owner(const Vector2& p_point) {
+
+	Object *owner=NULL;
+	Vector2 closest_point=Vector2();
+	float closest_point_d=1e20;
+
+	for (Map<int,NavMesh>::Element*E=navpoly_map.front();E;E=E->next()) {
+
+		if (!E->get().linked)
+			continue;
+		for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) {
+
+			Polygon &p=F->get();
+			for(int i=2;i<p.edges.size();i++) {
+
+				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))) {
+
+					E->get().owner;
+				}
+
+			}
+		}
+	}
+
+	for (Map<int,NavMesh>::Element*E=navpoly_map.front();E;E=E->next()) {
+
+		if (!E->get().linked)
+			continue;
+		for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) {
+
+			Polygon &p=F->get();
+			int es = p.edges.size();
+			for(int i=0;i<es;i++) {
+
+				Vector2 edge[2]={
+					_get_vertex(p.edges[i].point),
+					_get_vertex(p.edges[(i+1)%es].point)
+				};
+
+
+				Vector2 spoint=Geometry::get_closest_point_to_segment_2d(p_point,edge);
+				float d = spoint.distance_squared_to(p_point);
+				if (d<closest_point_d) {
+
+					closest_point=spoint;
+					closest_point_d=d;
+					owner=E->get().owner;
+				}
+			}
+		}
+	}
+
+	return owner;
+
+}
+
 
 void Navigation2D::_bind_methods() {
 
@@ -648,6 +704,7 @@ void Navigation2D::_bind_methods() {
 
 	ObjectTypeDB::bind_method(_MD("get_simple_path","start","end","optimize"),&Navigation2D::get_simple_path,DEFVAL(true));
 	ObjectTypeDB::bind_method(_MD("get_closest_point","to_point"),&Navigation2D::get_closest_point);
+	ObjectTypeDB::bind_method(_MD("get_closest_point_owner","to_point"),&Navigation2D::get_closest_point_owner);
 
 }
 
diff --git a/scene/2d/navigation2d.h b/scene/2d/navigation2d.h
index 7ff01bb442e..7a33105b770 100644
--- a/scene/2d/navigation2d.h
+++ b/scene/2d/navigation2d.h
@@ -130,6 +130,7 @@ public:
 
 	Vector<Vector2> get_simple_path(const Vector2& p_start, const Vector2& p_end,bool p_optimize=true);
 	Vector2 get_closest_point(const Vector2& p_point);
+	Object* get_closest_point_owner(const Vector2& p_point);
 
 	Navigation2D();
 };
diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index 6612d6bf125..8866a658012 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -614,6 +614,37 @@ Vector3 Navigation::get_closest_point_normal(const Vector3& p_point){
 }
 
 
+Object* Navigation::get_closest_point_owner(const Vector3& p_point){
+
+	Vector3 closest_point;
+	Object *owner=NULL;
+	float closest_point_d=1e20;
+
+	for (Map<int,NavMesh>::Element*E=navmesh_map.front();E;E=E->next()) {
+
+		if (!E->get().linked)
+			continue;
+		for(List<Polygon>::Element *F=E->get().polygons.front();F;F=F->next()) {
+
+			Polygon &p=F->get();
+			for(int i=2;i<p.edges.size();i++) {
+
+				Face3 f(_get_vertex(p.edges[0].point),_get_vertex(p.edges[i-1].point),_get_vertex(p.edges[i].point));
+				Vector3 inters = f.get_closest_point_to(p_point);
+				float d = inters.distance_to(p_point);
+				if (d<closest_point_d) {
+					closest_point=inters;
+					closest_point_d=d;
+					owner=E->get().owner;
+				}
+			}
+		}
+	}
+
+	return owner;
+
+}
+
 void Navigation::set_up_vector(const Vector3& p_up) {
 
 
@@ -633,9 +664,10 @@ void Navigation::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("navmesh_remove","id"),&Navigation::navmesh_remove);
 
 	ObjectTypeDB::bind_method(_MD("get_simple_path","start","end","optimize"),&Navigation::get_simple_path,DEFVAL(true));
-    ObjectTypeDB::bind_method(_MD("get_closest_point_to_segment","start","end","use_collision"),&Navigation::get_closest_point_to_segment,DEFVAL(false));
+	ObjectTypeDB::bind_method(_MD("get_closest_point_to_segment","start","end","use_collision"),&Navigation::get_closest_point_to_segment,DEFVAL(false));
 	ObjectTypeDB::bind_method(_MD("get_closest_point","to_point"),&Navigation::get_closest_point);
 	ObjectTypeDB::bind_method(_MD("get_closest_point_normal","to_point"),&Navigation::get_closest_point_normal);
+	ObjectTypeDB::bind_method(_MD("get_closest_point_owner","to_point"),&Navigation::get_closest_point_owner);
 
 	ObjectTypeDB::bind_method(_MD("set_up_vector","up"),&Navigation::set_up_vector);
 	ObjectTypeDB::bind_method(_MD("get_up_vector"),&Navigation::get_up_vector);
diff --git a/scene/3d/navigation.h b/scene/3d/navigation.h
index 19977c3110a..54cec8f1f71 100644
--- a/scene/3d/navigation.h
+++ b/scene/3d/navigation.h
@@ -135,9 +135,10 @@ public:
 	void navmesh_remove(int p_id);
 
 	Vector<Vector3> get_simple_path(const Vector3& p_start, const Vector3& p_end,bool p_optimize=true);
-    Vector3 get_closest_point_to_segment(const Vector3& p_from,const Vector3& p_to,const bool& p_use_collision=false);
+	Vector3 get_closest_point_to_segment(const Vector3& p_from,const Vector3& p_to,const bool& p_use_collision=false);
 	Vector3 get_closest_point(const Vector3& p_point);
 	Vector3 get_closest_point_normal(const Vector3& p_point);
+	Object* get_closest_point_owner(const Vector3& p_point);
 
 	Navigation();
 };