[occt.git] / src / math / math_Recipes.cxx

// Copyright (c) 1997-1999 Matra Datavision
// Copyright (c) 1999-2014 OPEN CASCADE SAS
//
// This file is part of Open CASCADE Technology software library.
//
// This library is free software; you can redistribute it and/or modify it under
// the terms of the GNU Lesser General Public License version 2.1 as published
// by the Free Software Foundation, with special exception defined in the file
// OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT
// distribution for complete text of the license and disclaimer of any warranty.
//
// Alternatively, this file may be used under the terms of Open CASCADE
// commercial license or contractual agreement.

// This preprocessor directive is a kludge to get around
// a bug in the Sun Workshop 5.0 compiler, it keeps the
// /usr/include/memory.h file from being #included
// with an incompatible extern "C" definition of memchr
// October 18, 2000  <rboehne@ricardo-us.com>
#if __SUNPRO_CC == 0x500
#define	_MEMORY_H
#endif

//#ifndef OCCT_DEBUG
#define No_Standard_RangeError
#define No_Standard_OutOfRange
#define No_Standard_DimensionError
//#endif

#include <cmath>

#include <math_Recipes.hxx>

#include <Standard_Failure.hxx>
#include <Standard_NotImplemented.hxx>

#include <math_Vector.hxx>
#include <math_IntegerVector.hxx>
#include <math_Matrix.hxx>
#include <Message_ProgressScope.hxx>

namespace {
static inline Standard_Real PYTHAG (const Standard_Real a, const Standard_Real b)
{
  Standard_Real at = fabs (a), bt = fabs (b), ct = 0.;
  if (at > bt) {
    ct = bt / at;
    ct = at * sqrt (1.0 + ct * ct);
  } else if (bt) {
    ct = at / bt;
    ct = bt * sqrt (1.0 + ct * ct);
  }
  return ct;
}
}

#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))


#define ROTATE(a,i,j,k,l) g=a(i,j);\
                          h=a(k,l);\
                          a(i,j)=g-s*(h+g*tau);\
                          a(k,l)=h+s*(g-h*tau);

#define M 714025
#define IA 1366
#define IC 150889

static void EigenSort(math_Vector& d, math_Matrix& v) { // descending order

      Standard_Integer k, i, j;
      Standard_Real p;
      Standard_Integer n = d.Length();

      for(i = 1; i < n; i++) {
        p = d(k = i);
        for(j = i + 1; j <= n; j++)
          if(d(j) >= p) p = d(k = j);
        if(k != i) {
          d(k) = d(i);
          d(i) = p;
          for(j = 1; j <= n; j++) {
            p = v(j, i);
            v(j, i) = v(j, k);
            v(j, k) = p;
          }
	}
      }
   }

Standard_Integer Jacobi(math_Matrix& a, math_Vector& d, math_Matrix& v, Standard_Integer& nrot) {

      Standard_Integer n = a.RowNumber();
      Standard_Integer j, iq, ip, i;
      Standard_Real tresh, theta, tau, t, sm, s, h, g, c;
      math_Vector b(1, n);
      math_Vector z(1, n);

      for(ip = 1; ip <= n; ip++) {
        for(iq = 1; iq <= n; iq++)
	  v(ip, iq) = 0.0;
        v(ip, ip) = 1.0;
      }
      for(ip = 1; ip <= n; ip++) {
        b(ip) = d(ip) = a(ip, ip);
        z(ip) = 0.0;
      }
      nrot = 0;
      for(i = 1; i <= 50; i++) {
        sm = 0.0;
        for(ip = 1; ip < n; ip++) {
          for(iq = ip + 1; iq <= n; iq++)
            sm += fabs(a(ip, iq));
	}
        if(sm == 0.0) {
	  EigenSort(d, v);
	  return math_Status_OK;
	}
        if(i < 4) {
          tresh = 0.2 * sm / (n * n);
        }
        else {
          tresh = 0.0;
        }
        for(ip = 1; ip < n; ip++) {
          for(iq = ip + 1; iq <= n; iq++) {
            g = 100.0 * fabs(a(ip, iq));
            if(i > 4 && 
               fabs(d(ip)) + g == fabs(d(ip)) &&
               fabs(d(iq)) + g == fabs(d(iq))) a(ip, iq) = 0.0;
            else if(fabs(a(ip, iq)) > tresh) {
              h = d(iq) - d(ip);
              if(fabs(h) + g == fabs(h)) 
                t = a(ip, iq) / h;
              else {
                theta = 0.5 * h / a(ip, iq);
                t = 1.0 / (fabs(theta) + sqrt(1.0 + theta * theta));
                if(theta < 0.0) t = -t;
              }
              c = 1.0 / sqrt(1 + t * t);
              s = t * c;
              tau = s / (1.0 + c);
              h = t * a(ip, iq);
              z(ip) -= h;
              z(iq) += h;
              d(ip) -= h;
              d(iq) += h;
              a(ip, iq) = 0.0;
              for(j = 1; j < ip; j++) {
                ROTATE(a, j, ip, j, iq)
              }
              for(j = ip + 1; j < iq; j++) {
                ROTATE(a, ip, j, j, iq)
	      }
              for(j = iq + 1; j <= n; j++) {
                ROTATE(a, ip, j, iq, j)
              }
              for(j = 1; j <= n; j++) {
                ROTATE(v, j, ip, j, iq)
	      }
              nrot++;
            }
          }
        }
        for(ip = 1; ip <= n; ip++) {
          b(ip) += z(ip);
          d(ip) = b(ip);
          z(ip) = 0.0;
        }
      }
      EigenSort(d, v);
      return math_Status_NoConvergence;
}

