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