/***************************************************************
 * PrMatrix6.cpp
 *
 * This implements a 3x3 matrix class.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/20/97: K.C. Chang: added inline methods.
 * 11/05/97: K.C. Chang: created.
 */

#include "PrMatrix6.h"
#include "PrVector3.h"
#include <stdio.h>

PrMatrix6 PrMatrix6::operator-() const
{
    PrMatrix6 tmpM;
    negate(tmpM);
    return tmpM;
}

void PrMatrix6::negate(PrMatrix6 &dest) const
{
    mat3_[0].negate(dest.mat3_[0]);
    mat3_[1].negate(dest.mat3_[1]);
    mat3_[2].negate(dest.mat3_[2]);
    mat3_[3].negate(dest.mat3_[3]);
}

PrMatrix6 PrMatrix6::operator+(const PrMatrix6 &m) const
{
    PrMatrix6 tmpM;
    add(m,tmpM);
    return tmpM;
}

void PrMatrix6::add(const PrMatrix6 &m,PrMatrix6 &dest) const
{
    mat3_[0].add(m.mat3_[0],dest.mat3_[0]);
    mat3_[1].add(m.mat3_[1],dest.mat3_[1]);
    mat3_[2].add(m.mat3_[2],dest.mat3_[2]);
    mat3_[3].add(m.mat3_[3],dest.mat3_[3]);
}

PrMatrix6 PrMatrix6::operator-(const PrMatrix6 &m) const
{
    PrMatrix6 tmpM;
    subtract(m,tmpM);
    return tmpM;
}

void PrMatrix6::subtract(const PrMatrix6 &m,PrMatrix6 &dest) const
{
    mat3_[0].subtract(m.mat3_[0],dest.mat3_[0]);
    mat3_[1].subtract(m.mat3_[1],dest.mat3_[1]);
    mat3_[2].subtract(m.mat3_[2],dest.mat3_[2]);
    mat3_[3].subtract(m.mat3_[3],dest.mat3_[3]);
}

// (*this) * m
// [A0 A1;A2 A3]*[B0 B1;B2 B3] = [A0*B0+A1*B2 A0*B1+A1*B3;
//                                A2*B0+A3*B2 A2*B1+A3*B3]
PrMatrix6 PrMatrix6::operator*(const PrMatrix6 &m) const
{
    PrMatrix6 tmpM;
    multiply(m,tmpM);
    return tmpM;
}

void PrMatrix6::multiply(const PrMatrix6 &m,PrMatrix6 &dest) const
{
    PrMatrix3 tmpM3;

    mat3_[0].multiply(m.mat3_[0],dest.mat3_[0]);
    mat3_[1].multiply(m.mat3_[2],tmpM3);
    dest.mat3_[0] += tmpM3;

    mat3_[0].multiply(m.mat3_[1],dest.mat3_[1]);
    mat3_[1].multiply(m.mat3_[3],tmpM3);
    dest.mat3_[1] += tmpM3;
    
    mat3_[2].multiply(m.mat3_[0],dest.mat3_[2]);
    mat3_[3].multiply(m.mat3_[2],tmpM3);
    dest.mat3_[2] += tmpM3;

    mat3_[2].multiply(m.mat3_[1],dest.mat3_[3]);
    mat3_[3].multiply(m.mat3_[3],tmpM3);
    dest.mat3_[3] += tmpM3;
}

// (*this) * v
PrVector6 PrMatrix6::operator*(const PrVector6 &v) const
{
    PrVector6 tmpV;
    multiply(v,tmpV);
    return tmpV;
}

void PrMatrix6::multiply(const PrVector6 &v,PrVector6 &dest) const
{
    PrVector3 tmpV3;

    mat3_[0].multiply(v[0],dest[0]);
    mat3_[1].multiply(v[1],tmpV3);
    dest[0] += tmpV3;
		   
    mat3_[2].multiply(v[0],dest[1]);
    mat3_[3].multiply(v[1],tmpV3);
    dest[1] += tmpV3;
}

PrMatrix6 PrMatrix6::operator*(Float s) const
{
    PrMatrix6 tmpM;
    multiply(s,tmpM);
    return tmpM;
}

void PrMatrix6::multiply(Float s,PrMatrix6 &dest) const
{
    mat3_[0].multiply(s,dest.mat3_[0]);
    mat3_[1].multiply(s,dest.mat3_[1]);
    mat3_[2].multiply(s,dest.mat3_[2]);
    mat3_[3].multiply(s,dest.mat3_[3]);
}

PrMatrix6 &PrMatrix6::operator+=(const PrMatrix6 &m)
{
    mat3_[0] += m.mat3_[0];
    mat3_[1] += m.mat3_[1];
    mat3_[2] += m.mat3_[2];
    mat3_[3] += m.mat3_[3];
    return (*this);
}

