#ifndef MATH3D_H
#define MATH3D_H
/*******************************************************************
 Copyright © 1998 Virtual Paradigm 

 $Release  : 1.0.0                 	    	                       
 $Revision : 												       
	r1.00	05-06-98											   
                                                                 
 $Comments : 

*******************************************************************/				                       				

#include <math.h>       
#include <assert.h>
#include <float.h>      
#include <assert.h>

#include "../datastructure/baseDS.h"

//float const PI = 3.14159265358979323846;

class TVECTOR;
class TMATRIX;

/*----------------------------------------------------------------------------
globally useful functions.
*/

TMATRIX operator*( TMATRIX const &Multiplicand,
				   TMATRIX const &Multiplier );

TVECTOR operator*( TMATRIX const &Multiplicand,
	   		       TVECTOR const &Multiplier );

float* operator*( TMATRIX &Multiplicand,
				  TVECTOR &Multiplier );

void   orthoOrientation( TMATRIX &Orientation );

/*----------------------------------------------------------------------------
TVECTOR.  This class represents a 3D vector.
*/

class TVECTOR{
	public:
	    float v[3];
	    float wv[3];
	    float wvLast[3];

		float const *GetElements( void ) const { return v; };
		inline float GetElement( int Row ) const;
		inline void  SetElement( int Row, float Value );
		inline float GetX( void ) const;
		inline float GetY( void ) const;
		inline float GetZ( void ) const;
		inline void  SetX( float Value );
		inline void  SetY( float Value );
		inline void  SetZ( float Value );
		inline float &operator()( int unsigned Row );
		inline float const &operator()( int unsigned Row ) const;
		inline TVECTOR &operator=( TVECTOR const & );
		inline TVECTOR &operator+=(TVECTOR const &addend);
		inline TVECTOR &operator-=(TVECTOR const &lessor);
		inline TVECTOR &operator*=(float const multiplier);
		inline TVECTOR &operator/=(float const divisor);
		float  Length( void ) const;
		TVECTOR &Normalize( void );
		inline TVECTOR (TVECTOR const &);
		inline TVECTOR (void);
		inline TVECTOR (float const *pElements);
		inline TVECTOR (float X, float Y, float Z = 0);
};

/*----------------------------------------------------------------------------
TMATRIX.  This class represents row major 3x3 homogeneous matrices.
*/

class TMATRIX{
	public:
	    float v[3][3];

		TMATRIX( void );
		TMATRIX( float (*pElements)[3] );
		enum mode { SkewSymmetric, Zero };
		inline TMATRIX( mode MustBeZero );
		inline TMATRIX( TVECTOR const &CrossVector, mode MustBeSkewSymmetric );
        inline TMATRIX &operator+=( TMATRIX const &A );
	    inline float &operator()( int unsigned Row, int unsigned Column );
		inline float const &operator()( int unsigned Row, int unsigned Column ) const;
		inline float GetElement( int Row, int Column ) const;
		inline TMATRIX &SetElement( int Row, int Column, float Value );

	protected:
	    enum do_not_initialize { DoNotInitialize };
	    inline TMATRIX( do_not_initialize );
};

/*----------------------------------------------------------------------------
inline function definitions.
*/

inline TVECTOR &TVECTOR::operator+=(TVECTOR const &addend)
{
    SetX(GetX() + addend.GetX());
    SetY(GetY() + addend.GetY());
    SetZ(GetZ() + addend.GetZ());

    return(*this);
}

inline TVECTOR operator+( TVECTOR const &Operand1,
						  TVECTOR const &Operand2 )
{
    return TVECTOR(Operand1.GetX() + Operand2.GetX(),
                    Operand1.GetY() + Operand2.GetY(),
                    Operand1.GetZ() + Operand2.GetZ());
}

inline TVECTOR &TVECTOR::operator-=(TVECTOR const &lessor)
{
    SetX(GetX() - lessor.GetX());
    SetY(GetY() - lessor.GetY());
    SetZ(GetZ() - lessor.GetZ());

    return(*this);
}

inline TVECTOR operator-( TVECTOR const &Operand1,
						  TVECTOR const &Operand2 )
{
    return TVECTOR(Operand1.GetX() - Operand2.GetX(),
                    Operand1.GetY() - Operand2.GetY(),
                    Operand1.GetZ() - Operand2.GetZ());
}