Standard_Integer LU_Decompose(math_Matrix& a, 
                     math_IntegerVector& indx, 
                     Standard_Real&   d, 
                     math_Vector& vv,
                     Standard_Real    TINY,
                     const Message_ProgressRange& theProgress) { 

     Standard_Integer i, imax=0, j, k;
     Standard_Real big, dum, sum, temp;

     Standard_Integer n = a.RowNumber();
     d = 1.0;

     Message_ProgressScope aPS(theProgress, "math_Gauss LU_Decompose", n);

     for(i = 1; i <= n; i++) {
       big = 0.0;
       for (j = 1; j <= n; j++) 
         if((temp = fabs(a(i, j))) > big) big = temp;
       if(big <= TINY) { 
         return math_Status_SingularMatrix;
       }
       vv(i) = 1.0 / big;
     }

     for(j = 1; j <= n && aPS.More(); j++, aPS.Next()) {
       for(i = 1; i < j; i++) {
         sum = a(i,j);
         for(k = 1; k < i; k++)
	   sum -= a(i,k) * a(k,j);
         a(i,j) = sum;
       }
       big = 0.0;
       for(i = j; i <= n; i++) {
         sum = a(i,j);
         for(k = 1; k < j; k++) 
           sum -= a(i,k) * a(k,j);
         a(i,j) = sum;
         // Note that comparison is made so as to have imax updated even if argument is NAN, Inf or IND, see #25559
         if((dum = vv(i) * fabs(sum)) < big)
         {
           continue;
         }
         big = dum;
         imax = i;
       }
       if(j != imax) {
         for(k = 1; k <= n; k++) {
           dum = a(imax,k);
           a(imax,k) = a(j,k);
           a(j,k) = dum;
         }
         d = -d;
         vv(imax) = vv(j);
       }
       indx(j) = imax;
       if(fabs(a(j, j)) <= TINY) {
         return math_Status_SingularMatrix;
       }
       if(j != n) {
         dum = 1.0 / (a(j,j));
         for(i = j + 1; i <= n; i++)
	   a(i,j) *= dum;
       }
     }

     if (j <= n)
     {
        return math_Status_UserAborted;
     }

     return math_Status_OK;
}

Standard_Integer LU_Decompose(math_Matrix& a, 
                    math_IntegerVector& indx, 
                    Standard_Real&   d, 
                    Standard_Real    TINY,
                    const Message_ProgressRange& theProgress) { 

     math_Vector vv(1, a.RowNumber());
     return LU_Decompose(a, indx, d, vv, TINY, theProgress);
}

