GodotComponentTest/utils/DebugGeometry.cs

using System.Collections.Generic;
using Godot;

namespace GodotComponentTest.utils;

public class DebugGeometry : Spatial {
    private ImmediateGeometry _immediateGeometry;

    private List<Transform> _transformStack;
    private Transform _currentTransform = Transform.Identity;

    public override void _Ready() {
        base._Ready();

        _immediateGeometry = (ImmediateGeometry)FindNode("ImmediateGeometry");

        Clear();
    }

    public void Clear() {
        _immediateGeometry.Clear();
        _transformStack = new List<Transform>();
        _transformStack.Add(Transform.Identity);
    }

    public void PushTransform(Transform transform) {
        _transformStack.Add(transform);
        _currentTransform = transform;
    }

    public void PushTranslated(Vector3 offset) {
        PushTransform(_currentTransform.Translated(offset));
    }

    public void PopTransform() {
        _transformStack.RemoveAt(_transformStack.Count - 1);
        _currentTransform = PeekTransform();
    }

    public Transform PeekTransform() {
        return _transformStack[_transformStack.Count - 1];
    }

    public void Begin(Mesh.PrimitiveType primitive, Texture texture = null) {
        _immediateGeometry.Begin(primitive, texture);
    }

    public void End() {
        _immediateGeometry.End();
    }

    public void AddVertex(Vector3 vertex) {
        _immediateGeometry.AddVertex(_currentTransform.Xform(vertex));
    }

    public void SetColor(Color color) {
        _immediateGeometry.SetColor(color);
    }

    public void AddBox(Vector3 extents) {
        Transform currentTransform = PeekTransform();
        // bottom square
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * -0.5f));

        // top square
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * -0.5f));

        // side
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * 0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * -0.5f, extents.y * -0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * 0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * 0.5f, extents.z * -0.5f));
        AddVertex(new Vector3(extents.x * 0.5f, extents.y * -0.5f, extents.z * -0.5f));
    }
}