Logo Search packages:      
Sourcecode: warzone2100 version File versions

astar.c

Go to the documentation of this file.
/*
      This file is part of Warzone 2100.
      Copyright (C) 1999-2004  Eidos Interactive
      Copyright (C) 2005-2007  Warzone Resurrection Project

      Warzone 2100 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.

      Warzone 2100 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 Warzone 2100; if not, write to the Free Software
      Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/** @file
 *  A* based path finding
 */

#include <assert.h>

#include "lib/framework/frame.h"

#include "objects.h"
#include "map.h"
#include "raycast.h"
#include "fpath.h"

#include "astar.h"

static SDWORD     astarOuter, astarRemove;

/** Keeps track of the amount of iterations done in the inner loop of our A*
 *  implementation.
 *
 *  @see FPATH_LOOP_LIMIT
 *
 *  @ingroup pathfinding
 */
00044 int astarInner = 0;

/** The structure to store a node of the route in the hash table
 */
00048 typedef struct _fp_node
{
      SWORD x,y;        // map coords
      SWORD dist, est;  // distance so far and estimate to end
      SWORD type;       // open or closed node
      struct _fp_node *psOpen;
      struct _fp_node   *psRoute;   // Previous point in the route
      struct _fp_node *psNext;
} FP_NODE;

// types of node
#define NT_OPEN         1
#define NT_CLOSED 2

/** List of open nodes in the hash table
 */
00064 static FP_NODE* psOpen;

/** Size of closed hash table
 */
00068 #define FPATH_TABLESIZE       4091

/** Hash table for closed nodes
 */
00072 static FP_NODE* apsNodes[FPATH_TABLESIZE] = { NULL };

#define NUM_DIR         8

// Convert a direction into an offset
// dir 0 => x = 0, y = -1
static const Vector2i aDirOffset[NUM_DIR] =
{
      { 0, 1},
      {-1, 1},
      {-1, 0},
      {-1,-1},
      { 0,-1},
      { 1,-1},
      { 1, 0},
      { 1, 1},
};

// reset the astar counters
00091 void astarResetCounters(void)
{
      astarInner = 0;
      astarOuter = 0;
      astarRemove = 0;
}

/* next four used in HashPJW */
#define     BITS_IN_int32_t (sizeof(int32_t) * CHAR_BIT)
#define     THREE_QUARTERS  ((BITS_IN_int32_t * 3) / 4)
#define     ONE_EIGHTH      (BITS_IN_int32_t / 8)
#define     HIGH_BITS       (~((int32_t)(~0) >> ONE_EIGHTH))

/** Hash string
 *  Adaptation of Peter Weinberger's (PJW) generic hashing algorithm listed in
 *  Binstock+Rex, "Practical Algorithms" p 69.
 *
 *  Hacked to use coordinates instead of a string
 */
00110 static SDWORD fpathHashFunc(int32_t x, int32_t y)
{
      char    buf[sizeof(x) + sizeof(y)];
      char   *c;
      int32_t hashValue = 0;

      memcpy(&buf[0],         &x, sizeof(x));
      memcpy(&buf[sizeof(x)], &y, sizeof(y));

      for (c = &buf[0]; c != &buf[ARRAY_SIZE(buf)]; ++c)
      {
            int32_t highBits;

            if (*c == 0)
                  continue;

            hashValue = (hashValue << ONE_EIGHTH) + *c;
            highBits = hashValue & HIGH_BITS;

            if (highBits != 0)
            {
                  hashValue = (hashValue ^ (highBits >> THREE_QUARTERS)) & ~HIGH_BITS;
            }
      }

      hashValue %= ARRAY_SIZE(apsNodes);

      return hashValue;
}

/** Add a node to the hash table
 *
 *  @param apsTable hash table
 *  @param psNode to add to the given hash table
 */
00145 static void fpathHashAdd(FP_NODE *apsTable[], FP_NODE *psNode)
{
      SDWORD      index;

      index = fpathHashFunc(psNode->x, psNode->y);

      psNode->psNext = apsTable[index];
      apsTable[index] = psNode;
}

/** See if a node is in the hash table
 *  Check whether there is a node for the given coordinates in the hash table
 *
 *  @param apsTable the hash table to check
 *  @param x,y the coordinates to check for
 *  @return a pointer to the node if one could be found, or NULL otherwise.
 */