PrMatrix6 &PrMatrix6::operator-=(const PrMatrix6 &m)
{
    mat3_[0] -= m.mat3_[0];
    mat3_[1] -= m.mat3_[1];
    mat3_[2] -= m.mat3_[2];
    mat3_[3] -= m.mat3_[3];
    return (*this);
}

// (*this) * m
PrMatrix6 &PrMatrix6::operator*=(const PrMatrix6 &m)
{
    PrMatrix6 tmpM;
    multiply(m,tmpM);
    (*this) = tmpM;
    return (*this);
}

PrMatrix6 &PrMatrix6::operator*=(Float s)
{
    mat3_[0] *= s;
    mat3_[1] *= s;
    mat3_[2] *= s;
    mat3_[3] *= s;
    return (*this);
}

// [m0 m1;m2 m3]^T = [m0^T m2^T; m1^T m3^T]
PrMatrix6 PrMatrix6::transpose() const
{
    PrMatrix6 tmpM;
    transpose(tmpM);
    return tmpM;
}

void PrMatrix6::transpose(PrMatrix6 &dest) const
{
    mat3_[0].transpose(dest.mat3_[0]);
    mat3_[2].transpose(dest.mat3_[1]);
    mat3_[1].transpose(dest.mat3_[2]);
    mat3_[3].transpose(dest.mat3_[3]);
}

// set the diagonal
void PrMatrix6::diagonal(const PrVector3 &v1,const PrVector3 &v2)
{
    mat3_[0].diagonal(v1[0],v1[1],v1[2]);
    mat3_[3].diagonal(v2[0],v2[1],v2[2]);
}

PrVector6 PrMatrix6::diagonal() const
{
    PrVector6 tmpV;
    diagonal(tmpV);
    return tmpV;
}

void PrMatrix6::diagonal(PrVector6 &dest) const
{
    mat3_[0].diagonal(dest[0]);
    mat3_[3].diagonal(dest[1]);
}

void PrMatrix6::display(char *name)
{
  if (name != NULL)
    printf("%s =\n",name);

  printf("[");
  for(int i=0;i<6;i++)
    {
      for(int j=0;j<6;j++)
	{
	  printf("%f",(*this).elementAt(i,j));
	  if (j<5)
	    printf(" ");
	}
      if (i<5)
	printf(";\n");
    }
  printf("]\n");

  /*
    mat3_[0].display("M6(0,0)");
    mat3_[1].display("M6(0,1)");
    mat3_[2].display("M6(1,0)");
    mat3_[3].display("M6(1,1)");
  */
}

PrMatrix6 PrMatrix6::operator~() const
{
    PrMatrix6 tmpM;
    inverse(tmpM);
    return tmpM;
}

// y = (*this) x
// solve for x given y
void PrMatrix6::solve(const PrVector6 &y,PrVector6 &x) const
{
  PrMatrix6 lu;

  LUdecomp(lu);
  BackSub(lu,y,x);
}

// y = (*this) x
// solve for x given y
//(symmetric and positive definite)
void PrMatrix6::solveSPD(const PrVector6 &y,PrVector6 &x) const
{
  PrMatrix6 lu;

  LUdecompSPD(lu);
  BackSubSPD(lu,y,x);
}

/* Matrix Inverse by Crout's LU decomposition */
/* p275-285, Numerical Methods for Engineers by Chapra and Canale */

/* output: ainv */
void PrMatrix6::inverse(PrMatrix6 &ainv) const
{
  register int i,k,n=6;
  PrMatrix6 lu;
  PrVector6 x,y;

  // LU ainv = I
  // a x = y
  // LU x = y

  LUdecomp(lu);

  for(k=0;k<n;k++)
    {
      y.zero();
      y.elementAt(k) = 1.0;

      BackSub(lu,y,x);
      for(i=0;i<n;i++)
	ainv.elementAt(i,k) = x.elementAt(i);
    }
}

/*
 * Private Function: LUdecomp
 */
// y = LU x
// find x given LU and y 
// diag(U) = [1 1 1 ... 1]
void PrMatrix6::BackSub(const PrMatrix6 &lu,const PrVector6 &y,PrVector6 &x) const
{
  /* LU x = y ----> x */
  /* L d  = y --> fw sub */
  /* U x = d ---> bw sub */
  register int i,j,n=6;
  Float sum;
  Float d[6];

  d[0] = y.elementAt(0)/lu.elementAt(0,0);   /* L */
  for(i=1;i<n;i++)
    {
      sum = 0.0;
      for(j=0;j<i;j++)
	sum += lu.elementAt(i,j)*d[j]; /* L */
      d[i] = (y.elementAt(i)-sum)/lu.elementAt(i,i);
    }
  x.elementAt(n-1) = d[n-1];
  for(i=n-2;i>=0;i--)
    {
      sum = 0.0;
      for(j=i+1;j<n;j++)
	sum += lu.elementAt(i,j)*x.elementAt(j); /* U */
      x.elementAt(i) = d[i] - sum;
    }
}

