/*
** PURPOSE: Perform simple matrix math such as vector addition, cross-product,
**          matrix multiply, etc.
** AUTHOR:  Rob McEwen
** DATE:    99/7/7
** NOTES:   1) All vectors and matrices are assumed to have dimension 3.  
**
*/
#include <math.h>
#include "matrixMath.h"
/*#define UNITTEST*/
#define DIM 3
/*
** Take the standard 2-norm. This one returns the answer, since it is a scalar.
*/
double Vnorm( double v[] )
{
   double Vnorm2 = 0.;
   int i;
   for(i=0; i<DIM; i++) Vnorm2 += pow(v[i],2.);
   return( sqrt( Vnorm2 ) );
}
/*
** vsum = v1 + v2.  
*/
void Vadd( double vsum[], double v1[], double v2[] )
{
   int i;
   for(i=0; i<DIM; i++) vsum[i] = v1[i] + v2[i];
   return;
}
/*
** vcross = v1 x v2.  
*/
void Vcross( double vcross[], double v1[], double v2[] )
{
   vcross[0] = v1[1]*v2[2] - v1[2]*v2[1];
   vcross[1] = v1[2]*v2[0] - v1[0]*v2[2];
   vcross[2] = v1[0]*v2[1] - v1[1]*v2[0];
}
/*
** v_B = T_B_A * v_A
*/
void TVMult( double *v_B, double T_B_A[DIM][DIM], double *v_A )
{
   int i, j;
   for( i=0; i<DIM; i++ )
   {
     v_B[i] = 0.0;
     for( j=0; j<DIM; j++ ) v_B[i] += T_B_A[i][j]*v_A[j];
   }
   return;
}
/*
** T_B_A = T_A_B'
*/
void TTransp( double T_B_A[DIM][DIM], double T_A_B[DIM][DIM] )
{
   int i, j;
   for( i=0; i<DIM; i++ )
   {
     for( j=0; j<DIM; j++ ) T_B_A[j][i] = T_A_B[i][j];
   }
   return;
}
/*
** T_C_A = T_C_B * T_B_A
*/
void TTMult( double T_C_A[DIM][DIM], 
             double T_C_B[DIM][DIM], 
             double T_B_A[DIM][DIM] )
{
   int i, j, k;
   for( i=0; i<DIM; i++ )
   {
     for( j=0; j<DIM; j++ ) 
     {
       T_C_A[i][j] = 0.0;
       for( k=0; k<DIM; k++ ) T_C_A[i][j] += T_C_B[i][k]*T_B_A[k][j];
     }
   }
   return;
}

void printMat( double T[DIM][DIM] )
{
   int i,j;
   for(i=0; i<DIM; i++) 
   {
     for(j=0; j<DIM; j++) printf(" %10f  ", T[i][j]);
     printf("\n");
   }
   return;
}
/*
** T_B_A is a direction cosine matrix that recoordinatizes a vector from the
** A frame into the B frame.  B is displaced from A by a right-handed rotation
** about the x axis.
*/
void TRoll( double T_B_A[DIM][DIM], double roll )
{
   double sr, cr;
   sr = sin(roll);
   cr = cos(roll);

   T_B_A[0][0] = 1.;       T_B_A[0][1] = 0.;       T_B_A[0][2] = 0.;
   T_B_A[1][0] = 0.;       T_B_A[1][1] = cr;       T_B_A[1][2] = sr;
   T_B_A[2][0] = 0.;       T_B_A[2][1] =-sr;       T_B_A[2][2] = cr;
}
/*
** T_B_A is a direction cosine matrix that recoordinatizes a vector from the
** A frame into the B frame.  B is displaced from A by a right-handed rotation
** about the y axis.
*/
void TPitch( double T_B_A[DIM][DIM], double pitch )
{
   double sp, cp;
   sp = sin(pitch);
   cp = cos(pitch);

   T_B_A[0][0] = cp;       T_B_A[0][1] = 0.;       T_B_A[0][2] =-sp;
   T_B_A[1][0] = 0.;       T_B_A[1][1] = 1.;       T_B_A[1][2] = 0.;
   T_B_A[2][0] = sp;       T_B_A[2][1] = 0.;       T_B_A[2][2] = cp;
}
/*
** T_B_A is a direction cosine matrix that recoordinatizes a vector from the
** A frame into the B frame.  B is displaced from A by a right-handed rotation
** about the z axis.
*/
void TYaw( double T_B_A[DIM][DIM], double yaw )
{
   double sy, cy;
   sy = sin(yaw);
   cy = cos(yaw);

   T_B_A[0][0] = cy;       T_B_A[0][1] = sy;       T_B_A[0][2] = 0.;
   T_B_A[1][0] =-sy;       T_B_A[1][1] = cy;       T_B_A[1][2] = 0.;
   T_B_A[2][0] = 0.;       T_B_A[2][1] = 0.;       T_B_A[2][2] = 1.;
}




