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