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

// Copyright (c) 1997-1999 Matra Datavision
// Copyright (c) 1999-2012 OPEN CASCADE SAS
//
// The content of this file is subject to the Open CASCADE Technology Public
// License Version 6.5 (the "License"). You may not use the content of this file
// except in compliance with the License. Please obtain a copy of the License
// at http://www.opencascade.org and read it completely before using this file.
//
// The Initial Developer of the Original Code is Open CASCADE S.A.S., having its
// main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France.
//
// The Original Code and all software distributed under the License is
// distributed on an "AS IS" basis, without warranty of any kind, and the
// Initial Developer hereby disclaims all such warranties, including without
// limitation, any warranties of merchantability, fitness for a particular
// purpose or non-infringement. Please see the License for the specific terms
// and conditions governing the rights and limitations under the License.

// 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 DEB
#define No_Standard_RangeError
#define No_Standard_OutOfRange
#define No_Standard_DimensionError
//#endif

#include <math.h>

#include <math_Recipes.hxx>

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

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

static Standard_Real at, bt, ct;
#define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
(ct=bt/at,at*sqrt(1.0+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(1.0+ct*ct)) : 0.0))

#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) { 

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

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

     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; j++) {
       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;
         if((dum = vv(i) * fabs(sum)) >= big) {
           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;
       }
     }
     return math_Status_OK;
}

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

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

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_Real Random2(Standard_Integer& idum) {

  static Standard_Integer iy, ir[98];
  static Standard_Integer iff = 0;
 
  Standard_Integer j;

  if(idum < 0 || iff == 0) {
    iff = 1;
    if((idum = (IC - idum) % M) < 0) idum = -idum;
    for(j = 1; j <= 97; j++) {
      idum = (IA * idum + IC) % M;
      ir[j] = idum;
    }
    idum = (IA * idum + IC) % M;
    iy = idum;
  }
  j = (Standard_Integer)(1 + 97.0 * iy / M);
  if(j > 97 || j < 1) Standard_Failure::Raise();
  iy = ir[j];
  idum = (IA * idum + IC) % M;
  ir[j] = idum;
  return Standard_Real(iy) / Standard_Real(M);
}


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;
     Standard_Boolean diag;

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