/// <summary>
/// Calculates algorythm's cell-weights per cell
/// </summary>
/// <param name="cellCoords"></param>
/// <param name="unitCoords"></param>
/// <param name="targetCoords"></param>
/// <returns></returns>
private CellWeightValues CalculateCellWeights(Coords cellCoords, Coords unitCoords, Coords targetCoords)
{
CoordsCalculator coordsCalculator = new CoordsCalculator((FloatCoords)cellCoords);
return(new CellWeightValues
{
AbsoluteDistanceToUnit = coordsCalculator.DistanceToFloatCoords((FloatCoords)unitCoords),
AbsoluteDistanceToTarget = coordsCalculator.DistanceToFloatCoords((FloatCoords)targetCoords)
});
}
public void Test_ShouldReturnCorrectDistance_WhenGivenTwoPoints(int x1, int y1, int x2, int y2, double expectedDistance)
{
// Arrange
FloatCoords floatCoords = new FloatCoords()
{
x = x1,
y = y1
};
FloatCoords target = new FloatCoords()
{
x = x2,
y = y2
};
CoordsCalculator coordsCalculator = new CoordsCalculator(floatCoords);
// Act
double actual = coordsCalculator.DistanceToFloatCoords(target);
// Assert
Assert.AreEqual(expectedDistance, actual);
}
/// <summary>
/// returns a path to the target that avoids obstacles
/// </summary>
/// <param name="targetCoords">Target</param>
/// <param name="unit">Model containing info about the unit's location</param>
/// <returns>Path</returns>
/// <exception cref="NoPathFoundException">Throws exception when no path found</exception>
public List <FloatCoords> FindPath(FloatCoords targetCoords, LocationModel unit)
{
if (IsCellImpassable(worldController.GetCellFromCoords((Coords)targetCoords).worldCellModel, unit))
{
throw new NoPathFoundException(unit.Coords, (Coords)targetCoords);
}
CoordsCalculator unitCoordsCalculator = new CoordsCalculator(unit.FloatCoords);
CoordsCalculator targetCoordsCalculator = new CoordsCalculator(targetCoords);
LinkedList <FloatCoords> currentPath = new LinkedList <FloatCoords>();
List <Coords> visitedCoords = new List <Coords>();
FloatCoords currentCoords = unit.FloatCoords;
bool IsPreviousNode(Coords target) => currentPath.Last?.Previous != null && (Coords)currentPath.Last.Previous.Value == target;
while ((Coords)currentCoords != (Coords)targetCoords)
{
Dictionary <Coords, CellWeightValues> currentNeighbours = CalculateNeighboursWeights((Coords)currentCoords, (Coords)targetCoords, unit);
// Find most probable cell,
// WHERE:
// 1. NOT Cell has been visited and isn't the previous node
List <FloatCoords> mostProbableCandidates = (
from KeyValuePair <Coords, CellWeightValues> pair in currentNeighbours
where !(visitedCoords.Contains(pair.Key) &&
!IsPreviousNode(pair.Key)) &&
!IsDiagonalPathBlocked((Coords)currentCoords, pair.Key, unit)
let cellWeightValues = pair.Value
orderby cellWeightValues.Weight, cellWeightValues.AbsoluteDistanceToTarget
select(FloatCoords) pair.Key).ToList();
if (!mostProbableCandidates.Any())
{
throw new NoPathFoundException(unit.Coords, (Coords)targetCoords);
}
FloatCoords mostProbableCoords = mostProbableCandidates.First();
if (ENABLE_ANIMATION)
{
worldController.GetCellFromCoords((Coords)currentCoords).worldCellView.Colour = Color.Red;
worldController.GetCellFromCoords((Coords)mostProbableCoords).worldCellView.Colour = Color.Blue;
Thread.Sleep(ANIMATION_DELAY_MILLIS);
}
if (!visitedCoords.Contains((Coords)mostProbableCoords))
{
visitedCoords.Add((Coords)mostProbableCoords);
}
// Add to list if mostProbableCoords:
// 1. The path is empty
// 2. OR the node isn't in the path AND it isn't too far away
// Else, if mostProbableCoords is already known, move to that node
if (!currentPath.Any() ||
!currentPath.Contains(mostProbableCoords) &&
!(unitCoordsCalculator.DistanceToFloatCoords(mostProbableCoords) > SearchLimit ||
targetCoordsCalculator.DistanceToFloatCoords(mostProbableCoords) > SearchLimit))
{
currentPath.AddLast(mostProbableCoords);
}
else if (currentPath.Contains(mostProbableCoords))
{
while ((Coords)currentPath.Last.Value != (Coords)mostProbableCoords)
{
currentPath.RemoveLast();
}
}
currentCoords = currentPath.Last.Value;
}
if (!currentPath.Any())
{
throw new NoPathFoundException(unit.Coords, (Coords)targetCoords);
}
Queue <FloatCoords> route = ReduceWaypoints(currentPath.ToList(), unit);
return(route.ToList());
}
/// <summary>
/// Checks if cell is impassible by unit
/// </summary>
/// <param name="cell">Cell in question</param>
/// <param name="unit">Unit that tries to pass</param>
/// <returns>Impassibility</returns>
private bool IsCellImpassable(WorldCellModel cell, LocationModel unit)
{
CoordsCalculator coordsCalculator = new CoordsCalculator((FloatCoords)cell.RealCoords);
return(coordsCalculator.DistanceToFloatCoords((FloatCoords)unit.Coords) > SearchLimit || CellIsObstacle(cell, unit));
}