Merge branch 'master' of github.com:okamstudio/godot

1 files changed, 123 insertions, 9 deletions

diff --git a/scene/2d/navigation2d.cpp b/scene/2d/navigation2d.cpp
index 46af68444a..5db0e0a9fc 100644
--- a/scene/2d/navigation2d.cpp
+++ b/scene/2d/navigation2d.cpp
@@ -32,6 +32,7 @@ void Navigation2D::_navpoly_link(int p_id) {
 		p.edges.resize(plen);
 
 		Vector2 center;
+		float sum=0;
 
 		for(int j=0;j<plen;j++) {
 
@@ -46,8 +47,23 @@ void Navigation2D::_navpoly_link(int p_id) {
 			center+=ep;
 			e.point=_get_point(ep);
 			p.edges[j]=e;
+
+
+			int idxn = indices[(j+1)%plen];
+			if (idxn<0 || idxn>=len) {
+				valid=false;
+				break;
+			}
+
+			Vector2 epn = nm.xform.xform(r[idxn]);
+
+			sum+=(epn.x-ep.x)*(epn.y+ep.y);
+
+
 		}
 
+		p.clockwise=sum>0;
+
 		if (!valid) {
 			nm.polygons.pop_back();
 			ERR_CONTINUE(!valid);
@@ -75,9 +91,13 @@ void Navigation2D::_navpoly_link(int p_id) {
 			} else {
 
 				if (C->get().B!=NULL) {
-					print_line(String()+_get_vertex(ek.a)+" -> "+_get_vertex(ek.b));
+					ConnectionPending pending;
+					pending.polygon=&p;
+					pending.edge=j;
+					p.edges[j].P=C->get().pending.push_back(pending);
+					continue;
+					//print_line(String()+_get_vertex(ek.a)+" -> "+_get_vertex(ek.b));
 				}
-				ERR_CONTINUE(C->get().B!=NULL); //wut
 
 				C->get().B=&p;
 				C->get().B_edge=j;
@@ -117,7 +137,12 @@ void Navigation2D::_navpoly_unlink(int p_id) {
 			EdgeKey ek(edges[i].point,edges[next].point);
 			Map<EdgeKey,Connection>::Element *C=connections.find(ek);
 			ERR_CONTINUE(!C);
-			if (C->get().B) {
+
+			if (edges[i].P) {
+				C->get().pending.erase(edges[i].P);
+				edges[i].P=NULL;
+
+			} else if (C->get().B) {
 				//disconnect
 
 				C->get().B->edges[C->get().B_edge].C=NULL;
@@ -133,6 +158,20 @@ void Navigation2D::_navpoly_unlink(int p_id) {
 				C->get().B=NULL;
 				C->get().B_edge=-1;
 
+				if (C->get().pending.size()) {
+					//reconnect if something is pending
+					ConnectionPending cp = C->get().pending.front()->get();
+					C->get().pending.pop_front();
+
+					C->get().B=cp.polygon;
+					C->get().B_edge=cp.edge;
+					C->get().A->edges[C->get().A_edge].C=cp.polygon;
+					C->get().A->edges[C->get().A_edge].C_edge=cp.edge;
+					cp.polygon->edges[cp.edge].C=C->get().A;
+					cp.polygon->edges[cp.edge].C_edge=C->get().A_edge;
+					cp.polygon->edges[cp.edge].P=NULL;
+				}
+
 			} else {
 				connections.erase(C);
 				//erase
@@ -493,17 +532,30 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2& p_start, const Vect
 					left = _get_vertex(p->edges[prev].point);
 					right = _get_vertex(p->edges[prev_n].point);
 
-					if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5) < 0){
+					if (p->clockwise) {
 						SWAP(left,right);
 					}
+					/*if (CLOCK_TANGENT(apex_point,left,(left+right)*0.5) < 0){
+						SWAP(left,right);
+					}*/
 				}
 
 				bool skip=false;
 
+			/*	print_line("-----\nAPEX: "+(apex_point-end_point));
+				print_line("LEFT:");
+				print_line("\tPortal: "+(portal_left-end_point));
+				print_line("\tPoint: "+(left-end_point));
+				print_line("\tFree: "+itos(CLOCK_TANGENT(apex_point,portal_left,left) >= 0));
+				print_line("RIGHT:");
+				print_line("\tPortal: "+(portal_right-end_point));
+				print_line("\tPoint: "+(right-end_point));
+				print_line("\tFree: "+itos(CLOCK_TANGENT(apex_point,portal_right,right) <= 0));
+*/
 
 				if (CLOCK_TANGENT(apex_point,portal_left,left) >= 0){
 					//process
-					if (portal_left==apex_point || CLOCK_TANGENT(apex_point,left,portal_right) > 0) {
+					if (portal_left.distance_squared_to(apex_point)<CMP_EPSILON || CLOCK_TANGENT(apex_point,left,portal_right) > 0) {
 						left_poly=p;
 						portal_left=left;
 					} else {
@@ -516,14 +568,16 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2& p_start, const Vect
 						apex_poly=p;
 						portal_left=apex_point;
 						portal_right=apex_point;
-						path.push_back(apex_point);
+						if (path[path.size()-1].distance_to(apex_point)>CMP_EPSILON)
+							path.push_back(apex_point);
 						skip=true;
+						//print_line("addpoint left");
 					}
 				}
 
 				if (!skip && CLOCK_TANGENT(apex_point,portal_right,right) <= 0){
 					//process
-					if (portal_right==apex_point || CLOCK_TANGENT(apex_point,right,portal_left) < 0) {
+					if (portal_right.distance_squared_to(apex_point)<CMP_EPSILON || CLOCK_TANGENT(apex_point,right,portal_left) < 0) {
 						right_poly=p;
 						portal_right=right;
 					} else {
@@ -536,7 +590,10 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2& p_start, const Vect
 						apex_poly=p;
 						portal_right=apex_point;
 						portal_left=apex_point;
-						path.push_back(apex_point);
+						if (path[path.size()-1].distance_to(apex_point)>CMP_EPSILON)
+							path.push_back(apex_point);
+						//print_line("addpoint right");
+
 					}
 				}
 
@@ -547,7 +604,7 @@ Vector<Vector2> Navigation2D::get_simple_path(const Vector2& p_start, const Vect
 
 			}
 
-			if (path[path.size()-1]!=begin_point)
+			if (path[path.size()-1].distance_to(begin_point)>CMP_EPSILON)
 				path.push_back(begin_point);
 
 			path.invert();
@@ -639,6 +696,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 +761,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);
 
 }