polyfit.c

ngspice又一个电子CAD仿真软件代码.功能更全

#include <config.h>
#include <math.h>

#ifdef HAVE_STRING_H
#include <string.h>
#endif
#include "polyfit.h"
#include "polyeval.h"

/* Takes n = (degree+1) doubles, and fills in result with the n
 * coefficients of the polynomial that will fit them. It also takes a
 * pointer to an array of n ^ 2 + n doubles to use for scratch -- we
 * want to make this fast and avoid doing tmallocs for each call.  */
bool
ft_polyfit(double *xdata, double *ydata, double *result,
	   int degree, double *scratch)
{
    double *mat1 = scratch;
    int l, k, j, i;
    int n = degree + 1;
    double *mat2 = scratch + n * n; /* XXX These guys are hacks! */
    double d;

    memset((char *) result, 0, n * sizeof(double));
    memset((char *) mat1, 0, n * n * sizeof (double));
    memcpy((char *) mat2, (char *) ydata, n * sizeof (double));

    /* Fill in the matrix with x^k for 0 <= k <= degree for each point */
    l = 0;
    for (i = 0; i < n; i++) {
	d = 1.0;
        for (j = 0; j < n; j++) {
            mat1[l] = d;
	    d *= xdata[i];
            l += 1;
        }
    }

    /* Do Gauss-Jordan elimination on mat1. */
    for (i = 0; i < n; i++) {
      int lindex;
      double largest;
        /* choose largest pivot */
        for (j=i, largest = mat1[i * n + i], lindex = i; j < n; j++) {
          if (fabs(mat1[j * n + i]) > largest) {
            largest = fabs(mat1[j * n + i]);
            lindex = j;
          }
        }
        if (lindex != i) {
          /* swap rows i and lindex */
          for (k = 0; k < n; k++) {
            d = mat1[i * n + k];
            mat1[i * n + k] = mat1[lindex * n + k];
            mat1[lindex * n + k] = d;
          }
          d = mat2[i];
          mat2[i] = mat2[lindex];
          mat2[lindex] = d;
        }
        /* Make sure we have a non-zero pivot. */
        if (mat1[i * n + i] == 0.0) {
            /* this should be rotated. */
            return (FALSE);
        }
        for (j = i + 1; j < n; j++) {
            d = mat1[j * n + i] / mat1[i * n + i];
            for (k = 0; k < n; k++)
                mat1[j * n + k] -= d * mat1[i * n + k];
            mat2[j] -= d * mat2[i];
        }
    }

    for (i = n - 1; i > 0; i--)
        for (j = i - 1; j >= 0; j--) {
            d = mat1[j * n + i] / mat1[i * n + i];
            for (k = 0; k < n; k++)
                mat1[j * n + k] -= 
                        d * mat1[i * n + k];
            mat2[j] -= d * mat2[i];
        }
    
    /* Now write the stuff into the result vector. */
    for (i = 0; i < n; i++) {
        result[i] = mat2[i] / mat1[i * n + i];
        /* printf(cp_err, "result[%d] = %G\n", i, result[i]);*/
    }

#define ABS_TOL 0.001
#define REL_TOL 0.001

    /* Let's check and make sure the coefficients are ok.  If they aren't,
     * just return FALSE.  This is not the best way to do it.
     */
    for (i = 0; i < n; i++) {
        d = ft_peval(xdata[i], result, degree);
        if (fabs(d - ydata[i]) > ABS_TOL) {
            /*
            fprintf(cp_err,
                "Error: polyfit: x = %le, y = %le, int = %le\n",
                    xdata[i], ydata[i], d);
            printmat("mat1", mat1, n, n);
            printmat("mat2", mat2, n, 1);
            */
            return (FALSE);
        } else if (fabs(d - ydata[i]) / (fabs(d) > ABS_TOL ? fabs(d) :
                ABS_TOL) > REL_TOL) {
            /*
            fprintf(cp_err,
                "Error: polyfit: x = %le, y = %le, int = %le\n",
                    xdata[i], ydata[i], d);
            printmat("mat1", mat1, n, n);
            printmat("mat2", mat2, n, 1);
            */
            return (FALSE);
        }
    }

    return (TRUE);
}