godot/core/math/a_star.cpp
Juan Linietsky 827a9aa829 Added a generic AStar implementation to Godot.
It's pretty fast, use it for games where Navigation does not cut it.
2016-09-13 18:17:18 -03:00

413 lines
7.5 KiB
C++

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