private PathState CalculateAStartPath(int startX, int startY, int startZ, int targetX, int targetY, int targetZ, int searchLimit, List <int> steps) { MiniMapSector[] sectors = new MiniMapSector[4]; sectors[0] = AcquireSector(startX - Constants.PathMatrixCenter, startY - Constants.PathMatrixCenter, startZ, false); sectors[1] = AcquireSector(startX - Constants.PathMatrixCenter, startY + Constants.PathMatrixCenter, startZ, false); sectors[2] = AcquireSector(startX + Constants.PathMatrixCenter, startY + Constants.PathMatrixCenter, startZ, false); sectors[3] = AcquireSector(startX + Constants.PathMatrixCenter, startY - Constants.PathMatrixCenter, startZ, false); // initial sector position (start position in minimap storage) var sectorMaxX = sectors[0].SectorX + Constants.MiniMapSectorSize; var sextorMaxY = sectors[0].SectorY + Constants.MiniMapSectorSize; var closedList = new Dictionary <int, PathItem>(); var openList = new Utils.PriorityQueue <KeyValuePair <PathItem, int> >(Comparer <KeyValuePair <PathItem, int> > .Create(ComparePathNodes)); PathItem currentNode = new PathItem(startX, startY); closedList.Add(Hash2DPosition(startX, startY), currentNode); PathItem foundNode = null; PathState ret = PathState.PathErrorInternal; while (currentNode != null) { if (closedList.Count > searchLimit) { ret = PathState.PathErrorTooFar; break; } if (currentNode.X == targetX && currentNode.Y == targetY && (foundNode == null || currentNode.PathCost < foundNode.PathCost)) { foundNode = currentNode; } if (foundNode != null && (currentNode.PathHeuristic >= foundNode.PathCost)) { break; } for (int i = -1; i <= 1; i++) { for (int j = -1; j <= 1; j++) { if (i == 0 && j == 0) { continue; } int currentPosX = currentNode.X + i; int currentPosY = currentNode.Y + j; int sectorIndex; if (currentPosX < sectorMaxX) { sectorIndex = currentPosY < sextorMaxY ? 0 : 1; } else { sectorIndex = currentPosY < sextorMaxY ? 3 : 2; } int cost = sectors[sectorIndex].GetCost(currentPosX, currentPosY, startZ); if (cost >= Constants.PathCostObstacle) { continue; } int modifier = 1; if ((i * j) != 0) { modifier = 3; } int pathCost = currentNode.PathCost + modifier * cost; var direction = DirectionFromPosToPos(currentNode.X, currentNode.Y, currentPosX, currentPosY); PathItem neighborNode; if (closedList.TryGetValue(Hash2DPosition(currentPosX, currentPosY), out PathItem handledNode)) { neighborNode = handledNode; if (neighborNode.PathCost <= pathCost) { continue; } } else { neighborNode = new PathItem(currentPosX, currentPosY); closedList.Add(Hash2DPosition(currentPosX, currentPosY), neighborNode); } neighborNode.Predecessor = currentNode; neighborNode.Cost = cost; neighborNode.PathCost = pathCost; neighborNode.PathHeuristic = neighborNode.PathCost + Distance(currentPosX, currentPosY, targetX, targetY); neighborNode.Direction = direction; openList.Push(new KeyValuePair <PathItem, int>(neighborNode, neighborNode.PathHeuristic)); } } if (openList.Count > 0) { currentNode = openList.Pop().Key; } else { currentNode = null; } } if (foundNode != null) { currentNode = foundNode; while (currentNode != null) { steps.Add((int)currentNode.Direction | currentNode.Cost << 16); if (steps.Count + 1 >= Constants.PathMaxSteps) { break; } currentNode = currentNode.Predecessor; } steps.RemoveAt(steps.Count - 1); steps.Reverse(); ret = PathState.PathExists; } return(ret); }
internal PathState CalculatePath(int startX, int startY, int startZ, int targetX, int targetY, int targetZ, bool forceDiagonal, bool forceExact, List <int> steps) { int dX = targetX - startX; int dY = targetY - startY; if (steps == null) { return(PathState.PathErrorInternal); } else if (targetZ > startZ) { return(PathState.PathErrorGoDownstairs); } else if (targetZ < startZ) { return(PathState.PathErrorGoUpstairs); } else if (dX == 0 && dY == 0) { return(PathState.PathEmpty); } else if (System.Math.Abs(dX) >= Constants.PathMaxDistance || System.Math.Abs(dY) > Constants.PathMaxDistance) { return(PathState.PathErrorTooFar); } // check if this tile is known to be obstacle // simple case, adjacent square if (System.Math.Abs(dX) + System.Math.Abs(dY) == 1) { int cost = GetFieldCost(targetX, targetY, targetZ); if (forceExact || cost < Constants.PathCostObstacle) { if (dX == 1 && dY == 0) { steps.Add((int)PathDirection.East); } else if (dX == 0 && dY == -1) { steps.Add((int)PathDirection.North); } else if (dX == -1 && dY == 0) { steps.Add((int)PathDirection.West); } else if (dX == 0 && dY == 1) { steps.Add((int)PathDirection.South); } steps[0] = steps[0] | cost << 16; return(PathState.PathExists); } return(PathState.PathEmpty); } // simple case, adjacent diagonal square (while diagonal is actually allowed) if (forceDiagonal && System.Math.Abs(dX) == 1 && System.Math.Abs(dY) == 1) { int cost = GetFieldCost(targetX, targetY, targetZ); if (forceExact || cost < Constants.PathCostObstacle) { if (dX == 1 && dY == -1) { steps.Add((int)PathDirection.NorthEast); } else if (dX == -1 && dY == -1) { steps.Add((int)PathDirection.NorthWest); } else if (dX == -1 && dY == 1) { steps.Add((int)PathDirection.SouthWest); } else if (dX == 1 && dY == 1) { steps.Add((int)PathDirection.SouthEast); } steps[0] = steps[0] | cost << 16; return(PathState.PathExists); } return(PathState.PathEmpty); } // A* Algorithm // acquiring 4 directional sectors MiniMapSector[] tmpSectors = new MiniMapSector[4]; tmpSectors[0] = AcquireSector(startX - Constants.PathMatrixCenter, startY - Constants.PathMatrixCenter, startZ, false); tmpSectors[1] = AcquireSector(startX - Constants.PathMatrixCenter, startY + Constants.PathMatrixCenter, startZ, false); tmpSectors[2] = AcquireSector(startX + Constants.PathMatrixCenter, startY + Constants.PathMatrixCenter, startZ, false); tmpSectors[3] = AcquireSector(startX + Constants.PathMatrixCenter, startY - Constants.PathMatrixCenter, startZ, false); // obtain local variables of constants int matrixCenter = Constants.PathMatrixCenter; int matrixSize = Constants.PathMatrixSize; // heuristic multiplier var minCost = int.MaxValue; foreach (var sector in tmpSectors) { minCost = System.Math.Min(minCost, sector.MinCost); } // initial sector position (start position in minimap storage) var sectorMaxX = tmpSectors[0].SectorX + Constants.MiniMapSectorSize; var sextorMaxY = tmpSectors[0].SectorY + Constants.MiniMapSectorSize; // obtain the center of the grid, and resetting it // the center of the grid is matchin our initial position, so we will use MatrixCenter with offset as a workaround PathItem pathItem = m_PathMatrix[matrixCenter * matrixSize + matrixCenter]; pathItem.Reset(); pathItem.Predecessor = null; pathItem.Cost = int.MaxValue; pathItem.PathCost = int.MaxValue; pathItem.PathHeuristic = 0; m_PathDirty.Add(pathItem); // push the initial position to the closed list // obtain the final position at our grid PathItem lastPathNode = m_PathMatrix[(matrixCenter + dY) * Constants.PathMatrixSize + (matrixCenter + dX)]; lastPathNode.Predecessor = null; lastPathNode.Reset(); int tmpIndex; if (targetX < sectorMaxX) { tmpIndex = targetY < sextorMaxY ? 0 : 1; } else { tmpIndex = targetY < sextorMaxY ? 3 : 2; } lastPathNode.Cost = tmpSectors[tmpIndex].GetCost(targetX, targetY, targetZ); lastPathNode.PathCost = 0; // from the constructor, the distance is the manhattan distance from start_pos to target_pos lastPathNode.PathHeuristic = lastPathNode.Cost + (lastPathNode.Distance - 1) * minCost; // now add that to our closed list m_PathDirty.Add(lastPathNode); // clear our heap and push the current node to it. m_PathHeap.Clear(false); m_PathHeap.AddItem(lastPathNode, lastPathNode.PathHeuristic); PathItem currentPathItem = null; PathItem tmpPathItem = null; // looping through the very first SQM in the heap while ((currentPathItem = m_PathHeap.ExtractMinItem() as PathItem) != null) { // check if the current move won't exceed our current shortest path, otherwise end it up // if it exceeds, then we will loop again through our heap, if exists // if not, then we are done searching if the current path is undefined that means we can't // reach that field if (currentPathItem.HeapKey < pathItem.PathCost) { for (int i = -1; i <= 1; i++) { for (int j = -1; j <= 1; j++) { if (i != 0 || j != 0) { int gridX = currentPathItem.X + i; int gridY = currentPathItem.Y + j; // check if that grid is in the range or validity if (!(gridX < -matrixCenter || gridX > matrixCenter || gridY < -matrixCenter || gridY > matrixCenter)) { int currentPathCost; if (i * j == 0) // straight movement (not diagonal) { currentPathCost = currentPathItem.PathCost + currentPathItem.Cost; } else // diagonal movements worth as 3 as a normal movement; { currentPathCost = currentPathItem.PathCost + 3 * currentPathItem.Cost; } tmpPathItem = m_PathMatrix[(matrixCenter + gridY) * matrixSize + (matrixCenter + gridX)]; if (tmpPathItem.PathCost > currentPathCost) { tmpPathItem.Predecessor = currentPathItem; tmpPathItem.PathCost = currentPathCost; if (tmpPathItem.Cost == int.MaxValue) { int currentPosX = startX + tmpPathItem.X; int currentPosY = startY + tmpPathItem.Y; if (currentPosX < sectorMaxX) { tmpIndex = currentPosY < sextorMaxY ? 0 : 1; } else { tmpIndex = currentPosY < sextorMaxY ? 3 : 2; } tmpPathItem.Cost = tmpSectors[tmpIndex].GetCost(currentPosX, currentPosY, startZ); tmpPathItem.PathHeuristic = tmpPathItem.Cost + (tmpPathItem.Distance - 1) * minCost; m_PathDirty.Add(tmpPathItem); } if (!(tmpPathItem == pathItem || tmpPathItem.Cost >= Constants.PathCostObstacle)) { if (tmpPathItem.HeapParent != null) { m_PathHeap.UpdateKey(tmpPathItem, currentPathCost + tmpPathItem.PathHeuristic); } else { tmpPathItem.Reset(); m_PathHeap.AddItem(tmpPathItem, currentPathCost + tmpPathItem.PathHeuristic); } } } } } } } } } var ret = PathState.PathErrorInternal; if (pathItem.PathCost < int.MaxValue) { currentPathItem = pathItem; tmpPathItem = null; while (currentPathItem != null) { if (!forceExact && currentPathItem.X == lastPathNode.X && currentPathItem.Y == lastPathNode.Y && lastPathNode.Cost >= Constants.PathCostObstacle) { currentPathItem = null; break; } if (currentPathItem.Cost == Constants.PathCostUndefined) { break; } if (tmpPathItem != null) { dX = currentPathItem.X - tmpPathItem.X; dY = currentPathItem.Y - tmpPathItem.Y; if (dX == 1 && dY == 0) { steps.Add((int)PathDirection.East); } else if (dX == 1 && dY == -1) { steps.Add((int)PathDirection.NorthEast); } else if (dX == 0 && dY == -1) { steps.Add((int)PathDirection.North); } else if (dX == -1 && dY == -1) { steps.Add((int)PathDirection.NorthWest); } else if (dX == -1 && dY == 0) { steps.Add((int)PathDirection.West); } else if (dX == -1 && dY == 1) { steps.Add((int)PathDirection.SouthWest); } else if (dX == 0 && dY == 1) { steps.Add((int)PathDirection.South); } else if (dX == 1 && dY == 1) { steps.Add((int)PathDirection.SouthEast); } steps[steps.Count - 1] = steps[steps.Count - 1] | currentPathItem.Cost << 16; if (steps.Count + 1 >= Constants.PathMaxSteps) { break; } } tmpPathItem = currentPathItem; currentPathItem = currentPathItem.Predecessor; } if (steps.Count == 0) { ret = PathState.PathEmpty; } else { ret = PathState.PathExists; } } else { ret = PathState.PathErrorUnreachable; } foreach (var tmp in m_PathDirty) { tmp.Cost = int.MaxValue; tmp.PathCost = int.MaxValue; } m_PathDirty.Clear(); return(ret); }