Fawkes API  Fawkes Development Version
angle.h
1 
2 /***************************************************************************
3  * angle.h - angle related math helper functions
4  *
5  * Created: Wed Jul 13 16:51:46 2005 (from FireVision)
6  * Copyright 2005-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_ANGLE_H_
25 #define __UTILS_MATH_ANGLE_H_
26 
27 #include <cmath>
28 
29 namespace fawkes {
30 
31 
32 /** Convert an angle given in degrees to radians.
33  * @param deg original value in degrees
34  * @return converted value in radians
35  */
36 inline float
37 deg2rad(float deg)
38 {
39  return (deg * M_PI / 180.f);
40 }
41 
42 
43 /** Convert an angle given in radians to degrees.
44  * @param rad original value in radians
45  * @return converted value in degrees
46  */
47 inline float
48 rad2deg(float rad)
49 {
50  return (rad * 180.f / M_PI);
51 }
52 
53 
54 /** Get distance between two 2D cartesian coordinates.
55  * @param x1 X coordinate of first point
56  * @param y1 Y coordinate of first point
57  * @param x2 X coordinate of second point
58  * @param y2 Y coordinate of second point
59  * @return distance between points
60  */
61 inline float
62 distance(float x1, float y1, float x2, float y2)
63 {
64  return sqrt( (x2-x1) * (x2-x1) + (y2-y1) * (y2-y1) );
65 }
66 
67 /** Normalize angle in radian between -PI (inclusive) and PI (exclusive).
68  * The given angle in radians is taken as an angle on the unit circle.
69  * It is then normalized into the range -PI and PI, such that it is the
70  * exact same angle on the unit circle but in the usual angle range.
71  * @param angle_rad original value
72  * @return normalized angle
73  */
74 inline float
75 normalize_mirror_rad(float angle_rad)
76 {
77  const float pi = static_cast<float>(M_PI);
78  if ( (angle_rad < -1.0f * pi) || (angle_rad >= pi) ) {
79  return ( angle_rad - 2.0f * pi * round(angle_rad / (2.0f * pi)) );
80  } else {
81  return angle_rad;
82  }
83 }
84 
85 /** Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
86  * The given angle in radians is taken as an angle on the unit circle.
87  * It is then normalized into the range 0 and 2*PI, such that it is the
88  * exact same angle on the unit circle but in the usual angle range.
89  * @param angle_rad original value
90  * @return normalized angle
91  */
92 inline float
93 normalize_rad(float angle_rad)
94 {
95  const float twopi = static_cast<float>(2 * M_PI);
96  if ( (angle_rad < 0) || (angle_rad >= twopi) ) {
97  return angle_rad - twopi * floor(angle_rad / twopi);
98  } else {
99  return angle_rad;
100  }
101 }
102 
103 
104 /** Normalizes angle in radian between -3*PI and 3*PI.
105  * If the angle is above 2*PI or below 2*PI the angle will be clipped.
106  * The largest full amount of (-)2*PI is subtracted, such that only the amount
107  * within the range [-2*PI, 2*PI] remains. Then (-)2*PI is added again.
108  * @param angle_rad original value
109  * @return normalized angle
110  */
111 inline float
112 normalize_bigmirror_rad(float angle_rad)
113 {
114  if ( (angle_rad < -2*M_PI) || (angle_rad > 2*M_PI) ) {
115  return (normalize_mirror_rad(angle_rad) + copysign(2*M_PI, angle_rad) );
116  } else {
117  return angle_rad;
118  }
119 }
120 
121 
122 /** Determines the distance between two angle provided as radians.
123  * @param angle_rad1 first angle in radian
124  * @param angle_rad2 second angle in radian
125  * @return distance between the two angles
126  */
127 inline float
128 angle_distance(float angle_rad1,
129  float angle_rad2)
130 {
131  return fabs( normalize_mirror_rad(angle_rad2 - angle_rad1) );
132 }
133 
134 /** Determines the signed distance between from "angle_from" to "angle_to" provided as radians.
135  * @param angle_to angle to which the signed value is calculated
136  * @param angle_from angle from which the signed value is calculated
137  * @return signed distance from angle "angle_from" to "angle_to"
138  */
139 inline float
140 angle_distance_signed(float angle_from, float angle_to)
141 {
142  return normalize_mirror_rad(angle_to - angle_from);
143 }
144 
145 
146 
147 } // end namespace fawkes
148 
149 #endif
float normalize_bigmirror_rad(float angle_rad)
Normalizes angle in radian between -3*PI and 3*PI.
Definition: angle.h:112
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:62
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:93
Fawkes library namespace.
float angle_distance_signed(float angle_from, float angle_to)
Determines the signed distance between from "angle_from" to "angle_to" provided as radians...
Definition: angle.h:140
float normalize_mirror_rad(float angle_rad)
Normalize angle in radian between -PI (inclusive) and PI (exclusive).
Definition: angle.h:75
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:48
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:37
float angle_distance(float angle_rad1, float angle_rad2)
Determines the distance between two angle provided as radians.
Definition: angle.h:128