/*! \author Luxor */ cPath::cPath( Location startPos, Location finalPos, P_CHAR pc ) { if ( pc == NULL ) pc_serial = INVALID; else pc_serial = pc->getSerial32(); open_list.clear(); closed_list.clear(); path_list.clear(); m_pathFound = false; m_startPos = startPos; m_finalPos = finalPos; // Create the start node and put it in the open list // path_node startNode = { 0, startPos, NULL }; --> does not compile on win vc++ path_node startNode; path_node *sNode; startNode.cost = 0; startNode.pos = startPos; startNode.parentNode = &startNode; nodes_vector.push( startNode ); sNode = &(nodes_vector.back()); addToOpenList( sNode ); currNode = nextNode = sNode; }
const Node* explore(const State& startState, TerminationChecker& terminationChecker) { ++iterationCounter; clearOpenList(); openList.reorder(fComparator); Planner::incrementGeneratedNodeCount(); Node*& startNode = nodes[startState]; if (startNode == nullptr) { startNode = nodePool->construct(Node{nullptr, startState, Action(), 0, domain.heuristic(startState), true}); } else { startNode->g = 0; startNode->action = Action(); startNode->predecessors.clear(); startNode->parent = nullptr; } startNode->iteration = iterationCounter; addToOpenList(*startNode); while (!terminationChecker.reachedTermination() && openList.isNotEmpty()) { Node* const currentNode = popOpenList(); if (domain.isGoal(currentNode->state)) { return currentNode; } terminationChecker.notifyExpansion(); expandNode(currentNode); } return openList.top(); }
void expandNode(Node* sourceNode) { Planner::incrementExpandedNodeCount(); for (auto successor : domain.successors(sourceNode->state)) { auto successorState = successor.state; Node*& successorNode = nodes[successorState]; if (successorNode == nullptr) { successorNode = createNode(sourceNode, successor); if (successorNode->parent->parent == nullptr) { // its the root node [start node] mark top level actions // successorNode->topLevelAction = successorNode->action; // safeTopLevelActionNodes.push_back(successorNode); } checkSafeNode(successorNode); if (successorNode->parent != nullptr) { // as long as the parent isn't null if (successorNode->parent->parent == nullptr) { // if we're marking TLAs successorNode->topLevelAction = successorNode->action; // give them their action } else { // otherwise we're marking non TLAs successorNode->topLevelAction = successorNode->parent->topLevelAction; // set the node's TLA to its parent's } } } // If the node is outdated it should be updated. if (successorNode->iteration != iterationCounter) { successorNode->iteration = iterationCounter; successorNode->predecessors.clear(); successorNode->g = Domain::COST_MAX; successorNode->open = false; // It is not on the open list yet, but it will be // parent, action, and actionCost is outdated too, but not relevant. } // Add the current state as the predecessor of the child state successorNode->predecessors.emplace_back(sourceNode, successor.action, successor.actionCost); // Skip if we got back to the parent if (sourceNode->parent != nullptr && successorState == sourceNode->parent->state) { continue; } // only generate those state that are not visited yet or whose cost value are lower than this path Cost successorGValueFromCurrent{sourceNode->g + successor.actionCost}; if (successorNode->g > successorGValueFromCurrent) { successorNode->g = successorGValueFromCurrent; successorNode->parent = sourceNode; successorNode->action = successor.action; if (!successorNode->open) { addToOpenList(*successorNode); } else { openList.update(*successorNode); } } } }
Node* explore(const State& startState, TerminationChecker& terminationChecker) { ++iterationCounter; clearOpenList(); openList.reorder(fComparator); Planner::incrementGeneratedNodeCount(); Node*& startNode = nodes[startState]; if (startNode == nullptr) { startNode = nodePool->construct(Node{nullptr, startState, Action(), 0, domain.heuristic(startState), true}); } else { startNode->g = 0; startNode->action = Action(); startNode->predecessors.clear(); startNode->parent = nullptr; } startNode->iteration = iterationCounter; addToOpenList(*startNode); Node* currentNode = startNode; checkSafeNode(currentNode); while (!terminationChecker.reachedTermination() && !domain.isGoal(currentNode->state)) { // if (domain.safetyPredicate(currentNode->state)) { // try to find nodes which lead to safety // currentNode = popOpenList(); // terminationChecker.notifyExpansion(); // expandNode(currentNode); // } // // if (currentNode == startNode) { // if we can't just do LSS-LRTA* // while (!terminationChecker.reachedTermination() && !domain.isGoal(currentNode->state)) { // currentNode = popOpenList(); // terminationChecker.notifyExpansion(); // expandNode(currentNode); // } // } Node* const currentNode = popOpenList(); if (domain.isGoal(currentNode->state)) { return currentNode; } terminationChecker.notifyExpansion(); expandNode(currentNode); } return openList.top(); }
void expandNode(Node* sourceNode) { Planner::incrementExpandedNodeCount(); for (auto successor : domain.successors(sourceNode->state)) { auto successorState = successor.state; Node*& successorNode = nodes[successorState]; if (successorNode == nullptr) { successorNode = createNode(sourceNode, successor); } // If the node is outdated it should be updated. if (successorNode->iteration != iterationCounter) { successorNode->iteration = iterationCounter; successorNode->predecessors.clear(); successorNode->g = Domain::COST_MAX; successorNode->open = false; // It is not on the open list yet, but it will be // parent, action, and actionCost is outdated too, but not relevant. } // Add the current state as the predecessor of the child state successorNode->predecessors.emplace_back(sourceNode, successor.action, successor.actionCost); // Skip if we got back to the parent if (sourceNode->parent != nullptr && successorState == sourceNode->parent->state) { continue; } // only generate those state that are not visited yet or whose cost value are lower than this path Cost successorGValueFromCurrent{sourceNode->g + successor.actionCost}; if (successorNode->g > successorGValueFromCurrent) { successorNode->g = successorGValueFromCurrent; successorNode->parent = sourceNode; successorNode->action = successor.action; if (!successorNode->open) { addToOpenList(*successorNode); } else { openList.update(*successorNode); } } } }
void learn(const TerminationChecker& terminationChecker) { ++iterationCounter; // Reorder the open list based on the heuristic values openList.reorder(hComparator); sweepBackSafety(); while (!terminationChecker.reachedTermination() && openList.isNotEmpty()) { auto currentNode = popOpenList(); currentNode->iteration = iterationCounter; Cost currentHeuristicValue = currentNode->h; // update heuristic actionDuration of each predecessor for (auto predecessor : currentNode->predecessors) { Node* predecessorNode = predecessor.predecessor; if (predecessorNode->iteration == iterationCounter && !predecessorNode->open) { // This node was already learned and closed in the current iteration continue; // TODO Review this. This could be incorrect if the action costs are not uniform } if (!predecessorNode->open) { // This node is not open yet, because it was not visited in the current planning iteration predecessorNode->h = currentHeuristicValue + predecessor.actionCost; assert(predecessorNode->iteration == iterationCounter - 1); predecessorNode->iteration = iterationCounter; addToOpenList(*predecessorNode); } else if (predecessorNode->h > currentHeuristicValue + predecessor.actionCost) { // This node was visited in this learning phase, but the current path is better then the previous predecessorNode->h = currentHeuristicValue + predecessor.actionCost; openList.update(*predecessorNode); } } } }
std::vector<goap::Action> goap::AStar::plan(const WorldState& start, const WorldState& goal, const std::vector<Action>& actions) { // Feasible we'd re-use a planner, so clear out the prior results open_.clear(); closed_.clear(); known_nodes_.clear(); Node starting_node(start, 0, calculateHeuristic(start, goal), 0, nullptr); // TODO figure out a more memory-friendly way of doing this... known_nodes_[starting_node.id_] = starting_node; open_.push_back(std::move(starting_node)); //int iters = 0; while (open_.size() > 0) { //++iters; //std::cout << "\n-----------------------\n"; //std::cout << "Iteration " << iters << std::endl; // Look for Node with the lowest-F-score on the open list. Switch it to closed, // and hang onto it -- this is our latest node. Node& current(popAndClose()); //std::cout << "Open list\n"; //printOpenList(); //std::cout << "Closed list\n"; //printClosedList(); //std::cout << "\nCurrent is " << current << std::endl; // Is our current state the goal state? If so, we've found a path, yay. if (current.ws_.meetsGoal(goal)) { std::vector<Action> the_plan; do { the_plan.push_back(*current.action_); current = known_nodes_.at(current.parent_id_); } while (current.parent_id_ != 0); return the_plan; } // Check each node REACHABLE from current for (const auto& action : actions) { if (action.eligibleFor(current.ws_)) { WorldState possibility = action.actOn(current.ws_); //std::cout << "Hmm, " << action.name() << " could work..." << "resulting in " << possibility << std::endl; // Skip if already closed if (memberOfClosed(possibility)) { //std::cout << "...but that one's closed out.\n"; continue; } auto needle = memberOfOpen(possibility); if (needle==end(open_)) { // not a member of open list // Make a new node, with current as its parent, recording G & H Node found(possibility, current.g_ + action.cost(), calculateHeuristic(possibility, goal), current.id_, &action); known_nodes_[found.id_] = found; // Add it to the open list (maintaining sort-order therein) addToOpenList(std::move(found)); } else { // already a member of the open list // check if the current G is better than the recorded G if ((current.g_ + action.cost()) < needle->g_) { //std::cout << "My path is better\n"; needle->parent_id_ = current.id_; // make current its parent needle->g_ = current.g_ + action.cost(); // recalc G & H needle->h_ = calculateHeuristic(possibility, goal); std::sort(open_.begin(), open_.end()); // resort open list to account for the new F } } } } } // If there's nothing left to evaluate, then we have no possible path left throw std::runtime_error("A* planner could not find a path from start to goal"); }
/*! \author Luxor \brief Adds an element to the open list */ void cPath::addToOpenList( Location pos, path_node* parentNode, UI32 cost ) { path_node* node = create_node( pos, parentNode, cost ); addToOpenList( node ); }
/*! \author Luxor \brief Looks for every tile reachable walking by pos, and adds them to the open list */ UI08 cPath::addReachableNodes( path_node* node ) { P_CHAR pc = pointers::findCharBySerial( pc_serial ); LOGICAL bWalkable[ 4 ]; UI08 num = 0; SI08 zAdd = 0; Location loc; Location pos = node->pos; // North - 0 loc = Loc( pos.x, pos.y - 1, pos.z ); if ( (zAdd = isWalkable( loc, WALKFLAG_ALL, pc )) != illegal_z ) { bWalkable[ 0 ] = true; loc.z = zAdd; addToOpenList( loc, node, STRAIGHT_COST ); num++; } else bWalkable[ 0 ] = false; // South - 1 loc = Loc( pos.x, pos.y + 1, pos.z ); if ( (zAdd = isWalkable( loc, WALKFLAG_ALL, pc )) != illegal_z ) { bWalkable[ 1 ] = true; loc.z = zAdd; addToOpenList( loc, node, STRAIGHT_COST ); num++; } else bWalkable[ 1 ] = false; // East - 2 loc = Loc( pos.x + 1, pos.y, pos.z ); if ( (zAdd = isWalkable( loc, WALKFLAG_ALL, pc )) != illegal_z ) { bWalkable[ 2 ] = true; loc.z = zAdd; addToOpenList( loc, node, STRAIGHT_COST ); num++; } else bWalkable[ 2 ] = false; // West - 3 loc = Loc( pos.x - 1, pos.y, pos.z ); if ( (zAdd = isWalkable( loc, WALKFLAG_ALL, pc )) != illegal_z ) { bWalkable[ 3 ] = true; loc.z = zAdd; addToOpenList( loc, node, STRAIGHT_COST ); num++; } else bWalkable[ 3 ] = false; // North-East loc = Loc( pos.x + 1, pos.y - 1, pos.z ); if ( bWalkable[0] && bWalkable[2] ) { // Avoid to go near a tile angle if ( (zAdd = isWalkable( loc, WALKFLAG_ALL, pc )) != illegal_z ) { loc.z = zAdd; addToOpenList( loc, node, OBLIQUE_COST ); num++; } } // North-West loc = Loc( pos.x - 1, pos.y - 1, pos.z ); if ( bWalkable[0] && bWalkable[3] ) { // Avoid to go near a tile angle if ( (zAdd = isWalkable( loc, WALKFLAG_ALL, pc )) != illegal_z ) { loc.z = zAdd; addToOpenList( loc, node, OBLIQUE_COST ); num++; } } // South-East loc = Loc( pos.x + 1, pos.y + 1, pos.z ); if ( bWalkable[1] && bWalkable[2] ) { // Avoid to go near a tile angle if ( (zAdd = isWalkable( loc, WALKFLAG_ALL, pc )) != illegal_z ) { loc.z = zAdd; addToOpenList( loc, node, OBLIQUE_COST ); num++; } } // South-West loc = Loc( pos.x - 1, pos.y + 1, pos.z ); if ( bWalkable[1] && bWalkable[3] ) { // Avoid to go near a tile angle if ( (zAdd = isWalkable( loc, WALKFLAG_ALL, pc )) != illegal_z ) { loc.z = zAdd; addToOpenList( loc, node, OBLIQUE_COST ); num++; } } return num; }