/*
 * Private Function: LUdecomp
 */
/* output: lu */
void PrMatrix6::LUdecomp(PrMatrix6 &lu) const
{
  Float sum;
  register int i,j,k,n=6;
  const PrMatrix6 &a = *this;

  for(i=0;i<n;i++)
    lu.elementAt(i,0) = a.elementAt(i,0);  /* L */
  for(j=1;j<n;j++)
    lu.elementAt(0,j) = a.elementAt(0,j)/lu.elementAt(0,0);  /* U */
  for(j=1;j<n-1;j++)
    {
      for(i=j;i<n;i++)
	{
	  sum = 0.0;
	  for(k=0;k<j;k++)
	    sum += lu.elementAt(i,k)*lu.elementAt(k,j);
	  lu.elementAt(i,j) = a.elementAt(i,j)-sum;  /* L */
	}
      for(k=j+1;k<n;k++)
	{
	  sum = 0.0;
	  for(i=0;i<j;i++)
	    sum += lu.elementAt(j,i)*lu.elementAt(i,k);
	  lu.elementAt(j,k) = (a.elementAt(j,k)-sum)/lu.elementAt(j,j); /* U */
	}
    }
  sum = 0.0;
  for(k=0;k<n-1;k++)
    sum += lu.elementAt(n-1,k)*lu.elementAt(k,n-1);
  lu.elementAt(n-1,n-1) = a.elementAt(n-1,n-1)-sum;       /* L */
}

/* Matrix Inverse by Cholesky's LU decomposition */
/* p288-290, Numerical Methods for Engineers by Chapra and Canale */

/* output: ainv */
//(symmetric and positive definite)
void PrMatrix6::inverseSPD(PrMatrix6 &ainv) const
{
  register int i,k,n=6;
  PrMatrix6 lu;
  PrVector6 x,y;

  // LU ainv = I
  // a x = y
  // LU x = y

  LUdecompSPD(lu);

  for(k=0;k<n;k++)
    {
      y.zero();
      y.elementAt(k) = 1.0;

      BackSubSPD(lu,y,x);
      for(i=0;i<n;i++)
	ainv.elementAt(i,k) = x.elementAt(i);
    }
}

// y = LU x
// y = LL' x
// find x given LU and y 
//(symmetric and positive definite)
void PrMatrix6::BackSubSPD(const PrMatrix6 &lu,const PrVector6 &y,PrVector6 &x) const
{
  /* U = L' */
  /* LU = LL' */
  /* LU x = y ----> x */
  /* L d  = y --> fw sub */
  /* U x = d ---> bw sub */
  register int i,j,n=6;
  Float sum;
  Float d[6];

  d[0] = y.elementAt(0)/lu.elementAt(0,0);        /* fw: L */
  for(i=1;i<n;i++)
    {
      sum = 0.0;
      for(j=0;j<i;j++)
	sum += lu.elementAt(i,j)*d[j];
      d[i] = (y.elementAt(i) - sum)/lu.elementAt(i,i);
    }
  x.elementAt(n-1) = d[n-1]/lu.elementAt(n-1,n-1);        /* bw: U */
  for(i=n-2;i>=0;i--)
    {
      sum = 0.0;
      for(j=i+1;j<n;j++)
	sum += lu.elementAt(i,j)*x.elementAt(j);
      x.elementAt(i) = (d[i] - sum)/lu.elementAt(i,i);
    }
}

//(symmetric and positive definite)
void PrMatrix6::LUdecompSPD(PrMatrix6 &lu) const
{
  Float sum;
  register int i,j,k,n=6;
  const PrMatrix6 &a = *this;

  /* U = L' */
  for (k=0;k<n;k++)
    {
      for (i=0;i<k;i++)
	{
	  sum = 0.0;
	  for (j=0;j<i;j++)
	    sum += lu.elementAt(i,j) * lu.elementAt(k,j);
	  lu.elementAt(i,k) = lu.elementAt(k,i) 
	    = (a.elementAt(k,i)-sum)/lu.elementAt(i,i);
	}
      sum = 0.0;
      for (j=0;j<k;j++)
	sum += lu.elementAt(k,j)*lu.elementAt(k,j);
      lu.elementAt(k,k) = sqrt(a.elementAt(k,k)-sum);
    }
}