00162 static FP_NODE *fpathHashPresent(FP_NODE *apsTable[], int32_t x, int32_t y)
{
      FP_NODE *psFound;

      for (psFound = apsTable[fpathHashFunc(x, y)]; psFound; psFound = psFound->psNext)
      {
            // Check whether we found the node we're looking for
            if (psFound->x == x
             && psFound->y == y)
            {
                  return psFound;
            }
      }

      return NULL;
}

/** Reset the hash tables
 */
00181 static void fpathHashReset(void)
{
      int i;

      for(i = 0; i < ARRAY_SIZE(apsNodes); ++i)
      {
            while (apsNodes[i])
            {
                  FP_NODE* toFree = apsNodes[i];
                  apsNodes[i] = apsNodes[i]->psNext;

                  free(toFree);
            }
      }
}

/** Compare two nodes
 */
00199 static inline SDWORD fpathCompare(FP_NODE *psFirst, FP_NODE *psSecond)
{
      SDWORD      first,second;

      first = psFirst->dist + psFirst->est;
      second = psSecond->dist + psSecond->est;
      if (first < second)
      {
            return -1;
      }
      else if (first > second)
      {
            return 1;
      }

      // equal totals, give preference to node closer to target
      if (psFirst->est < psSecond->est)
      {
            return -1;
      }
      else if (psFirst->est > psSecond->est)
      {
            return 1;
      }

      // exactly equal
      return 0;
}

/** make a 50/50 random choice
 */
00230 static BOOL fpathRandChoice(void)
{
      return ONEINTWO;
}

/** Add a node to the open list
 */
00237 static void fpathOpenAdd(FP_NODE *psNode)
{
      psNode->psOpen = psOpen;
      psOpen = psNode;
}

/** Get the nearest entry in the open list
 */
00245 static FP_NODE *fpathOpenGet(void)
{
      FP_NODE     *psNode, *psCurr, *psPrev, *psParent = NULL;
      SDWORD      comp;

      if (psOpen == NULL)
      {
            return NULL;
      }

      // find the node with the lowest distance
      psPrev = NULL;
      psNode = psOpen;
      for(psCurr = psOpen; psCurr; psCurr = psCurr->psOpen)
      {
            comp = fpathCompare(psCurr, psNode);
            if (comp < 0 || (comp == 0 && fpathRandChoice()))
            {
                  psParent = psPrev;
                  psNode = psCurr;
            }
            psPrev = psCurr;
      }

      // remove the node from the list
      if (psNode == psOpen)
      {
            // node is at head of list
            psOpen = psOpen->psOpen;
      }
      else
      {
            psParent->psOpen = psNode->psOpen;
      }

      return psNode;
}

/** Estimate the distance to the target point
 */
00285 static SDWORD fpathEstimate(SDWORD x, SDWORD y, SDWORD fx, SDWORD fy)
{
      SDWORD xdiff, ydiff;

      xdiff = x > fx ? x - fx : fx - x;
      ydiff = y > fy ? y - fy : fy - y;

      xdiff = xdiff * 10;
      ydiff = ydiff * 10;

      return xdiff > ydiff ? xdiff + ydiff/2 : xdiff/2 + ydiff;
}

/** Generate a new node
 */
00300 static FP_NODE *fpathNewNode(SDWORD x, SDWORD y, SDWORD dist, FP_NODE *psRoute)
{
      FP_NODE     *psNode = malloc(sizeof(FP_NODE));

      if (psNode == NULL)
      {
            debug(LOG_ERROR, "fpathNewNode: Out of memory");
            return NULL;
      }

      psNode->x = (SWORD)x;
      psNode->y = (SWORD)y;
      psNode->dist = (SWORD)dist;
      psNode->psRoute = psRoute;
      psNode->type = NT_OPEN;

      return psNode;
}

// Variables for the callback
static SDWORD     finalX,finalY, vectorX,vectorY;
static BOOL       obstruction;

/** The visibility ray callback
 */
00325 static BOOL fpathVisCallback(SDWORD x, SDWORD y, SDWORD dist)
{
      SDWORD      vx,vy;

      dist = dist;

      // See if this point is past the final point (dot product)
      vx = x - finalX;
      vy = y - finalY;
      if (vx*vectorX + vy*vectorY <=0)
      {
            return false;
      }

      if (fpathBlockingTile(map_coord(x), map_coord(y)))
      {
            // found an obstruction
            obstruction = true;
            return false;
      }

      return true;
}

