private static void ExploreSides(Dictionary <Vector3I, GridLocation> map,
Queue <Block> cubeQueue, StepVectors steps, Vector3I currentPosition, FatNode fatNode)
{
foreach (var step in steps.EnumerateSides())
{
var peakedGridPosition = currentPosition + step;
if (!map.ContainsKey(peakedGridPosition))
{
continue;
}
var peakedLocation = map[peakedGridPosition];
if (peakedLocation.Visited)
{
if (peakedLocation.Node != null)
{
// Add both directions.
fatNode.Neighbours.Add(peakedLocation.Node);
peakedLocation.Node.Neighbours.Add(fatNode);
}
}
else
{
// TODO: check for obstacles even before enqueueing the block?
if (!cubeQueue.Contains(peakedLocation.Block)) // Note: this can be optimized
{
cubeQueue.Enqueue(peakedLocation.Block);
}
}
}
}
/// <param name="characterOrientationUpInGrid">character orientation up converted to grid coordinates</param>
internal static Vector3I GuessWhichSideIsUp(Vector3D characterOrientationUpInGrid)
{
var closestAngleVector = new StepVectors(Vector3I.Up).Enumerate()
.Select(v => new Tuple <double, Vector3I>(characterOrientationUpInGrid.Dot(v), v))
.OrderBy(x => x.Item1)
.Select(pair => pair.Item2).First();
return(((characterOrientationUpInGrid + closestAngleVector).LengthSquared() >
(characterOrientationUpInGrid - closestAngleVector).LengthSquared())
? closestAngleVector
: -closestAngleVector);
}
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);
}