int GOAPAstar::calculateH(WorldState from, WorldState to)
{
auto care = to.GetCare();
auto diff = ( ( from.GetFlags() & care ) ^ ( to.GetFlags() & care ) );
int distance = 0;
for (int i = 0; i < StateType::STATE_NUM; ++i)
if ( ( diff & ( 1LL << i ) ) != 0 )
++distance;
return distance;
}
void GOAPAstar::Plan(GOAPlanner *ap)
{
// clear open and closed lists
_open.clear();
_closed.clear();
// TODO: Early out if _current == _desired, add plan WANDER
WorldState goal = ap->_desired;
// put start in the open list
astarnode start;
start.ws = ap->_current;
start.parent_ws = ap->_current;
start.g = 0;
start.h = calculateH(ap->_current, goal);
start.f = start.g + start.h;
start.action_name = "";
_open.push_back(start);
for (;;)
{
if (_open.size() == 0)
return;
// find the node with the lowest rank
astarnode curr = openPopLowest();
auto care = goal.GetCare();
bool match = ((curr.ws.GetFlags() & care) == (goal.GetFlags() & care));
// if we've reached our goal state
if (match)
{
reconstructPlan(ap, &curr);
// Success
return;
}
// add current to closed
_closed.push_back(curr);
// fill the transitions array
getPossibleStateTransitions(ap, curr.ws);
// iterate over all possible transitions
for (auto &pair : _transitions)
{
AIAction &action = pair.first;
WorldState &future = pair.second;
astarnode neighbor;
int cost = curr.g + action.GetCost();
int open_index = nodeInOpened(future);
int close_index = nodeInClosed(future);
// if neighbor is in OPEn and cost less than g(neighbor)
if (open_index >= 0 && cost < _open[open_index].g)
{
// remove neighbor from OPEN, because new patch is better
_open.erase(_open.begin() + open_index);
open_index = -1;
}
// if neighbor in CLOSED and cost less than g(neighbor)
if (close_index >= 0 && cost < _closed[close_index].g)
{
// remove neighbor from CLOSED
_closed.erase(_closed.begin() + close_index);
}
// if neighbor not in OPEN and neighbor not in CLOSED
if (close_index == -1 && open_index == -1)
{
neighbor.ws = future;
neighbor.g = cost;
neighbor.h = calculateH(neighbor.ws, goal);
neighbor.f = neighbor.g + neighbor.h;
neighbor.action_name = action.GetName();
neighbor.parent_ws = curr.ws;
_open.push_back(neighbor);
}
}
}
return;
}
