23 static int putinterval(
double*,
int,
double*,
int,
double*,
int,
double,
int);
24 static void printmat(
char*,
double*,
int,
int);
34 double *data, *ndata, *oscale, *nscale;
35 int olen, nlen, degree;
37 double *result, *scratch, *xdata, *ydata;
38 int sign, lastone, i,
l;
40 if ((olen < 2) || (nlen < 2)) {
41 fprintf(
cp_err,
"Error: lengths too small to interpolate.\n");
44 if ((degree < 1) || (degree > olen)) {
45 fprintf(
cp_err,
"Error: degree is %d, can't interpolate.\n",
50 if (oscale[1] < oscale[0])
55 scratch = (
double *)
tmalloc((degree + 1) * (degree + 2) *
57 result = (
double *)
tmalloc((degree + 1) *
sizeof (double));
58 xdata = (
double *)
tmalloc((degree + 1) *
sizeof (double));
59 ydata = (
double *)
tmalloc((degree + 1) *
sizeof (double));
62 DCOPY(data, ydata, degree + 1);
63 DCOPY(oscale, xdata, degree + 1);
65 while (!
ft_polyfit(xdata, ydata, result, degree, scratch)) {
71 fprintf(
cp_err,
"ft_interpolate: Internal Error.\n");
83 for (i = 0; i < degree; i++) {
84 lastone =
putinterval(result, degree, ndata, lastone,
85 nscale, nlen, xdata[i], sign);
91 for (l = degree + 1; l < olen; l++) {
94 for (i = 0; i < degree; i++) {
95 xdata[i] = xdata[i + 1];
96 ydata[i] = ydata[i + 1];
101 while (!
ft_polyfit(xdata, ydata, result, degree, scratch)) {
104 "interpolate: Internal Error.\n");
108 lastone =
putinterval(result, degree, ndata, lastone,
109 nscale, nlen, xdata[i], sign);
129 double *xdata, *ydata, *result, *scratch;
132 double *mat1 = scratch;
135 double *mat2 = scratch + n * n;
149 bzero((
char *) result, n *
sizeof(
double));
150 bzero((
char *) mat1, n * n *
sizeof (
double));
151 DCOPY(ydata, mat2, n);
155 for (i = 0; i < n; i++) {
157 for (j = 0; j < n; j++) {
165 for (i = 0; i < n; i++) {
170 for (j=i, largest = mat1[i * n + i], lindex = i; j < n; j++) {
171 if (fabs(mat1[j * n + i]) > largest) {
172 largest = fabs(mat1[j * n + i]);
178 for (k = 0; k < n; k++) {
180 mat1[i * n + k] = mat1[lindex * n + k];
181 mat1[lindex * n + k] = d;
184 mat2[i] = mat2[lindex];
188 if (mat1[i * n + i] == 0.0)
189 for (j = i; j < n; j++)
190 if (mat1[j * n + i] != 0.0) {
192 for (k = 0; k < n; k++) {
205 if (mat1[i * n + i] == 0.0) {
209 for (j = i + 1; j < n; j++) {
210 d = mat1[j * n + i] / mat1[i * n + i];
211 for (k = 0; k < n; k++)
212 mat1[j * n + k] -= d * mat1[i * n + k];
213 mat2[j] -= d * mat2[i];
217 for (i = n - 1; i > 0; i--)
218 for (j = i - 1; j >= 0; j--) {
219 d = mat1[j * n + i] / mat1[i * n + i];
220 for (k = 0; k < n; k++)
223 mat2[j] -= d * mat2[i];
227 for (i = 0; i < n; i++) {
228 result[i] = mat2[i] / mat1[i * n + i];
232 #define ABS_TOL 0.001 233 #define REL_TOL 0.001 238 for (i = 0; i < n; i++) {
239 d =
ft_peval(xdata[i], result, degree);
240 if (fabs(d - ydata[i]) >
ABS_TOL) {
250 else if (fabs(d - ydata[i]) / (fabs(d) >
ABS_TOL ? fabs(d) :
276 double *poly, *nvec, *nscale, oval;
277 int degree, last, nlen, sign;
282 for (end = last + 1; end < nlen; end++)
283 if (nscale[end] * sign > oval * sign)
287 for (i = last + 1; i <= end; i++)
288 nvec[i] =
ft_peval(nscale[i], poly, degree);
302 printf(
"\n\r=== Matrix: %s ===\n\r", name);
303 for (i = 0; i <
m; i++) {
305 for (j = 0; j < n; j++)
306 printf(
"%G ", mat[i * n + j]);
327 for (i = degree - 1; i >= 0; i--) {
int bzero(char *ptr, int num)
bool ft_interpolate(double *data, double *ndata, double *oscale, int olen, double *nscale, int nlen, int degree)
bool ft_polyfit(double *xdata, double *ydata, double *result, int degree, double *scratch)
double ft_peval(double x, double *coeffs, int degree)