void LU_Solve(const math_Matrix& a,
              const math_IntegerVector& indx, 
              math_Vector& b) {

     Standard_Integer i, ii = 0, ip, j;
     Standard_Real sum;

     Standard_Integer n=a.RowNumber();
     Standard_Integer nblow=b.Lower()-1;
     for(i = 1; i <= n; i++) {
       ip = indx(i);
       sum = b(ip+nblow);
       b(ip+nblow) = b(i+nblow);
       if(ii) 
         for(j = ii; j < i; j++)
	   sum -= a(i,j) * b(j+nblow);
       else if(sum) ii = i;
       b(i+nblow) = sum;
     }
     for(i = n; i >= 1; i--) {
       sum = b(i+nblow);
       for(j = i + 1; j <= n; j++)
	 sum -= a(i,j) * b(j+nblow);
       b(i+nblow) = sum / a(i,i);
     }
}

Standard_Integer LU_Invert(math_Matrix& a) {

     Standard_Integer n=a.RowNumber();
     math_Matrix inv(1, n, 1, n);
     math_Vector col(1, n);
     math_IntegerVector indx(1, n);
     Standard_Real d;
     Standard_Integer i, j;

     Standard_Integer Error = LU_Decompose(a, indx, d);
     if(!Error) {
       for(j = 1; j <= n; j++) {
         for(i = 1; i <= n; i++)
	   col(i) = 0.0;
         col(j) = 1.0;
         LU_Solve(a, indx, col);
         for(i = 1; i <= n; i++)
	   inv(i,j) = col(i);
       }
       for(j = 1; j <= n; j++) {
         for(i = 1; i <= n; i++) {
           a(i,j) = inv(i,j);
         }
       }
     }
    
     return Error;
}

Standard_Integer SVD_Decompose(math_Matrix& a,
                     math_Vector& w,
                     math_Matrix& v) {

     math_Vector rv1(1, a.ColNumber());
     return SVD_Decompose(a, w, v, rv1);
   }


