/////////////////////////////////////////////////////////////////////
// Polhemus Inc., www.polhemus.com
// © 2002 Polhemus Inc., All Rights Reserved
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
//
//  Filename:           $Workfile: VPmat3x3.h $
//
//  Project Name:       Polhemus Tracker Command Interface API 
//
//  Original Author:    S. Gagnon (adopted from JCarroll)
//
//  Description:        Fast 3x3 Matrix, adopted from OOTRAK:MatrixFast3x3
//
//  VSS $Header: /Viper/VPCmdIF/VPCmdIF/VPmat3x3.h 1     9/22/17 6:00p Suzanne $  
//
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
#ifndef _PMAT3X3_H_
#define _PMAT3X3_H_
//#include "PTypes.h"
//#include "C:\Dev\Projects\G4Plus\Windows\G4PlusCmdLib\PTypes.h"
#include "VPtypesUT.h"
#if defined (PMAT_EXPORTS)
#define PMAT_API __declspec(dllexport)
#else
#define PMAT_API __declspec(dllimport)
#endif

/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
// CLASS CMat3x3
/////////////////////////////////////////////////////////////////////
class CMat3x3
{
public:
    CMat3x3( void ){};
	CMat3x3( float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz )
	{
		m_vX = C3Vec(xx, xy, xz);
		m_vY = C3Vec(yx, yy, yz);
		m_vZ = C3Vec(zx, zy, zz);
	}
	CMat3x3( double xx, double xy, double xz, double yx, double yy, double yz, double zx, double zy, double zz )
	{
		m_vX = C3Vec((float)xx, (float)xy, (float)xz);
		m_vY = C3Vec((float)yx, (float)yy, (float)yz);
		m_vZ = C3Vec((float)zx, (float)zy, (float)zz);
	}

	CMat3x3( const float rv[9] )
	{
		m_vX = &rv[0];
		m_vY = &rv[3];
		m_vZ = &rv[6];
	}

    ~CMat3x3( void ){};

	CMat3x3( const CMat3x3 & rv)
    {
        m_vX = rv.m_vX;
        m_vY = rv.m_vY;
        m_vZ = rv.m_vZ;
    }

    CMat3x3 & operator= (const CMat3x3 & rv )
    { m_vX  = rv.m_vX; 
      m_vY  = rv.m_vY; 
      m_vZ  = rv.m_vZ; 
      return *this;
    }

	CMat3x3 & operator= ( const float rv[9] )
	{
		m_vX = &rv[0];
		m_vY = &rv[3];
		m_vZ = &rv[6];
		return *this;
	}

	const CMat3x3	operator * ( CMat3x3 & rv ) const; // C=AB

    CMat3x3 & operator*= ( float f ) // Scale by f
    { Scale( f ); return *this; }
    CMat3x3 & operator*= ( double f ) // Scale by f
    { Scale( (float)f ); return *this; }

    CMat3x3 & operator*= ( CMat3x3 & rv );	// A = AB
   

    CMat3x3 & operator+= (const CMat3x3 & rv )  // A = A+B
    { m_vX  += rv.m_vX; 
      m_vY  += rv.m_vY; 
      m_vZ  += rv.m_vZ; 
      return *this;
    }

    CMat3x3 & operator-= (const CMat3x3 & rv )
    { m_vX  -= rv.m_vX; 
      m_vY  -= rv.m_vY; 
      m_vZ  -= rv.m_vZ; 
      return *this;
    }

    float * operator[] (int32_t i)
    {
      return ( (i==0) ? m_vX : ((i==1) ? m_vY : ( (i==2) ? m_vZ : m_vX )) ) ;
    }

    //operator float *        () // pointer to 9 floats
    //                        { return (float *)m_vX; }

    bool operator==          ( const CMat3x3 & rv ) const
    { return (m_vX == rv.m_vX) && (m_vY == rv.m_vY) && (m_vZ == rv.m_vZ); }

    bool operator!=          ( const CMat3x3 & rv ) const
    { return !(*this == rv); }


