/* ##H_FILE#
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
FILE :          MTH2d.h
MODULE :        MTH     (Common Mathematic Library) 
 
DESCRIPTION : 2D Vectorial implementation
 
VERSION :       MTH V5.0.13 / Alexandre LANGER [ALX] Ubi R&D / Add Comments
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
*/
 
#ifndef MTH2D_H
#define MTH2D_H
 
#include "MTH_Real.h"
 
 
/* ##TYPEDEF#----------------------------------------------------------------------------
                          Types definition
---------------------------------------------------------------------------------------*/
 
/* 2D Vector : */
typedef struct MTH2D_tdstVector_
{
    MTH_tdxReal xX;
    MTH_tdxReal xY;
} MTH2D_tdstVector;
 
/* 2D Matrix */
typedef struct MTH2D_tdstMatrix_
{
    MTH2D_tdstVector stCol_0;
    MTH2D_tdstVector stCol_1;
} MTH2D_tdstMatrix;
 
 
 
/* ##MACRO#----------------------------------------------------------------------------
                          MACRO definition
---------------------------------------------------------------------------------------*/
 
/* ##-###################################### 
   ## MACRO AND FUNCTION FOR 2D MATHEMATICS
   ######################################### */
 
 
/* ##-########################### 
   ## Vector Operations
   ############################## */
 
/* ##M==================================================================================
NAME :          MTH2D_M_vAddVector
DESCRIPTION :   Add two vectors : VectDest= VectA + VectB
INPUT :         VectDest, VectA, VectB : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vAddVector( VectDest, VectA, VectB)                             \
       {        (VectDest)->xX = MTH_M_xAdd((VectA)->xX, (VectB)->xX);          \
                (VectDest)->xY = MTH_M_xAdd((VectA)->xY, (VectB)->xY); }         
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSubVector
DESCRIPTION :   Sub two vectors : VectDest= VectA - VectB
INPUT :         VectDest, VectA, VectB : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSubVector( VectDest, VectA, VectB)                             \
        {       (VectDest)->xX = MTH_M_xSub((VectA)->xX, (VectB)->xX);          \
                (VectDest)->xY = MTH_M_xSub((VectA)->xY, (VectB)->xY); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vNegVector
DESCRIPTION :   Negation of a vector : VectDest= - VectA
INPUT :         VectDest, VectA : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vNegVector( VectDest, VectA)                                    \
        {       (VectDest)->xX = MTH_M_xNeg( (VectA)->xX );                     \
                (VectDest)->xY = MTH_M_xNeg( (VectA)->xY ); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_bEqualVector
DESCRIPTION :   Test if two vectors are equal
INPUT :         VectA, VectB : address of MTH2D_tdstVector
OUTPUT :        VectA==VectB : Boolean
=======================================================================================*/
#define MTH2D_M_bEqualVector( VectA, VectB)                                             \
                (                                                                       \
                     (MTH_M_bEqual((VectA)->xX, (VectB)->xX)) &&                        \
                     (MTH_M_bEqual((VectA)->xY, (VectB)->xY))                           \
                )
 
