SHARE
TWEET

Untitled

a guest Mar 18th, 2019 63 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "isearch.h"
  2. #include <algorithm>
  3.  
  4. ISearch::ISearch()
  5. {
  6.     hweight = 1;
  7.     breakingties = CN_SP_BT_GMAX;
  8. }
  9.  
  10. ISearch::~ISearch(void) {}
  11.  
  12.  
  13. SearchResult ISearch::startSearch(ILogger *Logger, const Map &map, const EnvironmentOptions &options)
  14. {
  15.     Node curNode, stNode, fnNode, *prevNode;
  16.     stNode.i = map.getStart().first;
  17.     stNode.j = map.getStart().second;
  18.     fnNode.i = map.getGoal().first;
  19.     fnNode.j = map.getGoal().second;
  20.     stNode.H = computeHFromCellToCell(stNode.i, stNode.j, fnNode.i, fnNode.j, options);
  21.     stNode.g = 0;
  22.     stNode.F = stNode.H * ISearch::hweight;
  23.     ISearch::open.push_back(stNode);
  24.     while (!ISearch::open.empty()) {
  25.         sresult.numberofsteps++;
  26.         curNode = ISearch::open.back();
  27.         prevNode = &curNode;
  28.         ISearch::open.pop_back();
  29.         ISearch::close.push_back(curNode);
  30.         if (curNode == fnNode) {
  31.             sresult.pathfound = true;
  32.             sresult.nodescreated = ISearch::close.size() + ISearch::open.size();
  33.             while (!(curNode == stNode)) {
  34.                 lppath.push_front(curNode);
  35.                 curNode = *curNode.parent;
  36.             }
  37.         } else {
  38.             for (auto neiNode : ISearch::findSuccessors(curNode, map, options)) {
  39.                 if (std::find(ISearch::open.begin(), ISearch::open.end(), neiNode) == ISearch::open.end() &&
  40.                     std::find(ISearch::close.begin(), ISearch::close.end(), neiNode) == ISearch::close.end()) {
  41.                     neiNode.H = computeHFromCellToCell(neiNode.i, neiNode.j, fnNode.i, fnNode.j, options);
  42.                     neiNode.g = curNode.g + 1;
  43.                     neiNode.parent = new Node(&curNode);
  44.                     neiNode.F = neiNode.H * ISearch::hweight + neiNode.g;
  45.                     ISearch::open.push_back(neiNode);
  46.                 }
  47.             }
  48.             std::sort(ISearch::open.begin(), ISearch::open.end(), [this](const Node & a, const Node & b) {
  49.                 return (a.F > b.F || (a.F == b.F && ISearch::breakingties == (a.H > b.H)));
  50.             });
  51.         }
  52.     }
  53.     //sresult.hppath = &hppath; //Here is a constant pointer
  54.     sresult.lppath = &lppath;
  55.     return sresult;
  56. }
  57.  
  58. std::vector<Node> ISearch::findSuccessors(Node curNode, const Map &map, const EnvironmentOptions &options)
  59. {
  60.     std::vector<Node> successors;
  61.     Node sucNode;
  62.     sucNode.i = curNode.i - 1;
  63.     sucNode.j = curNode.j;
  64.     for (size_t i = 0; i < 4; ++i){
  65.         if (map.CellOnGrid(sucNode.i, sucNode.j))
  66.             if (map.CellIsTraversable(sucNode.i, sucNode.j))
  67.                 successors.push_back(sucNode);
  68.         sucNode.i += (i < 2 ? 1 : -1);
  69.         sucNode.j += ((0 < i && i < 3) ? 1 : -1);
  70.     }
  71.     return successors;
  72. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top