1 /**********
2 Copyright 1990 Regents of the University of California. All rights reserved.
3 Author: 1985 Wayne A. Christopher, U. C. Berkeley CAD Group
4
5 Spice is covered now covered by the BSD Copyright:
6
7 Copyright (c) 1985-1991 The Regents of the University of California.
8 All rights reserved.
9
10 Permission is hereby granted, without written agreement and without license
11 or royalty fees, to use, copy, modify, and distribute this software and its
12 documentation for any purpose, provided that the above copyright notice and
13 the following two paragraphs appear in all copies of this software.
14
15 IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR
16 DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
17 OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF
18 CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
19
20 THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
21 INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
22 FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
23 "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO PROVIDE
24 MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
25 **********/
26
27 /*
28 * Polynomial interpolation code.
29 */
30 #include <config.h>
31
32 #include <stdio.h>
33 #include <stdlib.h>
34 #include <string.h>
35 #include <math.h>
36
37 #ifndef HAVE_BZERO
38 #define bcopy(a,b,c) memcpy((b),(a),(c))
39 #define bzero(a,b) memset((a),0,(b))
40 #define bcmp(a,b,c) memcmp((a),(b),(c))
41 #endif
42
43
44 #if 0
45 static void
46 printmat (char *name, double *mat, int m, int n)
47 {
48 int i, j;
49
50 printf ("\n\r=== Matrix: %s ===\n\r", name);
51 for (i = 0; i < m; i++)
52 {
53 printf (" | ");
54 for (j = 0; j < n; j++)
55 printf ("%G ", mat[i * n + j]);
56 printf ("|\n\r");
57 }
58 printf ("===\n\r");
59 return;
60 }
61 #endif
62
63 static double
ft_peval(double x,double * coeffs,int degree)64 ft_peval (double x, double *coeffs, int degree)
65 {
66 double y;
67 int i;
68
69 if (!coeffs)
70 return 0.0; /* XXX Should not happen */
71
72 y = coeffs[degree]; /* there are (degree+1) coeffs */
73
74 for (i = degree - 1; i >= 0; i--)
75 {
76 y *= x;
77 y += coeffs[i];
78 }
79
80 return y;
81 }
82
83
84 /* Returns the index of the last element that was calculated. oval is the
85 * value of the old scale at the end of the interval that is being interpolated
86 * from, and sign is 1 if the old scale was increasing, and -1 if it was
87 * decreasing.
88 */
89 static int
putinterval(double * poly,int degree,double * nvec,int last,double * nscale,int nlen,double oval,int sign)90 putinterval (double *poly, int degree, double *nvec, int last, double *nscale,
91 int nlen, double oval, int sign)
92 {
93 int end, i;
94
95 /* See how far we have to go. */
96 for (end = last + 1; end < nlen; end++)
97 if (nscale[end] * sign > oval * sign)
98 break;
99 end--;
100
101 for (i = last + 1; i <= end; i++)
102 nvec[i] = ft_peval (nscale[i], poly, degree);
103 return (end);
104 }
105
106
107 /* Takes n = (degree+1) doubles, and fills in result with the n coefficients
108 * of the polynomial that will fit them. It also takes a pointer to an
109 * array of n ^ 2 + n doubles to use for scratch -- we want to make this
110 * fast and avoid doing mallocs for each call.
111 */
112 static int
ft_polyfit(double * xdata,double * ydata,double * result,int degree,double * scratch)113 ft_polyfit (double *xdata, double *ydata, double *result, int degree,
114 double *scratch)
115 {
116 double *mat1 = scratch;
117 int l, k, j, i;
118 int n = degree + 1;
119 double *mat2 = scratch + n * n; /* XXX These guys are hacks! */
120 double d;
121
122 bzero ((char *) result, n * sizeof (double));
123 bzero ((char *) mat1, n * n * sizeof (double));
124 bcopy ((char *) ydata, (char *) mat2, n * sizeof (double));
125
126 /* Fill in the matrix with x^k for 0 <= k <= degree for each point */
127 l = 0;
128 for (i = 0; i < n; i++)
129 {
130 d = 1.0;
131 for (j = 0; j < n; j++)
132 {
133 mat1[l] = d;
134 d *= xdata[i];
135 l += 1;
136 }
137 }
138
139 /* Do Gauss-Jordan elimination on mat1. */
140 for (i = 0; i < n; i++)
141 {
142 int lindex;
143 double largest;
144 /* choose largest pivot */
145 for (j = i, largest = mat1[i * n + i], lindex = i; j < n; j++)
146 {
147 if (fabs (mat1[j * n + i]) > largest)
148 {
149 largest = fabs (mat1[j * n + i]);
150 lindex = j;
151 }
152 }
153 if (lindex != i)
154 {
155 /* swap rows i and lindex */
156 for (k = 0; k < n; k++)
157 {
158 d = mat1[i * n + k];
159 mat1[i * n + k] = mat1[lindex * n + k];
160 mat1[lindex * n + k] = d;
161 }
162 d = mat2[i];
163 mat2[i] = mat2[lindex];
164 mat2[lindex] = d;
165 }
166
167 /* Make sure we have a non-zero pivot. */
168 if (mat1[i * n + i] == 0.0)
169 {
170 /* this should be rotated. */
171 return (0);
172 }
173 for (j = i + 1; j < n; j++)
174 {
175 d = mat1[j * n + i] / mat1[i * n + i];
176 for (k = 0; k < n; k++)
177 mat1[j * n + k] -= d * mat1[i * n + k];
178 mat2[j] -= d * mat2[i];
179 }
180 }
181
182 for (i = n - 1; i > 0; i--)
183 for (j = i - 1; j >= 0; j--)
184 {
185 d = mat1[j * n + i] / mat1[i * n + i];
186 for (k = 0; k < n; k++)
187 mat1[j * n + k] -= d * mat1[i * n + k];
188 mat2[j] -= d * mat2[i];
189 }
190
191 /* Now write the stuff into the result vector. */
192 for (i = 0; i < n; i++)
193 {
194 result[i] = mat2[i] / mat1[i * n + i];
195 /* printf(stderr, "result[%d] = %G\n", i, result[i]); */
196 }
197
198 #define ABS_TOL 0.001
199 #define REL_TOL 0.001
200
201 /* Let's check and make sure the coefficients are ok. If they aren't,
202 * just return false. This is not the best way to do it.
203 */
204 for (i = 0; i < n; i++)
205 {
206 d = ft_peval (xdata[i], result, degree);
207 if (fabs (d - ydata[i]) > ABS_TOL)
208 {
209 /*
210 fprintf(stderr,
211 "Error: polyfit: x = %le, y = %le, int = %le\n",
212 xdata[i], ydata[i], d);
213 printmat("mat1", mat1, n, n);
214 printmat("mat2", mat2, n, 1);
215 */
216 return (0);
217 }
218 else if (fabs (d - ydata[i]) / (fabs (d) > ABS_TOL ? fabs (d) :
219 ABS_TOL) > REL_TOL)
220 {
221 /*
222 fprintf(stderr,
223 "Error: polyfit: x = %le, y = %le, int = %le\n",
224 xdata[i], ydata[i], d);
225 printmat("mat1", mat1, n, n);
226 printmat("mat2", mat2, n, 1);
227 */
228 return (0);
229 }
230 }
231
232 return (1);
233 }
234
235
236 /* Interpolate data from oscale to nscale. data is assumed to be olen long,
237 * ndata will be nlen long. Returns false if the scales are too strange
238 * to deal with. Note that we are guaranteed that either both scales are
239 * strictly increasing or both are strictly decreasing.
240 *
241 * Usage: return indicates success or failure
242 *
243 * data[] = old y
244 * oscale[] = old x
245 * olen = size of above array
246 *
247 * ndata[] = routine fills in with new y
248 * nscale[] = user fills in with new x
249 * nlen = user fills in with size of above array
250 *
251 * note that degree > 2 will result in bumpy curves if the derivatives
252 * are not smooth
253 */
254 int
ft_interpolate(double * data,double * ndata,double * oscale,int olen,double * nscale,int nlen,int degree)255 ft_interpolate (double *data, double *ndata, double *oscale, int olen,
256 double *nscale, int nlen, int degree)
257 {
258 double *result, *scratch, *xdata, *ydata;
259 int sign, lastone, i, l;
260 int rc;
261
262 if ((olen < 2) || (nlen < 2))
263 {
264 /* fprintf(stderr, "Error: lengths too small to interpolate.\n"); */
265 return (0);
266 }
267 if ((degree < 1) || (degree > olen))
268 {
269 /* fprintf(stderr, "Error: degree is %d, can't interpolate.\n",
270 degree); */
271 return (0);
272 }
273
274 if (oscale[1] < oscale[0])
275 sign = -1;
276 else
277 sign = 1;
278
279 scratch = (double *) malloc ((degree + 1) * (degree + 2) * sizeof (double));
280 result = (double *) malloc ((degree + 1) * sizeof (double));
281 xdata = (double *) malloc ((degree + 1) * sizeof (double));
282 ydata = (double *) malloc ((degree + 1) * sizeof (double));
283
284 /* Deal with the first degree pieces. */
285 bcopy ((char *) data, (char *) ydata, (degree + 1) * sizeof (double));
286 bcopy ((char *) oscale, (char *) xdata, (degree + 1) * sizeof (double));
287
288 while (!ft_polyfit (xdata, ydata, result, degree, scratch))
289 {
290 /* If it doesn't work this time, bump the interpolation
291 * degree down by one.
292 */
293
294 if (--degree == 0)
295 {
296 /* fprintf(stderr, "ft_interpolate: Internal Error.\n"); */
297 rc = 0;
298 goto bot;
299 }
300
301 }
302
303 /* Add this part of the curve. What we do is evaluate the polynomial
304 * at those points between the last one and the one that is greatest,
305 * without being greater than the leftmost old scale point, or least
306 * if the scale is decreasing at the end of the interval we are looking
307 * at.
308 */
309 lastone = -1;
310 for (i = 0; i < degree; i++)
311 {
312 lastone = putinterval (result, degree, ndata, lastone,
313 nscale, nlen, xdata[i], sign);
314 }
315
316 /* Now plot the rest, piece by piece. l is the
317 * last element under consideration.
318 */
319 for (l = degree + 1; l < olen; l++)
320 {
321
322 /* Shift the old stuff by one and get another value. */
323 for (i = 0; i < degree; i++)
324 {
325 xdata[i] = xdata[i + 1];
326 ydata[i] = ydata[i + 1];
327 }
328 ydata[i] = data[l];
329 xdata[i] = oscale[l];
330
331 while (!ft_polyfit (xdata, ydata, result, degree, scratch))
332 {
333 if (--degree == 0)
334 {
335 /* fprintf(stderr,
336 "interpolate: Internal Error.\n"); */
337 rc = 0;
338 goto bot;
339 }
340 }
341 lastone = putinterval (result, degree, ndata, lastone,
342 nscale, nlen, xdata[i], sign);
343 }
344 if (lastone < nlen - 1) /* ??? */
345 ndata[nlen - 1] = data[olen - 1];
346
347 rc = 1;
348
349 bot:
350 free (scratch);
351 free (xdata);
352 free (ydata);
353 free (result);
354 return (rc);
355 }
356
357