daily pastebin goal
0%
SHARE
TWEET

Untitled

a guest May 29th, 2012 349 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // Create by Ofer Rubinstein for Sumerian Blood.
  2. // Code license: CC-BY
  3. // Origin of blog post http://pompidev.net/2012/05/29/a-practical-overview-of-awith-code/
  4.  
  5. struct AScore {
  6.         public:
  7.                 AScore()
  8.                 {
  9.                         g = 0;
  10.                         h = 0;
  11.                         f = 0;
  12.                 }
  13.                 double g, h, f;
  14. };
  15.  
  16. struct ACell {
  17.         ACell()
  18.         {
  19.                 x = 0;
  20.                 y = 0;
  21.                 Enable = false;
  22.         }
  23.         bool operator<(const ACell & d) const
  24.         {
  25.                 return s.f>d.s.f;
  26.         }
  27.  
  28.         AScore s;
  29.         bool Enable;
  30.         unsigned int x, y;
  31. };
  32.  
  33. struct Vector {
  34.         Vector()
  35.         {
  36.                 x = 0;
  37.                 y = 0;
  38.         }
  39.         void Normalize()
  40.         {
  41.                 double l = sqrt(x*x+y*y);
  42.                 if (l>0.000001)
  43.                 {
  44.                         x/=l;
  45.                         y/=l;
  46.                 }
  47.                 else
  48.                 {
  49.                         x = 0;
  50.                         y = 0;
  51.                 }
  52.         }
  53.         double Length()
  54.         {
  55.                 return sqrt(x*x+y*y);
  56.         }
  57.        
  58.         double AngleWith (Vector v)
  59.         {
  60.                 double a = fabs(atan2(y, x)-atan2(v.y, v.x));
  61.                 if (a>Pi)
  62.                         a=2*Pi-a;
  63.                 return a;
  64.         }
  65.  
  66.         double SignAngleWith (Vector v)
  67.         {
  68.                 return atan2(y, x)-atan2(v.y, v.x);
  69.         }
  70.         double x;
  71.         double y;
  72. };
  73.  
  74. class AStar {
  75.         public:
  76.                 // A heuristic interface that you can implement and provide to the AStar for different search priorities
  77.                 class Heuristic {
  78.                         public:
  79.                                 // Called before the A* search begins
  80.                                 virtual void Start(ACell First, ACell Goal) = 0;
  81.                                 // Calculates the estimated distance to the goal
  82.                                 virtual double Calculate (ACell w1, ACell w2, double Factor) = 0;
  83.                                 // Return true if reaching the "goal" parameter provided to find means we can stop the search
  84.                                 virtual bool IsGoal() {return true;};
  85.                                 // Returns true if we should stop the search
  86.                                 virtual bool IsStop (ACell w, double Factor) = 0;
  87.                                 // Returns true if we have failed to achieve our goal(works in conjuction with IsStop)
  88.                                 virtual bool IsCancel() = 0;
  89.                                 // Called every iteration of the search
  90.                                 virtual void Step() = 0;
  91.                                 virtual ~Heuristic(){};
  92.                 };
  93.  
  94.                 // ctr
  95.                 AStar()
  96.                 {
  97.                         Ready = false;
  98.                         Factor = 1;
  99.                         h = NULL;
  100.                 }
  101.  
  102.                 // Setting the heuristic to use in the search
  103.                 void SetHeuristic (Heuristic & h)
  104.                 {
  105.                         this->h = &h;
  106.                 }
  107.  
  108.                 // Was the AStar instance initialized?
  109.                 bool IsReady()
  110.                 {
  111.                         return Ready;
  112.                 }
  113.  
  114.                 // Initialize the AStar with the field or "maze's" dimensions, and also a factor to scale doing the search map for performance.
  115.                 void Initialize (unsigned int w, unsigned int h, double Factor)
  116.                 {
  117.                         Ready = true;
  118.                         this->Factor = Factor;
  119.                         unsigned int fh = h/Factor;
  120.                         unsigned int fw = w/Factor;
  121.                         Score.resize(fh);
  122.                         From.resize(fh);
  123.                         Closed.resize(fh);
  124.                         Open.resize(fh);
  125.                         for (unsigned int y = 0; y<Score.size(); y++)
  126.                         {
  127.                                 Score[y].resize(fw);
  128.                                 From[y].resize(fw);
  129.                                 Closed[y].resize(fw);
  130.                                 Open[y].resize(fw);
  131.                                 for (unsigned int x = 0; x<Score[0].size(); x++)
  132.                                 {
  133.                                         Closed[y][x] = false;
  134.                                         Open[y][x] = false;
  135.                                 }
  136.                         }
  137.                 }
  138.  
  139.                 // The actual search function. Providing the start and goal of the search, the Weight maps of how passable or non passable is the terrain and the resulting search path
  140.                 // Returns true if reached goal, false if otherwise.
  141.                 bool Find(ACell start, ACell goal, std::vector<std::vector<double> > & Weight, std::list<ACell> & Path)
  142.                 {
  143.                         double Root = sqrt(2.0);
  144.                         for (unsigned int y = 0; y<Score.size(); y++)
  145.                                 for (unsigned int x = 0; x<Score[0].size(); x++)
  146.                                 {
  147.                                         From[y][x].Enable = false;
  148.                                         Closed[y][x] = false;
  149.                                         Open[y][x] = false;
  150.                                 }
  151.                         start.x = min(start.x/Factor, Score[0].size()-1);
  152.                         start.y = min(start.y/Factor, Score.size()-1);
  153.                         goal.x = min(goal.x/Factor, Score[0].size()-1);
  154.                         goal.y = min(goal.y/Factor, Score.size()-1);
  155.                         if (h!=NULL)
  156.                         {
  157.                                 h->Start(start, goal);
  158.                                 start.s.h = h->Calculate (start, goal, Factor);
  159.                                 start.s.f = start.s.h;
  160.                         }
  161.                         std::priority_queue<ACell> openset;
  162.                         openset.push (start);
  163.  
  164.                         while (!openset.empty())
  165.                         {
  166.  
  167.                                 std::list<ACell>::iterator q;
  168.                                 ACell current;
  169.                                 current = openset.top();
  170.                                 if (h!=NULL)
  171.                                         h->Step();
  172.                                 if (h!=NULL && h->IsStop (current, Factor))
  173.                                 {
  174.                                         if (!h->IsCancel())
  175.                                                 Reconstruct (start, current, Path);
  176.                                         openset = std::priority_queue<ACell>();
  177.                                         return true;
  178.                                 }
  179.                                 if (h->IsGoal() && current.x==goal.x && current.y==goal.y)
  180.                                 {
  181.                                         Reconstruct (start, goal, Path);
  182.                                         openset = std::priority_queue<ACell>();
  183.                                         return true;
  184.                                 }
  185.                                 openset.pop();
  186.                                 Open[current.y][current.x] = false;
  187.                                 Closed[current.y][current.x] = true;
  188.                                 unsigned int x1 = (current.x>1?current.x-1:0);
  189.                                 unsigned int y1 = (current.y>1?current.y-1:0);
  190.                                 unsigned int x2 = (current.x<Score[0].size()-1?current.x+1:Score[0].size()-1);
  191.                                 unsigned int y2 = (current.y<Score.size()-1?current.y+1:Score.size()-1);
  192.                                 for (unsigned int ny = y1; ny<=y2; ny++)
  193.                                         for (unsigned int nx = x1; nx<=x2; nx++)
  194.                                         {
  195.                                                 if (nx==current.x && ny==current.y)
  196.                                                         continue;
  197.                                                 std::list<ACell>::iterator q;
  198.                                                 if (Closed[ny][nx])
  199.                                                         continue;
  200.                                                 double dx = (double)nx-(double)current.x;
  201.                                                 double dy = (double)ny-(double)current.y;
  202.                                                 ACell neighbour;
  203.                                                 neighbour.x = nx;
  204.                                                 neighbour.y = ny;
  205. //                                              double s = Score[current.y][current.x]+sqrt(dx*dx+dy*dy)+Weight[neighbour.y][neighbour.x];
  206.                                                 double s = Score[current.y][current.x]+(((nx==current.x) || (ny==current.y))?1:Root)+Weight[neighbour.y][neighbour.x];
  207.                                                 neighbour.s.g = s;
  208.                                                 bool IsFound = false;
  209.                                                 if (Open[ny][nx])
  210.                                                         IsFound = true;
  211.                                                 bool b = false;
  212.                                                 if (!IsFound)
  213.                                                 {
  214.                                                         if (h!=NULL)
  215.                                                                 neighbour.s.h = h->Calculate (neighbour, goal, Factor);
  216.                                                         neighbour.s.f = neighbour.s.g+neighbour.s.h;
  217.                                                         openset.push(neighbour);
  218.                                                         Open[neighbour.y][neighbour.x] = true;
  219.                                                         b = true;
  220.                                                 }
  221.                                                 else
  222.                                                         b = (s < Score[neighbour.y][neighbour.x]);
  223.  
  224.                                                 if (b)
  225.                                                 {
  226.                                                         current.Enable = true;
  227.                                                         From[neighbour.y][neighbour.x] = current;
  228.                                                         neighbour.s.f = neighbour.s.g+neighbour.s.h;
  229.                                                         Score[neighbour.y][neighbour.x] = s;
  230.                                                 }
  231.                                         }
  232.  
  233.                         }
  234.                         return false;
  235.                 }
  236.         private:
  237.                 // Once the search is done, this function is used to reconstruct the actual resulting optimal path.
  238.                 void Reconstruct (ACell start, ACell goal, std::list<ACell> & Path)
  239.                 {
  240. //                      Path.clear(); TODO: Need to provide a clear path
  241.                         ACell current = goal;
  242.                         while (current.x!=start.x || current.y!=start.y)
  243.                         {
  244.                                 current.x*=Factor;
  245.                                 current.y*=Factor;
  246.                                 Path.push_front(current);
  247.                                 current.x/=Factor;
  248.                                 current.y/=Factor;
  249.                                 current = From[current.y][current.x];
  250.                                 if (!current.Enable)
  251.                                         return;
  252.                         }
  253.                 }
  254.  
  255.                 Heuristic * h;
  256.                 std::vector<std::vector<bool> > Closed, Open;
  257.                 std::vector<std::vector<double> > Score;
  258.                 std::vector<std::vector<ACell> > From;
  259.                 double Factor;
  260.                 bool Ready;
  261. };
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