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