Fawkes API  Fawkes Development Version
pf_vector.h
1 
2 /***************************************************************************
3  * pf_vector.h: Vector functions
4  *
5  * Created: Thu May 24 18:41:48 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 #ifndef PF_VECTOR_H
36 #define PF_VECTOR_H
37 
38 #ifdef __cplusplus
39 extern "C" {
40 #endif
41 
42 #include <stdio.h>
43 
44 /// @cond EXTERNAL
45 
46 // The basic vector
47 typedef struct
48 {
49  double v[3];
50 } pf_vector_t;
51 
52 
53 // The basic matrix
54 typedef struct
55 {
56  double m[3][3];
57 } pf_matrix_t;
58 
59 
60 // Return a zero vector
61 pf_vector_t pf_vector_zero();
62 
63 // Check for NAN or INF in any component
64 int pf_vector_finite(pf_vector_t a);
65 
66 // Print a vector
67 void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt);
68 
69 // Simple vector addition
70 pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
71 
72 // Simple vector subtraction
73 pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
74 
75 // Transform from local to global coords (a + b)
76 pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
77 
78 // Transform from global to local coords (a - b)
79 pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
80 
81 
82 // Return a zero matrix
83 pf_matrix_t pf_matrix_zero();
84 
85 // Check for NAN or INF in any component
86 int pf_matrix_finite(pf_matrix_t a);
87 
88 // Print a matrix
89 void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt);
90 
91 // Compute the matrix inverse. Will also return the determinant,
92 // which should be checked for underflow (indicated singular matrix).
93 //pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
94 
95 // Decompose a covariance matrix [a] into a rotation matrix [r] and a
96 // diagonal matrix [d] such that a = r * d * r^T.
97 void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a);
98 
99 /// @endcond
100 
101 #ifdef __cplusplus
102 }
103 #endif
104 
105 #endif