class Node
{
public:
Node(int _x, int _y, int _h, double _cost, double _way, double _heuristic, Node* _parent) : x(_x), y(_y), h(_h), cost(_cost), way(_way), heuristic(_heuristic), parent(_parent) {}
int x, y;
unsigned int h;
Node *parent;
double cost, way, heuristic;
};
bool checkNext(Node *next, std::list<Node*> &opened, const std::list<Node*> &closed)
{
for(std::list<Node*>::const_iterator it = closed.begin(); it!=closed.end(); ++it)
{
if((*it)->x==next->x && (*it)->y==next->y)
return false;
}
for(std::list<Node*>::iterator iter = opened.begin(); iter!=opened.end(); ++iter)
{
if((*iter)->x==next->x && (*iter)->y==next->y){
if((*iter)->cost>next->cost) {
opened.erase(iter);
return true;
}
else
return false;
}
}
return true;
}
bool compare_cost(Node *first, Node *second)
{
if(first->cost < second->cost)
return true;
return false;
}
void generatePathImpl(Node *act, Node *end, const video::IImage &hmap, std::list<Node*> &opened, std::list<Node*> &closed)
{
if((act->x+10)<hmap.getDimension().Width) //the next one on the right side
{
double heur = sqrt(double((pow((act->x+10)-(end->x), 2.0)+pow((act->y)-(end->y), 2.0))));
double cost = (((unsigned)hmap.getPixel(act->x, act->y).getAverage()-(unsigned)(hmap.getPixel(act->x+10, act->y).getAverage())/255)*0.4)+0.3;
Node *next = new Node(act->x+10, act->y, hmap.getPixel(act->x+10, act->y).getAverage(), cost+heur, 10, heur, act);
if(checkNext(next, opened, closed))
opened.push_back(next);
}
if((act->x+10)<hmap.getDimension().Width && (act->y+10)<hmap.getDimension().Height) //the next one 10 steps right and 10 steps up
{
double heur = sqrt(double((pow((act->x+10)-(end->x), 2.0)+pow((act->y+10)-(end->y), 2.0))));
double cost = (((unsigned)hmap.getPixel(act->x, act->y+10).getAverage()-(unsigned)(hmap.getPixel(act->x+10, act->y+10).getAverage())/255)*0.4)+0.3;
Node *next = new Node(act->x+10, act->y+10, hmap.getPixel(act->x+10, act->y+10).getAverage(), cost+heur, 10, heur, act);
if(checkNext(next, opened, closed))
opened.push_back(next);
}
if((act->y+10)<hmap.getDimension().Height) //3
{
double heur = sqrt(double((pow((act->x)-(end->x), 2.0)+pow((act->y+10)-(end->y), 2.0))));
double cost = (((unsigned)hmap.getPixel(act->x, act->y+10).getAverage()-(unsigned)(hmap.getPixel(act->x, act->y+10).getAverage())/255)*0.4)+0.3;
Node *next = new Node(act->x, act->y+10, hmap.getPixel(act->x, act->y+10).getAverage(), cost+heur, 10, heur, act);
if(checkNext(next, opened, closed))
opened.push_back(next);
}
if((act->x-10)>0 && (act->y+10)<hmap.getDimension().Height) //4
{
double heur = sqrt(double((pow((act->x-10)-(end->x), 2.0)+pow((act->y-10)-(end->y), 2.0))));
double cost = (((unsigned)hmap.getPixel(act->x, act->y-10).getAverage()-(unsigned)(hmap.getPixel(act->x-10, act->y-10).getAverage())/255)*0.4)+0.3;
Node *next = new Node(act->x-10, act->y-10, hmap.getPixel(act->x-10, act->y-10).getAverage(), cost+heur, 10, heur, act);
if(checkNext(next, opened, closed))
opened.push_back(next);
}
if((act->x-10)>0) //5
{
double heur = sqrt(double((pow((act->x-10)-(end->x), 2.0)+pow((act->y)-(end->y), 2.0))));
double cost = (((unsigned)hmap.getPixel(act->x, act->y).getAverage()-(unsigned)(hmap.getPixel(act->x-10, act->y).getAverage())/255)*0.4)+0.3;
Node *next = new Node(act->x-10, act->y, hmap.getPixel(act->x-10, act->y).getAverage(), cost+heur, 10, heur, act);
if(checkNext(next, opened, closed))
opened.push_back(next);
}
if((act->x-10)>0 && (act->y-10)>0) //6
{
double heur = sqrt(double((pow((act->x-10)-(end->x), 2.0)+pow((act->y-10)-(end->y), 2.0))));
double cost = (((unsigned)hmap.getPixel(act->x, act->y-10).getAverage()-(unsigned)(hmap.getPixel(act->x-10, act->y-10).getAverage())/255)*0.4)+0.3;
Node *next = new Node(act->x-10, act->y-10, hmap.getPixel(act->x-10, act->y-10).getAverage(), cost+heur, 10, heur, act);
if(checkNext(next, opened, closed))
opened.push_back(next);
}
if((act->y-10)>0) //7
{
double heur = sqrt(double((pow((act->x)-(end->x), 2.0)+pow((act->y-10)-(end->y), 2.0))));
double cost = (((unsigned)hmap.getPixel(act->x, act->y-10).getAverage()-(unsigned)(hmap.getPixel(act->x, act->y-10).getAverage())/255)*0.4)+0.3;
Node *next = new Node(act->x, act->y-10, hmap.getPixel(act->x, act->y-10).getAverage(), cost+heur, 10, heur, act);
if(checkNext(next, opened, closed))
opened.push_back(next);
}
if((act->x+10)<hmap.getDimension().Width && (act->y-10)>0) //8
{
double heur = sqrt(double((pow((act->x+10)-(end->x), 2.0)+pow((act->y-10)-(end->y), 2.0))));
double cost = (((unsigned)hmap.getPixel(act->x, act->y-10).getAverage()-(unsigned)(hmap.getPixel(act->x+10, act->y-10).getAverage())/255)*0.4)+0.3;
Node *next = new Node(act->x+10, act->y-10, hmap.getPixel(act->x+10, act->y-10).getAverage(), cost+heur, 10, heur, act);
if(checkNext(next, opened, closed))
opened.push_back(next);
}
closed.push_back(act);
if(find(opened.begin(), opened.end(), act)!=opened.end())
opened.erase(find(opened.begin(), opened.end(), act));
opened.sort(compare_cost);
}
std::stack<Node* > generatePath(Node *start, Node *end, const video::IImage &hmap)
{
std::stack<Node*> paths;
std::list<Node*> opened;
std::list<Node*> closed;
if(start->x == end->x && start->y == start->y)
{
return paths;
}
opened.push_back(start);
generatePathImpl(start, end, hmap, opened, closed);
while (opened.size() != 0)
{
start = (*opened.begin());
opened.pop_front();
generatePathImpl(start, end, hmap, opened, closed);
}
for(std::list<Node*>::iterator iter = closed.begin(); iter!=closed.end(); ++iter)
{
if((*iter)->x == end->x && (*iter)->y == end->y)
{
for(Node* act = (*iter); act != 0; act=act->parent) {
paths.push(act);
}
return paths;
}
}
return paths;
}