MatrixRmn.cpp

/*
 *
 * RayTrace Software Package, prerelease 0.1.0.   June 2003
 *
 * Mathematics Subpackage (VrMath)
 *
 * Author: Samuel R. Buss
 *
 * Software accompanying the book
 *		3D Computer Graphics: A Mathematical Introduction with OpenGL,
 *		by S. Buss, Cambridge University Press, 2003.
 *
 * Software is "as-is" and carries no warranty.  It may be used without
 *   restriction, but if you modify it, please change the filenames to
 *   prevent confusion between different versions.  Please acknowledge
 *   all use of the software in any publications or products based on it.
 *
 * Bug reports: Sam Buss, sbuss@ucsd.edu.
 * Web page: http://math.ucsd.edu/~sbuss/MathCG
 *
 */

//
// MatrixRmn.cpp:  Matrix over reals  (Variable dimensional vector)
//
//    Not very sophisticated yet.  Needs more functionality
//		To do: better handling of resizing.
//

#include "MatrixRmn.h"

MatrixRmn MatrixRmn::WorkMatrix;		// Temporary work matrix

// Fill the diagonal entries with the value d.  The rest of the matrix is unchanged.
void MatrixRmn::SetDiagonalEntries( double d )
{
	long diagLen = Min( NumRows, NumCols );
	double* dPtr = x;
	for ( ; diagLen>0; diagLen-- ) {
		*dPtr = d;
		dPtr += NumRows+1;
	}
}

// Fill the diagonal entries with values in vector d.  The rest of the matrix is unchanged.
void MatrixRmn::SetDiagonalEntries( const VectorRn& d )
{
	long diagLen = Min( NumRows, NumCols );
	assert ( d.length == diagLen );
	double* dPtr = x;
	double* from = d.x;
	for ( ; diagLen>0; diagLen-- ) {
		*dPtr = *(from++);
		dPtr += NumRows+1;
	}
}

// Fill the superdiagonal entries with the value d.  The rest of the matrix is unchanged.
void MatrixRmn::SetSuperDiagonalEntries( double d )
{
	long sDiagLen = Min( NumRows, (long)(NumCols-1) );
	double* to = x + NumRows;
	for ( ; sDiagLen>0; sDiagLen-- ) {
		*to = d;
		to += NumRows+1;
	}
}

// Fill the superdiagonal entries with values in vector d.  The rest of the matrix is unchanged.
void MatrixRmn::SetSuperDiagonalEntries( const VectorRn& d )
{
	long sDiagLen = Min( (long)(NumRows-1), NumCols );
	assert ( sDiagLen == d.length );
	double* to = x + NumRows;
	double* from = d.x;
	for ( ; sDiagLen>0; sDiagLen-- ) {
		*to = *(from++);
		to += NumRows+1;
	}
}

// Fill the subdiagonal entries with the value d.  The rest of the matrix is unchanged.
void MatrixRmn::SetSubDiagonalEntries( double d )
{
	long sDiagLen = Min( NumRows, NumCols ) - 1;
	double* to = x + 1;
	for ( ; sDiagLen>0; sDiagLen-- ) {
		*to = d;
		to += NumRows+1;
	}
}

// Fill the subdiagonal entries with values in vector d.  The rest of the matrix is unchanged.
void MatrixRmn::SetSubDiagonalEntries( const VectorRn& d )
{
	long sDiagLen = Min( NumRows, NumCols ) - 1;
	assert ( sDiagLen == d.length );
	double* to = x + 1;
	double* from = d.x;
	for ( ; sDiagLen>0; sDiagLen-- ) {
		*to = *(from++);
		to += NumRows+1;
	}
}

// Set the i-th column equal to d.
void MatrixRmn::SetColumn(long i, const VectorRn& d )
{
	assert ( NumRows==d.GetLength() );
	double* to = x+i*NumRows;
	const double* from = d.x;
	for ( i=NumRows; i>0; i-- ) {
		*(to++) = *(from++);
	}
}

// Set the i-th column equal to d.
void MatrixRmn::SetRow(long i, const VectorRn& d )
{
	assert ( NumCols==d.GetLength() );
	double* to = x+i;
	const double* from = d.x;
	for ( i=NumRows; i>0; i-- ) {
		*to = *(from++);
		to += NumRows;
	}
}

// Sets a "linear" portion of the array with the values from a vector d
// The first row and column position are given by startRow, startCol.
// Successive positions are found by using the deltaRow, deltaCol values
//	to increment the row and column indices.  There is no wrapping around.
void MatrixRmn::SetSequence( const VectorRn& d, long startRow, long startCol, long deltaRow, long deltaCol )
{
	long length = d.length;
	assert( startRow>=0 && startRow<NumRows && startCol>=0 && startCol<NumCols );
	assert( startRow+(length-1)*deltaRow>=0 && startRow+(length-1)*deltaRow<NumRows );
 	assert( startCol+(length-1)*deltaCol>=0 && startCol+(length-1)*deltaCol<NumCols );
	double *to = x + startRow + NumRows*startCol;
	double *from = d.x;
	long stride = deltaRow + NumRows*deltaCol;
	for ( ; length>0; length-- ) {
		*to = *(from++);
		to += stride;
	}
}