/*
** PURPOSE: Test this stuff.  Compile and link with the command:
** gcc -lm matrixMath.c -o matrixMath
*/
#ifdef UNITTEST
#define PI 3.14159265358979
void main(void)
{
   int i, j;
   double normV1, Vsum[3], vc[3], v3[3];
   double v1[] = {1., 2., 3.};
   double v2[] = {4., 5., 6.};
   double A[3][3] = { {1.,2.,3.},
                      {4.,5.,6.},
                      {7.,8.,9.}};
   double B[3][3] = { {10.,11.,12.},
                      {13.,14.,15.},
                      {16.,17.,18.}};
   double ATransp[3][3], C[3][3], TR[3][3], TRTransp[3][3], TP[3][3], TY[3][3];
   double roll = PI/4., pitch=PI/6., yaw = PI/8.;

/*
** Try a triply dimensioned array:
*/
#if 0
   /*Wrong!*/
   double T_S_R[2][3][3] = { { {1.,2.,3.},  {10.,11.,12.} },
                             { {4.,5.,6.},  {13.,14.,15.} },
                             { {7.,8.,9.},  {16.,17.,18.} }};
   /*Wrong!*/
   double T_S_R[2][3][3] = { { {1.,10.},  {4.,13.}, {7.,16.}},
                             { {2.,11.},  {5.,14.}, {8.,17.}},
                             { {3.,12.},  {6.,15.}, {9.,18.}}};
   /*Correct!*/
   double T_S_R[2][3][3] = { { {1.,2.,3.},   {4.,5.,6.},   {7.,8.,9.}    },
                             { {10.,11.,12.},{13.,14.,15.},{16.,17.,18.} } };
#endif
   /*Correct and easier to read.*/
   double T_S_R[2][3][3] = { { {1.,2.,3.},   
                               {4.,5.,6.},   
                               {7.,8.,9.}     },
                             { {10.,11.,12.},
                               {13.,14.,15.},
                               {16.,17.,18.}  } };

   normV1 = Vnorm( v1 );
   printf(" The norm should be 3.74165738677394; it is %f\n", normV1);

   Vadd( Vsum, v1, v2 );
   printf(" The sum of v1 and v2 is:\n");
   for(i=0; i<3; i++) printf(" %f + %f = %f\n", v1[i], v2[i], Vsum[i]);

   Vadd( Vsum, Vsum, v2 );
   printf(" The sum of Vsum and v2 is:\n");
   for(i=0; i<3; i++) printf(" Vsum = %f\n", Vsum[i]);

   Vcross( vc, v1, v2 );
   printf(" The cross product of v1 and v2 is:\n");
   for(i=0; i<3; i++) printf(" %f  %f = %f\n", v1[i], v2[i], vc[i]);

   printf(" The A matrix is:\n");
   for(i=0; i<3; i++) 
   {
     for(j=0; j<3; j++) printf(" %f  ", A[i][j]);
     printf("\n");
   }
   
   TVMult(v3, A, v2);
   printf(" A*v2 = \n");
   for(i=0; i<3; i++) printf(" %f\n", v3[i]);

   TTransp( ATransp, A );
   printf(" The transpose of A is:\n");
   printMat( ATransp );

   TTMult(C, A, B);
   printf(" The matrix B is:\n");
   printMat(B);
   printf(" The product A*B is:\n");
   printMat(C);

   TRoll(TR, roll);
   printf(" The roll DC matrix is:\n");
   printMat(TR);

   TTransp(TRTransp, TR);
   TTMult (C, TRTransp, TR);
   printf(" TR'*TR = \n");
   printMat(C);

   TPitch(TP, pitch);
   printf(" The pitch DC matrix is:\n");
   printMat(TP);

   TYaw(TY, yaw);
   printf(" The yaw DC matrix is:\n");
   printMat(TY);
/*
** Print out the triply dimensioned array.
*/
   printf(" T_S_R[0] is:\n");
   printMat(T_S_R[0]);
   printf(" T_S_R[1] is:\n");
   printMat(T_S_R[1]);

}
#endif