/***************************************************************
 * PrMatrix3.h
 *
 * This class provides a 3x3 matrix.
 *
 ****************************************************************/

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

#ifndef _PrMatrix3_h
#define _PrMatrix3_h

#include "PrGlobalDefn.h"
#include <assert.h>
#include "PrVector3.h"

class PrMatrix3
{
  public:
    PrMatrix3(); // initialized to [0]
    PrMatrix3(const PrMatrix3 &m);
    PrMatrix3(Float x1,Float x2,Float x3,
	      Float x4,Float x5,Float x6,
	      Float x7,Float x8,Float x9);
    ~PrMatrix3() {}

    PrMatrix3 &operator=(const PrMatrix3 &m);
    Float *operator[](int row);
    const Float *operator[](int row) const;

    Float &elementAt(int i,int j);
    const Float &elementAt(int i,int j) const;

    inline void zero();

    PrMatrix3 operator-() const;
    void negate(PrMatrix3 &dest) const;

    // dPhi = R - Rd
    void angularError(const PrMatrix3 &Rd,PrVector3 &dPhi) const;

    PrMatrix3 operator~() const;
    void inverse(PrMatrix3 &dest) const;
    //(symmetric and positive definite)
    void inverseSPD(PrMatrix3 &dest) const;

    // y = (*this) x
    // solve for x given y
    void solve(const PrVector3 &y,PrVector3 &x) const;
    //(symmetric and positive definite)
    void solveSPD(const PrVector3 &y,PrVector3 &x) const;

    PrMatrix3 operator+(const PrMatrix3 &m) const;
    void add(const PrMatrix3 &m,PrMatrix3 &dest) const;

    PrMatrix3 operator-(const PrMatrix3 &m) const;
    void subtract(const PrMatrix3 &m,PrMatrix3 &dest) const;

    PrMatrix3 operator*(const PrMatrix3 &m) const;
    void multiply(const PrMatrix3 &m,PrMatrix3 &dest) const;

    PrVector3 operator*(const PrVector3 &v) const;
    void multiply(const PrVector3 &v,PrVector3 &dest) const;

    PrMatrix3 operator*(Float s) const;
    void multiply(Float s,PrMatrix3 &dest) const;

    PrMatrix3 &operator+=(const PrMatrix3 &m);
    PrMatrix3 &operator-=(const PrMatrix3 &m);
    PrMatrix3 &operator*=(const PrMatrix3 &m);
    PrMatrix3 &operator*=(Float s);

    PrMatrix3 transpose() const;
    void transpose(PrMatrix3 &dest) const;

    void diagonal(Float x,Float y,Float z); // set the diagonal
    PrVector3 diagonal() const;
    void diagonal(PrVector3 &dest) const;

    void display(char *name=NULL);

  private:
    Float data_[9];

    void LUdecomp(PrMatrix3 &lu) const;
    //(symmetric and positive definite)
    void LUdecompSPD(PrMatrix3 &lu) const;

    // y = LU x
    // find x given LU and y 
    void BackSub(const PrMatrix3 &LU,const PrVector3 &y,PrVector3 &x) const;
    //(symmetric and positive definite)
    void BackSubSPD(const PrMatrix3 &LU,const PrVector3 &y,PrVector3 &x) const;
};

// inline methods

inline PrMatrix3::PrMatrix3()
{
    zero();
}

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

inline PrMatrix3::PrMatrix3(Float x1,Float x2,Float x3,
			    Float x4,Float x5,Float x6,
			    Float x7,Float x8,Float x9)
{
  data_[0] = x1;  data_[1] = x2;  data_[2] = x3;
  data_[3] = x4;  data_[4] = x5;  data_[5] = x6;
  data_[6] = x7;  data_[7] = x8;  data_[8] = x9;
}

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

inline void PrMatrix3::zero()
{
    for(int i=0;i<9;i++)
      data_[i]=0.0;
}

inline Float *PrMatrix3::operator[](int row)
{
    assert(row>=0 && row<3);
    return (data_+row*3);
}

inline const Float *PrMatrix3::operator[](int row) const
{
    assert(row>=0 && row<3);
    return (data_+row*3);
}

inline Float &PrMatrix3::elementAt(int i,int j)
{
    assert(i>=0 && i<3 && j>=0 && j<3);
    return data_[i*3+j];
}

inline const Float &PrMatrix3::elementAt(int i,int j) const
{
    assert(i>=0 && i<3 && j>=0 && j<3);
    return data_[i*3+j];
}
#endif // _PrMatrix3_h