/* ##M==================================================================================
NAME :          MTH2D_M_vAddScalarVector
DESCRIPTION :   Add a scalare with a vector : VectDest= a.Id + VectA 
INPUT :         VectDest: address of MTH2D_tdstVector,
                a: MTH_tdxReal, VectA: address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vAddScalarVector( VectDest, a, VectA)   \
  {                                                     \
    (VectDest)->xX = MTH_M_xAdd((VectA)->xX, (a));      \
    (VectDest)->xY = MTH_M_xAdd((VectA)->xY, (a));      \
  }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSubScalarVector
DESCRIPTION :   Add a scalare with a vector : VectDest= VectA - a.Id
INPUT :         VectDest: address of MTH2D_tdstVector,
                a: MTH_tdxReal, VectA: address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSubScalarVector( VectDest, a, VectA)   \
  {                                                     \
    (VectDest)->xX = MTH_M_xSub((VectA)->xX, (a));      \
    (VectDest)->xY = MTH_M_xSub((VectA)->xY, (a));      \
  }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMulScalarVector
DESCRIPTION :   Multiplicate a scalare with a vector : VectDest= a*VectA
INPUT :         VectDest: address of MTH2D_tdstVector,
                a: MTH_tdxReal, VectA: address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMulScalarVector( VectDest, a, VectA)                   \
        {       (VectDest)->xX = MTH_M_xMul((a), (VectA)->xX);          \
                (VectDest)->xY = MTH_M_xMul((a), (VectA)->xY); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vDivScalarVector
DESCRIPTION :   Divide a vector by a scalare : VectDest= VectA/a
INPUT :         VectDest, VectA: address of MTH2D_tdstVector, a: MTH_tdxReal
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vDivScalarVector( VectDest, VectA, a)                   \
        {       (VectDest)->xX = MTH_M_xDiv((VectA)->xX, (a));          \
                (VectDest)->xY = MTH_M_xDiv((VectA)->xY, (a)); }        
 
/* ##M==================================================================================
NAME :          MTH2D_M_xDotProductVector
DESCRIPTION :   Return the Dot Product of two vectors
INPUT :         VectA, VectB : address of MTH2D_tdstVector
OUTPUT :        VectA.VectB  : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xDotProductVector( VectA, VectB)                                \
                MTH_M_xAdd(                                                     \
                MTH_M_xMul((VectA)->xX, (VectB)->xX),                           \
            MTH_M_xMul((VectA)->xY, (VectB)->xY)                                \
                        )
 
/* ##M==================================================================================
NAME :          MTH2D_M_xSqrVector
DESCRIPTION :   Return the square of a vector
INPUT :         VectA        : address of MTH2D_tdstVector
OUTPUT :        VectA.VectA  : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xSqrVector( VectA)                                                              \
                MTH_M_xAdd(                                                                     \
                MTH_M_xSqr((VectA)->xX),                                                        \
            MTH_M_xSqr((VectA)->xY)                                                             \
                        )
 
/* ##M==================================================================================
NAME :          MTH2D_M_xNormVector
DESCRIPTION :   Return the norm of a vector
INPUT :         VectA     : address of MTH2D_tdstVector
OUTPUT :        ||VectA|| : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xNormVector( VectA )                                                    \
                        MTH_M_xSqrt( MTH2D_M_xSqrVector( VectA ) )
 
/* ##M==================================================================================
NAME :          MTH2D_M_xInvNormVector
DESCRIPTION :   Return the inverse of the norm of a vector
INPUT :         VectA       : address of MTH2D_tdstVector
OUTPUT :        1/||VectA|| : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xInvNormVector( VectA )                                                 \
                        MTH_M_xInvSqrt( MTH2D_M_xSqrVector( VectA ) )
 
/* ##M==================================================================================
NAME :          MTH2D_M_xVectorGapSqr
DESCRIPTION :   Return the square Gap between two vectors
INPUT :         VectA, VectB         : address of MTH2D_tdstVector
OUTPUT :        Sqr(||VectA-VectB||) : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xVectorGapSqr(VectA, VectB)                                     \
                        (MTH_M_xSqrAddSqr(                                      \
             MTH_M_xSub( (VectA)->xX, (VectB)->xX),                             \
             MTH_M_xSub( (VectA)->xY, (VectB)->xY) ) )
     
/* ##M==================================================================================
NAME :          MTH2D_M_xVectorGap
DESCRIPTION :   Return the Gap between two vectors
INPUT :         VectA, VectB    : address of MTH2D_tdstVector
OUTPUT :        ||VectA-VectB|| : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xVectorGap(VectA, VectB)                                                \
                        (MTH_M_xSqrt( MTH2D_M_xVectorGapSqr(VectA, VectB) ))
 
/* ##M==================================================================================
NAME :          MTH2D_M_vFillVector
DESCRIPTION :   Fill each vector element with a value
INPUT :         VectA: address of MTH2D_tdstVector, a: MTH_tdxReal
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vFillVector( VectDest, a)                                               \
        {               (VectDest)->xX = (a);                                           \
                        (VectDest)->xY = (a); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vNormalizeVector
DESCRIPTION :   Normalize a vector : VectDest= VectA/||VectA||
INPUT :         VectDest, VectA : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vNormalizeVector(VectDest, VectA)                               \
                {                                                               \
         MTH_tdxReal inv_norm;                                                  \
         inv_norm= MTH2D_M_xInvNormVector( VectA);                              \
         MTH2D_M_vMulScalarVector(VectDest, inv_norm, VectA );                  \
         }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vGetVectorElements
DESCRIPTION :   Get x, y values of a vector
INPUT :         x, y : address of MTH_tdxReal, VectA : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vGetVectorElements( Vx, Vy, VectA)                              \
        {       *Vx = (VectA)->xX;                                              \
                *Vy = (VectA)->xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetVectorElements
DESCRIPTION :   Set a vector with  x, y values
INPUT :         VectDest : address of MTH2D_tdstVector, x, y : address of MTH_tdxReal, 
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetVectorElements( VectDest, Vx, Vy)                   \
        {               (VectDest)->xX = (Vx);                          \
                        (VectDest)->xY = (Vy); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_xGetXofVector
DESCRIPTION :   Return x element of a vector
INPUT :         VectA : address of MTH2D_tdstVector
OUTPUT :        x     : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xGetXofVector( VectA )                                                  \
                        (VectA)->xX
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetXofVector
DESCRIPTION :   Set x element of a vector
INPUT :         VectDest : address of MTH2D_tdstVector, Vx : MTH_tdxReal
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetXofVector( VectDest, Vx)                                    \
                {       (VectDest)->xX = (Vx); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_xGetYofVector
DESCRIPTION :   Return y element of a vector
INPUT :         VectA : address of MTH2D_tdstVector
OUTPUT :        y     : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xGetYofVector( VectA )                                                  \
                        (VectA)->xY
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetYofVector
DESCRIPTION :   Set y element of a vector
INPUT :         VectDest : address of MTH2D_tdstVector, Vy : MTH_tdxReal
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetYofVector( VectDest, Vy)                                    \
                {       (VectDest)->xY = (Vy); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vCopyVector
DESCRIPTION :   Copy a vector : VectDest=VectA 
INPUT :         VectDest, VectA : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vCopyVector( VectDest, VectA)                                   \
                { memcpy(VectDest, VectA, sizeof(MTH2D_tdstVector)); }            
 
/* ##M==================================================================================
NAME :          MTH2D_M_vNullVector
DESCRIPTION :   Set all elements of a vector to zero
INPUT :         VectDest : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vNullVector( VectDest)                                                  \
                {       (VectDest)->xX = MTH_C_ZERO;                                    \
                        (VectDest)->xY = MTH_C_ZERO; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vScaleVector
DESCRIPTION :   Set each element of  VectDest with the multiplication of element of VectA by element of VectB
INPUT :         VectDest, VectA, VectB : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vScaleVector( VectDest, VectA, VectB )                          \
                {       (VectDest)->xX = MTH_M_xMul( (VectA)->xX, (VectB)->xX); \
                        (VectDest)->xY = MTH_M_xMul( (VectA)->xY, (VectB)->xY); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMiddleVector
DESCRIPTION :   Calculate the middle of two vectors : VectDest= (VectA+VectB)/2 
INPUT :         VectDest, VectA, VectB : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMiddleVector( VectDest, VectA, VectB )                         \
                {       MTH2D_M_vAddVector( VectDest, VectA, VectB );           \
                        MTH2D_M_vMulScalarVector( VectDest, MTH_C_Inv2 , VectDest); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vLinearInterpolVector
DESCRIPTION :   Calculate the lineat interpolation of two vectors : VectDest= VectA + t.(VectB-VectA)
INPUT :         VectDest, VectA, VectB: address of MTH2D_tdstVector, t: MTH_tdxReal
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vLinearInterpolVector( VectDest, VectA, VectB, t)       \
                {       (VectDest)->xX= MTH_M_xLinearInterpol( (VectA)->xX, (VectB)->xX, (t) ); \
                        (VectDest)->xY= MTH_M_xLinearInterpol( (VectA)->xY, (VectB)->xY, (t) ); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMaxVector
DESCRIPTION :   Make a vector with max elements of two other vectors
INPUT :         VectDest, VectA, VectB: address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMaxVector( VectDest, VectA, VectB)     \
                {       (VectDest)->xX= MTH_M_xMax( (VectA)->xX, (VectB)->xX ); \
                        (VectDest)->xY= MTH_M_xMax( (VectA)->xY, (VectB)->xY ); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMinVector
DESCRIPTION :   Make a vector with min elements of two other vectors
INPUT :         VectDest, VectA, VectB: address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMinVector( VectDest, VectA, VectB)     \
                {       (VectDest)->xX= MTH_M_xMin( (VectA)->xX, (VectB)->xX ); \
                        (VectDest)->xY= MTH_M_xMin( (VectA)->xY, (VectB)->xY ); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_bIsNullVector
DESCRIPTION :   Test if a vector is null
INPUT :         VectA : address of MTH2D_tdstVector
OUTPUT :        VectA==Null vector : Boolean
=======================================================================================*/
#define MTH2D_M_bIsNullVector( VectA )  \
                ( MTH_M_bEqualZero((VectA)->xX) && MTH_M_bEqualZero((VectA)->xY) )
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMulAddVector
DESCRIPTION :   Multiply a scalar to a vector, and add it to a other vector: VectDest = x.VectA + VectB
INPUT :         VectDest: address of MTH2D_tdstVector,
                x: MTH_tdxReal, VectA, VectB : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMulAddVector( VectDest, x,  VectA, VectB)                                      \
        {       (VectDest)->xX = MTH_M_xAdd(MTH_M_xMul((x),(VectA)->xX) , (VectB)->xX);         \
                (VectDest)->xY = MTH_M_xAdd(MTH_M_xMul((x),(VectA)->xY), (VectB)->xY); }
 
