1 // Copyright (c) 1997-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
4 // This file is part of Open CASCADE Technology software library.
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.
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
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
25 #define No_Standard_RangeError
26 #define No_Standard_OutOfRange
27 #define No_Standard_DimensionError
32 #include <math_Recipes.hxx>
34 #include <Standard_Failure.hxx>
35 #include <Standard_NotImplemented.hxx>
37 #include <math_Vector.hxx>
38 #include <math_IntegerVector.hxx>
39 #include <math_Matrix.hxx>
42 static inline Standard_Real PYTHAG (const Standard_Real a, const Standard_Real b)
44 Standard_Real at = fabs (a), bt = fabs (b), ct = 0.;
47 ct = at * sqrt (1.0 + ct * ct);
50 ct = bt * sqrt (1.0 + ct * ct);
56 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
59 #define ROTATE(a,i,j,k,l) g=a(i,j);\
61 a(i,j)=g-s*(h+g*tau);\
68 static void EigenSort(math_Vector& d, math_Matrix& v) { // descending order
70 Standard_Integer k, i, j;
72 Standard_Integer n = d.Length();
74 for(i = 1; i < n; i++) {
76 for(j = i + 1; j <= n; j++)
77 if(d(j) >= p) p = d(k = j);
81 for(j = 1; j <= n; j++) {
90 Standard_Integer Jacobi(math_Matrix& a, math_Vector& d, math_Matrix& v, Standard_Integer& nrot) {
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;
98 for(ip = 1; ip <= n; ip++) {
99 for(iq = 1; iq <= n; iq++)
103 for(ip = 1; ip <= n; ip++) {
104 b(ip) = d(ip) = a(ip, ip);
108 for(i = 1; i <= 50; i++) {
110 for(ip = 1; ip < n; ip++) {
111 for(iq = ip + 1; iq <= n; iq++)
112 sm += fabs(a(ip, iq));
116 return math_Status_OK;
119 tresh = 0.2 * sm / (n * n);
124 for(ip = 1; ip < n; ip++) {
125 for(iq = ip + 1; iq <= n; iq++) {
126 g = 100.0 * fabs(a(ip, iq));
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) {
132 if(fabs(h) + g == fabs(h))
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;
139 c = 1.0 / sqrt(1 + t * t);
148 for(j = 1; j < ip; j++) {
149 ROTATE(a, j, ip, j, iq)
151 for(j = ip + 1; j < iq; j++) {
152 ROTATE(a, ip, j, j, iq)
154 for(j = iq + 1; j <= n; j++) {
155 ROTATE(a, ip, j, iq, j)
157 for(j = 1; j <= n; j++) {
158 ROTATE(v, j, ip, j, iq)
164 for(ip = 1; ip <= n; ip++) {
171 return math_Status_NoConvergence;
174 Standard_Integer LU_Decompose(math_Matrix& a,
175 math_IntegerVector& indx,
178 Standard_Real TINY) {
180 Standard_Integer i, imax=0, j, k;
181 Standard_Real big, dum, sum, temp;
183 Standard_Integer n = a.RowNumber();
186 for(i = 1; i <= n; i++) {
188 for (j = 1; j <= n; j++)
189 if((temp = fabs(a(i, j))) > big) big = temp;
191 return math_Status_SingularMatrix;
195 for(j = 1; j <= n; j++) {
196 for(i = 1; i < j; i++) {
198 for(k = 1; k < i; k++)
199 sum -= a(i,k) * a(k,j);
203 for(i = j; i <= n; i++) {
205 for(k = 1; k < j; k++)
206 sum -= a(i,k) * a(k,j);
208 if((dum = vv(i) * fabs(sum)) >= big) {
214 for(k = 1; k <= n; k++) {
223 if(fabs(a(j, j)) <= TINY) {
224 return math_Status_SingularMatrix;
227 dum = 1.0 / (a(j,j));
228 for(i = j + 1; i <= n; i++)
232 return math_Status_OK;
235 Standard_Integer LU_Decompose(math_Matrix& a,
236 math_IntegerVector& indx,
238 Standard_Real TINY) {
240 math_Vector vv(1, a.RowNumber());
241 return LU_Decompose(a, indx, d, vv, TINY);
244 void LU_Solve(const math_Matrix& a,
245 const math_IntegerVector& indx,
248 Standard_Integer i, ii = 0, ip, j;
251 Standard_Integer n=a.RowNumber();
252 Standard_Integer nblow=b.Lower()-1;
253 for(i = 1; i <= n; i++) {
256 b(ip+nblow) = b(i+nblow);
258 for(j = ii; j < i; j++)
259 sum -= a(i,j) * b(j+nblow);
263 for(i = n; i >= 1; i--) {
265 for(j = i + 1; j <= n; j++)
266 sum -= a(i,j) * b(j+nblow);
267 b(i+nblow) = sum / a(i,i);
271 Standard_Integer LU_Invert(math_Matrix& a) {
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);
278 Standard_Integer i, j;
280 Standard_Integer Error = LU_Decompose(a, indx, d);
282 for(j = 1; j <= n; j++) {
283 for(i = 1; i <= n; i++)
286 LU_Solve(a, indx, col);
287 for(i = 1; i <= n; i++)
290 for(j = 1; j <= n; j++) {
291 for(i = 1; i <= n; i++) {
300 Standard_Integer SVD_Decompose(math_Matrix& a,
304 math_Vector rv1(1, a.ColNumber());
305 return SVD_Decompose(a, w, v, rv1);
309 Standard_Integer SVD_Decompose(math_Matrix& a,
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();
320 for(i = 1; i <= n; i++) {
325 for(k = i; k <= m; k++) {
327 if (aki > 0) scale += aki;
331 for(k = i; k <= m; k++) {
333 s += a(k,i) * a(k,i);
336 g = -SIGN(sqrt(s), f);
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);
344 for(k = i; k <= m; k++)
345 a(k,j) += f * a(k,i);
348 for(k = i; k <= m; k++)
354 if(i <= m && i != n) {
355 for(k = l; k <= n; k++) {
357 if (aik > 0) scale += aik;
361 for(k = l; k <= n; k++) {
363 s += a(i,k) * a(i,k);
366 g = -SIGN(sqrt(s), f);
369 for (k = l; k <= n; k++)
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);
379 for (k = l; k <= n; k++)
384 if (aw < 0) aw = - aw;
386 if (ar > 0) ar = aw + ar;
388 if (anorm < ar) anorm = ar;
390 for(i = n; i >= 1; i--) {
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);
402 for(j = l; j <= n; j++)
403 v(i,j) = v(j,i) = 0.0;
409 for(i = n; i >= 1; i--) {
412 if(i < n) for(j = l; j <= n; j++)
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);
425 for(j = i; j <= m; j++)
429 for(j = i; j <= m; j++)
434 for(k = n; k >= 1; k--) {
435 for(its = 1; its <= 30; its++) {
437 for(l = k; l >= 1; l--) {
439 if(fabs(rv1(l)) + anorm == anorm) {
443 if(fabs(w(nm)) + anorm == anorm) break;
448 for(i = l; i <= k; i++) {
450 if(fabs(f) + anorm != anorm) {
457 for(j = 1; j <= m; j++) {
460 a(j,nm) = y * c + z * s;
461 a(j,i) = z * c - y * s;
470 for(j = 1; j <= n; j++)
476 return math_Status_NoConvergence;
483 f = ((y - z) * (y + z) + (g - h) * (g + h)) / ( 2.0 * h * y);
485 f = ((x - z) * (x + z) + h * ((y / ( f + SIGN(g, f))) - h)) / x;
488 for(j = l; j <= nm; j++) {
502 for(jj = 1; jj <= n; jj++) {
505 v(jj,j) = x * c + z * s;
506 v(jj,i) = z * c - x * s;
515 f = (c * g) + (s * y);
516 x = (c * y) - (s * g);
517 for(jj = 1; jj <= m; jj++) {
520 a(jj,j) = y * c + z * s;
521 a(jj,i) = z * c - y * s;
529 return math_Status_OK;
532 void SVD_Solve(const math_Matrix& u,
533 const math_Vector& w,
534 const math_Matrix& v,
535 const math_Vector& b,
538 Standard_Integer jj, j, i;
541 Standard_Integer m = u.RowNumber();
542 Standard_Integer n = u.ColNumber();
543 math_Vector tmp(1, n);
545 for(j = 1; j <= n; j++) {
548 for(i = 1; i <= m; i++)
554 for(j = 1; j <= n; j++) {
556 for(jj = 1; jj <= n; jj++)
557 s += v(j,jj) * tmp(jj);
562 Standard_Real Random2(Standard_Integer& idum) {
564 static Standard_Integer iy, ir[98];
565 static Standard_Integer iff = 0;
569 if(idum < 0 || iff == 0) {
571 if((idum = (IC - idum) % M) < 0) idum = -idum;
572 for(j = 1; j <= 97; j++) {
573 idum = (IA * idum + IC) % M;
576 idum = (IA * idum + IC) % M;
579 j = (Standard_Integer)(1 + 97.0 * iy / M);
580 if(j > 97 || j < 1) Standard_Failure::Raise();
582 idum = (IA * idum + IC) % M;
584 return Standard_Real(iy) / Standard_Real(M);
589 Standard_Integer DACTCL_Decompose(math_Vector& a,
590 const math_IntegerVector& indx,
591 const Standard_Real MinPivot) {
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;
600 for (j = 1; j <= Neq; j++) {
601 diag = Standard_False;
605 if (jh-2 == 0) diag = Standard_True;
610 // Reduction des coefficients non diagonaux:
611 // =========================================
612 for (i = is; i <= ie; i++) {
617 if (ih > mh) ih = mh;
622 for (idot = 1; idot <= ih; idot++) {
623 dot = dot +a(idot1+idot)*a(idot2+idot);
629 diag = Standard_True;
633 // Reduction des coefficients diagonaux:
634 // =====================================
638 for (i = ir; i <= ie; i++) {
641 if (aa < 0) aa = - aa;
643 return math_Status_SingularMatrix;
646 a(jd) = a(jd)-d*a(i);
652 return math_Status_OK;
656 Standard_Integer DACTCL_Solve(const math_Vector& a,
658 const math_IntegerVector& indx,
659 const Standard_Real MinPivot) {
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;
667 for (j = 1; j <= Neq; j++) {
672 // Reduction du second membre:
673 // ===========================
678 for (idot = 1; idot <= jh1; idot++) {
679 dot = dot + a(idot1+idot)*b(idot2+idot);
686 // Division par les pivots diagonaux:
687 // ==================================
688 for (i = 1; i <= Neq; i++) {
691 if (aa < 0) aa = - aa;
693 return math_Status_SingularMatrix;
698 // Substitution arriere:
699 // =====================
701 for (j = Neq-1; j > 0; j--) {
707 for (i = is; i <= j; i++) {
708 b(i) = b(i)-a(i+k)*d;
713 return math_Status_OK;