Fawkes API
Fawkes Development Version
|
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