sp_matrix.h
00001 /* 00002 J. Neira 00003 J. A. Castellanos 00004 Robotics and Real Time Group 00005 University of Zaragoza 00006 00007 sp_matrix.h 00008 Implements basic MATRIX operations 00009 */ 00010 /* 00011 * This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU General Public License for more details. 00020 * 00021 * You should have received a copy of the GNU General Public License 00022 * along with this program; if not, write to the Free Software 00023 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00024 * 00025 */ 00026 00027 #ifndef _SP_MATRIX_H 00028 #define _SP_MATRIX_H 00029 00030 #ifdef __cplusplus 00031 extern "C" { 00032 #endif 00033 00034 #define MAX_ROWS (7) 00035 #define MAX_COLS (7) 00036 00037 typedef struct { 00038 int rows; 00039 int cols; 00040 float data[MAX_ROWS][MAX_COLS]; 00041 } MATRIX; 00042 00043 typedef struct { 00044 int elements; 00045 float data[MAX_ROWS]; 00046 } VECTOR; 00047 00048 #define DOF (3) 00049 00050 typedef struct { 00051 int mat[DOF]; 00052 int range; 00053 } BMAT; 00054 00055 #define MROWS(m) ((m).rows) 00056 #define MCOLS(m) ((m).cols) 00057 #define MDATA(m,i,j) ((m).data[i][j]) 00058 00059 #define VELEMENTS(v) ((v).elements) 00060 #define VDATA(v,i) ((v).data[i]) 00061 00062 #define M_SQUARE(m) ((m).rows == (m).cols) 00063 #define M_COMPAT_DIM(m, n) ((m).cols == (n).rows) 00064 #define M_EQUAL_DIM(m, n) (((m).rows == (n).rows) && ((m).cols == (n).cols)) 00065 #define V_EQUAL_DIM(v, w) (((v).elements == (w).elements)) 00066 #define MV_COMPAT_DIM(m, v) ((m).cols == (v).elements) 00067 00068 #define FIRST(b) ((b).mat[0]) 00069 #define SECOND(b) ((b).mat[1]) 00070 #define THIRD(b) ((b).mat[2]) 00071 #define RANGE(b) ((b).range) 00072 00073 #define SQUARE(x) ((x)*(x)) 00074 00075 MATRIX create_matrix (int rows, int cols); 00076 void initialize_matrix (MATRIX *m, int rows, int cols); 00077 void diagonal_matrix (MATRIX *m, int dim, float el1, float el2, float el3); 00078 void print_matrix (char *message, MATRIX const *m); 00079 VECTOR create_vector (int elements); 00080 void initialize_vector (VECTOR *v, int elements); 00081 void print_vector (char *message, VECTOR const *v); 00082 float cross_product (MATRIX const *m, int f1, int c1, int f2, int c2); 00083 int determinant (MATRIX const *m, float *result); 00084 int inverse_matrix (MATRIX const *m, MATRIX *n); 00085 int multiply_matrix_vector (MATRIX const *m, VECTOR const *v, VECTOR *r); 00086 00087 #ifdef __cplusplus 00088 } 00089 #endif 00090 00091 #endif 00092