1 /* gcvspl.f -- translated by f2c (version of 16 February 1991  0:35:15).
2    You must link the resulting object file with the libraries:
3     -lF77 -lI77 -lm -lc   (in that order)
4 */
5 
6 #include "SimTKcommon.h"
7 #include "simmath/internal/common.h"
8 
9 int SimTK_SIMMATH_EXPORT SimTK_gcvspl_(const SimTK_Real *, const SimTK_Real *, int *, const SimTK_Real *, const SimTK_Real *, int *, int *,
10             int *, int *, SimTK_Real *, SimTK_Real *, int *, SimTK_Real *, int *);
11 
12 SimTK_Real SimTK_SIMMATH_EXPORT SimTK_splder_(int *, int *, int *, SimTK_Real *, const SimTK_Real *, const SimTK_Real *, int *, SimTK_Real *, int);
13 
14 #define abs(x) ((x) >= 0 ? (x) : -(x))
15 #define min(a,b) ((a) <= (b) ? (a) : (b))
16 #define max(a,b) ((a) >= (b) ? (a) : (b))
17 
18 
19 /* Table of constant values */
20 
21 static SimTK_Real c_b6 = SimTK_Real(1e-15);
22 
SimTK_gcvspl_(const SimTK_Real * x,const SimTK_Real * y,int * ny,const SimTK_Real * wx,const SimTK_Real * wy,int * m,int * n,int * k,int * md,SimTK_Real * val,SimTK_Real * c,int * nc,SimTK_Real * wk,int * ier)23 int SimTK_gcvspl_(const SimTK_Real *x, const SimTK_Real *y, int *ny,
24     const SimTK_Real *wx, const SimTK_Real *wy, int *m, int *n, int *k,
25     int *md, SimTK_Real *val, SimTK_Real *c, int *nc, SimTK_Real *
26     wk, int *ier)
27 {
28 
29     int m2 = 0;
30     int nm1 = 0;
31     SimTK_Real el = 0.;
32 
33     /* System generated locals */
34     int y_dim1, y_offset, c_dim1, c_offset, i__1;
35 
36     /* Local variables */
37     int nm2m1, nm2p1;
38     extern SimTK_Real splc_(int *, int *, int *, const SimTK_Real *,
39         int *, const SimTK_Real *, const SimTK_Real *, int *, SimTK_Real *,
40         SimTK_Real *, SimTK_Real *, SimTK_Real *, int *, SimTK_Real *,
41          SimTK_Real *, SimTK_Real *, SimTK_Real *, SimTK_Real *);
42     extern /* Subroutine */ int prep_(int *, int *, const SimTK_Real *,
43         const SimTK_Real *, SimTK_Real *, SimTK_Real *);
44     int i, j;
45     SimTK_Real alpha;
46     extern /* Subroutine */ int basis_(int *, int *, const SimTK_Real *,
47         SimTK_Real *, SimTK_Real *, SimTK_Real *);
48     SimTK_Real r1, r2, r3, r4;
49     int ib;
50     SimTK_Real gf2, gf1, gf3, gf4;
51     int iwe;
52     SimTK_Real err;
53 
54 
55     /* Parameter adjustments */
56     --wk;
57 
58     c_dim1 = *nc;
59     c_offset = c_dim1 + 1;
60     c -= c_offset;
61 
62     --wy;
63     --wx;
64 
65     y_dim1 = *ny;
66     y_offset = y_dim1 + 1;
67     y -= y_offset;
68 
69     --x;
70 
71     /* Function Body */
72 
73 
74     *ier = 0;
75     if (abs(*md) > 4 ||
76         *md == 0 ||
77         ( abs(*md) == 1 && *val < 0. ) ||
78         ( abs(*md) == 3 && *val < 0. ) ||
79         ( abs(*md) == 4 && (*val < 0. || *val > (SimTK_Real) (*n - *m)))) {
80     *ier = 3;
81     return 0;
82     }
83     if (*md > 0) {
84     m2 = *m << 1;
85     nm1 = *n - 1;
86     } else {
87     if (m2 != *m << 1 || nm1 != *n - 1) {
88         *ier = 3;
89         return 0;
90     }
91     }
92     if (*m <= 0 || *n < m2) {
93     *ier = 1;
94     return 0;
95     }
96     if (wx[1] <= 0.) {
97     *ier = 2;
98     }
99     i__1 = *n;
100     for (i = 2; i <= i__1; ++i) {
101     if (wx[i] <= 0. || x[i - 1] >= x[i]) {
102         *ier = 2;
103     }
104     if (*ier != 0) {
105         return 0;
106     }
107     }
108     i__1 = *k;
109     for (j = 1; j <= i__1; ++j) {
110     if (wy[j] <= 0.) {
111         *ier = 2;
112     }
113     if (*ier != 0) {
114         return 0;
115     }
116     }
117 
118 
119     nm2p1 = *n * (m2 + 1);
120     nm2m1 = *n * (m2 - 1);
121     ib = nm2p1 + 7;
122     iwe = ib + nm2m1;
123 
124     if (*md > 0) {
125     basis_(m, n, &x[1], &wk[ib], &r1, &wk[7]);
126     prep_(m, n, &x[1], &wx[1], &wk[iwe], &el);
127     el /= r1;
128     }
129     if (abs(*md) != 1) {
130     goto L20;
131     }
132     r1 = *val;
133     goto L100;
134 
135 
136 L20:
137     if (*md < -1) {
138     r1 = wk[4];
139     } else {
140     r1 = 1 / el;
141     }
142     r2 = r1 * 2;
143     gf2 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r2, &
144         c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
145 L40:
146     gf1 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r1, &
147         c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
148     if (gf1 > gf2) {
149     goto L50;
150     }
151     if (wk[4] <= 0) {
152     goto L100;
153     }
154     r2 = r1;
155     gf2 = gf1;
156     r1 /= 2.;
157     goto L40;
158 L50:
159     r3 = r2 * 2;
160 L60:
161     gf3 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, &
162         c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
163     if (gf3 > gf2) {
164     goto L70;
165     }
166     if (wk[4] >= SimTK_Real(999999999999999.88)) {
167     goto L100;
168     }
169     r2 = r3;
170     gf2 = gf3;
171     r3 *= 2.;
172     goto L60;
173 L70:
174     r2 = r3;
175     gf2 = gf3;
176     alpha = (r2 - r1) / SimTK_Real(1.618033983);
177     r4 = r1 + alpha;
178     r3 = r2 - alpha;
179     gf3 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, &
180         c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
181     gf4 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r4, &
182         c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
183 L80:
184     if (gf3 <= gf4) {
185     r2 = r4;
186     gf2 = gf4;
187     err = (r2 - r1) / (r1 + r2);
188     if (err * err + 1 == 1 || err <= SimTK_Real(1e-6)) {
189         goto L90;
190     }
191     r4 = r3;
192     gf4 = gf3;
193     alpha /= SimTK_Real(1.618033983);
194     r3 = r2 - alpha;
195     gf3 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r3, &
196         c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]
197         );
198     } else {
199     r1 = r3;
200     gf1 = gf3;
201     err = (r2 - r1) / (r1 + r2);
202     if (err * err + 1 == 1 || err <= SimTK_Real(1e-6)) {
203         goto L90;
204     }
205     r3 = r4;
206     gf3 = gf4;
207     alpha /= SimTK_Real(1.618033983);
208     r4 = r1 + alpha;
209     gf4 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r4, &
210         c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]
211         );
212     }
213     goto L80;
214 L90:
215     r1 = (r1 + r2) / 2;
216 
217 
218 L100:
219     gf1 = splc_(m, n, k, &y[y_offset], ny, &wx[1], &wy[1], md, val, &r1, &
220         c_b6, &c[c_offset], nc, &wk[1], &wk[ib], &wk[iwe], &el, &wk[7]);
221 
222     return 0;
223 }
224 
225 
226 
227 
228 /* BASIS.FOR, 1985-06-03 */
229 
basis_(int * m,int * n,const SimTK_Real * x,SimTK_Real * b,SimTK_Real * bl,SimTK_Real * q)230 int basis_(int *m, int *n, const SimTK_Real *x, SimTK_Real
231     *b, SimTK_Real *bl, SimTK_Real *q)
232 {
233     /* System generated locals */
234     int b_dim1, b_offset, q_offset, i__1, i__2, i__3, i__4;
235     SimTK_Real d__1;
236 
237     /* Local variables */
238     int nmip1, i, j, k, l;
239     SimTK_Real u, v, y;
240     int j1, j2, m2, ir, mm1, mp1;
241     SimTK_Real arg;
242 
243 
244 
245     /* Parameter adjustments */
246     q_offset = 1 - *m;
247     q -= q_offset;
248 
249     b_dim1 = *m - 1 - (1 - *m) + 1;
250     b_offset = 1 - *m + b_dim1;
251     b -= b_offset;
252 
253     --x;
254 
255     if (*m == 1) {
256     i__1 = *n;
257     for (i = 1; i <= i__1; ++i) {
258         b[i * b_dim1] = 1.;
259     }
260     *bl = 1.;
261     return 0;
262     }
263 
264     mm1 = *m - 1;
265     mp1 = *m + 1;
266     m2 = *m << 1;
267     i__1 = *n;
268     for (l = 1; l <= i__1; ++l) {
269     i__2 = *m;
270     for (j = -mm1; j <= i__2; ++j) {
271         q[j] = 0.;
272     }
273     q[mm1] = 1.;
274     if (l != 1 && l != *n) {
275         q[mm1] = 1 / (x[l + 1] - x[l - 1]);
276     }
277     arg = x[l];
278     i__2 = m2;
279     for (i = 3; i <= i__2; ++i) {
280         ir = mp1 - i;
281         v = q[ir];
282         if (l < i) {
283         i__3 = i;
284         for (j = l + 1; j <= i__3; ++j) {
285             u = v;
286             v = q[ir + 1];
287             q[ir] = u + (x[j] - arg) * v;
288             ++ir;
289         }
290         }
291         i__3 = l - i + 1;
292         j1 = max(i__3,1);
293         i__3 = l - 1, i__4 = *n - i;
294         j2 = min(i__3,i__4);
295         if (j1 <= j2) {
296         if (i < m2) {
297             i__3 = j2;
298             for (j = j1; j <= i__3; ++j) {
299             y = x[i + j];
300             u = v;
301             v = q[ir + 1];
302             q[ir] = u + (v - u) * (y - arg) / (y - x[j]);
303             ++ir;
304             }
305         } else {
306             i__3 = j2;
307             for (j = j1; j <= i__3; ++j) {
308             u = v;
309             v = q[ir + 1];
310             q[ir] = (arg - x[j]) * u + (x[i + j] - arg) * v;
311             ++ir;
312             }
313         }
314         }
315         nmip1 = *n - i + 1;
316         if (nmip1 < l) {
317         i__3 = l - 1;
318         for (j = nmip1; j <= i__3; ++j) {
319             u = v;
320             v = q[ir + 1];
321             q[ir] = (arg - x[j]) * u + v;
322             ++ir;
323         }
324         }
325     }
326     i__2 = mm1;
327     for (j = -mm1; j <= i__2; ++j) {
328         b[j + l * b_dim1] = q[j];
329     }
330     }
331 
332     i__1 = mm1;
333     for (i = 1; i <= i__1; ++i) {
334     i__2 = mm1;
335     for (k = i; k <= i__2; ++k) {
336         b[-k + i * b_dim1] = 0.;
337         b[k + (*n + 1 - i) * b_dim1] = 0.;
338     }
339     }
340 
341     *bl = 0.;
342     i__1 = *n;
343     for (i = 1; i <= i__1; ++i) {
344     i__2 = mm1;
345     for (k = -mm1; k <= i__2; ++k) {
346         *bl += (d__1 = b[k + i * b_dim1], abs(d__1));
347     }
348     }
349     *bl /= *n;
350 
351     return 0;
352 }
353 
354 
355 
356 /* PREP.FOR, 1985-07-04 */
357 
358 
prep_(int * m,int * n,const SimTK_Real * x,const SimTK_Real * w,SimTK_Real * we,SimTK_Real * el)359 int prep_(int *m, int *n, const SimTK_Real *x, const SimTK_Real *
360     w, SimTK_Real *we, SimTK_Real *el)
361 {
362     /* System generated locals */
363     int i__1, i__2, i__3;
364     SimTK_Real d__1;
365 
366     /* Local variables */
367     SimTK_Real f;
368     int i, j, k, l;
369     SimTK_Real y, f1;
370     int i1, i2, m2;
371     SimTK_Real ff;
372     int jj, jm, kl, nm, ku;
373     SimTK_Real wi;
374     int n2m, mp1, i2m1, inc, i1p1, m2m1, m2p1;
375 
376 
377 
378 /* WE(-M:M,N) */
379     /* Parameter adjustments */
380     --we;
381     --w;
382     --x;
383 
384     /* Function Body */
385     m2 = *m << 1;
386     mp1 = *m + 1;
387     m2m1 = m2 - 1;
388     m2p1 = m2 + 1;
389     nm = *n - *m;
390     f1 = -1.;
391     if (*m != 1) {
392     i__1 = *m;
393     for (i = 2; i <= i__1; ++i) {
394         f1 = -f1 * i;
395     }
396     i__1 = m2m1;
397     for (i = mp1; i <= i__1; ++i) {
398         f1 *= i;
399     }
400     }
401 
402     i1 = 1;
403     i2 = *m;
404     jm = mp1;
405     i__1 = *n;
406     for (j = 1; j <= i__1; ++j) {
407     inc = m2p1;
408     if (j > nm) {
409         f1 = -f1;
410         f = f1;
411     } else {
412         if (j < mp1) {
413         inc = 1;
414         f = f1;
415         } else {
416         f = f1 * (x[j + *m] - x[j - *m]);
417         }
418     }
419     if (j > mp1) {
420         ++i1;
421     }
422     if (i2 < *n) {
423         ++i2;
424     }
425     jj = jm;
426     ff = f;
427     y = x[i1];
428     i1p1 = i1 + 1;
429     i__2 = i2;
430     for (i = i1p1; i <= i__2; ++i) {
431         ff /= y - x[i];
432     }
433     we[jj] = ff;
434     jj += m2;
435     i2m1 = i2 - 1;
436     if (i1p1 <= i2m1) {
437         i__2 = i2m1;
438         for (l = i1p1; l <= i__2; ++l) {
439         ff = f;
440         y = x[l];
441         i__3 = l - 1;
442         for (i = i1; i <= i__3; ++i) {
443             ff /= y - x[i];
444         }
445         i__3 = i2;
446         for (i = l + 1; i <= i__3; ++i) {
447             ff /= y - x[i];
448         }
449         we[jj] = ff;
450         jj += m2;
451         }
452     }
453     ff = f;
454     y = x[i2];
455     i__2 = i2m1;
456     for (i = i1; i <= i__2; ++i) {
457         ff /= y - x[i];
458     }
459     we[jj] = ff;
460     jj += m2;
461     jm += inc;
462     }
463 
464     kl = 1;
465     n2m = m2p1 * *n + 1;
466     i__1 = *m;
467     for (i = 1; i <= i__1; ++i) {
468     ku = kl + *m - i;
469     i__2 = ku;
470     for (k = kl; k <= i__2; ++k) {
471         we[k] = 0.;
472         we[n2m - k] = 0.;
473     }
474     kl += m2p1;
475     }
476 
477     jj = 0;
478     *el = 0.;
479     i__1 = *n;
480     for (i = 1; i <= i__1; ++i) {
481     wi = w[i];
482     i__2 = m2p1;
483     for (j = 1; j <= i__2; ++j) {
484         ++jj;
485         we[jj] /= wi;
486         *el += (d__1 = we[jj], abs(d__1));
487     }
488     }
489     *el /= *n;
490 
491     return 0;
492 }
493 
494 
495 
496 /* SPLC.FOR, 1985-12-12 */
497 
splc_(int * m,int * n,int * k,const SimTK_Real * y,int * ny,const SimTK_Real * wx,const SimTK_Real * wy,int * mode,SimTK_Real * val,SimTK_Real * p,SimTK_Real * eps,SimTK_Real * c,int * nc,SimTK_Real * stat,SimTK_Real * b,SimTK_Real * we,SimTK_Real * el,SimTK_Real * bwe)498 SimTK_Real splc_(int *m, int *n, int *k, const SimTK_Real *y, int *
499     ny, const SimTK_Real *wx, const SimTK_Real *wy, int *mode, SimTK_Real *val,
500     SimTK_Real *p, SimTK_Real *eps, SimTK_Real *c, int *nc,
501     SimTK_Real *stat, SimTK_Real *b, SimTK_Real *we, SimTK_Real *el,
502     SimTK_Real *bwe)
503 {
504     /* System generated locals */
505     int y_dim1, y_offset, c_dim1, c_offset, b_dim1, b_offset, we_dim1,
506         we_offset, bwe_dim1, bwe_offset, i__1, i__2, i__3, i__4;
507     SimTK_Real ret_val, d__1;
508 
509     /* Local variables */
510     int i, j, l;
511     extern SimTK_Real trinv_(SimTK_Real *, SimTK_Real *, int *, int *)
512         ;
513     SimTK_Real dp;
514     int km;
515     SimTK_Real dt;
516     int kp;
517     extern /* Subroutine */ int bandet_(SimTK_Real *, int *, int *),
518         bansol_(SimTK_Real *, const SimTK_Real *, int *, SimTK_Real *,
519         int *, int *, int *, int *);
520     SimTK_Real pel, esn, trn;
521 
522 
523 
524 /* ***  Check on p-value */
525 
526     /* Parameter adjustments */
527     bwe_dim1 = *m - (-(*m)) + 1;
528     bwe_offset = -(*m) + bwe_dim1;
529     bwe -= bwe_offset;
530 
531     we_dim1 = *m - (-(*m)) + 1;
532     we_offset = -(*m) + we_dim1;
533     we -= we_offset;
534 
535     b_dim1 = *m - 1 - (1 - *m) + 1;
536     b_offset = 1 - *m + b_dim1;
537     b -= b_offset;
538 
539     --stat;
540 
541     c_dim1 = *nc;
542     c_offset = c_dim1 + 1;
543     c -= c_offset;
544 
545     --wy;
546     --wx;
547 
548     y_dim1 = *ny;
549     y_offset = y_dim1 + 1;
550     y -= y_offset;
551 
552     /* Function Body */
553     dp = *p;
554     stat[4] = *p;
555     pel = *p * *el;
556     if (pel < *eps) {
557     dp = *eps / *el;
558     stat[4] = 0.;
559     }
560     if (pel * *eps > 1.) {
561     dp = 1 / (*el * *eps);
562     stat[4] = dp;
563     }
564 
565     i__1 = *n;
566     for (i = 1; i <= i__1; ++i) {
567     i__2 = *m, i__3 = i - 1;
568     km = -min(i__2,i__3);
569     i__2 = *m, i__3 = *n - i;
570     kp = min(i__2,i__3);
571     i__2 = kp;
572     for (l = km; l <= i__2; ++l) {
573         if (abs(l) == *m) {
574         bwe[l + i * bwe_dim1] = dp * we[l + i * we_dim1];
575         } else {
576         bwe[l + i * bwe_dim1] = b[l + i * b_dim1] + dp * we[l + i *
577             we_dim1];
578         }
579     }
580     }
581 
582     bandet_(&bwe[bwe_offset], m, n);
583     bansol_(&bwe[bwe_offset], &y[y_offset], ny, &c[c_offset], nc, m, n, k);
584     stat[3] = trinv_(&we[we_offset], &bwe[bwe_offset], m, n) * dp;
585     trn = stat[3] / *n;
586 
587     esn = 0.;
588     i__1 = *k;
589     for (j = 1; j <= i__1; ++j) {
590     i__2 = *n;
591     for (i = 1; i <= i__2; ++i) {
592         dt = -y[i + j * y_dim1];
593         i__3 = *m - 1, i__4 = i - 1;
594         km = -min(i__3,i__4);
595         i__3 = *m - 1, i__4 = *n - i;
596         kp = min(i__3,i__4);
597         i__3 = kp;
598         for (l = km; l <= i__3; ++l) {
599         dt += b[l + i * b_dim1] * c[i + l + j * c_dim1];
600         }
601         esn += dt * dt * wx[i] * wy[j];
602     }
603     }
604     esn /= *n * *k;
605 
606     stat[6] = esn / trn;
607     stat[1] = stat[6] / trn;
608     stat[2] = esn;
609     if (abs(*mode) != 3) {
610     stat[5] = stat[6] - esn;
611     if (abs(*mode) == 1) {
612         ret_val = 0.;
613     }
614     if (abs(*mode) == 2) {
615         ret_val = stat[1];
616     }
617     if (abs(*mode) == 4) {
618         ret_val = (d__1 = stat[3] - *val, abs(d__1));
619     }
620     } else {
621     stat[5] = esn - *val * (trn * 2 - 1);
622     ret_val = stat[5];
623     }
624 
625     return ret_val;
626 }
627 
628 
629 
630 /* BANDET.FOR, 1985-06-03 */
631 
bandet_(SimTK_Real * e,int * m,int * n)632 int bandet_(SimTK_Real *e, int *m, int *n)
633 {
634     /* System generated locals */
635     int e_dim1, e_offset, i__1, i__2, i__3, i__4;
636 
637     /* Local variables */
638     int i, k, l;
639     SimTK_Real di, dl;
640     int mi, km, lm;
641     SimTK_Real du;
642 
643 
644 
645     /* Parameter adjustments */
646 
647     e_dim1 = *m - (-(*m)) + 1;
648     e_offset = -(*m) + e_dim1;
649     e -= e_offset;
650 
651     /* Function Body */
652     if (*m <= 0) {
653     return 0;
654     }
655     i__1 = *n;
656     for (i = 1; i <= i__1; ++i) {
657     di = e[i * e_dim1];
658     i__2 = *m, i__3 = i - 1;
659     mi = min(i__2,i__3);
660     if (mi >= 1) {
661         i__2 = mi;
662         for (k = 1; k <= i__2; ++k) {
663         di -= e[-k + i * e_dim1] * e[k + (i - k) * e_dim1];
664         }
665         e[i * e_dim1] = di;
666     }
667     i__2 = *m, i__3 = *n - i;
668     lm = min(i__2,i__3);
669     if (lm >= 1) {
670         i__2 = lm;
671         for (l = 1; l <= i__2; ++l) {
672         dl = e[-l + (i + l) * e_dim1];
673         i__3 = *m - l, i__4 = i - 1;
674         km = min(i__3,i__4);
675         if (km >= 1) {
676             du = e[l + i * e_dim1];
677             i__3 = km;
678             for (k = 1; k <= i__3; ++k) {
679             du -= e[-k + i * e_dim1] * e[l + k + (i - k) * e_dim1]
680                 ;
681             dl -= e[-l - k + (l + i) * e_dim1] * e[k + (i - k) *
682                 e_dim1];
683             }
684             e[l + i * e_dim1] = du;
685         }
686         e[-l + (i + l) * e_dim1] = dl / di;
687         }
688     }
689     }
690 
691     return 0;
692 }
693 
694 
695 
696 /* BANSOL.FOR, 1985-12-12 */
697 
bansol_(SimTK_Real * e,const SimTK_Real * y,int * ny,SimTK_Real * c,int * nc,int * m,int * n,int * k)698 int bansol_(SimTK_Real *e, const SimTK_Real *y, int *ny,
699     SimTK_Real *c, int *nc, int *m, int *n, int *k)
700 {
701     /* System generated locals */
702     int e_dim1, e_offset, y_dim1, y_offset, c_dim1, c_offset, i__1, i__2,
703         i__3, i__4;
704 
705     /* Local variables */
706     SimTK_Real d;
707     int i, j, l, mi, nm1;
708 
709 
710 
711 /* ***  Check on special cases: M=0, M=1, M>1 */
712 
713     /* Parameter adjustments */
714 
715     c_dim1 = *nc;
716     c_offset = c_dim1 + 1;
717     c -= c_offset;
718 
719     y_dim1 = *ny;
720     y_offset = y_dim1 + 1;
721     y -= y_offset;
722 
723     e_dim1 = *m - (-(*m)) + 1;
724     e_offset = -(*m) + e_dim1;
725     e -= e_offset;
726 
727     /* Function Body */
728     nm1 = *n - 1;
729     if ((i__1 = *m - 1) < 0) {
730     goto L10;
731     } else if (i__1 == 0) {
732     goto L40;
733     } else {
734     goto L80;
735     }
736 
737 L10:
738     i__1 = *n;
739     for (i = 1; i <= i__1; ++i) {
740     i__2 = *k;
741     for (j = 1; j <= i__2; ++j) {
742         c[i + j * c_dim1] = y[i + j * y_dim1] / e[i * e_dim1];
743     }
744     }
745     return 0;
746 
747 L40:
748     i__1 = *k;
749     for (j = 1; j <= i__1; ++j) {
750     c[j * c_dim1 + 1] = y[j * y_dim1 + 1];
751     i__2 = *n;
752     for (i = 2; i <= i__2; ++i) {
753         c[i + j * c_dim1] = y[i + j * y_dim1] - e[i * e_dim1 - 1] * c[i -
754             1 + j * c_dim1];
755     }
756     c[*n + j * c_dim1] /= e[*n * e_dim1];
757     for (i = nm1; i >= 1; --i) {
758         c[i + j * c_dim1] = (c[i + j * c_dim1] - e[i * e_dim1 + 1] * c[i
759             + 1 + j * c_dim1]) / e[i * e_dim1];
760     }
761     }
762     return 0;
763 
764 
765 L80:
766     i__1 = *k;
767     for (j = 1; j <= i__1; ++j) {
768     c[j * c_dim1 + 1] = y[j * y_dim1 + 1];
769     i__2 = *n;
770     for (i = 2; i <= i__2; ++i) {
771         i__3 = *m, i__4 = i - 1;
772         mi = min(i__3,i__4);
773         d = y[i + j * y_dim1];
774         i__3 = mi;
775         for (l = 1; l <= i__3; ++l) {
776         d -= e[-l + i * e_dim1] * c[i - l + j * c_dim1];
777         }
778         c[i + j * c_dim1] = d;
779     }
780     c[*n + j * c_dim1] /= e[*n * e_dim1];
781     for (i = nm1; i >= 1; --i) {
782         i__2 = *m, i__3 = *n - i;
783         mi = min(i__2,i__3);
784         d = c[i + j * c_dim1];
785         i__2 = mi;
786         for (l = 1; l <= i__2; ++l) {
787         d -= e[l + i * e_dim1] * c[i + l + j * c_dim1];
788         }
789         c[i + j * c_dim1] = d / e[i * e_dim1];
790     }
791     }
792     return 0;
793 }
794 
795 
796 
797 /* TRINV.FOR, 1985-06-03 */
798 
799 
trinv_(SimTK_Real * b,SimTK_Real * e,int * m,int * n)800 SimTK_Real trinv_(SimTK_Real *b, SimTK_Real *e, int *m, int *n)
801 {
802     /* System generated locals */
803     int b_dim1, b_offset, e_dim1, e_offset, i__1, i__2, i__3;
804     SimTK_Real ret_val;
805 
806     /* Local variables */
807     int i, j, k;
808     SimTK_Real dd, dl;
809     int mi;
810     SimTK_Real du;
811     int mn, mp;
812 
813 
814 
815 /* ***  Assess central 2*M+1 bands of E**-1 and store in array E */
816 
817     /* Parameter adjustments */
818 
819     e_dim1 = *m - (-(*m)) + 1;
820     e_offset = -(*m) + e_dim1;
821     e -= e_offset;
822 
823     b_dim1 = *m - (-(*m)) + 1;
824     b_offset = -(*m) + b_dim1;
825     b -= b_offset;
826 
827     /* Function Body */
828     e[*n * e_dim1] = 1 / e[*n * e_dim1];
829     for (i = *n - 1; i >= 1; --i) {
830     i__1 = *m, i__2 = *n - i;
831     mi = min(i__1,i__2);
832     dd = 1 / e[i * e_dim1];
833     i__1 = mi;
834     for (k = 1; k <= i__1; ++k) {
835         e[k + *n * e_dim1] = e[k + i * e_dim1] * dd;
836         e[-k + e_dim1] = e[-k + (k + i) * e_dim1];
837     }
838     dd += dd;
839     for (j = mi; j >= 1; --j) {
840         du = 0.;
841         dl = 0.;
842         i__1 = mi;
843         for (k = 1; k <= i__1; ++k) {
844         du -= e[k + *n * e_dim1] * e[j - k + (i + k) * e_dim1];
845         dl -= e[-k + e_dim1] * e[k - j + (i + j) * e_dim1];
846         }
847         e[j + i * e_dim1] = du;
848         e[-j + (j + i) * e_dim1] = dl;
849         dd -= e[j + *n * e_dim1] * dl + e[-j + e_dim1] * du;
850     }
851     e[i * e_dim1] = dd / 2;
852     }
853 
854     dd = 0.;
855     i__1 = *n;
856     for (i = 1; i <= i__1; ++i) {
857     i__2 = *m, i__3 = i - 1;
858     mn = -min(i__2,i__3);
859     i__2 = *m, i__3 = *n - i;
860     mp = min(i__2,i__3);
861     i__2 = mp;
862     for (k = mn; k <= i__2; ++k) {
863         dd += b[k + i * b_dim1] * e[-k + (k + i) * e_dim1];
864     }
865     }
866     ret_val = dd;
867     i__1 = *m;
868     for (k = 1; k <= i__1; ++k) {
869     e[k + *n * e_dim1] = 0.;
870     e[-k + e_dim1] = 0.;
871     }
872     return ret_val;
873 }
874 
875 
876 
877 /* SPLDER.FOR, 1985-06-11 */
878 
879 
SimTK_splder_(int * ider,int * m,int * n,SimTK_Real * t,const SimTK_Real * x,const SimTK_Real * c,int * l,SimTK_Real * q,int coffset)880 SimTK_Real SimTK_splder_(int *ider, int *m, int *n, SimTK_Real *t,
881     const SimTK_Real *x, const SimTK_Real *c, int *l, SimTK_Real *q, int coffset)
882 {
883     /* System generated locals */
884     int i__1, i__2;
885     SimTK_Real ret_val;
886 
887     /* Local variables */
888     int lk1i1;
889     SimTK_Real xjki;
890     int i, j, k;
891     SimTK_Real z;
892     int i1, j1, k1, j2, m2, ii, jj, ki, jl, lk, mi, nk, lm, ml, jm,
893          ir, ju;
894     extern /* Subroutine */ int search_(int *, const SimTK_Real *, SimTK_Real *,
895          int *);
896     SimTK_Real tt;
897     int lk1, mp1, m2m1, jin, nki, npm, lk1i, nki1;
898 
899 
900 
901 /* ***  Derivatives of IDER.ge.2*M are alway zero */
902 
903     /* Parameter adjustments */
904 
905     --q;
906     c -= coffset;
907     --x;
908 
909     /* Function Body */
910     m2 = *m << 1;
911     k = m2 - *ider;
912     if (k < 1) {
913     ret_val = 0.;
914     return ret_val;
915     }
916 
917     search_(n, &x[1], t, l);
918 
919     tt = *t;
920     mp1 = *m + 1;
921     npm = *n + *m;
922     m2m1 = m2 - 1;
923     k1 = k - 1;
924     nk = *n - k;
925     lk = *l - k;
926     lk1 = lk + 1;
927     lm = *l - *m;
928     jl = *l + 1;
929     ju = *l + m2;
930     ii = *n - m2;
931     ml = -(*l);
932     i__1 = ju;
933     for (j = jl; j <= i__1; ++j) {
934     if (j >= mp1 && j <= npm) {
935         q[j + ml] = c[coffset*(j - *m)];
936     } else {
937         q[j + ml] = 0.;
938     }
939     }
940 
941     if (*ider > 0) {
942     jl -= m2;
943     ml += m2;
944     i__1 = *ider;
945     for (i = 1; i <= i__1; ++i) {
946         ++jl;
947         ++ii;
948         j1 = max(1,jl);
949         j2 = min(*l,ii);
950         mi = m2 - i;
951         j = j2 + 1;
952         if (j1 <= j2) {
953         i__2 = j2;
954         for (jin = j1; jin <= i__2; ++jin) {
955             --j;
956             jm = ml + j;
957             q[jm] = (q[jm] - q[jm - 1]) / (x[j + mi] - x[j]);
958         }
959         }
960         if (jl >= 1) {
961         goto L6;
962         }
963         i1 = i + 1;
964         j = ml + 1;
965         if (i1 <= ml) {
966         i__2 = ml;
967         for (jin = i1; jin <= i__2; ++jin) {
968             --j;
969             q[j] = -q[j - 1];
970         }
971         }
972 L6:
973         ;
974     }
975     i__1 = k;
976     for (j = 1; j <= i__1; ++j) {
977         q[j] = q[j + *ider];
978     }
979     }
980 
981     if (k1 >= 1) {
982     i__1 = k1;
983     for (i = 1; i <= i__1; ++i) {
984         nki = nk + i;
985         ir = k;
986         jj = *l;
987         ki = k - i;
988         nki1 = nki + 1;
989         if (*l >= nki1) {
990         i__2 = *l;
991         for (j = nki1; j <= i__2; ++j) {
992             q[ir] = q[ir - 1] + (tt - x[jj]) * q[ir];
993             --jj;
994             --ir;
995         }
996         }
997         lk1i = lk1 + i;
998         j1 = max(1,lk1i);
999         j2 = min(*l,nki);
1000         if (j1 <= j2) {
1001         i__2 = j2;
1002         for (j = j1; j <= i__2; ++j) {
1003             xjki = x[jj + ki];
1004             z = q[ir];
1005             q[ir] = z + (xjki - tt) * (q[ir - 1] - z) / (xjki - x[jj])
1006                 ;
1007             --ir;
1008             --jj;
1009         }
1010         }
1011         if (lk1i <= 0) {
1012         jj = ki;
1013         lk1i1 = 1 - lk1i;
1014         i__2 = lk1i1;
1015         for (j = 1; j <= i__2; ++j) {
1016             q[ir] += (x[jj] - tt) * q[ir - 1];
1017             --jj;
1018             --ir;
1019         }
1020         }
1021     }
1022     }
1023 
1024     z = q[k];
1025     if (*ider > 0) {
1026     i__1 = m2m1;
1027     for (j = k; j <= i__1; ++j) {
1028         z *= j;
1029     }
1030     }
1031     ret_val = z;
1032     return ret_val;
1033 }
1034 
1035 
1036 
1037 /* SEARCH.FOR, 1985-06-03 */
1038 
1039 
search_(int * n,const SimTK_Real * x,SimTK_Real * t,int * l)1040 int search_(int *n, const SimTK_Real *x, SimTK_Real *t,
1041     int *l)
1042 {
1043     int il, iu;
1044 
1045     /* Parameter adjustments */
1046 
1047     --x;
1048 
1049     /* Function Body */
1050 
1051     if (*t < x[1]) {
1052     *l = 0;
1053     return 0;
1054     }
1055     if (*t >= x[*n]) {
1056     *l = *n;
1057     return 0;
1058     }
1059     *l = max(*l,1);
1060     if (*l >= *n) {
1061     *l = *n - 1;
1062     }
1063 
1064     if (*t >= x[*l]) {
1065     goto L5;
1066     }
1067     --(*l);
1068     if (*t >= x[*l]) {
1069     return 0;
1070     }
1071 
1072     il = 1;
1073 L3:
1074     iu = *l;
1075 L4:
1076     *l = (il + iu) / 2;
1077     if (iu - il <= 1) {
1078     return 0;
1079     }
1080     if (*t < x[*l]) {
1081     goto L3;
1082     }
1083     il = *l;
1084     goto L4;
1085 L5:
1086     if (*t < x[*l + 1]) {
1087     return 0;
1088     }
1089     ++(*l);
1090     if (*t < x[*l + 1]) {
1091     return 0;
1092     }
1093     il = *l + 1;
1094     iu = *n;
1095     goto L4;
1096 
1097 }
1098