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