/* MTH2D_M_vMullAddVector( VectDest, x,  VectA, VectB) : VectDest = x.VectA + VectB */
/* WARNING : THIS MUST DESAPEAR !!! */
#define MTH2D_M_vMullAddVector  MTH2D_M_vMulAddVector
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetBaseIVector
DESCRIPTION :   Set a vector to Ox base vector : VectDest = ( 1, 0 )
INPUT :         VectDest: address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetBaseIVector( VectDest )     \
 {      (VectDest)->xX = MTH_C_ONE;             \
        (VectDest)->xY = MTH_C_ZERO;            \
 }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetBaseJVector
DESCRIPTION :   Set a vector to Oy base vector : VectDest = ( 0, 1 )
INPUT :         VectDest: address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetBaseJVector( VectDest )     \
 {      (VectDest)->xX = MTH_C_ZERO;            \
        (VectDest)->xY = MTH_C_ONE;             \
 }
 
/* ##-########################### 
   ## Matrix operations
   ############################## */   
 
/* ##M==================================================================================
NAME :          MTH2D_M_vCopyMatrix
DESCRIPTION :   Copy MatA in MatDest
INPUT :         MatDest, MatA: address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vCopyMatrix( MatDest, MatA)                                                     \
                 {      memcpy(MatDest, MatA, sizeof(MTH2D_tdstMatrix)); } 
 
/* ##M==================================================================================
NAME :          MTH2D_M_vAddMatrix
DESCRIPTION :   Add two matrix : MatDest= MatA + MatB
INPUT :         MatDest, MatA, MatB: address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vAddMatrix( MatDest, MatA, MatB)                                \
        {          MTH2D_M_vAddVector( &((MatDest)->stCol_0),                   \
                                &((MatA)->stCol_0), &((MatB)->stCol_0));        \
                   MTH2D_M_vAddVector( &((MatDest)->stCol_1),                   \
                               &((MatA)->stCol_1), &((MatB)->stCol_1)); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSubMatrix
DESCRIPTION :   Sub two matrix : MatDest= MatA - MatB
INPUT :         MatDest, MatA, MatB: address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSubMatrix( MatDest, MatA, MatB)                                \
        {          MTH2D_M_vSubVector( &((MatDest)->stCol_0),                   \
                                &((MatA)->stCol_0), &((MatB)->stCol_0));        \
                   MTH2D_M_vSubVector( &((MatDest)->stCol_1),                   \
                               &((MatA)->stCol_1), &((MatB)->stCol_1)); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vNegMatrix
DESCRIPTION :   Make the Nagation of a matrix : MatDest= -MatA
INPUT :         MatDest, MatA: address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vNegMatrix( MatDest, MatA)                                      \
        {          MTH2D_M_vNegVector( &((MatDest)->stCol_0),                   \
                                &((MatA)->stCol_0));                            \
                   MTH2D_M_vNegVector( &((MatDest)->stCol_1),                   \
                               &((MatA)->stCol_1)); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMulMatrixMatrixWithoutBuffer
DESCRIPTION :   Multiply two matrix : MatDest= MatA*MatB, MatDest must be a other matrix
INPUT :         MatDest, MatA, MatB: address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMulMatrixMatrixWithoutBuffer(MatDest, MatA, MatB)      \
        {               (MatDest)->stCol_0.xX =  MTH_M_xMulAddMul(              \
                (MatA)->stCol_0.xX, (MatB)->stCol_0.xX,                         \
                (MatA)->stCol_1.xX, (MatB)->stCol_0.xY );                       \
            (MatDest)->stCol_0.xY =  MTH_M_xMulAddMul(                          \
                (MatA)->stCol_0.xY, (MatB)->stCol_0.xX,                         \
                (MatA)->stCol_1.xY, (MatB)->stCol_0.xY );                       \
                    (MatDest)->stCol_1.xX =  MTH_M_xMulAddMul(                  \
                (MatA)->stCol_0.xX, (MatB)->stCol_1.xX,                         \
                (MatA)->stCol_1.xX, (MatB)->stCol_1.xY );                       \
                    (MatDest)->stCol_1.xY =  MTH_M_xMulAddMul(                  \
                (MatA)->stCol_0.xY, (MatB)->stCol_1.xX,                         \
                (MatA)->stCol_1.xY, (MatB)->stCol_1.xY ); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMulMatrixMatrix
DESCRIPTION :   Multiply two matrix : MatDest= MatA*MatB
INPUT :         MatDest, MatA, MatB: address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMulMatrixMatrix(Mat_Dest, Mat_A, Mat_B)                \
                        if( (Mat_Dest==Mat_A) || (Mat_Dest==Mat_B) )    \
            {                                                           \
              MTH2D_tdstMatrix Mtemp;                                   \
                                                                        \
              MTH2D_M_vMulMatrixMatrixWithoutBuffer(                    \
                                        &Mtemp, Mat_A, Mat_B);          \
              MTH2D_M_vCopyMatrix( Mat_Dest, &Mtemp);                   \
            }                                                           \
            else                                                        \
            {                                                           \
             MTH2D_M_vMulMatrixMatrixWithoutBuffer(Mat_Dest,            \
                                        Mat_A,  Mat_B);                 \
             } 
                                                                                  
/* ##M==================================================================================
NAME :          MTH2D_M_vMulScalarMatrix
DESCRIPTION :   Multiply a scalar with a matrix : MatDest= a*MatA
INPUT :         MatDest : address of MTH2D_tdstMatrix
                a: MTH_tdxReal, MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMulScalarMatrix(MatDest, a, MatA)                              \
                {       MTH2D_M_vMulScalarVector( &((MatDest)->stCol_0),        \
                                        (a), &((MatA)->stCol_0));               \
                        MTH2D_M_vMulScalarVector( &((MatDest)->stCol_1),        \
                                                  (a), &((MatA)->stCol_1)); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vDivScalarMatrix
DESCRIPTION :   Divide a matrix by a scalar : MatDest= MatA/a
INPUT :         MatDest : address of MTH2D_tdstMatrix
                MatA : address of MTH2D_tdstMatrix, a: MTH_tdxReal
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vDivScalarMatrix(MatDest, MatA, a)                              \
                {       MTH2D_M_vDivScalarVector( &((MatDest)->stCol_0),        \
                                        &((MatA)->stCol_0), (a));               \
                        MTH2D_M_vDivScalarVector( &((MatDest)->stCol_1),        \
                                                  &((MatA)->stCol_1), (a)); }                            
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSqrMatrix
DESCRIPTION :   Square a matrix : MatDest= MatA*MatA
INPUT :         MatDest : address of MTH2D_tdstMatrix
                MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSqrMatrix(MatDest, MatA)                                               \
                { MTH2D_M_vMulMatrixMatrix( MatDest, MatA, MatA); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vTranspMatrixWithoutBuffer
DESCRIPTION :   Transpose a matrix : MatDest= transp MatA, Matdest must be a other matrix
INPUT :         MatDest : address of MTH2D_tdstMatrix
                MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vTranspMatrixWithoutBuffer(MatDest, MatA)               \
        {   (MatDest)->stCol_0.xX = (MatA)->stCol_0.xX;                 \
            (MatDest)->stCol_0.xY = (MatA)->stCol_1.xX;                 \
            (MatDest)->stCol_1.xX = (MatA)->stCol_0.xY;                 \
            (MatDest)->stCol_1.xY = (MatA)->stCol_1.xY; }
 
 
/* MTH2D_M_vTranpMatrixWithoutBuffer(MatDest, MatA): MatDest= transp MatA */
/* WARNING : THIS MUST DESAPEAR !!! */
#define MTH2D_M_vTranpMatrixWithoutBuffer MTH2D_M_vTranspMatrixWithoutBuffer
 
