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