/***************************************************************
 * PrMatrix3.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 "PrMatrix3.h"
#include <stdio.h>

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

void PrMatrix3::negate(PrMatrix3 &dest) const
{
    for(int i=0;i<9;i++)
      dest.data_[i] = -data_[i];
}

PrMatrix3 PrMatrix3::operator+(const PrMatrix3 &m) const
{
    return PrMatrix3(*this) += m;
}

void PrMatrix3::add(const PrMatrix3 &m,PrMatrix3 &dest) const
{
    for(int i=0;i<9;i++)
      dest.data_[i] = data_[i]+m.data_[i];
}

PrMatrix3 PrMatrix3::operator-(const PrMatrix3 &m) const
{
    return PrMatrix3(*this) -= m;
}

void PrMatrix3::subtract(const PrMatrix3 &m,PrMatrix3 &dest) const
{
    for(int i=0;i<9;i++)
      dest.data_[i] = data_[i]-m.data_[i];
}

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

void PrMatrix3::multiply(const PrMatrix3 &m,PrMatrix3 &dest) const
{
    const Float *b = m.data_;
    Float *c = dest.data_;

    // first row
    c[0] = data_[0]*b[0] + data_[1]*b[3] + data_[2]*b[6];
    c[1] = data_[0]*b[1] + data_[1]*b[4] + data_[2]*b[7];
    c[2] = data_[0]*b[2] + data_[1]*b[5] + data_[2]*b[8];
    // second row
    c[3] = data_[3]*b[0] + data_[4]*b[3] + data_[5]*b[6];
    c[4] = data_[3]*b[1] + data_[4]*b[4] + data_[5]*b[7];
    c[5] = data_[3]*b[2] + data_[4]*b[5] + data_[5]*b[8];
    // third row
    c[6] = data_[6]*b[0] + data_[7]*b[3] + data_[8]*b[6];
    c[7] = data_[6]*b[1] + data_[7]*b[4] + data_[8]*b[7];
    c[8] = data_[6]*b[2] + data_[7]*b[5] + data_[8]*b[8];
}

// (*this) * v
PrVector3 PrMatrix3::operator*(const PrVector3 &v) const
{
    return PrVector3(data_[0]*v[0] + data_[1]*v[1] + data_[2]*v[2],
		     data_[3]*v[0] + data_[4]*v[1] + data_[5]*v[2],
		     data_[6]*v[0] + data_[7]*v[1] + data_[8]*v[2]);
}

void PrMatrix3::multiply(const PrVector3 &v,PrVector3 &dest) const
{
    // first row
    dest[0] = data_[0]*v[0] + data_[1]*v[1] + data_[2]*v[2];
    // second row
    dest[1] = data_[3]*v[0] + data_[4]*v[1] + data_[5]*v[2];
    // third row
    dest[2] = data_[6]*v[0] + data_[7]*v[1] + data_[8]*v[2];
}

PrMatrix3 PrMatrix3::operator*(Float s) const
{
    return PrMatrix3(*this) *= s;
}

void PrMatrix3::multiply(Float s,PrMatrix3 &dest) const
{
    for(int i=0;i<9;i++)
      dest.data_[i] = data_[i]*s;
}

PrMatrix3 &PrMatrix3::operator+=(const PrMatrix3 &m)
{
    for(int i=0;i<9;i++)
      data_[i] += m.data_[i];
    return (*this);
}

PrMatrix3 &PrMatrix3::operator-=(const PrMatrix3 &m)
{
    for(int i=0;i<9;i++)
      data_[i] -= m.data_[i];
    return (*this);
}

// (*this) * m
PrMatrix3 &PrMatrix3::operator*=(const PrMatrix3 &m)
{
    const Float *b = m.data_;
    Float c[9];

    // first row
    c[0] = data_[0]*b[0] + data_[1]*b[3] + data_[2]*b[6];
    c[1] = data_[0]*b[1] + data_[1]*b[4] + data_[2]*b[7];
    c[2] = data_[0]*b[2] + data_[1]*b[5] + data_[2]*b[8];
    // second row
    c[3] = data_[3]*b[0] + data_[4]*b[3] + data_[5]*b[6];
    c[4] = data_[3]*b[1] + data_[4]*b[4] + data_[5]*b[7];
    c[5] = data_[3]*b[2] + data_[4]*b[5] + data_[5]*b[8];
    // third row
    c[6] = data_[6]*b[0] + data_[7]*b[3] + data_[8]*b[6];
    c[7] = data_[6]*b[1] + data_[7]*b[4] + data_[8]*b[7];
    c[8] = data_[6]*b[2] + data_[7]*b[5] + data_[8]*b[8];

    for(int i=0;i<9;i++)
      data_[i] = c[i];
    return (*this);
}

PrMatrix3 &PrMatrix3::operator*=(Float s)
{
    for(int i=0;i<9;i++)
      data_[i] *= s;
    return (*this);
}

// [0 1 2;3 4 5;6 7 8]^T = [0 3 6;1 4 7;2 5 8]  
PrMatrix3 PrMatrix3::transpose() const
{
    PrMatrix3 tmpM;
    transpose(tmpM);
    return tmpM;
}

void PrMatrix3::transpose(PrMatrix3 &dest) const
{
    Float *b = dest.data_;
    b[0]=data_[0];
    b[1]=data_[3];
    b[2]=data_[6];
    b[3]=data_[1];
    b[4]=data_[4];
    b[5]=data_[7];
    b[6]=data_[2];
    b[7]=data_[5];
    b[8]=data_[8];
}

void PrMatrix3::diagonal(Float x,Float y,Float z) // set the diagonal
{
    data_[0] = x;
    data_[4] = y;
    data_[8] = z;
}

PrVector3 PrMatrix3::diagonal() const
{
    return PrVector3(data_[0],data_[4],data_[8]);
}

void PrMatrix3::diagonal(PrVector3 &dest) const
{
    dest[0] = data_[0];
    dest[1] = data_[4];
    dest[2] = data_[8];
}

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

    printf("%.6f %.6f %.6f\n",data_[0],data_[1],data_[2]);
    printf("%.6f %.6f %.6f\n",data_[3],data_[4],data_[5]);
    printf("%.6f %.6f %.6f\n",data_[6],data_[7],data_[8]);
}

void PrMatrix3::angularError(const PrMatrix3 &Rd,PrVector3 &dPhi) const
{
  const PrMatrix3 &R = *this;
  dPhi[0] = -0.5*((R[1][0]*Rd[2][0]+R[1][1]*Rd[2][1]+R[1][2]*Rd[2][2])
		  -(R[2][0]*Rd[1][0]+R[2][1]*Rd[1][1]+R[2][2]*Rd[1][2]));
  dPhi[1] = -0.5*((R[2][0]*Rd[0][0]+R[2][1]*Rd[0][1]+R[2][2]*Rd[0][2])
		  -(R[0][0]*Rd[2][0]+R[0][1]*Rd[2][1]+R[0][2]*Rd[2][2]));
  dPhi[2] = -0.5*((R[0][0]*Rd[1][0]+R[0][1]*Rd[1][1]+R[0][2]*Rd[1][2])
		  -(R[1][0]*Rd[0][0]+R[1][1]*Rd[0][1]+R[1][2]*Rd[0][2]));

  /*
  dPhi[0] = -0.5*((T[0][1]*Td[0][2]+T[1][1]*Td[1][2]+T[2][1]*Td[2][2])
		  -(T[0][2]*Td[0][1]+T[1][2]*Td[1][1]+T[2][2]*Td[2][1]));
  dPhi[1] = -0.5*((T[0][2]*Td[0][0]+T[1][2]*Td[1][0]+T[2][2]*Td[2][0])
		  -(T[0][0]*Td[0][2]+T[1][0]*Td[1][2]+T[2][0]*Td[2][2]));
  dPhi[2] = -0.5*((T[0][0]*Td[0][1]+T[1][0]*Td[1][1]+T[2][0]*Td[2][1])
		  -(T[0][1]*Td[0][0]+T[1][1]*Td[1][0]+T[2][1]*Td[2][0]));

  dPhi[0] = -0.5*((xr[1]*xrd[2]+xr[4]*xrd[5]+xr[7]*xrd[8])
	       -(xr[2]*xrd[1]+xr[5]*xrd[4]+xr[8]*xrd[7]));
  dPhi[1] = -0.5*((xr[2]*xrd[0]+xr[5]*xrd[3]+xr[8]*xrd[6])
	       -(xr[0]*xrd[2]+xr[3]*xrd[5]+xr[6]*xrd[8]));
  dPhi[2] = -0.5*((xr[0]*xrd[1]+xr[3]*xrd[4]+xr[6]*xrd[7])
	       -(xr[1]*xrd[0]+xr[4]*xrd[3]+xr[7]*xrd[6]));
  */
}

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

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

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

