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