Navigation Manager and World Manager for Dynamic Runtime Changes

Extensions.cs

using Godot;
using System.Collections.Generic;
using System.Linq;

namespace Project.Extensions;
public static class GodotExtensions
{
    public static T? GetChildByType<T>(this Node node, bool recursive = true)where T : Node
    {
        int childCount = node?.GetChildCount() ?? 0;
        for (int i = 0; i < childCount; i++)
        {
            Node? child = node?.GetChild(i) ?? null;
            if (child is T childT)
            { 
                return childT;
            }

            if (recursive && child?.GetChildCount() > 0)
            {
                T? recursiveResult = child.GetChildByType<T>(true);
                if (recursiveResult != null)
                { 
                    return recursiveResult;
                }
            }
        }
        return null;
    }

    public static IEnumerable<T> WithoutIndices<T>(this IEnumerable<T> enumerable, IEnumerable<int> indices)
    {
        for (int i = 0; i < enumerable.Count(); i++)
        {
            if (!indices.Contains(i))
            { 
                yield return enumerable.ElementAt(i);
            }
        }
    }
}

NavigationManager.cs

using Godot;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using Project.Extensions;
namespace Project.Scenes.Singletons;
public partial class NavigationManager : Node
{
    private NavigationRegion3D _navigationRegion = null!;
    public bool IsBaking { get; private set; } = false;
    private bool _shouldRebake = false;
    private Dictionary<int, (int minIndex, int maxIndex)> _indexIndicesMapping = new Dictionary<int, (int minIndex, int maxIndex)>();
    private Dictionary<int, (int minIndex, int maxIndex)> _indexVertexMapping = new Dictionary<int, (int minIndex, int maxIndex)>();
    public override void _Ready()
    {
        _navigationRegion = GetTree().Root.GetChildByType<NavigationRegion3D>()!;
#if DEBUG
        NavigationServer3D.SetDebugEnabled(true);
#endif

    }
    private NavigationMeshSourceGeometryData3D _cachedNavigationData = new NavigationMeshSourceGeometryData3D();
    public async void RebakeOnSeperateThread()
    {
        if (IsBaking)
        {
            _shouldRebake = true;
        }
        else
        {
            IsBaking = true;

            NavigationMesh navMesh = (NavigationMesh)_navigationRegion.NavigationMesh.Duplicate();
            _navigationRegion.NavigationMesh = null;
            _navigationRegion.NavigationMesh = navMesh;
            await Task.Run(() =>
            {
                NavigationServer3D.BakeFromSourceGeometryData(_navigationRegion.NavigationMesh, _cachedNavigationData, Callable.From(() => { }));
            });
            BakeFinished();
        }
    }

    public void AddMesh(int index, Mesh mesh, Transform3D transform)
    {
        int cachedIndicesLength = _cachedNavigationData.Indices.Length;
        int cachedVerticesLength = _cachedNavigationData.Vertices.Length;
        _cachedNavigationData.AddMesh(mesh, transform);
        _indexIndicesMapping.Add(index, (cachedIndicesLength, _cachedNavigationData.Indices.Length));
        _indexVertexMapping.Add(index, (cachedVerticesLength, _cachedNavigationData.Vertices.Length));
    }
    public void AddNavigationalChild(Node3D node)
    {
        _navigationRegion.AddChild(node);
    }
    public void RemoveMesh(int index)
    {
        _cachedNavigationData.Indices = _cachedNavigationData.Indices.WithoutIndices(QuickIndexGenerator(_indexIndicesMapping[index].minIndex, _indexIndicesMapping[index].maxIndex)).ToArray();
        _cachedNavigationData.Vertices = _cachedNavigationData.Vertices.WithoutIndices(QuickIndexGenerator(_indexVertexMapping[index].minIndex, _indexVertexMapping[index].maxIndex)).ToArray();
        _indexIndicesMapping.Remove(index);
        _indexVertexMapping.Remove(index);
    }
    private void BakeFinished()
    {  
        IsBaking = false;
        if (_shouldRebake && _cachedNavigationData is not null)
        {
            _shouldRebake = false;
            RebakeOnSeperateThread();
        }
    }
    private int[] QuickIndexGenerator(int min, int max)
    {
        int[] ints = new int[max - min];
        int minCache = min;
        for (int i = 0; i < max - min; i++)
        {
            ints[i] = minCache++;
        }
        return ints;
    }
}

WorldManager.cs

using Godot;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using Project.Extensions;
using Project.Scenes.Blocks;
using Project.Scenes.Singletons;

namespace VSpartan.Scenes.Services;
public partial class WorldManager : Node
{
    [ExportCategory("World Size Settings")]
    [Export]
    public int Height { get; set; }
    [Export]
    public int Width { get; set; }
    [Export]
    public int Length { get; set; }
    [Export]
    public string? MapSeed { get; set; }

