Subversion Repositories FlightCtrl

/*  matmatrix.c

The following functions are based on the Numerical Recipes.

*/

/*****************************************************************************
  INCLUDES
**************************************************************************** */

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "matmatrix.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]]);
      }
    }
  }
}
#undef SWAP