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