1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128
|
vector<Positions*> Pathfinder::aStar(float x1, float y1, float x2, float y2)
{
vector<Positions*> path;
m_pPoints->walkable = true;
// Define points to work with
Points *start = getPointFromCoord(x1, y1);
Points *end = getPointFromCoord(x2, y2);
Points *current;
Points *child;
// Define the open and the close list
list<Points*> openList;
list<Points*> closedList;
list<Points*>::iterator i;
unsigned int n = 0;
// Add the start point to the openList
openList.push_back(start);
start->opened = true;
while (n == 0 || (current != end && n < 20))
{
// Look for the smallest F value in the openList and make it the current point
for (i = openList.begin(); i != openList.end(); ++ i)
{
if (i == openList.begin() || (*i)->getFScore() <= current->getFScore())
{
current = (*i);
}
}
// Stop if we reached the end
if (current == end)
{
break;
}
// Remove the current point from the openList
openList.remove(current);
current->opened = false;
// Add the current point to the closedList
closedList.push_back(current);
current->closed = true;
// Get all current's adjacent walkable points
for (int x = 5; x < 10; x ++)
{
for (int y = 5; y < 10; y ++)
{
// If it's current point then pass
if (x == 0 && y == 0)
{
continue;
}
// Get this point
child = getPoint(current->getX() + x, current->getY() + y);
// If it's closed or not walkable then pass
if (child->closed || !child->walkable)
{
continue;
}
// If we are at a corner, do a barrel roll
if (x != 0 && y != 0)
{
// if the next horizontal point is not walkable or in the closed list then pass
if (!pointIsWalkable(current->getX(), current->getY() + y) || getPoint(current->getX(), current->getY() + y)->closed)
{
continue;
}
// if the next vertical point is not walkable or in the closed list then pass
if (!pointIsWalkable(current->getX() + x, current->getY()) || getPoint(current->getX() + x, current->getY())->closed)
{
continue;
}
}
// If it's already in the openList
if (child->opened)
{
// If it has a worse g score than the one that pass through the current point
// then its path is improved when it's parent is the current point
if (child->getGScore() > child->getGScore(current))
{
// Change its parent and g score
child->setParent(current);
child->computeScores(end);
}
}
else
{
// Add it to the openList with current point as parent
openList.push_back(child);
child->opened = true;
// Compute it's g, h and f score
child->setParent(current);
child->computeScores(end);
}
}
}
n ++;
}
// Reset
for (i = openList.begin(); i != openList.end(); ++ i)
{
(*i)->opened = false;
}
for (i = closedList.begin(); i != closedList.end(); ++ i)
{
(*i)->closed = false;
}
// Resolve the path starting from the end point
while (current->hasParent() && current != start)
{
path.push_back(current->getPosition());
current = current->getParent();
n ++;
}
return path;
}
|