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