summaryrefslogtreecommitdiffstats
path: root/core/math/a_star.cpp
diff options
context:
space:
mode:
authorRémi Verschelde <rverschelde@gmail.com>2017-03-05 16:44:50 +0100
committerRémi Verschelde <rverschelde@gmail.com>2017-03-05 16:44:50 +0100
commit5dbf1809c6e3e905b94b8764e99491e608122261 (patch)
tree5e5a5360db15d86d59ec8c6e4f7eb511388c5a9a /core/math/a_star.cpp
parent45438e9918d421b244bfd7776a30e67dc7f2d3e3 (diff)
downloadredot-engine-5dbf1809c6e3e905b94b8764e99491e608122261.tar.gz
A Whole New World (clang-format edition)
I can show you the code Pretty, with proper whitespace Tell me, coder, now when did You last write readable code? I can open your eyes Make you see your bad indent Force you to respect the style The core devs agreed upon A whole new world A new fantastic code format A de facto standard With some sugar Enforced with clang-format A whole new world A dazzling style we all dreamed of And when we read it through It's crystal clear That now we're in a whole new world of code
Diffstat (limited to 'core/math/a_star.cpp')
-rw-r--r--core/math/a_star.cpp308
1 files changed, 138 insertions, 170 deletions
diff --git a/core/math/a_star.cpp b/core/math/a_star.cpp
index 7e31957e3e..110185c2d2 100644
--- a/core/math/a_star.cpp
+++ b/core/math/a_star.cpp
@@ -29,55 +29,52 @@
#include "a_star.h"
#include "geometry.h"
-
int AStar::get_available_point_id() const {
if (points.empty()) {
return 1;
}
- return points.back()->key()+1;
+ return points.back()->key() + 1;
}
void AStar::add_point(int p_id, const Vector3 &p_pos, real_t p_weight_scale) {
- ERR_FAIL_COND(p_id<0);
+ 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;
+ 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;
+ points[p_id]->pos = p_pos;
+ points[p_id]->weight_scale = p_weight_scale;
}
}
-Vector3 AStar::get_point_pos(int p_id) const{
+Vector3 AStar::get_point_pos(int p_id) const {
- ERR_FAIL_COND_V(!points.has(p_id),Vector3());
+ ERR_FAIL_COND_V(!points.has(p_id), Vector3());
return points[p_id]->pos;
-
}
-real_t AStar::get_point_weight_scale(int p_id) const{
+real_t AStar::get_point_weight_scale(int p_id) const {
- ERR_FAIL_COND_V(!points.has(p_id),0);
+ ERR_FAIL_COND_V(!points.has(p_id), 0);
return points[p_id]->weight_scale;
-
}
-void AStar::remove_point(int p_id){
+void AStar::remove_point(int p_id) {
ERR_FAIL_COND(!points.has(p_id));
- Point* p = points[p_id];
+ Point *p = points[p_id];
- for(int i=0;i<p->neighbours.size();i++) {
+ for (int i = 0; i < p->neighbours.size(); i++) {
- Segment s(p_id,p->neighbours[i]->id);
+ Segment s(p_id, p->neighbours[i]->id);
segments.erase(s);
p->neighbours[i]->neighbours.erase(p);
}
@@ -86,54 +83,49 @@ void AStar::remove_point(int p_id){
points.erase(p_id);
}
-void AStar::connect_points(int p_id,int p_with_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);
+ ERR_FAIL_COND(p_id == p_with_id);
-
- Point* a = points[p_id];
- Point* b = points[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;
+ 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;
+ s.from_point = b;
+ s.to_point = a;
}
segments.insert(s);
-
-
}
-void AStar::disconnect_points(int p_id,int p_with_id){
+void AStar::disconnect_points(int p_id, int p_with_id) {
- Segment s(p_id,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{
+bool AStar::are_points_connected(int p_id, int p_with_id) const {
- Segment s(p_id,p_with_id);
+ Segment s(p_id, p_with_id);
return segments.has(s);
}
-void AStar::clear(){
+void AStar::clear() {
- for (const Map<int,Point*>::Element *E=points.front();E;E=E->next()) {
+ for (const Map<int, Point *>::Element *E = points.front(); E; E = E->next()) {
memdelete(E->get());
}
@@ -141,142 +133,130 @@ void AStar::clear(){
points.clear();
}
+int AStar::get_closest_point(const Vector3 &p_point) const {
-int AStar::get_closest_point(const Vector3& p_point) const{
-
- int closest_id=-1;
- real_t closest_dist=1e20;
+ int closest_id = -1;
+ real_t closest_dist = 1e20;
- for (const Map<int,Point*>::Element *E=points.front();E;E=E->next()) {
+ for (const Map<int, Point *>::Element *E = points.front(); E; E = E->next()) {
real_t d = p_point.distance_squared_to(E->get()->pos);
- if (closest_id<0 || d<closest_dist) {
- closest_dist=d;
- closest_id=E->key();
+ 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 {
+Vector3 AStar::get_closest_pos_in_segment(const Vector3 &p_point) const {
real_t closest_dist = 1e20;
- bool found=false;
+ bool found = false;
Vector3 closest_point;
+ for (const Set<Segment>::Element *E = segments.front(); E; E = E->next()) {
- for (const Set<Segment>::Element *E=segments.front();E;E=E->next()) {
-
- Vector3 segment[2]={
+ Vector3 segment[2] = {
E->get().from_point->pos,
E->get().to_point->pos,
};
- Vector3 p = Geometry::get_closest_point_to_segment(p_point,segment);
+ Vector3 p = Geometry::get_closest_point_to_segment(p_point, segment);
real_t d = p_point.distance_squared_to(p);
- if (!found || d<closest_dist) {
+ if (!found || d < closest_dist) {
- closest_point=p;
- closest_dist=d;
- found=true;
+ closest_point = p;
+ closest_dist = d;
+ found = true;
}
}
return closest_point;
}
-bool AStar::_solve(Point* begin_point, Point* end_point) {
+bool AStar::_solve(Point *begin_point, Point *end_point) {
pass++;
SelfList<Point>::List open_list;
- bool found_route=false;
-
+ bool found_route = false;
- for(int i=0;i<begin_point->neighbours.size();i++) {
+ 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;
+ 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;
+ if (end_point == n) {
+ found_route = true;
break;
}
}
- while(!found_route) {
+ while (!found_route) {
- if (open_list.first()==NULL) {
+ if (open_list.first() == NULL) {
//could not find path sadly
break;
}
//check open list
- SelfList<Point> *least_cost_point=NULL;
- real_t least_cost=1e30;
+ SelfList<Point> *least_cost_point = NULL;
+ real_t least_cost = 1e30;
//this could be faster (cache previous results)
- for (SelfList<Point> *E=open_list.first();E;E=E->next()) {
+ for (SelfList<Point> *E = open_list.first(); E; E = E->next()) {
- Point *p=E->self();
+ Point *p = E->self();
- real_t cost=p->distance;
- cost+=p->pos.distance_to(end_point->pos);
- cost*=p->weight_scale;
+ real_t cost = p->distance;
+ cost += p->pos.distance_to(end_point->pos);
+ cost *= p->weight_scale;
- if (cost<least_cost) {
+ if (cost < least_cost) {
- least_cost_point=E;
- least_cost=cost;
+ least_cost_point = E;
+ least_cost = cost;
}
}
-
- Point *p=least_cost_point->self();
+ 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];
+ for (int i = 0; i < es; i++) {
+ Point *e = p->neighbours[i];
real_t distance = p->pos.distance_to(e->pos) + p->distance;
- distance*=e->weight_scale;
-
-
+ distance *= e->weight_scale;
- if (e->last_pass==pass) {
+ if (e->last_pass == pass) {
//oh this was visited already, can we win the cost?
- if (e->distance>distance) {
+ if (e->distance > distance) {
- e->prev_point=p;
- 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
+ e->prev_point = p;
+ e->distance = distance;
+ e->last_pass = pass; //mark as used
open_list.add(&e->list);
- if (e==end_point) {
+ if (e == end_point) {
//oh my reached end! stop algorithm
- found_route=true;
+ found_route = true;
break;
-
}
-
}
}
@@ -287,46 +267,43 @@ bool AStar::_solve(Point* begin_point, Point* end_point) {
}
//clear the openf list
- while(open_list.first()) {
- open_list.remove( open_list.first() );
+ while (open_list.first()) {
+ open_list.remove(open_list.first());
}
return found_route;
-
}
PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
- ERR_FAIL_COND_V(!points.has(p_from_id),PoolVector<Vector3>());
- ERR_FAIL_COND_V(!points.has(p_to_id),PoolVector<Vector3>());
-
+ ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector<Vector3>());
+ ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector<Vector3>());
pass++;
- Point* a = points[p_from_id];
- Point* b = points[p_to_id];
+ Point *a = points[p_from_id];
+ Point *b = points[p_to_id];
- if (a==b) {
+ if (a == b) {
PoolVector<Vector3> ret;
ret.push_back(a->pos);
return ret;
}
+ Point *begin_point = a;
+ Point *end_point = b;
- Point *begin_point=a;
- Point *end_point=b;
-
- bool found_route=_solve(begin_point,end_point);
+ bool found_route = _solve(begin_point, end_point);
if (!found_route)
return PoolVector<Vector3>();
//midpoints
- Point *p=end_point;
- int pc=1; //begin point
- while(p!=begin_point) {
+ Point *p = end_point;
+ int pc = 1; //begin point
+ while (p != begin_point) {
pc++;
- p=p->prev_point;
+ p = p->prev_point;
}
PoolVector<Vector3> path;
@@ -335,54 +312,49 @@ PoolVector<Vector3> AStar::get_point_path(int p_from_id, int p_to_id) {
{
PoolVector<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;
+ 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
-
+ w[0] = p->pos; //assign first
}
return path;
-
}
-
PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
- ERR_FAIL_COND_V(!points.has(p_from_id),PoolVector<int>());
- ERR_FAIL_COND_V(!points.has(p_to_id),PoolVector<int>());
-
+ ERR_FAIL_COND_V(!points.has(p_from_id), PoolVector<int>());
+ ERR_FAIL_COND_V(!points.has(p_to_id), PoolVector<int>());
pass++;
- Point* a = points[p_from_id];
- Point* b = points[p_to_id];
+ Point *a = points[p_from_id];
+ Point *b = points[p_to_id];
- if (a==b) {
+ if (a == b) {
PoolVector<int> ret;
ret.push_back(a->id);
return ret;
}
+ Point *begin_point = a;
+ Point *end_point = b;
- Point *begin_point=a;
- Point *end_point=b;
-
- bool found_route=_solve(begin_point,end_point);
+ bool found_route = _solve(begin_point, end_point);
if (!found_route)
return PoolVector<int>();
//midpoints
- Point *p=end_point;
- int pc=1; //begin point
- while(p!=begin_point) {
+ Point *p = end_point;
+ int pc = 1; //begin point
+ while (p != begin_point) {
pc++;
- p=p->prev_point;
+ p = p->prev_point;
}
PoolVector<int> path;
@@ -391,15 +363,14 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
{
PoolVector<int>::Write w = path.write();
- p=end_point;
- int idx=pc-1;
- while(p!=begin_point) {
- w[idx--]=p->id;
- p=p->prev_point;
+ 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
-
+ w[0] = p->id; //assign first
}
return path;
@@ -407,34 +378,31 @@ PoolVector<int> AStar::get_id_path(int p_from_id, int p_to_id) {
void AStar::_bind_methods() {
- ClassDB::bind_method(D_METHOD("get_available_point_id"),&AStar::get_available_point_id);
- ClassDB::bind_method(D_METHOD("add_point","id","pos","weight_scale"),&AStar::add_point,DEFVAL(1.0));
- ClassDB::bind_method(D_METHOD("get_point_pos","id"),&AStar::get_point_pos);
- ClassDB::bind_method(D_METHOD("get_point_weight_scale","id"),&AStar::get_point_weight_scale);
- ClassDB::bind_method(D_METHOD("remove_point","id"),&AStar::remove_point);
-
- ClassDB::bind_method(D_METHOD("connect_points","id","to_id"),&AStar::connect_points);
- ClassDB::bind_method(D_METHOD("disconnect_points","id","to_id"),&AStar::disconnect_points);
- ClassDB::bind_method(D_METHOD("are_points_connected","id","to_id"),&AStar::are_points_connected);
+ ClassDB::bind_method(D_METHOD("get_available_point_id"), &AStar::get_available_point_id);
+ ClassDB::bind_method(D_METHOD("add_point", "id", "pos", "weight_scale"), &AStar::add_point, DEFVAL(1.0));
+ ClassDB::bind_method(D_METHOD("get_point_pos", "id"), &AStar::get_point_pos);
+ ClassDB::bind_method(D_METHOD("get_point_weight_scale", "id"), &AStar::get_point_weight_scale);
+ ClassDB::bind_method(D_METHOD("remove_point", "id"), &AStar::remove_point);
- ClassDB::bind_method(D_METHOD("clear"),&AStar::clear);
+ ClassDB::bind_method(D_METHOD("connect_points", "id", "to_id"), &AStar::connect_points);
+ ClassDB::bind_method(D_METHOD("disconnect_points", "id", "to_id"), &AStar::disconnect_points);
+ ClassDB::bind_method(D_METHOD("are_points_connected", "id", "to_id"), &AStar::are_points_connected);
- ClassDB::bind_method(D_METHOD("get_closest_point","to_pos"),&AStar::get_closest_point);
- ClassDB::bind_method(D_METHOD("get_closest_pos_in_segment","to_pos"),&AStar::get_closest_pos_in_segment);
+ ClassDB::bind_method(D_METHOD("clear"), &AStar::clear);
- ClassDB::bind_method(D_METHOD("get_point_path","from_id","to_id"),&AStar::get_point_path);
- ClassDB::bind_method(D_METHOD("get_id_path","from_id","to_id"),&AStar::get_id_path);
+ ClassDB::bind_method(D_METHOD("get_closest_point", "to_pos"), &AStar::get_closest_point);
+ ClassDB::bind_method(D_METHOD("get_closest_pos_in_segment", "to_pos"), &AStar::get_closest_pos_in_segment);
+ ClassDB::bind_method(D_METHOD("get_point_path", "from_id", "to_id"), &AStar::get_point_path);
+ ClassDB::bind_method(D_METHOD("get_id_path", "from_id", "to_id"), &AStar::get_id_path);
}
-
AStar::AStar() {
- pass=1;
+ pass = 1;
}
-
AStar::~AStar() {
- pass=1;
+ pass = 1;
}