Fawkes API  Fawkes Development Version
pf_vector.c
00001 
00002 /***************************************************************************
00003  *  pf_vector.c: Vector functions
00004  *
00005  *  Created: Thu May 24 18:42:09 2012
00006  *  Copyright  2000  Brian Gerkey
00007  *             2000  Kasper Stoy
00008  *             2012  Tim Niemueller [www.niemueller.de]
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 Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 /*  From:
00025  *  Player - One Hell of a Robot Server (LGPL)
00026  *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
00027  *                      gerkey@usc.edu    kaspers@robotics.usc.edu
00028  */
00029 /**************************************************************************
00030  * Desc: Vector functions
00031  * Author: Andrew Howard
00032  * Date: 10 Dec 2002
00033  *************************************************************************/
00034 
00035 #include <math.h>
00036 //#include <gsl/gsl_matrix.h>
00037 //#include <gsl/gsl_eigen.h>
00038 //#include <gsl/gsl_linalg.h>
00039 
00040 #include "pf_vector.h"
00041 #include "eig3.h"
00042 
00043 /// @cond EXTERNAL
00044 
00045 // Return a zero vector
00046 pf_vector_t pf_vector_zero()
00047 {
00048   pf_vector_t c;
00049 
00050   c.v[0] = 0.0;
00051   c.v[1] = 0.0;
00052   c.v[2] = 0.0;
00053   
00054   return c;
00055 }
00056 
00057 
00058 // Check for NAN or INF in any component
00059 int pf_vector_finite(pf_vector_t a)
00060 {
00061   int i;
00062   
00063   for (i = 0; i < 3; i++)
00064     if (!finite(a.v[i]))
00065       return 0;
00066   
00067   return 1;
00068 }
00069 
00070 
00071 // Print a vector
00072 void pf_vector_fprintf(pf_vector_t a, FILE *file, const char *fmt)
00073 {
00074   int i;
00075 
00076   for (i = 0; i < 3; i++)
00077   {
00078     fprintf(file, fmt, a.v[i]);
00079     fprintf(file, " ");
00080   }
00081   fprintf(file, "\n");
00082 
00083   return;     
00084 }
00085 
00086 
00087 // Simple vector addition
00088 pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b)
00089 {
00090   pf_vector_t c;
00091 
00092   c.v[0] = a.v[0] + b.v[0];
00093   c.v[1] = a.v[1] + b.v[1];
00094   c.v[2] = a.v[2] + b.v[2];
00095   
00096   return c;
00097 }
00098 
00099 
00100 // Simple vector subtraction
00101 pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b)
00102 {
00103   pf_vector_t c;
00104 
00105   c.v[0] = a.v[0] - b.v[0];
00106   c.v[1] = a.v[1] - b.v[1];
00107   c.v[2] = a.v[2] - b.v[2];
00108   
00109   return c;
00110 }
00111 
00112 
00113 // Transform from local to global coords (a + b)
00114 pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
00115 {
00116   pf_vector_t c;
00117 
00118   c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
00119   c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
00120   c.v[2] = b.v[2] + a.v[2];
00121   c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
00122   
00123   return c;
00124 }
00125 
00126 
00127 // Transform from global to local coords (a - b)
00128 pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b)
00129 {
00130   pf_vector_t c;
00131 
00132   c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]);
00133   c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]);
00134   c.v[2] = a.v[2] - b.v[2];
00135   c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
00136   
00137   return c;
00138 }
00139 
00140 
00141 // Return a zero matrix
00142 pf_matrix_t pf_matrix_zero()
00143 {
00144   int i, j;
00145   pf_matrix_t c;
00146 
00147   for (i = 0; i < 3; i++)
00148     for (j = 0; j < 3; j++)
00149       c.m[i][j] = 0.0;
00150         
00151   return c;
00152 }
00153 
00154 
00155 // Check for NAN or INF in any component
00156 int pf_matrix_finite(pf_matrix_t a)
00157 {
00158   int i, j;
00159   
00160   for (i = 0; i < 3; i++)
00161     for (j = 0; j < 3; j++)
00162       if (!finite(a.m[i][j]))
00163         return 0;
00164   
00165   return 1;
00166 }
00167 
00168 
00169 // Print a matrix
00170 void pf_matrix_fprintf(pf_matrix_t a, FILE *file, const char *fmt)
00171 {
00172   int i, j;
00173 
00174   for (i = 0; i < 3; i++)
00175   {
00176     for (j = 0; j < 3; j++)
00177     {
00178       fprintf(file, fmt, a.m[i][j]);
00179       fprintf(file, " ");
00180     }
00181     fprintf(file, "\n");
00182   }
00183   return;     
00184 }
00185 
00186 
00187 /*
00188 // Compute the matrix inverse
00189 pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det)
00190 {
00191   double lndet;
00192   int signum;
00193   gsl_permutation *p;
00194   gsl_matrix_view A, Ai;
00195 
00196   pf_matrix_t ai;
00197 
00198   A = gsl_matrix_view_array((double*) a.m, 3, 3);
00199   Ai = gsl_matrix_view_array((double*) ai.m, 3, 3);
00200   
00201   // Do LU decomposition
00202   p = gsl_permutation_alloc(3);
00203   gsl_linalg_LU_decomp(&A.matrix, p, &signum);
00204 
00205   // Check for underflow
00206   lndet = gsl_linalg_LU_lndet(&A.matrix);
00207   if (lndet < -1000)
00208   {
00209     //printf("underflow in matrix inverse lndet = %f", lndet);
00210     gsl_matrix_set_zero(&Ai.matrix);
00211   }
00212   else
00213   {
00214     // Compute inverse
00215     gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix);
00216   }
00217 
00218   gsl_permutation_free(p);
00219 
00220   if (det)
00221     *det = exp(lndet);
00222 
00223   return ai;
00224 }
00225 */
00226 
00227 
00228 // Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
00229 // matrix [d] such that a = r d r^T.
00230 void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a)
00231 {
00232   int i, j;
00233   /*
00234   gsl_matrix *aa;
00235   gsl_vector *eval;
00236   gsl_matrix *evec;
00237   gsl_eigen_symmv_workspace *w;
00238 
00239   aa = gsl_matrix_alloc(3, 3);
00240   eval = gsl_vector_alloc(3);
00241   evec = gsl_matrix_alloc(3, 3);
00242   */
00243 
00244   double aa[3][3];
00245   double eval[3];
00246   double evec[3][3];
00247 
00248   for (i = 0; i < 3; i++)
00249   {
00250     for (j = 0; j < 3; j++)
00251     {
00252       //gsl_matrix_set(aa, i, j, a.m[i][j]);
00253       aa[i][j] = a.m[i][j];
00254     }
00255   }
00256 
00257   // Compute eigenvectors/values
00258   /*
00259   w = gsl_eigen_symmv_alloc(3);
00260   gsl_eigen_symmv(aa, eval, evec, w);
00261   gsl_eigen_symmv_free(w);
00262   */
00263 
00264   eigen_decomposition(aa,evec,eval);
00265 
00266   *d = pf_matrix_zero();
00267   for (i = 0; i < 3; i++)
00268   {
00269     //d->m[i][i] = gsl_vector_get(eval, i);
00270     d->m[i][i] = eval[i];
00271     for (j = 0; j < 3; j++)
00272     {
00273       //r->m[i][j] = gsl_matrix_get(evec, i, j);
00274       r->m[i][j] = evec[i][j];
00275     }
00276   }
00277   
00278   //gsl_matrix_free(evec);
00279   //gsl_vector_free(eval);
00280   //gsl_matrix_free(aa);
00281   
00282   return;
00283 }
00284 
00285 /// @endcond