path.cc

Go to the documentation of this file.

/*
 *  path.cc - Pathfinding algorithms.
 *
 *  Copyright (C) 2000-2001  The Exult Team
 *
 *  This program 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; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program 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.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <iostream>

#include "hash_utils.h"
#include "PathFinder.h"
#include "exult_constants.h"
#include "vec.h"

using std::cout;
using std::endl;
using std::size_t;

Tile_coord *Find_path
  (
  Tile_coord start,   // Where to start from.
  Tile_coord goal,    // Where to end up.
  Pathfinder_client *client,  // Provides costs.
  int& pathlen      // Length of path returned.
  );


/*
 *  Iterate through neighbors of a tile (in 2 dimensions).
 */
class Neighbor_iterator
  {
  Tile_coord tile;    // Original tile.
  static int coords[16];    // Coords to go through ((x,y) pairs)
  int index;      // 0-7.
public:
  Neighbor_iterator(Tile_coord t) : tile(t), index(0)
    {  }
          // Get next neighbor.
  int operator()(Tile_coord& newt)
    {
    if (index < 8)
      {
      newt = Tile_coord(tile.tx + coords[2*index],
        tile.ty + coords[2*index + 1], tile.tz);
      index++;
          // Handle world-wrapping.
      newt.tx = (newt.tx + c_num_tiles)%c_num_tiles;
      newt.ty = (newt.ty + c_num_tiles)%c_num_tiles;
      return (1);
      }
    return (0);
    }
  };

/*
 *  Statics:
 */
int Neighbor_iterator::coords[16] = {
  -1, -1, 0, -1, 1, -1,
  -1,  0,        1,  0,
  -1,  1, 0,  1, 1,  1
  };

/*
 *  A node for our search:
 */
class Search_node
  {
  Tile_coord tile;    // The coords (x, y, z) in tiles.
  short start_cost;   // Actual cost from start.
  short goal_cost;    // Estimated cost to goal.
  short total_cost;   // Sum of the two above.
  Search_node *parent;    // Prev. in path.
  Search_node *priority_next; // ->next with same total_cost, or
          //   NULL if not in 'open' set.
public:
  Search_node(Tile_coord& t, short scost, short gcost, Search_node *p)
    : tile(t), start_cost(scost), goal_cost(gcost),
      parent(p), priority_next(0)
    {
    total_cost = gcost + scost;
    }
          // For creating a key to search for.
  Search_node(Tile_coord& t) : tile(t)
    {  }
  Tile_coord get_tile() const
    { return tile; }
  int get_start_cost()
    { return start_cost; }
  int get_goal_cost()
    { return goal_cost; }
  int get_total_cost()
    { return total_cost; }
  int is_open()     // In 'open' priority queue?
    { return priority_next != 0; }
  void update(short scost, short gcost, Search_node *p)
    {
    start_cost = scost;
    goal_cost = gcost;
    total_cost = gcost + scost;
    parent = p;
    }
          // Create path back to start.
  Tile_coord *create_path(int& pathlen)
    {
    int cnt = 1;    // This.
          // Count back to start.
    Search_node *each = this;
    while ((each = each->parent) != 0)
      cnt++;
    pathlen = cnt - 1;  // Don't want starting tile.
    Tile_coord *result = new Tile_coord[pathlen];
    each = this;
    for (int i = pathlen - 1; i >= 0; i--)
      {
      result[i] = each->tile;
      each = each->parent;
      }
    return result;
    }
#if VERIFYCHAIN
          // Returns 0 if bad chain.
  int verify_chain(Search_node *last, int removed = 0)
    {
    if (!last)
      return (1);
    int found = 0;
    Search_node *prev = last;
    int cnt = 0;
    do
      {
      Search_node *next = prev->priority_next;
      if (next == this)
        found = 1;
      prev = next;
      if (cnt > 10000)
        break;
      }
    while (prev != last);
    if (!found && !removed)
      return (0);
    if (cnt == 10000)
      return (0);
    return (1);
    }
#endif
          // Add to chain of same priorities.
  void add_to_chain(Search_node *&last)
    {
    if (last)
      {
      priority_next = last->priority_next;
      last->priority_next = this;
      }
    else
      {
      last = this;
      priority_next = this;
      }
#if VERIFYCHAIN
    if (!verify_chain(last))
      cout << "Bad chain after adding." << endl;
#endif
    }
          // Remove this from its chain.
  void remove_from_chain(Search_node *&last)
    {
#if VERIFYCHAIN
    if (!verify_chain(last))
      cout << "Bad chain before removing." << endl;
#endif
    if (priority_next == this)
          // Only one in chain?
      last = 0;
    else
      {   // Got to find prev. to this.
      Search_node *prev = last;
      do
        {
        Search_node *next = prev->priority_next;
        if (next == this)
          break;
        prev = next;
        }
      while (prev != last);
      if (prev)
        {
        prev->priority_next = priority_next;
        if (last == this)
          last = priority_next;
        }
      }
    priority_next = 0;  // No longer in 'open'.
#if VERIFYCHAIN
    if (!verify_chain(last, 1))
      cout << "Bad chain after removing." << endl;
#endif
    }
          // Remove 1st from a priority chain.
  static Search_node *remove_first_from_chain(Search_node *&last)
    {
    Search_node *first = last->priority_next;
    if (first == last)  // Last entry?
      last = 0;
    else
      last->priority_next = first->priority_next;
    first->priority_next = 0;
    return first;
    }
  };