/* ##M==================================================================================
NAME :          MTH2D_M_vTranspMatrix
DESCRIPTION :   Transpose a matrix : MatDest= transp MatA
INPUT :         MatDest : address of MTH2D_tdstMatrix
                MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vTranspMatrix(Mat_Dest, Mat_A)                          \
                        if( Mat_Dest==Mat_A )                           \
            {                                                           \
              MTH2D_tdstMatrix Mtemp;                                   \
                                                                        \
              MTH2D_M_vTranspMatrixWithoutBuffer(&Mtemp, Mat_A);                \
              MTH2D_M_vCopyMatrix( Mat_Dest, &Mtemp);                   \
            }                                                           \
            else                                                        \
            {                                                           \
             MTH2D_M_vTranspMatrixWithoutBuffer(Mat_Dest, Mat_A);       \
            } 
            
/* MTH2D_M_vTranpMatrix(MatDest, MatA)          : MatDest= transp MatA */
/* WARNING : THIS MUST DESAPEAR !!! */
#define MTH2D_M_vTranpMatrix  MTH2D_M_vTranspMatrix
 
/* ##M==================================================================================
NAME :          MTH2D_M_xTraceMatrix
DESCRIPTION :   Return the trace of a matrix
INPUT :         MatA : address of MTH2D_tdstMatrix
OUTPUT :        trace MatA : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xTraceMatrix(MatA)                                      \
          MTH_M_xAdd((MatA)->stCol_0.xX, (MatA)->stCol_1.xY)
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetIdentityMatrix
DESCRIPTION :   Set a matrix to the Identity matrix : MatDest= Id
INPUT :         MatDest : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetIdentityMatrix(MatDest)                             \
        {               (MatDest)->stCol_0.xX = MTH_C_ONE;              \
            (MatDest)->stCol_0.xY = MTH_C_ZERO;                         \
            (MatDest)->stCol_1.xX = MTH_C_ZERO;                         \
            (MatDest)->stCol_1.xY = MTH_C_ONE; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_bEqualMatrix
DESCRIPTION :   Test if two matrix are equal
INPUT :         MatA, MatB : address of MTH2D_tdstMatrix
OUTPUT :        MatA==MatB : Boolean
=======================================================================================*/
#define MTH2D_M_bEqualMatrix(MatA, MatB)                                \
                        (                                               \
            MTH_M_bEqual( (MatA)->stCol_0.xX, (MatB)->stCol_0.xX) &&    \
            MTH_M_bEqual( (MatA)->stCol_0.xY, (MatB)->stCol_0.xY) &&    \
            MTH_M_bEqual( (MatA)->stCol_1.xX, (MatB)->stCol_1.xX) &&    \
            MTH_M_bEqual( (MatA)->stCol_1.xY, (MatB)->stCol_1.xY)       \
            )
 
 
