1 files changed, 27 insertions, 3 deletions

diff --git a/scene/3d/navigation.cpp b/scene/3d/navigation.cpp
index 48820706d..bfa8add09 100644
--- a/scene/3d/navigation.cpp
+++ b/scene/3d/navigation.cpp
@@ -85,9 +85,14 @@ void Navigation::_navmesh_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
+				//ERR_CONTINUE(C->get().B!=NULL); //wut
 
 				C->get().B=&p;
 				C->get().B_edge=j;
@@ -126,8 +131,13 @@ void Navigation::_navmesh_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;
@@ -143,6 +153,20 @@ void Navigation::_navmesh_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