Fawkes API Fawkes Development Version
pf_vector.c
1
2/***************************************************************************
3 * pf_vector.c: Vector functions
4 *
5 * Created: Thu May 24 18:42:09 2012
6 * Copyright 2000 Brian Gerkey
7 * 2000 Kasper Stoy
8 * 2012 Tim Niemueller [www.niemueller.de]
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL file in the doc directory.
22 */
23
24/* From:
25 * Player - One Hell of a Robot Server (LGPL)
26 * Copyright (C) 2000 Brian Gerkey & Kasper Stoy
27 * gerkey@usc.edu kaspers@robotics.usc.edu
28 */
29/**************************************************************************
30 * Desc: Vector functions
31 * Author: Andrew Howard
32 * Date: 10 Dec 2002
33 *************************************************************************/
34
35#include <math.h>
36//#include <gsl/gsl_matrix.h>
37//#include <gsl/gsl_eigen.h>
38//#include <gsl/gsl_linalg.h>
39
40#include "pf_vector.h"
41#include "eig3.h"
42
43/// @cond EXTERNAL
44
45// Return a zero vector
46pf_vector_t pf_vector_zero()
47{
48 pf_vector_t c;
49
50 c.v[0] = 0.0;
51 c.v[1] = 0.0;
52 c.v[2] = 0.0;
53
54 return c;
55}
56
57
58// Check for NAN or INF in any component
59int pf_vector_finite(pf_vector_t a)
60{
61 int i;
62
63 for (i = 0; i < 3; i++)
64 if (!finite(a.v[i]))
65 return 0;
66
67 return 1;
68}
69
70
71// Print a vector
72void pf_vector_fprintf(pf_vector_t a, FILE *file, const char *fmt)
73{
74 int i;
75
76 for (i = 0; i < 3; i++)
77 {
78 fprintf(file, fmt, a.v[i]);
79 fprintf(file, " ");
80 }
81 fprintf(file, "\n");
82
83 return;
84}
85
86
87// Simple vector addition
88pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b)
89{
90 pf_vector_t c;
91
92 c.v[0] = a.v[0] + b.v[0];
93 c.v[1] = a.v[1] + b.v[1];
94 c.v[2] = a.v[2] + b.v[2];
95
96 return c;
97}
98
99
100// Simple vector subtraction
101pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b)
102{
103 pf_vector_t c;
104
105 c.v[0] = a.v[0] - b.v[0];
106 c.v[1] = a.v[1] - b.v[1];
107 c.v[2] = a.v[2] - b.v[2];
108
109 return c;
110}
111
112
113// Transform from local to global coords (a + b)
114pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
115{
116 pf_vector_t c;
117
118 c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
119 c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
120 c.v[2] = b.v[2] + a.v[2];
121 c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
122
123 return c;
124}
125
126
127// Transform from global to local coords (a - b)
128pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b)
129{
130 pf_vector_t c;
131
132 c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]);
133 c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]);
134 c.v[2] = a.v[2] - b.v[2];
135 c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
136
137 return c;
138}
139
140
141// Return a zero matrix
142pf_matrix_t pf_matrix_zero()
143{
144 int i, j;
145 pf_matrix_t c;
146
147 for (i = 0; i < 3; i++)
148 for (j = 0; j < 3; j++)
149 c.m[i][j] = 0.0;
150
151 return c;
152}
153
154
155// Check for NAN or INF in any component
156int pf_matrix_finite(pf_matrix_t *a)
157{
158 int i, j;
159
160 for (i = 0; i < 3; i++)
161 for (j = 0; j < 3; j++)
162 if (!finite(a->m[i][j]))
163 return 0;
164
165 return 1;
166}
167
168
169// Print a matrix
170void pf_matrix_fprintf(pf_matrix_t *a, FILE *file, const char *fmt)
171{
172 int i, j;
173
174 for (i = 0; i < 3; i++)
175 {
176 for (j = 0; j < 3; j++)
177 {
178 fprintf(file, fmt, a->m[i][j]);
179 fprintf(file, " ");
180 }
181 fprintf(file, "\n");
182 }
183 return;
184}
185
186
187/*
188// Compute the matrix inverse
189pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det)
190{
191 double lndet;
192 int signum;
193 gsl_permutation *p;
194 gsl_matrix_view A, Ai;
195
196 pf_matrix_t ai;
197
198 A = gsl_matrix_view_array((double*) a.m, 3, 3);
199 Ai = gsl_matrix_view_array((double*) ai.m, 3, 3);
200
201 // Do LU decomposition
202 p = gsl_permutation_alloc(3);
203 gsl_linalg_LU_decomp(&A.matrix, p, &signum);
204
205 // Check for underflow
206 lndet = gsl_linalg_LU_lndet(&A.matrix);
207 if (lndet < -1000)
208 {
209 //printf("underflow in matrix inverse lndet = %f", lndet);
210 gsl_matrix_set_zero(&Ai.matrix);
211 }
212 else
213 {
214 // Compute inverse
215 gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix);
216 }
217
218 gsl_permutation_free(p);
219
220 if (det)
221 *det = exp(lndet);
222
223 return ai;
224}
225*/
226
227
228// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
229// matrix [d] such that a = r d r^T.
230void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t *a)
231{
232 int i, j;
233 /*
234 gsl_matrix *aa;
235 gsl_vector *eval;
236 gsl_matrix *evec;
237 gsl_eigen_symmv_workspace *w;
238
239 aa = gsl_matrix_alloc(3, 3);
240 eval = gsl_vector_alloc(3);
241 evec = gsl_matrix_alloc(3, 3);
242 */
243
244 double aa[3][3];
245 double eval[3];
246 double evec[3][3];
247
248 for (i = 0; i < 3; i++)
249 {
250 for (j = 0; j < 3; j++)
251 {
252 //gsl_matrix_set(aa, i, j, a.m[i][j]);
253 aa[i][j] = a->m[i][j];
254 }
255 }
256
257 // Compute eigenvectors/values
258 /*
259 w = gsl_eigen_symmv_alloc(3);
260 gsl_eigen_symmv(aa, eval, evec, w);
261 gsl_eigen_symmv_free(w);
262 */
263
264 eigen_decomposition(aa,evec,eval);
265
266 *d = pf_matrix_zero();
267 for (i = 0; i < 3; i++)
268 {
269 //d->m[i][i] = gsl_vector_get(eval, i);
270 d->m[i][i] = eval[i];
271 for (j = 0; j < 3; j++)
272 {
273 //r->m[i][j] = gsl_matrix_get(evec, i, j);
274 r->m[i][j] = evec[i][j];
275 }
276 }
277
278 //gsl_matrix_free(evec);
279 //gsl_vector_free(eval);
280 //gsl_matrix_free(aa);
281
282 return;
283}
284
285/// @endcond