godot/scene/2d/navigation2d.cpp

#include "navigation2d.h"

#define USE_ENTRY_POINT

void Navigation2D::_navpoly_link(int p_id) {

	ERR_FAIL_COND(!navpoly_map.has(p_id));
	NavMesh &nm=navpoly_map[p_id];
	ERR_FAIL_COND(nm.linked);

	DVector<Vector2> vertices=nm.navpoly->get_vertices();
	int len = vertices.size();
	if (len==0)
		return;

	DVector<Vector2>::Read r=vertices.read();

	for(int i=0;i<nm.navpoly->get_polygon_count();i++) {

		//build

		List<Polygon>::Element *P=nm.polygons.push_back(Polygon());
		Polygon &p=P->get();
		p.owner=&nm;

		Vector<int> poly = nm.navpoly->get_polygon(i);
		int plen=poly.size();
		const int *indices=poly.ptr();
		bool valid=true;
		p.edges.resize(plen);

		Vector2 center;
		float sum=0;

		for(int j=0;j<plen;j++) {

			int idx = indices[j];
			if (idx<0 || idx>=len) {
				valid=false;
				break;
			}

			Polygon::Edge e;
			Vector2 ep=nm.xform.xform(r[idx]);
			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);
			continue;
		}

		p.center=center/plen;

		//connect

		for(int j=0;j<plen;j++) {

			int next = (j+1)%plen;
			EdgeKey ek(p.edges[j].point,p.edges[next].point);

			Map<EdgeKey,Connection>::Element *C=connections.find(ek);
			if (!C) {

				Connection c;
				c.A=&p;
				c.A_edge=j;
				c.B=NULL;
				c.B_edge=-1;
				connections[ek]=c;
			} else {

				if (C->get().B!=NULL) {
					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));
				}

				C->get().B=&p;
				C->get().B_edge=j;
				C->get().A->edges[C->get().A_edge].C=&p;
				C->get().A->edges[C->get().A_edge].C_edge=j;;
				p.edges[j].C=C->get().A;
				p.edges[j].C_edge=C->get().A_edge;
				//connection successful.
			}
		}
	}

	nm.linked=true;

}


void Navigation2D::_navpoly_unlink(int p_id) {

	ERR_FAIL_COND(!navpoly_map.has(p_id));
	NavMesh &nm=navpoly_map[p_id];
	ERR_FAIL_COND(!nm.linked);

	//print_line("UNLINK");

	for (List<Polygon>::Element *E=nm.polygons.front();E;E=E->next()) {


		Polygon &p=E->get();

		int ec = p.edges.size();
		Polygon::Edge *edges=p.edges.ptr();

		for(int i=0;i<ec;i++) {
			int next = (i+1)%ec;

			EdgeKey ek(edges[i].point,edges[next].point);
			Map<EdgeKey,Connection>::Element *C=connections.find(ek);
			ERR_CONTINUE(!C);

			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;
				C->get().B->edges[C->get().B_edge].C_edge=-1;
				C->get().A->edges[C->get().A_edge].C=NULL;
				C->get().A->edges[C->get().A_edge].C_edge=-1;

				if (C->get().A==&E->get()) {

					C->get().A=C->get().B;
					C->get().A_edge=C->get().B_edge;
				}
				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
			}
		}
	}

	nm.polygons.clear();

	nm.linked=false;


}


int Navigation2D::navpoly_create(const Ref<NavigationPolygon>& p_mesh, const Matrix32& p_xform, Object *p_owner) {

	int id = last_id++;
	NavMesh nm;
	nm.linked=false;
	nm.navpoly=p_mesh;
	nm.xform=p_xform;
	nm.owner=p_owner;
	navpoly_map[id]=nm;

	_navpoly_link(id);

	return id;
}