00349 BOOL fpathTileLOS(SDWORD x1,SDWORD y1, SDWORD x2,SDWORD y2)
{
      // convert to world coords
      x1 = world_coord(x1) + TILE_UNITS / 2;
      y1 = world_coord(y1) + TILE_UNITS / 2;
      x2 = world_coord(x2) + TILE_UNITS / 2;
      y2 = world_coord(y2) + TILE_UNITS / 2;

      // Initialise the callback variables
      finalX = x2;
      finalY = y2;
      vectorX = x1 - x2;
      vectorY = y1 - y2;
      obstruction = false;

      rayCast(x1,y1, rayPointsToAngle(x1,y1,x2,y2),
                  RAY_MAXLEN, fpathVisCallback);

      return !obstruction;
}

// Optimise the route
static void fpathOptimise(FP_NODE *psRoute)
{
      FP_NODE     *psCurr, *psSearch, *psTest;
      BOOL  los;

      ASSERT( psRoute != NULL,
            "fpathOptimise: NULL route pointer" );

      psCurr = psRoute;
      do
      {
            // work down the route looking for a failed LOS
            los = true;
            psSearch = psCurr->psRoute;
            while (psSearch)
            {
                  psTest = psSearch->psRoute;
                  if (psTest)
                  {
                        los = fpathTileLOS(psCurr->x,psCurr->y, psTest->x,psTest->y);
                  }
                  if (!los)
                  {
                        break;
                  }
                  psSearch = psTest;
            }

            // store the previous successful point
            psCurr->psRoute = psSearch;
            psCurr = psSearch;
      } while (psCurr);
}

