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; } }