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

                File : Matrix.cpp
                Date : 26 august 96
                
********************************************************************************************/

#include "TDE.h"

void TDE_vGetTranslateVertexMatrix(TDE_tdsSuperObjectMatrix *p_stMatrix, TDE_tdsVertex *p_stTranslate)
{
  *p_stTranslate = p_stMatrix->stTranslateVertex;
}


void TDE_vGetAngleMatrix(TDE_tdsSuperObjectMatrix *p_stMatrix, TDE_tdxAngle *p_xAngle)
{
  *p_xAngle = p_stMatrix->xAngle;
}



void TDE_vCreateRotationMatrix(TDE_tdsMatrix *p_stMatrix, TDE_tdxAngle xAngle)
{
    TDE_tdxValue  xCosi, xSinu;
    
    xCosi = TDE_M_Cos(xAngle);
    xSinu = TDE_M_Sin(xAngle);
        
    p_stMatrix->a4_xM[0][0] = xCosi;                
    p_stMatrix->a4_xM[0][1] = -xSinu;                     
    p_stMatrix->a4_xM[1][0] = xSinu;                                   
    p_stMatrix->a4_xM[1][1] = xCosi;
}

void TDE_vCreateScaleMatrix(TDE_tdsMatrix *p_stMatrix, TDE_tdxValue xZX, TDE_tdxValue xZY)
{
    p_stMatrix->a4_xM[0][0] = xZX;                
    p_stMatrix->a4_xM[0][1] = 0;                     
    p_stMatrix->a4_xM[1][0] = 0;                                   
    p_stMatrix->a4_xM[1][1] = xZY;
}


void TDE_vMulMatrixByMatrix(TDE_tdsMatrix *p_stMat1, TDE_tdsMatrix *p_stMat2, TDE_tdsMatrix *p_stResult)
{
    TDE_tdsMatrix stMtemp;
    
    stMtemp.a4_xM[0][0] = TDE_M_Mul(p_stMat1->a4_xM[0][0], p_stMat2->a4_xM[0][0])+
                          TDE_M_Mul(p_stMat1->a4_xM[0][1], p_stMat2->a4_xM[1][0]);
        
    stMtemp.a4_xM[0][1] = TDE_M_Mul(p_stMat1->a4_xM[0][0], p_stMat2->a4_xM[0][1])+
                          TDE_M_Mul(p_stMat1->a4_xM[0][1], p_stMat2->a4_xM[1][1]);
                     
    stMtemp.a4_xM[1][0] = TDE_M_Mul(p_stMat1->a4_xM[1][0], p_stMat2->a4_xM[0][0])+
                          TDE_M_Mul(p_stMat1->a4_xM[1][1], p_stMat2->a4_xM[1][0]);
                                   
    stMtemp.a4_xM[1][1] = TDE_M_Mul(p_stMat1->a4_xM[1][0], p_stMat2->a4_xM[0][1])+
                          TDE_M_Mul(p_stMat1->a4_xM[1][1], p_stMat2->a4_xM[1][1]);
                                
    *p_stResult = stMtemp;        
}

void TDE_vMulMatrixByVertex(TDE_tdsMatrix *p_stMat, TDE_tdsVertex *p_stV, TDE_tdsVertex *p_stR)
{
    TDE_tdsVertex stVtemp;
    
    stVtemp.xX = TDE_M_Mul(p_stMat->a4_xM[0][0], p_stV->xX) +
                 TDE_M_Mul(p_stMat->a4_xM[0][1], p_stV->xY);
                                  
    stVtemp.xY = TDE_M_Mul(p_stMat->a4_xM[1][0], p_stV->xX) +
                 TDE_M_Mul(p_stMat->a4_xM[1][1], p_stV->xY);
                 
    *p_stR = stVtemp;                 
}

void TDE_vAddVertexToVertex(TDE_tdsVertex *p_stV1, TDE_tdsVertex *p_stV2, TDE_tdsVertex *p_stResult)
{
    p_stResult->xX = p_stV1->xX + p_stV2->xX;
    p_stResult->xY = p_stV1->xY + p_stV2->xY;    
}

void TDE_vSubVertexToVertex(TDE_tdsVertex *p_stV1, TDE_tdsVertex *p_stV2, TDE_tdsVertex *p_stResult)
{
    p_stResult->xX = p_stV1->xX - p_stV2->xX;
    p_stResult->xY = p_stV1->xY - p_stV2->xY;    
}