#ifndef DONT_HAVE_HASH_SET

/*
 *  Hash function for nodes:
 */
class Hash_node
  {
public:
  size_t operator() (const Search_node *a) const
    {
    const Tile_coord t = a->get_tile();
    return ((t.tz << 24) + (t.ty << 12) + t.tx);
    }
  };

/*
 *  For testing if two nodes match.
 */
class Equal_nodes
  {
public:
      bool operator() (const Search_node *a, const Search_node *b) const
        {
    Tile_coord ta = a->get_tile(), tb = b->get_tile();
    return ta == tb;
    }
  };

#else

/*
 *  "Less than" relation for nodes
 */
class Less_nodes
  {
public:
      bool operator() (const Search_node *a, const Search_node *b) const
        {
      Tile_coord ta = a->get_tile(), tb = b->get_tile();
      uint32 apos = ta.tx << 16, bpos = tb.tx << 16;
      apos |= ta.ty << 4;
      bpos |= tb.ty << 4;
      apos |= ta.tz;
      bpos |= tb.tz;
      /* Because #(short x short) is <= #int, we can define an injective projection,
      ** which is all we need. */
      return apos < bpos;
    }
  };

#endif


/*
 *  The priority queue for the A* algorithm:
 */
class A_star_queue
  {
  Exult_vector<Search_node*> open;// Nodes to be done, by priority. Each
          //   is a ->last node in chain.
  int best;     // Index of 1st non-null ent. in open.
          // For finding each tile's node:
#ifndef DONT_HAVE_HASH_SET
  typedef hash_set<Search_node *, Hash_node, Equal_nodes> Lookup_set;
#else
  typedef std::set<Search_node *, Less_nodes> Lookup_set;
#endif
  Lookup_set lookup;
public:
#ifndef DONT_HAVE_HASH_SET
  A_star_queue() : open(256), lookup(1000)
#else
  A_star_queue() : open(256), lookup()
#endif
    {
    open.insert(open.begin(), 256, (Search_node *) 0);
    best = open.size(); // Best is past end.
    }
  ~A_star_queue()
    {
#if 1 
  /* 
  This _should_ work, but might hang some hash_set implementations.
  The problem is that on deleting the Search_node, the hash_set can
  no longer properly evaluate the hash value (since the hash function
  dereferences the Search_node* stored). This might cause an endless
  loop.
  */
    for(Lookup_set::iterator X = lookup.begin(); X != lookup.end();) {
      Search_node *sn = *X;
      X++;
      delete sn; // only delete this _after_ iterating
}
#endif
    lookup.clear();   // Remove all nodes.
    }
  void add_back(Search_node *nd)  // Add an existing node back to 'open'.
    {
    int total_cost = nd->get_total_cost();
    Search_node *last = total_cost < open.size() ?
            open[total_cost] : 0;
    nd->add_to_chain(last); // Add node to this chain.
    open.put(total_cost, last);
    if (total_cost < best)
      best = total_cost;
    }
  void add(Search_node *nd) // Add new node to 'open' set.
    {
    lookup.insert(nd);
    add_back(nd);
    }
          // Remove node from 'open' set.
  void remove_from_open(Search_node *nd)
    {
    if (!nd->is_open())
      return;   // Nothing to do.
    int total_cost = nd->get_total_cost();
    Search_node *last = total_cost < open.size() ?
            open[total_cost] : 0;
    nd->remove_from_chain(last);
          // Store updated 'last'.
    open.put(total_cost, last);
    if (!last)    // Last in chain?
      {
      if (total_cost == best)
        {
        int cnt = open.size();
        for (best++; best < cnt; best++)
          if (open[best] != 0)
            break;
        }
      }
    }
  Search_node *pop()    // Pop best from priority queue.
    {
    Search_node *last = best < open.size() ? open[best] : 0;
    if (!last)
      return (0);
          // Return 1st in list.
    Search_node *node = Search_node::remove_first_from_chain(last);
          // Store updated 'last'.
    open.put(best, last);
    if (!last)    // List now empty?
      {
      int cnt = open.size();
      for (best++; best < cnt; best++)
        if (open[best] != 0)
          break;
      }
    return node;
    }
          // Find node for given tile.
  Search_node *find(Tile_coord tile)
    {
    Search_node key(tile);
#ifndef DONT_HAVE_HASH_SET
    hash_set<Search_node *, Hash_node, Equal_nodes>::iterator it =
              lookup.find(&key);
#else
    std::set<Search_node *, Less_nodes>::iterator it =
      lookup.find(&key);
#endif
    if (it != lookup.end())
      return *it;
    else
      return 0;
    }
  };

