src/math/math_Recipes.cxx

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