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