Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
A* Implementation
//
// 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
// 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
// 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
// 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
// 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
// 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
// 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
// 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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.