// The matrix A is loaded, in into "this" matrix, based at (0,0).
//  The size of "this" matrix must be large enough to accomodate A.
//	The rest of "this" matrix is left unchanged.  It is not filled with zeroes!

void MatrixRmn::LoadAsSubmatrix( const MatrixRmn& A )
{
	assert( A.NumRows<=NumRows && A.NumCols<=NumCols );
	int extraColStep = NumRows - A.NumRows;
	double *to = x;
	double *from = A.x;
	for ( long i=A.NumCols; i>0; i-- ) {			// Copy columns of A, one per time thru loop
		for ( long j=A.NumRows; j>0; j-- ) {		// Copy all elements of this column of A
			*(to++) = *(from++);
		}
		to += extraColStep;
	}	
}

// The matrix A is loaded, in transposed order into "this" matrix, based at (0,0).
//  The size of "this" matrix must be large enough to accomodate A.
//	The rest of "this" matrix is left unchanged.  It is not filled with zeroes!
void MatrixRmn::LoadAsSubmatrixTranspose( const MatrixRmn& A )
{
	assert( A.NumRows<=NumCols && A.NumCols<=NumRows );
	double* rowPtr = x;
	double* from = A.x;
	for ( long i=A.NumCols; i>0; i-- ) {				// Copy columns of A, once per loop
		double* to = rowPtr;
		for ( long j=A.NumRows; j>0; j-- ) {			// Loop copying values from the column of A
			*to = *(from++);
			to += NumRows;
		}
		rowPtr ++;
	}	
}

// Calculate the Frobenius Norm (square root of sum of squares of entries of the matrix)
double MatrixRmn::FrobeniusNorm() const
{
	return sqrt( FrobeniusNormSq() );
}

// Multiply this matrix by column vector v.  
// Result is column vector "result"
void MatrixRmn::Multiply( const VectorRn& v, VectorRn& result ) const
{
	assert ( v.GetLength()==NumCols && result.GetLength()==NumRows );
	double* out = result.GetPtr();				// Points to entry in result vector
	const double* rowPtr = x;					// Points to beginning of next row in matrix
	for ( long j = NumRows; j>0; j-- ) {
		const double* in = v.GetPtr();
		const double* m = rowPtr++;
		*out = 0.0f;
		for ( long i = NumCols; i>0; i-- ) {
			*out += (*(in++)) * (*m);
			m += NumRows;
		}
		out++;
	}
}

// Multiply transpose of this matrix by column vector v.  
//    Result is column vector "result"
// Equivalent to mult by row vector on left
void MatrixRmn::MultiplyTranspose( const VectorRn& v, VectorRn& result ) const	
{
	assert ( v.GetLength()==NumRows && result.GetLength()==NumCols );
	double* out = result.GetPtr();				// Points to entry in result vector
	const double* colPtr = x;					// Points to beginning of next column in matrix
	for ( long i=NumCols; i>0; i-- ) {
		const double* in=v.GetPtr();
		*out = 0.0f;
		for ( long j = NumRows; j>0; j-- ) {
			*out += (*(in++)) * (*(colPtr++));
		}
		out++;
	}
}

// Form the dot product of a vector v with the i-th column of the array
double MatrixRmn::DotProductColumn( const VectorRn& v, long colNum ) const
{
	assert ( v.GetLength()==NumRows );
	double* ptrC = x+colNum*NumRows;
	double* ptrV = v.x;	
	double ret = 0.0;
	for ( long i = NumRows; i>0; i-- ) {
		ret += (*(ptrC++))*(*(ptrV++));
	}
	return ret;
}

// Add a constant to each entry on the diagonal
MatrixRmn& MatrixRmn::AddToDiagonal( double d )				// Adds d to each diagonal entry
{
	long diagLen = Min( NumRows, NumCols );
	double* dPtr = x;
	for ( ; diagLen>0; diagLen-- ) {
		*dPtr += d;
		dPtr += NumRows+1;
	}
	return *this;
}

// Multiply two MatrixRmn's
MatrixRmn& MatrixRmn::Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
{
	assert( A.NumCols == B.NumRows && A.NumRows == dst.NumRows && B.NumCols == dst.NumCols );
	long length = A.NumCols;

	double *bPtr = B.x;						// Points to beginning of column in B
	double *dPtr = dst.x;
	for ( long i = dst.NumCols; i>0; i-- ) {
		double *aPtr = A.x;					// Points to beginning of row in A
		for ( long j = dst.NumRows; j>0; j-- ) {
			*dPtr = DotArray( length, aPtr, A.NumRows, bPtr, 1 );
			dPtr++;
			aPtr++;
		}
		bPtr += B.NumRows;
	}

	return dst;
}

