35 #ifndef GEOMETRY_ANGLES_UTILS_H 36 #define GEOMETRY_ANGLES_UTILS_H 48 static inline double from_degrees(
double degrees)
50 return degrees * M_PI / 180.0;
56 static inline double to_degrees(
double radians)
58 return radians * 180.0 / M_PI;
68 static inline double normalize_angle_positive(
double angle)
70 return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
81 static inline double normalize_angle(
double angle)
83 double a = normalize_angle_positive(angle);
102 static inline double shortest_angular_distance(
double from,
double to)
104 return normalize_angle(to-from);
116 static inline double two_pi_complement(
double angle)
119 if (angle > 2*M_PI || angle < -2.0*M_PI)
120 angle = fmod(angle, 2.0*M_PI);
122 return (2*M_PI+angle);
124 return (-2*M_PI+angle);
140 static bool find_min_max_delta(
double from,
double left_limit,
double right_limit,
double &result_min_delta,
double &result_max_delta)
144 delta[0] = shortest_angular_distance(from,left_limit);
145 delta[1] = shortest_angular_distance(from,right_limit);
147 delta[2] = two_pi_complement(delta[0]);
148 delta[3] = two_pi_complement(delta[1]);
150 if(fabs(delta[0]) < 1e-6)
152 result_min_delta = delta[0];
153 result_max_delta = std::max<double>(delta[1],delta[3]);
157 if(fabs(delta[1]) < 1e-6)
159 result_max_delta = delta[1];
160 result_min_delta = std::min<double>(delta[0],delta[2]);
165 double delta_min = delta[0];
166 double delta_min_2pi = delta[2];
167 if(delta[2] < delta_min)
169 delta_min = delta[2];
170 delta_min_2pi = delta[0];
173 double delta_max = delta[1];
174 double delta_max_2pi = delta[3];
175 if(delta[3] > delta_max)
177 delta_max = delta[3];
178 delta_max_2pi = delta[1];
183 if((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi))
185 result_min_delta = delta_max_2pi;
186 result_max_delta = delta_min_2pi;
187 if(left_limit == -M_PI && right_limit == M_PI)
192 result_min_delta = delta_min;
193 result_max_delta = delta_max;
215 static inline bool shortest_angular_distance_with_limits(
double from,
double to,
double left_limit,
double right_limit,
double &shortest_angle)
218 double min_delta = -2*M_PI;
219 double max_delta = 2*M_PI;
220 double min_delta_to = -2*M_PI;
221 double max_delta_to = 2*M_PI;
222 bool flag = find_min_max_delta(from,left_limit,right_limit,min_delta,max_delta);
223 double delta = shortest_angular_distance(from,to);
224 double delta_mod_2pi = two_pi_complement(delta);
229 if(delta >= min_delta && delta <= max_delta)
231 shortest_angle = delta;
234 else if(delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta)
236 shortest_angle = delta_mod_2pi;
241 find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
242 if(fabs(min_delta_to) < fabs(max_delta_to))
243 shortest_angle = std::max<double>(delta,delta_mod_2pi);
244 else if(fabs(min_delta_to) > fabs(max_delta_to))
245 shortest_angle = std::min<double>(delta,delta_mod_2pi);
248 if (fabs(delta) < fabs(delta_mod_2pi))
249 shortest_angle = delta;
251 shortest_angle = delta_mod_2pi;
258 find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
260 if(fabs(min_delta) < fabs(max_delta))
261 shortest_angle = std::min<double>(delta,delta_mod_2pi);
262 else if (fabs(min_delta) > fabs(max_delta))
263 shortest_angle = std::max<double>(delta,delta_mod_2pi);
266 if (fabs(delta) < fabs(delta_mod_2pi))
267 shortest_angle = delta;
269 shortest_angle = delta_mod_2pi;
274 shortest_angle = delta;