Standard_Integer SVD_Decompose(math_Matrix& a,
                     math_Vector& w,
                     math_Matrix& v,
                     math_Vector& rv1) {

     Standard_Integer flag, i, its, j, jj, k, l=0, nm=0;
     Standard_Real ar, aw, aik, aki, c, f, h, s, x, y, z;
     Standard_Real anorm = 0.0, g = 0.0, scale = 0.0;
     Standard_Integer m = a.RowNumber();
     Standard_Integer n = a.ColNumber();

     for(i = 1; i <= n; i++) {
       l = i + 1;
       rv1(i) = scale * g;
       g = s = scale = 0.0;
       if(i <= m) {
         for(k = i; k <= m; k++) {
	   aki = a(k,i);
	   if (aki > 0) scale += aki;
	   else         scale -= aki;
	 }
         if(scale) {
           for(k = i; k <= m; k++) {
             a(k,i) /= scale;
             s += a(k,i) * a(k,i);
           }
           f = a(i,i);
           g = -SIGN(sqrt(s), f);
           h = f * g - s;
           a(i,i) = f - g;
           if(i != n) {
             for(j = l; j <= n; j++) {
               for(s = 0.0, k = i; k <= m; k++)
		 s += a(k,i) * a(k,j);
               f = s / h;
               for(k = i; k <= m; k++)
		 a(k,j) += f * a(k,i);
             }
           }
           for(k = i; k <= m; k++)
	     a(k,i) *= scale;
         }
       }
       w(i) = scale * g;
       g = s = scale = 0.0;
       if(i <= m && i != n) {
         for(k = l; k <= n; k++) {
	   aik = a(i,k);
	   if (aik > 0) scale += aik;
	   else         scale -= aik;
	 }
         if(scale) {
           for(k = l; k <= n; k++) {
             a(i,k) /= scale;
             s += a(i,k) * a(i,k);
           } 
           f = a(i,l);
           g = -SIGN(sqrt(s), f);
           h = f * g - s;
           a(i,l) = f - g;
           for (k = l; k <= n; k++)
	     rv1(k) = a(i,k) / h;
           if(i != m) {
             for(j = l; j <=m; j++) {
                for(s = 0.0, k = l; k <= n; k++)
		  s += a(j,k) * a(i,k);
                for(k = l; k <=n; k++)
		  a(j,k) += s * rv1(k);
             }
           }
           for (k = l; k <= n; k++)
	     a(i,k) *= scale;
         }
       }
       aw = w(i);
       if (aw < 0) aw = - aw;
       ar = rv1(i);
       if (ar > 0) ar = aw + ar;
       else        ar = aw - ar;
       if (anorm < ar) anorm = ar;
     }
     for(i = n; i >= 1; i--) {
       if(i < n) {
         if(g) {
           for(j = l; j <= n; j++)
	     v(j,i) = (a(i,j) / a(i,l)) / g;
           for(j = l; j <= n; j++) {
             for(s = 0.0, k = l; k <= n; k++)
	       s += a(i,k) * v(k,j);
             for(k = l; k <= n; k++)
	       v(k,j) += s * v(k,i);
           }
         }
         for(j = l; j <= n; j++)
	   v(i,j) = v(j,i) = 0.0;
       } 
       v(i,i) = 1.0;
       g = rv1(i);
       l = i;
     }
     for(i = n; i >= 1; i--) {
       l = i + 1;
       g = w(i);
       if(i < n) for(j = l; j <= n; j++)
	 a(i,j) = 0.0;
       if(g) {
         g = 1.0 / g;
         if(i != n) {
           for(j = l; j <= n; j++) {
             for(s = 0.0, k = l; k <= m; k++)
	       s += a(k,i) * a(k,j);
             f = (s / a(i,i)) * g;
             for(k = i; k <= m; k++)
	       a(k,j) += f * a(k,i);
           }
         }
         for(j = i; j <= m; j++)
	   a(j,i) *= g;
       }
       else {
         for(j = i; j <= m; j++)
	   a(j,i) = 0.0;
       }
       ++a(i,i);
     }
     for(k = n; k >= 1; k--) {
       for(its = 1; its <= 30; its++) {
         flag = 1;
         for(l = k; l >= 1; l--) {
           nm = l - 1;
           if(fabs(rv1(l)) + anorm == anorm) {
             flag = 0;
             break;
           }
           if(fabs(w(nm)) + anorm == anorm) break;
         }
         if(flag) {
           c = 0.0;
           s = 1.0;
           for(i = l; i <= k; i++) {
             f = s * rv1(i);
             if(fabs(f) + anorm != anorm) {
               g = w(i);
               h = PYTHAG(f, g);
               w(i) = h;
               h = 1.0 / h;
               c = g * h;
               s = (-f * h);
               for(j = 1; j <= m; j++) {
                 y = a(j,nm);
                 z = a(j,i);
                 a(j,nm) = y * c + z * s;
                 a(j,i) = z * c - y * s;
               }
             }
           }
         }
         z = w(k);
         if(l == k) {
           if(z < 0.0) {
             w(k) = -z;
             for(j = 1; j <= n; j++)
	       v(j,k) = (-v(j,k));
           }
           break;
         }
         if(its == 30) {
           return math_Status_NoConvergence;
         }
         x = w(l);
         nm = k - 1;
         y = w(nm);
         g = rv1(nm);
         h = rv1(k);
         f = ((y - z) * (y + z) + (g - h) * (g + h)) / ( 2.0 * h * y);
         g = PYTHAG(f, 1.0);
         f = ((x - z) * (x + z) + h * ((y / ( f + SIGN(g, f))) - h)) / x;
         
         c = s = 1.0;
         for(j = l; j <= nm; j++) {
           i = j + 1;
           g = rv1(i);
           y = w(i);
           h = s * g;
           g = c * g;
           z = PYTHAG(f, h);
           rv1(j) = z;
           c = f / z;
           s = h / z;
           f = x * c + g * s;
           g = g * c - x * s;
           h = y * s;
           y = y * c;
           for(jj = 1; jj <= n; jj++) {
             x = v(jj,j);
             z = v(jj,i);
             v(jj,j) = x * c + z * s;
             v(jj,i) = z * c - x * s;
           }
           z = PYTHAG(f, h);
           w(j) = z;
           if(z) {
             z = 1.0 / z;
             c = f * z;
             s = h * z;
           }
           f = (c * g) + (s * y);
           x = (c * y) - (s * g);
           for(jj = 1; jj <= m; jj++) {
             y = a(jj,j);
             z = a(jj,i);
             a(jj,j) = y * c + z * s;
             a(jj,i) = z * c - y * s;
           }
         }
         rv1(l) = 0.0;
         rv1(k) = f;
         w(k) = x;
       }
     }
     return math_Status_OK;
}

