35 #ifndef GEOMETRY_ANGLES_UTILS_H 36 #define GEOMETRY_ANGLES_UTILS_H 50 return degrees * M_PI / 180.0;
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;
static bool shortest_angular_distance_with_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
Returns the delta from "from_angle" to "to_angle" making sure it does not violate limits specified by...
static double shortest_angular_distance(double from, double to)
shortest_angular_distance
static double normalize_angle(double angle)
normalize
static bool shortest_angular_distance_with_large_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
Returns the delta from from_angle to to_angle, making sure it does not violate limits specified by le...
static double normalize_angle_positive(double angle)
normalize_angle_positive
static double from_degrees(double degrees)
Convert degrees to radians.
static double two_pi_complement(double angle)
returns the angle in [-2*M_PI, 2*M_PI] going the other way along the unit circle. ...
static double to_degrees(double radians)
Convert radians to degrees.
static bool find_min_max_delta(double from, double left_limit, double right_limit, double &result_min_delta, double &result_max_delta)
This function is only intended for internal use and not intended for external use. If you do use it, read the documentation very carefully. Returns the min and max amount (in radians) that can be moved from "from" angle to "left_limit" and "right_limit".