src/math/math_Recipes.cxx

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