void SVD_Solve(const math_Matrix& u,
               const math_Vector& w,
               const math_Matrix& v,
               const math_Vector& b,
               math_Vector& x) {

     Standard_Integer jj, j, i;
     Standard_Real s;

     Standard_Integer m = u.RowNumber();
     Standard_Integer n = u.ColNumber();
     math_Vector tmp(1, n);

     for(j = 1; j <= n; j++) {
       s = 0.0;
       if(w(j)) {
         for(i = 1; i <= m; i++)
	   s += u(i,j) * b(i);
         s /= w(j);
       }
       tmp(j) = s;
     }
     for(j = 1; j <= n; j++) {
       s = 0.0;
       for(jj = 1; jj <= n; jj++)
	 s += v(j,jj) * tmp(jj);
       x(j) = s;
     }
}  

Standard_Integer DACTCL_Decompose(math_Vector& a, 
			const math_IntegerVector& indx,
                        const Standard_Real MinPivot) {

     Standard_Integer i, j, Neq = indx.Length();
     Standard_Integer jr, jd, jh, is, ie, k, ir, id, ih, mh;
     Standard_Integer idot, idot1, idot2;
     Standard_Real aa, d, dot;
     Standard_Boolean diag;

     jr = 0;
     for (j = 1; j <= Neq; j++) {
       diag = Standard_False;
       jd = indx(j);
       jh = jd-jr;
       is = j-jh+2;
       if (jh-2 == 0) diag = Standard_True;
       if (jh-2 > 0) {
	 ie = j-1;
	 k = jr+2;
	 id = indx(is-1);
	 // Reduction des coefficients non diagonaux:
	 // =========================================
	 for (i = is; i <= ie; i++) {
	   ir = id;
	   id = indx(i);
	   ih = id - ir - 1;
	   mh = i  - is + 1;
	   if (ih > mh) ih = mh;
	   if (ih > 0.0) {
	     dot = 0.0;
	     idot1 = k-ih-1;
	     idot2 = id-ih-1;
	     for (idot = 1; idot <= ih; idot++) {
	       dot = dot +a(idot1+idot)*a(idot2+idot);
	     }
	     a(k) = a(k)-dot;
	   }
	   k++;
	 }
	 diag = Standard_True;
       }

       if (diag) {
	 // Reduction des coefficients diagonaux:
	 // =====================================
	 ir = jr+1;
	 ie = jd-1;
	 k = j-jd;
	 for (i = ir; i <= ie; i++) {
	   id = indx(k+i);
	   aa = a(id);
	   if (aa < 0) aa = - aa;
	   if (aa <= MinPivot) 
	     return math_Status_SingularMatrix;
	   d = a(i);
	   a(i) = d/a(id);
	   a(jd) = a(jd)-d*a(i);
	 }

       }
       jr = jd;
     }
     return math_Status_OK;
}


Standard_Integer DACTCL_Solve(const math_Vector& a, 
		    math_Vector& b,
		    const math_IntegerVector& indx,
                    const Standard_Real MinPivot) {

     Standard_Integer i, j, Neq = indx.Length();
     Standard_Integer jr, jd, jh, is, k, id;
     Standard_Integer jh1, idot, idot1, idot2;
     Standard_Real aa, d, dot;

     jr = 0;
     for (j = 1; j <= Neq; j++) {
       jd = indx(j);
       jh = jd-jr;
       is = j-jh+2;

       // Reduction du second membre:
       // ===========================
       dot = 0.0;
       idot1 = jr;
       idot2 = is-2;
       jh1 = jh-1;
       for (idot = 1; idot <= jh1; idot++) {
	 dot = dot + a(idot1+idot)*b(idot2+idot);
       }
       b(j) = b(j)-dot;
       
       jr = jd;
     }

     // Division par les pivots diagonaux:
     // ==================================
     for (i = 1; i <= Neq; i++) {
       id = indx(i);
       aa = a(id);
       if (aa < 0) aa = - aa;
       if (aa <= MinPivot) 
	 return math_Status_SingularMatrix;
       b(i) = b(i)/a(id);
     }

     
     // Substitution arriere:
     // =====================
     jd = indx(Neq);
     for (j = Neq-1; j > 0; j--) {
       d = b(j+1);
       jr = indx(j);
       if (jd-jr > 1) {
	 is = j-jd+jr+2;
	 k = jr-is+1;
	 for (i = is; i <= j; i++) {
	   b(i) = b(i)-a(i+k)*d;
	 }
       }
       jd = jr;
     }
     return math_Status_OK;

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