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 // Note that comparison is made so as to have imax updated even if argument is NAN, Inf or IND, see #25559
209 if((dum = vv(i) * fabs(sum)) < big)
217 for(k = 1; k <= n; k++) {
226 if(fabs(a(j, j)) <= TINY) {
227 return math_Status_SingularMatrix;
230 dum = 1.0 / (a(j,j));
231 for(i = j + 1; i <= n; i++)
235 return math_Status_OK;
238 Standard_Integer LU_Decompose(math_Matrix& a,
239 math_IntegerVector& indx,
241 Standard_Real TINY) {
243 math_Vector vv(1, a.RowNumber());
244 return LU_Decompose(a, indx, d, vv, TINY);
247 void LU_Solve(const math_Matrix& a,
248 const math_IntegerVector& indx,
251 Standard_Integer i, ii = 0, ip, j;
254 Standard_Integer n=a.RowNumber();
255 Standard_Integer nblow=b.Lower()-1;
256 for(i = 1; i <= n; i++) {
259 b(ip+nblow) = b(i+nblow);
261 for(j = ii; j < i; j++)
262 sum -= a(i,j) * b(j+nblow);
266 for(i = n; i >= 1; i--) {
268 for(j = i + 1; j <= n; j++)
269 sum -= a(i,j) * b(j+nblow);
270 b(i+nblow) = sum / a(i,i);
274 Standard_Integer LU_Invert(math_Matrix& a) {
276 Standard_Integer n=a.RowNumber();
277 math_Matrix inv(1, n, 1, n);
278 math_Vector col(1, n);
279 math_IntegerVector indx(1, n);
281 Standard_Integer i, j;
283 Standard_Integer Error = LU_Decompose(a, indx, d);
285 for(j = 1; j <= n; j++) {
286 for(i = 1; i <= n; i++)
289 LU_Solve(a, indx, col);
290 for(i = 1; i <= n; i++)
293 for(j = 1; j <= n; j++) {
294 for(i = 1; i <= n; i++) {
303 Standard_Integer SVD_Decompose(math_Matrix& a,
307 math_Vector rv1(1, a.ColNumber());
308 return SVD_Decompose(a, w, v, rv1);
312 Standard_Integer SVD_Decompose(math_Matrix& a,
317 Standard_Integer flag, i, its, j, jj, k, l=0, nm=0;
318 Standard_Real ar, aw, aik, aki, c, f, h, s, x, y, z;
319 Standard_Real anorm = 0.0, g = 0.0, scale = 0.0;
320 Standard_Integer m = a.RowNumber();
321 Standard_Integer n = a.ColNumber();
323 for(i = 1; i <= n; i++) {
328 for(k = i; k <= m; k++) {
330 if (aki > 0) scale += aki;
334 for(k = i; k <= m; k++) {
336 s += a(k,i) * a(k,i);
339 g = -SIGN(sqrt(s), f);
343 for(j = l; j <= n; j++) {
344 for(s = 0.0, k = i; k <= m; k++)
345 s += a(k,i) * a(k,j);
347 for(k = i; k <= m; k++)
348 a(k,j) += f * a(k,i);
351 for(k = i; k <= m; k++)
357 if(i <= m && i != n) {
358 for(k = l; k <= n; k++) {
360 if (aik > 0) scale += aik;
364 for(k = l; k <= n; k++) {
366 s += a(i,k) * a(i,k);
369 g = -SIGN(sqrt(s), f);
372 for (k = l; k <= n; k++)
375 for(j = l; j <=m; j++) {
376 for(s = 0.0, k = l; k <= n; k++)
377 s += a(j,k) * a(i,k);
378 for(k = l; k <=n; k++)
379 a(j,k) += s * rv1(k);
382 for (k = l; k <= n; k++)
387 if (aw < 0) aw = - aw;
389 if (ar > 0) ar = aw + ar;
391 if (anorm < ar) anorm = ar;
393 for(i = n; i >= 1; i--) {
396 for(j = l; j <= n; j++)
397 v(j,i) = (a(i,j) / a(i,l)) / g;
398 for(j = l; j <= n; j++) {
399 for(s = 0.0, k = l; k <= n; k++)
400 s += a(i,k) * v(k,j);
401 for(k = l; k <= n; k++)
402 v(k,j) += s * v(k,i);
405 for(j = l; j <= n; j++)
406 v(i,j) = v(j,i) = 0.0;
412 for(i = n; i >= 1; i--) {
415 if(i < n) for(j = l; j <= n; j++)
420 for(j = l; j <= n; j++) {
421 for(s = 0.0, k = l; k <= m; k++)
422 s += a(k,i) * a(k,j);
423 f = (s / a(i,i)) * g;
424 for(k = i; k <= m; k++)
425 a(k,j) += f * a(k,i);
428 for(j = i; j <= m; j++)
432 for(j = i; j <= m; j++)
437 for(k = n; k >= 1; k--) {
438 for(its = 1; its <= 30; its++) {
440 for(l = k; l >= 1; l--) {
442 if(fabs(rv1(l)) + anorm == anorm) {
446 if(fabs(w(nm)) + anorm == anorm) break;
451 for(i = l; i <= k; i++) {
453 if(fabs(f) + anorm != anorm) {
460 for(j = 1; j <= m; j++) {
463 a(j,nm) = y * c + z * s;
464 a(j,i) = z * c - y * s;
473 for(j = 1; j <= n; j++)
479 return math_Status_NoConvergence;
486 f = ((y - z) * (y + z) + (g - h) * (g + h)) / ( 2.0 * h * y);
488 f = ((x - z) * (x + z) + h * ((y / ( f + SIGN(g, f))) - h)) / x;
491 for(j = l; j <= nm; j++) {
505 for(jj = 1; jj <= n; jj++) {
508 v(jj,j) = x * c + z * s;
509 v(jj,i) = z * c - x * s;
518 f = (c * g) + (s * y);
519 x = (c * y) - (s * g);
520 for(jj = 1; jj <= m; jj++) {
523 a(jj,j) = y * c + z * s;
524 a(jj,i) = z * c - y * s;
532 return math_Status_OK;
535 void SVD_Solve(const math_Matrix& u,
536 const math_Vector& w,
537 const math_Matrix& v,
538 const math_Vector& b,
541 Standard_Integer jj, j, i;
544 Standard_Integer m = u.RowNumber();
545 Standard_Integer n = u.ColNumber();
546 math_Vector tmp(1, n);
548 for(j = 1; j <= n; j++) {
551 for(i = 1; i <= m; i++)
557 for(j = 1; j <= n; j++) {
559 for(jj = 1; jj <= n; jj++)
560 s += v(j,jj) * tmp(jj);
565 Standard_Integer DACTCL_Decompose(math_Vector& a,
566 const math_IntegerVector& indx,
567 const Standard_Real MinPivot) {
569 Standard_Integer i, j, Neq = indx.Length();
570 Standard_Integer jr, jd, jh, is, ie, k, ir, id, ih, mh;
571 Standard_Integer idot, idot1, idot2;
572 Standard_Real aa, d, dot;
573 Standard_Boolean diag;
576 for (j = 1; j <= Neq; j++) {
577 diag = Standard_False;
581 if (jh-2 == 0) diag = Standard_True;
586 // Reduction des coefficients non diagonaux:
587 // =========================================
588 for (i = is; i <= ie; i++) {
593 if (ih > mh) ih = mh;
598 for (idot = 1; idot <= ih; idot++) {
599 dot = dot +a(idot1+idot)*a(idot2+idot);
605 diag = Standard_True;
609 // Reduction des coefficients diagonaux:
610 // =====================================
614 for (i = ir; i <= ie; i++) {
617 if (aa < 0) aa = - aa;
619 return math_Status_SingularMatrix;
622 a(jd) = a(jd)-d*a(i);
628 return math_Status_OK;
632 Standard_Integer DACTCL_Solve(const math_Vector& a,
634 const math_IntegerVector& indx,
635 const Standard_Real MinPivot) {
637 Standard_Integer i, j, Neq = indx.Length();
638 Standard_Integer jr, jd, jh, is, k, id;
639 Standard_Integer jh1, idot, idot1, idot2;
640 Standard_Real aa, d, dot;
643 for (j = 1; j <= Neq; j++) {
648 // Reduction du second membre:
649 // ===========================
654 for (idot = 1; idot <= jh1; idot++) {
655 dot = dot + a(idot1+idot)*b(idot2+idot);
662 // Division par les pivots diagonaux:
663 // ==================================
664 for (i = 1; i <= Neq; i++) {
667 if (aa < 0) aa = - aa;
669 return math_Status_SingularMatrix;
674 // Substitution arriere:
675 // =====================
677 for (j = Neq-1; j > 0; j--) {
683 for (i = is; i <= j; i++) {
684 b(i) = b(i)-a(i+k)*d;
689 return math_Status_OK;