0022904: Clean up sccsid variables
[occt.git] / src / math / math_Recipes.cxx
CommitLineData
7fd59977 1// This preprocessor directive is a kludge to get around
2// a bug in the Sun Workshop 5.0 compiler, it keeps the
3// /usr/include/memory.h file from being #included
4// with an incompatible extern "C" definition of memchr
5// October 18, 2000 <rboehne@ricardo-us.com>
6#if __SUNPRO_CC == 0x500
7#define _MEMORY_H
8#endif
9
10//#ifndef DEB
11#define No_Standard_RangeError
12#define No_Standard_OutOfRange
13#define No_Standard_DimensionError
14//#endif
15
16#include <math.h>
17
18#include <math_Recipes.hxx>
19
20#include <Standard_Failure.hxx>
21#include <Standard_NotImplemented.hxx>
22
23#include <math_Vector.hxx>
24#include <math_IntegerVector.hxx>
25#include <math_Matrix.hxx>
26
27static Standard_Real at, bt, ct;
28#define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
29(ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)) : 0.0))
30
31#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
32
33
34#define ROTATE(a,i,j,k,l) g=a(i,j);\
35 h=a(k,l);\
36 a(i,j)=g-s*(h+g*tau);\
37 a(k,l)=h+s*(g-h*tau);
38
39#define M 714025
40#define IA 1366
41#define IC 150889
42
43static void EigenSort(math_Vector& d, math_Matrix& v) { // descending order
44
45 Standard_Integer k, i, j;
46 Standard_Real p;
47 Standard_Integer n = d.Length();
48
49 for(i = 1; i < n; i++) {
50 p = d(k = i);
51 for(j = i + 1; j <= n; j++)
52 if(d(j) >= p) p = d(k = j);
53 if(k != i) {
54 d(k) = d(i);
55 d(i) = p;
56 for(j = 1; j <= n; j++) {
57 p = v(j, i);
58 v(j, i) = v(j, k);
59 v(j, k) = p;
60 }
61 }
62 }
63 }
64
65Standard_Integer Jacobi(math_Matrix& a, math_Vector& d, math_Matrix& v, Standard_Integer& nrot) {
66
67 Standard_Integer n = a.RowNumber();
68 Standard_Integer j, iq, ip, i;
69 Standard_Real tresh, theta, tau, t, sm, s, h, g, c;
70 math_Vector b(1, n);
71 math_Vector z(1, n);
72
73 for(ip = 1; ip <= n; ip++) {
74 for(iq = 1; iq <= n; iq++)
75 v(ip, iq) = 0.0;
76 v(ip, ip) = 1.0;
77 }
78 for(ip = 1; ip <= n; ip++) {
79 b(ip) = d(ip) = a(ip, ip);
80 z(ip) = 0.0;
81 }
82 nrot = 0;
83 for(i = 1; i <= 50; i++) {
84 sm = 0.0;
85 for(ip = 1; ip < n; ip++) {
86 for(iq = ip + 1; iq <= n; iq++)
87 sm += fabs(a(ip, iq));
88 }
89 if(sm == 0.0) {
90 EigenSort(d, v);
91 return math_Status_OK;
92 }
93 if(i < 4) {
94 tresh = 0.2 * sm / (n * n);
95 }
96 else {
97 tresh = 0.0;
98 }
99 for(ip = 1; ip < n; ip++) {
100 for(iq = ip + 1; iq <= n; iq++) {
101 g = 100.0 * fabs(a(ip, iq));
102 if(i > 4 &&
103 fabs(d(ip)) + g == fabs(d(ip)) &&
104 fabs(d(iq)) + g == fabs(d(iq))) a(ip, iq) = 0.0;
105 else if(fabs(a(ip, iq)) > tresh) {
106 h = d(iq) - d(ip);
107 if(fabs(h) + g == fabs(h))
108 t = a(ip, iq) / h;
109 else {
110 theta = 0.5 * h / a(ip, iq);
111 t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
112 if(theta < 0.0) t = -t;
113 }
114 c = 1.0 / sqrt(1 + t * t);
115 s = t * c;
116 tau = s / (1.0 + c);
117 h = t * a(ip, iq);
118 z(ip) -= h;
119 z(iq) += h;
120 d(ip) -= h;
121 d(iq) += h;
122 a(ip, iq) = 0.0;
123 for(j = 1; j < ip; j++) {
124 ROTATE(a, j, ip, j, iq)
125 }
126 for(j = ip + 1; j < iq; j++) {
127 ROTATE(a, ip, j, j, iq)
128 }
129 for(j = iq + 1; j <= n; j++) {
130 ROTATE(a, ip, j, iq, j)
131 }
132 for(j = 1; j <= n; j++) {
133 ROTATE(v, j, ip, j, iq)
134 }
135 nrot++;
136 }
137 }
138 }
139 for(ip = 1; ip <= n; ip++) {
140 b(ip) += z(ip);
141 d(ip) = b(ip);
142 z(ip) = 0.0;
143 }
144 }
145 EigenSort(d, v);
146 return math_Status_NoConvergence;
147}
148
149Standard_Integer LU_Decompose(math_Matrix& a,
150 math_IntegerVector& indx,
151 Standard_Real& d,
152 math_Vector& vv,
153 Standard_Real TINY) {
154
155 Standard_Integer i, imax=0, j, k;
156 Standard_Real big, dum, sum, temp;
157
158 Standard_Integer n = a.RowNumber();
159 d = 1.0;
160
161 for(i = 1; i <= n; i++) {
162 big = 0.0;
163 for (j = 1; j <= n; j++)
164 if((temp = fabs(a(i, j))) > big) big = temp;
165 if(big <= TINY) {
166 return math_Status_SingularMatrix;
167 }
168 vv(i) = 1.0 / big;
169 }
170 for(j = 1; j <= n; j++) {
171 for(i = 1; i < j; i++) {
172 sum = a(i,j);
173 for(k = 1; k < i; k++)
174 sum -= a(i,k) * a(k,j);
175 a(i,j) = sum;
176 }
177 big = 0.0;
178 for(i = j; i <= n; i++) {
179 sum = a(i,j);
180 for(k = 1; k < j; k++)
181 sum -= a(i,k) * a(k,j);
182 a(i,j) = sum;
183 if((dum = vv(i) * fabs(sum)) >= big) {
184 big = dum;
185 imax = i;
186 }
187 }
188 if(j != imax) {
189 for(k = 1; k <= n; k++) {
190 dum = a(imax,k);
191 a(imax,k) = a(j,k);
192 a(j,k) = dum;
193 }
194 d = -d;
195 vv(imax) = vv(j);
196 }
197 indx(j) = imax;
198 if(fabs(a(j, j)) <= TINY) {
199 return math_Status_SingularMatrix;
200 }
201 if(j != n) {
202 dum = 1.0 / (a(j,j));
203 for(i = j + 1; i <= n; i++)
204 a(i,j) *= dum;
205 }
206 }
207 return math_Status_OK;
208}
209
210Standard_Integer LU_Decompose(math_Matrix& a,
211 math_IntegerVector& indx,
212 Standard_Real& d,
213 Standard_Real TINY) {
214
215 math_Vector vv(1, a.RowNumber());
216 return LU_Decompose(a, indx, d, vv, TINY);
217}
218
219void LU_Solve(const math_Matrix& a,
220 const math_IntegerVector& indx,
221 math_Vector& b) {
222
223 Standard_Integer i, ii = 0, ip, j;
224 Standard_Real sum;
225
226 Standard_Integer n=a.RowNumber();
227 Standard_Integer nblow=b.Lower()-1;
228 for(i = 1; i <= n; i++) {
229 ip = indx(i);
230 sum = b(ip+nblow);
231 b(ip+nblow) = b(i+nblow);
232 if(ii)
233 for(j = ii; j < i; j++)
234 sum -= a(i,j) * b(j+nblow);
235 else if(sum) ii = i;
236 b(i+nblow) = sum;
237 }
238 for(i = n; i >= 1; i--) {
239 sum = b(i+nblow);
240 for(j = i + 1; j <= n; j++)
241 sum -= a(i,j) * b(j+nblow);
242 b(i+nblow) = sum / a(i,i);
243 }
244}
245
246Standard_Integer LU_Invert(math_Matrix& a) {
247
248 Standard_Integer n=a.RowNumber();
249 math_Matrix inv(1, n, 1, n);
250 math_Vector col(1, n);
251 math_IntegerVector indx(1, n);
252 Standard_Real d;
253 Standard_Integer i, j;
254
255 Standard_Integer Error = LU_Decompose(a, indx, d);
256 if(!Error) {
257 for(j = 1; j <= n; j++) {
258 for(i = 1; i <= n; i++)
259 col(i) = 0.0;
260 col(j) = 1.0;
261 LU_Solve(a, indx, col);
262 for(i = 1; i <= n; i++)
263 inv(i,j) = col(i);
264 }
265 for(j = 1; j <= n; j++) {
266 for(i = 1; i <= n; i++) {
267 a(i,j) = inv(i,j);
268 }
269 }
270 }
271
272 return Error;
273}
274
275Standard_Integer SVD_Decompose(math_Matrix& a,
276 math_Vector& w,
277 math_Matrix& v) {
278
279 math_Vector rv1(1, a.ColNumber());
280 return SVD_Decompose(a, w, v, rv1);
281 }
282
283
284Standard_Integer SVD_Decompose(math_Matrix& a,
285 math_Vector& w,
286 math_Matrix& v,
287 math_Vector& rv1) {
288
289 Standard_Integer flag, i, its, j, jj, k, l=0, nm=0;
290 Standard_Real ar, aw, aik, aki, c, f, h, s, x, y, z;
291 Standard_Real anorm = 0.0, g = 0.0, scale = 0.0;
292 Standard_Integer m = a.RowNumber();
293 Standard_Integer n = a.ColNumber();
294
295 for(i = 1; i <= n; i++) {
296 l = i + 1;
297 rv1(i) = scale * g;
298 g = s = scale = 0.0;
299 if(i <= m) {
300 for(k = i; k <= m; k++) {
301 aki = a(k,i);
302 if (aki > 0) scale += aki;
303 else scale -= aki;
304 }
305 if(scale) {
306 for(k = i; k <= m; k++) {
307 a(k,i) /= scale;
308 s += a(k,i) * a(k,i);
309 }
310 f = a(i,i);
311 g = -SIGN(sqrt(s), f);
312 h = f * g - s;
313 a(i,i) = f - g;
314 if(i != n) {
315 for(j = l; j <= n; j++) {
316 for(s = 0.0, k = i; k <= m; k++)
317 s += a(k,i) * a(k,j);
318 f = s / h;
319 for(k = i; k <= m; k++)
320 a(k,j) += f * a(k,i);
321 }
322 }
323 for(k = i; k <= m; k++)
324 a(k,i) *= scale;
325 }
326 }
327 w(i) = scale * g;
328 g = s = scale = 0.0;
329 if(i <= m && i != n) {
330 for(k = l; k <= n; k++) {
331 aik = a(i,k);
332 if (aik > 0) scale += aik;
333 else scale -= aik;
334 }
335 if(scale) {
336 for(k = l; k <= n; k++) {
337 a(i,k) /= scale;
338 s += a(i,k) * a(i,k);
339 }
340 f = a(i,l);
341 g = -SIGN(sqrt(s), f);
342 h = f * g - s;
343 a(i,l) = f - g;
344 for (k = l; k <= n; k++)
345 rv1(k) = a(i,k) / h;
346 if(i != m) {
347 for(j = l; j <=m; j++) {
348 for(s = 0.0, k = l; k <= n; k++)
349 s += a(j,k) * a(i,k);
350 for(k = l; k <=n; k++)
351 a(j,k) += s * rv1(k);
352 }
353 }
354 for (k = l; k <= n; k++)
355 a(i,k) *= scale;
356 }
357 }
358 aw = w(i);
359 if (aw < 0) aw = - aw;
360 ar = rv1(i);
361 if (ar > 0) ar = aw + ar;
362 else ar = aw - ar;
363 if (anorm < ar) anorm = ar;
364 }
365 for(i = n; i >= 1; i--) {
366 if(i < n) {
367 if(g) {
368 for(j = l; j <= n; j++)
369 v(j,i) = (a(i,j) / a(i,l)) / g;
370 for(j = l; j <= n; j++) {
371 for(s = 0.0, k = l; k <= n; k++)
372 s += a(i,k) * v(k,j);
373 for(k = l; k <= n; k++)
374 v(k,j) += s * v(k,i);
375 }
376 }
377 for(j = l; j <= n; j++)
378 v(i,j) = v(j,i) = 0.0;
379 }
380 v(i,i) = 1.0;
381 g = rv1(i);
382 l = i;
383 }
384 for(i = n; i >= 1; i--) {
385 l = i + 1;
386 g = w(i);
387 if(i < n) for(j = l; j <= n; j++)
388 a(i,j) = 0.0;
389 if(g) {
390 g = 1.0 / g;
391 if(i != n) {
392 for(j = l; j <= n; j++) {
393 for(s = 0.0, k = l; k <= m; k++)
394 s += a(k,i) * a(k,j);
395 f = (s / a(i,i)) * g;
396 for(k = i; k <= m; k++)
397 a(k,j) += f * a(k,i);
398 }
399 }
400 for(j = i; j <= m; j++)
401 a(j,i) *= g;
402 }
403 else {
404 for(j = i; j <= m; j++)
405 a(j,i) = 0.0;
406 }
407 ++a(i,i);
408 }
409 for(k = n; k >= 1; k--) {
410 for(its = 1; its <= 30; its++) {
411 flag = 1;
412 for(l = k; l >= 1; l--) {
413 nm = l - 1;
414 if(fabs(rv1(l)) + anorm == anorm) {
415 flag = 0;
416 break;
417 }
418 if(fabs(w(nm)) + anorm == anorm) break;
419 }
420 if(flag) {
421 c = 0.0;
422 s = 1.0;
423 for(i = l; i <= k; i++) {
424 f = s * rv1(i);
425 if(fabs(f) + anorm != anorm) {
426 g = w(i);
427 h = PYTHAG(f, g);
428 w(i) = h;
429 h = 1.0 / h;
430 c = g * h;
431 s = (-f * h);
432 for(j = 1; j <= m; j++) {
433 y = a(j,nm);
434 z = a(j,i);
435 a(j,nm) = y * c + z * s;
436 a(j,i) = z * c - y * s;
437 }
438 }
439 }
440 }
441 z = w(k);
442 if(l == k) {
443 if(z < 0.0) {
444 w(k) = -z;
445 for(j = 1; j <= n; j++)
446 v(j,k) = (-v(j,k));
447 }
448 break;
449 }
450 if(its == 30) {
451 return math_Status_NoConvergence;
452 }
453 x = w(l);
454 nm = k - 1;
455 y = w(nm);
456 g = rv1(nm);
457 h = rv1(k);
458 f = ((y - z) * (y + z) + (g - h) * (g + h)) / ( 2.0 * h * y);
459 g = PYTHAG(f, 1.0);
460 f = ((x - z) * (x + z) + h * ((y / ( f + SIGN(g, f))) - h)) / x;
461
462 c = s = 1.0;
463 for(j = l; j <= nm; j++) {
464 i = j + 1;
465 g = rv1(i);
466 y = w(i);
467 h = s * g;
468 g = c * g;
469 z = PYTHAG(f, h);
470 rv1(j) = z;
471 c = f / z;
472 s = h / z;
473 f = x * c + g * s;
474 g = g * c - x * s;
475 h = y * s;
476 y = y * c;
477 for(jj = 1; jj <= n; jj++) {
478 x = v(jj,j);
479 z = v(jj,i);
480 v(jj,j) = x * c + z * s;
481 v(jj,i) = z * c - x * s;
482 }
483 z = PYTHAG(f, h);
484 w(j) = z;
485 if(z) {
486 z = 1.0 / z;
487 c = f * z;
488 s = h * z;
489 }
490 f = (c * g) + (s * y);
491 x = (c * y) - (s * g);
492 for(jj = 1; jj <= m; jj++) {
493 y = a(jj,j);
494 z = a(jj,i);
495 a(jj,j) = y * c + z * s;
496 a(jj,i) = z * c - y * s;
497 }
498 }
499 rv1(l) = 0.0;
500 rv1(k) = f;
501 w(k) = x;
502 }
503 }
504 return math_Status_OK;
505}
506
507void SVD_Solve(const math_Matrix& u,
508 const math_Vector& w,
509 const math_Matrix& v,
510 const math_Vector& b,
511 math_Vector& x) {
512
513 Standard_Integer jj, j, i;
514 Standard_Real s;
515
516 Standard_Integer m = u.RowNumber();
517 Standard_Integer n = u.ColNumber();
518 math_Vector tmp(1, n);
519
520 for(j = 1; j <= n; j++) {
521 s = 0.0;
522 if(w(j)) {
523 for(i = 1; i <= m; i++)
524 s += u(i,j) * b(i);
525 s /= w(j);
526 }
527 tmp(j) = s;
528 }
529 for(j = 1; j <= n; j++) {
530 s = 0.0;
531 for(jj = 1; jj <= n; jj++)
532 s += v(j,jj) * tmp(jj);
533 x(j) = s;
534 }
535}
536
537Standard_Real Random2(Standard_Integer& idum) {
538
539 static Standard_Integer iy, ir[98];
540 static Standard_Integer iff = 0;
541
542 Standard_Integer j;
543
544 if(idum < 0 || iff == 0) {
545 iff = 1;
546 if((idum = (IC - idum) % M) < 0) idum = -idum;
547 for(j = 1; j <= 97; j++) {
548 idum = (IA * idum + IC) % M;
549 ir[j] = idum;
550 }
551 idum = (IA * idum + IC) % M;
552 iy = idum;
553 }
554 j = (Standard_Integer)(1 + 97.0 * iy / M);
555 if(j > 97 || j < 1) Standard_Failure::Raise();
556 iy = ir[j];
557 idum = (IA * idum + IC) % M;
558 ir[j] = idum;
559 return Standard_Real(iy) / Standard_Real(M);
560}
561
562
563
564Standard_Integer DACTCL_Decompose(math_Vector& a,
565 const math_IntegerVector& indx,
566 const Standard_Real MinPivot) {
567
568 Standard_Integer i, j, Neq = indx.Length();
569 Standard_Integer jr, jd, jh, is, ie, k, ir, id, ih, mh;
570 Standard_Integer idot, idot1, idot2;
571 Standard_Real aa, d, dot;
572 Standard_Boolean diag;
573
574 jr = 0;
575 for (j = 1; j <= Neq; j++) {
576 diag = Standard_False;
577 jd = indx(j);
578 jh = jd-jr;
579 is = j-jh+2;
580 if (jh-2 == 0) diag = Standard_True;
581 if (jh-2 > 0) {
582 ie = j-1;
583 k = jr+2;
584 id = indx(is-1);
585 // Reduction des coefficients non diagonaux:
586 // =========================================
587 for (i = is; i <= ie; i++) {
588 ir = id;
589 id = indx(i);
590 ih = id - ir - 1;
591 mh = i - is + 1;
592 if (ih > mh) ih = mh;
593 if (ih > 0.0) {
594 dot = 0.0;
595 idot1 = k-ih-1;
596 idot2 = id-ih-1;
597 for (idot = 1; idot <= ih; idot++) {
598 dot = dot +a(idot1+idot)*a(idot2+idot);
599 }
600 a(k) = a(k)-dot;
601 }
602 k++;
603 }
604 diag = Standard_True;
605 }
606
607 if (diag) {
608 // Reduction des coefficients diagonaux:
609 // =====================================
610 ir = jr+1;
611 ie = jd-1;
612 k = j-jd;
613 for (i = ir; i <= ie; i++) {
614 id = indx(k+i);
615 aa = a(id);
616 if (aa < 0) aa = - aa;
617 if (aa <= MinPivot)
618 return math_Status_SingularMatrix;
619 d = a(i);
620 a(i) = d/a(id);
621 a(jd) = a(jd)-d*a(i);
622 }
623
624 }
625 jr = jd;
626 }
627 return math_Status_OK;
628}
629
630
631Standard_Integer DACTCL_Solve(const math_Vector& a,
632 math_Vector& b,
633 const math_IntegerVector& indx,
634 const Standard_Real MinPivot) {
635
636 Standard_Integer i, j, Neq = indx.Length();
637 Standard_Integer jr, jd, jh, is, k, id;
638 Standard_Integer jh1, idot, idot1, idot2;
639 Standard_Real aa, d, dot;
640 Standard_Boolean diag;
641
642 jr = 0;
643 for (j = 1; j <= Neq; j++) {
644 diag = Standard_False;
645 jd = indx(j);
646 jh = jd-jr;
647 is = j-jh+2;
648
649 // Reduction du second membre:
650 // ===========================
651 dot = 0.0;
652 idot1 = jr;
653 idot2 = is-2;
654 jh1 = jh-1;
655 for (idot = 1; idot <= jh1; idot++) {
656 dot = dot + a(idot1+idot)*b(idot2+idot);
657 }
658 b(j) = b(j)-dot;
659
660 jr = jd;
661 }
662
663 // Division par les pivots diagonaux:
664 // ==================================
665 for (i = 1; i <= Neq; i++) {
666 id = indx(i);
667 aa = a(id);
668 if (aa < 0) aa = - aa;
669 if (aa <= MinPivot)
670 return math_Status_SingularMatrix;
671 b(i) = b(i)/a(id);
672 }
673
674
675 // Substitution arriere:
676 // =====================
677 jd = indx(Neq);
678 for (j = Neq-1; j > 0; j--) {
679 d = b(j+1);
680 jr = indx(j);
681 if (jd-jr > 1) {
682 is = j-jd+jr+2;
683 k = jr-is+1;
684 for (i = is; i <= j; i++) {
685 b(i) = b(i)-a(i+k)*d;
686 }
687 }
688 jd = jr;
689 }
690 return math_Status_OK;
691
692}
693