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 

Last updated 12 September 2005 21:38:45