13 #include <unordered_map>
21 template <
class state,
class action,
class environment,
class dataStructure = DVCBSQueue<state>,
22 class priorityQueue = DVCBSOpenClosed<state> >
25 DVCBS(
int tieBreaking,
bool allSolutions =
false,
bool leq =
false)
32 DVCBS(
bool allSolutions =
false,
bool leq =
false)
40 void GetPath(environment *
env,
const state& from,
const state& to,
69 virtual const char *
GetName() {
return "DVCBS"; }
83 auto l =
queue.forwardQueue.Lookup(
env->GetStateHash(s), childID);
89 return queue.backwardQueue.Lookup(
env->GetStateHash(s), childID);
95 auto l =
queue.forwardQueue.Lookup(
env->GetStateHash(s), childID);
97 return queue.forwardQueue.Lookat(childID).g;
104 auto l =
queue.backwardQueue.Lookup(
env->GetStateHash(s), childID);
106 return queue.backwardQueue.Lookat(childID).g;
113 uint64_t necessary = 0;
114 for (
const auto &i :
counts)
142 thePath.push_back(
queue.backwardQueue.Lookup(
node).data);
149 thePath.push_back(
queue.backwardQueue.Lookup(
node).data);
159 cost +=
queue.backwardQueue.Lookup(
node).g;
162 cost +=
queue.backwardQueue.Lookup(
node).g;
171 thePath.push_back(
queue.forwardQueue.Lookup(
node).data);
178 thePath.push_back(
queue.forwardQueue.Lookup(
node).data);
187 cost +=
queue.forwardQueue.Lookup(
node).g;
188 printf(
"cost: %d ",cost);
191 cost +=
queue.forwardQueue.Lookup(
node).g;
197 void Expand(uint64_t nextID,
198 priorityQueue ¤t,
199 priorityQueue &opposite,
232 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
236 if (InitializeSearch(env, from, to, forward, backward, thePath) ==
false)
239 while (!ExpandAVertexCover(thePath))
243 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
246 std::vector<state> &thePath)
249 forwardHeuristic = forward;
250 backwardHeuristic = backward;
251 currentSolutionEstimate = 0;
252 forwardUnnecessaryNodesInPath = 0;
253 backwardUnnecessaryNodesInPath = 0;
254 forwardMeetingPoint = 0;
255 backwardMeetingPoint = 0;
256 currentCost = DBL_MAX;
257 expansionsUntilSolution = 0;
268 queue.forwardQueue.AddOpenNode(start, env->GetStateHash(start), 0, forwardHeuristic->HCost(start, goal));
269 queue.backwardQueue.AddOpenNode(goal, env->GetStateHash(goal), 0, backwardHeuristic->HCost(goal, start));
274 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
277 std::vector<uint64_t> nForward, nBackward;
278 bool result = queue.getVertexCover(nForward, nBackward,tieBreakingPolicy);
282 if (currentCost == DBL_MAX)
287 ExtractFromMiddle(thePath);
292 if ((!isAllSolutions && !
fless(queue.GetLowerBound(), currentCost)) || (isAllSolutions && !
flesseq(queue.GetLowerBound(), currentCost))){
293 ExtractFromMiddle(thePath);
297 else if (nForward.size() > 0 &&
300 std::unordered_map<state*,bool> mapData;
301 for (
int i =0; i< nForward.size(); i++){
302 mapData[&(queue.forwardQueue.Lookup(nForward[i]).data)] =
true;
304 for (
int j =0; j< nBackward.size(); j++){
305 if (mapData.find(&(queue.backwardQueue.Lookup(nBackward[j]).data)) != mapData.end()){
306 ExtractFromMiddle(thePath);
312 struct compareBackward {
313 compareBackward(dataStructure currQueue) : queue(currQueue) {}
314 bool operator () (uint64_t i, uint64_t j) {
return (queue.backwardQueue.Lookup(i).h<queue.backwardQueue.Lookup(j).h); }
317 struct compareForward {
318 compareForward(dataStructure currQueue) : queue(currQueue) {}
319 bool operator () (uint64_t i, uint64_t j) {
return (queue.forwardQueue.Lookup(i).h<queue.forwardQueue.Lookup(j).h); }
322 double currentLowerBound = queue.GetLowerBound();
324 if (nForward.size() == 0){
325 for (
int j =0; j< ((int)nBackward.size());j++){
326 double oldKey = queue.backwardQueue.getFirstKey(
kOpenReady);
327 if (queue.backwardQueue.Lookup(nBackward[j]).where !=
kClosed){
328 counts[currentLowerBound]++;
329 Expand(nBackward[j], queue.backwardQueue, queue.forwardQueue, backwardHeuristic, start);
331 if ((!isAllSolutions && !
fless(queue.GetLowerBound(), currentCost)) || (isAllSolutions && !
flesseq(queue.GetLowerBound(), currentCost))){
332 ExtractFromMiddle(thePath);
335 if (currentLowerBound != queue.GetLowerBound() || oldKey != queue.backwardQueue.getFirstKey(
kOpenReady)){
341 else if (nBackward.size() == 0){
342 for (
int i =0; i< ((int)nForward.size());i++){
343 double oldKey = queue.forwardQueue.getFirstKey(
kOpenReady);
344 if (queue.forwardQueue.Lookup(nForward[i]).where !=
kClosed){
345 counts[currentLowerBound]++;
346 Expand(nForward[i], queue.forwardQueue, queue.backwardQueue, forwardHeuristic, goal);
348 if ((!isAllSolutions && !
fless(queue.GetLowerBound(), currentCost)) || (isAllSolutions && !
flesseq(queue.GetLowerBound(), currentCost))){
349 ExtractFromMiddle(thePath);
352 if (currentLowerBound != queue.GetLowerBound() || oldKey != queue.forwardQueue.getFirstKey(
kOpenReady)){
358 int i = nForward.size()-1;
359 int j = nBackward.size()-1;
360 while (i >= 0 || j >=0 ){
361 if ((!isAllSolutions && !
fless(queue.GetLowerBound(), currentCost)) || (isAllSolutions && !
flesseq(queue.GetLowerBound(), currentCost)))
363 ExtractFromMiddle(thePath);
368 expandForward =
false;
371 expandForward =
true;
374 if (queue.forwardQueue.Lookup(nForward[i]).g >= queue.backwardQueue.Lookup(nBackward[j]).g){
375 expandForward =
true;
378 expandForward =
false;
382 if (queue.forwardQueue.Lookup(nForward[i]).where !=
kClosed){
383 counts[currentLowerBound]++;
384 Expand(nForward[i], queue.forwardQueue, queue.backwardQueue, forwardHeuristic, goal);
389 if (queue.backwardQueue.Lookup(nBackward[j]).where !=
kClosed){
390 counts[currentLowerBound]++;
391 Expand(nBackward[j], queue.backwardQueue, queue.forwardQueue, backwardHeuristic, start);
395 if (currentLowerBound != queue.GetLowerBound()){
404 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
408 printf(
"cost1: %d",cost);
409 cost += ExtractCostToGoal(middleNode);
410 printf(
"cost2: %d",cost);
411 cost += ExtractCostToStart(middleNode);
412 printf(
"cost3: %d",cost);
416 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
420 std::vector<state> pFor, pBack;
421 ExtractPathToGoal(middleNode, pBack);
422 ExtractPathToStart(middleNode, pFor);
423 reverse(pFor.begin(), pFor.end());
425 thePath.insert( thePath.end(), pBack.begin()+1, pBack.end() );
428 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
431 return ExpandAVertexCover(thePath);
435 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
437 priorityQueue ¤t,
438 priorityQueue &opposite,
441 if (current.Lookup(nextID).where ==
kClosed){
445 uint64_t tmp = current.CloseAtIndex(nextID);
446 assert(tmp == nextID);
449 if ( (!isAllSolutions &&
fgreatereq(current.Lookup(nextID).g + current.Lookup(nextID).h, currentCost)) || (isAllSolutions &&
fgreater(current.Lookup(nextID).g + current.Lookup(nextID).h, currentCost))){
455 env->GetSuccessors(current.Lookup(nextID).data, neighbors);
456 for (
auto &succ : neighbors)
460 auto loc = current.Lookup(env->GetStateHash(succ), childID);
463 double edgeCost = env->GCost(current.Lookup(nextID).data, succ);
464 if ( (!isAllSolutions &&
fgreatereq(current.Lookup(nextID).g+edgeCost, currentCost)) || (isAllSolutions &&
fgreater(current.Lookup(nextID).g+edgeCost, currentCost))){
475 if (
fless(current.Lookup(nextID).g+edgeCost, current.Lookup(childID).g))
477 double oldGCost = current.Lookup(childID).g;
478 current.Lookup(childID).parentID = nextID;
479 current.Lookup(childID).g = current.Lookup(nextID).g+edgeCost;
481 current.KeyChanged(childID,oldGCost+current.Lookup(childID).h);
484 current.KeyChanged(childID,oldGCost);
488 auto loc = opposite.Lookup(env->GetStateHash(succ), reverseLoc);
491 if (
fless(current.Lookup(nextID).g+edgeCost + opposite.Lookup(reverseLoc).g, currentCost))
493 currentCost = current.Lookup(nextID).g+edgeCost + opposite.Lookup(reverseLoc).g;
494 expansionsUntilSolution = nodesExpanded;
500 current.Remove(childID);
508 auto loc = opposite.Lookup(env->GetStateHash(succ), reverseLoc);
515 double newNodeF = current.Lookup(nextID).g + edgeCost + heuristic->
HCost(succ, target);
516 if ((!isAllSolutions &&
fless(newNodeF , currentCost)) || (isAllSolutions &&
flesseq(newNodeF , currentCost)))
519 if (
fless(newNodeF , queue.GetLowerBound()) || (isLEQ &&
flesseq(newNodeF , queue.GetLowerBound())))
520 current.AddOpenNode(succ,
521 env->GetStateHash(succ),
522 current.Lookup(nextID).g + edgeCost,
523 heuristic->
HCost(succ, target),
526 current.AddOpenNode(succ,
527 env->GetStateHash(succ),
528 current.Lookup(nextID).g + edgeCost,
529 heuristic->
HCost(succ, target),
534 if (
fless(current.Lookup(nextID).g + edgeCost + opposite.Lookup(reverseLoc).g, currentCost))
536 currentCost = current.Lookup(nextID).g + edgeCost + opposite.Lookup(reverseLoc).g;
537 expansionsUntilSolution = nodesExpanded;
552 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
555 uint64_t doubles = 0;
556 for (
unsigned int x = 0; x < queue.forwardQueue.size(); x++)
559 const auto &data = queue.forwardQueue.Lookat(x);
562 auto loc = queue.backwardQueue.Lookup(env->GetStateHash(data.data), key);
571 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
574 OpenGLDraw(queue.forwardQueue);
575 OpenGLDraw(queue.backwardQueue);
578 template <
class state,
class action,
class environment,
class dataStructure,
class priorityQueue>
582 double transparency = 0.9;
591 for (
unsigned int x = 0; x < q.size(); x++)
593 const auto &data = q.Lookat(x);
601 env->SetColor(0.0, 0.5, 0.5, transparency);
602 env->OpenGLDraw(data.data);
606 env->SetColor(0.0, 1.0, 0.0, transparency);
607 env->OpenGLDraw(data.data);
609 else if (data.where ==
kClosed)
611 if (&q == &queue.backwardQueue)
612 env->SetColor(1.0, 0.0, 1.0, transparency);
614 env->SetColor(1.0, 0.0, 0.0, transparency);
615 env->OpenGLDraw(data.data);