/* ##M==================================================================================
NAME :          MTH2D_M_bMatchMatrix
DESCRIPTION :   Test if two matrix are neary equal
INPUT :         MatA, MatB : address of MTH2D_tdstMatrix
                eps : MTH_tdxReal
OUTPUT :        (MatA match MatB to within about eps) : Boolean
=======================================================================================*/
#define MTH2D_M_bMatchMatrix(MatA, MatB, eps)                                           \
                        (                                                               \
            MTH_M_bEqualWithEpsilon( (MatA)->stCol_0.xX, (MatB)->stCol_0.xX, eps) &&    \
            MTH_M_bEqualWithEpsilon( (MatA)->stCol_0.xY, (MatB)->stCol_0.xY, eps) &&    \
            MTH_M_bEqualWithEpsilon( (MatA)->stCol_1.xX, (MatB)->stCol_1.xX, eps) &&    \
            MTH_M_bEqualWithEpsilon( (MatA)->stCol_1.xY, (MatB)->stCol_1.xY, eps) &&    \
            )
 
/* ##M==================================================================================
NAME :          MTH2D_M_vFillMatrix
DESCRIPTION :   Fill each matrix element with "a" value
INPUT :         MatDest : address of MTH2D_tdstMatrix
                a       : MTH_tdxReal
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vFillMatrix(MatDest, a)               \
           {    (MatDest)->stCol_0.xX = (a);          \
            (MatDest)->stCol_0.xY = (a);              \
            (MatDest)->stCol_1.xX = (a);              \
            (MatDest)->stCol_1.xY = (a); }
                                                                                                  
/* ##M==================================================================================
NAME :          MTH2D_M_xDetMatrix
DESCRIPTION :   Return the determinant of a matrix
INPUT :         MatA     : address of MTH2D_tdstMatrix
OUTPUT :        det MatA : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xDetMatrix(MatA)                                        \
                MTH_M_xSub(                                             \
                MTH_M_xMul( (MatA)->stCol_0.xX, (MatA)->stCol_1.xY ),   \
                MTH_M_xMul( (MatA)->stCol_0.xY, (MatA)->stCol_1.xX ))
 
/* ##M==================================================================================
NAME :          MTH2D_M_vInverMatrix
DESCRIPTION :   Invert a matrix : MatDest= Inv MatA
INPUT :         Matdest, MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vInverMatrix(MatDest, MatA)                                             \
        {                                                                               \
            MTH2D_tdstMatrix MatTmp;                                                    \
            MTH_tdxReal det;                                                            \
            MatTmp.stCol_0.xX = (MatA)->stCol_1.xY;                                     \
            MatTmp.stCol_0.xY = MTH_M_xNeg( (MatA)->stCol_0.xY );                       \
            MatTmp.stCol_1.xX = MTH_M_xNeg( (MatA)->stCol_1.xX );                       \
            MatTmp.stCol_1.xY = (MatA)->stCol_0.xX;                                     \
            det=MTH2D_M_xDetMatrix( MatA );                                             \
            MTH2D_M_vDivScalarMatrix(MatDest, &(MatTmp), det );                         \
            }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vGetMatrixColumnElements
DESCRIPTION :   Get x, y column col values in MatA
INPUT :         x, y : MTH_tdxReal
                col : int
                MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vGetMatrixColumnElements( Vx, Vy, col, MatA)  \
     if( col==0 )                                             \
       { *Vx= (MatA)->stCol_0.xX; *Vy= (MatA)->stCol_0.xY; }  \
     else                                                     \
       { *Vx= (MatA)->stCol_1.xX; *Vy= (MatA)->stCol_1.xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetMatrixColumnElements
DESCRIPTION :   Set column col x, y values in MatDest
INPUT :         MatDest : address of MTH2D_tdstMatrix
                x, y : MTH_tdxReal
                col : int
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetMatrixColumnElements( MatDest, Vx, Vy, col)      \
     if( col==0 )                                                    \
       {(MatDest)->stCol_0.xX= (Vx); (MatDest)->stCol_0.xY= (Vy); }  \
      else                                                           \
       {(MatDest)->stCol_1.xX= (Vx); (MatDest)->stCol_1.xY= (Vy); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_xGetMatrixElement
DESCRIPTION :   Return element at (lin, col) in MatASet
INPUT :         MatA : address of MTH2D_tdstMatrix
                lin, col : int
OUTPUT :        a : MTH_tdxReal
=======================================================================================*/
#define MTH2D_M_xGetMatrixElement( lin, col, MatA)                      \
        ( col==0 ) ?                                                    \
             ( lin==0 ? (MatA)->stCol_0.xX : (MatA)->stCol_0.xY) :      \
             ( lin==0 ? (MatA)->stCol_1.xX : (MatA)->stCol_1.xY) 
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetMatrixElement
DESCRIPTION :   Set a element in MatDest at (lin, col)
INPUT :         MatDest : address of MTH2D_tdstMatrix
                a : MTH_tdxReal
                lin, col : int
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetMatrixElement( MatDest, a, lin, col)                                \
            if( col==0 )                                                                \
            { if(lin==0){(MatDest)->stCol_0.xX=(a);}else{(MatDest)->stCol_0.xY=(a);}}   \
            else                                                                        \
            { if(lin==0){(MatDest)->stCol_1.xX=(a);}else{(MatDest)->stCol_1.xY=(a);}}
                           
 
 
 
