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 // The basic matrix
53 typedef struct
54 {
55  double m[3][3];
56 } pf_matrix_t;
57 
58 // Return a zero vector
59 pf_vector_t pf_vector_zero();
60 
61 // Check for NAN or INF in any component
62 int pf_vector_finite(pf_vector_t a);
63 
64 // Print a vector
65 void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt);
66 
67 // Simple vector addition
68 pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
69 
70 // Simple vector subtraction
71 pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
72 
73 // Transform from local to global coords (a + b)
74 pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
75 
76 // Transform from global to local coords (a - b)
77 pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
78 
79 // Return a zero matrix
80 pf_matrix_t pf_matrix_zero();
81 
82 // Check for NAN or INF in any component
83 int pf_matrix_finite(pf_matrix_t *a);
84 
85 // Print a matrix
86 void pf_matrix_fprintf(pf_matrix_t *s, FILE *file, const char *fmt);
87 
88 // Compute the matrix inverse. Will also return the determinant,
89 // which should be checked for underflow (indicated singular matrix).
90 //pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
91 
92 // Decompose a covariance matrix [a] into a rotation matrix [r] and a
93 // diagonal matrix [d] such that a = r * d * r^T.
94 void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t *a);
95 
96 /// @endcond
97 
98 #ifdef __cplusplus
99 }
100 #endif
101 
102 #endif