pf_vector.h
Go to the documentation of this file.00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy 00004 * gerkey@usc.edu kaspers@robotics.usc.edu 00005 * 00006 * This library is free software; you can redistribute it and/or 00007 * modify it under the terms of the GNU Lesser General Public 00008 * License as published by the Free Software Foundation; either 00009 * version 2.1 of the License, or (at your option) any later version. 00010 * 00011 * This library is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00014 * Lesser General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU Lesser General Public 00017 * License along with this library; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00019 * 00020 */ 00021 /************************************************************************** 00022 * Desc: Vector functions 00023 * Author: Andrew Howard 00024 * Date: 10 Dec 2002 00025 * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $ 00026 *************************************************************************/ 00027 00028 #ifndef PF_VECTOR_H 00029 #define PF_VECTOR_H 00030 00031 #ifdef __cplusplus 00032 extern "C" { 00033 #endif 00034 00035 #include <stdio.h> 00036 00037 // The basic vector 00038 typedef struct 00039 { 00040 double v[3]; 00041 } pf_vector_t; 00042 00043 00044 // The basic matrix 00045 typedef struct 00046 { 00047 double m[3][3]; 00048 } pf_matrix_t; 00049 00050 00051 // Return a zero vector 00052 pf_vector_t pf_vector_zero(); 00053 00054 // Check for NAN or INF in any component 00055 int pf_vector_finite(pf_vector_t a); 00056 00057 // Print a vector 00058 void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt); 00059 00060 // Simple vector addition 00061 pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b); 00062 00063 // Simple vector subtraction 00064 pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b); 00065 00066 // Transform from local to global coords (a + b) 00067 pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b); 00068 00069 // Transform from global to local coords (a - b) 00070 pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b); 00071 00072 00073 // Return a zero matrix 00074 pf_matrix_t pf_matrix_zero(); 00075 00076 // Check for NAN or INF in any component 00077 int pf_matrix_finite(pf_matrix_t a); 00078 00079 // Print a matrix 00080 void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt); 00081 00082 // Compute the matrix inverse. Will also return the determinant, 00083 // which should be checked for underflow (indicated singular matrix). 00084 //pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det); 00085 00086 // Decompose a covariance matrix [a] into a rotation matrix [r] and a 00087 // diagonal matrix [d] such that a = r * d * r^T. 00088 void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a); 00089 00090 #ifdef __cplusplus 00091 } 00092 #endif 00093 00094 #endif