35 #ifndef GEOMETRY_ANGLES_UTILS_H
36 #define GEOMETRY_ANGLES_UTILS_H
50 return degrees * M_PI / 180.0;
56 static inline double to_degrees(
double radians)
58 return radians * 180.0 / M_PI;
70 const double result = fmod(angle, 2.0*M_PI);
71 if(result < 0)
return result + 2.0*M_PI;
85 const double result = fmod(angle + M_PI, 2.0*M_PI);
86 if(result <= 0.0)
return result + M_PI;
120 if (angle > 2*M_PI || angle < -2.0*M_PI)
121 angle = fmod(angle, 2.0*M_PI);
123 return (2*M_PI+angle);
125 return (-2*M_PI+angle);
141 static bool find_min_max_delta(
double from,
double left_limit,
double right_limit,
double &result_min_delta,
double &result_max_delta)
153 result_min_delta = delta[0];
154 result_max_delta = std::max<double>(delta[1],delta[3]);
160 result_max_delta = delta[1];
161 result_min_delta = std::min<double>(delta[0],delta[2]);
166 double delta_min = delta[0];
167 double delta_min_2pi = delta[2];
168 if(delta[2] < delta_min)
170 delta_min = delta[2];
171 delta_min_2pi = delta[0];
174 double delta_max = delta[1];
175 double delta_max_2pi = delta[3];
176 if(delta[3] > delta_max)
178 delta_max = delta[3];
179 delta_max_2pi = delta[1];
184 if((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi))
186 result_min_delta = delta_max_2pi;
187 result_max_delta = delta_min_2pi;
188 if(left_limit == -M_PI && right_limit == M_PI)
193 result_min_delta = delta_min;
194 result_max_delta = delta_max;
229 if(std::fabs(delta) > std::fabs(delta_2pi))
230 std::swap(delta, delta_2pi);
232 if(left_limit > right_limit) {
244 shortest_angle = delta;
251 double to2 = from + delta;
252 if(left_limit <= to2 && to2 <= right_limit) {
254 shortest_angle = delta;
255 return left_limit <= from && from <= right_limit;
259 to2 = from + delta_2pi;
260 if(left_limit <= to2 && to2 <= right_limit) {
262 shortest_angle = delta_2pi;
263 return left_limit <= from && from <= right_limit;
267 shortest_angle = delta;
292 double min_delta = -2*M_PI;
293 double max_delta = 2*M_PI;
294 double min_delta_to = -2*M_PI;
295 double max_delta_to = 2*M_PI;
303 if(delta >= min_delta && delta <= max_delta)
305 shortest_angle = delta;
308 else if(delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta)
310 shortest_angle = delta_mod_2pi;
316 if(fabs(min_delta_to) < fabs(max_delta_to))
317 shortest_angle = std::max<double>(delta,delta_mod_2pi);
318 else if(fabs(min_delta_to) > fabs(max_delta_to))
319 shortest_angle = std::min<double>(delta,delta_mod_2pi);
322 if (fabs(delta) < fabs(delta_mod_2pi))
323 shortest_angle = delta;
325 shortest_angle = delta_mod_2pi;
334 if(fabs(min_delta) < fabs(max_delta))
335 shortest_angle = std::min<double>(delta,delta_mod_2pi);
336 else if (fabs(min_delta) > fabs(max_delta))
337 shortest_angle = std::max<double>(delta,delta_mod_2pi);
340 if (fabs(delta) < fabs(delta_mod_2pi))
341 shortest_angle = delta;
343 shortest_angle = delta_mod_2pi;
348 shortest_angle = delta;