// A* findpath
00406 SDWORD fpathAStarRoute(SDWORD routeMode, ASTAR_ROUTE *psRoutePoints,
                               SDWORD sx, SDWORD sy, SDWORD fx, SDWORD fy)
{
      FP_NODE           *psFound, *psCurr, *psNew, *psParent, *psNext;
static      FP_NODE           *psNearest, *psRoute;
      SDWORD            dir, x,y, currDist;
      SDWORD            index;
      SDWORD            retval;
      const int       tileSX = map_coord(sx);
      const int       tileSY = map_coord(sy);
      const int       tileFX = map_coord(fx);
      const int       tileFY = map_coord(fy);

      if (routeMode == ASR_NEWROUTE)
      {
            fpathHashReset();

            // Add the start point to the open list
            psCurr = fpathNewNode(tileSX,tileSY, 0, NULL);
            if (!psCurr)
            {
                  fpathHashReset();
                  return ASR_FAILED;
            }
            // estimate the estimated distance/moves
            psCurr->est = (SWORD)fpathEstimate(psCurr->x, psCurr->y, tileFX, tileFY);
            psOpen = NULL;
            fpathOpenAdd(psCurr);
            fpathHashAdd(apsNodes, psCurr);
            psRoute = NULL;
            psNearest = NULL;
      }

      // search for a route
      while (psOpen != NULL)
      {
            if (astarInner > FPATH_LOOP_LIMIT)
            {
                  return ASR_PARTIAL;
            }

            psCurr = fpathOpenGet();

            if (psCurr->x == tileFX && psCurr->y == tileFY)
            {
                  // reached the target
                  psRoute = psCurr;
                  break;
            }

            // note the nearest node to the target so far
            if (psNearest == NULL || psCurr->est < psNearest->est)
            {
                  psNearest = psCurr;
            }

            astarOuter += 1;

            // loop through possible moves in 8 directions to find a valid move
            for(dir=0; dir<NUM_DIR; dir+=1)
            {
                  /* make non-orthogonal-adjacent moves' dist a bit longer/cost a bit more
                     5  6  7
                       \|/
                     4 -I- 0
                       /|\
                     3  2  1
                     odd:orthogonal-adjacent tiles even:non-orthogonal-adjacent tiles
                     odd ones get extra 4 units(1.414 times) of distance/costs
                  */
                  if (dir % 2 == 0)
                  {
                        currDist = psCurr->dist + 10;
                  }
                  else
                  {
                        currDist = psCurr->dist + 14;
                  }

                  // Try a new location
                  x = psCurr->x + aDirOffset[dir].x;
                  y = psCurr->y + aDirOffset[dir].y;


                  // See if the node has already been visited
                  psFound = fpathHashPresent(apsNodes, x,y);
                  if (psFound && psFound->dist <= currDist)
                  {
                        // already visited node by a shorter route
//                      debug( LOG_NEVER, "blocked (%d)     : %3d, %3d dist %d\n", psFound->type, x, y, currDist );
                        continue;
                  }

                  // If the tile hasn't been visited see if it is a blocking tile
                  if (!psFound && fpathBlockingTile(x,y))
                  {
                        // tile is blocked, skip it
//                      debug( LOG_NEVER, "blocked          : %3d, %3d\n", x, y );
                        continue;
                  }

                  astarInner += 1;
                  ASSERT(astarInner >= 0, "astarInner overflowed!");

                  // Now insert the point into the appropriate list
                  if (!psFound)
                  {
                        // Not in open or closed lists - add to the open list
                        psNew = fpathNewNode(x,y, currDist, psCurr);
                        if (psNew)
                        {
                              psNew->est = (SWORD)fpathEstimate(x,y, tileFX, tileFY);
                              fpathOpenAdd(psNew);
                              fpathHashAdd(apsNodes, psNew);
//                            debug( LOG_NEVER, "new              : %3d, %3d (%d,%d) = %d\n", x, y, currDist, psNew->est, currDist + psNew->est );
                        }
                  }
                  else if (psFound->type == NT_OPEN)
                  {
                        astarRemove += 1;

                        // already in the open list but this is shorter
                        psFound->dist = (SWORD)currDist;
                        psFound->psRoute = psCurr;
//                      debug( LOG_NEVER, "replace open     : %3d, %3d dist %d\n", x, y, currDist, psFound->est, currDist + psFound->est );
                  }
                  else if (psFound->type == NT_CLOSED)
                  {
                        // already in the closed list but this is shorter
                        psFound->type = NT_OPEN;
                        psFound->dist = (SWORD)currDist;
                        psFound->psRoute = psCurr;
                        fpathOpenAdd(psFound);
//                      debug( LOG_NEVER, "replace closed   : %3d, %3d dist %d\n", x, y, currDist, psFound->est, currDist + psFound->est );
                  }
                  else
                  {
                        ASSERT(!"the open and closed lists are fried/wrong", "fpathAStarRoute: the open and closed lists are f***ed");
                  }
                  if (psFound)
                  {
                        psFound->est = (SWORD)fpathEstimate(x,y, tileFX, tileFY);
                  }
            }

            // add the current point to the closed nodes
//          fpathHashAdd(apsClosed, psCurr);
            psCurr->type = NT_CLOSED;
//          debug( LOG_NEVER, "HashAdd - closed : %3d,%3d (%d,%d) = %d\n", psCurr->pos.x, psCurr->pos.y, psCurr->dist, psCurr->est, psCurr->dist+psCurr->est );
      }

      retval = ASR_OK;

      // return the nearest route if no actual route was found
      if (!psRoute && psNearest)
//          !((psNearest->pos.x == tileSX) && (psNearest->pos.y == tileSY)))
      {
            psRoute = psNearest;
            fx = world_coord(psNearest->x) + TILE_UNITS / 2;
            fy = world_coord(psNearest->y) + TILE_UNITS / 2;
            retval = ASR_NEAREST;
      }

      // optimise the route if one was found
      if (psRoute)
      {
            // optimise the route if one was found
            fpathOptimise(psRoute);

            // get the route in the correct order
            //    If as I suspect this is to reverse the list, then it's my suspicion that
            //    we could route from destination to source as opposed to source to
            //    destination. We could then save the reversal. to risky to try now...Alex M
            psParent = NULL;
            for(psCurr=psRoute; psCurr; psCurr=psNext)
            {
                  psNext = psCurr->psRoute;
                  psCurr->psRoute = psParent;
                  psParent = psCurr;
            }
            psRoute = psParent;

            psCurr = psRoute;
            index = psRoutePoints->numPoints;
            while (psCurr && index < TRAVELSIZE)
            {
                  psRoutePoints->asPos[index].x =     psCurr->x;
                  psRoutePoints->asPos[index].y =     psCurr->y;
                  index += 1;
                  psCurr = psCurr->psRoute;
            }
            psRoutePoints->numPoints = index;
            psRoutePoints->finalX = psRoutePoints->asPos[index-1].x;
            psRoutePoints->finalY = psRoutePoints->asPos[index-1].y;
      }
      else
      {
            retval = ASR_FAILED;
      }

      fpathHashReset();
      return retval;
}

Generated by  Doxygen 1.6.0   Back to index