private void FindPath() { _iterations++; if (WayPoint.Count > 0) { //log.Debug("WayPoint.Count > 0"); return; } if (_iterations > 500) { //log.Debug("Iterations > 50"); return; } // pop the top item off the openlist and find surrounding cells var node = _openList.Pop(); //log.DebugFormat("FindPath({0},{1})", node.X, node.Y); _closedList.Push(node); var surroundingCells = _terrainMap.FindAdjacentCells(node.X, node.Y, _unitType, 1); foreach (var mapCoordinates in surroundingCells) { // skip any nodes that are already in the closed list if (!_closedList.Contains(mapCoordinates.Col, mapCoordinates.Row)) { var distance = node.G; if (_terrainMap[mapCoordinates.Col, mapCoordinates.Row].Roads > 0) { distance += 1; } else { distance += 3; } if (_openList.Contains(mapCoordinates.Col, mapCoordinates.Row)) { // check to see if this path is shorter than the one on the open list, if so, then update it, otherwise skip var tempNode = new AStarNode(node.X, node.Y, mapCoordinates.Col, mapCoordinates.Row, _endCol, _endRow, distance); _openList.UpdateNodeIfBetter(tempNode); } else { _openList.Push(new AStarNode(node.X, node.Y, mapCoordinates.Col, mapCoordinates.Row, _endCol, _endRow, distance)); } } } var smallestNode = _openList.FindSmallestNode(); if (smallestNode == null) { //log.Debug("smallestNode == null"); return; } // check if this is the destination node if (smallestNode.X == _endCol && smallestNode.Y == _endRow) { // consolidate the actual path into the WayPoint list WayPoint.Add(new Offset(_endCol, _endRow)); // walk back to the starting point var tempNode = _closedList.GetNode(smallestNode.Source.X, smallestNode.Source.Y); while (tempNode.X != _startCol || tempNode.Y != _startRow) { WayPoint.Insert(0, new Offset(tempNode.X, tempNode.Y)); tempNode = _closedList.GetNode(tempNode.Source.X, tempNode.Source.Y); } // clear the open and closed lists _openList.Clear(); _closedList.Clear(); //log.Debug("success"); return; } _openList.Push(smallestNode); FindPath(); }
public void Push(AStarNode node) { _items.Add(node); }