Godot De Casteljau Algorithm for 3D Path Splitting

DeCasteljau.cs

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

[Tool]
public partial class DeCasteljau : Node3D
{
    [Export]
    public Path3D path;

    [Export]
    public Path3D outputPath;

    private bool showDebugPoints = true;
    private bool showDebugSubCurves = true;
    private bool explodeDebugSubCurves = false;
    private bool showOutputCombinedCurve = true;

    [Export]
    public bool ShowDebugPoints {
        get => showDebugPoints;
        set {
            showDebugPoints = value;
            UpdateCurves();
        }
    }

    [Export]
    public bool ShowDebugSubCurves {
        get => showDebugSubCurves;
        set {
            showDebugSubCurves = value;
            UpdateCurves();
        }
    }

    [Export]
    public bool ExplodeDebugSubCurves {
        get => explodeDebugSubCurves;
        set {
            explodeDebugSubCurves = value;
            UpdateCurves();
        }
    }

    [Export]
    public bool ShowOutputCombinedCurve {
        get => showOutputCombinedCurve;
        set {
            showOutputCombinedCurve = value;
            UpdateCurves();
        }
    }

    
    private float midpoint = 0.5f;

    [Export]
    public float Midpoint {
        get => midpoint;
        set {
            midpoint = value;
            UpdateCurves();
        }
    }

    public override void _Ready()
    {
        
    }

    private void UpdateCurves()
    {
        if(path != null && outputPath != null)
            DeCasteljau3D_Split(path.Curve, midpoint);
    }

    private void DeCasteljau3D_Split(Curve3D curve, float t)
    {
        Dictionary<int,List<int>> curveIdxMap = new();
        List<Curve3D> curves = new();

        // Decompose into 2-point curves
        for(var i = 0; i < curve.PointCount - 1; i++)
        {
            var c = new Curve3D();
            c.AddPoint(curve.GetPointPosition(i), null, curve.GetPointOut(i));
            c.AddPoint(curve.GetPointPosition(i+1), curve.GetPointIn(i+1), null);
            curves.Add(c);

            curveIdxMap[i] = new List<int> { i, i+1 };
        }

        foreach(var c in outputPath.GetChildren()) {
            outputPath.RemoveChild(c);
        }

        var _renderDebugPoints = (Vector3[] p) => {
            _addDebugSphere(p[0], 0.1f);
            _addDebugSphere(p[3], 0.1f);
            _addDebugSphere(p[1], 0.01f);
            _addDebugSphere(p[2], 0.01f);
        };

        // Figure out which sub-curve the cut will be on
        var bp = curve.SampleBaked(curve.GetBakedLength() * t);
        _addDebugSphere(bp, 0.075f);
        var errorBias = 0.01f;
        var subCurveIdx = -1;
        var localMidpoint = 0f;

        for(var i = 0; i < curves.Count; i++)
        {
            var subClosest = curves[i].GetClosestPoint(bp);
            if((bp - subClosest).Length() < errorBias) {
                GD.Print("T probably belongs in sub-curve ", i);
                subCurveIdx = i;

                // Calculate midpoint
                var prevCombinedSegmentLength = curves.GetRange(0, subCurveIdx).Sum(c => c.GetBakedLength());
                var desiredOffset = curve.GetBakedLength() * t;
                var desiredLocalOffset = desiredOffset - prevCombinedSegmentLength;
                localMidpoint = desiredLocalOffset / curves[i].GetBakedLength();
                GD.Print("DLO ", desiredLocalOffset, " Local midpoint ", localMidpoint);

                break;
            }
        }

        (Vector3[] Left,Vector3[] Right, CurvePoint K) = DCJ2(_GetPointsAndControlsFromCurve(curves[subCurveIdx]), localMidpoint);

        _addDebugSphere(K.Point, 0.1f);
        _addDebugSphere(K.In, 0.01f);
        _addDebugSphere(K.Out, 0.01f);

        _renderDebugPoints(Left);
        _renderDebugPoints(Right);

        if(showDebugSubCurves) {
            // Debug sub-curves
            var newCurves = curves.GetRange(0, subCurveIdx);
            newCurves.Add(_GetCurveFromPointsAndControls(Left));
            newCurves.Add(_GetCurveFromPointsAndControls(Right));
            newCurves.AddRange(curves.GetRange(subCurveIdx + 1, curves.Count - (subCurveIdx + 1)));

            for(var i = 0; i < newCurves.Count; i++)
            {
                var p = new Path3D();
                outputPath.AddChild(p);
                p.Owner = GetTree().EditedSceneRoot;
                p.Curve = newCurves[i];
                if(explodeDebugSubCurves) p.Position = path.Position + new Vector3(i * 0.5f, 0, 2);
            }
        }

        Curve3D combinedCurve = null;
        if(showOutputCombinedCurve)
        {
            // Combine L+R
            var replacementCurve = _combineCurves(_GetCurveFromPointsAndControls(Left), _GetCurveFromPointsAndControls(Right));

            // Put it all back together
            var newCurves = curves.GetRange(0, subCurveIdx);
            newCurves.Add(replacementCurve);
            newCurves.AddRange(curves.GetRange(subCurveIdx + 1, curves.Count - (subCurveIdx + 1)));

            GD.Print("New Curves: ", newCurves.Count);

            combinedCurve = newCurves[0];
            for(var i = 1; i < newCurves.Count; i++)
            {
                combinedCurve = _combineCurves(combinedCurve, newCurves[i]);
            }
        }

        outputPath.Curve = combinedCurve;
    }

