| 1 |
/* |
| 2 |
Copyright (c) 2000-2003 Lee Thomason (www.grinninglizard.com) |
| 3 |
Grinning Lizard Utilities. |
| 4 |
|
| 5 |
This software is provided 'as-is', without any express or implied |
| 6 |
warranty. In no event will the authors be held liable for any |
| 7 |
damages arising from the use of this software. |
| 8 |
|
| 9 |
Permission is granted to anyone to use this software for any |
| 10 |
purpose, including commercial applications, and to alter it and |
| 11 |
redistribute it freely, subject to the following restrictions: |
| 12 |
|
| 13 |
1. The origin of this software must not be misrepresented; you must |
| 14 |
not claim that you wrote the original software. If you use this |
| 15 |
software in a product, an acknowledgment in the product documentation |
| 16 |
would be appreciated but is not required. |
| 17 |
|
| 18 |
2. Altered source versions must be plainly marked as such, and |
| 19 |
must not be misrepresented as being the original software. |
| 20 |
|
| 21 |
3. This notice may not be removed or altered from any source |
| 22 |
distribution. |
| 23 |
*/ |
| 24 |
|
| 25 |
|
| 26 |
#ifndef GRINNINGLIZARD_MICROPATHER_INCLUDED |
| 27 |
#define GRINNINGLIZARD_MICROPATHER_INCLUDED |
| 28 |
|
| 29 |
/** @mainpage MicroPather |
| 30 |
|
| 31 |
MicroPather is a path finder and A* solver (astar or a-star) written in platform independent |
| 32 |
C++ that can be easily integrated into existing code. MicroPather focuses on being a path |
| 33 |
finding engine for video games but is a generic A* solver. MicroPather is open source, with |
| 34 |
a license suitable for open source or commercial use. |
| 35 |
|
| 36 |
An overview of using MicroPather is in the <A HREF="../readme.htm">readme</A> or |
| 37 |
on the Grinning Lizard website: http://grinninglizard.com/micropather/ |
| 38 |
*/ |
| 39 |
|
| 40 |
#include <vector> |
| 41 |
#include <list> |
| 42 |
#include <assert.h> |
| 43 |
#include <float.h> |
| 44 |
|
| 45 |
#ifdef _DEBUG |
| 46 |
#ifndef DEBUG |
| 47 |
#define DEBUG |
| 48 |
#endif |
| 49 |
#endif |
| 50 |
|
| 51 |
#ifndef GRINLIZ_TYPES_INCLUDED |
| 52 |
#if defined(_MSC_VER) && (_MSC_VER >= 1400 ) |
| 53 |
#include <stdlib.h> |
| 54 |
typedef uintptr_t UPTR; |
| 55 |
#else |
| 56 |
#include <stdint.h> |
| 57 |
typedef uintptr_t UPTR; |
| 58 |
#endif |
| 59 |
#endif |
| 60 |
|
| 61 |
/* USE_LIST and USE_BINARY_HASH change the some of details the pather algorithms. They |
| 62 |
are set optimally in my experiments, but different applications may benefit |
| 63 |
from a different combination of these #defines. |
| 64 |
*/ |
| 65 |
#define USE_LIST |
| 66 |
//#define USE_BINARY_HASH |
| 67 |
|
| 68 |
namespace micropather |
| 69 |
{ |
| 70 |
const float FLT_BIG = FLT_MAX / 2.0f; |
| 71 |
|
| 72 |
/** |
| 73 |
Used to pass the cost of states from the cliet application to MicroPather. This |
| 74 |
structure is copied in a vector. |
| 75 |
|
| 76 |
@sa AdjacentCost |
| 77 |
*/ |
| 78 |
struct StateCost |
| 79 |
{ |
| 80 |
void* state; ///< The state as a void* |
| 81 |
float cost; ///< The cost to the state. Use FLT_MAX for infinite cost. |
| 82 |
}; |
| 83 |
|
| 84 |
|
| 85 |
/** |
| 86 |
A pure abstract class used to define a set of callbacks. |
| 87 |
The client application inherits from |
| 88 |
this class, and the methods will be called when MicroPather::Solve() is invoked. |
| 89 |
|
| 90 |
The notion of a "state" is very important. It must have the following properties: |
| 91 |
- Unique |
| 92 |
- Unchanging (unless MicroPather::Reset() is called) |
| 93 |
|
| 94 |
If the client application represents states as objects, then the state is usually |
| 95 |
just the object cast to a void*. If the client application sees states as numerical |
| 96 |
values, (x,y) for example, then state is an encoding of these values. MicroPather |
| 97 |
never interprets or modifies the value of state. |
| 98 |
*/ |
| 99 |
class Graph |
| 100 |
{ |
| 101 |
public: |
| 102 |
virtual ~Graph() {} |
| 103 |
|
| 104 |
/** |
| 105 |
Return the least possible cost between 2 states. For example, if your pathfinding |
| 106 |
is based on distance, this is simply the straight distance between 2 points on the |
| 107 |
map. If you pathfinding is based on minimum time, it is the minimal travel time |
| 108 |
between 2 points given the best possible terrain. |
| 109 |
*/ |
| 110 |
virtual float LeastCostEstimate( void* stateStart, void* stateEnd ) = 0; |
| 111 |
|
| 112 |
/** |
| 113 |
Return the exact cost from the given state to all its neighboring states. This |
| 114 |
may be called multiple times, or cached by the solver. It *must* return the same |
| 115 |
exact values for every call to MicroPather::Solve(). It should generally be a simple, |
| 116 |
fast function with no callbacks into the pather. |
| 117 |
*/ |
| 118 |
virtual void AdjacentCost( void* state, std::vector< micropather::StateCost > *adjacent ) = 0; |
| 119 |
|
| 120 |
/** |
| 121 |
This function is only used in DEBUG mode - it dumps output to stdout. Since void* |
| 122 |
aren't really human readable, normally you print out some concise info (like "(1,2)") |
| 123 |
without an ending newline. |
| 124 |
|
| 125 |
@note If you are using other grinning lizard utilities, you should use GLOUTPUT for output. |
| 126 |
*/ |
| 127 |
virtual void PrintStateInfo( void* state ) = 0; |
| 128 |
}; |
| 129 |
|
| 130 |
|
| 131 |
class PathNode; |
| 132 |
|
| 133 |
struct NodeCost |
| 134 |
{ |
| 135 |
PathNode* node; |
| 136 |
float cost; |
| 137 |
}; |
| 138 |
|
| 139 |
|
| 140 |
class PathNode |
| 141 |
{ |
| 142 |
friend class none; // Trashy trick to get rid of compiler warning, because |
| 143 |
// this class has a private constructor and destructor - |
| 144 |
// it can never be "new" or created on the stack, only |
| 145 |
// by special allocators. |
| 146 |
public: |
| 147 |
void Init( unsigned _frame, |
| 148 |
void* _state, |
| 149 |
float _costFromStart, |
| 150 |
float _estToGoal, |
| 151 |
PathNode* _parent ) |
| 152 |
{ |
| 153 |
state = _state; |
| 154 |
costFromStart = _costFromStart; |
| 155 |
estToGoal = _estToGoal; |
| 156 |
totalCost = _costFromStart + _estToGoal; |
| 157 |
parent = _parent; |
| 158 |
frame = _frame; |
| 159 |
numAdjacent = -1; |
| 160 |
left = 0; |
| 161 |
#ifdef USE_BINARY_HASH |
| 162 |
right = 0; |
| 163 |
#endif |
| 164 |
#ifdef USE_LIST |
| 165 |
next = 0; |
| 166 |
prev = 0; |
| 167 |
#endif |
| 168 |
inOpen = 0; |
| 169 |
inClosed = 0; |
| 170 |
} |
| 171 |
|
| 172 |
void Reuse( unsigned _frame, float _costFromStart, float _estToGoal, PathNode* _parent ) |
| 173 |
{ |
| 174 |
costFromStart = _costFromStart; |
| 175 |
estToGoal = _estToGoal; |
| 176 |
parent = _parent; |
| 177 |
frame = _frame; |
| 178 |
|
| 179 |
inOpen = 0; |
| 180 |
inClosed = 0; |
| 181 |
} |
| 182 |
|
| 183 |
void *state; // the client state |
| 184 |
float costFromStart; // exact |
| 185 |
float estToGoal; // estimated |
| 186 |
float totalCost; // could be a function, but save some math. |
| 187 |
PathNode* parent; // the parent is used to reconstruct the path |
| 188 |
unsigned frame; // unique id for this path, so the solver can distinguish |
| 189 |
// correct from stale values |
| 190 |
|
| 191 |
enum { |
| 192 |
/* |
| 193 |
@enum MAX_CACHE. If you want to optimize "down deep" this is the way |
| 194 |
to go. MAX_CACHE determines the number of adjacent nodes cached by |
| 195 |
MicroPather. If your nodes generally have 8 or 3 neighbors (common cases) |
| 196 |
changing this may increase performance, sometimes dramatically. |
| 197 |
*/ |
| 198 |
MAX_CACHE = 8 |
| 199 |
}; |
| 200 |
// If there are 4 or less adjacent states, they will be cached as *nodes*. |
| 201 |
NodeCost adjacent[ MAX_CACHE ]; |
| 202 |
int numAdjacent; // -1 is unknown & needs to be queried |
| 203 |
// 0-4 adjacent is known & in 'adjacent' |
| 204 |
// 5+ known, but has to be queried from Graph |
| 205 |
|
| 206 |
PathNode* left; // Binary tree, where the 'state' is what is being compared. |
| 207 |
// Also used as a "next" pointer for memory layout. |
| 208 |
#ifdef USE_BINARY_HASH |
| 209 |
PathNode* right; |
| 210 |
#endif |
| 211 |
#ifdef USE_LIST |
| 212 |
PathNode *next, *prev; |
| 213 |
#endif |
| 214 |
|
| 215 |
unsigned inOpen:1; |
| 216 |
unsigned inClosed:1; |
| 217 |
|
| 218 |
#ifdef USE_LIST |
| 219 |
void Unlink() { |
| 220 |
next->prev = prev; |
| 221 |
prev->next = next; |
| 222 |
next = prev = 0; |
| 223 |
} |
| 224 |
void AddBefore( PathNode* addThis ) { |
| 225 |
addThis->next = this; |
| 226 |
addThis->prev = prev; |
| 227 |
prev->next = addThis; |
| 228 |
prev = addThis; |
| 229 |
} |
| 230 |
#ifdef DEBUG |
| 231 |
void CheckList() |
| 232 |
{ |
| 233 |
assert( totalCost == FLT_MAX ); |
| 234 |
for( PathNode* it = next; it != this; it=it->next ) { |
| 235 |
assert( it->prev == this || it->totalCost >= it->prev->totalCost ); |
| 236 |
assert( it->totalCost <= it->next->totalCost ); |
| 237 |
} |
| 238 |
} |
| 239 |
#endif |
| 240 |
#endif |
| 241 |
|
| 242 |
private: |
| 243 |
PathNode(); |
| 244 |
~PathNode(); |
| 245 |
PathNode( const PathNode& ); |
| 246 |
void operator=( const PathNode& ); |
| 247 |
}; |
| 248 |
|
| 249 |
|
| 250 |
/** |
| 251 |
Create a MicroPather object to solve for a best path. Detailed usage notes are |
| 252 |
on the main page. |
| 253 |
*/ |
| 254 |
class MicroPather |
| 255 |
{ |
| 256 |
friend class micropather::PathNode; |
| 257 |
|
| 258 |
public: |
| 259 |
enum |
| 260 |
{ |
| 261 |
SOLVED, |
| 262 |
NO_SOLUTION, |
| 263 |
START_END_SAME, |
| 264 |
}; |
| 265 |
|
| 266 |
/** |
| 267 |
Construct the pather, passing a pointer to the object that implements |
| 268 |
the Graph callbacks. |
| 269 |
|
| 270 |
@param graph The "map" that implements the Graph callbacks. |
| 271 |
@param allocate The block size that the node cache is allocated from. In some |
| 272 |
cases setting this parameter will improve the perfomance of |
| 273 |
the pather. |
| 274 |
- If you have a small map, for example the size of a chessboard, set allocate |
| 275 |
to be the number of states+1. 65 for a chessboard. This will allow |
| 276 |
MicroPather to used a fixed amount of memory. |
| 277 |
- If your map is large, something like 1/4 the number of possible |
| 278 |
states is good. For example, Lilith3D normally has about 16,000 |
| 279 |
states, so 'allocate' should be about 4000. |
| 280 |
*/ |
| 281 |
MicroPather( Graph* graph, unsigned allocate = 250 ); |
| 282 |
~MicroPather(); |
| 283 |
|
| 284 |
/** |
| 285 |
Solve for the path from start to end. |
| 286 |
|
| 287 |
@param startState Input, the starting state for the path. |
| 288 |
@param endState Input, the ending state for the path. |
| 289 |
@param path Output, a vector of states that define the path. Empty if not found. |
| 290 |
@param totalCost Output, the cost of the path, if found. |
| 291 |
@return Success or failure, expressed as SOLVED, NO_SOLUTION, or START_END_SAME. |
| 292 |
*/ |
| 293 |
int Solve( void* startState, void* endState, std::vector< void* >* path, float* totalCost ); |
| 294 |
|
| 295 |
/// Should be called whenever the cost between states or the connection between states changes. |
| 296 |
void Reset(); |
| 297 |
|
| 298 |
/* Deprecated. Too complex for the practical value. |
| 299 |
@warning It is very easy to "miss" a state that needs to be changed and break the pather. |
| 300 |
|
| 301 |
As described on the main page, it is possible to change the costs (but not the connections) |
| 302 |
of states without calling Reset(). Be sure that all effected states are passed to |
| 303 |
StateCostChange. |
| 304 |
*/ |
| 305 |
//void StateCostChange( void* state ); |
| 306 |
|
| 307 |
/** |
| 308 |
Return the "checksum" of the last path returned by Solve(). Useful for debugging, |
| 309 |
and a quick way to see if 2 paths are the same. |
| 310 |
*/ |
| 311 |
UPTR Checksum() { return checksum; } |
| 312 |
|
| 313 |
void DumpHashTable(); |
| 314 |
|
| 315 |
// Debugging function to return all states that were used |
| 316 |
// by the last "solve" |
| 317 |
void StatesInPool( std::vector< void* >* stateVec ); |
| 318 |
|
| 319 |
private: |
| 320 |
MicroPather( const MicroPather& ); // undefined and unsupported |
| 321 |
void operator=( const MicroPather ); // undefined and unsupported |
| 322 |
|
| 323 |
void GoalReached( PathNode* node, void* start, void* end, std::vector< void* > *path ); |
| 324 |
void RecPathNodeWalk( PathNode* node, int* count ); |
| 325 |
|
| 326 |
// Allocates and returns memory for a new, unititialized node. |
| 327 |
PathNode* AllocatePathNode(); |
| 328 |
|
| 329 |
// Returns a path node that is ready to go. |
| 330 |
// Strictly speaking, the name is somewhat misleading, as it |
| 331 |
// may be reused from the cache, but it will be ready for use regardless. |
| 332 |
PathNode* NewPathNode( void* state, float costFromStart, float estToEnd, PathNode* parent ); |
| 333 |
|
| 334 |
// Finds the node for state, or returns null. |
| 335 |
// A node that doesn't have a correct "frame" will never be found. |
| 336 |
PathNode* FindPathNode( void* state, bool findStale = false ); |
| 337 |
|
| 338 |
NodeCost* GetNodeNeighbors( PathNode* node, std::vector< StateCost >* neighbor, std::vector< NodeCost >* neighborNode ); |
| 339 |
|
| 340 |
#ifdef DEBUG |
| 341 |
void DumpStats(); |
| 342 |
#endif |
| 343 |
|
| 344 |
enum { |
| 345 |
#ifdef DEBUG_PATH |
| 346 |
HASH_SIZE = 16, // In debug mode, stress the allocators more than in release. |
| 347 |
// The HASH_SIZE must be a power of 2. |
| 348 |
#else |
| 349 |
HASH_SIZE = 256, |
| 350 |
#endif |
| 351 |
}; |
| 352 |
|
| 353 |
unsigned Hash( void* voidval ) { |
| 354 |
UPTR val = (UPTR)voidval; |
| 355 |
return (unsigned)((HASH_SIZE-1) & ( val + (val>>8) + (val>>16) + (val>>24) )); |
| 356 |
} |
| 357 |
|
| 358 |
const unsigned ALLOCATE; // how big a block of pathnodes to allocate at once |
| 359 |
const unsigned BLOCKSIZE; // how many useable pathnodes are in a block |
| 360 |
|
| 361 |
Graph* graph; |
| 362 |
PathNode* hashTable[HASH_SIZE]; // a fixed hashtable used to "jumpstart" a binary search |
| 363 |
PathNode* pathNodeMem; // pointer to root of PathNode blocks |
| 364 |
unsigned availMem; // # PathNodes available in the current block |
| 365 |
unsigned pathNodeCount; // the # of PathNodes in use |
| 366 |
unsigned frame; // incremented with every solve, used to determine |
| 367 |
// if cached data needs to be refreshed |
| 368 |
UPTR checksum; // the checksum of the last successful "Solve". |
| 369 |
|
| 370 |
std::vector< StateCost > neighborVec; // really local vars to Solve, but put here to reduce memory allocation |
| 371 |
std::vector< NodeCost > neighborNodeVec; // really local vars to Solve, but put here to reduce memory allocation |
| 372 |
}; |
| 373 |
|
| 374 |
inline PathNode* MicroPather::FindPathNode( void* state, bool findStale ) |
| 375 |
{ |
| 376 |
unsigned key = Hash( state ); |
| 377 |
|
| 378 |
PathNode* root = hashTable[key]; |
| 379 |
#ifdef USE_BINARY_HASH |
| 380 |
while ( root ) { |
| 381 |
if ( root->state == state ) { |
| 382 |
if ( findStale || root->frame == frame ) |
| 383 |
return root; |
| 384 |
else |
| 385 |
return 0; |
| 386 |
} |
| 387 |
else if ( state > root->state ) { |
| 388 |
root = root->right; |
| 389 |
} |
| 390 |
else { |
| 391 |
assert( state < root->state ); |
| 392 |
root = root->left; |
| 393 |
} |
| 394 |
} |
| 395 |
#else |
| 396 |
while( root ) { |
| 397 |
if ( root->state == state ) { |
| 398 |
if ( findStale || root->frame == frame ) |
| 399 |
return root; |
| 400 |
else |
| 401 |
return 0; |
| 402 |
} |
| 403 |
else { |
| 404 |
root = root->left; |
| 405 |
} |
| 406 |
} |
| 407 |
#endif |
| 408 |
return 0; |
| 409 |
} |
| 410 |
}; // namespace grinliz |
| 411 |
|
| 412 |
#endif |
| 413 |
|