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