Exemple #1
0
        /// <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);
                    }
                }
            }
        }
Exemple #2
0
        /// <summary>
        /// Seek a target in a 2-dimensional grid
        /// </summary>
        /// <param name="grid">The grid to search</param>
        /// <param name="startPoint">The start point to seek from</param>
        /// <param name="targetPoint">The target to seek to</param>
        /// <param name="cutCorners">Whether or not to cut a corner</param>
        /// <param name="movementCost">The cost to move left/right/up/down from one node to another</param>
        /// <param name="diagonalCost">The cost to move in a diagonal from one node to another</param>
        /// <param name="ascentCost">The additional cost to apply when ascending</param>
        /// <param name="descentCost">The additional cost to apply when descending</param>
        /// <returns>An array of points in world space needed to pass through to get to the target</returns>
        /// <exception cref="ArgumentOutOfRangeException">
        /// If the start or target are out of range of the grid
        /// </exception>
        public static Point2D[] Seek(
            GridNode2D[,] grid, Point2D startPoint, Point2D targetPoint, bool cutCorners,
            int movementCost, int diagonalCost, int ascentCost, int descentCost
        )
        {
            GridCoord2D start = new GridCoord2D((int) startPoint.X, (int) startPoint.Y);
            GridCoord2D target = new GridCoord2D((int) targetPoint.X, (int) targetPoint.Y);

            // Check that the starting position is not out of range of the grid
            if (start.X > grid.GetLength(0) || start.X < 0 || start.Y >= grid.GetLength(1) || start.Y < 0) {
                throw new ArgumentOutOfRangeException(
                    "Invalid starting grid reference. Got " + start.X + "," + start.Y +
                    ". But grid only runs from 0,0 to " + grid.GetLength(0) + "," + grid.GetLength(1)
                );
            }

            // check that the target position is not out of range of the grid
            if (target.X > grid.GetLength(0) || target.X < 0 || target.Y >= grid.GetLength(1) || target.Y < 0) {
                throw new ArgumentOutOfRangeException(
                    "Invalid target grid reference. Got " + target.X + "," + target.Y +
                    ". But grid only runs from 0,0 to " + grid.GetLength(0) + "," + grid.GetLength(1)
                );
            }

            GridNodeHeap2D openNodes = new GridNodeHeap2D();
            bool[,] closedNodes = new bool[grid.GetLength(0), grid.GetLength(1)];
            GridNodeCalc2D currentNode = grid[start.X, start.Y].ToGridNodeCalc2D();
            GridNodeCalc2D targetNode = null;

            // Set the grid position for the first node
            currentNode.GridPosition = new GridCoord2D(start.X, start.Y);

            // Add the starting position to the open list
            openNodes.Add(currentNode);

            // Continue until the open list is empty
            while (openNodes.Count > 0) {
                currentNode = openNodes.Remove();

                // If we have reached the target node then break from the loop
                if (currentNode.GridPosition == target) {
                    targetNode = currentNode;
                    break;
                }

                // Add the current node to the closed list
                closedNodes[currentNode.GridPosition.X, currentNode.GridPosition.Y] = true;

                // Add additional nodes to the open list, ready to search
                calculateOpenList(
                    currentNode, target, grid, openNodes, closedNodes, cutCorners,
                    movementCost, diagonalCost, ascentCost, descentCost
                );
            }

            if (targetNode == null) {
                return null;
            }

            List<Point2D> path = new List<Point2D>();
            path.Add(targetNode.ReferenceNode.Position);

            while (targetNode.Parent != null) {
                path.Add(targetNode.Parent.ReferenceNode.Position);
                targetNode = targetNode.Parent;
            }

            // The path will be from the target to the start point...
            // reverse it so it makes sense to the consumer
            path.Reverse();

            // Return the new array of path points
            return path.ToArray();
        }