private static ASNode *initNodeMatrix(int w, int h)
{
//allocate the matrix
int blockSize = w * h * sizeof(ASNode);
ASNode *matrix = (ASNode *)Marshal.AllocHGlobal(blockSize);
ZeroMemory((byte *)matrix, blockSize);
return(matrix);
}
public bool search(ASNode *firstNode, ASNode *endNode)
{
//return searchRecursive(firstNode, 0);
PtrQueue nodeStack = p_Queue;
nodeStack.Clear();
nodeStack.Push(firstNode);
int count = 1;
while (count != 0)
{
ASNode *current = (ASNode *)nodeStack.Pop();
(*current).State = ASNodeState.CLOSED;
count--;
//sort by total cost
/*
* adjacent.Sort(delegate(ASNode n1, ASNode n2) {
* float n1F = n1.G + n1.H;
* float n2F = n2.G + n2.H;
* return n1F.CompareTo(n2F);
* });
*/
//get the adjacent nodes
getAdjacent(current);
ASNode **adjacent = p_AdjacentNodes;
ASNode **adjacentEnd = adjacent + 8;
while (adjacent != adjacentEnd)
{
if ((*adjacent) == (ASNode *)0)
{
break;
}
//we hit the end?
if (*adjacent == endNode)
{
return(true);
}
//get the pointer for this node
nodeStack.Push(*adjacent);
adjacent++;
count++;
}
}
/*no path was found*/
return(false);
}
public ASContext(int width, int height, int stackSize)
{
p_Width = width;
p_Height = height;
/*allocate*/
initAdjacent(out p_AdjacentNodes, out p_AdjacentLocations);
p_NodeMatrix = initNodeMatrix(width, height);
p_Queue = new PtrQueue(stackSize);
int sz = sizeof(ASNode);
return;
}
public void Dispose()
{
//already disposed?
if (p_AdjacentLocations == (int *)0)
{
return;
}
Marshal.FreeHGlobal((IntPtr)p_AdjacentNodes);
Marshal.FreeHGlobal((IntPtr)p_AdjacentLocations);
Marshal.FreeHGlobal((IntPtr)p_NodeMatrix);
p_Queue.Dispose();
p_AdjacentNodes = (ASNode **)0;
p_AdjacentLocations = (int *)0;
p_NodeMatrix = (ASNode *)0;
}
public static List <Point> ASSearch(IPathfinderContext context, Point start, Point end, bool *concreteMatrix)
{
int startX = start.X, startY = start.Y;
int endX = end.X, endY = end.Y;
int width = context.Width;
int height = context.Height;
//validate context
if (!(context is ASContext) || context == null)
{
throw new Exception("Invalid A* search context!");
}
ASContext ctx = context as ASContext;
//verify start/end is not collidable!
bool startCollide = *(concreteMatrix + (startY * width) + startX);
bool endCollide = *(concreteMatrix + (endY * width) + endX);
if (startCollide || endCollide)
{
bool resolved = false;
/*attempt to resolve the collision*/
if (startCollide)
{
resolved = ResolveCollision(ref start, end, width, height, concreteMatrix);
}
if (endCollide)
{
resolved = ResolveCollision(ref end, start, width, height, concreteMatrix);
}
//not resolved?
if (!resolved)
{
return(new List <Point>());
}
startX = start.X; startY = start.Y;
endX = end.X; endY = end.Y;
}
//already there?
if (startX == endX && startY == endY)
{
return(new List <Point>());
}
//reset the context
ASNode *nodeMatrix = ctx.reset(
concreteMatrix,
endX,
endY);
//get the node at the start and end
ASNode *startNode = nodeMatrix + (startY * width) + startX;
ASNode *endNode = nodeMatrix + (endY * width) + endX;
//search
bool found = ctx.search(startNode, endNode);
if (!found)
{
return(new List <Point>());
}
//cycle back from the end point all the way back to the start
//adding each node location on the way.
//note: since we have done this backwards, just reverse the list
ASNode * current = endNode;
List <Point> buffer = new List <Point>();
buffer.Add(end);
while (current != startNode)
{
//get the X/Y from the pointer.
short x = (short)((current - nodeMatrix) % width);
short y = (short)((current - nodeMatrix) / width);
buffer.Add(new Point(x, y));
//resolve the parent
ASNode *parent = nodeMatrix + ((*current).ParentY * width) + (*current).ParentX;
current = parent;
}
buffer.Add(start);
buffer.Reverse();
return(buffer);
}
private static void resetNodeMatrix(ASNode *matrix, int w, int h)
{
ZeroMemory((byte *)matrix, (w * h) * sizeof(ASNode));
}
public void getAdjacent(ASNode *current)
{
//zero fill the current adjacent nodes
ASNode **buffer = p_AdjacentNodes;
zeroAdjacent(buffer);
//populate adjacent
short currentX = (short)((current - p_NodeMatrix) % p_Width);
short currentY = (short)((current - p_NodeMatrix) / p_Width);
float currentG = (*current).G;
getAdjacentLocations(currentX, currentY, p_AdjacentLocations);
//iterate through each adjacent location
int *adjacentLocationPtr = p_AdjacentLocations;
int *adjacentLocationEnd = p_AdjacentLocations + 16;
int *adjacentLocationDiagonals = p_AdjacentLocations + 8;
bool allowDiagonal = true;
while (adjacentLocationPtr != adjacentLocationEnd)
{
//read x,y
int x = *(adjacentLocationPtr++);
int y = *(adjacentLocationPtr++);
//bound check
if (x < 0 || y < 0 ||
x >= p_Width || y >= p_Height)
{
continue;
}
//only allow diagonals when all adjacent NESW blocks
//are not collidable.
bool isDiagonal = adjacentLocationPtr > adjacentLocationDiagonals;
if (isDiagonal && !allowDiagonal)
{
continue;
}
//get the node at this location
int offset = (y * p_Width) + x;
bool isConcrete = *(p_ConcreteMatrix + offset);
ASNode *node = p_NodeMatrix + offset;
//solid?
if (isConcrete)
{
allowDiagonal = false;
continue;
}
//already closed?
if ((*node).State == ASNodeState.CLOSED)
{
continue;
}
//open?
if ((*node).State == ASNodeState.OPEN)
{
//get the distance from current node to this node.
float distance = calcH(x, y, currentX, currentY);
float gNew = currentG + distance;
if (gNew < (*node).G)
{
(*node).ParentX = currentX;
(*node).ParentY = currentY;
(*node).G = gNew;
(*(buffer++)) = node;
}
continue;
}
//not tested
(*node).ParentX = currentX;
(*node).ParentY = currentY;
(*node).G = currentG + calcH(
x, y,
currentX, currentY);
(*node).State = ASNodeState.OPEN;
(*(buffer++)) = node;
}
}
