/*!
\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;
}