inline TVECTOR operator-( TVECTOR const &Operand1 )
{
    return TVECTOR(-Operand1.GetX(),-Operand1.GetY(),-Operand1.GetZ());
}

inline TVECTOR &TVECTOR::operator*=(float const multiplier)
{
    SetX(GetX() * multiplier);
    SetY(GetY() * multiplier);
    SetZ(GetZ() * multiplier);

    return(*this);
}

inline TVECTOR operator*( TVECTOR const &Multiplicand,
						  float const &Multiplier )
{
    return TVECTOR(Multiplicand.GetX() * Multiplier,
                    Multiplicand.GetY() * Multiplier,
                    Multiplicand.GetZ() * Multiplier);
}

inline TVECTOR operator*( float const &Multiplier,
						  TVECTOR const &Multiplicand ) 
{
    return Multiplicand * Multiplier;
}
      
inline TVECTOR &TVECTOR::operator/=(float const divisor)
{
    SetX(GetX() / divisor);
    SetY(GetY() / divisor);
    SetZ(GetZ() / divisor);

    return(*this);
}

inline TVECTOR operator/( TVECTOR const &Quotient,
						  float const &Divisor )
{
    return Quotient * (float(1.0)/Divisor);
}

inline TVECTOR CrossProduct( TVECTOR const &Operand1,
						     TVECTOR const &Operand2 )
{
    float X = Operand1.GetY() * Operand2.GetZ() -
                Operand1.GetZ() * Operand2.GetY();
    float Y = Operand1.GetZ() * Operand2.GetX() -
                Operand1.GetX() * Operand2.GetZ();
    float Z = Operand1.GetX() * Operand2.GetY() -
                Operand1.GetY() * Operand2.GetX();

    return TVECTOR(X,Y,Z);
}

inline float DotProduct( TVECTOR const &Operand1, TVECTOR const &Operand2 )
{
    return Operand1.GetX() * Operand2.GetX() +
            Operand1.GetY() * Operand2.GetY() +
            Operand1.GetZ() * Operand2.GetZ();
}

inline float TVECTOR::GetElement( int Row ) const
{
    return v[Row];
}

inline float TVECTOR::GetX( void ) const
{
    return v[0];
}

inline float TVECTOR::GetY( void ) const
{
    return v[1];
}

inline float TVECTOR::GetZ( void ) const
{
    return v[2];
}

inline float &TVECTOR::operator()( int unsigned Row )
{
    assert(Row < 3);
    return v[Row];
}

inline float const &TVECTOR::operator()( int unsigned Row ) const
{
    assert(Row < 3);
    return v[Row];
}

inline void TVECTOR::SetElement( int Row, float Value )
{
    v[Row] = Value;
}

inline void TVECTOR::SetX( float Value )
{
    v[0] = Value;
}

inline void TVECTOR::SetY( float Value )
{
    v[1] = Value;
}

inline void TVECTOR::SetZ( float Value )
{
    v[2] = Value;
}

inline TVECTOR::TVECTOR( void )
{
    v[0] = v[1] = v[2] = 0;
}

inline TVECTOR::TVECTOR( float X, float Y, float Z )
{
    v[0] = X;
    v[1] = Y;
    v[2] = Z;
}

inline TVECTOR::TVECTOR( TVECTOR const &Source )
{
    v[0] = Source.v[0];
    v[1] = Source.v[1];
    v[2] = Source.v[2];
}

inline TVECTOR &TVECTOR::operator=( TVECTOR const &Source )
{
    v[0] = Source.v[0];
    v[1] = Source.v[1];
    v[2] = Source.v[2];

    return *this;
}

inline TVECTOR::TVECTOR( float const *pElements )
{
    assert(pElements);
    v[0] = pElements[0];
    v[1] = pElements[1];
    v[2] = pElements[2];
}

inline float &TMATRIX::operator()( int unsigned Row, int unsigned Column )
{
    assert((Row < 3) && (Column < 3));
    return v[Row][Column];
}

inline float const &TMATRIX::operator()( int unsigned Row,
        int unsigned Column ) const
{
    assert((Row < 3) && (Column < 3));
    return v[Row][Column];
}

