private void AddCandidates(CellPath path, int di, int dj, List <CellPath> candidates, bool allowDiagonals) { int i = path.I; int j = path.J; CellData c = _grid[i, j]; var c01 = _grid[i - 1, j]; var c10 = _grid[i, j - 1]; var c12 = _grid[i, j + 1]; var c21 = _grid[i + 1, j]; if (AreCommunicating(c, c01)) { AddCandidate(c01, 1, di, dj, candidates, path); } if (AreCommunicating(c, c21)) { AddCandidate(c21, 1, di, dj, candidates, path); } if (AreCommunicating(c, c10)) { AddCandidate(c10, 1, di, dj, candidates, path); } if (AreCommunicating(c, c12)) { AddCandidate(c12, 1, di, dj, candidates, path); } if (allowDiagonals) { var c00 = _grid[i - 1, j - 1]; var c02 = _grid[i - 1, j + 1]; var c20 = _grid[i + 1, j - 1]; var c22 = _grid[i + 1, j + 1]; double weightDiagonal = Math.Sqrt(2); if (CanMoveDiagonallyTo(c, c00, c01, c10)) { AddCandidate(c00, weightDiagonal, di, dj, candidates, path); } if (CanMoveDiagonallyTo(c, c20, c21, c10)) { AddCandidate(c20, weightDiagonal, di, dj, candidates, path); } if (CanMoveDiagonallyTo(c, c02, c01, c12)) { AddCandidate(c02, weightDiagonal, di, dj, candidates, path); } if (CanMoveDiagonallyTo(c, c22, c21, c12)) { AddCandidate(c22, weightDiagonal, di, dj, candidates, path); } } }
private void AddCandidate(CellData c, double weight, int di, int dj, List <CellPath> candidates, CellPath path) { double distanceToDestination = Math.Sqrt(Math.Pow(di - c.I, 2) + Math.Pow(dj - c.J, 2)); weight = weight / c.Speed + c.Weight; if (c.CandidateRef == null) { var candidateRef = new CellPath(c.I, c.J, path.W + weight, distanceToDestination, path); candidates.Add(candidateRef); c.CandidateRef = candidateRef; } else { double newWeight = path.W + weight; if (newWeight < c.CandidateRef.W) { c.CandidateRef.W = newWeight; c.CandidateRef.Path = path; } } }
public List <short> GetPath(short source, short target, List <short> occupiedCells, bool allowDiagonals, bool stopNextToTarget) { int c; CellPath candidate; var srcPos = MapPoint.FromCellId(source); var dstPos = MapPoint.FromCellId(target); int si = srcPos.X + 1; int sj = srcPos.Y + 1; var srcCell = _grid[si, sj]; if (srcCell.Zone == -1) { CellData bestFit = null; int bestDist = int.MaxValue; int bestFloorDiff = int.MaxValue; for (int i = -1; i <= 1; i++) { for (int j = -1; j <= 1; j++) { if (i == 0 && j == 0) { continue; } var cell = _grid[si + i, sj + j]; if (cell.Zone == -1) { continue; } int floorDiff = Math.Abs(cell.Floor - srcCell.Floor); int dist = Math.Abs(i) + Math.Abs(j); if (bestFit == null || floorDiff < bestFloorDiff || (floorDiff <= bestFloorDiff && dist < bestDist)) { bestFit = cell; bestDist = dist; bestFloorDiff = floorDiff; } } } if (bestFit != null) { return new List <short>() { source, MapPoint.FromCoords(bestFit.I + 1, bestFit.J + 1).CellId } } ; throw new Exception($"Player is stuck in '{si}/{sj}."); } int di = dstPos.X + 1; int dj = dstPos.Y + 1; MapPoint cellPos; foreach (short cellId in occupiedCells) { cellPos = MapPoint.FromCellId(cellId); _grid[cellPos.X + 1, cellPos.Y + 1].Weight += OCCUPIED_CELL_WEIGHT; } List <CellPath> candidates = new List <CellPath>(); List <CellPath> selections = new List <CellPath>(); double distSrcDst = Math.Sqrt(Math.Pow(si - di, 2) + Math.Pow(sj - dj, 2)); var selection = new CellPath(si, sj, 0, distSrcDst, null); CellPath reachingPath = null; var closestPath = selection; while (selection.I != di || selection.J != dj) { AddCandidates(selection, di, dj, candidates, allowDiagonals); int n = candidates.Count; if (n == 0) { selection = closestPath; break; } double minPotentialWeight = double.MaxValue; int selectionIndex = 0; for (c = 0; c < n; c++) { candidate = candidates[c]; if (candidate.W + candidate.D < minPotentialWeight) { selection = candidate; minPotentialWeight = candidate.W + candidate.D; selectionIndex = c; } } selections.Add(selection); candidates.RemoveAt(selectionIndex); if (selection.D == 0 || (stopNextToTarget && selection.D < 1.5)) { if (reachingPath == null || selection.W < reachingPath.W) { reachingPath = selection; closestPath = selection; List <CellPath> trimmedCandidates = new List <CellPath>(); for (c = 0; c < candidates.Count; c++) { candidate = candidates[c]; if (candidate.W + candidate.D < reachingPath.W) { trimmedCandidates.Add(candidate); } else { _grid[candidate.I, candidate.J].CandidateRef = null; } } candidates = trimmedCandidates; } } else { if (selection.D < closestPath.D) { closestPath = selection; } } } for (c = 0; c < candidates.Count; c++) { candidate = candidates[c]; _grid[candidate.I, candidate.J].CandidateRef = null; } for (int s = 0; s < selections.Count; s++) { selection = selections[s]; _grid[selection.I, selection.J].CandidateRef = null; } foreach (short cellId in occupiedCells) { cellPos = MapPoint.FromCellId(cellId); _grid[cellPos.X + 1, cellPos.Y + 1].Weight -= OCCUPIED_CELL_WEIGHT; } List <short> shortestPath = new List <short>(); while (closestPath != null) { shortestPath.Insert(0, MapPoint.FromCoords(closestPath.I - 1, closestPath.J - 1).CellId); closestPath = closestPath.Path; } return(shortestPath); }