Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef GEOMETRY_ANGLES_UTILS_H
00036 #define GEOMETRY_ANGLES_UTILS_H
00037
00038 #include <algorithm>
00039 #include <cmath>
00040
00041 namespace angles
00042 {
00043
00048 static inline double from_degrees(double degrees)
00049 {
00050 return degrees * M_PI / 180.0;
00051 }
00052
00056 static inline double to_degrees(double radians)
00057 {
00058 return radians * 180.0 / M_PI;
00059 }
00060
00061
00068 static inline double normalize_angle_positive(double angle)
00069 {
00070 return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
00071 }
00072
00073
00081 static inline double normalize_angle(double angle)
00082 {
00083 double a = normalize_angle_positive(angle);
00084 if (a > M_PI)
00085 a -= 2.0 *M_PI;
00086 return a;
00087 }
00088
00089
00102 static inline double shortest_angular_distance(double from, double to)
00103 {
00104 return normalize_angle(to-from);
00105 }
00106
00116 static inline double two_pi_complement(double angle)
00117 {
00118
00119 if (angle > 2*M_PI || angle < -2.0*M_PI)
00120 angle = fmod(angle, 2.0*M_PI);
00121 if(angle < 0)
00122 return (2*M_PI+angle);
00123 else if (angle > 0)
00124 return (-2*M_PI+angle);
00125
00126 return(2*M_PI);
00127 }
00128
00140 static bool find_min_max_delta(double from, double left_limit, double right_limit, double &result_min_delta, double &result_max_delta)
00141 {
00142 double delta[4];
00143
00144 delta[0] = shortest_angular_distance(from,left_limit);
00145 delta[1] = shortest_angular_distance(from,right_limit);
00146
00147 delta[2] = two_pi_complement(delta[0]);
00148 delta[3] = two_pi_complement(delta[1]);
00149
00150 if(delta[0] == 0)
00151 {
00152 result_min_delta = delta[0];
00153 result_max_delta = std::max<double>(delta[1],delta[3]);
00154 return true;
00155 }
00156
00157 if(delta[1] == 0)
00158 {
00159 result_max_delta = delta[1];
00160 result_min_delta = std::min<double>(delta[0],delta[2]);
00161 return true;
00162 }
00163
00164
00165 double delta_min = delta[0];
00166 double delta_min_2pi = delta[2];
00167 if(delta[2] < delta_min)
00168 {
00169 delta_min = delta[2];
00170 delta_min_2pi = delta[0];
00171 }
00172
00173 double delta_max = delta[1];
00174 double delta_max_2pi = delta[3];
00175 if(delta[3] > delta_max)
00176 {
00177 delta_max = delta[3];
00178 delta_max_2pi = delta[1];
00179 }
00180
00181
00182
00183 if((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi))
00184 {
00185 result_min_delta = delta_max_2pi;
00186 result_max_delta = delta_min_2pi;
00187 if(left_limit == -M_PI && right_limit == M_PI)
00188 return true;
00189 else
00190 return false;
00191 }
00192 result_min_delta = delta_min;
00193 result_max_delta = delta_max;
00194 return true;
00195 }
00196
00197
00215 static inline bool shortest_angular_distance_with_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
00216 {
00217
00218 double min_delta = -2*M_PI;
00219 double max_delta = 2*M_PI;
00220 double min_delta_to = -2*M_PI;
00221 double max_delta_to = 2*M_PI;
00222 bool flag = find_min_max_delta(from,left_limit,right_limit,min_delta,max_delta);
00223 double delta = shortest_angular_distance(from,to);
00224 double delta_mod_2pi = two_pi_complement(delta);
00225
00226
00227 if(flag)
00228 {
00229 if(delta >= min_delta && delta <= max_delta)
00230 {
00231 shortest_angle = delta;
00232 return true;
00233 }
00234 else if(delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta)
00235 {
00236 shortest_angle = delta_mod_2pi;
00237 return true;
00238 }
00239 else
00240 {
00241 find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
00242 if(fabs(min_delta_to) < fabs(max_delta_to))
00243 shortest_angle = std::max<double>(delta,delta_mod_2pi);
00244 else if(fabs(min_delta_to) > fabs(max_delta_to))
00245 shortest_angle = std::min<double>(delta,delta_mod_2pi);
00246 else
00247 {
00248 if (fabs(delta) < fabs(delta_mod_2pi))
00249 shortest_angle = delta;
00250 else
00251 shortest_angle = delta_mod_2pi;
00252 }
00253 return false;
00254 }
00255 }
00256 else
00257 {
00258 find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
00259
00260 if(fabs(min_delta) < fabs(max_delta))
00261 shortest_angle = std::min<double>(delta,delta_mod_2pi);
00262 else if (fabs(min_delta) > fabs(max_delta))
00263 shortest_angle = std::max<double>(delta,delta_mod_2pi);
00264 else
00265 {
00266 if (fabs(delta) < fabs(delta_mod_2pi))
00267 shortest_angle = delta;
00268 else
00269 shortest_angle = delta_mod_2pi;
00270 }
00271 return false;
00272 }
00273
00274 shortest_angle = delta;
00275 return false;
00276 }
00277 }
00278
00279 #endif