Subversion Repositories FlightCtrl

/*  matmatrix.c

The following functions are based on the Numerical Recipes.

*/

#ifndef __MATMATH_H__
#define __MATMATH_H__

/*****************************************************************************
  INCLUDES
**************************************************************************** */
#include "math.h"
#include "float.h"

/*****************************************************************************
  TYPEDEFS
*****************************************************************************/
#define f32_t float
#define ui32_t unsigned int
#define i32_t int

#define PI 3.1415926535897932384626433832795

#define matSetFull(matM,Row,Column,X)               matM[Row+1][Column+1] = (X)
#define matSetDiagonal(matM,Row,Column,X)           matM[Row+1][Column+1] = (X)
#define matGetFull(matM,Row,Column,X)              *(X) = matM[Row+1][Column+1]
#define matGetDiagonal(matM,Row,Column,X)          *(X) = matM[Row+1][Column+1]


/* B = A */
extern void matrix_copy( f32_t **A, f32_t **B, int rows,
                        int cols );

/* C = A + B */
extern void matrix_add( f32_t **A, f32_t **B, f32_t **C, int m, int n );

/* C = A - B */
extern void matrix_sub( f32_t **A, f32_t **B, f32_t **C, int m, int n );

/*   C =  A x  B , A(a_rows x a_cols), B(a_cols x b_cols) */
extern void matrix_mult( f32_t **A, f32_t **B, f32_t **C,
                        int a_rows, int a_cols, int b_cols );

/*  C =  A x B^T, A(a_rows x a_cols), B(b_cols x a_cols) */
extern void matrix_mult_transpose( f32_t **A, f32_t **B, f32_t **C,
                                  int a_rows, int a_cols, int b_cols );

/* C = A^T x B, A(a_cols x a_rows), B(a_cols x b_cols) */
extern void matrix_transpose_mult( f32_t **A, f32_t **B, f32_t **C,
                                  int a_rows, int a_cols, int b_cols );

/* B = A^-1 */
extern void take_inverse( f32_t **A, f32_t **B, int size);
extern void gaussj( f32_t **A, int n, f32_t **B);

/* Matrix allocation routines */
extern f32_t **matrix(long row_low, long row_high, long column_low, long column_high);

/* Deallocation routines */
extern void free_matrix(f32_t **m, long row_low, long row_high, long column_low, long column_high);
#endif