db38d6bf574a5aa60b17e6c6b8104a2234c7045a
[occt.git] / src / math / math_Recipes.cxx
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
27 static 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
43 static 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
65 Standard_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
149 Standard_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
210 Standard_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
219 void 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
246 Standard_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
275 Standard_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
284 Standard_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
507 void 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
537 Standard_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
564 Standard_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
631 Standard_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