src/pathfinder/yapf/astar.hpp

   1 /*
   2  * This file is part of OpenTTD.
   3  * OpenTTD is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2.
   4  * OpenTTD is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
   5  * See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with OpenTTD. If not, see <http://www.gnu.org/licenses/>.
   6  */
   7
   8 /** @file astar.hpp A-star pathfinder implementation. */
   9
  10 #ifndef ASTAR_HPP
  11 #define ASTAR_HPP
  12
  13 #include "../../misc/array.hpp"
  14 #include "../../misc/hashtable.hpp"
  15 #include "../../misc/binaryheap.hpp"
  16
  17 /**
  18  * A-star node template common base
  19  *
  20  * This struct contains the common fields that the A-star algorithm below
  21  * requires a node to have. Users of the A-star pathfinder must define a
  22  * node class that derives from this struct, using that node class itself
  23  * as template argument. Such a class must define a Key type to be used in
  24  * hashes, and a GetKey method to get the key for a particular node. It
  25  * may also define a Set method to initalise the node, which must take a
  26  * parent Node as first argument, and a Dump method to dump its contents;
  27  * either one defined must hook into this base class's corresponding own
  28  * method.
  29  */
  30 template <class Node>
  31 struct AstarNodeBase {
  32         Node *m_hash_next; ///< next node in hash bucket
  33         Node *m_parent;    ///< parent node in path
  34         int   m_cost;      ///< cost of this node
  35         int   m_estimate;  ///< estimated cost to target
  36
  37         /** Initialise this node */
  38         inline void Set (Node *parent)
  39         {
  40                 m_hash_next = NULL;
  41                 m_parent = parent;
  42                 m_cost = 0;
  43                 m_estimate = 0;
  44         }
  45
  46         /** Get the next node in the hash bucket, to be used internally */
  47         inline Node *GetHashNext() const
  48         {
  49                 return m_hash_next;
  50         }
  51
  52         /** Set the next node in the hash bucket, to be used internally */
  53         inline void SetHashNext (Node *next)
  54         {
  55                 m_hash_next = next;
  56         }
  57
  58         /** Get the cost of this node */
  59         inline int GetCost() const
  60         {
  61                 return m_cost;
  62         }
  63
  64         /** Get the estimated final cost to the target */
  65         inline int GetCostEstimate() const
  66         {
  67                 return m_estimate;
  68         }
  69
  70         /** Compare estimated final cost with another node */
  71         inline bool operator < (const Node& other) const
  72         {
  73                 return m_estimate < other.m_estimate;
  74         }
  75
  76         /** Dump this node */
  77         template <class D> void Dump (D &dmp) const
  78         {
  79                 dmp.WriteStructT("m_parent", this->m_parent);
  80                 dmp.WriteLine("m_cost = %d", this->m_cost);
  81                 dmp.WriteLine("m_estimate = %d", this->m_estimate);
  82         }
  83 };
  84
  85 /**
  86  * A-star pathfinder implementation class
  87  *
  88  * Instantiate this class by supplying your node class as template argument;
  89  * such a class must derive from AstarNodeBase above, and provide a Key type
  90  * for hashes and a GetKey method to retrieve the key for a node.
  91  */
  92 template <class TNode, int open_hash_bits, int closed_hash_bits>
  93 struct Astar {
  94 public:
  95         /** make TNode visible from outside of class */
  96         typedef TNode Node;
  97         /** make TNode::Key a property of HashTable */
  98         typedef typename TNode::Key Key;
  99
 100 protected:
 101         /** here we store full item data (Node) */
 102         SmallArray<Node, 65536, 256> m_arr;
 103         /** hash table of pointers to open item data */
 104         CHashTableT<Node, open_hash_bits  > m_open;
 105         /** hash table of pointers to closed item data */
 106         CHashTableT<Node, closed_hash_bits> m_closed;
 107         /** priority queue of pointers to open item data */
 108         CBinaryHeapT<Node> m_open_queue;
 109         /** new open node under construction */
 110         Node              *m_new_node;
 111
 112 public:
 113         /** default constructor */
 114         Astar() : m_open_queue(2048), m_new_node(NULL)
 115         {
 116         }
 117
 118         /** return number of open nodes */
 119         inline int OpenCount()
 120         {
 121                 return m_open.Count();
 122         }
 123
 124         /** return number of closed nodes */
 125         inline int ClosedCount()
 126         {
 127                 return m_closed.Count();
 128         }
 129
 130         /** Create a new node */
 131         inline Node *CreateNewNode (Node *parent)
 132         {
 133                 if (m_new_node == NULL) m_new_node = m_arr.AppendC();
 134                 m_new_node->Set (parent);
 135                 return m_new_node;
 136         }
 137
 138         /** Create a new node, one parameter */
 139         template <class T1>
 140         inline Node *CreateNewNode (Node *parent, T1 t1)
 141         {
 142                 if (m_new_node == NULL) m_new_node = m_arr.AppendC();
 143                 m_new_node->Set (parent, t1);
 144                 return m_new_node;
 145         }
 146
 147         /** Create a new node, two parameters */
 148         template <class T1, class T2>
 149         inline Node *CreateNewNode (Node *parent, T1 t1, T2 t2)
 150         {
 151                 if (m_new_node == NULL) m_new_node = m_arr.AppendC();
 152                 m_new_node->Set (parent, t1, t2);
 153                 return m_new_node;
 154         }
 155
 156         /** Notify the nodelist that we don't want to discard the given node. */
 157         inline void FoundBestNode (Node *n)
 158         {
 159                 /* node should be a newly created one */
 160                 assert (n == m_new_node);
 161
 162                 m_new_node = NULL;
 163         }
 164
 165         /** insert given item as open node (into m_open and m_open_queue) */
 166         inline void InsertOpenNode(Node& item)
 167         {
 168                 assert(m_closed.Find(item.GetKey()) == NULL);
 169                 m_open.Push(item);
 170                 m_open_queue.Include(&item);
 171                 if (&item == m_new_node) {
 172                         m_new_node = NULL;
 173                 }
 174         }
 175
 176         /** return the best open node */
 177         inline Node *GetBestOpenNode()
 178         {
 179                 if (!m_open_queue.IsEmpty()) {
 180                         return m_open_queue.Begin();
 181                 }
 182                 return NULL;
 183         }
 184
 185         /** remove and return the open node specified by a key */
 186         inline Node& PopOpenNode(const Key& key)
 187         {
 188                 Node& item = m_open.Pop(key);
 189                 uint idxPop = m_open_queue.FindIndex(item);
 190                 m_open_queue.Remove(idxPop);
 191                 return item;
 192         }
 193
 194         /** close node */
 195         inline void InsertClosedNode(Node& item)
 196         {
 197                 assert(m_open.Find(item.GetKey()) == NULL);
 198                 m_closed.Push(item);
 199         }
 200
 201         /** Replace an existing (open) node. */
 202         inline void ReplaceNode (const Key &key, Node *n1, const Node *n2)
 203         {
 204                 /* pop old node from open list and queue */
 205                 PopOpenNode (key);
 206                 /* update old node with new data */
 207                 *n1 = *n2;
 208                 /* add updated node to open list and queue */
 209                 InsertOpenNode (*n1);
 210         }
 211
 212         /** Insert a new initial node. */
 213         inline void InsertInitialNode (Node *n)
 214         {
 215                 /* node to insert should be a newly created one */
 216                 assert (n == m_new_node);
 217
 218                 /* closed list should be empty when adding initial nodes */
 219                 assert (m_closed.Count() == 0);
 220
 221                 /* insert the new node only if it is not there yet */
 222                 const Key key = n->GetKey();
 223                 Node *m = m_open.Find (key);
 224                 if (m == NULL) {
 225                         InsertOpenNode(*n);
 226                 } else {
 227                         /* two initial nodes with same key;
 228                          * pick the one with the lowest cost */
 229                         if (n->GetCostEstimate() < m->GetCostEstimate()) {
 230                                 ReplaceNode (key, m, n);
 231                         }
 232                 }
 233         }
 234
 235         /** Insert a new node */
 236         inline void InsertNode (Node *n)
 237         {
 238                 /* node to insert should be a newly created one */
 239                 assert (n == m_new_node);
 240
 241                 const Key key = n->GetKey();
 242
 243                 /* check new node against open list */
 244                 Node *m = m_open.Find (key);
 245                 if (m != NULL) {
 246                         /* another node exists with the same key in the open list
 247                          * is it better than new one? */
 248                         if (n->GetCostEstimate() < m->GetCostEstimate()) {
 249                                 ReplaceNode (key, m, n);
 250                         }
 251                         return;
 252                 }
 253
 254                 /* check new node against closed list */
 255                 m = m_closed.Find(key);
 256                 if (m != NULL) {
 257                         /* another node exists with the same key in the closed list
 258                          * is it better than new one? */
 259                         assert (m->GetCostEstimate() <= n->GetCostEstimate());
 260                         return;
 261                 }
 262
 263                 /* the new node is really new
 264                  * add it to the open list */
 265                 InsertOpenNode(*n);
 266         }
 267
 268         /** Helper for creating output of this array. */
 269         template <class D> void Dump(D &dmp) const
 270         {
 271                 dmp.WriteStructT("m_arr", &m_arr);
 272         }
 273 };
 274
 275 #endif /* ASTAR_HPP */