DebugVisualizerExtension/NrxDebugVisualizer/Scenes/DebugVisualizerScene.cs

using Num.Roto.Visualization.Math.Geometry;
using Num.Roto.Visualization.SceneGraph.Geometry;
using Num.Roto.Visualization.SceneGraph.Material;
using Num.Roto.Visualization.SceneGraph.Scene;
using Num.Roto.Visualization.SceneGraph.Scene.Nodes;
using Num.Roto.Visualization.Types;
using System.Numerics;

namespace NrxDebugVisualizer.Scenes;

internal sealed class DebugVisualizerScene : Num.Roto.Visualization.SceneGraph.Scenes.SceneBase
{
    private static uint Counter;
    private static uint Uid => Counter++;

    private SceneNode Points { get; }
    private SceneNode Frames { get; }


    internal DebugVisualizerScene() : base("NrxDebuggerVisualizerScene")
    {
        Points = new SceneNode(nameof(Points), Objects);
        Frames = new SceneNode(nameof(Frames), Objects);
    }
    public void AddPointGeometry(Vector3 position, float pointSize)
    {
        var point = new Points([position], "Point");
        _ = new SceneGeometry("Point" + $".{Uid}", point, new Material(Colors4.White, null), Points)
        {
            PointSize = pointSize,
            ComputeFragColor = false
        };
      }
    public void AddPointListGeometry(Vector3[] positions, float pointSize)
    {
        var points = new Points(positions, "Points");
        var material = new Material(Colors4.White, null)
        {
            EmissiveColor = Colors4.White,
            VertexColorBlendFactor = 1
        };
        _ = new SceneGeometry("Points" + $".{Uid}", points, material, Points)
        {
            PointSize = pointSize,
            ComputeFragColor = false
        };
    }
    public void AddFrameGeometry(Frame frame, float length)
    {
        AddFrameListGeometry([frame], length);
    }
    public void AddFrameListGeometry(Frame[] frames, float length)
    {
        var material = new Material(Colors4.White, null)
        {
            EmissiveColor = Colors4.White,
            VertexColorBlendFactor = 1
        };
        var name = frames.Length > 1 ? "FrameList" : "Frame";
        var firstFrame = frames[0];
        var frameContainer = new SceneNode(name + $".{Uid}", Frames)
        {
            Frame = firstFrame
        };
        var inverse = firstFrame.Inverse();
        for (var i = 0; i < frames.Length; ++i)
        {
            frames[i] = inverse * frames[i];
        }
        var frameGeometry = new SceneGeometry("Geometry", Crosses.DefaultCross(0.5f), material, frameContainer)
        {
            InstanceFrames = [.. frames],
            Flags = 0,
            LineWidth = 1
        };
        if (frames.Length > 1)
        {
            var size = 2 * (frames.Length - 1);
            var path = new Vector3[size];
            path[0] = frames[0].Translation;
            for (var i = 1; i < frames.Length - 1; ++i)
            {
                path[2 * i] = path[2 * i - 1] = frames[i].Translation;
            }
            path[^1] = frames[^1].Translation;
            _ = new SceneGeometry("Path", new Lines(path), material, frameGeometry);
        }
    }
}