39 #define M_PI 3.1415926 48 static inline double from_degrees(
double degrees) {
49 return degrees * M_PI / 180.0;
55 static inline double to_degrees(
double radians) {
56 return radians * 180.0 / M_PI;
66 static inline double normalize_angle_positive(
double angle) {
67 return fmod(fmod(angle, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI);
78 static inline double normalize_angle(
double angle) {
79 double a = normalize_angle_positive(angle);
101 static inline double shortest_angular_distance(
double from,
double to) {
102 return normalize_angle(to - from);
114 static inline double two_pi_complement(
double angle) {
116 if (angle > 2 * M_PI || angle < -2.0 * M_PI) {
117 angle = fmod(angle, 2.0 * M_PI);
121 return (2 * M_PI + angle);
122 }
else if (angle > 0) {
123 return (-2 * M_PI + angle);
140 static bool find_min_max_delta(
double from,
double left_limit,
141 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) {
151 result_min_delta = delta[0];
152 result_max_delta = std::max<double>(delta[1], delta[3]);
156 if (fabs(delta[1]) < 1e-6) {
157 result_max_delta = delta[1];
158 result_min_delta = std::min<double>(delta[0], delta[2]);
163 double delta_min = delta[0];
164 double delta_min_2pi = delta[2];
166 if (delta[2] < delta_min) {
167 delta_min = delta[2];
168 delta_min_2pi = delta[0];
171 double delta_max = delta[1];
172 double delta_max_2pi = delta[3];
174 if (delta[3] > delta_max) {
175 delta_max = delta[3];
176 delta_max_2pi = delta[1];
181 if ((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi)) {
182 result_min_delta = delta_max_2pi;
183 result_max_delta = delta_min_2pi;
185 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,
216 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,
224 double delta = shortest_angular_distance(from, to);
225 double delta_mod_2pi = two_pi_complement(delta);
229 if (delta >= min_delta && delta <= max_delta) {
230 shortest_angle = delta;
232 }
else if (delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta) {
233 shortest_angle = delta_mod_2pi;
236 find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);
238 if (fabs(min_delta_to) < fabs(max_delta_to)) {
239 shortest_angle = std::max<double>(delta, delta_mod_2pi);
240 }
else if (fabs(min_delta_to) > fabs(max_delta_to)) {
241 shortest_angle = std::min<double>(delta, delta_mod_2pi);
243 if (fabs(delta) < fabs(delta_mod_2pi)) {
244 shortest_angle = delta;
246 shortest_angle = delta_mod_2pi;
253 find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);
255 if (fabs(min_delta) < fabs(max_delta)) {
256 shortest_angle = std::min<double>(delta, delta_mod_2pi);
257 }
else if (fabs(min_delta) > fabs(max_delta)) {
258 shortest_angle = std::max<double>(delta, delta_mod_2pi);
260 if (fabs(delta) < fabs(delta_mod_2pi)) {
261 shortest_angle = delta;
263 shortest_angle = delta_mod_2pi;
270 shortest_angle = delta;