// $Id: CovMatrix3Invert.cc,v 1.7 2002/09/19 16:45:18 sachs Exp $
//
// Invert a positive definite symmetric 3x3 matrix 
// David Sachs, Fermilab - April 2002

#include "CovMatrices/CovMatrix3.h"
#include "CovMatrixElement.h"
#define CRAMER

#ifndef CRAMER
// Invert a positive definite symmetric 3x3 matrix -  using L*D*L^T decomposition
//
// Base on algorithm in Golub & Van Loan "Matrix Computations" 3rd Ed. pp 138-139
// The exact formulas used were determined by hand.
// 
// This method is slightly slower than the direct computation for a 3x3 symmetric
// matrix but allow confirmation of positive definiteness

bool  CovMatrices::CovMatrix3::invert(void)
{

// Invert by 
//
// a) decomposing M = G*D*G^T with G unit lower triangular and D positive diagonal
//	(if M is not positive definite this will fail, leaving this unchanged)
// b) inverting G to form H
// c) multiplying H^T * D^-1 * H to get M^-1.
//
// If the matrix is pos. def. it is inverted and 1 is returned.
// If the matrix is not pos. def. it remains unaltered and 0 is returned.

// NOTE: The LDl^T algoritm should always work for posive definite symmetric matrices.
// It will also work for many, but not all, non-positive-definite symmetric matrice,
// but may be unstable.  In particular it will fail if M[0,0]  is zero, which
// is quite possible for a nonsingular symmetric matrix.

  // double h10;				// below-diagonal elements of H
  // double h20, h21;				// will be defined later
	
  double d00, d11, d22;			// 1/diagonal elements of D = 
					// diagonal elements of D^-1

  double g10;				// below-diagonal elements of G
  double g20, g21;

  double G10;				// UNSCALED below-diagonal elements of G
  double G20, G21;			


// Form G and D -- compute diagonal members of D^-1 directly 
//-------

// Scale first column by 1/M00

  d00 = M[M00]; 
  if (d00 <= 0.) return false;	// Not positive definite
  d00 = 1.0 / d00;

  G10 = M[M10];
  g10 = M[M10] * d00;
  G20 = M[M20];
  g20 = M[M20] * d00;

// Form d11

  d11 = M[M11] - (G10 * g10);
  if (d11 <= 0.) return false;  // Not positive definite
  d11 = 1.0 / d11;

// Subtract inter-column column dot products from rest of column 1 and
// scale to get column 1 of G 

  G21 = (M[M21] - (G10 * g20));
  g21 = G21 * d11;

// Form d22

  d22 = M[M22] - (G20 * g20) - (G21 * g21);
  if (d22 <= 0.) return false;  // Not positive definite
  d22 = 1.0 / d22;

// Form H = 1/G  -- Diagonal elements of G and H are 1.0
//-------------

// Because the unscaled matrix elements  of G are no longer needed
// define the elements of H to occupy the same memory
// Because of their simple forms, h10, h21, h32 and h43 are defined by their formulas
#define h10 (-g10)
#define h20 G20
#define h21 (-g21)


  // h10 = -g10;
  // h21 = -g21;
     h20 =  g10 * g21 - g20;

// Change this to its inverse = H^T * D^-1 * H
//------------------------------------
// NOTE: Common subexpressions are parenthesized to provide proper ordering 
// for optimizing C++ compilers

  M[M00] = d00 + h10 * (h10*d11) + h20 * (h20*d22);
  M[M01] = (h10*d11) + h20 * (h21*d22);
  M[M11] = d11 + h21 * (h21*d22);
  M[M02] = (h20*d22);
  M[M12] = (h21*d22);
  M[M22] = d22;

  return true;

}

#else

// This code was taken from CLHEP/Matrix
bool CovMatrices::CovMatrix3::invert(void)
{
   double det;
   double c00,c10,c20,c11,c21,c22;

   // Determine all cofactors. Becaue of symmetry there are only 6 
   c00 = (M[M11]) * (M[M22]) - (M[M21]) * (M[M21]);
   c10 = (M[M21]) * (M[M20]) - (M[M10]) * (M[M22]);
   c20 = (M[M10]) * (M[M21]) - (M[M11]) * (M[M20]);
   c11 = (M[M22]) * (M[M00]) - (M[M20]) * (M[M20]);
   c21 = (M[M20]) * (M[M10]) - (M[M21]) * (M[M00]);
   c22 = (M[M00]) * (M[M11]) - (M[M10]) * (M[M10]);


   // Determine determinant
   det = M[M00]*c00 + M[M10]*c10 + M[M20]*c20;
   if (det==0.) return false;
   det = 1.0/det;	// 1/determinant
   M[M00] = det*c00;
   M[M10] = det*c10;
   M[M11] = det*c11;
   M[M20] = det*c20;
   M[M21] = det*c21;
   M[M22] = det*c22;
   return true;
}
#endif