public TrailblazerPather_TwinAStar(PathfindData pathfindData) : base(pathfindData)
{
rraOpenSet = new Priority_Queue.FastPriorityQueue <CellRefNode>(map.Area);
rraClosedSet = new Dictionary <CellRef, int>();
cellRefNodeCache = new Dictionary <CellRef, CellRefNode>();
var cellPassRules = ThatApply <CellPassabilityRule>(
new CellPassabilityRule_PathGrid(pathfindData),
new CellPassabilityRule_DoorByPawn(pathfindData),
new CellPassabilityRule_NoPassDoors(pathfindData)
);
var passRules = Enumerable.Empty <PassabilityRule>();
var cellCostRules = ThatApply <CellCostRule>(
new CellCostRule_PathGrid(pathfindData),
new CellCostRule_Walls(pathfindData)
);
var costRules = ThatApply <CostRule>(new CostRule_MoveTicks(pathfindData));
rraPathfinderGrid = new PathfinderGrid(cellCostRules, costRules, cellPassRules, passRules);
// Initialize the RRA* algorithm
foreach (IntVec3 cell in pathfindData.DestRect)
{
CellRef cellRef = pathfindData.map.GetCellRef(cell);
rraClosedSet[cellRef] = 0;
rraOpenSet.Enqueue(GetNode(cellRef), 0);
}
}
public void CreateVariables()
{
openSet = new Priority_Queue.FastPriorityQueue <PathfindingNode <Tile> >(10000);
openSet.Enqueue(nodes[tileStart], 0);
closedSet = new Priority_Queue.FastPriorityQueue <PathfindingNode <Tile> >(10000);
cameFrom = new Dictionary <PathfindingNode <Tile>, PathfindingNode <Tile> >();
gScore = new Dictionary <PathfindingNode <Tile>, float>();
fScore = new Dictionary <PathfindingNode <Tile>, float>();
}
public void GenerateVilliagePaths()
{
var camefrom = new Dictionary <Centers, Centers>();
var costsofar = new Dictionary <Centers, int>();
frontier = new Priority_Queue.FastPriorityQueue <FastTownNode>(_basemap.centerlist.Count());
foreach (Centers cnt in landlist)
{
var mapdata = cnt.mapData;
var radiuslist = landlist.FindAll(x => heuristic(cnt.center, x.center) < .1);
foreach (Centers othernode in radiuslist)
// foreach (Centers othernode in cnt.neigbors)
{
{
if (!mapdata.Villiage.TownAlreadyMeasured(othernode) && !othernode.mapData.Water)
{
MeasureTownDistance(camefrom, costsofar, landlist, cnt, mapdata, othernode);
}
}
}
}
}
public 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.PathErrorGoUpstairs);
}
else if (targetZ < startZ)
{
return(PathState.PathErrorGoDownstairs);
}
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 overallMinCost = int.MaxValue;
foreach (var sector in tmpSectors)
{
overallMinCost = System.Math.Min(overallMinCost, 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
PathQueueNode pathNode = m_PathMatrix[matrixCenter * matrixSize + matrixCenter];
pathNode.Reset();
pathNode.Predecessor = null;
pathNode.Cost = pathNode.PathCost = int.MaxValue;
pathNode.PathHeuristic = 0;
m_PathDirty.Add(pathNode); // push the initial position to the closed list
// obtain the final position at our grid
PathQueueNode lastPathNode = m_PathMatrix[(matrixCenter + dY) * Constants.PathMatrixSize + (matrixCenter + dX)];
lastPathNode.Predecessor = null;
lastPathNode.Reset();
int tmpIndex;
if (targetX < sectorMaxX)
{
if (targetY < sextorMaxY)
{
tmpIndex = (int)Directions.North;
}
else
{
tmpIndex = (int)Directions.East;
}
}
else if (targetY < sextorMaxY)
{
tmpIndex = (int)Directions.West;
}
else
{
tmpIndex = (int)Directions.South;
}
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) * overallMinCost;
// now add that to our closed list
m_PathDirty.Add(lastPathNode);
// clear our heap and push the current node to it.
m_PathPriorityQueue = new Priority_Queue.FastPriorityQueue <PathQueueNode>(50000);
m_PathPriorityQueue.Enqueue(lastPathNode, lastPathNode.PathHeuristic);
PathQueueNode currentPathNode = null;
PathQueueNode tmpPathNode = null;
uint s = 0, s2 = 0;
// looping through the very first SQM in the heap
while (m_PathPriorityQueue.Count > 0)
{
s++;
currentPathNode = m_PathPriorityQueue.Dequeue();
// 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 (currentPathNode.Priority >= pathNode.PathCost)
{
break;
}
for (int i = -1; i <= 1; i++)
{
for (int j = -1; j <= 1; j++)
{
if (i == 0 && j == 0)
{
continue;
}
int gridX = currentPathNode.x + i;
int gridY = currentPathNode.y + j;
// check if that grid is in the range or validity
if (gridX < -matrixCenter || gridX > matrixCenter || gridY < -matrixCenter || gridY > matrixCenter)
{
continue;
}
int currentPathCost;
if (i * j == 0) // straight movement (not diagonal)
{
currentPathCost = currentPathNode.PathCost + currentPathNode.Cost;
}
else // diagonal movements worth as 3 as a normal movement;
{
currentPathCost = currentPathNode.PathCost + 3 * currentPathNode.Cost;
}
tmpPathNode = m_PathMatrix[(matrixCenter + gridY) * matrixSize + (matrixCenter + gridX)];
if (tmpPathNode.PathCost > currentPathCost)
{
tmpPathNode.Predecessor = currentPathNode;
tmpPathNode.PathCost = currentPathCost;
if (tmpPathNode.Cost == int.MaxValue)
{
int currentPosX = startX + tmpPathNode.x;
int currentPosY = startY + tmpPathNode.y;
if (currentPosX < sectorMaxX)
{
if (currentPosY < sextorMaxY)
{
tmpIndex = (int)Directions.North;
}
else
{
tmpIndex = (int)Directions.East;
}
}
else if (currentPosY < sextorMaxY)
{
tmpIndex = (int)Directions.West;
}
else
{
tmpIndex = (int)Directions.South;
}
tmpPathNode.Cost = tmpSectors[tmpIndex].GetCost(currentPosX, currentPosY, startZ);
tmpPathNode.PathHeuristic = tmpPathNode.Cost + (tmpPathNode.Distance - 1) * overallMinCost;
m_PathDirty.Add(tmpPathNode);
}
if (tmpPathNode == pathNode || tmpPathNode.Cost >= Constants.PathCostObstacle)
{
continue;
}
s2++;
if (tmpPathNode.Queue == m_PathPriorityQueue)
{
m_PathPriorityQueue.UpdatePriority(tmpPathNode, currentPathCost + tmpPathNode.PathHeuristic);
}
else
{
tmpPathNode.Reset();
m_PathPriorityQueue.Enqueue(tmpPathNode, currentPathCost + tmpPathNode.PathHeuristic);
}
}
}
}
}
//Debug.Log("Recuresive Count: " + s + ", " + s2);
var ret = PathState.PathErrorInternal;
if (pathNode.PathCost < int.MaxValue)
{
currentPathNode = pathNode;
tmpPathNode = null;
while (currentPathNode != null)
{
if (!forceExact && currentPathNode.x == lastPathNode.x && currentPathNode.y == lastPathNode.y && lastPathNode.Cost >= Constants.PathCostObstacle)
{
currentPathNode = null;
break;
}
if (currentPathNode.Cost == Constants.PathCostUndefined)
{
break;
}
if (tmpPathNode != null)
{
dX = currentPathNode.x - tmpPathNode.x;
dY = currentPathNode.y - tmpPathNode.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] | currentPathNode.Cost << 16;
if (steps.Count + 1 >= Constants.PathMaxSteps)
{
break;
}
}
tmpPathNode = currentPathNode;
currentPathNode = currentPathNode.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);
}
internal static void EnqueueNode(this Priority_Queue.FastPriorityQueue <SolutionNode> queue, SolutionNode node)
{
queue.Enqueue(node, node.TotalDistance);
}