// Multiply two MatrixRmn's,  Transpose the first matrix before multiplying
MatrixRmn& MatrixRmn::TransposeMultiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
{
	assert( A.NumRows == B.NumRows && A.NumCols == dst.NumRows && B.NumCols == dst.NumCols );
	long length = A.NumRows;

	double *bPtr = B.x;										// bPtr Points to beginning of column in B
	double *dPtr = dst.x;
	for ( long i = dst.NumCols; i>0; i-- ) {				// Loop over all columns of dst
		double *aPtr = A.x;									// aPtr Points to beginning of column in A
		for ( long j = dst.NumRows; j>0; j-- ) {			// Loop over all rows of dst
			*dPtr = DotArray( length, aPtr, 1, bPtr, 1 );
			dPtr ++;
			aPtr += A.NumRows;
		}
		bPtr += B.NumRows;
	}

	return dst;
}

// Multiply two MatrixRmn's.  Transpose the second matrix before multiplying
MatrixRmn& MatrixRmn::MultiplyTranspose( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
{
	assert( A.NumCols == B.NumCols && A.NumRows == dst.NumRows && B.NumRows == dst.NumCols );
	long length = A.NumCols;

	double *bPtr = B.x;						// Points to beginning of row in B
	double *dPtr = dst.x;
	for ( long i = dst.NumCols; i>0; i-- ) {
		double *aPtr = A.x;					// Points to beginning of row in A
		for ( long j = dst.NumRows; j>0; j-- ) {
			*dPtr = DotArray( length, aPtr, A.NumRows, bPtr, B.NumRows );
			dPtr++;
			aPtr++;
		}
		bPtr++;
	}

	return dst;
}

// Solves the equation   (*this)*xVec = b;  
// Uses row operations.  Assumes *this is square and invertible.   
// No error checking for divide by zero or instability (except with asserts)
void MatrixRmn::Solve( const VectorRn& b, VectorRn* xVec ) const
{
	assert ( NumRows==NumCols && NumCols==xVec->GetLength() && NumRows==b.GetLength() );

	// Copy this matrix and b into an Augmented Matrix
	MatrixRmn& AugMat = GetWorkMatrix( NumRows, NumCols+1 );
	AugMat.LoadAsSubmatrix( *this );
	AugMat.SetColumn( NumRows, b );

	// Put into row echelon form with row operations
	AugMat.ConvertToRefNoFree();

	// Solve for x vector values using back substitution
	double* xLast = xVec->x+NumRows-1;				// Last entry in xVec
	double* endRow = AugMat.x+NumRows*NumCols-1;	// Last entry in the current row of the coefficient part of Augmented Matrix
	double* bPtr = endRow+NumRows;				// Last entry in augmented matrix (end of last column, in augmented part)
	for ( long i = NumRows; i>0; i-- ) {
		double accum = *(bPtr--);
		// Next loop computes back substitution terms
		double* rowPtr = endRow;					// Points to entries of the current row for back substitution.
		double* xPtr = xLast;						// Points to entries in the x vector (also for back substitution)
		for ( long j=NumRows-i; j>0; j-- ) {			
			accum -= (*rowPtr)*(*(xPtr--));
			rowPtr -= NumCols;						// Previous entry in the row
		}
		assert( *rowPtr != 0.0 );					// Are not supposed to be any free variables in this matrix
		*xPtr = accum/(*rowPtr);
		endRow--;
	}
}

// ConvertToRefNoFree
// Converts the matrix (in place) to row echelon form
// For us, row echelon form allows any non-zero values, not just 1's, in the 
//		position for a lead variable.
// The "NoFree" version operates on the assumption that no free variable will be found.
// Algorithm uses row operations and row pivoting (only).  
// Augmented matrix is correctly accomodated.  Only the first square part participates 
//		in the main work of row operations.
void MatrixRmn::ConvertToRefNoFree()
{
	// Loop over all columns (variables)
	// Find row with most non-zero entry.
	// Swap to the highest active row
	// Subtract appropriately from all the lower rows (row op of type 3)
	long numIters = Min(NumRows,NumCols);
	double* rowPtr1 = x;
	const long diagStep = NumRows+1;
	long lenRowLeft = NumCols;
	for ( ; numIters>1; numIters-- ) {
		// Find row with most non-zero entry.
		double* rowPtr2 = rowPtr1;
		double maxAbs = fabs(*rowPtr1);
		double *rowPivot = rowPtr1;
		long i;
		for ( i=numIters-1; i>0; i-- ) {
			const double& newMax = *(++rowPivot);
			if ( newMax > maxAbs ) {
				maxAbs = *rowPivot;
				rowPtr2 = rowPivot;
			}
			else if ( -newMax > maxAbs ) {
				maxAbs = -newMax;
				rowPtr2 = rowPivot;
			}
		}
		// Pivot step: Swap the row with highest entry to the current row
		if ( rowPtr1 != rowPtr2 ) {
			double *to = rowPtr1;
			for ( long i=lenRowLeft; i>0; i-- ) {
				double temp = *to;
				*to = *rowPtr2;
				*rowPtr2 = temp;
				to += NumRows;
				rowPtr2 += NumRows;
			}
		}
		// Subtract this row appropriately from all the lower rows (row operation of type 3)
		rowPtr2 = rowPtr1;
		for ( i=numIters-1; i>0; i-- ) {
			rowPtr2++;
			double* to = rowPtr2;
			double* from = rowPtr1;
			assert( *from != 0.0 );
			double alpha = (*to)/(*from);
			*to = 0.0;
			for ( long j=lenRowLeft-1; j>0; j-- ) {
				to += NumRows;
				from += NumRows;
				*to -= (*from)*alpha;
			}
		}
		// Update for next iteration of loop
		rowPtr1 += diagStep;
		lenRowLeft--;
	}

}

// Calculate the c=cosine and s=sine values for a Givens transformation.
// The matrix M = ( (c, -s), (s, c) ) in row order transforms the
//   column vector (a, b)^T to have y-coordinate zero.
void MatrixRmn::CalcGivensValues( double a, double b, double *c, double *s )
{
	double denomInv = sqrt(a*a + b*b);
	if ( denomInv==0.0 ) {
		*c = 1.0;
		*s = 0.0;
	}
	else {
		denomInv = 1.0/denomInv;
		*c = a*denomInv;
		*s = -b*denomInv;
	}
}

// Applies Givens transform to columns i and i+1.
// Equivalent to postmultiplying by the matrix
//      ( c  -s )
//		( s   c )
// with non-zero entries in rows i and i+1 and columns i and i+1
void MatrixRmn::PostApplyGivens( double c, double s, long idx )
{
	assert ( 0<=idx && idx<NumCols );
	double *colA = x + idx*NumRows;
	double *colB = colA + NumRows;
	for ( long i = NumRows; i>0; i-- ) {
		double temp = *colA;
		*colA = (*colA)*c + (*colB)*s;
		*colB = (*colB)*c - temp*s;
		colA++;
		colB++;
	}
}	

// Applies Givens transform to columns idx1 and idx2.
// Equivalent to postmultiplying by the matrix
//      ( c  -s )
//		( s   c )
// with non-zero entries in rows idx1 and idx2 and columns idx1 and idx2
void MatrixRmn::PostApplyGivens( double c, double s, long idx1, long idx2 )
{
	assert ( idx1!=idx2 && 0<=idx1 && idx1<NumCols && 0<=idx2 && idx2<NumCols );
	double *colA = x + idx1*NumRows;
	double *colB = x + idx2*NumRows;
	for ( long i = NumRows; i>0; i-- ) {
		double temp = *colA;
		*colA = (*colA)*c + (*colB)*s;
		*colB = (*colB)*c - temp*s;
		colA++;
		colB++;
	}
}	


// ********************************************************************************************
// Singular value decomposition.
// Return othogonal matrices U and V and diagonal matrix with diagonal w such that
//     (this) = U * Diag(w) * V^T     (V^T is V-transpose.)
// Diagonal entries have all non-zero entries before all zero entries, but are not
//		necessarily sorted.  (Someday, I will write ComputedSortedSVD that handles
//		sorting the eigenvalues by magnitude.)
// ********************************************************************************************
void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const
{
	assert ( U.NumRows==NumRows && V.NumCols==NumCols 
			 && U.NumRows==U.NumCols && V.NumRows==V.NumCols
			 && w.GetLength()==Min(NumRows,NumCols) );

	double temp=0.0;
	VectorRn& superDiag = VectorRn::GetWorkVector( w.GetLength()-1 );		// Some extra work space.  Will get passed around.

	// Choose larger of U, V to hold intermediate results
	// If U is larger than V, use U to store intermediate results
	// Otherwise use V.  In the latter case, we form the SVD of A transpose,
	//		(which is essentially identical to the SVD of A).
	MatrixRmn* leftMatrix;
	MatrixRmn* rightMatrix;
	if ( NumRows >= NumCols ) {
		U.LoadAsSubmatrix( *this );				// Copy A into U
		leftMatrix = &U;
		rightMatrix = &V;
	}
	else {
		V.LoadAsSubmatrixTranspose( *this );		// Copy A-transpose into V
		leftMatrix = &V;
		rightMatrix = &U;
	}

	// Do the actual work to calculate the SVD 
	// Now matrix has at least as many rows as columns
	CalcBidiagonal( *leftMatrix, *rightMatrix, w, superDiag );
	ConvertBidiagToDiagonal( *leftMatrix, *rightMatrix, w, superDiag );

}

// ************************************************ CalcBidiagonal **************************
// Helper routine for SVD computation
// U is a matrix to be bidiagonalized.
// On return, U and V are orthonormal and w holds the new diagonal
//	  elements and superDiag holds the super diagonal elements.
	
void MatrixRmn::CalcBidiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag )
{
	assert ( U.NumRows>=V.NumRows );

	// The diagonal and superdiagonal entries of the bidiagonalized
	//	  version of the U matrix
	//	  are stored in the vectors w and superDiag (temporarily).

	// Apply Householder transformations to U.
	// Householder transformations come in pairs.
	//   First, on the left, we map a portion of a column to zeros
	//   Second, on the right, we map a portion of a row to zeros
	const long rowStep = U.NumCols;
	const long diagStep = U.NumCols+1;
	double *diagPtr = U.x;
	double* wPtr = w.x;
	double* superDiagPtr = superDiag.x;
	long colLengthLeft = U.NumRows;
	long rowLengthLeft = V.NumCols;
	while (true) {
		// Apply a Householder xform on left to zero part of a column
		SvdHouseholder( diagPtr, colLengthLeft, rowLengthLeft, 1, rowStep, wPtr ); 

		if ( rowLengthLeft==2 ) {
			*superDiagPtr = *(diagPtr+rowStep);
			break;
		}
		// Apply a Householder xform on the right to zero part of a row
		SvdHouseholder( diagPtr+rowStep, rowLengthLeft-1, colLengthLeft, rowStep, 1, superDiagPtr ); 

		rowLengthLeft--;
		colLengthLeft--;
		diagPtr += diagStep;
		wPtr++;
		superDiagPtr++;
	}

	int extra = 0;
	diagPtr += diagStep;
	wPtr++;
	if ( colLengthLeft > 2 ) {
		extra = 1;
		// Do one last Householder transformation when the matrix is not square
		colLengthLeft--;
		SvdHouseholder( diagPtr, colLengthLeft, 1, 1, 0, wPtr );
	}
	else {
		*wPtr = *diagPtr;
	}

	// Form U and V from the Householder transformations
	V.ExpandHouseholders( V.NumCols-2, 1, U.x+U.NumRows, U.NumRows, 1 );
	U.ExpandHouseholders( V.NumCols-1+extra, 0, U.x, 1, U.NumRows );

	// Done with bidiagonalization
	return;
}

