コード例 #1
0
ファイル: hholder.cpp プロジェクト: alod83/IS-MOOS
void QRZ(Matrix& X, UpperTriangularMatrix& U)
{
   REPORT
   Tracer et("QRZ(1)");
   int n = X.Nrows(); int s = X.Ncols(); U.ReSize(s); U = 0.0;
   Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
   int j, k; int J = s; int i = s;
   while (i--)
   {
      Real* xj0 = xi0; Real* xi = xi0; k = n;
      if (k) for (;;)
      {
         u = u0; Real Xi = *xi; Real* xj = xj0;
         j = J; while(j--) *u++ += Xi * *xj++;
         if (!(--k)) break;
         xi += s; xj0 += s;
      }

      Real sum = sqrt(*u0); *u0 = sum; u = u0+1;
      if (sum == 0.0) Throw(SingularException(U));
      int J1 = J-1; j = J1; while(j--) *u++ /= sum;

      xj0 = xi0; xi = xi0++; k = n;
      if (k) for (;;)
      {
         u = u0+1; Real Xi = *xi; Real* xj = xj0;
         Xi /= sum; *xj++ = Xi;
         j = J1; while(j--) *xj++ -= *u++ * Xi;
         if (!(--k)) break;
          xi += s; xj0 += s;
      }
      u0 += J--;
   }
}
コード例 #2
0
ファイル: newmatnl.cpp プロジェクト: 151706061/sofa
void NonLinearLeastSquares::MakeCovariance()
{
   if (Covariance.Nrows()==0)
   {
      UpperTriangularMatrix UI = U.i();
      Covariance << UI * UI.t() * errorvar;
      SE << Covariance;                 // get diagonals
      for (int i = 1; i<=n_param; i++) SE(i) = sqrt(SE(i));
   }
}
コード例 #3
0
  inline
  Matrix<C>::Matrix(const UpperTriangularMatrix<C>& M)
    : entries_(M.row_dimension()*M.column_dimension()),
      rowdim_(M.row_dimension()), coldim_(M.column_dimension())
  {
    for (size_type i(0); i < rowdim_; i++)
      for (size_type j(i); j < coldim_; j++)
	{
	  this->operator () (i, j) = M(i, j);
	}
  }