// y = (*this) x
// solve for x given y
//(symmetric and positive definite)
void PrMatrix3::solveSPD(const PrVector3 &y,PrVector3 &x) const
{
  PrMatrix3 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 PrMatrix3::inverse(PrMatrix3 &ainv) const
{
  register int i,k,n=3;
  PrMatrix3 lu;
  PrVector3 x,y;

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

  LUdecomp(lu);

  for(k=0;k<n;k++)
    {
      
      for(i=0;i<n;i++)
	y[i] = 0.0;
      y[k] = 1.0;

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

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

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

/* output: lu */
// diag(U) = [1 1 1 ... 1]
void PrMatrix3::LUdecomp(PrMatrix3 &lu) const
{
  Float sum;
  register int i,j,k,n=3;
  const PrMatrix3 &a = *this;

  for(i=0;i<n;i++)
    lu[i][0] = a[i][0];  /* L */
  for(j=1;j<n;j++)
    lu[0][j] = a[0][j]/lu[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[i][k]*lu[k][j];
	  lu[i][j] = a[i][j]-sum;  /* L */
	}
      for(k=j+1;k<n;k++)
	{
	  sum = 0.0;
	  for(i=0;i<j;i++)
	    sum += lu[j][i]*lu[i][k];
	  lu[j][k] = (a[j][k]-sum)/lu[j][j];  /* U */
	}
    }
  sum = 0.0;
  for(k=0;k<n-1;k++)
    sum += lu[n-1][k]*lu[k][n-1];
  lu[n-1][n-1] = a[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 PrMatrix3::inverseSPD(PrMatrix3 &ainv) const
{
  register int i,k,n=3;
  PrMatrix3 lu;
  PrVector3 x,y;

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

  LUdecompSPD(lu);

  for(k=0;k<n;k++)
    {
      y.values(0.0,0.0,0.0);
      y[k] = 1.0;

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

// y = LU x
// y = LL' x
// find x given LU and y 
//(symmetric and positive definite)
void PrMatrix3::BackSubSPD(const PrMatrix3 &LU,const PrVector3 &y,PrVector3 &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=3;
  Float sum;
  Float d[3];

  d[0] = y[0]/LU[0][0];        /* fw: L */
  for(i=1;i<n;i++)
    {
      sum = 0.0;
      for(j=0;j<i;j++)
	sum += LU[i][j]*d[j];
      d[i] = (y[i] - sum)/LU[i][i];
    }
  x[n-1] = d[n-1]/LU[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[i][j]*x[j];
      x[i] = (d[i] - sum)/LU[i][i];
    }
}

//(symmetric and positive definite)
void PrMatrix3::LUdecompSPD(PrMatrix3 &lu) const
{
  Float sum;
  register int i,j,k,n=3;
  const PrMatrix3 &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[i][j] * lu[k][j];
	  lu[i][k] = lu[k][i] = (a[k][i]-sum)/lu[i][i];
	}
      sum = 0.0;
      for (j=0;j<k;j++)
	sum += lu[k][j]*lu[k][j];
      lu[k][k] = sqrt(a[k][k]-sum);
    }
}