static int tracing = 0;

/*
 *  First cut at using the A* pathfinding algorithm.
 *
 *  Output: ->(allocated) array of Tile_coords to follow, or 0 if failed.
 */

Tile_coord *Find_path
  (
  Tile_coord start,   // Where to start from.
  Tile_coord goal,    // Where to end up.
  Pathfinder_client *client,  // Provides costs.
  int& pathlen      // Length of path returned.
  )
  {
  A_star_queue nodes;   // The priority queue & hash table.
  int max_cost = client->estimate_cost(start, goal);
          // Create start node.
  nodes.add(new Search_node(start, 0, max_cost, 0));
          // Figure when to give up.
  max_cost = client->get_max_cost(max_cost);
  Search_node *node;    // Try 'best' node each iteration.
  while ((node = nodes.pop()) != 0)
    {
    if (tracing)
      cout << "Goal: (" << goal.tx << ", " << goal.ty <<
      "), Node: (" << node->get_tile().tx << ", " <<
      node->get_tile().ty << ")" << endl;
    Tile_coord curtile = node->get_tile();
    if (client->at_goal(curtile, goal))
          // Success.
      return node->create_path(pathlen);
          // Go through surrounding tiles.
    Neighbor_iterator get_next(curtile);
    Tile_coord ntile(0, 0, 0);
    while (get_next(ntile))
      {   // Get cost to next tile.
      int step_cost = client->get_step_cost(curtile, ntile);
          // Blocked?
      if (step_cost == -1)
        continue;
          // Get cost from start to ntile.
      int new_cost = node->get_start_cost() + step_cost;
          // See if next tile already seen.
      Search_node *next = nodes.find(ntile);
          // Already there, and cheaper?
      if (next && next->get_start_cost() <= new_cost)
        continue;
      int new_goal_cost = client->estimate_cost(ntile, goal);
          // Skip nodes too far away.
      if (new_cost + new_goal_cost >= max_cost)
        continue;
      if (!next)  // Create if necessary.
        {
        next = new Search_node(ntile, new_cost,
            new_goal_cost, node);
        nodes.add(next);
        }
      else
        { // It's going to move.
        nodes.remove_from_open(next);
        next->update(new_cost, new_goal_cost, node);
        nodes.add_back(next);
        }
      }
    }
  pathlen = 0;      // Failed if here.
  return 0; 
  }

---