|
// |
|
// Pathfinder.m |
|
// MazeGame |
|
// |
|
// Created by Jonathan Fischer on 3/13/12. |
|
// |
|
|
|
#import "Pathfinder.h" |
|
#import "Path.h" |
|
|
|
static const float kStraightMovementCost = 1.0f; |
|
static const float kDiagonalMovementCost = 1.4142135f; |
|
static const float kTieBreakerFudge = 1.0f + (1.0f / 1000.0f); |
|
|
|
static float DistanceEstimate(MapCoordinate start, MapCoordinate end) |
|
{ |
|
return fmaxf(fabsf(start.x - end.x), fabsf(start.y - end.y)); |
|
} |
|
|
|
@implementation Pathfinder |
|
@synthesize collisionMap = _collisionMap; |
|
|
|
- (id)initWithCollisionMap:(CollisionMap *)collisionMap |
|
{ |
|
self = [super init]; |
|
if (self) { |
|
[self setCollisionMap:collisionMap]; |
|
} |
|
|
|
return self; |
|
} |
|
|
|
- (void)dealloc |
|
{ |
|
[self setCollisionMap:nil]; |
|
[super dealloc]; |
|
} |
|
|
|
- (void)setCollisionMap:(CollisionMap *)collisionMap |
|
{ |
|
if (collisionMap == nil) { |
|
[_collisionMap release]; |
|
free(_cells); |
|
|
|
_cells = NULL; |
|
_cellsWide = 0; |
|
_cellsHigh = 0; |
|
} else { |
|
CollisionMap *map = [collisionMap retain]; |
|
[_collisionMap release]; |
|
_collisionMap = map; |
|
|
|
free(_cells); |
|
_cellsWide = [collisionMap tilesWide]; |
|
_cellsHigh = [collisionMap tilesHigh]; |
|
int numCells = _cellsWide * _cellsHigh; |
|
_cells = (PathfindingCell *)calloc(sizeof(PathfindingCell), numCells); |
|
} |
|
} |
|
|
|
- (void)reset |
|
{ |
|
int numCells = _cellsWide * _cellsHigh; |
|
memset(_cells, 0, sizeof(PathfindingCell) * numCells); |
|
|
|
for (int y = 0; y < _cellsHigh; y++) { |
|
for (int x = 0; x < _cellsWide; x++) { |
|
PathfindingCell *cell = &(_cells[x + y * _cellsWide]); |
|
cell->coordinate.x = x; |
|
cell->coordinate.y = y; |
|
} |
|
} |
|
|
|
_openListHead = NULL; |
|
_closedListHead = NULL; |
|
} |
|
|
|
- (void)addToOpenList:(PathfindingCell *)cell |
|
{ |
|
if (_openListHead == NULL) { |
|
_openListHead = cell; |
|
_openListHead->openNext = NULL; |
|
_openListHead->openPrevious = NULL; |
|
} else { |
|
// Check first to see if this new node goes at the front of the list |
|
if (cell->f < _openListHead->f) { |
|
cell->openNext = _openListHead; |
|
cell->openPrevious = NULL; |
|
_openListHead->openPrevious = cell; |
|
|
|
_openListHead = cell; |
|
} else { |
|
PathfindingCell *current = _openListHead; |
|
PathfindingCell *last = _openListHead; |
|
|
|
while (current != NULL) { |
|
if (cell->f < current->f) { |
|
cell->openPrevious = current->openPrevious; |
|
cell->openNext = current; |
|
current->openPrevious = cell; |
|
|
|
if (cell->openPrevious != NULL) { |
|
cell->openPrevious->openNext = cell; |
|
} |
|
break; |
|
} |
|
|
|
last = current; |
|
current = current->openNext; |
|
} |
|
|
|
// If we got this far, then we made it all the way to the |
|
// end of the list. In that case, last will still point |
|
// to the end, so append the new cell there. |
|
last->openNext = cell; |
|
cell->openPrevious = last; |
|
} |
|
} |
|
} |
|
|
|
- (void)addToClosedList:(PathfindingCell *)cell |
|
{ |
|
if (_closedListHead == NULL) { |
|
_closedListHead = cell; |
|
cell->closedNext = NULL; |
|
cell->closedPrevious = NULL; |
|
} else { |
|
cell->closedNext = _closedListHead; |
|
cell->closedPrevious = NULL; |
|
_closedListHead->closedPrevious = cell; |
|
_closedListHead = cell; |
|
} |
|
} |
|
|
|
- (PathfindingCell *)firstCellFromOpenList |
|
{ |
|
if (_openListHead == NULL) { |
|
return NULL; |
|
} else { |
|
PathfindingCell *cell = _openListHead; |
|
_openListHead = _openListHead->openNext; |
|
|
|
if (_openListHead) { |
|
_openListHead->openPrevious = NULL; |
|
} |
|
|
|
cell->openPrevious = cell->openNext = NULL; |
|
return cell; |
|
} |
|
} |
|
|
|
- (void)removeCellFromOpenList:(PathfindingCell *)cell |
|
{ |
|
if (cell == NULL) return; |
|
|
|
if (cell == _openListHead) { |
|
_openListHead = cell->openNext; |
|
} |
|
|
|
if (cell->openPrevious != NULL) { |
|
cell->openPrevious->openNext = cell->openNext; |
|
} |
|
|
|
if (cell->openNext != NULL) { |
|
cell->openNext->openPrevious = cell->openPrevious; |
|
} |
|
|
|
cell->openPrevious = NULL; |
|
cell->openNext = NULL; |
|
} |
|
|
|
- (void)removeCellFromClosedList:(PathfindingCell *)cell |
|
{ |
|
if (cell == NULL) return; |
|
|
|
if (cell == _closedListHead) { |
|
_closedListHead = cell->closedNext; |
|
} |
|
|
|
if (cell->closedPrevious != NULL) { |
|
cell->closedPrevious->closedNext = cell->closedNext; |
|
} |
|
|
|
if (cell->closedNext != NULL) { |
|
cell->closedNext->closedPrevious = cell->closedPrevious; |
|
} |
|
|
|
cell->closedPrevious = NULL; |
|
cell->closedNext = NULL; |
|
} |
|
|
|
- (BOOL)cellInOpenList:(PathfindingCell *)cell |
|
{ |
|
if (cell == NULL) return NO; |
|
return _openListHead == cell || cell->openPrevious || cell->openNext; |
|
} |
|
|
|
- (BOOL)cellInClosedList:(PathfindingCell *)cell |
|
{ |
|
if (cell == NULL) return NO; |
|
return _closedListHead == cell || cell->closedPrevious || cell->closedNext; |
|
} |
|
|
|
- (Path*)pathFromStart:(CGPoint)startPosition toEnd:(CGPoint)endPosition |
|
{ |
|
if (_collisionMap == nil) { |
|
NSLog(@"PathfindingMap: No collision map to find a path on."); |
|
return nil; |
|
} |
|
|
|
[self reset]; |
|
|
|
MapCoordinate start, end; |
|
start = [_collisionMap coordinateForPosition:startPosition]; |
|
end = [_collisionMap coordinateForPosition:endPosition]; |
|
|
|
if (MapCoordinatesAreEqual(start, end)) { |
|
NSLog(@"Pathfinder - Start and end points are within the same tile."); |
|
|
|
Path *path = [[Path alloc] init]; |
|
[path addWaypoint:endPosition]; |
|
return path; |
|
} |
|
|
|
PathfindingCell *startCell = &(_cells[start.x + (start.y * _cellsWide)]); |
|
startCell->g = 0.0f; |
|
startCell->h = DistanceEstimate(start, end); |
|
startCell->f = startCell->h; |
|
|
|
[self addToOpenList:startCell]; |
|
|
|
while (_openListHead != NULL) { |
|
PathfindingCell *current = [self firstCellFromOpenList]; |
|
[self addToClosedList:current]; |
|
|
|
if (MapCoordinatesAreEqual(current->coordinate, end)) { |
|
// We've made it to the destination node. Build a path using |
|
// the parent pointers in the path nodes |
|
Path *path = [[Path alloc] init]; |
|
PathfindingCell *cell = current; |
|
while (cell != nil) { |
|
CGPoint position = [_collisionMap centerOfTileAtCoordinate:cell->coordinate]; |
|
[path addWaypoint:position]; |
|
cell = cell->parent; |
|
} |
|
|
|
[path reversePath]; |
|
|
|
return path; |
|
} |
|
|
|
// For each neighbor of current (all 8 of them) |
|
MapCoordinate currentCoordinate, neighborCoordinate; |
|
currentCoordinate = current->coordinate; |
|
|
|
float newCost; |
|
|
|
for (int y = -1; y < 2; y++) { |
|
for (int x = -1; x < 2; x++) { |
|
// Don't consider the current coordinate |
|
if (x == 0 && y == 0) { |
|
continue; |
|
} |
|
|
|
// Figure out how much it will cost to move to the next tile. Moving diagonally |
|
// costs a little bit more than moving straight up and down, to help favor straight |
|
// paths. |
|
if (x == 0 || y == 0) { |
|
newCost = current->g + kStraightMovementCost; |
|
} else { |
|
newCost = current->g + kDiagonalMovementCost; |
|
} |
|
|
|
neighborCoordinate.x = currentCoordinate.x + x; |
|
neighborCoordinate.y = currentCoordinate.y + y; |
|
if (neighborCoordinate.x < 0 || neighborCoordinate.x >= _cellsWide) { |
|
continue; |
|
} |
|
|
|
if (neighborCoordinate.y < 0 || neighborCoordinate.y >= _cellsHigh) { |
|
continue; |
|
} |
|
|
|
// Is this tile a wall? |
|
CollisionTile tileProperties = [_collisionMap tileAtCoordinate:neighborCoordinate]; |
|
if (tileProperties.value > 1) { |
|
continue; |
|
} |
|
|
|
PathfindingCell *neighborCell = &(_cells[neighborCoordinate.x + (neighborCoordinate.y * _cellsWide)]); |
|
|
|
// Is the tile already in the open list? |
|
if ([self cellInOpenList:neighborCell]) { |
|
if (newCost < neighborCell->g) { |
|
// This new path is better than the one already in the open list, so |
|
// remove it from the existing one. |
|
[self removeCellFromOpenList:neighborCell]; |
|
} |
|
} |
|
|
|
// Is the cell already in the closed list? |
|
if ([self cellInClosedList:neighborCell]) { |
|
if (newCost < neighborCell->g) { |
|
// This should never happen if we have a monotone admissable heuristic. |
|
// However, games often have inadmissable heuristics, so we need to check |
|
// for it. |
|
[self removeCellFromClosedList:neighborCell]; |
|
} |
|
} |
|
|
|
if (![self cellInClosedList:neighborCell] && |
|
![self cellInOpenList:neighborCell]) { |
|
float distance = DistanceEstimate(neighborCoordinate, end) * kTieBreakerFudge; |
|
|
|
neighborCell->g = newCost; |
|
neighborCell->h = distance; |
|
neighborCell->f = newCost + distance; |
|
neighborCell->parent = current; |
|
[self addToOpenList:neighborCell]; |
|
} |
|
} |
|
} |
|
} |
|
|
|
NSLog(@"Failed to find a path from %.1f, %.1f to %.1f, %.1f!", startPosition.x, startPosition.y, |
|
endPosition.x, endPosition.y); |
|
return nil; |
|
} |
|
|
|
@end |