una implementación que hice hace tiempo para un proyecto que, como todos los que he empezado, está en la carpeta fracasos.
typedef Vector2i PathNode;
typedef std::list<PathNode> Path;
class IPath
{
public:
virtual bool GetPath(PathNode &ini,PathNode &end,Path &path) = 0;
};
class AStar : public IPath
{
public:
public:
enum ePathInfo
{
PATH_INCOMPLETE,
PATH_COMPLETE,
PATH_NO_PATH
};
AStar(Map *_m):
m(_m)
{}
bool GetPath(PathNode &ini,PathNode &end,Path &p)
{
path = &p;
if(CalculePath(ini,end) != PATH_INCOMPLETE)
{
return false;
}
return true;
}
protected:
Map *m;
struct Node
{
Node(Vector2i &p,int _g = 0):
g(_g),
pos(p)
{
}
Vector2i pos;
Node *parent;
float f,g,h;
float calc_dist(Vector2i &dest)
{
//Vector2i v = dest-pos;
h = fabs(dest[0]-pos[0]) + fabs(dest[1]-pos[1]);
//h = vectorDist(Vector2f(v[0],v[1]));
f = g + h;
return f;
}
float weith()
{
f = g + h;
return f;
}
struct compare
{
bool operator()(Node*& u, Node*& v)
{
if(u->weith() > v->weith())
return true; //para que open.Top sea el menor
return false;
}
};
};
typedef std::list<Node*> NodeList;
std::list<Node*> close;
Path *path;
std::priority_queue<Node*,std::vector<Node*>,Node::compare> open;
Node *root;
ePathInfo CalculePath(Vector2i& ini,Vector2i& fin)
{
int max = 0;
Node *min;
root = new Node(ini);
root->calc_dist(fin);
root->parent = 0;
open.push(root);
debuglog << "calculando path "<< endl;
// assert()
while(max++ < 1000)
{
min = GetBest();
if(!min)
{
debuglog << "caminante, no hay camino" << endl;
return PATH_NO_PATH;
}
if(min->pos == fin)
{
BuildPath(min);
return PATH_INCOMPLETE;
}
CreateChild(min,fin);
close.push_back(min);
}
BuildPath(min);
return PATH_COMPLETE;
}
void FreeMemory()
{
while(!open.empty())
{
delete open.top();
open.pop();
}
std::list<Node*>::iterator i = close.begin();
for(;i!= close.end(); ++i)
{
delete *i;
}
close.clear();
}
Node *GetBest()
{
if(open.empty())
{
return 0;
}
Node *n = open.top();
assert(n);
open.pop();
if(!open.empty())
while(n->pos == open.top()->pos) open.pop();
return n;
}
void BuildPath(Node *d)
{
path->push_front(d->pos);
while(d->parent != root )
{
path->push_front(d->parent->pos);
d = d->parent;
}
path->push_front(d->parent->pos);
FreeMemory();
}
Node* SearchInNodes(Vector2i &n)
{
std::list<Node*>::iterator i = close.begin();
for(;i!= close.end(); ++i)
{
if((*i)->pos == n)
{
return *i;
}
}
return 0;
}
void CreateChild(Node* min,Vector2i &dest)
{
int v[8]= {1,0,-1,0 ,
0,1,0,-1};
Tile *t;
Node *n;
for (int i = 0; i< 4;++i )
{
t = m->GetTile(min->pos[0]+v[i],min->pos[1]+v[i+4]);
assert(t);
if(t && !t->IsSolid())
{
Vector2i v(min->pos[0]+v[i],min->pos[1]+v[i+4]);
// debuglog << "[" << v[0] << ","<< v[1]<< "]" << endl;
if(!SearchInNodes(v))
{
n = new Node(v);
assert(n);
n->parent = min;
n->g = 1;
//if(n->pos == dest) return n;
n->calc_dist(dest);
open.push(n);
}
}
}
}
};
