Reverse arc-length parameterization implementation

// This could have as many coordinates as you want, but make sure
// to change the distance function accordingly
typedef Position = { x:Float, y:Float };

typedef Parametric = Float -> Position;

// Computes a reverse arc-length parameterization to solve the
// travel speed problem and give samples with O(1) complexity.
// For the sake of simplicity, the code is somewhat unoptimized.
// The initialization stage is O(n^2) but could be brought to
// O(n log(n)).
class ArcSampler
{
    private var samples:Array<Position> = new Array<Position>();
    private var indices:Array<Int> = new Array<Int>();

    // Distance between two 2D points
    static private function distance(a:Position, b:Position) : Float
    {
        var x = a.x - b.x, y = a.y - b.y;
        return Math.sqrt(x * x + y * y);
    }

    // f is the parametrisation of the curve, ie a function of time
    // steps is the amount of steps, ie the number of samples - 1
    // t will be in the closed interval [lowt, hight]
    public function new(f:Parametric, steps:Int, ?lowt = 0., ?hight = 1.)
    {
        var lengths = new Array<Float>();
        // Convenience function to map [0, 1] to [lowt, hight] linearly
        var range = function (t:Float) return lowt + t * (hight - lowt);

        // Direct application of the reverse arc-length parameterization algorithm
        lengths.push(0);
        samples.push(f(range(0)));
        if(steps < 1)
            throw "ArcSampler : must use at least one step";
        for(i in 1 ... steps + 1)
        {
            samples.push(f(range(i / steps)));
            lengths.push(distance(samples[i], samples[i-1]) + lengths[i-1]);
        }
        for(i in 1 ... steps + 1)
            lengths[i] /= lengths[steps];

        indices[0] = 0;
        for(i in 1 ... steps + 1)
        {
            var s = i / steps;
            // Index of the highest length that is less or equal to s
            // Can be optimized with a binary search instead
            var j = lengths.filter(function(l) return l <= s).length - 1;
            indices.push(j);
        }
    }

    // Linear interpolation with factor t between a and b
    static private function lerp(a:Position, b:Position, t:Float) : Position
    {
        return { x:(1 - t) * a.x + t * b.x, y:(1 - t) * a.y + t * b.y};
    }

    // Returns the point at length s% of the total length of the curve
    // using the reverse arc-length parameterization
    public function curve(s:Float) : Position
    {
        if(s < 0 || s > 1)
            throw "Invalid s parameter : must be in [0, 1]";
        if(s == 1)
            return samples[samples.length - 1];
        var i = Std.int(s * indices.length);
        var t = indices[i];
        return lerp(samples[t], samples[t+1], s * indices.length - i);
    }
}