Fawkes API  Fawkes Development Version
coord.h
1 
2 /***************************************************************************
3  * coord.h - coordinate transformation and coord sys related functions
4  *
5  * Created: Wed Jul 16 19:07:37 2008 (RoboCup 2008, Suzhou)
6  * Copyright 2008 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version. A runtime exception applies to
14  * this software (see LICENSE.GPL_WRE file mentioned below for details).
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_WRE file in the doc directory.
22  */
23 
24 #ifndef __UTILS_MATH_COORD_H_
25 #define __UTILS_MATH_COORD_H_
26 
27 #include <cmath>
28 
29 namespace fawkes {
30 
31 
32 /** Convert a 2D cartesian coordinate to a 2D polar coordinate.
33  * @param polar_phi Phi of the polar coordinate
34  * @param polar_dist distnace of the polar coordinate
35  * @param cart_x upon return contains X of the cartesian coordinate
36  * @param cart_y upon return contains Y of the cartesian coordinate
37  */
38 inline void
39 cart2polar2d(float cart_x, float cart_y,
40  float *polar_phi, float *polar_dist)
41 {
42  *polar_phi = atan2f(cart_y, cart_x);
43  *polar_dist = sqrtf(cart_x * cart_x + cart_y * cart_y);
44 }
45 
46 /** Convert a 3D cartesian coordinate (x, y, z) to a 3D polar coordinate.
47  * @param cart_x in
48  * @param cart_y in
49  * @param cart_z in
50  * @param polar_phi out
51  * @param polar_theta out
52  * @param polar_r out
53  */
54 inline void
55 cart2polar3d( float cart_x, float cart_y, float cart_z,
56  float& polar_phi, float& polar_theta, float& polar_r)
57 {
58  polar_r = sqrtf( cart_x*cart_x + cart_y*cart_y + cart_z*cart_z );
59  polar_phi = atan2f( cart_y, cart_x );
60  polar_theta = -1.0 * atan2f( cart_z, sqrtf( cart_x*cart_x + cart_y*cart_y ) );
61 }
62 
63 /** Convert a 2D polar coordinate to a 2D cartesian coordinate.
64  * @param polar_phi Phi of the polar coordinate
65  * @param polar_dist distnace of the polar coordinate
66  * @param cart_x upon return contains X of the cartesian coordinate
67  * @param cart_y upon return contains Y of the cartesian coordinate
68  */
69 inline void
70 polar2cart2d(float polar_phi, float polar_dist,
71  float *cart_x, float *cart_y)
72 {
73  *cart_x = polar_dist * cosf(polar_phi);
74  *cart_y = polar_dist * sinf(polar_phi);
75 }
76 
77 
78 } // end namespace fawkes
79 
80 #endif
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
Definition: coord.h:70
Fawkes library namespace.
void cart2polar2d(float cart_x, float cart_y, float *polar_phi, float *polar_dist)
Convert a 2D cartesian coordinate to a 2D polar coordinate.
Definition: coord.h:39
void cart2polar3d(float cart_x, float cart_y, float cart_z, float &polar_phi, float &polar_theta, float &polar_r)
Convert a 3D cartesian coordinate (x, y, z) to a 3D polar coordinate.
Definition: coord.h:55