// Helper routine for CalcBidiagonal
// Performs a series of Householder transformations on a matrix
// Stores results compactly into the matrix:   The Householder vector u (normalized)
//   is stored into the first row/column being transformed.
// The leading term of that row (= plus/minus its magnitude is returned
//	 separately into "retFirstEntry"
void MatrixRmn::SvdHouseholder( double* basePt, 
							    long colLength, long numCols, long colStride, long rowStride, 
								double* retFirstEntry )
{

	// Calc norm of vector u
	double* cPtr = basePt;
	double norm = 0.0;
	long i;
	for ( i=colLength; i>0 ; i-- ) {
		norm += Square( *cPtr );
		cPtr += colStride;
	}
	norm = sqrt(norm);					// Norm of vector to reflect to axis  e_1

	// Handle sign issues		
	double imageVal;					// Choose sign to maximize distance
	if ( (*basePt) < 0.0 ) {
		imageVal = norm;
		norm = 2.0*norm*(norm-(*basePt));	
	}
	else {
		imageVal = -norm;
		norm = 2.0*norm*(norm+(*basePt));
	}
	norm = sqrt(norm);					// Norm is norm of reflection vector

	if ( norm==0.0 ) {			// If the vector being transformed is equal to zero
		// Force to zero in case of roundoff errors
		cPtr = basePt;
		for ( i=colLength; i>0; i-- ) {
			*cPtr = 0.0;
			cPtr += colStride;
		}
		*retFirstEntry = 0.0;
		return;
	}

	*retFirstEntry = imageVal;

	// Set up the normalized Householder vector
	*basePt -= imageVal;					// First component changes. Rest stay the same.
	// Normalize the vector
	norm = 1.0/norm;					// Now it is the inverse norm
	cPtr = basePt;
	for ( i=colLength; i>0 ; i-- ) {
		*cPtr *= norm;
		cPtr += colStride;
	}

	// Transform the rest of the U matrix with the Householder transformation
	double *rPtr = basePt;
	for ( long j=numCols-1; j>0; j-- ) {
		rPtr += rowStride;
		// Calc dot product with Householder transformation vector
		double dotP = DotArray( colLength, basePt, colStride, rPtr, colStride );
		// Transform with I - 2*dotP*(Householder vector)
		AddArrayScale( colLength, basePt, colStride, rPtr, colStride, -2.0*dotP );
	}
}

