You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
116 lines
3.5 KiB
116 lines
3.5 KiB
#include <math.h>
|
|
|
|
#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 mallocs 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);
|
|
}
|