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