/*******************************************************************
  Math3d.cpp                  Copyright © 1998 Virtual Paradigm  
  ----------------------------------------------------------------
  $Release  : 1.0.0 
  $Revision : 												       
	r1.00	05-06-98
                                                                   
  $Author(s):													   
  
  $Purpose  : 
																   
*******************************************************************/

#include <assert.h>
#include <math.h>
#include "math3d.h"

 
/*----------------------------------------------------------------------------
matrix multiplication.
*/

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

    for(int i = 0;i < 3;i++)
    {
        for(int j = 0;j < 3;j++)
        {
            float Value = 0;
          
            for(int k = 0;k < 3;k++)
            {
                Value += Multiplicand.GetElement(i,k) *
                        Multiplier.GetElement(k,j);
            }

            ReturnMatrix.SetElement(i,j,Value);
        }
    }

    return ReturnMatrix;
}

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

    for(int i = 0;i < 3;i++)
    {
        float Value = 0;
      
        for(int k = 0;k < 3;k++)
        {
            Value += Multiplicand.GetElement(i,k) *
                    Multiplier.GetElement(k);
        }

        ReturnPoint.SetElement(i,Value);
    }

    return ReturnPoint;
}


float* operator*( TMATRIX &Multiplicand,
		          TVECTOR &Multiplier ){
    float returnPoint[3];

    for(int i = 0;i < 3;i++){
        float Value = 0;
      
        for(int k = 0;k < 3;k++){
            Value += Multiplicand.GetElement(i,k) *
                     Multiplier.GetElement(k);
        }

		returnPoint[i] = Value;
//        ReturnPoint.SetElement(i,Value);
    }


//	float i=0;

    return returnPoint;
}


/*----------------------------------------------------------------------------
constructors.
*/

TMATRIX::TMATRIX( void )
{
    for(int Counter = 0;Counter < 9;Counter++)
    {
        v[0][Counter] = 0;
    }

    v[0][0] = v[1][1] = v[2][2] = 1;
}

/*----------------------------------------------------------------------------
vector length and normalize.
*/

float TVECTOR::Length( void ) const
{
    return (float)(   sqrt(GetX()*GetX() + GetY()*GetY() + GetZ()*GetZ())   );
}

TVECTOR &TVECTOR::Normalize( void )
{
    float MyLength = Length();

    if(MyLength != 0)
    {
        SetX(GetX() / MyLength);
        SetY(GetY() / MyLength);
        SetZ(GetZ() / MyLength);
    }

    return *this;
}
  
void orthoOrientation( TMATRIX &Orientation )
{
    TVECTOR X(Orientation(0,0),Orientation(1,0),Orientation(2,0));
    TVECTOR Y(Orientation(0,1),Orientation(1,1),Orientation(2,1));
    TVECTOR Z;

    X.Normalize();
    Z = CrossProduct(X,Y).Normalize();
    Y = CrossProduct(Z,X).Normalize();

    Orientation(0,0) = X(0); Orientation(0,1) = Y(0); Orientation(0,2) = Z(0);
    Orientation(1,0) = X(1); Orientation(1,1) = Y(1); Orientation(1,2) = Z(1);
    Orientation(2,0) = X(2); Orientation(2,1) = Y(2); Orientation(2,2) = Z(2);
}