    private Curve3D _combineCurves(Curve3D a, Curve3D b)
    {
        // End point of a and first point of b are assumed to be identical
        Curve3D c = new();
        c.BakeInterval = 0.1f;

        for(var i = 0; i < a.PointCount; i++)
        {
            c.AddPoint(a.GetPointPosition(i));
            c.SetPointIn(i, a.GetPointIn(i));
            c.SetPointOut(i, (i < a.PointCount - 1) ? a.GetPointOut(i) : b.GetPointOut(0));
        }

        for(var i = 1; i < b.PointCount; i++)
        {
            c.AddPoint(b.GetPointPosition(i));
            c.SetPointIn(a.PointCount + i - 1, b.GetPointIn(i));
            c.SetPointOut(a.PointCount + i - 1, b.GetPointOut(i));
        }

        GD.Print("Combined curve has ", c.PointCount, " points");

        return c;
    }

    private void _addDebugSphere(Vector3 x, float r) {
        if(!showDebugPoints) return;

        var p = new CsgSphere3D();
        outputPath.AddChild(p);
        p.Owner = GetTree().EditedSceneRoot;
        p.Radius = r;
        p.Position = x;
    }

    private Vector3[] _GetPointsAndControlsFromCurve(Curve3D c)
    {
        return new[] {
            c.GetPointPosition(0),
            c.GetPointPosition(0) + c.GetPointOut(0),
            c.GetPointPosition(1) + c.GetPointIn(1),
            c.GetPointPosition(1)
        };
    }

    private Curve3D _GetCurveFromPointsAndControls(Vector3[] p)
    {
        var c = new Curve3D();
        c.AddPoint(p[0]);
        c.SetPointOut(0, p[1] - p[0]); // Make out relative to point
        c.AddPoint(p[3]);
        c.SetPointIn(1, p[2] - p[3]);

        return c;
    }

    private (Vector3[] Left,Vector3[] Right, CurvePoint K) DCJ2(Vector3[] points, float t)
    {
        // Adapted from Paul LeBeau's explanation here: https://stackoverflow.com/a/18681336
        if(points.Length != 4) throw new Exception();

        var A = points[0];
        var B = points[1];
        var C = points[2];
        var D = points[3];
        var E = A.Lerp(B, t);
        var F = B.Lerp(C, t);
        var G = C.Lerp(D, t);
        var H = E.Lerp(F, t);
        var J = F.Lerp(G, t);
        var K = H.Lerp(J, t);

        var left = new Vector3[] { A, E, H, K };
        var right = new Vector3[] { K, J, G, D };

        return (left, right, new CurvePoint { Point = K, In = H, Out = J });
    }

    private struct CurvePoint
    {
        public Vector3 Point;
        public Vector3 In;
        public Vector3 Out;
        public int Idx;
    }
}