// ********************************* ExpandHouseholders ********************************************
// The matrix will be square.
//   numXforms = number of Householder transformations to concatenate
//		Each Householder transformation is represented by a unit vector
//		Each successive Householder transformation starts one position later
//			and has one more implied leading zero
//	 basePt = beginning of the first Householder transform
//	 colStride, rowStride: Householder xforms are stored in "columns"
//   numZerosSkipped is the number of implicit zeros on the front each
//			Householder transformation vector (only values supported are 0 and 1).
void MatrixRmn::ExpandHouseholders( long numXforms, int numZerosSkipped, const double* basePt, long colStride, long rowStride )
{
	// Number of applications of the last Householder transform
	//     (That are not trivial!)
	long numToTransform = NumCols-numXforms+1-numZerosSkipped;			
	assert( numToTransform>0 );

	if ( numXforms==0 ) {
		SetIdentity();
		return;
	}

	// Handle the first one separately as a special case,
	// "this" matrix will be treated to simulate being preloaded with the identity
	long hDiagStride = rowStride+colStride;
	const double* hBase = basePt + hDiagStride*(numXforms-1);	// Pointer to the last Householder vector
	const double* hDiagPtr = hBase + colStride*(numToTransform-1);		// Pointer to last entry in that vector
	long i;
	double* diagPtr = x+NumCols*NumRows-1;					// Last entry in matrix (points to diagonal entry)
	double* colPtr = diagPtr-(numToTransform-1);			// Pointer to column in matrix
	for ( i=numToTransform; i>0; i-- ) {
		CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hDiagPtr) );
		*diagPtr += 1.0;						// Add back in 1 to the diagonal entry (since xforming the identity)
		diagPtr -= (NumRows+1);					// Next diagonal entry in this matrix
		colPtr -= NumRows;						// Next column in this matrix
		hDiagPtr -= colStride;
	}

	// Now handle the general case
	// A row of zeros must be in effect added to the top of each old column (in each loop)
	double* colLastPtr = x + NumRows*NumCols - numToTransform - 1; 
	for ( i = numXforms-1; i>0; i-- ) {
		numToTransform++;							// Number of non-trivial applications of this Householder transformation
		hBase -= hDiagStride;						// Pointer to the beginning of the Householder transformation
		colPtr = colLastPtr;
		for ( long j = numToTransform-1; j>0; j-- ) {
			// Get dot product
			double dotProd2N = -2.0*DotArray( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1 );
			*colPtr = dotProd2N*(*hBase);			// Adding onto zero at initial point
			AddArrayScale( numToTransform-1, hBase+colStride, colStride, colPtr+1, 1, dotProd2N ); 
			colPtr -= NumRows;
		}
		// Do last one as a special case (may overwrite the Householder vector)
		CopyArrayScale( numToTransform, hBase, colStride, colPtr, 1, -2.0*(*hBase) );
		*colPtr += 1.0;				// Add back one one as identity
		// Done with this Householder transformation
		colLastPtr --;
	}    

	if ( numZerosSkipped!=0 ) {
		assert( numZerosSkipped==1 );
		// Fill first row and column with identity (More generally: first numZerosSkipped many rows and columns)
		double* d = x;
		*d = 1;
		double* d2 = d;
		for ( i=NumRows-1; i>0; i-- ) {
			*(++d) = 0;
			*(d2+=NumRows) = 0;
		}
	}
}

// **************** ConvertBidiagToDiagonal ***********************************************
// Do the iterative transformation from bidiagonal form to diagonal form using
//		Givens transformation.  (Golub-Reinsch)
// U and V are square.  Size of U less than or equal to that of U.
void MatrixRmn::ConvertBidiagToDiagonal( MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag ) const
{
	// These two index into the last bidiagonal block  (last in the matrix, it will be
	//	first one handled.
	long lastBidiagIdx = V.NumRows-1;
	long firstBidiagIdx = 0;
	double eps = 1.0e-15 * Max(w.MaxAbs(), superDiag.MaxAbs());

	while ( true ) {
		bool workLeft = UpdateBidiagIndices( &firstBidiagIdx, &lastBidiagIdx, w, superDiag, eps );
		if ( !workLeft ) {
			break;
		}

		// Get ready for first Givens rotation
		// Push non-zero to M[2,1] with Givens transformation
		double* wPtr = w.x+firstBidiagIdx;
		double* sdPtr = superDiag.x+firstBidiagIdx;
		double extraOffDiag=0.0;
		if ( (*wPtr)==0.0 ) {
			ClearRowWithDiagonalZero( firstBidiagIdx, lastBidiagIdx, U, wPtr, sdPtr, eps );
			if ( firstBidiagIdx>0 ) {
				if ( NearZero( *(--sdPtr), eps ) ) {
					*sdPtr = 0.0;
				}
				else {
					ClearColumnWithDiagonalZero( firstBidiagIdx, V, wPtr, sdPtr, eps );
				}
			}
			continue;
		}

		// Estimate an eigenvalue from bottom four entries of M
		// This gives a lambda value which will shift the Givens rotations
		// Last four entries of M^T * M are  ( ( A, B ), ( B, C ) ).
		double A;
		A = (firstBidiagIdx<lastBidiagIdx-1) ? Square(superDiag[lastBidiagIdx-2]): 0.0;
		double BSq = Square(w[lastBidiagIdx-1]);
		A += BSq;										// The "A" entry of M^T * M
		double C = Square(superDiag[lastBidiagIdx-1]);	
		BSq *= C;										// The squared "B" entry
		C += Square(w[lastBidiagIdx]);					// The "C" entry
		double lambda;									// lambda will hold the estimated eigenvalue
		lambda = sqrt( Square((A-C)*0.5) + BSq );		// Use the lambda value that is closest to C.
		if ( A > C ) {
			lambda = -lambda;
		}
		lambda += (A+C)*0.5;						// Now lambda equals the estimate for the last eigenvalue
		double t11 = Square(w[firstBidiagIdx]);
		double t12 = w[firstBidiagIdx]*superDiag[firstBidiagIdx];

		double c, s;
		CalcGivensValues( t11-lambda, t12, &c, &s );
		ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, wPtr+1 );
		V.PostApplyGivens( c, -s, firstBidiagIdx );
		long i;
		for ( i=firstBidiagIdx; i<lastBidiagIdx-1; i++ ) {
			// Push non-zero from M[i+1,i] to M[i,i+2] 
			CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
			ApplyGivensCBTD( c, s, wPtr, sdPtr, &extraOffDiag, extraOffDiag, wPtr+1, sdPtr+1 );
			U.PostApplyGivens( c, -s, i );
			// Push non-zero from M[i,i+2] to M[1+2,i+1]
			CalcGivensValues( *sdPtr, extraOffDiag, &c, &s );
			ApplyGivensCBTD( c, s, sdPtr, wPtr+1, &extraOffDiag, extraOffDiag, sdPtr+1, wPtr+2 );
			V.PostApplyGivens( c, -s, i+1 );
			wPtr++;
			sdPtr++;
		}
		// Push non-zero value from M[i+1,i] to M[i,i+1] for i==lastBidiagIdx-1
		CalcGivensValues( *wPtr, extraOffDiag, &c, &s );
		ApplyGivensCBTD( c, s, wPtr, &extraOffDiag, sdPtr, wPtr+1 );
		U.PostApplyGivens( c, -s, i );

		// DEBUG
		// DebugCalcBidiagCheck( V, w, superDiag, U );
	}
}

// This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry on the same row.
// We use Givens rotations to "chase" the non-zero entry across the row; when it reaches the last
//	column, it is finally zeroed away.
// wPtr points to the zero entry on the diagonal.  sdPtr points to the non-zero superdiagonal entry on the same row.
void MatrixRmn::ClearRowWithDiagonalZero( long firstBidiagIdx, long lastBidiagIdx, MatrixRmn& U, double *wPtr, double *sdPtr, double eps )
{
	double curSd = *sdPtr;		// Value being chased across the row
	*sdPtr = 0.0;
	long i=firstBidiagIdx+1;
	while (true) { 
		// Rotate row i and row firstBidiagIdx (Givens rotation)
		double c, s;
		CalcGivensValues( *(++wPtr), curSd, &c, &s );
		U.PostApplyGivens( c, -s, i, firstBidiagIdx );
		*wPtr = c*(*wPtr) - s*curSd;
		if ( i==lastBidiagIdx ) {
			break;
		}
		curSd = s*(*(++sdPtr));		// New value pops up one column over to the right
		*sdPtr = c*(*sdPtr);
		i++;
	}
}

// This is called when there is a zero diagonal entry, with a non-zero superdiagonal entry in the same column.
// We use Givens rotations to "chase" the non-zero entry up the column; when it reaches the last
//	column, it is finally zeroed away.
// wPtr points to the zero entry on the diagonal.  sdPtr points to the non-zero superdiagonal entry in the same column.
void MatrixRmn::ClearColumnWithDiagonalZero( long endIdx, MatrixRmn& V, double *wPtr, double *sdPtr, double eps )
{
	double curSd = *sdPtr;		// Value being chased up the column
	*sdPtr = 0.0;
	long i = endIdx-1;
	while ( true ) {
		double c, s;
		CalcGivensValues( *(--wPtr), curSd, &c, &s );
		V.PostApplyGivens( c, -s, i, endIdx );
		*wPtr = c*(*wPtr) - s*curSd;
		if ( i==0 ) {
			break;
		}
		curSd = s*(*(--sdPtr));		// New value pops up one row above
		if ( NearZero( curSd, eps ) ) {
			break;
		}
		*sdPtr = c*(*sdPtr);
		i--;
	}
}



// Matrix A is  ( ( a c ) ( b d ) ), i.e., given in column order.
// Mult's G[c,s]  times  A, replaces A.
void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, double *d )
{
	double temp = *a;
	*a = cosine*(*a) - sine*(*b);
	*b = sine*temp + cosine*(*b);
	temp = *c;
	*c = cosine*(*c) - sine*(*d);
	*d = sine*temp + cosine*(*d);
}

// Now matrix A given in row order, A = ( ( a b c ) ( d e f ) ).
// Return G[c,s] * A, replace A.  d becomes zero, no need to return.
//  Also, it is certain the old *c value is taken to be zero!
void MatrixRmn::ApplyGivensCBTD( double cosine, double sine, double *a, double *b, double *c, 
															 double  d, double *e, double *f )
{
	*a = cosine*(*a) - sine*d;
	double temp = *b;
	*b = cosine*(*b) - sine*(*e);
	*e = sine*temp + cosine*(*e);
	*c =  -sine*(*f);
	*f =  cosine*(*f);
}

// Helper routine for SVD conversion from bidiagonal to diagonal
bool MatrixRmn::UpdateBidiagIndices( long *firstBidiagIdx, long *lastBidiagIdx, VectorRn& w, VectorRn& superDiag, double eps )
{
	long lastIdx = *lastBidiagIdx;
	double* sdPtr = superDiag.GetPtr( lastIdx-1 );		// Entry above the last diagonal entry
	while ( NearZero(*sdPtr, eps) ) {				
		*(sdPtr--) = 0.0;
		lastIdx--;
		if ( lastIdx == 0 ) {
			return false;
		}
	}
	*lastBidiagIdx = lastIdx;
	long firstIdx = lastIdx-1;
	double* wPtr = w.GetPtr( firstIdx );
	while ( firstIdx > 0 ) {
		if ( NearZero( *wPtr, eps ) ) {			// If this diagonal entry (near) zero
			*wPtr = 0.0;
			break;
		}
		if ( NearZero(*(--sdPtr), eps) ) {		// If the entry above the diagonal entry is (near) zero
			*sdPtr = 0.0;
			break;
		}
		wPtr--;
		firstIdx--;
	}
	*firstBidiagIdx = firstIdx;
	return true;
}


// ******************************************DEBUG STUFFF

bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const
{
	// Special SVD test code

	MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
	IV.SetIdentity();
	MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
	MatrixRmn::TransposeMultiply( V, V, VTV );
	IV -= VTV;
	double error = IV.FrobeniusNorm();

	MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
	IU.SetIdentity();
	MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
	MatrixRmn::TransposeMultiply( U, U, UTU );
	IU -= UTU;
	error += IU.FrobeniusNorm();

	MatrixRmn Diag( U.GetNumRows(), V.GetNumRows() );
	Diag.SetZero();
	Diag.SetDiagonalEntries( w );
	MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
	MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
	MatrixRmn::Multiply( U, Diag, B );
	MatrixRmn::MultiplyTranspose( B, V, C );
	C -= *this;
	error += C.FrobeniusNorm();

	bool ret = ( fabs(error)<=1.0e-13*w.MaxAbs() );
	assert ( ret );
	return ret;
}

bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
{
	// Special SVD test code

	MatrixRmn IV( V.GetNumRows(), V.GetNumColumns() );
	IV.SetIdentity();
	MatrixRmn VTV( V.GetNumRows(), V.GetNumColumns() );
	MatrixRmn::TransposeMultiply( V, V, VTV );
	IV -= VTV;
	double error = IV.FrobeniusNorm();

	MatrixRmn IU( U.GetNumRows(), U.GetNumColumns() );
	IU.SetIdentity();
	MatrixRmn UTU( U.GetNumRows(), U.GetNumColumns() );
	MatrixRmn::TransposeMultiply( U, U, UTU );
	IU -= UTU;
	error += IU.FrobeniusNorm();

	MatrixRmn DiagAndSuper( U.GetNumRows(), V.GetNumRows() );
	DiagAndSuper.SetZero();
	DiagAndSuper.SetDiagonalEntries( w );
	if ( this->GetNumRows()>=this->GetNumColumns() ) {
		DiagAndSuper.SetSequence( superDiag, 0, 1, 1, 1 );
	}
	else {
		DiagAndSuper.SetSequence( superDiag, 1, 0, 1, 1 );
	}
	MatrixRmn B(U.GetNumRows(), V.GetNumRows() );
	MatrixRmn C(U.GetNumRows(), V.GetNumRows() );
	MatrixRmn::Multiply( U, DiagAndSuper, B );
	MatrixRmn::MultiplyTranspose( B, V, C );
	C -= *this;
	error += C.FrobeniusNorm();

	bool ret = ( fabs(error)<1.0e-13*Max(w.MaxAbs(),superDiag.MaxAbs()) );
	assert ( ret );
	return ret;
}