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; }