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