inline float TMATRIX::GetElement( int Row, int Column ) const
{
    return v[Row][Column];
}

inline TMATRIX &TMATRIX::SetElement( int Row, int Column, float Value )
{
    v[Row][Column] = Value;

    return *this;
}

inline TMATRIX::TMATRIX( do_not_initialize )
{
}

inline TMATRIX::TMATRIX( float (*pElements)[3] )
{
    assert(pElements);

    TMATRIX &M = *this;
    for(int i = 0;i < 3;i++)
    {
        for(int j = 0;j < 3;j++)
        {
            M(i,j) = pElements[i][j];
        }
    }
}
    
inline TMATRIX::TMATRIX( TVECTOR const &Vector, mode Mode )
{
    assert(Mode == SkewSymmetric);

    TMATRIX &M = *this;

    M(0,0) =  float(0);      M(0,1) = -Vector.GetZ(); M(0,2) =  Vector.GetY();
    M(1,0) =  Vector.GetZ(); M(1,1) =  float(0);      M(1,2) = -Vector.GetX();
    M(2,0) = -Vector.GetY(); M(2,1) =  Vector.GetX(); M(2,2) = float(0);
}

inline TMATRIX::TMATRIX( mode Mode )
{
    assert(Mode == Zero);

    for(int Counter = 0;Counter < 3;Counter++)
    {
        v[Counter][0] = float(0);
        v[Counter][1] = float(0);
        v[Counter][2] = float(0);
    }
}

inline TMATRIX Transpose( TMATRIX const &Matrix )
{
    TMATRIX Return;

    for(int Counter = 0;Counter < 3;Counter++)
    {
        Return(0,Counter) = Matrix(Counter,0);
        Return(1,Counter) = Matrix(Counter,1);
        Return(2,Counter) = Matrix(Counter,2);
    }

    return Return;
}   

inline TMATRIX operator+( TMATRIX const &Operand1,
						  TMATRIX const &Operand2 )
{
    TMATRIX Return;

    for(int Counter = 0;Counter < 3;Counter++)
    {
        Return(0,Counter) = Operand1(0,Counter) + Operand2(0,Counter);
        Return(1,Counter) = Operand1(1,Counter) + Operand2(1,Counter);
        Return(2,Counter) = Operand1(2,Counter) + Operand2(2,Counter);
    }

    return Return;
}   
    
inline TMATRIX operator-( TMATRIX const &Operand1,
						  TMATRIX const &Operand2 )
{
    TMATRIX Return;

    for(int Counter = 0;Counter < 3;Counter++)
    {
        Return(0,Counter) = Operand1(0,Counter) - Operand2(0,Counter);
        Return(1,Counter) = Operand1(1,Counter) - Operand2(1,Counter);
        Return(2,Counter) = Operand1(2,Counter) - Operand2(2,Counter);
    }

    return Return;
}   
    
inline TMATRIX operator*( float const Value, TMATRIX const &Matrix )
{
    TMATRIX Return;

    for(int Counter = 0;Counter < 3;Counter++)
    {
        Return(0,Counter) = Value * Matrix(0,Counter);
        Return(1,Counter) = Value * Matrix(1,Counter);
        Return(2,Counter) = Value * Matrix(2,Counter);
    }

    return Return;
}   

inline TMATRIX &TMATRIX::operator+=( TMATRIX const &Operand1 )
{
    for(int Counter = 0;Counter < 3;Counter++)
    {
        v[0][Counter] += Operand1(0,Counter);
        v[1][Counter] += Operand1(1,Counter);
        v[2][Counter] += Operand1(2,Counter);
    }

    return *this;
}   

inline TMATRIX operator*( TMATRIX const &Matrix, float const Value )
{
    return Value * Matrix;
}

inline TMATRIX operator-( TMATRIX const &Matrix )
{
    TMATRIX Return;

    for(int Counter = 0;Counter < 3;Counter++)
    {
        Return(0,Counter) = -Matrix(0,Counter);
        Return(1,Counter) = -Matrix(1,Counter);
        Return(2,Counter) = -Matrix(2,Counter);
    }

    return Return;
}   

inline TVECTOR Normalize( TVECTOR const &A )
{
    TVECTOR Return = A;
    return Return.Normalize();
}

#endif
