Added a generic AStar implementation to Godot.
It's pretty fast, use it for games where Navigation does not cut it.
(cherry picked from commit 827a9aa829)
This commit is contained in:
Juan Linietsky
Rémi Verschelde
parent
e88a540b91
commit
7d25d20861
412
core/math/a_star.cpp
Normal file
412
core/math/a_star.cpp
Normal file
|
|
@ -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;
|
||||
}
|
||||
92
core/math/a_star.h
Normal file
92
core/math/a_star.h
Normal file
|
|
@ -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
|
||||
|
|
@ -33,6 +33,7 @@
|
|||
#include "io/config_file.h"
|
||||
#include "os/main_loop.h"
|
||||
#include "io/packet_peer.h"
|
||||
#include "math/a_star.h"
|
||||
#include "globals.h"
|
||||
#include "object_type_db.h"
|
||||
#include "geometry.h"
|
||||
|
|
@ -149,6 +150,7 @@ void register_core_types() {
|
|||
|
||||
ObjectTypeDB::register_type<PackedDataContainer>();
|
||||
ObjectTypeDB::register_virtual_type<PackedDataContainerRef>();
|
||||
ObjectTypeDB::register_type<AStar>();
|
||||
|
||||
ip = IP::create();
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue