}RowMatrix::value_type UdUfactor (RowMatrix& UD, const SymMatrix& M)/* * Modified upper triangular Cholesky factor of a * Positive definate or Semi-definate Matrix M * Wraps UdUfactor for non in place factorisation * Output: *    UD the UdU' factorisation of M with strict lower triangle zero * Return: *    see in-place UdUfactor */{	noalias(UD) = M;	RowMatrix::value_type rcond = UdUfactor (UD, M.size1());	Lzero (UD);	// Zero lower triangle ignored by UdUfactor	return rcond;}LTriMatrix::value_type LdLfactor (LTriMatrix& LD, const SymMatrix& M)/* * Modified lower triangular Cholesky factor of a * Positive definate or Semi-definate Matrix M * Wraps LdLfactor for non in place factorisation * Output: *    LD the LdL' factorisation of M * Return: *    see in-place LdLfactor */{	noalias(LD) = M;	LTriMatrix::value_type rcond = LdLfactor (LD, M.size1());	return rcond;}UTriMatrix::value_type UCfactor (UTriMatrix& UC, const SymMatrix& M)/* * Upper triangular Cholesky factor of a * Positive definate or Semi-definate Matrix M * Wraps UCfactor for non in place factorisation * Output: *    UC the UC*UC' factorisation of M * Return: *    see in-place UCfactor */{	noalias(UC) = UpperTri(M);	UTriMatrix::value_type rcond = UCfactor (UC, UC.size1());	return rcond;}bool UdUinverse (RowMatrix& UD)/* * In-place (destructive) inversion of diagonal and unit upper triangular matrices in UD * BE VERY CAREFUL THIS IS NOT THE INVERSE OF UD *  Inversion on d and U is seperate: inv(U)*inv(d)*inv(U') = inv(U'dU) NOT EQUAL inv(UdU') * Lower triangle of UD is ignored and unmodified * Only diagonal part d can be singular (zero elements), inverse is computed of all elements other then singular * Reference: A+G p.223 * * Output: *    UD: inv(U), inv(d) * Return: *    singularity (of d), true iff d has a zero element */{	std::size_t i,j,k;	const std::size_t n = UD.size1();	assert (n == UD.size2());	// Invert U in place	if (n > 1)	{		i = n-2;		do {			RowMatrix::Row UDi(UD,i);			for (j = n-1; j > i; --j)			{				RowMatrix::value_type UDij = - UDi[j];				for (k = i+1; k < j; ++k)					UDij -= UDi[k] * UD(k,j);				UDi[j] = UDij;			}		} while (i-- > 0);	}	// Invert d in place	bool singular = false;	for (i = 0; i < n; ++i)	{		// Detect singular element		if (UD(i,i) != 0)			UD(i,i) = Float(1) / UD(i,i);		else			singular = true;	}	return singular;}bool UTinverse (UTriMatrix& U)/* * In-place (destructive) inversion of upper triangular matrix in U * * Output: *    U: inv(U) * Return: *    singularity (of U), true iff diagonal of U has a zero element */{	const std::size_t n = U.size1();	assert (n == U.size2());	bool singular = false;	// Invert U in place	if (n > 0)	{		std::size_t i = n-1;		do {			UTriMatrix::Row Ui(U,i);			UTriMatrix::value_type d = Ui[i];			if (d == 0)			{				singular = true;				break;			}			d = 1/d;			Ui[i] = d;			for (std::size_t j = n-1; j > i; --j)			{				UTriMatrix::value_type e = 0.;				for (std::size_t k = i+1; k <= j; ++k)					e -= Ui[k] * U(k,j);				Ui[j] = e*d;			}		} while (i-- > 0);	}	return singular;}void UdUrecompose_transpose (RowMatrix& M)/* * In-place recomposition of Symmetric matrix from U'dU factor store in UD format *  Generally used for recomposing result of UdUinverse * Note definateness of result depends purely on diagonal(M) *  i.e. if d is positive definate (>0) then result is positive definate * Reference: A+G p.223 * In place computation uses simple structure of solution due to triangular zero elements *  Defn: R = (U' d) row i , C = U column j   -> M(i,j) = R dot C; *  However M(i,j) only dependant R(k<=i), C(k<=j) due to zeros *  Therefore in place multiple sequences such k < i <= j * Input: *    M - U'dU factorisation (UD format) * Output: *    M - U'dU recomposition (symmetric) */{	std::size_t i,j,k;	const std::size_t n = M.size1();	assert (n == M.size2());	// Recompose M = (U'dU) in place	if (n > 0)	{		i = n-1;		do {			RowMatrix::Row Mi(M,i);			// (U' d) row i of lower triangle from upper trinagle			for (j = 0; j < i; ++j)				Mi[j] = M(j,i) * M(j,j);			// (U' d) U in place			j = n-1;			do { // j>=i				// Compute matrix product (U'd) row i * U col j				RowMatrix::value_type Mij = Mi[j];				if (j > i)					// Optimised handling of 1 in U					Mij *= Mi[i];				for (k = 0; k < i; ++k)		// Inner loop k < i <=j, only strict triangular elements					Mij += Mi[k] * M(k,j);		// M(i,k) element of U'd, M(k,j) element of U				M(j,i) = Mi[j] = Mij;			} while (j-- > i);		} while (i-- > 0);	}}void UdUrecompose (RowMatrix& M)/* * In-place recomposition of Symmetric matrix from UdU' factor store in UD format *  See UdUrecompose_transpose() * Input: *    M - UdU' factorisation (UD format) * Output: *    M - UdU' recomposition (symmetric) */{	std::size_t i,j,k;	const std::size_t n = M.size1();	assert (n == M.size2());	// Recompose M = (UdU') in place	for (i = 0; i < n; ++i)	{		RowMatrix::Row Mi(M,i);		// (d U') col i of lower triangle from upper trinagle		for (j = i+1; j < n; ++j) {			RowMatrix::Row Mj(M,j);			Mj[i] = M(i,j) * Mj[j];		}		// U (d U') in place		for (j = 0; j <= i; ++j)	// j<=i		{			// Compute matrix product (U'd) row i * U col j			RowMatrix::value_type Mij = Mi[j];			if (j > i)					// Optimised handling of 1 in U				Mij *= Mi[i];			for (k = i+1; k < n; ++k)		// Inner loop k > i >=j, only strict triangular elements				Mij += Mi[k] * M(k,j);		// M(i,k) element of U'd, M(k,j) element of U			M(j,i) = Mi[j] = Mij;		}	}}void Lzero (RowMatrix& M)/* * Zero strict lower triangle of Matrix */{	std::size_t i,j;	const std::size_t n = M.size1();	assert (n == M.size2());	for (i = 1; i < n; ++i)	{		RowMatrix::Row Ui(M,i);		for (j = 0; j < i; ++j)		{			Ui[j] = 0;		}	}}void Uzero (RowMatrix& M)/* * Zero strict upper triangle of Matrix */{	std::size_t i,j;	const std::size_t n = M.size1();	assert (n == M.size2());	for (i = 0; i < n; ++i)	{		RowMatrix::Row Li(M,i);		for (j = i+1; j < n; ++j)		{			Li[j] = 0;		}	}}void UdUfromUCholesky (RowMatrix& U)/* * Convert a normal upper triangular Cholesky factor into * a Modified Cholesky factor. * Lower triangle of UD is ignored and unmodified * Ignores Columns with zero diagonal element *  Correct for zero columns i.e. UD is Cholesky factor of a PSD Matrix * Note: There is no inverse to this function toCholesky as square losses the sign * * Input: *    U Normal Cholesky factor (Upper triangular) * Output: *    U Modified Cholesky factor (UD format) */{	std::size_t i,j;	const std::size_t n = U.size1();	assert (n == U.size2());	for (j = 0; j < n; ++j)	{		RowMatrix::value_type sd = U(j,j);		U(j,j) = sd*sd;					// Devide columns by square of non zero diagonal		if (sd != 0)		{			for (i = 0; i < j; ++i)			{				U(i,j) /= sd;			}		}	}}void UdUseperate (RowMatrix& U, Vec& d, const RowMatrix& UD)/* * Extract the seperate U and d parts of the UD factorisation * Output: *    U and d parts of UD */{	std::size_t i,j;	const std::size_t n = UD.size1();	assert (n == UD.size2());	for (j = 0; j < n; ++j)	{					// Extract d and set diagonal to 1		d[j] = UD(j,j);		RowMatrix::Row Uj(U,j);		Uj[j] = 1;		for (i = 0; i < j; ++i)		{			U(i,j) = UD(i,j);			// Zero lower triangle of U			Uj[i] = 0;		}	}}/* * Function built using UdU factorisation */void UdUrecompose (SymMatrix& X, const RowMatrix& M){						// Abuse X as a RowMatrix	RowMatrix& X_matrix = X.asRowMatrix();		// assign elements of common top left block of R into L	std::size_t top = std::min(X_matrix.size1(), M.size1());	std::size_t left = std::min(X_matrix.size2(), M.size2());	X_matrix.sub_matrix(0,top, 0,left) .assign (M.sub_matrix(0,top, 0,left));	UdUrecompose (X_matrix);}SymMatrix::value_type UdUinversePDignoreInfinity (SymMatrix& M)/* * inverse of Positive Definate matrix * The inverse is able to deal with infinities on the leading diagonal * Input: *     M is a symmetric matrix * Output: *     M inverse of M, only updated if return value >0 * Return: *     reciprocal condition number, -1 if negative, 0 if semi-definate (including zero) */{					// Abuse as a RowMatrix	RowMatrix& M_matrix = M.asRowMatrix();					// Must use variant1, variant2 cannot deal with infinity	SymMatrix::value_type orig_rcond = UdUfactor_variant1 (M_matrix, M_matrix.size1());					// Ignore the normal rcond and recompute ingnoring infinites	SymMatrix::value_type rcond = rcond_ignore_infinity_internal (diag(M_matrix));	assert (rcond == orig_rcond || orig_rcond == 0); (void)orig_rcond;	// Only invert and recompose if PD	if (rcond > 0) {		bool singular = UdUinverse (M_matrix);		assert (!singular); (void)singular;		UdUrecompose_transpose (M_matrix);	}	return rcond;}SymMatrix::value_type UdUinversePD (SymMatrix& M)/* * inverse of Positive Definate matrix * Input: *     M is a symmetric matrix * Output: *     M inverse of M, only updated if return value >0 * Return: *     reciprocal condition number, -1 if negative, 0 if semi-definate (including zero) */{					// Abuse as a RowMatrix	RowMatrix& M_matrix = M.asRowMatrix();	SymMatrix::value_type rcond = UdUfactor (M_matrix, M_matrix.size1());	// Only invert and recompose if PD	if (rcond > 0) {		bool singular = UdUinverse (M_matrix);		assert (!singular); (void)singular;		UdUrecompose_transpose (M_matrix);	}	return rcond;}SymMatrix::value_type UdUinversePD (SymMatrix& M, SymMatrix::value_type& detM)/* * As above but also computes determinant of original M if M is PSD */{					// Abuse as a RowMatrix	RowMatrix& M_matrix = M.asRowMatrix();	SymMatrix::value_type rcond = UdUfactor (M_matrix, M_matrix.size1());	// Only invert and recompose if PD	if (rcond > 0) {		detM = UdUdet(M_matrix);		bool singular = UdUinverse (M_matrix);		assert (!singular); (void)singular;		UdUrecompose_transpose (M_matrix);	}	return rcond;}SymMatrix::value_type UdUinversePD (SymMatrix& MI, const SymMatrix& M)/* * inverse of Positive Definate matrix * Input: *    M is a symmetric matrix * Output: *    MI inverse of M, only valid if return value >0 * Return: *    reciprocal condition number, -1 if negative, 0 if semi-definate (including zero) */{	MI = M;					// Abuse as a RowMatrix	RowMatrix& MI_matrix = MI.asRowMatrix();	SymMatrix::value_type rcond = UdUfactor (MI_matrix, MI_matrix.size1());	// Only invert and recompose if PD	if (rcond > 0) {		bool singular = UdUinverse (MI_matrix);		assert (!singular); (void)singular;		UdUrecompose_transpose (MI_matrix);	}	return rcond;}SymMatrix::value_type UdUinversePD (SymMatrix& MI, SymMatrix::value_type& detM, const SymMatrix& M)/* * As above but also computes determinant of original M if M is PSD */{	MI = M;					// Abuse as a RowMatrix	RowMatrix& MI_matrix = MI.asRowMatrix();	SymMatrix::value_type rcond = UdUfactor (MI_matrix, MI_matrix.size1());	if (rcond >= 0) {		detM = UdUdet (MI_matrix);					// Only invert and recompose if PD		if (rcond > 0) {			detM = UdUdet (MI_matrix);			bool singular = UdUinverse (MI_matrix);			assert (!singular); (void)singular;			UdUrecompose_transpose (MI_matrix);		}	}	return rcond;}}//namespace