/// <summary> /// Initializes a discrete grid using the information provided. /// </summary> /// <param name="startPoint">The point to start the route at</param> /// <param name="endPoint">The point to end the route at</param> /// <param name="field">The field that needs to be discretised</param> /// <param name="gridSize">The size of the grid to be palced placed over <paramref name="field"/></param> /// <param name="movingObject">The object that will be moving.</param> /// <returns>A GridSquare array, initialized to represent <paramref name="field"/>.</returns> /// /// Produces a GridSquare array that is set up for use by <see cref="FindPath"/>. /// /// Uses the ObjectClearance and <paramref name="movingObject" />'s <see cref="IPositionedObject.Size"/> property to determine the /// apparent size of opponents, and marks the squares that contain them as <see cref="SquareType.Obstacle"/>. /// /// Marks the square that contains <paramref name="endPoint" /> as <see cref="SquareType.Destination"/>. /// /// Marks the square that contains <paramref name="startPoint" /> as <see cref="SquareType.Origin"/>. /// /// Works in parallel as far as possible, using the Microsoft Task Parallel Library (http://msdn.microsoft.com/en-us/library/dd460717.aspx). protected GridSquare[] InitGrid(PointF startPoint, PointF endPoint, Field field, Size gridSize, IPositionedObject movingObject) { var playersize = (movingObject.Size + new Size(ObjectClearance, ObjectClearance)).Scale(Resolution).Scale(2.0f).Ceiling(); var clearance = Math.Max(playersize.Width, playersize.Height); // The amount to increase an obstacle's size by to allow for the player's size var grid = new GridSquare[gridSize.Height * gridSize.Width]; // Initialize the grid Parallel.For(0, grid.Length, i => { grid[i] = new GridSquare { Location = PointExtensions.FromIndex(i, gridSize.Width) }; }); Parallel.ForEach(from p in field.Players where p.Team == Team.Opposition select p, player => { var centerGridPoint = player.Position.Scale(Resolution).Floor(); var minX = Math.Max(0, centerGridPoint.X - playersize.Width - clearance); var maxX = Math.Min(centerGridPoint.X + playersize.Width + clearance, gridSize.Width); var minY = Math.Max(0, centerGridPoint.Y - playersize.Height - clearance); var maxY = Math.Min(centerGridPoint.Y + playersize.Height + clearance, gridSize.Height); for (var i = minX; i < maxX; i++) { for (var j = minY; j < maxY; j++) { if (i < 0 || j < 0) { continue; } var gridPoint = new Point(i, j); grid[gridPoint.ToIndex(gridSize.Width)].Type = SquareType.Obstacle; } } }); var gridEndPoint = endPoint.Scale(Resolution).Floor(); grid[gridEndPoint.ToIndex(gridSize.Width)].Type = SquareType.Destination; var gridStartPoint = startPoint.Scale(Resolution).Floor(); grid[gridStartPoint.ToIndex(gridSize.Width)].Type = SquareType.Origin; return(grid); }
/// <summary> /// Finds a route using the A* algorithm /// </summary> /// <param name="startPoint">The point to start the algorithm from.</param> /// <param name="endPoint">The point to find a route to.</param> /// <param name="field">The field in which the route must be found.</param> /// <param name="movingObject">The object that will move around the <paramref name="field"/>.</param> /// <returns> /// A Route if a route can be found. /// /// null otherwise /// </returns> /// <exception cref="InvalidOperationException"> /// Thrown if the specified resolution is 0 in one or more directions. /// </exception> /// /// Determines a route from <paramref name="startPoint" /> to <paramref name="endPoint" /> using the A* algorithm. /// /// Algorithm taken from @cite aiModernApproach public virtual Route FindPath(PointF startPoint, PointF endPoint, Field field, IPositionedObject movingObject) { if (Resolution.Height < 1 || Resolution.Width < 1) { throw new InvalidOperationException("Resolution must be greater than or equal to 1 pixel by 1 pixel"); } var gridSize = field.Size.Scale(Resolution).Ceiling(); var grid = InitGrid(startPoint, endPoint, field, gridSize, movingObject); var startPoints = from g in grid where g.Type == SquareType.Origin select g; var closedSet = new TStorageClass(); // The already checked points var openSet = new TStorageClass(); foreach (var f in from g in grid where g.Type == SquareType.Origin select g) // The points to check { openSet.Add(f); } var cameFrom = new Dictionary <GridSquare, GridSquare>(); // A list of route data already calculated // Initialise the origin points Parallel.ForEach(startPoints, g => { g.KnownScore = 0; g.HeuristicScore = CalculateHeuristic(g, endPoint); }); // While there are points available to check while (openSet.Any()) { var square = openSet.OrderBy(p => p.TotalScore).First(); if (square.Type == SquareType.Destination) { var path = ReconstructPath(cameFrom, cameFrom[square]); path.Path.Add(new LineSegment(cameFrom[square].Location, square.Location)); path.Scale(Resolution.Invert()); // Convert the path back to world coordinates from grid coordinates return(path); } openSet.Remove(square); closedSet.Add(square); var index = square.Location.ToIndex(gridSize.Width); // Calculate the neighboring indexes and discard the ones out of range. var neighbourIndexes = new[] { index + 1, index - 1, index + gridSize.Width, index - gridSize.Width, index + gridSize.Width + 1, index - gridSize.Width + 1, index + gridSize.Width - 1, index - gridSize.Width - 1 }.Where(i => (i >= 0) && (i < grid.Length)); foreach (var i in neighbourIndexes) { var neighbour = grid[i]; if (closedSet.Contains(neighbour)) { continue; // Already checked this square. Skip it } // Work out the distance to the origin var tentativeKnownScore = square.KnownScore + CalculateLength(square.Location, neighbour.Location); bool tentativeIsBetter; if (!openSet.Contains(neighbour)) { // First time this point has been considered. The current distance is the best guess. openSet.Add(neighbour); neighbour.HeuristicScore = CalculateHeuristic(neighbour, endPoint); tentativeIsBetter = true; } else if (tentativeKnownScore < neighbour.KnownScore) { // Point has been considered before and the last consideration was given a higher score, so use the knew one. tentativeIsBetter = true; } else { // Point has been considered before and the last consideration was given a lower score, so use that one. tentativeIsBetter = false; } if (tentativeIsBetter) { // If necessary, update the square's known score and note the path to that square. cameFrom[neighbour] = square; neighbour.KnownScore = tentativeKnownScore; } } } return(null); }
/// <summary> /// Finds a route using a parallelized A* algorithm /// </summary> /// <param name="startPoint">The point to start the algorithm from.</param> /// <param name="endPoint">The point to find a route to.</param> /// <param name="field">The field in which the route must be found.</param> /// <param name="movingObject">The object that will move around the <paramref name="field"/>.</param> /// <returns> /// A Route if a route can be found. /// /// null otherwise /// </returns> /// <exception cref="InvalidOperationException"> /// Thrown if the specified resolution is 0 in one or more directions. /// </exception> /// /// Determines a route from <paramref name="startPoint" /> to <paramref name="endPoint" /> using a parallelized A* algorithm. /// /// Algorithm taken from @cite aiModernApproach public override Route FindPath(PointF startPoint, PointF endPoint, Field field, IPositionedObject movingObject) { if (Resolution.Height < 1 || Resolution.Width < 1) { throw new InvalidOperationException("Resolution must be greater than or equal to 1 pixel by 1 pixel"); } var gridSize = field.Size.Scale(Resolution).Ceiling(); var grid = InitGrid(startPoint, endPoint, field, gridSize, movingObject); var startPoints = from g in grid where g.Type == SquareType.Origin select g; var closedSet = new TStorageClass(); // The already checked points var openSet = new TStorageClass(); foreach (var f in from g in grid where g.Type == SquareType.Origin select g) // The points to check { openSet.Add(f); } var cameFrom = new Dictionary <GridSquare, GridSquare>(); // A list of route data already calculated // Initialize the origin points Parallel.ForEach(startPoints, g => { g.KnownScore = 0; g.HeuristicScore = CalculateLength(g.Location, endPoint); }); var tasks = new List <Task>(); // While there are points available to check while (openSet.Any() || tasks.Any(t => !t.IsCompleted)) { var square = openSet.OrderBy(p => p.TotalScore).First(); if (square.Type == SquareType.Destination) { var path = ReconstructPath(cameFrom, cameFrom[square]); path.Path.Add(new LineSegment(cameFrom[square].Location, square.Location)); path.Scale(Resolution.Invert()); // Convert the path back to world coordinates from grid coordinates return(path); } openSet.Remove(square); closedSet.Add(square); var index = square.Location.ToIndex(gridSize.Width); // Calculate the neighboring indexes and discard the ones out of range. var neighbourIndexes = new[] { index + 1, index - 1, index + gridSize.Width, index - gridSize.Width, index + gridSize.Width + 1, index - gridSize.Width + 1, index + gridSize.Width - 1, index - gridSize.Width - 1 }.Where(i => (i >= 0) && (i < grid.Length) && !closedSet.Contains(grid[i])).ToList(); foreach (var neighbour in neighbourIndexes.Select(i => grid[i]).Where(neighbour => !openSet.Contains(neighbour))) { openSet.Add(neighbour); cameFrom[neighbour] = null; neighbour.HeuristicScore = CalculateHeuristic(neighbour, endPoint); } Parallel.ForEach(neighbourIndexes, (i, state) => { var neighbour = grid[i]; // Work out the distance to the origin var tentativeKnownScore = square.KnownScore + CalculateLength(square.Location, neighbour.Location); if (tentativeKnownScore < neighbour.KnownScore) { // If necessary, update the square's known score and note the path to that square. cameFrom[neighbour] = square; neighbour.KnownScore = tentativeKnownScore; } }); } return(null); }
/// <summary> /// Converts an IPositionedObject to a PositionedObject for interop purposes. /// </summary> /// <param name="obj">The object to convert</param> /// <returns>A PositionedObject that represents (but is not linked to) the IPositionedObject</returns> public static PositionedObject FromIPositionedObject(IPositionedObject obj) { return(new PositionedObject { Position = obj.Position, Size = obj.Size }); }
/// <summary> /// Finds a route using a Potential Field algorithm including a mass term /// </summary> /// <param name="startPoint">The point to start the algorithm from.</param> /// <param name="endPoint">The point to find a route to.</param> /// <param name="field">The field in which the route must be found.</param> /// <param name="movingObject">The object that will move around the <paramref name="field"/>.</param> /// <returns> /// A Route if a route can be found. /// /// Otherwise, the route that has been found so far. /// </returns> /// Determines a route from <paramref name="startPoint" /> to <paramref name="endPoint" /> using a Potential Field algorithm. /// The algorithm has been modified to include the effects of inertia, in order to damp down the route that is found (reducing /// oscillations) /// /// Algorithm taken from @cite intelligentAlgorithmPathPlanning public virtual Route FindPath(PointF startPoint, PointF endPoint, Field field, IPositionedObject movingObject) { const int attractiveConstant = 1; const int repulsiveConstant = -5000000; const int repulsiveDistance = 150; const double timestep = 0.1; const int mass = 40; const double flowResistance = 0.1; var currentVelocity = new Vector(2, 0); var currentPosition = new Vector(new double[] { startPoint.X, startPoint.Y }); var endVector = new Vector(new double[] { endPoint.X, endPoint.Y }); var route = new Route(); var distance = endVector - currentPosition; while (distance.Norm() > 10) { var attractForce = attractiveConstant * distance; var repulseForce = new Vector(new[] { 0.0, 0.0 }); var position = currentPosition; repulseForce = (from p in field.Players .Where(p => p.Team == Team.Opposition) select new Vector(new double[] { p.Position.X, p.Position.Y }) into playerPos select playerPos - position into opDistance let distMag = opDistance.Norm() where distMag <= repulsiveDistance let directionVector = opDistance.Normalize() select repulsiveConstant *directionVector / (distMag * distMag)).Aggregate(repulseForce, (current, force) => current + force); var totalForce = attractForce + repulseForce; var angle = Math.Abs(Math.Acos(Vector.ScalarProduct(currentVelocity, attractForce) / (currentVelocity.Norm() * attractForce.Norm()))); if (angle < Math.PI / 2) { var rotMat = new Matrix(new[] { new[] { Math.Cos(145), -Math.Sin(145) }, new[] { Math.Sin(145), Math.Cos(145) } }); attractForce = (rotMat * repulseForce.ToColumnMatrix()).GetColumnVector(0); totalForce = attractForce + repulseForce; } currentVelocity += totalForce * timestep / mass; currentVelocity -= flowResistance * currentVelocity; var oldPosition = currentPosition; currentPosition += currentVelocity * timestep; route.Path.Add(new LineSegment(new PointF((float)oldPosition[0], (float)oldPosition[1]), new PointF((float)currentPosition[0], (float)currentPosition[1]))); if (route.Path.Count > 80000) { break; } distance = endVector - currentPosition; } return(route); }