    void    Identity    ( void );
    //bool    Extract     ( CPiStreamBuf & strbuf );
	bool	Extract		( LPCSTR szBuf );
	bool	ExtractT	( LPCTSTR szBuf );
	bool	Extract		( uint8_t * pBuf );
    //bool    Serialize   ( CPiStreamBuf & strbuf ) const;
    //void    ToCmdString ( std::string & s, int32_t w=9, int32_t p=5, CHAR sep=',' ) const;
    void    ToDispString( tstring & s, LPCTSTR szFmt=PI_G_FMT, TCHAR sep=PI_SPACE, TCHAR * rowSep=_T(" ")) const;
    void    Zero        ( void );

    void    Add         ( const CMat3x3 & rv )
						{ m_vX += rv.m_vX;
						  m_vY += rv.m_vY;
						  m_vZ += rv.m_vZ;
						}

    void    Transpose   ( void );
    void    Scale       ( float );
    void    Invert      ( void );

    static size_t   Size ( void )
	{
		return 3 * C3Vec::Size();
	}


    float & xx( void ) { return m_vX.at(0); } 
    float & xy( void ) { return m_vX.at(1); }
    float & xz( void ) { return m_vX.at(2); }
	float & yx(void) { return m_vY.at(0); }
	float & yy(void) { return m_vY.at(1); }
	float & yz(void) { return m_vY.at(2); }
	float & zx(void) { return m_vZ.at(0); }
	float & zy(void) { return m_vZ.at(1); }
	float & zz(void) { return m_vZ.at(2); }

	void	MultiplyBy		( CMat3x3 & rv ); // A = AB
    void	Multiply_T_By	( CMat3x3 & rhs); // A = A^t B


    //  This = This [transpose] * rhs  [transpose]
    // rhs = right hand side
    //void multiplyBy(MatrixFast3x3& rhs);
    //void multiplyByTransposeOf(MatrixFast3x3& rhs);
    //void multiplyTransposeByTransposeOf(MatrixFast3x3& rhs);

    // This = lhs * This [transpose]
    // lhs = left hand side
    //void leftMultiplyBy(MatrixFast3x3& lhs);
    //void leftMultiplyByTransposeOf(MatrixFast3x3& lhs);


 
protected:

	const C3Vec XCol(void)       // Returns X Column vector
	{
		return C3Vec(xx(), yx(), zx());
	}

	const C3Vec YCol(void)       // Returns Y Column vector
	{
		return C3Vec(xy(), yy(), zy());
	}

	const C3Vec ZCol(void)       // Returns Z Column vector
	{
		return C3Vec(xz(), yz(), zz());
	}

    // Row Vectors
    C3Vec m_vX;  
	C3Vec m_vY;
	C3Vec m_vZ;
};

/////////////////////////////////////////////////////////////////////
// CLASS CDirCos
/////////////////////////////////////////////////////////////////////
class CDirCos : public CMat3x3
{

public:
    CDirCos() : CMat3x3() {};

	CDirCos( float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz )
		: CMat3x3( xx, xy, xz, yx, yy, yz, zx, zy, zz ) {};

	CDirCos( const float rv[9] )
		: CMat3x3( rv ) {};

	CDirCos( const CMat3x3 & rv)
		: CMat3x3( rv ) {};

   // CPiDirCos(const CPiEuler &e);
   // CPiDirCos(const CPiQtrn &q);

	CDirCos(const COriVec &v);
    
    //CPiDirCos(CPiDirCos &dc); // copy constructor

    //void fromEuler( const CPiEuler &, bool bDegrees=true ); // input in degrees
    //void fromQtrn( const CPiQtrn & );

	void fromEuler( const COriVec & );
	void fromQtrn( const COriVec & );


    //void toEuler(CPiEuler &e, bool bConvert=true ) ; // if bConvert, output in Degrees, else in Radians
    //void toQtrn(CPiQtrn &q) ;

	void toEuler(COriVec &v);
	//void toQtrn(COriVec &v);

    void toModeling();

    void slerpToward(CDirCos right, float amount);
    void difference(CDirCos &right) { Multiply_T_By(right); }

    // rotate a position by this orientation... around the position's origin
	void transform(CPosVec &pos);
	void rotate(CPosVec &pos) { transform(pos); }
    
};


/////////////////////////////////////////////////////////////////////
// END $Workfile: VPmat3x3.h $
/////////////////////////////////////////////////////////////////////
#endif // _PMAT3X3_H_