コード例 #4
0
void updateQRZ(UpperTriangularMatrix& X, UpperTriangularMatrix& U)
{
   REPORT
   Tracer et("updateQRZ(3)");
   int s = X.Ncols();
   if (s != U.Ncols())
      Throw(ProgramException("Incompatible dimensions",X,U));
   if (s == 0) return; 
   Real* xi0 = X.data(); Real* u = U.data();
   for (int i=1; i<=s; ++i)
   {
		  Real r = *u; Real sum = 0.0;
			{
         Real* xi=xi0; int k=i; int l=s;
			   while(k--) { sum += square(*xi); xi+= --l;}
			}
      sum = sqrt(sum + square(r));
      if (sum == 0.0) { REPORT X.column(i) = 0.0; *u = 0.0; }
      else
      {
         Real frs = fabs(r) + sum;
         Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
         if (r <= 0) { REPORT *u = sum; alpha = -alpha; }
         else { REPORT *u = -sum; }
         {
            Real* xj0=xi0; int k=i; int l=s;
            while(k--) { *xj0 *= alpha; --l; xj0 += l;}
         }
         Real* xj0=xi0; Real* uj=u;
         for (int j=i+1; j<=s; ++j)
         {
            Real sum = 0.0; ++xj0; ++uj;
            Real* xi=xi0; Real* xj=xj0; int k=i; int l=s; 
            while(k--) { sum += *xi * *xj; --l; xi += l; xj += l; }
            sum += a0 * *uj;
            xi=xi0; xj=xj0; k=i; l=s;
            while(k--) { *xj -= sum * *xi; --l; xi += l; xj += l; }
            *uj -= sum * a0;
         }
      }
			++xi0; u += s-i+1;
   }
}
コード例 #5
0
// produces the Cholesky decomposition of A - x.t() * x where A = chol.t() * chol
void downdate_Cholesky(UpperTriangularMatrix &chol, RowVector x)
{
   int nRC = chol.Nrows();
	
   // solve R^T a = x
   LowerTriangularMatrix L = chol.t();
   ColumnVector a(nRC); a = 0.0;
   int i, j;
	
   for (i = 1; i <= nRC; ++i)
   {
      // accumulate subtr sum
      Real subtrsum = 0.0;
      for(int k = 1; k < i; ++k) subtrsum += a(k) * L(i,k);

      a(i) = (x(i) - subtrsum) / L(i,i);
   }

   // test that l2 norm of a is < 1
   Real squareNormA = a.SumSquare();
   if (squareNormA >= 1.0)
      Throw(ProgramException("downdate_Cholesky() fails", chol));

   Real alpha = sqrt(1.0 - squareNormA);

   // compute and apply Givens rotations to the vector a
   ColumnVector cGivens(nRC);  cGivens = 0.0;
   ColumnVector sGivens(nRC);  sGivens = 0.0;
   for(i = nRC; i >= 1; i--)
      alpha = pythag(alpha, a(i), cGivens(i), sGivens(i));

   // apply Givens rotations to the jth column of chol
   ColumnVector xtilde(nRC); xtilde = 0.0;
   for(j = nRC; j >= 1; j--)
   {
      // only the first j rotations have an affect on chol,0
      for(int k = j; k >= 1; k--)
         GivensRotation(cGivens(k), -sGivens(k), chol(k,j), xtilde(j));
   }
}
コード例 #6
0
void updateQRZ(const UpperTriangularMatrix& X, Matrix& MX, Matrix& MU)
{
   REPORT
   Tracer et("updateQRZ(4)");
   int s = X.Ncols();
   if (s != MX.Nrows())
      Throw(ProgramException("Incompatible dimensions",X,MX));
   if (s != MU.Nrows())
      Throw(ProgramException("Incompatible dimensions",X,MU));
   int t = MX.Ncols();
   if (t != MU.Ncols())
      Throw(ProgramException("Incompatible dimensions",MX,MU));
   if (s == 0) return;
    
   const Real* xi0 = X.data(); Real* mx = MX.data(); Real* muj = MU.data();
   for (int i=1; i<=s; ++i)
   {
		  Real sum = 0.0;
			{
         const Real* xi=xi0; int k=i; int l=s;
			   while(k--) { sum += square(*xi); xi+= --l;}
			}
      Real a0 = sqrt(2.0 - sum); Real* mxj0 = mx;
      for (int j=1; j<=t; ++j)
      {
         Real sum = 0.0;
         const Real* xi=xi0; Real* mxj=mxj0; int k=i; int l=s; 
         while(--k) { sum += *xi * *mxj; --l; xi += l; mxj += t; }
         sum += *xi * *mxj;    // last line of loop
         sum += a0 * *muj;
         xi=xi0; mxj=mxj0; k=i; l=s;
         while(--k) { *mxj -= sum * *xi; --l; xi += l; mxj += t; }
         *mxj -= sum * *xi;    // last line of loop
         *muj -= sum * a0; ++mxj0; ++muj;
      }
			++xi0;
   }
}
コード例 #7
0
// produces the Cholesky decomposition of EAE where A = chol.t() * chol
// and E produces a LEFT circular shift of the rows and columns from
// 1,...,k-1,k,k+1,...l,l+1,...,p to
// 1,...,k-1,k+1,...l,k,l+1,...,p to
void left_circular_update_Cholesky(UpperTriangularMatrix &chol, int k, int l)
{
   int nRC = chol.Nrows();
   int i, j;

   // I. compute shift of column k to the lth position
   Matrix cholCopy = chol;
   // a. grab column k
   ColumnVector columnK = cholCopy.Column(k);
   // b. shift columns k+1,...l to the LEFT
   for(j = k+1; j <= l; ++j)
      cholCopy.Column(j-1) = cholCopy.Column(j);
   // c. copy the elements of columnK into the lth column of cholCopy
   cholCopy.Column(l) = 0.0;
   for(i = 1; i <= k; ++i)
      cholCopy(i,l) = columnK(i);

   // II. apply and compute Given's rotations
   int nGivens = l-k;
   ColumnVector cGivens(nGivens); cGivens = 0.0;
   ColumnVector sGivens(nGivens); sGivens = 0.0;
   for(j = k; j <= nRC; ++j)
   {
      ColumnVector columnJ = cholCopy.Column(j);

      // apply the previous Givens rotations to columnJ
      int imax = j - k; if (imax > nGivens) imax = nGivens;
      for(int i = 1; i <= imax; ++i)
      {
         int gIndex = i;
         int topRowIndex = k + i - 1;
         GivensRotationR(cGivens(gIndex), sGivens(gIndex),
            columnJ(topRowIndex), columnJ(topRowIndex+1));
      }

      // compute a new Given's rotation when j < l
      if(j < l)
      {
         int gIndex = j-k+1;
         columnJ(j) = pythag(columnJ(j), columnJ(j+1), cGivens(gIndex),
            sGivens(gIndex));
         columnJ(j+1) = 0.0;
      }

      cholCopy.Column(j) = columnJ;
   }

   chol << cholCopy;
	
}
コード例 #8
0
ファイル: example.cpp プロジェクト: 151706061/sofa
void test4(Real* y, Real* x1, Real* x2, int nobs, int npred)
{
   cout << "\n\nTest 4 - QR triangularisation\n";

   // QR triangularisation method
 
   // load data - 1s into col 1 of matrix
   int npred1 = npred+1;
   Matrix X(nobs,npred1); ColumnVector Y(nobs);
   X.Column(1) = 1.0;  X.Column(2) << x1;  X.Column(3) << x2;  Y << y;

   // do Householder triangularisation
   // no need to deal with constant term separately
   Matrix X1 = X;                 // Want copy of matrix
   ColumnVector Y1 = Y;
   UpperTriangularMatrix U; ColumnVector M;
   QRZ(X1, U); QRZ(X1, Y1, M);    // Y1 now contains resids
   ColumnVector A = U.i() * M;
   ColumnVector Fitted = X * A;
   Real ResVar = Y1.SumSquare() / (nobs-npred1);

   // get variances of estimates
   U = U.i(); DiagonalMatrix D; D << U * U.t();

   // Get diagonals of Hat matrix
   DiagonalMatrix Hat;  Hat << X1 * X1.t();

   // print out answers
   cout << "\nEstimates and their standard errors\n\n";
   ColumnVector SE(npred1);
   for (int i=1; i<=npred1; i++) SE(i) = sqrt(D(i)*ResVar);
   cout << setw(11) << setprecision(5) << (A | SE) << endl;
   cout << "\nObservations, fitted value, residual value, hat value\n";
   cout << setw(9) << setprecision(3) << 
      (X.Columns(2,3) | Y | Fitted | Y1 | Hat.AsColumn());
   cout << "\n\n";
}
コード例 #9
0
// produces the Cholesky decomposition of EAE where A = chol.t() * chol
// and E produces a RIGHT circular shift of the rows and columns from
// 1,...,k-1,k,k+1,...l,l+1,...,p to
// 1,...,k-1,l,k,k+1,...l-1,l+1,...p
void right_circular_update_Cholesky(UpperTriangularMatrix &chol, int k, int l)
{
   int nRC = chol.Nrows();
   int i, j;
	
   // I. compute shift of column l to the kth position
   Matrix cholCopy = chol;
   // a. grab column l
   ColumnVector columnL = cholCopy.Column(l);
   // b. shift columns k,...l-1 to the RIGHT
   for(j = l-1; j >= k; --j)
      cholCopy.Column(j+1) = cholCopy.Column(j);
   // c. copy the top k-1 elements of columnL into the kth column of cholCopy
   cholCopy.Column(k) = 0.0;
   for(i = 1; i < k; ++i) cholCopy(i,k) = columnL(i);

    // II. determine the l-k Given's rotations
   int nGivens = l-k;
   ColumnVector cGivens(nGivens); cGivens = 0.0;
   ColumnVector sGivens(nGivens); sGivens = 0.0;
   for(i = l; i > k; i--)
   {
      int givensIndex = l-i+1;
      columnL(i-1) = pythag(columnL(i-1), columnL(i),
         cGivens(givensIndex), sGivens(givensIndex));
      columnL(i) = 0.0;
   }
   // the kth entry of columnL is the new diagonal element in column k of cholCopy
   cholCopy(k,k) = columnL(k);
	
   // III. apply these Given's rotations to subsequent columns
   // for columns k+1,...,l-1 we only need to apply the last nGivens-(j-k) rotations
   for(j = k+1; j <= nRC; ++j)
   {
      ColumnVector columnJ = cholCopy.Column(j);
      int imin = nGivens - (j-k) + 1; if (imin < 1) imin = 1;
      for(int gIndex = imin; gIndex <= nGivens; ++gIndex)
      {
         // apply gIndex Given's rotation
         int topRowIndex = k + nGivens - gIndex;
         GivensRotationR(cGivens(gIndex), sGivens(gIndex),
            columnJ(topRowIndex), columnJ(topRowIndex+1));
      }
      cholCopy.Column(j) = columnJ;
   }

   chol << cholCopy;
}
コード例 #10
0
ファイル: tmt.cpp プロジェクト: Jornason/DieHard
void Print(const UpperTriangularMatrix& X)
{
   ++PCN;
   cout << "\nMatrix type: " << X.Type().Value() << " (";
   cout << X.Nrows() << ", ";
   cout << X.Ncols() << ")\n\n";
   if (X.IsZero()) { cout << "All elements are zero\n" << flush; return; }
   int nr=X.Nrows(); int nc=X.Ncols();
   for (int i=1; i<=nr; i++)
   {
      int j;
      for (j=1; j<i; j++) cout << "\t";
      for (j=i; j<=nc; j++)  cout << X(i,j) << "\t";
      cout << "\n";
   }
   cout << flush; ++PCZ;
}
コード例 #11
0
// produces the Cholesky decomposition of A + x.t() * x where A = chol.t() * chol
void update_Cholesky(UpperTriangularMatrix &chol, RowVector x)
{
   int nc = chol.Nrows();
   ColumnVector cGivens(nc); cGivens = 0.0;
   ColumnVector sGivens(nc); sGivens = 0.0;
	
   for(int j = 1; j <= nc; ++j) // process the jth column of chol
   {
      // apply the previous Givens rotations k = 1,...,j-1 to column j
      for(int k = 1; k < j; ++k)
         GivensRotation(cGivens(k), sGivens(k), chol(k,j), x(j));

      // determine the jth Given's rotation
      pythag(chol(j,j), x(j), cGivens(j), sGivens(j));

      // apply the jth Given's rotation
      {
         Real tmp0 = cGivens(j) * chol(j,j) + sGivens(j) * x(j);
         chol(j,j) = tmp0; x(j) = 0.0;
      }

   }

}
コード例 #12
0
ファイル: hholder.cpp プロジェクト: CalebVDW/smr-motion
void updateQRZ(Matrix& X, UpperTriangularMatrix& U)
{
   REPORT
   Tracer et("updateQRZ");
   int n = X.Nrows(); int s = X.Ncols();
   if (s != U.Ncols())
      Throw(ProgramException("Incompatible dimensions",X,U));
   if (n == 0 || s == 0) return; 
   Real* xi0 = X.Store(); Real* u0 = U.Store(); Real* u;
   RowVector V(s); Real* v0 = V.Store(); Real* v; V = 0.0;
   int j, k; int J = s; int i = s;
   while (i--)
   {
      Real* xj0 = xi0; Real* xi = xi0; k = n;
      if (k) for (;;)
      {
         v = v0; Real Xi = *xi; Real* xj = xj0;
         j = J; while(j--) *v++ += Xi * *xj++;
         if (!(--k)) break;
         xi += s; xj0 += s;
      }

      Real r = *u0;
      Real sum = sqrt(*v0 + square(r));
      
      if (sum == 0.0)
      {
         REPORT
         u = u0; v = v0;
         j = J; while(j--) { *u++ = 0.0; *v++ = 0.0; }
         xj0 = xi0++; k = n;
         if (k) for (;;)
         {
            *xj0 = 0.0;
            if (!(--k)) break;
	          xj0 += s;
         }
         u0 += J--;
      }
      else
      {
         Real frs = fabs(r) + sum;
         Real a0 = sqrt(frs / sum); Real alpha = a0 / frs;
         if (r <= 0) { REPORT alpha = -alpha; *u0 = sum; }
         else { REPORT *u0 = -sum; }
      
         j = J - 1; v = v0 + 1; u = u0 + 1;     
         while (j--)
            { *v = a0 * *u + alpha * *v; *u -= a0 * *v; ++v; ++u; }

         xj0 = xi0; xi = xi0++; k = n;
         if (k) for (;;)
         {
            v = v0 + 1; Real Xi = *xi; Real* xj = xj0;
            Xi *= alpha; *xj++ = Xi;
            j = J - 1; while(j--) *xj++ -= *v++ * Xi;
            if (!(--k)) break;
	          xi += s; xj0 += s;
         }
         
         j = J; v = v0;
         while (j--) *v++ = 0.0;
         
         u0 += J--;
      }
   }
}
コード例 #13
0
ファイル: tmtc.cpp プロジェクト: Jornason/DieHard
void trymatc()
{
//   cout << "\nTwelfth test of Matrix package\n";
   Tracer et("Twelfth test of Matrix package");
   Tracer::PrintTrace();
   DiagonalMatrix D(15); D=1.5;
   Matrix A(15,15);
   int i,j;
   for (i=1;i<=15;i++) for (j=1;j<=15;j++) A(i,j)=i*i+j-150;
   { A = A + D; }
   ColumnVector B(15);
   for (i=1;i<=15;i++) B(i)=i+i*i-150.0;
   {
      Tracer et1("Stage 1");
      ColumnVector B1=B;
      B=(A*2.0).i() * B1;
      Matrix X = A*B-B1/2.0;
      Clean(X, 0.000000001); Print(X);
      A.ReSize(3,5);
      for (i=1; i<=3; i++) for (j=1; j<=5; j++) A(i,j) = i+100*j;

      B = A.AsColumn()+10000;
      RowVector R = (A+10000).AsColumn().t();
      Print( RowVector(R-B.t()) );
   }

   {
      Tracer et1("Stage 2");
      B = A.AsColumn()+10000;
      Matrix XR = (A+10000).AsMatrix(15,1).t();
      Print( RowVector(XR-B.t()) );
   }

   {
      Tracer et1("Stage 3");
      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0+10000;
      Matrix MR = (A+10000).AsColumn().t();
      Print( RowVector(MR-B.t()) );

      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0;
      MR = A.AsColumn().t();
      Print( RowVector(MR-B.t()) );
   }

   {
      Tracer et1("Stage 4");
      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0;
      RowVector R = A.AsColumn().t();
      Print( RowVector(R-B.t()) );
   }

   {
      Tracer et1("Stage 5");
      RowVector R = (A.AsColumn()-5000).t();
      B = ((R.t()+10000) - A.AsColumn())-5000;
      Print( RowVector(B.t()) );
   }

   {
      Tracer et1("Stage 6");
      B = A.AsColumn(); ColumnVector B1 = (A+10000).AsColumn() - 10000;
      Print(ColumnVector(B1-B));
   }

   {
      Tracer et1("Stage 7");
      Matrix X = B.AsMatrix(3,5); Print(Matrix(X-A));
      for (i=1; i<=3; i++) for (j=1; j<=5; j++) B(5*(i-1)+j) -= i+100*j;
      Print(B);
   }

   {
      Tracer et1("Stage 8");
      A.ReSize(7,7); D.ReSize(7);
      for (i=1; i<=7; i++) for (j=1; j<=7; j++) A(i,j) = i*j*j;
      for (i=1; i<=7; i++) D(i,i) = i;
      UpperTriangularMatrix U; U << A;
      Matrix X = A; for (i=1; i<=7; i++) X(i,i) = i;
      A.Inject(D); Print(Matrix(X-A));
      X = U; U.Inject(D); A = U; for (i=1; i<=7; i++) X(i,i) = i;
      Print(Matrix(X-A));
   }

   {
      Tracer et1("Stage 9");
      A.ReSize(7,5);
      for (i=1; i<=7; i++) for (j=1; j<=5; j++) A(i,j) = i+100*j;
      Matrix Y = A; Y = Y - ((const Matrix&)A); Print(Y);
      Matrix X = A; // X.Release();
      Y = A; Y = ((const Matrix&)X) - A; Print(Y); Y = 0.0;
      Y = ((const Matrix&)X) - ((const Matrix&)A); Print(Y);
   }

   {
      Tracer et1("Stage 10");
      // some tests on submatrices
      UpperTriangularMatrix U(20);
      for (i=1; i<=20; i++) for (j=i; j<=20; j++) U(i,j)=100 * i + j;
      UpperTriangularMatrix V = U.SymSubMatrix(1,5);
      UpperTriangularMatrix U1 = U;
      U1.SubMatrix(4,8,5,9) /= 2;
      U1.SubMatrix(4,8,5,9) += 388 * V;
      U1.SubMatrix(4,8,5,9) *= 2;
      U1.SubMatrix(4,8,5,9) += V;
      U1 -= U; UpperTriangularMatrix U2 = U1;
      U1 << U1.SubMatrix(4,8,5,9);
      U2.SubMatrix(4,8,5,9) -= U1; Print(U2);
      U1 -= (777*V); Print(U1);

      U1 = U; U1.SubMatrix(4,8,5,9) -= U.SymSubMatrix(1,5);
      U1 -= U;  U2 = U1; U1 << U1.SubMatrix(4,8,5,9);
      U2.SubMatrix(4,8,5,9) -= U1; Print(U2);
      U1 += V; Print(U1);

      U1 = U;
      U1.SubMatrix(3,10,15,19) += 29;
      U1 -= U;
      Matrix X = U1.SubMatrix(3,10,15,19); X -= 29; Print(X);
      U1.SubMatrix(3,10,15,19) *= 0; Print(U1);

      LowerTriangularMatrix L = U.t();
      LowerTriangularMatrix M = L.SymSubMatrix(1,5);
      LowerTriangularMatrix L1 = L;
      L1.SubMatrix(5,9,4,8) /= 2;
      L1.SubMatrix(5,9,4,8) += 388 * M;
      L1.SubMatrix(5,9,4,8) *= 2;
      L1.SubMatrix(5,9,4,8) += M;
      L1 -= L; LowerTriangularMatrix L2 = L1;
      L1 << L1.SubMatrix(5,9,4,8);
      L2.SubMatrix(5,9,4,8) -= L1; Print(L2);
      L1 -= (777*M); Print(L1);

      L1 = L; L1.SubMatrix(5,9,4,8) -= L.SymSubMatrix(1,5);
      L1 -= L; L2 =L1; L1 << L1.SubMatrix(5,9,4,8);
      L2.SubMatrix(5,9,4,8) -= L1; Print(L2);
      L1 += M; Print(L1);

      L1 = L;
      L1.SubMatrix(15,19,3,10) -= 29;
      L1 -= L;
      X = L1.SubMatrix(15,19,3,10); X += 29; Print(X);
      L1.SubMatrix(15,19,3,10) *= 0; Print(L1);
   }

   {
      Tracer et1("Stage 11");
      // more tests on submatrices
      Matrix M(20,30);
      for (i=1; i<=20; i++) for (j=1; j<=30; j++) M(i,j)=100 * i + j;
      Matrix M1 = M;

      for (j=1; j<=30; j++)
         { ColumnVector CV = 3 * M1.Column(j); M.Column(j) += CV; }
      for (i=1; i<=20; i++)
         { RowVector RV = 5 * M1.Row(i); M.Row(i) -= RV; }

      M += M1; Print(M);
 
   }


//   cout << "\nEnd of twelfth test\n";
}
コード例 #14
0
ファイル: StrainMeasure.cpp プロジェクト: Haider-BA/geode
template<class T,int d> void StrainMeasure<T,d>::initialize_rest_state_to_equilateral(const T side_length) {
  GEODE_ASSERT(Dm_inverse.size()==elements.size());
  UpperTriangularMatrix<T,d> Dm = side_length*equilateral_Dm(Vector<T,d>());
  Dm_inverse.fill(Dm.inverse());
}
コード例 #15
0
ファイル: tmtc.cpp プロジェクト: OpenCMISS-Dependencies/optpp
void trymatc()
{
//   cout << "\nTwelfth test of Matrix package\n";
   Tracer et("Twelfth test of Matrix package");
   Tracer::PrintTrace();
   DiagonalMatrix D(15); D=1.5;
   Matrix A(15,15);
   int i,j;
   for (i=1;i<=15;i++) for (j=1;j<=15;j++) A(i,j)=i*i+j-150;
   { A = A + D; }
   ColumnVector B(15);
   for (i=1;i<=15;i++) B(i)=i+i*i-150.0;
   {
      Tracer et1("Stage 1");
      ColumnVector B1=B;
      B=(A*2.0).i() * B1;
      Matrix X = A*B-B1/2.0;
      Clean(X, 0.000000001); Print(X);
      A.ReSize(3,5);
      for (i=1; i<=3; i++) for (j=1; j<=5; j++) A(i,j) = i+100*j;

      B = A.AsColumn()+10000;
      RowVector R = (A+10000).AsColumn().t();
      Print( RowVector(R-B.t()) );
   }

   {
      Tracer et1("Stage 2");
      B = A.AsColumn()+10000;
      Matrix XR = (A+10000).AsMatrix(15,1).t();
      Print( RowVector(XR-B.t()) );
   }

   {
      Tracer et1("Stage 3");
      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0+10000;
      Matrix MR = (A+10000).AsColumn().t();
      Print( RowVector(MR-B.t()) );

      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0;
      MR = A.AsColumn().t();
      Print( RowVector(MR-B.t()) );
   }

   {
      Tracer et1("Stage 4");
      B = (A.AsMatrix(15,1)+A.AsColumn())/2.0;
      RowVector R = A.AsColumn().t();
      Print( RowVector(R-B.t()) );
   }

   {
      Tracer et1("Stage 5");
      RowVector R = (A.AsColumn()-5000).t();
      B = ((R.t()+10000) - A.AsColumn())-5000;
      Print( RowVector(B.t()) );
   }

   {
      Tracer et1("Stage 6");
      B = A.AsColumn(); ColumnVector B1 = (A+10000).AsColumn() - 10000;
      Print(ColumnVector(B1-B));
   }

   {
      Tracer et1("Stage 7");
      Matrix X = B.AsMatrix(3,5); Print(Matrix(X-A));
      for (i=1; i<=3; i++) for (j=1; j<=5; j++) B(5*(i-1)+j) -= i+100*j;
      Print(B);
   }

   {
      Tracer et1("Stage 8");
      A.ReSize(7,7); D.ReSize(7);
      for (i=1; i<=7; i++) for (j=1; j<=7; j++) A(i,j) = i*j*j;
      for (i=1; i<=7; i++) D(i,i) = i;
      UpperTriangularMatrix U; U << A;
      Matrix X = A; for (i=1; i<=7; i++) X(i,i) = i;
      A.Inject(D); Print(Matrix(X-A));
      X = U; U.Inject(D); A = U; for (i=1; i<=7; i++) X(i,i) = i;
      Print(Matrix(X-A));
   }

   {
      Tracer et1("Stage 9");
      A.ReSize(7,5);
      for (i=1; i<=7; i++) for (j=1; j<=5; j++) A(i,j) = i+100*j;
      Matrix Y = A; Y = Y - ((const Matrix&)A); Print(Y);
      Matrix X = A;
      Y = A; Y = ((const Matrix&)X) - A; Print(Y); Y = 0.0;
      Y = ((const Matrix&)X) - ((const Matrix&)A); Print(Y);
   }

   {
      Tracer et1("Stage 10");
      // some tests on submatrices
      UpperTriangularMatrix U(20);
      for (i=1; i<=20; i++) for (j=i; j<=20; j++) U(i,j)=100 * i + j;
      UpperTriangularMatrix V = U.SymSubMatrix(1,5);
      UpperTriangularMatrix U1 = U;
      U1.SubMatrix(4,8,5,9) /= 2;
      U1.SubMatrix(4,8,5,9) += 388 * V;
      U1.SubMatrix(4,8,5,9) *= 2;
      U1.SubMatrix(4,8,5,9) += V;
      U1 -= U; UpperTriangularMatrix U2 = U1;
      U1 << U1.SubMatrix(4,8,5,9);
      U2.SubMatrix(4,8,5,9) -= U1; Print(U2);
      U1 -= (777*V); Print(U1);

      U1 = U; U1.SubMatrix(4,8,5,9) -= U.SymSubMatrix(1,5);
      U1 -= U;  U2 = U1; U1 << U1.SubMatrix(4,8,5,9);
      U2.SubMatrix(4,8,5,9) -= U1; Print(U2);
      U1 += V; Print(U1);

      U1 = U;
      U1.SubMatrix(3,10,15,19) += 29;
      U1 -= U;
      Matrix X = U1.SubMatrix(3,10,15,19); X -= 29; Print(X);
      U1.SubMatrix(3,10,15,19) *= 0; Print(U1);

      LowerTriangularMatrix L = U.t();
      LowerTriangularMatrix M = L.SymSubMatrix(1,5);
      LowerTriangularMatrix L1 = L;
      L1.SubMatrix(5,9,4,8) /= 2;
      L1.SubMatrix(5,9,4,8) += 388 * M;
      L1.SubMatrix(5,9,4,8) *= 2;
      L1.SubMatrix(5,9,4,8) += M;
      L1 -= L; LowerTriangularMatrix L2 = L1;
      L1 << L1.SubMatrix(5,9,4,8);
      L2.SubMatrix(5,9,4,8) -= L1; Print(L2);
      L1 -= (777*M); Print(L1);

      L1 = L; L1.SubMatrix(5,9,4,8) -= L.SymSubMatrix(1,5);
      L1 -= L; L2 =L1; L1 << L1.SubMatrix(5,9,4,8);
      L2.SubMatrix(5,9,4,8) -= L1; Print(L2);
      L1 += M; Print(L1);

      L1 = L;
      L1.SubMatrix(15,19,3,10) -= 29;
      L1 -= L;
      X = L1.SubMatrix(15,19,3,10); X += 29; Print(X);
      L1.SubMatrix(15,19,3,10) *= 0; Print(L1);
   }

   {
      Tracer et1("Stage 11");
      // more tests on submatrices
      Matrix M(20,30);
      for (i=1; i<=20; i++) for (j=1; j<=30; j++) M(i,j)=100 * i + j;
      Matrix M1 = M;

      for (j=1; j<=30; j++)
         { ColumnVector CV = 3 * M1.Column(j); M.Column(j) += CV; }
      for (i=1; i<=20; i++)
         { RowVector RV = 5 * M1.Row(i); M.Row(i) -= RV; }

      M += M1; Print(M);
 
   }

   {
      Tracer et1("Stage 12");
      // more tests on Release
      Matrix M(20,30);
      for (i=1; i<=20; i++) for (j=1; j<=30; j++) M(i,j)=100 * i + j;
      Matrix M1 = M;
      M.Release();
      Matrix M2 = M;
      Matrix X = M;   Print(X);
      X = M1 - M2;    Print(X);

#ifndef DONT_DO_NRIC
      nricMatrix N = M1;
      nricMatrix N1 = N;
      N.Release();
      nricMatrix N2 = N;
      nricMatrix Y = N;   Print(Y);
      Y = N1 - N2;        Print(Y);
      
      N = M1 / 2; N1 = N * 2; N.Release(); N2 = N * 2; Y = N; Print(N);
      Y = (N1 - M1) | (N2 - M1); Print(Y);
#endif

   }
   
   {
      Tracer et("Stage 13");
      // test sum of squares of rows or columns
      MultWithCarry mwc;
      DiagonalMatrix DM; Matrix X;
      // rectangular matrix
      Matrix A(20, 15);
      FillWithValues(mwc, A);
      // sum of squares of rows
      DM << A * A.t();
      ColumnVector CV = A.sum_square_rows();
      X = CV - DM.AsColumn(); Clean(X, 0.000000001); Print(X);
      DM << A.t() * A;
      RowVector RV = A.sum_square_columns();
      X = RV - DM.AsRow(); Clean(X, 0.000000001); Print(X);
      X = RV - A.t().sum_square_rows().t(); Clean(X, 0.000000001); Print(X);
      X = CV - A.t().sum_square_columns().t(); Clean(X, 0.000000001); Print(X);
      // UpperTriangularMatrix
      A.ReSize(17,17); FillWithValues(mwc, A);
      UpperTriangularMatrix UT; UT << A;
      Matrix A1 = UT;
      X = UT.sum_square_rows() - A1.sum_square_rows(); Print(X);
      X = UT.sum_square_columns() - A1.sum_square_columns(); Print(X);
      // LowerTriangularMatrix
      LowerTriangularMatrix LT; LT << A;
      A1 = LT;
      X = LT.sum_square_rows() - A1.sum_square_rows(); Print(X);
      X = LT.sum_square_columns() - A1.sum_square_columns(); Print(X);
      // SymmetricMatrix
      SymmetricMatrix SM; SM << A;
      A1 = SM;
      X = SM.sum_square_rows() - A1.sum_square_rows(); Print(X);
      X = SM.sum_square_columns() - A1.sum_square_columns(); Print(X);
      // DiagonalMatrix
      DM << A;
      A1 = DM;
      X = DM.sum_square_rows() - A1.sum_square_rows(); Print(X);
      X = DM.sum_square_columns() - A1.sum_square_columns(); Print(X);
      // BandMatrix
      BandMatrix BM(17, 3, 5); BM.Inject(A);
      A1 = BM;
      X = BM.sum_square_rows() - A1.sum_square_rows(); Print(X);
      X = BM.sum_square_columns() - A1.sum_square_columns(); Print(X);
      // SymmetricBandMatrix
      SymmetricBandMatrix SBM(17, 4); SBM.Inject(A);
      A1 = SBM;
      X = SBM.sum_square_rows() - A1.sum_square_rows(); Print(X);
      X = SBM.sum_square_columns() - A1.sum_square_columns(); Print(X);
      // IdentityMatrix
      IdentityMatrix IM(29);
      X = IM.sum_square_rows() - 1; Print(X);
      X = IM.sum_square_columns() - 1; Print(X);
      // Matrix with zero rows
      A1.ReSize(0,10);
      X.ReSize(1,10); X = 0; X -= A1.sum_square_columns(); Print(X);
      X.ReSize(0,1); X -= A1.sum_square_rows(); Print(X);
      // Matrix with zero columns
      A1.ReSize(10,0);
      X.ReSize(10,1); X = 0; X -= A1.sum_square_rows(); Print(X);
      X.ReSize(1,0); X -= A1.sum_square_columns(); Print(X);
      
   }
   
   {
      Tracer et("Stage 14");
      // test extend orthonormal
      MultWithCarry mwc;
      Matrix A(20,5); FillWithValues(mwc, A);
      // Orthonormalise
      UpperTriangularMatrix R;
      Matrix A_old = A;
      QRZ(A,R);
      // Check decomposition
      Matrix X = A * R - A_old; Clean(X, 0.000000001); Print(X);
      // Check orthogonality
      X = A.t() * A - IdentityMatrix(5);
      Clean(X, 0.000000001); Print(X);
      // Try orthonality extend 
      SquareMatrix A1(20);
      A1.Columns(1,5) = A;
      extend_orthonormal(A1,5);
      // check columns unchanged
      X = A - A1.Columns(1,5); Print(X);
      // Check orthogonality
      X = A1.t() * A1 - IdentityMatrix(20);
      Clean(X, 0.000000001); Print(X); 
      X = A1 * A1.t() - IdentityMatrix(20);
      Clean(X, 0.000000001); Print(X);
      // Test with smaller number of columns 
      Matrix A2(20,15);
      A2.Columns(1,5) = A;
      extend_orthonormal(A2,5);
      // check columns unchanged
      X = A - A2.Columns(1,5); Print(X);
      // Check orthogonality
      X = A2.t() * A2 - IdentityMatrix(15);
      Clean(X, 0.000000001); Print(X);
      // check it works with no columns to start with
      A2.ReSize(100,100);
      extend_orthonormal(A2,0);
      // Check orthogonality
      X = A2.t() * A2 - IdentityMatrix(100);
      Clean(X, 0.000000001); Print(X);
      X = A2 * A2.t() - IdentityMatrix(100);
      Clean(X, 0.000000001); Print(X);
 
   }
   

//   cout << "\nEnd of twelfth test\n";
}