Added a generic AStar implementation to Godot.

It's pretty fast, use it for games where Navigation does not cut it.

2 files changed, 504 insertions, 0 deletions

diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
new file mode 100644
index 0000000000..5bbbbdaa5a
--- /dev/null
+++ b/core/math/a_star.cpp
@@ -0,0 +1,412 @@
+#include "a_star.h"
+#include "geometry.h"
+
+
+int AStar::get_available_point_id() const {
+
+	if (points.empty()) {
+		return 1;
+	}
+
+	return points.back()->key()+1;
+}
+
+void AStar::add_point(int p_id, const Vector3 &p_pos, float p_weight_scale) {
+	ERR_FAIL_COND(p_id<0);
+	if (!points.has(p_id)) {
+		Point *pt = memnew( Point );
+		pt->id=p_id;
+		pt->pos=p_pos;
+		pt->weight_scale=p_weight_scale;
+		pt->prev_point=NULL;
+		pt->last_pass=0;
+		points[p_id]=pt;
+	} else {
+		points[p_id]->pos=p_pos;
+		points[p_id]->weight_scale=p_weight_scale;
+	}
+}
+
+Vector3 AStar::get_point_pos(int p_id) const{
+
+	ERR_FAIL_COND_V(!points.has(p_id),Vector3());
+
+	return points[p_id]->pos;
+
+}
+float AStar::get_point_weight_scale(int p_id) const{
+
+	ERR_FAIL_COND_V(!points.has(p_id),0);
+
+	return points[p_id]->weight_scale;
+
+}
+void AStar::remove_point(int p_id){
+
+	ERR_FAIL_COND(!points.has(p_id));
+
+	Point* p = points[p_id];
+
+	for(int i=0;i<p->neighbours.size();i++) {
+
+		Segment s(p_id,p->neighbours[i]->id);
+		segments.erase(s);
+		p->neighbours[i]->neighbours.erase(p);
+	}
+
+	memdelete(p);
+	points.erase(p_id);
+}
+
+void AStar::connect_points(int p_id,int p_with_id){
+
+	ERR_FAIL_COND(!points.has(p_id));
+	ERR_FAIL_COND(!points.has(p_with_id));
+	ERR_FAIL_COND(p_id==p_with_id);
+
+
+	Point* a = points[p_id];
+	Point* b = points[p_with_id];
+	a->neighbours.push_back(b);
+	b->neighbours.push_back(a);
+
+	Segment s(p_id,p_with_id);
+	if (s.from==p_id) {
+		s.from_point=a;
+		s.to_point=b;
+	} else {
+		s.from_point=b;
+		s.to_point=a;
+	}
+
+	segments.insert(s);
+
+
+}
+void AStar::disconnect_points(int p_id,int p_with_id){
+
+	Segment s(p_id,p_with_id);
+	ERR_FAIL_COND(!segments.has(s));
+
+
+	segments.erase(s);
+
+	Point *a = points[p_id];
+	Point *b = points[p_with_id];
+	a->neighbours.erase(b);
+	b->neighbours.erase(a);
+
+}
+bool AStar::are_points_connected(int p_id,int p_with_id) const{
+
+	Segment s(p_id,p_with_id);
+	return segments.has(s);
+}
+
+void AStar::clear(){
+
+	for (const Map<int,Point*>::Element *E=points.front();E;E=E->next()) {
+
+		memdelete(E->get());
+	}
+	segments.clear();
+	points.clear();
+}
+
+
+int AStar::get_closest_point(const Vector3& p_point) const{
+
+	int closest_id=-1;
+	float closest_dist=1e20;
+
+	for (const Map<int,Point*>::Element *E=points.front();E;E=E->next()) {
+
+		float d = p_point.distance_squared_to(E->get()->pos);
+		if (closest_id<0 || d<closest_dist) {
+			closest_dist=d;
+			closest_id=E->key();
+		}
+	}
+
+	return closest_id;
+
+
+}
+Vector3 AStar::get_closest_pos_in_segment(const Vector3& p_point) const {
+
+	float closest_dist = 1e20;
+	bool found=false;
+	Vector3 closest_point;
+
+
+	for (const Set<Segment>::Element *E=segments.front();E;E=E->next()) {
+
+		Vector3 segment[2]={
+			E->get().from_point->pos,
+			E->get().to_point->pos,
+		};
+
+		Vector3 p = Geometry::get_closest_point_to_segment(p_point,segment);
+		float d = p_point.distance_squared_to(p);
+		if (!found || d<closest_dist) {
+
+			closest_point=p;
+			closest_dist=d;
+			found=true;
+		}
+	}
+
+	return closest_point;
+}
+
+bool AStar::_solve(Point* begin_point, Point* end_point) {
+
+	pass++;
+
+	SelfList<Point>::List open_list;
+
+	bool found_route=false;
+
+
+	for(int i=0;i<begin_point->neighbours.size();i++) {
+
+		Point *n = begin_point->neighbours[i];
+		n->prev_point=begin_point;
+		n->distance=n->pos.distance_to(begin_point->pos);
+		n->distance*=n->weight_scale;
+		n->last_pass=pass;
+		open_list.add(&n->list);
+
+		if (end_point==n) {
+			found_route=true;
+			break;
+		}
+	}
+
+	while(!found_route) {
+
+		if (open_list.first()==NULL) {
+			//could not find path sadly
+			break;
+		}
+		//check open list
+
+		SelfList<Point> *least_cost_point=NULL;
+		float least_cost=1e30;
+
+		//this could be faster (cache previous results)
+		for (SelfList<Point> *E=open_list.first();E;E=E->next()) {
+
+			Point *p=E->self();
+
+			float cost=p->distance;
+			cost+=p->pos.distance_to(end_point->pos);
+			cost*=p->weight_scale;
+
+			if (cost<least_cost) {
+
+				least_cost_point=E;
+				least_cost=cost;
+			}
+		}
+
+
+		Point *p=least_cost_point->self();
+		//open the neighbours for search
+		int es = p->neighbours.size();
+
+		for(int i=0;i<es;i++) {
+
+
+			Point* e=p->neighbours[i];
+
+
+			float distance = p->pos.distance_to(e->pos) + p->distance;
+			distance*=e->weight_scale;
+
+
+
+			if (e->last_pass==pass) {
+				//oh this was visited already, can we win the cost?
+
+				if (e->distance>distance) {
+
+					e->prev_point=p;
+					e->distance=distance;
+				}
+			} else {
+				//add to open neighbours
+
+				e->prev_point=p;
+				e->distance=distance;
+				e->last_pass=pass; //mark as used
+				open_list.add(&e->list);
+
+				if (e==end_point) {
+					//oh my reached end! stop algorithm
+					found_route=true;
+					break;
+
+				}
+
+			}
+		}
+
+		if (found_route)
+			break;
+
+		open_list.remove(least_cost_point);
+	}
+
+	//clear the openf list
+	while(open_list.first()) {
+		open_list.remove( open_list.first() );
+	}
+
+	return found_route;
+
+}
+
+DVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
+
+	ERR_FAIL_COND_V(!points.has(p_from_id),DVector<Vector3>());
+	ERR_FAIL_COND_V(!points.has(p_to_id),DVector<Vector3>());
+
+
+	pass++;
+
+	Point* a = points[p_from_id];
+	Point* b = points[p_to_id];
+
+	if (a==b) {
+		DVector<Vector3> ret;
+		ret.push_back(a->pos);
+		return ret;
+	}
+
+
+	Point *begin_point=a;
+	Point *end_point=b;
+
+	bool found_route=_solve(begin_point,end_point);
+
+	if (!found_route)
+		return DVector<Vector3>();
+
+	//midpoints
+	Point *p=end_point;
+	int pc=1; //begin point
+	while(p!=begin_point) {
+		pc++;
+		p=p->prev_point;
+	}
+
+	DVector<Vector3> path;
+	path.resize(pc);
+
+	{
+		DVector<Vector3>::Write w = path.write();
+
+		Point *p=end_point;
+		int idx=pc-1;
+		while(p!=begin_point) {
+			w[idx--]=p->pos;
+			p=p->prev_point;
+		}
+
+		w[0]=p->pos; //assign first
+
+	}
+
+	return path;
+
+}
+
+
+DVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
+
+	ERR_FAIL_COND_V(!points.has(p_from_id),DVector<int>());
+	ERR_FAIL_COND_V(!points.has(p_to_id),DVector<int>());
+
+
+	pass++;
+
+	Point* a = points[p_from_id];
+	Point* b = points[p_to_id];
+
+	if (a==b) {
+		DVector<int> ret;
+		ret.push_back(a->id);
+		return ret;
+	}
+
+
+	Point *begin_point=a;
+	Point *end_point=b;
+
+	bool found_route=_solve(begin_point,end_point);
+
+	if (!found_route)
+		return DVector<int>();
+
+	//midpoints
+	Point *p=end_point;
+	int pc=1; //begin point
+	while(p!=begin_point) {
+		pc++;
+		p=p->prev_point;
+	}
+
+	DVector<int> path;
+	path.resize(pc);
+
+	{
+		DVector<int>::Write w = path.write();
+
+		p=end_point;
+		int idx=pc-1;
+		while(p!=begin_point) {
+			w[idx--]=p->id;
+			p=p->prev_point;
+		}
+
+		w[0]=p->id; //assign first
+
+	}
+
+	return path;
+}
+
+void AStar::_bind_methods() {
+
+	ObjectTypeDB::bind_method(_MD("get_available_point_id"),&AStar::get_available_point_id);
+	ObjectTypeDB::bind_method(_MD("add_point","id","pos","weight_scale"),&AStar::add_point,DEFVAL(1.0));
+	ObjectTypeDB::bind_method(_MD("get_point_pos","id"),&AStar::get_point_pos);
+	ObjectTypeDB::bind_method(_MD("get_point_weight_scale","id"),&AStar::get_point_weight_scale);
+	ObjectTypeDB::bind_method(_MD("remove_point","id"),&AStar::remove_point);
+
+	ObjectTypeDB::bind_method(_MD("connect_points","id","to_id"),&AStar::connect_points);
+	ObjectTypeDB::bind_method(_MD("disconnect_points","id","to_id"),&AStar::disconnect_points);
+	ObjectTypeDB::bind_method(_MD("are_points_connected","id","to_id"),&AStar::are_points_connected);
+
+	ObjectTypeDB::bind_method(_MD("clear"),&AStar::clear);
+
+	ObjectTypeDB::bind_method(_MD("get_closest_point","to_pos"),&AStar::get_closest_point);
+	ObjectTypeDB::bind_method(_MD("get_closest_pos_in_segment","to_pos"),&AStar::get_closest_pos_in_segment);
+
+	ObjectTypeDB::bind_method(_MD("get_point_path","from_id","to_id"),&AStar::get_point_path);
+	ObjectTypeDB::bind_method(_MD("get_id_path","from_id","to_id"),&AStar::get_id_path);
+
+}
+
+
+AStar::AStar() {
+
+	pass=1;
+}
+
+
+AStar::~AStar() {
+
+	pass=1;
+}
diff --git a/core/math/a_star.h b/core/math/a_star.h
new file mode 100644
index 0000000000..a0517b5941
--- /dev/null
+++ b/core/math/a_star.h
@@ -0,0 +1,92 @@
+#ifndef ASTAR_H
+#define ASTAR_H
+
+#include "reference.h"
+#include "self_list.h"
+
+class AStar: public Reference {
+
+	OBJ_TYPE(AStar,Reference)
+
+
+	uint64_t pass;
+
+	struct Point {
+
+		SelfList<Point> list;
+
+		int id;
+		Vector3 pos;
+		float weight_scale;
+		uint64_t last_pass;
+
+		Vector<Point*> neighbours;
+
+		//used for pathfinding
+		Point *prev_point;
+		float distance;
+
+		Point() : list(this) {}
+	};
+
+	Map<int,Point*> points;
+
+	struct Segment {
+		union {
+			struct {
+				int32_t from;
+				int32_t to;
+			};
+			uint64_t key;
+		};
+
+		Point *from_point;
+		Point *to_point;
+
+		bool operator<(const Segment& p_s) const { return key<p_s.key; }
+		Segment() { key=0; }
+		Segment(int p_from,int p_to) {
+			if (p_from > p_to) {
+				SWAP(p_from,p_to);
+			}
+
+			from=p_from;
+			to=p_to;
+		}
+	};
+
+
+	Set<Segment> segments;
+
+	bool _solve(Point *begin_point, Point *end_point);
+
+protected:
+
+	static void _bind_methods();
+public:
+
+	int get_available_point_id() const;
+
+	void add_point(int p_id,const Vector3& p_pos,float p_weight_scale=1);
+	Vector3 get_point_pos(int p_id) const;
+	float get_point_weight_scale(int p_id) const;
+	void remove_point(int p_id);
+
+	void connect_points(int p_id,int p_with_id);
+	void disconnect_points(int p_id,int p_with_id);
+	bool are_points_connected(int p_id,int p_with_id) const;
+
+	void clear();
+
+
+	int get_closest_point(const Vector3& p_point) const;
+	Vector3 get_closest_pos_in_segment(const Vector3& p_point) const;
+
+	DVector<Vector3> get_point_path(int p_from_id, int p_to_id);
+	DVector<int> get_id_path(int p_from_id, int p_to_id);
+
+	AStar();
+	~AStar();
+};
+
+#endif // ASTAR_H