413 lines
7.5 KiB
C++
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;
|
|
}
|