void search(ContractorHeap &heap,
const ContractorGraph &graph,
const unsigned number_of_targets,
const int node_limit,
const EdgeWeight weight_limit,
const NodeID forbidden_node)
{
int nodes = 0;
unsigned number_of_targets_found = 0;
while (!heap.Empty())
{
const NodeID node = heap.DeleteMin();
BOOST_ASSERT(node != SPECIAL_NODEID);
const auto node_weight = heap.GetKey(node);
if (++nodes > node_limit)
{
return;
}
if (node_weight > weight_limit)
{
return;
}
// Destination settled?
if (heap.GetData(node).target)
{
++number_of_targets_found;
if (number_of_targets_found >= number_of_targets)
{
return;
}
}
relaxNode(heap, graph, node, node_weight, forbidden_node);
}
}
void
FlexibleAStar::expand(node* current, node* goal, altheap* openList,
std::map<int, node*>* closedList)
{
// expand the current node
if(verbose)
{
double fVal = current->getLabelF(kTemporaryLabel);
double gVal = fVal - heuristic->h(current, goal);
debug->printNode(std::string("expanding... "), current);
std::cout << " g: "<<gVal<<" f: "<<fVal<<std::endl;
}
nodesExpanded++;
nodesTouched++;
policy->expand(current);
for(node* neighbour = policy->first(); neighbour != 0;
neighbour = policy->next())
{
nodesTouched++;
if(closedList->find(neighbour->getUniqueID()) == closedList->end())
{
if(openList->isIn(neighbour))
{
double fVal = neighbour->getLabelF(kTemporaryLabel);
relaxNode(current, neighbour, goal, policy->cost_to_n(), openList);
if(verbose)
{
if(neighbour->getLabelF(kTemporaryLabel) < fVal)
{
debug->printNode("\trelaxing...", neighbour);
std::cout << " gOld: "<<
(fVal - heuristic->h(neighbour, goal)) <<
" fOld: "<< fVal;
double fVal = neighbour->getLabelF(kTemporaryLabel);
std::cout << " gNew: " <<
(fVal - heuristic->h(neighbour, goal)) <<
" fNew: "<<neighbour->getLabelF(kTemporaryLabel);
}
else
{
debug->printNode("\tcannot relax node...", neighbour);
}
}
}
else
{
if(verbose)
debug->printNode("\tgenerating...", neighbour);
neighbour->setLabelF(kTemporaryLabel, MAXINT); // initial fCost
neighbour->setKeyLabel(kTemporaryLabel); // store priority here
neighbour->backpointer = 0; // reset any marked edges
openList->add(neighbour);
relaxNode(current, neighbour, goal, policy->cost_to_n(), openList);
nodesGenerated++;
if(verbose)
{
double fVal = neighbour->getLabelF(kTemporaryLabel);
std::cout << " g: "<<(fVal - heuristic->h(neighbour, goal))
<< " fNew: "<< fVal;
}
}
if(markForVis)
neighbour->drawColor = 1; // visualise touched
}
else
{
if(verbose)
{
debug->printNode("\tclosed...", neighbour);
double fCur = current->getLabelF(kTemporaryLabel);
double gCur = fCur - heuristic->h(current, goal);
double gAlt = gCur + policy->cost_to_n();
double fAlt = gAlt + heuristic->h(neighbour, goal);
double fClosed = neighbour->getLabelF(kTemporaryLabel);
std::cout << " (fClosed: "<<fClosed<<"; fAlt: "<<fAlt<<")";
}
debug->debugClosedNode(current, neighbour, policy->cost_to_n(), goal);
}
if(verbose)
std::cout << std::endl;
}
}
