A* Implementation

CollisionMap.h

//
//  CollisionMap.h
//  MazeGame
//
//  Created by Jonathan Fischer on 2/25/12.
//

#import <Foundation/Foundation.h>
#include "MapCommon.h"
@class Path;

typedef struct CollisionTile
{
    int value;
}CollisionTile;

@interface CollisionMap : NSObject
{
    int _tilesWide;
    int _tilesHigh;
    CGSize _tileSize;
    
    CollisionTile *_tiles;
}
- (id)initWithWidth:(int)width height:(int)height;

@property (nonatomic, readonly) int tilesWide;
@property (nonatomic, readonly) int tilesHigh;
@property (nonatomic, assign) CGSize tileSize;

- (MapCoordinate)coordinateForPosition:(CGPoint)point;
- (CGPoint)centerOfTileAtCoordinate:(MapCoordinate)coordinate;

- (Path *)pathFromPosition:(CGPoint)origin toPosition:(CGPoint)target;

- (void)setTile:(CollisionTile)tile atCoordinate:(MapCoordinate)coordinate;
- (CollisionTile)tileAtCoordinate:(MapCoordinate)coordinate;

@end

CollisionMap.m

//
//  CollisionMap.m
//  MazeGame
//
//  Created by Jonathan Fischer on 2/25/12.
//

#import "CollisionMap.h"
#import "Path.h"

@implementation CollisionMap

@synthesize tilesWide = _tilesWide;
@synthesize tilesHigh = _tilesHigh;
@synthesize tileSize = _tileSize;

- (id)initWithWidth:(int)width height:(int)height
{
    self = [super init];
    if (self) {
        _tilesWide = width;
        _tilesHigh = height;
        
        int numTiles = width * height;
        _tiles = (CollisionTile *)calloc(numTiles, sizeof(CollisionTile));
        
        for (int i = 0;i < numTiles; i++) {
            _tiles[i].value = 1;
        }
    }
    
    return self;
}

- (MapCoordinate)coordinateForPosition:(CGPoint)point
{
    if (CGSizeEqualToSize(_tileSize, CGSizeZero)) {
        return MakeMapCoordinate(0, 0);
    } else {
        float px = point.x / _tileSize.width;
        float py = point.y / _tileSize.height;
        return MakeMapCoordinate(floorf(px), floorf(py));
    }
}

- (CGPoint)centerOfTileAtCoordinate:(MapCoordinate)coordinate
{
    if (CGSizeEqualToSize(_tileSize, CGSizeZero)) {
        return CGPointZero;
    } else {
        float px = coordinate.x * _tileSize.width + (_tileSize.width / 2.0f);
        float py = coordinate.y * _tileSize.height + (_tileSize.height / 2.0f);
        return CGPointMake(px, py);
    }
}
- (Path *)pathFromPosition:(CGPoint)origin toPosition:(CGPoint)target
{
    //MapCoordinate sourceCoordinate = [self coordinateForPosition:origin];
    MapCoordinate targetCoordinate = [self coordinateForPosition:target];

    CollisionTile targetTile = [self tileAtCoordinate:targetCoordinate];
    if (targetTile.value != 1) {
        return nil;
    }
    
    // For now just do a really basic path
    Path *path = [[Path alloc] init];
    [path addWaypoint:target];
    return path;
}

- (void)setTile:(CollisionTile)tile atCoordinate:(MapCoordinate)coordinate
{
    if (coordinate.x < 0 || coordinate.x >= _tilesWide ||
        coordinate.y < 0 || coordinate.y >= _tilesHigh) {
        return;
    }
    
    _tiles[coordinate.x + (coordinate.y * _tilesWide)] = tile;
}

- (CollisionTile)tileAtCoordinate:(MapCoordinate)coordinate
{
    if (coordinate.x < 0 || coordinate.x >= _tilesWide ||
        coordinate.y < 0 || coordinate.y >= _tilesHigh) {
        return _tiles[0];
    }
    
    return _tiles[coordinate.x + (coordinate.y * _tilesWide)];
}

@end

MapCommon.c

