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