    private MultiMeshInstance3D _worldMesh = new MultiMeshInstance3D();
    private Dictionary<string, int> _staticBodyMeshMap = new Dictionary<string, int>();
    private Dictionary<Vector3, int> _blockLocationMap = new Dictionary<Vector3, int>();
    private NavigationManager _navigationManager = null!;
    private int _activeBlockCount;
    public override void _Ready()
    {
        _navigationManager = GetNode<NavigationManager>("/root/NavigationManager");

        GenerateWorld(MapSeed);
    }
    private void GenerateWorld(string? mapSeed)
    {
        //TODO MAKE DYNAMIC AND DETERMINE WHAT TYPES OF BLOCKS TO SPAWN
        PackedScene blockSpwaner = ResourceLoader.Load<PackedScene>("res://Scenes/Blocks/Block25.tscn"); 
        Mesh blockMesh = blockSpwaner.Instantiate<Block25>().GetNode<MeshInstance3D>("Cube").Mesh;


        MultiMesh multiMesh = new MultiMesh();
        multiMesh.TransformFormat = MultiMesh.TransformFormatEnum.Transform3D;
        multiMesh.Mesh = blockMesh;
        multiMesh.UseColors = true;
        multiMesh.InstanceCount = (Width * Length * Height) * 10;
        _worldMesh.Multimesh = multiMesh;

        FastNoiseLite noise = new FastNoiseLite();
        noise.Seed = mapSeed is null ? (int)DateTime.Now.Ticks : mapSeed.GetHashCode();
        noise.NoiseType = FastNoiseLite.NoiseTypeEnum.Perlin;
        noise.Frequency = 0.01f;
        noise.FractalLacunarity = 2f;
        noise.FractalGain = 0.5f;
        noise.FractalType = FastNoiseLite.FractalTypeEnum.Fbm;
        for (int x = 0; x < Width; x++)
        {
            for (int y = 0; y < Length; y++)
            {
                for (int z = 0; z < Height; z++)
                {
                    float noiseValue = noise.GetNoise3D(Width, Length, Height);
                    //TODO MAKE DYNAMIC AND SPAWN DIFFERENT TYPES OF BLOCKS
                    float sinVal = Mathf.Sin(x + y + z);
                    if (sinVal > 0.2f)
                    AddBlock<Block25>(new Vector3(x, z, y), false);
                }
            }
        }

        _navigationManager.AddNavigationalChild(_worldMesh);

        _navigationManager.RebakeOnSeperateThread();

    }
    public void AddBlock<T>(Vector3 position, bool rebake = true) where T : Block
    {
        Vector3 roundedPosition = new Vector3(Mathf.Round(position.X), Mathf.Round(position.Y), Mathf.Round(position.Z));
        if (_blockLocationMap.ContainsKey(roundedPosition))
        {
            position = FindClosestOpenPosition(position);
        }
        else
        {
            position = roundedPosition;
        }

        //TODO MAKE DYNAMIC AND DETERMINE WHAT TYPES OF BLOCKS TO SPAWN BASED ON TYPE T
        PackedScene blockSpwaner = ResourceLoader.Load<PackedScene>("res://Scenes/Blocks/Block25.tscn");
        Mesh blockMesh = blockSpwaner.Instantiate<Block25>().GetNode<MeshInstance3D>("Cube").Mesh;
        Shape3D meshShape = blockMesh.CreateTrimeshShape();

        Transform3D blockTransform = new Transform3D(Basis.Identity, new Vector3(position.X * 2, position.Y * 2, position.Z * 2));
        blockTransform = blockTransform.Scaled(new Vector3(0.5f, 0.5f, 0.5f));

        _navigationManager.AddMesh(_activeBlockCount, _worldMesh.Multimesh.Mesh, blockTransform);

        _worldMesh.Multimesh.SetInstanceTransform(_activeBlockCount, blockTransform);
        _worldMesh.Multimesh.SetInstanceColor(_activeBlockCount, new Color((float)GD.RandRange(0, 1), (float)GD.RandRange(0, 1), (float)GD.RandRange(0, 1)));
        CollisionShape3D collisionShape = new CollisionShape3D();
        collisionShape.Shape = meshShape;
        StaticBody3D staticBody = new StaticBody3D();
        staticBody.CollisionLayer = 0b0000_0001; //layer for ground right now
        staticBody.CollisionMask = 0b0000_0001; //layer for ground right now
        staticBody.Name = $"World_StaticBody_{_activeBlockCount}";
        staticBody.Transform = blockTransform;
        staticBody.AddChild(collisionShape);

        Node3D destructableHolder = new Node3D();
        destructableHolder.Name = $"World_DestructableHolder_{_activeBlockCount}";
        destructableHolder.Transform = blockTransform;
        Destructable destructable = new Destructable();
        destructable.Name = $"World_Destructable_{_activeBlockCount}";
        destructable.FragmentedObject = ResourceLoader.Load<PackedScene>("res://Scenes/Blocks/Block25_Broken.tscn");
        destructable.FragmentedObjectContainer = destructableHolder;
        destructableHolder.AddChild(destructable);

        _worldMesh.AddChild(destructableHolder);

        _worldMesh.AddChild(staticBody);

        _blockLocationMap.Add(position, _activeBlockCount);
        _staticBodyMeshMap.Add(staticBody.Name, _activeBlockCount);

        _activeBlockCount++;

        if (rebake)
        {
            _navigationManager.RebakeOnSeperateThread();
        }
    }
    public void RemoveBlock(StaticBody3D staticBody, bool explode = false, float explosionPower = 3.0f)
    {
        int? index = _staticBodyMeshMap.TryGetValue(staticBody.Name, out int value) ? value : null;
        if (index is null) { return; }
        Vector3 meshLocation = _worldMesh.Multimesh.GetInstanceTransform((int)index).Origin;

        RemoveBlock(meshLocation, staticBody, explode, explosionPower, (int)index);
    }
    public void RemoveBlock(string staticBodyName, bool explode = false, float explosionPower = 3.0f)
    {
        StaticBody3D staticBody = _worldMesh.GetNode<StaticBody3D>(staticBodyName);
        int? index = _staticBodyMeshMap.TryGetValue(staticBody.Name, out int value) ? value : null;
        if (index is null) { return; }
        Vector3 meshLocation = _worldMesh.Multimesh.GetInstanceTransform((int)index).Origin;
        RemoveBlock(meshLocation, staticBody, explode, explosionPower, (int)index);
    }
    public void RemoveBlock(Vector3 position, bool explode = false, float explosionPower = 3.0f)
    {
        int? index = GetBlockLocationClosestTo(position, out Vector3 roundedPosition);
        if (index is null) { return; }
        string staticBodyName = $"World_StaticBody_{index}";
        StaticBody3D staticBody = _worldMesh.GetNode<StaticBody3D>(staticBodyName);

        RemoveBlock(roundedPosition, staticBody, explode, explosionPower, (int)index);
    }
    private async void RemoveBlock(Vector3 meshLocation, StaticBody3D staticBody, bool explode, float explosionPower, int index)
    {
        _blockLocationMap.Remove(meshLocation);
        _staticBodyMeshMap.Remove(staticBody.Name);
        if (explode)
        {
            _worldMesh.GetNode<Node3D>($"World_DestructableHolder_{index}")?.GetNode<Destructable>($"World_Destructable_{index}")?.Destroy(explosionPower, new Vector3(0.5f, 0.5f, 0.5f));
        }
        _worldMesh.RemoveChild(staticBody);
        staticBody.QueueFree();
        _worldMesh.Multimesh.SetInstanceTransform(index, new Transform3D(Basis.Identity, new Vector3(-1, -1, -1)).Scaled(new Vector3(0.1f, 0.1f, 0.1f)));
        await Task.Run(() =>
        {
            _navigationManager.RemoveMesh(index);
        });
        _navigationManager.RebakeOnSeperateThread();
    }

