private void addInitialNode(int x, int y, MinHeap <GraphNode, float> queue, bool openB)
{
var flags = openB ? 2u : 1u;
var vertexID = graph.GetVertexID(x, y);
if (vertexID.ID != -1)
{
var node = new GraphNode()
{
VertexID = vertexID.ID,
ChunkID = vertexID.ChunkID,
_flags = flags
};
queue.Add(node, 0);
}
// Its not a graph node hence we add the direct h reachable
else
{
var node = new GraphNode()
{
VertexID = -1,
ChunkID = -1,
_flags = flags
};
var reachable = graph.GetDirectHReachable(x, y);
for (var i = 0; i < reachable.Count; i++)
{
var neighbor = new GraphNode()
{
VertexID = reachable[i].ID,
ChunkID = reachable[i].ChunkID,
_flags = flags
};
var cost = SubGoalGrid.Diagonal(x, y, reachable[i].GridPosition / graph.sizeY, reachable[i].GridPosition % graph.sizeY);
if (openB)
{
neighbor.ParentB = node;
neighbor.CostB = cost;
}
else
{
neighbor.ParentA = node;
neighbor.CostA = cost;
}
queue.Add(neighbor, cost);
}
}
}
// ConstructEdges constructs edges for subgoalds within the specified area
public void ConstructEdges(List <Vertex> includingNeighborVertices, Dictionary <int, int> includingNeighborGridPositionToVertex)
{
var chunkX = chunkNumber / chunkSizeY;
var chunkY = chunkNumber % chunkSizeY;
var startX = chunkX * chunkSize;
var startY = chunkY * chunkSize;
var endX = chunkX * chunkSize + chunkSize;
var endY = chunkY * chunkSize + chunkSize;
var edges = new List <Edge>();
for (var i = 0; i < includingNeighborVertices.Count; i++)
{
var x = includingNeighborVertices[i].GridPosition / sizeY;
var y = includingNeighborVertices[i].GridPosition % sizeY;
var gridPosition = x * sizeY + y;
if (x < startX || x >= endX || y < startY || y >= endY)
{
continue;
}
var otherSubGoals = GetDirectHReachable(x, y, includingNeighborGridPositionToVertex);
// Get the real vertex id
if (gridPositionToVertex.TryGetValue(gridPosition, out int vID))
{
vertices[vID] = new Vertex
{
EdgeOffset = edges.Count,
GridPosition = gridPosition
};
}
else
{
throw new Exception("This should never happen");
}
for (int j = 0, count = otherSubGoals.Count; j < count; j++)
{
var otherSubGoal = otherSubGoals[j];
if (otherSubGoal == i)
{
continue;
}
var ox = includingNeighborVertices[otherSubGoal].GridPosition / sizeY;
var oy = includingNeighborVertices[otherSubGoal].GridPosition % sizeY;
var oGridPosition = includingNeighborVertices[otherSubGoal].GridPosition;
var isOutsideOfChunk = (ox < startX || ox >= endX || oy < startY || oy >= endY);
var edgeID = (ulong)otherSubGoal << 32 | (ulong)(uint)i;
var toVertexID = -1;
if (isOutsideOfChunk)
{
toVertexID = -1;
}
else if (gridPositionToVertex.TryGetValue(oGridPosition, out int oID))
{
toVertexID = oID;
}
else
{
throw new Exception("This should never happen");
}
float cost = SubGoalGrid.Diagonal(x, y, ox, oy);
if (grid.GetWeight(x, y) == CellType.Road && grid.GetWeight(ox, oy) == CellType.Road)
{
cost *= 0.5f;
}
edges.Add(new Edge()
{
ToVertex = toVertexID,
ToVertexGridPosition = oGridPosition,
Cost = cost
});
}
}
this.edges = edges.ToArray();
}
public void FindSubGoalPath()
{
DebugDrawer.Clear();
// Convert visible grid to memory grid
var grid = GridController.Ground;
var pathGrid = GridController.Path;
var sizeX = GridController.active.size.x;
var sizeY = GridController.active.size.y;
var memoryGrid = createGraph();
// Set grid weights
var start = new Vector2Int(-1, -1);
var end = new Vector2Int(-1, -1);
for (var x = 0; x < sizeX; x++)
{
for (var y = 0; y < sizeY; y++)
{
var pathTile = pathGrid.GetTile(new Vector3Int(x, y, 0));
if (pathTile != null)
{
if (pathTile == GridController.active.start)
{
start = new Vector2Int(x, y);
}
else if (pathTile == GridController.active.end)
{
end = new Vector2Int(x, y);
}
}
}
}
if (start.x == -1 || end.x == -1)
{
Debug.Log("Couldn't find any start or end position");
return;
}
// List<Node> path;
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
SubGoalGrid sgg = new SubGoalGrid(memoryGrid);
sgg.ConstructSubgoals(0, 0, sizeX, sizeY);
sgg.ConstructEdges(0, 0, sizeX, sizeY);
Debug.Log(sgg.subGoals.Count);
sw.Stop();
Debug.Log("Preprocessing took " + sw.ElapsedMilliseconds + "ms");
sgg.DrawSubGoals();
List <Node> path;
var subGoalSearch = new SubGoalAStarSearch(sgg);
sw.Restart();
path = subGoalSearch.GetPath(sgg, start, end);
UnityEngine.Debug.Log("SubGoalSearch Path - Path" + (path == null ? " not " : " ") + "found in : " + sw.ElapsedMilliseconds + " ms");
if (path != null)
{
foreach (var pathTile in path)
{
if (pathTile.x == start.x && pathTile.y == start.y)
{
continue;
}
if (pathTile.x == end.x && pathTile.y == end.y)
{
continue;
}
if (pathTile.parent != null)
{
DebugDrawer.Draw(new Vector2Int(pathTile.parent.x, pathTile.parent.y), new Vector2Int(pathTile.x, pathTile.y), Color.blue);
}
DebugDrawer.DrawCube(new Vector2Int(pathTile.x, pathTile.y), Vector2Int.one, Color.blue);
}
}
}
// ConstructEdges constructs edges for subgoalds within the specified area
public void ConstructEdges(List <Vertex> includingNeighborVertices, Dictionary <int, int> includingNeighborGridPositionToVertex)
{
var chunkX = chunkNumber / chunkSizeY;
var chunkY = chunkNumber % chunkSizeY;
var startX = chunkX * overlayGraph.chunkSize;
var startY = chunkY * overlayGraph.chunkSize;
var endX = chunkX * overlayGraph.chunkSize + overlayGraph.chunkSize;
var endY = chunkY * overlayGraph.chunkSize + overlayGraph.chunkSize;
edges = new List <Edge>();
vertexEdgeMapping = new List <int>();
var createdEdges = new Dictionary <ulong, int>();
for (var i = 0; i < includingNeighborVertices.Count; i++)
{
var x = includingNeighborVertices[i].GridPosition / sizeY;
var y = includingNeighborVertices[i].GridPosition % sizeY;
var gridPosition = x * sizeY + y;
if (x < startX || x >= endX || y < startY || y >= endY)
{
continue;
}
var otherSubGoals = GetDirectHReachable(x, y, includingNeighborGridPositionToVertex);
// Get the real vertex id
if (gridPositionToVertex.TryGetValue(gridPosition, out int vID))
{
vertices[vID] = new Vertex
{
EdgeOffset = vertexEdgeMapping.Count,
GridPosition = gridPosition,
CellNumber = vertices[vID].CellNumber
};
}
else
{
throw new Exception("This should never happen");
}
for (int j = 0, count = otherSubGoals.Count; j < count; j++)
{
var otherSubGoal = otherSubGoals[j];
if (otherSubGoal == i)
{
continue;
}
var ox = includingNeighborVertices[otherSubGoal].GridPosition / sizeY;
var oy = includingNeighborVertices[otherSubGoal].GridPosition % sizeY;
var oGridPosition = includingNeighborVertices[otherSubGoal].GridPosition;
var isOutsideOfChunk = (ox < startX || ox >= endX || oy < startY || oy >= endY);
var edgeID = (ulong)otherSubGoal << 32 | (ulong)(uint)i;
if (createdEdges.TryGetValue(edgeID, out int edgeOffset) == false)
{
var toVertexID = -1;
if (isOutsideOfChunk)
{
toVertexID = -1;
}
else if (gridPositionToVertex.TryGetValue(oGridPosition, out int oID))
{
toVertexID = oID;
}
else
{
throw new Exception("This should never happen");
}
edges.Add(new Edge()
{
FromVertex = vID,
ToVertex = toVertexID,
ToVertexGridPosition = oGridPosition,
Cost = SubGoalGrid.Diagonal(x, y, ox, oy)
});
var newEdgeID = (ulong)i << 32 | (ulong)(uint)otherSubGoal;
createdEdges[newEdgeID] = edges.Count - 1;
vertexEdgeMapping.Add(edges.Count - 1);
}
else
{
vertexEdgeMapping.Add(edgeOffset);
}
}
}
}