TDE_tdxValue TDE_xNormeVertex(TDE_tdsVertex *p_stV)
{
    return TDE_M_Sqrt(TDE_M_Add(TDE_M_Sq(p_stV->xX),
                                TDE_M_Sq(p_stV->xY)));
}

void TDE_vNormerVertex(TDE_tdsVertex *p_stV, TDE_tdsVertex *p_stResult)
{
    TDE_tdxValue xNorme;

    xNorme = TDE_xNormeVertex(p_stV);

    p_stResult->xX = TDE_M_Div(p_stV->xX, xNorme);
    p_stResult->xY = TDE_M_Div(p_stV->xY, xNorme);
}

void TDE_vMulVertexToValue(TDE_tdsVertex *p_stV, TDE_tdxValue xValue, TDE_tdsVertex *p_stResult)
{
    p_stResult->xX = TDE_M_Mul( p_stV->xX, xValue);
    p_stResult->xY = TDE_M_Mul( p_stV->xY, xValue);
}


void TDE_vCreateSuperObjectMatrix(TDE_tdsSuperObjectMatrix *p_stMatrix)
{    
    TDE_tdsMatrix stR;

    TDE_vCreateRotationMatrix(&stR, p_stMatrix->xAngle);
    TDE_vCreateScaleMatrix(&(p_stMatrix->a4_xS), p_stMatrix->stScale.xX, p_stMatrix->stScale.xY);
    TDE_vMulMatrixByMatrix(&stR, &(p_stMatrix->a4_xS), &(p_stMatrix->a4_xRxS));
}


/**~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~**
 NAME : TDE_vCreateSuperObjectDescendancyMatrices
 VERSION : 2.0 / Valérie
           1.0 / Franck

 AIM : Create a SuperObject and his descendancy matrices
**~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~**/
void TDE_vCreateSuperObjectDescendancyMatrices(TDE_tdsSuperObject *p_stSuperObject)
{
    TDE_tdsSuperObject *p_stSuperObjectTemp;

    TDE_vCreateSuperObjectMatrix(&(p_stSuperObject->stMatrix));
    if ( p_stSuperObject->p_stChild != NULL )
    {    
        for ( p_stSuperObjectTemp = p_stSuperObject->p_stChild; p_stSuperObjectTemp != NULL; p_stSuperObjectTemp = p_stSuperObjectTemp->p_stRightBrother)
        {
           TDE_vCreateSuperObjectDescendancyMatrices(p_stSuperObjectTemp);
        }
    }
}


void TDE_vMulSOMatrixBySOMatrix(TDE_tdsSuperObjectMatrix *p_stMat1, TDE_tdsSuperObjectMatrix *p_stMat2, TDE_tdsSuperObjectMatrix *p_stResult)
{

    TDE_vMulMatrixByMatrix(&(p_stMat1->a4_xRxS), &(p_stMat2->a4_xRxS), &(p_stResult->a4_xRxS));
    TDE_vMulMatrixByVertex(&(p_stMat1->a4_xRxS), &(p_stMat2->stTranslateVertex), &(p_stResult->stTranslateVertex));
    TDE_vAddVertexToVertex(&(p_stResult->stTranslateVertex), &(p_stMat1->stTranslateVertex), &(p_stResult->stTranslateVertex));    
}


void PushMatrix(TDE_tdsSuperObjectMatrix *p_stMat)
{
    MS_Position++;
    if (MS_Position >= TDE_kSTACKDEPTH)
    {
        Erm_M_UpdateLastError  (Tde,
                                C_ucErmDefaultChannel,
                                E_uwTdeWarningMatrixStackOverflow,
                                C_lErmNoDebugData,
                                C_ucErmOpenInfoWindow,
                                C_ucAllowStopForDebug, NULL);
        MatrixStack[TDE_kSTACKDEPTH-1] = *p_stMat;    
        TDE_vMulSOMatrixBySOMatrix(&MatrixStack[TDE_kSTACKDEPTH-2], &MatrixStack[TDE_kSTACKDEPTH-1], &MatrixStack[TDE_kSTACKDEPTH-1]);
    }
    else
    {
        MatrixStack[MS_Position] = *p_stMat;    
        if (MS_Position>0)
        {
            TDE_vMulSOMatrixBySOMatrix(&MatrixStack[MS_Position-1], &MatrixStack[MS_Position], &MatrixStack[MS_Position]);
        }   
    }
}

void ResetMatrixStack(TDE_tdsCamera *stCamera)
{    
    MS_Position=-1;
    PushMatrix(stCamera);
}

void PopMatrix()
{
    MS_Position--;
}