/// <summary> /// Calculates the next set of nodes to add to the open list and adds them /// </summary> /// <param name="currentNode">The node we're calculating from</param> /// <param name="target">The target node</param> /// <param name="grid">The grid to search</param> /// <param name="openNodes">The open list to add to</param> /// <param name="closedNodes">The closed list to check</param> /// <param name="cutCorners">Whether to cut corners</param> /// <param name="movementCost">The cost of moving up/down/left/right</param> /// <param name="diagonalCost">The cost of moving diagonally</param> /// <param name="ascentCost">The additional cost of ascending</param> /// <param name="descentCost">The additional cost of descending</param> private static void calculateOpenList( GridNodeCalc2D currentNode, GridCoord2D target, GridNode2D[,] grid, GridNodeHeap2D openNodes, bool[,] closedNodes, bool cutCorners, int movementCost, int diagonalCost, int ascentCost, int descentCost ) { GridCoord2D currentGridPos = currentNode.GridPosition; // Make sure the X and Y coordinates are always within the grid boundaries int minX = Math.Max(currentGridPos.X - 1, 0); int maxX = Math.Min(currentGridPos.X + 1, grid.GetLength(0) - 1); int minY = Math.Max(currentGridPos.Y - 1, 0); int maxY = Math.Min(currentGridPos.Y + 1, grid.GetLength(1) - 1); for (int x = minX; x <= maxX; x++) { for (int y = minY; y <= maxY; y++) { GridNodeCalc2D node = grid[x, y].ToGridNodeCalc2D(); node.GridPosition = new GridCoord2D(x, y); // Skip over this node if it's the one we're currently looking at, or it's in the closed list if ( node.GridPosition == currentGridPos || closedNodes[node.GridPosition.X, node.GridPosition.Y]) { continue; } // Also skip if it's impassable if (node.ReferenceNode.Passable == false) { continue; } bool isDiagonal = ( node.GridPosition.X != currentGridPos.X && node.GridPosition.Y != currentGridPos.Y ); // If we're not meant to cut corners, tehn we need to check this now if (isDiagonal && !cutCorners && isCuttingCorner(node, currentGridPos, grid)) { continue; } // Is this node on the open list already? int refIndex = openNodes.IndexOf(node); if (refIndex == -1) { // If not then set the parent to the current node and calculate the cost node.Parent = currentNode; // Calculate the movement cost to this node node.MovementCost = currentNode.TotalCost; node.MovementCost += calculateMovementCost( movementCost, diagonalCost, ascentCost, descentCost, isDiagonal, (node.GridPosition.Y < currentGridPos.Y), (node.GridPosition.Y > currentGridPos.Y) ); // Calculate the heuristic cost node.HeuristicCost = ( Math.Abs(node.GridPosition.X - target.X) + Math.Abs(node.GridPosition.Y - target.Y) ); openNodes.Add(node); continue; } GridNodeCalc2D refNode = openNodes[refIndex]; int currentNodeCost = currentNode.MovementCost + currentNode.ReferenceNode.Penalty; int refNodeCost = refNode.MovementCost + refNode.ReferenceNode.Penalty; // The node was in the open list, so see if it costs less to move there if (refNodeCost < currentNodeCost) { // If so, set then set the parent to the current node refNode.Parent = currentNode.Parent; // Re-calculate the movement cost using the new parent refNode.MovementCost = refNode.Parent.TotalCost; refNode.MovementCost += calculateMovementCost( movementCost, diagonalCost, ascentCost, descentCost, isDiagonal, (refNode.GridPosition.Y < currentGridPos.Y), (refNode.GridPosition.Y > currentGridPos.Y) ); openNodes.Sort(refIndex); } } } }