Subversion Repositories FlightCtrl

/*  matmatrix.c

The following functions are based on the Numerical Recipes.

*/

#include <math.h>

#include <stdio.h>

#include <string.h>

  FUNCTIONS

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

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

  Functionname:     matrix_sub                      */ /*!

  Description:      C = A - B

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

void matrix_sub( f32_t **a, f32_t **b, f32_t **c, int m, int n )

{

  int i,j;

  f32_t  *a_ptr, *b_ptr, *c_ptr;

  for( j = 1; j <= m; j++)

  {

    a_ptr = &a[j][1];

    b_ptr = &b[j][1];

    c_ptr = &c[j][1];

    for( i = 1; i <= n; i++)

    {

      *c_ptr++ = *a_ptr++ - *b_ptr++;

    }

  }

}

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

  Functionname:     matrix_add                      */ /*!

  Description:      C = A + B

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

void matrix_add( f32_t **a, f32_t **b, f32_t **c, int m, int n )

{

  int i,j;

  f32_t  *a_ptr, *b_ptr, *c_ptr;

  for( j = 1; j <= m; j++)

  {

    a_ptr = &a[j][1];

    b_ptr = &b[j][1];

    c_ptr = &c[j][1];

    for( i = 1; i <= n; i++)

    {

      *c_ptr++ = *a_ptr++ + *b_ptr++;

    }

  }

}

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

  Functionname:     matrix_mult                      */ /*!

  Description:      C =  A x  B

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

void matrix_mult( f32_t  **a, f32_t  **b, f32_t  **c,

                 int a_rows, int a_cols, int b_cols )

{

  int i, j, k;

  f32_t  *a_ptr;

  f32_t  temp;

  for( i = 1; i <= a_rows; i++)

  {

    a_ptr = &a[i][0];

    for( j = 1; j <= b_cols; j++ )

    {

      temp = 0.0;

      for( k = 1; k <= a_cols; k++ )

      {

        temp = temp + (a_ptr[k] * b[k][j]);

      }

      c[i][j] = temp;

    }

  }

}

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

  Functionname:     matrix_mult_transpose                      */ /*!

  Description:      C =  A x B^T

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

void matrix_mult_transpose( f32_t **a, f32_t **b, f32_t **c,

                           int a_rows, int a_cols, int b_cols )

{

  int i, j, k;

  f32_t  *a_ptr, *b_ptr;

  f32_t  temp;

  for( i = 1; i <= a_rows; i++)

  {

    a_ptr = &a[i][0];

    for( j = 1; j <= b_cols; j++ )

    {

      b_ptr = &b[j][1];

      temp = (f32_t)0;

      for( k = 1; k <= a_cols; k++ )

      {

        temp += a_ptr[ k ] * *b_ptr++;

      }

      c[i][j] = temp;

    }

  }

}

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

  Functionname:     matrix_mult_transpose                      */ /*!

  Description:      C = A^T x B

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

void matrix_transpose_mult( f32_t **A, f32_t **B, f32_t **C,

                           int a_rows, int a_cols, int b_cols )

{

  int i, j, k;

  f32_t  temp;

  for( i = 1; i <= a_rows; i++)

  {

    for( j = 1; j <= b_cols; j++ )

    {

      temp = 0.0;

      for( k = 1; k <= a_cols; k++ )

      {

        temp += A[ k ][ i ] * B[ k ][ j ];

      }

      C[ i ][ j ] = temp;

    }

  }

}

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

  Functionname:     matrix_copy                      */ /*!

  Description:      B = A

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

void matrix_copy( f32_t **src, f32_t **dst, int num_rows, int num_cols )

{

  int  i, j;

  for( i = 1; i <= num_rows; i++ )

  {

    for( j = 1; j <= num_cols; j++ )

    {

      dst[ i ][ j ] = src[ i ][ j ];

    }

  }

}

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

  Functionname:     matrix_alloc                      */ /*!

  Description:      B = A

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

f32_t **matrix(long row_low, long row_high, long col_low, long col_high)

{

  long i, NumOfRows=row_high-row_low+1,NumOfCols=col_high-col_low+1;

  f32_t **m;

  /* allocate pointers to rows */

  m=(f32_t **)malloc((size_t)((NumOfRows+1)

    *sizeof(f32_t *)));

  m += 1;

  m -= row_low;

  /* allocate rows and set pointers to them */

  m[row_low]=(f32_t *)malloc((size_t)((NumOfRows*NumOfCols+1)

    *sizeof(f32_t)));

  m[row_low] += 1;

  m[row_low] -= col_low;

  for(i=row_low+1;i<=row_high;i++) m[i]=m[i-1]+NumOfCols;

  {  /* init values with 0.0F */

    int x, y;

    for(y = 1; y <= NumOfRows; y++)

    {

      for(x = 1; x <= NumOfCols; x++)

      {

        m[y][x] = 0.F;

      }

    }

  }

  /* return pointer to array of pointers to rows */

  return m;

}

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

  Functionname:     free_matrix                      */ /*!

  Description:      

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

void free_matrix(f32_t  **m, long row_low, long row_high, long col_low, long col_high)

/* free a matrix allocated by matrix() */

{

  free((char*) (m[row_low]+col_low-1));

  free((char*) (m+row_low-1));

}

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

  Functionname:     take_inverse                      */ /*!

  Description:      B = A^-1

  @param[in]        

  @return           void

  @pre              -

  @post             -

  @author         Michael Walter  

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

void take_inverse( f32_t **A, f32_t **B, int size)

{

  gaussj( A, size, B);  

  matrix_copy( A, B, size, size);

}

/* gaussj()

    This function performs gauss-jordan elimination to solve a set

    of linear equations, at the same time generating the inverse of

    the A matrix.

    (C) Copr. 1986-92 Numerical Recipes Software `2$m.1.9-153.

*/

#define SWAP(a,b) {temp=(a);(a)=(b);(b)=temp;}

void gaussj(f32_t **a, int n, f32_t **b)

{

  int i,icol,irow,j,k,l,ll;

  f32_t  big,dum,pivinv,temp;

  int indxc[5];

  int indxr[5];

  int ipiv[5];

  irow = 0;

  icol = 0;

  for (j=1;j<=n;j++)

  {

    ipiv[j]=0;

  }

  for (i=1;i<=n;i++)

  {

    big=0.0;

    for (j=1;j<=n;j++)

    {

      if (ipiv[j] != 1)

      {

        for (k=1;k<=n;k++)

        {

          if (ipiv[k] == 0)

          {

            if (fabs(a[j][k]) >= big)

            {

              big=fabs(a[j][k]);

              irow=j;

              icol=k;

            }

          }

        }

      }

    }

    ++(ipiv[icol]);

    if (irow != icol)

    {

      for (l=1;l<=n;l++)

      {

        SWAP(a[irow][l],a[icol][l])

      }

    }

    indxr[i]=irow;

    indxc[i]=icol;

    pivinv=1.0/a[icol][icol];

    a[icol][icol]=1.0;

    for (l=1;l<=n;l++)

    {

      a[icol][l] *= pivinv;

    }

    for (ll=1;ll<=n;ll++)

    {

      if (ll != icol)

      {

        dum=a[ll][icol];

        a[ll][icol]=0.0;

        for (l=1;l<=n;l++)

        {

          a[ll][l] -= a[icol][l]*dum;

        }

      }

    }

  }

  for (l=n;l>=1;l--)

  {

    if (indxr[l] != indxc[l])

    {

      for (k=1;k<=n;k++)

      {

        SWAP(a[k][indxr[l]],a[k][indxc[l]]);

      }

    }

  }