    private int? GetBlockLocationClosestTo(Vector3 position, out Vector3 roundedPosition)
    {
        float x = _blockLocationMap.Keys.Select(x => x.X).ClosestTo(position.X);
        float y = _blockLocationMap.Keys.Select(x => x.Y).ClosestTo(position.Y);
        float z = _blockLocationMap.Keys.Select(x => x.Z).ClosestTo(position.Z);
        roundedPosition = new Vector3(x, y, z);
        return _blockLocationMap.TryGetValue(new Vector3(x, y, z), out int value) ? value : null;
    }

    private Vector3 FindClosestOpenPosition(Vector3 position)
    {
        GetBlockLocationClosestTo(position, out Vector3 roundedPosition);

        float x = Mathf.Abs(position.X - roundedPosition.X);
        float y = Mathf.Abs(position.Y - roundedPosition.Y);
        float z = Mathf.Abs(position.Z - roundedPosition.Z);

        return new Vector3(
            Mathf.Round(x > y && x > z ? position.X + (position.X - roundedPosition.X) : position.X),
            Mathf.Round(y > x && y > z ? position.Y + (position.Y - roundedPosition.Y) : position.Y),
            Mathf.Round(z > x && z > y ? position.Z + (position.Z - roundedPosition.Z) : position.Z));
    }



}