void Navigation2D::navpoly_set_transform(int p_id, const Matrix32& p_xform){

	ERR_FAIL_COND(!navpoly_map.has(p_id));
	NavMesh &nm=navpoly_map[p_id];
	if (nm.xform==p_xform)
		return; //bleh
	_navpoly_unlink(p_id);
	nm.xform=p_xform;
	_navpoly_link(p_id);



}
void Navigation2D::navpoly_remove(int p_id){

	ERR_FAIL_COND(!navpoly_map.has(p_id));
	_navpoly_unlink(p_id);
	navpoly_map.erase(p_id);

}
#if 0
void Navigation2D::_clip_path(Vector<Vector2>& path, Polygon *from_poly, const Vector2& p_to_point, Polygon* p_to_poly) {

	Vector2 from = path[path.size()-1];

	if (from.distance_to(p_to_point)<CMP_EPSILON)
		return;
	Plane cut_plane;
	cut_plane.normal = (from-p_to_point).cross(up);
	if (cut_plane.normal==Vector2())
		return;
	cut_plane.normal.normalize();
	cut_plane.d = cut_plane.normal.dot(from);


	while(from_poly!=p_to_poly) {

		int pe = from_poly->prev_edge;
		Vector2 a = _get_vertex(from_poly->edges[pe].point);
		Vector2 b = _get_vertex(from_poly->edges[(pe+1)%from_poly->edges.size()].point);

		from_poly=from_poly->edges[pe].C;
		ERR_FAIL_COND(!from_poly);

		if (a.distance_to(b)>CMP_EPSILON) {

			Vector2 inters;
			if (cut_plane.intersects_segment(a,b,&inters)) {
				if (inters.distance_to(p_to_point)>CMP_EPSILON && inters.distance_to(path[path.size()-1])>CMP_EPSILON) {
					path.push_back(inters);
				}
			}
		}
	}
}
#endif

