hholder.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00008 
00009 // Copyright (C) 1991,2,3,4: R B Davies
00010 
00011 #define WANT_MATH
00012 //#define WANT_STREAM
00013 
00014 #include "include.h"
00015 
00016 #include "newmatap.h"
00017 
00018 #ifdef use_namespace
00019 namespace NEWMAT {
00020 #endif
00021 
00022 #ifdef DO_REPORT
00023 #define REPORT { static ExeCounter ExeCount(__LINE__,16); ++ExeCount; }
00024 #else
00025 #define REPORT {}
00026 #endif
00027 
00028 
00029 /*************************** QR decompositions ***************************/
00030 
00031 inline Real square(Real x) { return x*x; }
00032 
00033 void QRZT(Matrix& X, LowerTriangularMatrix& L)
00034 {
00035    REPORT
00036          Tracer et("QRZT(1)");
00037    int n = X.Ncols(); int s = X.Nrows(); L.resize(s);
00038    if (n == 0 || s == 0) { L = 0.0; return; }
00039    Real* xi = X.Store(); int k;
00040    for (int i=0; i<s; i++)
00041    {
00042       Real sum = 0.0;
00043       Real* xi0=xi; k=n; while(k--) { sum += square(*xi++); }
00044       sum = sqrt(sum);
00045       if (sum == 0.0)
00046       {
00047          REPORT
00048          k=n; while(k--) { *xi0++ = 0.0; }
00049          for (int j=i; j<s; j++) L.element(j,i) = 0.0;
00050       }
00051       else
00052       {
00053          L.element(i,i) = sum;
00054          Real* xj0=xi0; k=n; while(k--) { *xj0++ /= sum; }
00055          for (int j=i+1; j<s; j++)
00056          {
00057             sum=0.0;
00058             xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
00059             xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
00060             L.element(j,i) = sum;
00061          }
00062       }
00063    }
00064 }
00065 
00066 void QRZT(const Matrix& X, Matrix& Y, Matrix& M)
00067 {
00068    REPORT
00069    Tracer et("QRZT(2)");
00070    int n = X.Ncols(); int s = X.Nrows(); int t = Y.Nrows();
00071    if (Y.Ncols() != n)
00072       { Throw(ProgramException("Unequal row lengths",X,Y)); }
00073    M.resize(t,s);
00074    Real* xi = X.Store(); int k;
00075    for (int i=0; i<s; i++)
00076    {
00077       Real* xj0 = Y.Store(); Real* xi0 = xi;
00078       for (int j=0; j<t; j++)
00079       {
00080          Real sum=0.0;
00081          xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
00082          xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
00083          M.element(j,i) = sum;
00084       }
00085    }
00086 }
00087 
00088 /*
00089 void QRZ(Matrix& X, UpperTriangularMatrix& U)
00090 {
00091         Tracer et("QRZ(1)");
00092         int n = X.Nrows(); int s = X.Ncols(); U.resize(s);
00093         Real* xi0 = X.Store(); int k;
00094         for (int i=0; i<s; i++)
00095         {
00096                 Real sum = 0.0;
00097                 Real* xi = xi0; k=n; while(k--) { sum += square(*xi); xi+=s; }
00098                 sum = sqrt(sum);
00099                 U.element(i,i) = sum;
00100                 if (sum==0.0) Throw(SingularException(U));
00101                 Real* xj0=xi0; k=n; while(k--) { *xj0 /= sum; xj0+=s; }
00102                 xj0 = xi0;
00103                 for (int j=i+1; j<s; j++)
00104                 {
00105                         sum=0.0;
00106                         xi=xi0; k=n; xj0++; Real* xj=xj0;
00107                         while(k--) { sum += *xi * *xj; xi+=s; xj+=s; }
00108                         xi=xi0; k=n; xj=xj0;
00109                         while(k--) { *xj -= sum * *xi; xj+=s; xi+=s; }
00110                         U.element(i,j) = sum;
00111                 }
00112                 xi0++;
00113         }
00114 }
00115 */
00116 
00117 void QRZ(Matrix& X, UpperTriangularMatrix& U)
00118 {
00119    REPORT
00120    Tracer et("QRZ(1)");
00121    int n = X.Nrows(); int s = X.Ncols(); U.resize(s); U = 0.0;
00122    if (n == 0 || s == 0) return;
00123    Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
00124    int j, k; int J = s; int i = s;
00125    while (i--)
00126    {
00127       Real* xj0 = xi0; Real* xi = xi0; k = n;
00128       if (k) for (;;)
00129       {
00130          u = u0; Real Xi = *xi; Real* xj = xj0;
00131          j = J; while(j--) *u++ += Xi * *xj++;
00132          if (!(--k)) break;
00133          xi += s; xj0 += s;
00134       }
00135 
00136       Real sum = sqrt(*u0); *u0 = sum; u = u0+1;
00137       if (sum == 0.0)
00138       {
00139          REPORT
00140          j = J - 1; while(j--) *u++ = 0.0;
00141 
00142          xj0 = xi0++; k = n;
00143          if (k) for (;;)
00144          {
00145             *xj0 = 0.0;
00146             if (!(--k)) break;
00147                   xj0 += s;
00148          }
00149          u0 += J--;
00150       }
00151       else
00152       {
00153          int J1 = J-1; j = J1; while(j--) *u++ /= sum;
00154 
00155          xj0 = xi0; xi = xi0++; k = n;
00156          if (k) for (;;)
00157          {
00158             u = u0+1; Real Xi = *xi; Real* xj = xj0;
00159             Xi /= sum; *xj++ = Xi;
00160             j = J1; while(j--) *xj++ -= *u++ * Xi;
00161             if (!(--k)) break;
00162                   xi += s; xj0 += s;
00163          }
00164          u0 += J--;
00165       }
00166    }
00167 }
00168 
00169 void QRZ(const Matrix& X, Matrix& Y, Matrix& M)
00170 {
00171    REPORT
00172    Tracer et("QRZ(2)");
00173    int n = X.Nrows(); int s = X.Ncols(); int t = Y.Ncols();
00174    if (Y.Nrows() != n)
00175       { Throw(ProgramException("Unequal column lengths",X,Y)); }
00176    M.resize(s,t); M = 0;Real* m0 = M.Store(); Real* m;
00177    Real* xi0 = X.Store();
00178    int j, k; int i = s;
00179    while (i--)
00180    {
00181       Real* xj0 = Y.Store(); Real* xi = xi0; k = n;
00182       if (k) for (;;)
00183       {
00184          m = m0; Real Xi = *xi; Real* xj = xj0;
00185          j = t; while(j--) *m++ += Xi * *xj++;
00186          if (!(--k)) break;
00187          xi += s; xj0 += t;
00188       }
00189 
00190       xj0 = Y.Store(); xi = xi0++; k = n;
00191       if (k) for (;;)
00192       {
00193          m = m0; Real Xi = *xi; Real* xj = xj0;
00194          j = t; while(j--) *xj++ -= *m++ * Xi;
00195          if (!(--k)) break;
00196          xi += s; xj0 += t;
00197       }
00198       m0 += t;
00199    }
00200 }
00201 
00202 /*
00203 
00204 void QRZ(const Matrix& X, Matrix& Y, Matrix& M)
00205 {
00206         Tracer et("QRZ(2)");
00207         int n = X.Nrows(); int s = X.Ncols(); int t = Y.Ncols();
00208         if (Y.Nrows() != n)
00209         { Throw(ProgramException("Unequal column lengths",X,Y)); }
00210         M.resize(s,t);
00211         Real* xi0 = X.Store(); int k;
00212         for (int i=0; i<s; i++)
00213         {
00214                 Real* xj0 = Y.Store();
00215                 for (int j=0; j<t; j++)
00216                 {
00217                         Real sum=0.0;
00218                         Real* xi=xi0; Real* xj=xj0; k=n;
00219                         while(k--) { sum += *xi * *xj; xi+=s; xj+=t; }
00220                         xi=xi0; k=n; xj=xj0++;
00221                         while(k--) { *xj -= sum * *xi; xj+=t; xi+=s; }
00222                         M.element(i,j) = sum;
00223                 }
00224                 xi0++;
00225         }
00226 }
00227 */
00228 
00229 void updateQRZT(Matrix& X, LowerTriangularMatrix& L)
00230 {
00231    REPORT
00232          Tracer et("updateQRZT");
00233    int n = X.Ncols(); int s = X.Nrows();
00234    if (s != L.Nrows())
00235       Throw(ProgramException("Incompatible dimensions",X,L)); 
00236    if (n == 0 || s == 0) return;
00237    Real* xi = X.Store(); int k;
00238    for (int i=0; i<s; i++)
00239    {
00240       Real r = L.element(i,i); 
00241       Real sum = 0.0;
00242       Real* xi0=xi; k=n; while(k--) { sum += square(*xi++); }
00243       sum = sqrt(sum + square(r));
00244       if (sum == 0.0)
00245       {
00246          REPORT
00247          k=n; while(k--) { *xi0++ = 0.0; }
00248          for (int j=i; j<s; j++) L.element(j,i) = 0.0;
00249       }
00250       else
00251       {
00252          Real frs = fabs(r) + sum;
00253          Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
00254          if (r <= 0) { REPORT L.element(i,i) = sum; alpha = -alpha; }
00255          else { REPORT L.element(i,i) = -sum; }
00256          Real* xj0=xi0; k=n; while(k--) { *xj0++ *= alpha; }
00257          for (int j=i+1; j<s; j++)
00258          {
00259             sum = 0.0;
00260             xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
00261             sum += a0 * L.element(j,i);
00262             xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
00263             L.element(j,i) -= sum * a0;
00264          }
00265       }
00266    }
00267 }
00268 
00269 void updateQRZ(Matrix& X, UpperTriangularMatrix& U)
00270 {
00271    REPORT
00272    Tracer et("updateQRZ");
00273    int n = X.Nrows(); int s = X.Ncols();
00274    if (s != U.Ncols())
00275       Throw(ProgramException("Incompatible dimensions",X,U));
00276    if (n == 0 || s == 0) return; 
00277    Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
00278    RowVector V(s); Real* v0 = V.Store(); Real* v; V = 0.0;
00279    int j, k; int J = s; int i = s;
00280    while (i--)
00281    {
00282       Real* xj0 = xi0; Real* xi = xi0; k = n;
00283       if (k) for (;;)
00284       {
00285          v = v0; Real Xi = *xi; Real* xj = xj0;
00286          j = J; while(j--) *v++ += Xi * *xj++;
00287          if (!(--k)) break;
00288          xi += s; xj0 += s;
00289       }
00290 
00291       Real r = *u0;
00292       Real sum = sqrt(*v0 + square(r));
00293       
00294       if (sum == 0.0)
00295       {
00296          REPORT
00297          u = u0; v = v0;
00298          j = J; while(j--) { *u++ = 0.0; *v++ = 0.0; }
00299          xj0 = xi0++; k = n;
00300          if (k) for (;;)
00301          {
00302             *xj0 = 0.0;
00303             if (!(--k)) break;
00304                   xj0 += s;
00305          }
00306          u0 += J--;
00307       }
00308       else
00309       {
00310          Real frs = fabs(r) + sum;
00311          Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
00312          if (r <= 0) { REPORT alpha = -alpha; *u0 = sum; }
00313          else { REPORT *u0 = -sum; }
00314       
00315          j = J - 1; v = v0 + 1; u = u0 + 1;     
00316          while (j--)
00317             { *v = a0 * *u + alpha * *v; *u -= a0 * *v; ++v; ++u; }
00318 
00319          xj0 = xi0; xi = xi0++; k = n;
00320          if (k) for (;;)
00321          {
00322             v = v0 + 1; Real Xi = *xi; Real* xj = xj0;
00323             Xi *= alpha; *xj++ = Xi;
00324             j = J - 1; while(j--) *xj++ -= *v++ * Xi;
00325             if (!(--k)) break;
00326                   xi += s; xj0 += s;
00327          }
00328          
00329          j = J; v = v0;
00330          while (j--) *v++ = 0.0;
00331          
00332          u0 += J--;
00333       }
00334    }
00335 }
00336 
00337 // Matrix A's first n columns are orthonormal
00338 // so A.Columns(1,n).t() * A.Columns(1,n) is the identity matrix.
00339 // Fill out the remaining columns of A to make them orthonormal
00340 // so A.t() * A is the identity matrix 
00341 void extend_orthonormal(Matrix& A, int n)
00342 {
00343    REPORT
00344    Tracer et("extend_orthonormal");
00345    int nr = A.nrows(); int nc = A.ncols();
00346    if (nc > nr) Throw(IncompatibleDimensionsException(A));
00347    if (n > nc) Throw(IncompatibleDimensionsException(A));
00348    ColumnVector SSR;
00349    { Matrix A1 = A.Columns(1,n); SSR = A1.sum_square_rows(); }
00350    for (int i = n; i < nc; ++i)
00351    {
00352       // pick row with smallest SSQ
00353       int k; SSR.minimum1(k);
00354       // orthogonalise column with 1 at element k, 0 elsewhere
00355       // next line is rather inefficient
00356       ColumnVector X = - A.Columns(1, i) * A.SubMatrix(k, k, 1, i).t();
00357       X(k) += 1.0;
00358       // normalise
00359       X /= sqrt(X.SumSquare());
00360       // update row sums of squares
00361       for (k = 1; k <= nr; ++k) SSR(k) += square(X(k));
00362       // load new column into matrix
00363       A.Column(i+1) = X;
00364    }
00365 }
00366    
00367    
00368 
00369 
00370 
00371 #ifdef use_namespace
00372 }
00373 #endif
00374 
00375 


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32