private void addCandidate(CellPathData c, int w, int di, int dj, List <CellPath> candidates, CellPath path) { var i = c.i; var j = c.j; // The total weight of the candidate is the weight of previous path // plus its weight (calculated based on occupancy and speed factor) var distanceToDestination = Math.Sqrt((di - i) * (di - i) + (dj - j) * (dj - j)); w = w / c.speed + c.weight; if (c.candidateRef == null) { var candidateRef = new CellPath(i, j, path.w + w, (int)distanceToDestination, path); //candidates.push(candidateRef); candidates.Add(candidateRef); c.candidateRef = candidateRef; } else { var currentWeight = c.candidateRef.w; var newWeight = path.w + w; if (newWeight < currentWeight) { c.candidateRef.w = newWeight; c.candidateRef.path = path; } } }
public PathfinderManager() { //init grid for (var i = 0; i < WIDTH; i += 1) { var cellPathDatas = new CellPathData[HEIGHT]; for (var j = 0; j < HEIGHT; j += 1) { cellPathDatas[j] = new CellPathData(i, j); } Grid[i] = cellPathDatas; } //build map points constructMapPoints(); }
private bool areCommunicating(CellPathData c1, CellPathData c2) { // Cells are compatible only if they either have the same floor height... if (c1.floor == c2.floor) { // Same height return(true); } // ... or the same zone, different from 0 // ... or a zone of 0 and a floor difference smaller than ELEVATION_TOLERANCE if (c1.zone == c2.zone) { return(oldMovementSystem || (c1.zone != 0) || (Math.Abs(c1.floor - c2.floor) <= ELEVATION_TOLERANCE)); } return(false); }
private CellPathData updateCellPath(Cell cell, CellPathData cellPath) { if ((cell.L & 1) != 0) { cellPath.floor = cell.F; cellPath.zone = cell.Z; cellPath.speed = 1 + cell.S / 10; if (cellPath.zone != firstCellZone) { oldMovementSystem = false; } } else { cellPath.floor = -1; cellPath.zone = -1; } return(cellPath); }
private bool canMoveDiagonallyTo(CellPathData c1, CellPathData c2, CellPathData c3, CellPathData c4) { // Can move between c1 and c2 diagonally only if c1 and c2 are compatible and if c1 is compatible either with c3 or c4 return(areCommunicating(c1, c2) && (areCommunicating(c1, c3) || areCommunicating(c1, c4))); }
public Task <int[]> GetPath(Map map, int startCellId, int destCellId, List <int> occupiedCells, bool allowDiagonals = false, bool stopNextToTarget = false) { CellPath candidate = null; fillPathGrid(map, map.CellChangeMaps.Count == 0); var srcPos = getMapPoint(startCellId); // source index var dstPos = getMapPoint(destCellId); // source index var si = srcPos.X + 1; var sj = srcPos.Y + 1; var srcCell = Grid[si][sj]; if (srcCell.zone == -1) { // Searching for accessible cell around source CellPathData bestFit = null; var bestDist = Math.Pow(10, 1000); var bestFloorDiff = Math.Pow(10, 1000); for (var i = -1; i <= 1; i += 1) { for (var j = -1; j <= 1; j += 1) { if (i == 0 && j == 0) { continue; } var cell = Grid[si + i][sj + j]; if (cell.zone == -1) { continue; } var floorDiff = Math.Abs(cell.floor - srcCell.floor); var 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(Task.FromResult(new int[2] { startCellId, getCellId(bestFit.i - 1, bestFit.j - 1) })); } Console.WriteLine("[pathFinder.getPath] Player is stuck in {0}/{1}", si, sj); return(Task.FromResult(new int[1] { startCellId })); } var di = dstPos.X + 1; // destination i var dj = dstPos.Y + 1; // destination j // marking cells as occupied Point cellPos; //var cellId = 0; foreach (var cellId in occupiedCells) { cellPos = getMapPoint(cellId); Grid[cellPos.X + 1][cellPos.Y + 1].weight += OCCUPIED_CELL_WEIGHT; } // First cell in the path var distSrcDst = Math.Sqrt((si - di) * (si - di) + (sj - dj) * (sj - dj)); var selection = new CellPath(si, sj, 0, (int)distSrcDst, null); List <CellPath> candidates = new List <CellPath>(); List <CellPath> selections = new List <CellPath>(); // Adding cells to path until destination has been reached CellPath reachingPath = null; var closestPath = selection; while (selection.i != di || selection.j != dj) { addCandidates(selection, di, dj, candidates, allowDiagonals); // Looking for candidate with the smallest additional length to path // in O(number of candidates) var n = candidates.Count; if (n == 0) { // No possible path // returning the closest path to destination selection = closestPath; break; } var minPotentialWeight = Math.Pow(10, 1000); var selectionIndex = 0; for (int c = 0; c < n; c += 1) { 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 stopNextToTarget // then when reaching a distance of less than Math.sqrt(2) the destination is considered as reached // (the threshold has to be bigger than sqrt(2) but smaller than 2, to be safe we use the value 1.5) if (selection.d == 0 || (stopNextToTarget && selection.d < 1.5)) { // Selected path reached destination if (reachingPath == null || selection.w < reachingPath.w) { reachingPath = selection; closestPath = selection; // Clearing candidates dominated by current solution to speed up the algorithm List <CellPath> trimmedCandidates = new List <CellPath>(); for (int c = 0; c < candidates.Count; c += 1) { 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) { // 'selection' is the new closest path to destination closestPath = selection; } } } // Removing candidate reference in each cell in selections and active candidates for (int c = 0; c < candidates.Count; c += 1) { candidate = candidates[c]; Grid[candidate.i][candidate.j].candidateRef = null; } for (var s = 0; s < selections.Count; s += 1) { selection = selections[s]; Grid[selection.i][selection.j].candidateRef = null; } // Marking cells as unoccupied foreach (var cell in occupiedCells) { cellPos = getMapPoint(cell); Grid[cellPos.X + 1][cellPos.Y + 1].weight -= OCCUPIED_CELL_WEIGHT; } List <int> shortestPath = new List <int>(); while (closestPath != null) { shortestPath.Add(getCellId(closestPath.i - 1, closestPath.j - 1)); closestPath = closestPath.path; } return(Task.FromResult(shortestPath.ToArray())); }