1,13 → 1,378 |
/* matmatrix.c |
|
The following functions are based on the Numerical Recipes. |
|
*/ |
|
/***************************************************************************** |
INCLUDES |
**************************************************************************** */ |
|
#include "mat.h" |
#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 |
|
|