/* ##-########################### 
   ## Matrix-Vector operations
   ############################## */   
 
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMulMatrixVectorWithoutBuffer
DESCRIPTION :   Multiply a Matrix to a Vector : VectDest= MatA*VectA, VectDest must be
                a other vector
INPUT :         VectDest : address of MTH2D_tdstVector
                MatA : address of MTH2D_tdstMatrix
                VectA : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMulMatrixVectorWithoutBuffer( VectDest, MatA, VectA)           \
        {     (VectDest)->xX = MTH_M_xAdd(                                      \
                                MTH_M_xMul( (MatA)->stCol_0.xX, (VectA)->xX),   \
                        MTH_M_xMul( (MatA)->stCol_1.xX, (VectA)->xY));          \
             (VectDest)->xY = MTH_M_xAdd(                                       \
                                MTH_M_xMul( (MatA)->stCol_0.xY, (VectA)->xX),   \
                        MTH_M_xMul( (MatA)->stCol_1.xY, (VectA)->xY)); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vMulMatrixVector
DESCRIPTION :   Multiply a Matrix to a Vector : VectDest= MatA*VectA
INPUT :         VectDest : address of MTH2D_tdstVector
                MatA : address of MTH2D_tdstMatrix
                VectA : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vMulMatrixVector( VectDest, MatA, VectA)                        \
        if( VectA==VectDest )                                                   \
        {                                                                       \
          MTH2D_tdstVector Vtmp;                                                \
                                                                                \
          MTH2D_M_vCopyVector( &Vtmp, VectA);                                   \
          MTH2D_M_vMulMatrixVectorWithoutBuffer( VectDest, MatA, &Vtmp);        \
        }                                                                       \
        else                                                                    \
        {                                                                       \
          MTH2D_M_vMulMatrixVectorWithoutBuffer( VectDest, MatA, VectA);        \
        }                                                      
 
/* ##M==================================================================================
NAME :          MTH2D_M_vGetColumnInMatrix
DESCRIPTION :   Get a Vector column of a Matrix to a Vector : VectDest= columm col of MatA
INPUT :         VectDest : address of MTH2D_tdstVector
                MatA : address of MTH2D_tdstMatrix
                col : int
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vGetColumnInMatrix( VectDest, MatA, col )       \
                        if( col==0 )                            \
            { (VectDest)->xX=(MatA)->stCol_0.xX;                \
              (VectDest)->xY=(MatA)->stCol_0.xY; }              \
            else                                                \
            { (VectDest)->xX=(MatA)->stCol_1.xX;                \
              (VectDest)->xY=(MatA)->stCol_1.xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetColumnInMatrix
DESCRIPTION :   Put a Vector to a column of a Matrix : Set columm col of MatDest with VectA
INPUT :         MatDest : address of MTH2D_tdstMatrix
                VectA : address of MTH2D_tdstVector
                col : int
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetColumnInMatrix( MatDest, VectA, col)        \
                        if( col==0 )                            \
            { (MatDest)->stCol_0.xX=(VectA)->xX;                \
              (MatDest)->stCol_0.xY=(VectA)->xY; }              \
            else                                                \
            { (MatDest)->stCol_1.xX=(VectA)->xX;                \
              (MatDest)->stCol_1.xY=(VectA)->xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vGetLineInMatrix
DESCRIPTION :   Get a Vector line of a Matrix to a Vector : VectDest= line lin of MatA
INPUT :         VectDest : address of MTH2D_tdstVector
                MatA : address of MTH2D_tdstMatrix
                lin : int
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vGetLineInMatrix( VectDest, MatA, lin )         \
                        if( lin==0 )                            \
            { (VectDest)->xX=(MatA)->stCol_0.xX;                \
              (VectDest)->xY=(MatA)->stCol_1.xX; }              \
            else                                                \
            { (VectDest)->xX=(MatA)->stCol_0.xY;                \
              (VectDest)->xY=(MatA)->stCol_1.xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetLineInMatrix
DESCRIPTION :   Put a Vector to a line of a Matrix : Set line lin of MatDest with VectA
INPUT :         MatDest : address of MTH2D_tdstMatrix
                VectA : address of MTH2D_tdstVector
                lin : int
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetLineInMatrix( MatDest, VectA, lin)          \
                        if( lin==0 )                            \
            { (MatDest)->stCol_0.xX=(VectA)->xX;                \
              (MatDest)->stCol_1.xX=(VectA)->xY; }              \
            else                                                \
            { (MatDest)->stCol_0.xY=(VectDest)->xX;             \
              (MatDest)->stCol_1.xY=(VectDest)->xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vGetDiagInMatrix
DESCRIPTION :   Get a Vector diagonal of a Matrix to a Vector : VectDest= diagonal of MatA
INPUT :         VectDest : address of MTH2D_tdstVector
                MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vGetDiagInMatrix( VectDest, MatA)               \
        {               (VectDest)->xX= (MatA)->stCol_0.xX;     \
                        (VectDest)->xY= (MatA)->stCol_1.xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetDiagInMatrix
DESCRIPTION :   Put a Vector to a diagonal of a Matrix : Set diagonal of MatDest with VectA
INPUT :         MatDest : address of MTH2D_tdstMatrix
                VectA : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetDiagInMatrix( MatDest, VectA)               \
        {               (MatDest)->stCol_0.xX= (VectA)->xX;     \
                        (MatDest)->stCol_1.xY= (VectA)->xY; }   
 
/* ##M==================================================================================
NAME :          MTH2D_M_vGetVectorsInMatrix
DESCRIPTION :   Get all column Matrix to two Vectors
INPUT :         VaDest, VbDest : address of MTH2D_tdstVector
                MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vGetVectorsInMatrix( VaDest, VbDest, MatA)              \
        {     (VaDest)->xX=(MatA)->stCol_0.xX;                          \
              (VaDest)->xY=(MatA)->stCol_0.xY;                          \
              (VbDest)->xX=(MatA)->stCol_1.xX;                          \
              (VbDest)->xY=(MatA)->stCol_1.xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vSetVectorsInMatrix
DESCRIPTION :   Set all column Matrix with two Vectors
INPUT :         MatDest : address of MTH2D_tdstMatrix
                VectA, VectB : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vSetVectorsInMatrix( MatDest, VectA, VectB)             \
        {    (MatDest)->stCol_0.xX=(VectA)->xX;                         \
             (MatDest)->stCol_0.xY=(VectA)->xY;                         \
             (MatDest)->stCol_1.xX=(VectB)->xX;                         \
             (MatDest)->stCol_1.xY=(VectB)->xY; }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vPrintfVector
DESCRIPTION :   Print a Vector : Name + its values
INPUT :         Name : string
                VectA : address of MTH2D_tdstVector
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vPrintfVector( Name, VectA)                             \
        {       printf("%s = \n |%e", (Name), (VectA)->xX);             \
                printf("\n |%e\n", (VectA)->xY); }
 
/* ##M==================================================================================
NAME :          MTH2D_M_vPrintfMatrix
DESCRIPTION :   Print a Matrix : Name + its values
INPUT :         Name : string
                MatA : address of MTH2D_tdstMatrix
OUTPUT :        void
=======================================================================================*/
#define MTH2D_M_vPrintfMatrix( Name, MatA)                                                              \
        {       printf("%s = \n |%e %e|", (Name), (MatA)->stCol_0.xX, (MatA)->stCol_1.xX );             \
                printf("\n |%e %e|\n", (MatA)->stCol_0.xY, (MatA)->stCol_1.xY ); }
            
            
                
#endif /*  MTH2D_H */