//
//  MapCommon.c
//  MazeGame
//
//  Created by Jonathan Fischer on 3/4/12.
//

#include "MapCommon.h"

MapCoordinate MakeMapCoordinate(int x, int y)
{
    MapCoordinate coord = {
        .x = x,
        .y = y
    };
    return coord;
}

bool MapCoordinatesAreEqual(MapCoordinate coord1, MapCoordinate coord2)
{
    return (coord1.x == coord2.x && coord1.y == coord2.y);
}

MapCommon.h

//
//  MapCommon.h
//  MazeGame
//
//  Created by Jonathan Fischer on 3/4/12.
//

#ifndef MazeGame_MapCommon_h
#define MazeGame_MapCommon_h

#include <stdbool.h>

typedef struct MapCoordinate
{
    int x, y;
}MapCoordinate;

extern MapCoordinate MakeMapCoordinate(int x, int y);
extern bool MapCoordinatesAreEqual(MapCoordinate coord1, MapCoordinate coord2);

#endif

Path.h

//
//  Path.h
//  MazeGame
//
//  Created by Jonathan Fischer on 2/25/12.
//

#import <Foundation/Foundation.h>

@interface WaypointWrapper : NSObject
@property (nonatomic, assign) CGPoint waypoint;
@end

@interface Path : NSObject
{
    NSMutableArray *_waypoints;
}

@property (nonatomic, readonly) int numWaypoints;

- (void)addWaypoint:(CGPoint)waypoint;
- (CGPoint)popWaypoint;
- (void)reversePath;

- (NSArray *)waypoints;

@end

Path.m

//
//  Path.m
//  MazeGame
//
//  Created by Jonathan Fischer on 2/25/12.
//

#import "Path.h"

@implementation WaypointWrapper
@synthesize waypoint;
@end

@implementation Path

- (id)init
{
    self = [super init];
    if (self) {
        _waypoints = [[NSMutableArray alloc] init];
    }
    return self;
}

- (void)dealloc
{
    [_waypoints release];
    [super dealloc];
}

- (int)numWaypoints
{
    return [_waypoints count];
}

- (void)addWaypoint:(CGPoint)waypoint
{
    WaypointWrapper *wrapper = [[WaypointWrapper alloc] init];
    [wrapper setWaypoint:waypoint];
    [_waypoints addObject:wrapper];
    [wrapper release];
}

- (CGPoint)popWaypoint
{
    WaypointWrapper *wrapper = [_waypoints objectAtIndex:0];
    CGPoint waypoint = [wrapper waypoint];
    
    [_waypoints removeObjectAtIndex:0];
    return waypoint;
}

// Reverse the order of waypoints in the path.  Useful for building a path with A*
- (void)reversePath
{
    int first = 0, last = [_waypoints count] - 1;
    while (first < last) {
        [_waypoints exchangeObjectAtIndex:first withObjectAtIndex:last];
        first++;
        last--;
    }
}

- (NSArray *)waypoints
{
    return [_waypoints copy];
}

@end

Pathfinder.h

//
//  Pathfinder.h
//  MazeGame
//
//  Created by Jonathan Fischer on 3/13/12.
//

#import <Foundation/Foundation.h>
#import "CollisionMap.h"

struct PathfindingCell
{
    MapCoordinate coordinate;
    float f, g, h;
    struct PathfindingCell *parent;
    struct PathfindingCell *openPrevious, *openNext;
    struct PathfindingCell *closedPrevious, *closedNext;
};

typedef struct PathfindingCell PathfindingCell;

@interface Pathfinder : NSObject
{
    CollisionMap *_collisionMap;
    
    int _cellsWide;
    int _cellsHigh;
    PathfindingCell *_cells;
    
    PathfindingCell *_openListHead;
    PathfindingCell *_closedListHead;
}

- (id)initWithCollisionMap:(CollisionMap *)collisionMap;

@property (nonatomic, retain) CollisionMap *collisionMap;

- (Path*)pathFromStart:(CGPoint)startPosition toEnd:(CGPoint)endPosition;

@end

Pathfinder.m

//
//  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