// $Id: CovMatrix3Diagonalize.cc,v 1.6 2002/10/22 17:52:36 sachs Exp $
//
// Compute Eigenvalues and optionally Eigenvectors of a 3x3
//	Positive definite symmetric matrix.
//	Return true if the computation was successful.
//	Return false if the matrix could not be diagonalized.
//
// David Sachs, Fermilab - March 2002
//
// Based on LAPACK routine dspev.f
// 
#include "CovMatrices/CovMatrix3.h"
#include "CovMatrixEigCommon.h"

using namespace CovMatrices::LAPACK;

bool CovMatrices::CovMatrix3::diagonalize ( Vector3& eigenvalues, Vector3 eigenvectors[] ) const
{

  // Compute Eigenvalues and optionally Eigenvectors 
  // by using C++ versions of proper LAPACK routines
  //
  // Arguments are:
  //
  //  Vector3& eigenvalues:	OUTPUT eignvalues
  //
  //  Vector3 eigenvectors[3]:	OUTPUT eigenvectors
  //	Supply a null pointer to compute Eigenvectors only
  //
  // Return value is true if computation was successful
  //
  
  double in[6];		// Copy of original matrixC
  double val[3];	// Temporary to hold Eigenvalues
  double vec[9];	// Temporary to hold Eigenvectors
  double e[2], t[2];	// Temporary for tridagonal form
  
  memcpy(in, &M, sizeof(M));	// Copy input matrix

  // Convert Matrix to TriDiagonal form
  dsptrd(3, in, val, e, t);

  // For eigenvalues only call dsteqr
  // For eigenvectors call dopgtr and dsteqr
  if (!eigenvectors)
  {
    if (dsteqr(3, val, e) != 0) return false;
    eigenvalues = val;
    return true;
  }
  else
  {
    dopgtr( 3, in, t, vec);
    if (dsteqr(3, val, e, vec) !=0) return false;

    // store eigenvector and eigenvalues
    eigenvalues = val;
    eigenvectors[0] = vec;
    eigenvectors[1] = vec+3;
    eigenvectors[2] = vec+6;
    return true;
  }
}