public static NavGraph ToNavGraph(this FatNavGraph fatNavGraph)
{
var setOfEdges = new HashSet <Edge>();
foreach (var fatNode in fatNavGraph.Nodes)
{
foreach (var neighbour in fatNode.Neighbours)
{
setOfEdges.Add(new Edge(fatNode.Id, neighbour.Id));
}
}
return(new NavGraph(
nodes: fatNavGraph.Nodes.Select(n => n.ToSlimNode()).ToList(),
edges: setOfEdges.ToList()));
}
internal FatNavGraph CreateGraph(CubeGrid grid, Vector3D start, Vector3I up)
{
var map = new Dictionary <Vector3I, GridLocation>(capacity: 256);
var startBlock = InitMapAndFindStartBlock(grid, start, map);
var navGraph = new FatNavGraph();
var steps = new StepVectors(up);
var cubeQueue = new Queue <Block>();
var nodeBuilder = new FatNodeBuilder();
cubeQueue.Enqueue(startBlock);
while (cubeQueue.Count > 0)
{
var currentCube = cubeQueue.Dequeue();
var currentPosition = currentCube.GridPosition.ToVector3I();
map[currentPosition].Visited = true;
// check for obstacles (2 blocks above the site)
if (map.ContainsKey(currentPosition + up) || map.ContainsKey(currentPosition + 2 * up))
{
continue;
}
var fatNode = nodeBuilder.Create(currentCube.Position); // TODO(P): Add some position offset.
map[currentPosition].Node = fatNode;
navGraph.Nodes.Add(fatNode);
ExploreSides(map, cubeQueue, steps, currentPosition, fatNode);
}
return(navGraph);
}