private void searchForwardBasicGraph()
{
var min = forwardQueueGraph.Remove();
var chunk = graph.GetChunk(min.ChunkID);
var edgeEnd = (min.VertexID + 1 == chunk.vertices.Length) ? chunk.edges.Length : chunk.vertices[min.VertexID + 1].EdgeOffset;
for (var i = chunk.vertices[min.VertexID].EdgeOffset; i < edgeEnd; i++)
{
var edge = chunk.edges[i];
var edgeTarget = edge.ToVertex;
var edgeTargetChunkID = edgeTarget == -1 ? graph.GetChunkID(edge.ToVertexGridPosition) : chunk.chunkNumber;
var edgeTargetChunk = graph.GetChunk(edgeTargetChunkID);
var edgeTargetVertexID = edgeTarget == -1 ? edgeTargetChunk.gridPositionToVertex[edge.ToVertexGridPosition] : edgeTarget;
// Get the graph node
var node = getNodeGraph(edgeTargetChunk.vertices[edgeTargetVertexID].GridPosition, edgeTargetVertexID, edgeTargetChunkID);
// The new distance of this edge
var newDist = min.CostA + edge.Cost;
if (node.isOpenA() == false || newDist < node.CostA)
{
node.ParentA = min;
node.CostA = newDist;
if (node.isOpenA() == false)
{
node.setOpenA();
forwardQueueGraph.Add(node, newDist);
}
else
{
forwardQueueGraph.Update(node, newDist);
}
if (node.isOpenB())
{
var newPathLength = node.CostA + node.CostB;
if (newPathLength < shortestPath)
{
shortestPath = newPathLength;
middleNode = node;
}
}
}
}
}
public void FindChunkedPath()
{
DebugDrawer.Clear();
var pathGrid = GridController.Path;
var memoryGrid = createGraph();
var graph = new ChunkedPathfinding.Graph(memoryGrid, 128);
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
sw.Start();
graph.BuildAll();
UnityEngine.Debug.Log("ChunkedGraph Building took " + sw.ElapsedMilliseconds + " ms");
var sizeX = GridController.active.size.x;
var sizeY = GridController.active.size.y;
// 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;
}
sw.Restart();
List <ChunkedPathfinding.GraphPathfinder.GraphNode> path;
var pathfinder = new ChunkedPathfinding.GraphPathfinder(graph);
path = pathfinder.BidirectionalDijkstra(start.x, start.y, end.x, end.y);
UnityEngine.Debug.Log("ChunkedGraph - Path" + (path == null ? " not " : " ") + "found in : " + sw.ElapsedMilliseconds + " ms");
// graph.DrawCellBorders();
// graph.DrawOverlayGraph(1);
graph.DrawGraph();
// overlayGraph.DrawGraph();
if (path != null)
{
for (var i = 0; i < path.Count; i++)
{
int x;
int y;
if (i == 0)
{
x = start.x;
y = start.y;
}
else if (i == path.Count - 1)
{
x = end.x;
y = end.y;
}
else
{
var pathTile = path[i];
var chunk = graph.GetChunk(pathTile.ChunkID);
int gridPosition = chunk.vertices[pathTile.VertexID].GridPosition;
x = gridPosition / graph.sizeY;
y = gridPosition % graph.sizeY;
if (x == start.x && y == start.y)
{
continue;
}
if (x == end.x && y == end.y)
{
continue;
}
}
DebugDrawer.DrawCube(new Vector2Int(x, y), Vector2Int.one, Color.blue);
}
}
}