lib/polyfit.cc

Go to the documentation of this file.

//
// canola - canon canola 1614p emulator
// Copyright (C) 2012 Peter Miller
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License, version 3, as
// published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program. If not, see <http://www.gnu.org/licenses/>.
//

#include <lib/ac/assert.h>
#include <lib/ac/math.h>
#include <lib/ac/stdio.h>

#include <lib/polyfit.h>


polyfit::data_t polyfit::epsilon = 1e-8;


polyfit::~polyfit()
{
    delete [] coefficients;
    coefficients = 0;
    mat_free(matrix);
    matrix = 0;
}


polyfit::polyfit(int order) :
    rows(order + 1),
    cols(order + 2),
    coefficients(0),
    matrix(0)
{
    assert(order >= 1);
    matrix = mat_alloc();
}


void
polyfit::enter(double xord, double yord)
{
    assert(matrix);
    data_t product = 1.0;
    for (unsigned row = 0; row < rows; row++)
    {
        matrix[row][cols - 1] += product * yord;
        data_t product2 = product;
        for (unsigned col = 0; col < rows; col++)
        {
            matrix[row][col] += product2;
            product2 *= xord;
        }
        product *= xord;
    }
}


polyfit::solve_t
polyfit::solve(void)
{
    assert(matrix);
    if (!enough_points())
        return solve_insufficient;

    //
    // create and initialise the working area
    //
    data_t epsilon_magnitude = 0;
    data_t **working = mat_alloc();
    for (unsigned row = 0; row < rows; row++)
    {
        for (unsigned col = 0; col < cols; col++)
        {
            data_t n = matrix[row][col];
            data_t nn = fabsl(n);
            if (epsilon_magnitude < nn)
                epsilon_magnitude = nn;
            working[row][col] = n;
        }
    }
    // double Mantissa is 48 bits
    // long double Mantissa is 64 bits
    // throw 8 away
    epsilon = ldexpl(epsilon_magnitude, -56);
    if (epsilon > 1e-5)
        epsilon = 1e-5;

#if 0
    printf("epsilon = %g\n", (double)epsilon);
    mat_dump(working);
#endif

    //
    // reduce the matrix
    //
    for (unsigned row = 0; row < rows; row++)
    {
        //
        // find largest magnitude to pivot on
        //
        unsigned pivot_row = row;
        data_t pivot_mag = fabsl(working[row][row]);
        for (unsigned row2 = row + 1; row2 < rows; ++row2)
        {
            data_t mag = fabsl(working[row2][row]);
            if (mag > pivot_mag)
            {
                pivot_row = row2;
                pivot_mag = mag;
            }
        }
        if (pivot_row != row)
        {
            //
            // exchange rows
            //
            for (unsigned col = row; col < cols; col++)
            {
                data_t temp = working[row][col];
                working[row][col] = working[pivot_row][col];
                working[pivot_row][col] = temp;
            }
        }

        if (fabsl(working[row][row]) < epsilon)
        {
            //
            // singular matrix
            //
            mat_free(working);
            return solve_singular;
        }

        //
        // get a 1 in the pivot position
        //
        data_t factor = 1. / working[row][row];
        for (unsigned col = row; col < cols; col++)
        {
            data_t n = working[row][col] * factor;
            // if close to an integer, go there
            data_t nn = floorl(n + 0.5);
            if (fabsl(n - nn) < epsilon)
                n = nn;
            working[row][col] = n;
        }

        //
        // subtract multiple of pivot row from
        // all others to get zeros in rest of
        // pivot col.
        //
        for (unsigned row2 = 0; row2 < rows; row2++)
        {
            // except pivot row
            if (row2 != row)
            {
                data_t factor2 = working[row2][row];
                for (unsigned col = row; col < cols; col++)
                    working[row2][col] -= factor2 * working[row][col];
            }
        }
#if 0
        mat_dump(working);
#endif
    }

    //
    // create solution space
    //
    if (!coefficients)
        coefficients = new data_t [rows];

    //
    // copy solution
    //
    for (unsigned row = 0; row < rows; row++)
        coefficients[row] = working[row][rows];
    mat_free(working);
    return solve_ok;
}


double
polyfit::coefficient(int n)
{
    if (!coefficients || n < 0 || (unsigned)n >= rows)
        return 0;
    return coefficients[n];
}


double
polyfit::evaluate(double x)
{
    if (!coefficients)
        return 0;
    double sum = 0;
    for (int j = rows - 1; j >= 0; j--)
        sum = sum * x + coefficients[j];
    double rsum = floorl(sum + 0.5);
    if (fabsl(sum - rsum) < epsilon)
        sum = rsum;
    return sum;
}


polyfit::data_t **
polyfit::mat_alloc(void)
{
    data_t **result = new data_t * [rows];
    result[0] = new data_t [rows * cols];
    for (unsigned row = 1; row < rows; ++row)
        result[row] = result[row - 1] + cols;
    for (unsigned row = 0; row < rows; ++row)
        for (unsigned col = 0; col < cols; ++col)
            result[row][col] = 0;
    return result;
}


void
polyfit::mat_dump(data_t **mat)
{
    for (unsigned row = 0; row < rows; row++)
    {
        for (unsigned col = 0; col < cols; col++)
            printf("%10.5f", (double)mat[row][col]);
        putchar('\n');
    }
    putchar('\n');
}


void
polyfit::mat_free(data_t **mat)
{
    if (mat)
    {
        assert(mat[0]);
        delete [] mat[0];
        delete [] mat;
    }
}


void
polyfit::print(void)
    const
{
    printf("y = ");
    for (unsigned j = 0; j < rows; ++j)
    {
        if (j)
            printf(" + ");
        printf("%.12g", (double)coefficients[j]);
        switch (j)
        {
        case 0:
            break;

        case 1:
            printf(" * x");
            break;

        default:
            printf(" * x**%d", j);
            break;
        }
    }
    printf("\n");
}