Splines

/* LSE_FILE_HEADER */
/* LSE_LLIB_HEADER */

include "lse.llib.util.gs"


class LlSpline
{
    public float XK = 1.0, XS = 0.0;
    public float YK = 1.0, YS = 0.0;

    public void Init(float[] x, float[] y) {}

    public float Y(float x) { return 0.0; }
    public float DYDX(float x) { return 0.0; }
};


class LlLinearSpline isclass LlSpline
{
    float[] sx, sy;
    int count;

    public void Init(float[] x, float[] y)
    {
        LlUtil.Assert(x.size() == y.size() and x.size() >= 2);
        sx = x;
        sy = y;
        count = x.size() - 1;
    }

    final float linearFunc(float x0, float x1, float y0, float y1, float x)
    {
        if (x0 == x1)
            return y0;
        float k = (y1 - y0) / (x1 - x0);
        float b = y1 - k * x1;
        return k * x + b;
    }

    final int findSegment(float x)
    {
        if (x <= sx[0])
            return 0;
        else if (x >= sx[count])
            return count - 1;

        int l = 0;
        int r = count;
        while (l < r - 1)
        {
            int m = (l + r) / 2;
            if (x <= sx[m])
                r = m;
            else
                l = m;
        }
        return l;
    }

    public float Y(float x)
    {
        x = x * XK + XS;
        int l = findSegment(x);
        return linearFunc(sx[l], sx[l + 1], sy[l], sy[l + 1], x) * YK + YS;
    }

    final float linearFuncDeriv(float x0, float x1, float y0, float y1, float x)
    {
        if (x0 == x1)
            return y0;
        float k = (y1 - y0) / (x1 - x0);
        return k;
    }

    public float DYDX(float x)
    {
        x = x * XK + XS;
        int l = findSegment(x);
        return linearFuncDeriv(sx[l], sx[l + 1], sy[l], sy[l + 1], x) * YK * XK;
    }
};


class LlCubicSpline isclass LlSpline
{
    float[] px;
    float[] py;
    int scount;
    float[] sa;
    float[] sb;
    float[] sc;
    float[] sd;

    void interpolate()
    {
        float[] mr = new float[scount - 2];
        float[] mc = new float[scount - 1];
        float[] my = new float[scount - 1];

        float[] h = new float[scount];
        float[] e = new float[scount];

        sa = new float[scount];
        sb = new float[scount];
        sc = new float[scount + 1];
        sd = py;

        int i;
        for (i = 0; i < scount; ++i)
        {
            h[i] = px[i + 1] - px[i];
            e[i] = (py[i + 1] - py[i]) / h[i];
        }

        for (i = 1; i < scount; ++i)
        {
            int j = i - 1;
            mc[j] = 2.0 * (h[i - 1] + h[i]);

            my[j] = 3.0 * (e[i] - e[i - 1]);

            if (i < scount - 1)
                mr[j] = h[i];
        }

        for (i = 1; i < scount - 1; ++i)
        {
            int j = i - 1;
            float k = mr[j] / mc[j];
            mc[j + 1] = mc[j + 1] - mr[j] * k;
            my[j + 1] = my[j + 1] - my[j] * k;
        }

        if (scount > 1)
        {
            int k = scount - 1;
            int j = k - 1;
            sb[k] = my[j] / mc[j];

            for (i = scount - 2; i >= 1; --i)
            {
                j = i - 1;
                sb[i] = (my[j] - sb[i + 1] * mr[j]) / mc[j];
            }
        }

        sb[0] = 0.0;

        for (i = 0; i < scount - 1; ++i)
        {
            sa[i] = (sb[i + 1] - sb[i]) / (3.0 * h[i]);
            sc[i] = e[i] - h[i] / 3.0 * (sb[i + 1] + 2.0 * sb[i]);
        }

        int k = scount - 1;
        sa[k] = -sb[k] / (3.0 * h[k]);
        sc[k] = e[k] - h[k] / 3.0 * (2.0 * sb[k]);
        sc[scount] = (3.0 * sa[k] * h[k] + 2.0 * sb[k]) * h[k] + sc[k];
    }

    public void Init(float[] x, float[] y)
    {
        LlUtil.Assert(x.size() == y.size() and x.size() >= 2);
        px = x;
        py = y;
        scount = x.size() - 1;
        interpolate();
    }

    public float Y(float x)
    {
        x = x * XK + XS;
        float y = 0.0;
        if (x <= px[0])
        {
            float xr = x - px[0];
            y = sc[0] * xr + sd[0];
        }
        else if (x >= px[scount])
        {
            float xr = x - px[scount];
            y = sc[scount] * xr + sd[scount];
        }
        else
        {
            int l = 0;
            int r = scount;
            while (l < r - 1)
            {
                int m = (l + r) / 2;
                if (x <= px[m])
                    r = m;
                else
                    l = m;
            }
            float xr = x - px[l];
            y = ((sa[l] * xr + sb[l]) * xr + sc[l]) * xr + sd[l];
        }

        return y * YK + YS;
    }

    public float DYDX(float x)
    {
        x = x * XK + XS;
        float y = 0.0;
        if (x <= px[0])
        {
            y = sc[0];
        }
        else if (x >= px[scount])
        {
            y = sc[scount];
        }
        else
        {
            int l = 0;
            int r = scount;
            while (l < r - 1)
            {
                int m = (l + r) / 2;
                if (x <= px[m])
                    r = m;
                else
                    l = m;
            }
            float xr = x - px[l];
            y = (3.0 * sa[l] * xr + 2.0 * sb[l]) * xr + sc[l];
        }

        return y * YK * XK;
    }
};


class LlSplineConstructorBase
{
    public LlLinearSpline NewLinearSpline(float[] x, float[] y)
    {
        LlLinearSpline ret = new LlLinearSpline();
        ret.Init(x, y);
        return ret;
    }

    public LlCubicSpline NewCubicSpline(float[] x, float[] y)
    {
        LlCubicSpline ret = new LlCubicSpline();
        ret.Init(x, y);
        return ret;
    }
};


static class LlSplineConstructor isclass LlSplineConstructorBase
{
};