public void zeroAdjacent(ASNode **ptr)
{
ASNode **end = ptr + 8;
while (ptr != end)
{
(*(ptr++)) = (ASNode *)0;
}
}
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 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;
}
private static void initAdjacent(out ASNode **adjacent, out int *adjacentLocations)
{
adjacent = (ASNode **)Marshal.AllocHGlobal(8 * sizeof(ASNode *));
adjacentLocations = (int *)Marshal.AllocHGlobal(16 * sizeof(int));
}
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;
}
}