Vector<Vector2> Navigation2D::get_simple_path(const Vector2& p_start, const Vector2& p_end, bool p_optimize) {


	Polygon *begin_poly=NULL;
	Polygon *end_poly=NULL;
	Vector2 begin_point;
	Vector2 end_point;
	float begin_d=1e20;
	float end_d=1e20;

	//look for point inside triangle

	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();
			if (begin_d || end_d) {
				for(int i=2;i<p.edges.size();i++) {

					if (begin_d>0) {

						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))) {

							begin_poly=&p;
							begin_point=p_start;
							begin_d=0;
							if (end_d==0)
								break;

						}
					}

					if (end_d>0) {

						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))) {

							end_poly=&p;
							end_point=p_end;
							end_d=0;
							if (begin_d==0)
								break;
						}
					}

				}
			}

			p.prev_edge=-1;
		}
	}

	//start or end not inside triangle.. look for closest segment :|
	if (begin_d || end_d) {
		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)
					};


					if (begin_d>0) {
						Vector2 spoint=Geometry::get_closest_point_to_segment_2d(p_start,edge);
						float d = spoint.distance_to(p_start);
						if (d<begin_d) {
							begin_poly=&p;
							begin_point=spoint;
							begin_d=d;
						}
					}

					if (end_d>0) {
						Vector2 spoint=Geometry::get_closest_point_to_segment_2d(p_end,edge);
						float d = spoint.distance_to(p_end);
						if (d<end_d) {
							end_poly=&p;
							end_point=spoint;
							end_d=d;
						}
					}
				}
			}
		}
	}

	if (!begin_poly || !end_poly) {

		return Vector<Vector2>(); //no path
	}

	if (begin_poly==end_poly) {

		Vector<Vector2> path;
		path.resize(2);
		path[0]=begin_point;
		path[1]=end_point;
		//print_line("Direct Path");
		return path;
	}


	bool found_route=false;

	List<Polygon*> open_list;

	begin_poly->entry=p_start;

	for(int i=0;i<begin_poly->edges.size();i++) {

		if (begin_poly->edges[i].C) {

			begin_poly->edges[i].C->prev_edge=begin_poly->edges[i].C_edge;
#ifdef USE_ENTRY_POINT
			Vector2 edge[2]={
				_get_vertex(begin_poly->edges[i].point),
				_get_vertex(begin_poly->edges[(i+1)%begin_poly->edges.size()].point)
			};

			Vector2 entry = Geometry::get_closest_point_to_segment_2d(begin_poly->entry,edge);
			begin_poly->edges[i].C->distance = begin_poly->entry.distance_to(entry);
			begin_poly->edges[i].C->entry=entry;
#else
			begin_poly->edges[i].C->distance=begin_poly->center.distance_to(begin_poly->edges[i].C->center);
#endif
			open_list.push_back(begin_poly->edges[i].C);

			if (begin_poly->edges[i].C==end_poly) {
				found_route=true;
			}
		}
	}


	while(!found_route) {

		if (open_list.size()==0) {
		//	print_line("NOU OPEN LIST");
			break;
		}
		//check open list

		List<Polygon*>::Element *least_cost_poly=NULL;
		float least_cost=1e30;

		//this could be faster (cache previous results)
		for (List<Polygon*>::Element *E=open_list.front();E;E=E->next()) {

			Polygon *p=E->get();


			float cost=p->distance;
			cost+=p->center.distance_to(end_point);

			if (cost<least_cost) {

				least_cost_poly=E;
				least_cost=cost;
			}
		}


		Polygon *p=least_cost_poly->get();
		//open the neighbours for search
		int es = p->edges.size();

		for(int i=0;i<es;i++) {


			Polygon::Edge &e=p->edges[i];

			if (!e.C)
				continue;

#ifdef USE_ENTRY_POINT
			Vector2 edge[2]={
				_get_vertex(p->edges[i].point),
				_get_vertex(p->edges[(i+1)%es].point)
			};

			Vector2 edge_entry = Geometry::get_closest_point_to_segment_2d(p->entry,edge);
			float distance = p->entry.distance_to(edge_entry) + p->distance;

#else

			float distance = p->center.distance_to(e.C->center) + p->distance;

#endif


			if (e.C->prev_edge!=-1) {
				//oh this was visited already, can we win the cost?

				if (e.C->distance>distance) {

					e.C->prev_edge=e.C_edge;
					e.C->distance=distance;
#ifdef USE_ENTRY_POINT
					e.C->entry=edge_entry;
#endif
				}
			} else {
				//add to open neighbours

				e.C->prev_edge=e.C_edge;
				e.C->distance=distance;
#ifdef USE_ENTRY_POINT
				e.C->entry=edge_entry;
#endif

				open_list.push_back(e.C);

				if (e.C==end_poly) {
					//oh my reached end! stop algorithm
					found_route=true;
					break;

				}

			}
		}

		if (found_route)
			break;

		open_list.erase(least_cost_poly);
	}
	{
		       Polygon *p=end_poly;
		       int idx=0;

		       while(true) {
			   int prev = p->prev_edge;
			   int prev_n = (p->prev_edge+1)%p->edges.size();
			   Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point))*0.5;
			   String points;
			   for(int i=0;i<p->edges.size();i++) {
				   if (i>0)
					   points+=", ";
				   points+=_get_vertex(p->edges[i].point);
			   }
			   print_line("poly "+itos(idx++)+" - "+points);
			   p = p->edges[prev].C;
			   if (p==begin_poly)
			       break;
		       }
		   }
	if (found_route) {

		Vector<Vector2> path;

		if (p_optimize) {
			//string pulling

			Polygon *apex_poly=end_poly;
			Vector2 apex_point=end_point;
			Vector2 portal_left=apex_point;
			Vector2 portal_right=apex_point;
			Polygon *left_poly=end_poly;
			Polygon *right_poly=end_poly;
			Polygon *p=end_poly;
			path.push_back(end_point);

			while(p) {

				Vector2 left;
				Vector2 right;

//#define CLOCK_TANGENT(m_a,m_b,m_c) ( ((m_a)-(m_c)).cross((m_a)-(m_b)) )
#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)))

				if (p==begin_poly) {
					left=begin_point;
					right=begin_point;
				} else {
					int prev = p->prev_edge;
					int prev_n = (p->prev_edge+1)%p->edges.size();
					left = _get_vertex(p->edges[prev].point);
					right = _get_vertex(p->edges[prev_n].point);

					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("\tLeft Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_left,left)));
				print_line("\tLeft Distance: "+rtos(portal_left.distance_squared_to(apex_point)));
				print_line("\tLeft Test: "+rtos(CLOCK_TANGENT(apex_point,left,portal_right)));
				print_line("RIGHT:");
				print_line("\tPortal: "+(portal_right-end_point));
				print_line("\tPoint: "+(right-end_point));
				print_line("\tRight Tangent: "+rtos(CLOCK_TANGENT(apex_point,portal_right,right)));
				print_line("\tRight Distance: "+rtos(portal_right.distance_squared_to(apex_point)));
				print_line("\tRight Test: "+rtos(CLOCK_TANGENT(apex_point,right,portal_left)));


				if (CLOCK_TANGENT(apex_point,portal_left,left) >= 0){
					//process
					if (portal_left.distance_squared_to(apex_point)<CMP_EPSILON || CLOCK_TANGENT(apex_point,left,portal_right) > 0) {
						left_poly=p;
						portal_left=left;
						print_line("***ADVANCE LEFT");
					} else {

						//_clip_path(path,apex_poly,portal_right,right_poly);

						apex_point=portal_right;
						p=right_poly;
						left_poly=p;
						apex_poly=p;
						portal_left=apex_point;
						portal_right=apex_point;
						if (path[path.size()-1].distance_to(apex_point)>CMP_EPSILON)
							path.push_back(apex_point);
						skip=true;
						//print_line("addpoint left");
						print_line("***CLIP LEFT");
					}
				}

				if (!skip && CLOCK_TANGENT(apex_point,portal_right,right) <= 0){
					//process
					if (portal_right.distance_squared_to(apex_point)<CMP_EPSILON || CLOCK_TANGENT(apex_point,right,portal_left) < 0) {
						right_poly=p;
						portal_right=right;
						print_line("***ADVANCE RIGHT");
					} else {

						//_clip_path(path,apex_poly,portal_left,left_poly);

						apex_point=portal_left;
						p=left_poly;
						right_poly=p;
						apex_poly=p;
						portal_right=apex_point;
						portal_left=apex_point;
						if (path[path.size()-1].distance_to(apex_point)>CMP_EPSILON)
							path.push_back(apex_point);
						//print_line("addpoint right");
						print_line("***CLIP RIGHT");

					}
				}

				if (p!=begin_poly)
					p=p->edges[p->prev_edge].C;
				else
					p=NULL;

			}

			if (path[path.size()-1].distance_to(begin_point)>CMP_EPSILON)
				path.push_back(begin_point);

			path.invert();




		} else {
			//midpoints
			Polygon *p=end_poly;

			path.push_back(end_point);
			while(true) {
				int prev = p->prev_edge;
				int prev_n = (p->prev_edge+1)%p->edges.size();
				Vector2 point = (_get_vertex(p->edges[prev].point) + _get_vertex(p->edges[prev_n].point))*0.5;
				path.push_back(point);
				p = p->edges[prev].C;
				if (p==begin_poly)
					break;
			}

			path.push_back(begin_point);


			path.invert();;
		}

		return path;
	}


	return Vector<Vector2>();

}


Vector2 Navigation2D::get_closest_point(const Vector2& p_point) {

	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))) {

					return p_point; //inside triangle, nothing else to discuss
				}

			}
		}
	}

	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;
				}
			}
		}
	}

	return closest_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() {

	ObjectTypeDB::bind_method(_MD("navpoly_create","mesh:NavigationPolygon","xform","owner"),&Navigation2D::navpoly_create,DEFVAL(Variant()));
	ObjectTypeDB::bind_method(_MD("navpoly_set_transform","id","xform"),&Navigation2D::navpoly_set_transform);
	ObjectTypeDB::bind_method(_MD("navpoly_remove","id"),&Navigation2D::navpoly_remove);

	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);

}

Navigation2D::Navigation2D() {

	ERR_FAIL_COND( sizeof(Point)!=8 );
	cell_size=1; // one pixel
	last_id=1;

}