public void Test_ShouldReturnCorrectNeighbour_WhenGivenTwoPoints(int x1, int y1, int x2, int y2, int x3, int y3)
{
// Arrange
FloatCoords floatCoords = new FloatCoords()
{
x = x1,
y = y1
};
FloatCoords target = new FloatCoords()
{
x = x2,
y = y2
};
FloatCoords expected = new FloatCoords()
{
x = x3,
y = y3
};
CoordsCalculator coordsCalculator = new CoordsCalculator(floatCoords);
// Act
List <FloatCoords> actual = coordsCalculator.GetCommonNeighboursWith((Coords)target);
// Assert
Assert.IsTrue(actual.Any(neighbour => neighbour == expected));
}
// Checks if Diagonal move is possible/not blocked
public bool IsDiagonalPathBlocked(Coords originCoords, Coords destinationCoords, LocationModel unitLocationModel)
{
CoordsCalculator coordsCalculator = new CoordsCalculator((FloatCoords)originCoords);
if (!coordsCalculator.GetNeighbours().Contains((FloatCoords)destinationCoords))
{
throw new ArgumentException("destinationCoords must be neighbouring originCoords");
}
//checks if coords are next to each other
if (coordsCalculator.GetNonDiagonalNeighbours().Contains((FloatCoords)destinationCoords))
{
return(false);
}
Coords commonNeighbour1 = new Coords
{
x = originCoords.x,
y = destinationCoords.y
};
Coords commonNeighbour2 = new Coords
{
x = destinationCoords.x,
y = originCoords.y
};
WorldCellController cell1 = worldController.GetCellFromCoords(commonNeighbour1);
WorldCellController cell2 = worldController.GetCellFromCoords(commonNeighbour2);
//Checks if both coords are blocked by an obstacle
return(cell1 != null && IsCellImpassable(cell1.worldCellModel, unitLocationModel) ||
cell2 != null && IsCellImpassable(cell2.worldCellModel, unitLocationModel));
}
/// <summary>
/// Reduces amount of waypoints in a list, makes path smoother
/// </summary>
/// <param name="routeTemp"></param>
/// <param name="unitLocationModel"></param>
/// <returns></returns>
private Queue <FloatCoords> ReduceWaypoints(IEnumerable <FloatCoords> routeTemp, LocationModel unitLocationModel)
{
Queue <FloatCoords> route = new Queue <FloatCoords>(routeTemp);
FloatCoords target = route.Last();
Queue <FloatCoords> wayPoints = new Queue <FloatCoords>();
wayPoints.Enqueue(unitLocationModel.FloatCoords);
// While not arrived at destination
while (wayPoints.Last() != target)
{
FloatCoords currentCoords = wayPoints.Last();
CoordsCalculator coordsCalculator = new CoordsCalculator(currentCoords);
List <Coords> rayCells;
FloatCoords nextCoords;
do
{
nextCoords = route.Dequeue();
rayCells = coordsCalculator.GetCoordsOnRayWith(nextCoords);
} while (route.Count > 0 && AreAllCellsLegal(rayCells, unitLocationModel));
wayPoints.Enqueue(nextCoords);
}
return(wayPoints);
}
/// <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)
});
}
// sets the weight-values of all the neighbours of a cell
private Dictionary <Coords, CellWeightValues> CalculateNeighboursWeights(Coords currentCell, Coords targetCoords, LocationModel unit)
{
CoordsCalculator coordsCalculator = new CoordsCalculator((FloatCoords)currentCell);
FloatCoords[] neighbours = new FloatCoords[8];
coordsCalculator.GetNeighbours().CopyTo(neighbours, 0);
return((from neighbour in neighbours
where worldController.GetCellFromCoords((Coords)neighbour) != null &&
!IsCellImpassable(worldController.GetCellFromCoords((Coords)neighbour).worldCellModel, unit)
let cellWeights = CalculateCellWeights((Coords)neighbour, unit.Coords, targetCoords)
select new KeyValuePair <Coords, CellWeightValues>((Coords)neighbour, cellWeights)).ToDictionary(pair => pair.Key, pair => pair.Value));
}
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));
}
