sp_matrix.h

Go to the documentation of this file.

/**************************************************************************************************
 Software License Agreement (BSD License)

 Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
 All rights reserved.

 Redistribution and use in source and binary forms, with or without modification, are permitted
 provided that the following conditions are met:

  *Redistributions of source code must retain the above copyright notice, this list of
   conditions and the following disclaimer.
  *Redistributions in binary form must reproduce the above copyright notice, this list of
   conditions and the following disclaimer in the documentation and/or other materials provided
   with the distribution.
  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
   endorse or promote products derived from this software without specific prior written permission.
 
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
 IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***************************************************************************************************/
/*
  J. Neira
  J. A. Castellanos
  Robotics and Real Time Group
  University of Zaragoza
  
  sp_matrix.h
  Implements basic MATRIX operations
*/
/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#ifndef _SP_MATRIX_H
#define _SP_MATRIX_H

#ifdef __cplusplus
extern "C" {
#endif

#define MAX_ROWS (7)
#define MAX_COLS (7)

typedef struct {
  int   rows;
  int   cols;
  float data[MAX_ROWS][MAX_COLS];
} MATRIX;

typedef struct {
  int   elements;
  float data[MAX_ROWS];
} VECTOR;

#define DOF (3)

typedef struct {
  int mat[DOF];
  int range;
} BMAT;

#define MROWS(m)             ((m).rows)
#define MCOLS(m)             ((m).cols)
#define MDATA(m,i,j)         ((m).data[i][j])

#define VELEMENTS(v)         ((v).elements)
#define VDATA(v,i)           ((v).data[i])

#define M_SQUARE(m)          ((m).rows == (m).cols)
#define M_COMPAT_DIM(m, n)   ((m).cols == (n).rows)
#define M_EQUAL_DIM(m, n)    (((m).rows == (n).rows)  && ((m).cols == (n).cols))
#define V_EQUAL_DIM(v, w)    (((v).elements == (w).elements))
#define MV_COMPAT_DIM(m, v)  ((m).cols == (v).elements)

#define FIRST(b)             ((b).mat[0])
#define SECOND(b)            ((b).mat[1])
#define THIRD(b)             ((b).mat[2])
#define RANGE(b)             ((b).range)

#define SQUARE(x)            ((x)*(x))

MATRIX create_matrix (int rows, int cols);
void initialize_matrix (MATRIX *m, int rows, int cols);
void diagonal_matrix (MATRIX *m, int dim, float el1, float el2, float el3);
void print_matrix (char *message, MATRIX const *m);
VECTOR create_vector (int elements);
void initialize_vector (VECTOR *v, int elements);
void print_vector (char *message, VECTOR const *v);
float cross_product (MATRIX const *m, int f1, int c1, int f2, int c2);
int determinant (MATRIX const *m, float *result);
int inverse_matrix (MATRIX const *m, MATRIX *n);
int multiply_matrix_vector (MATRIX const *m, VECTOR const *v, VECTOR *r);

#ifdef __cplusplus
}
#endif

#endif