YDLIDAR SDK  V1.4.5
angles.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #pragma once
35 #include <algorithm>
36 #include <cmath>
37 
38 #ifndef M_PI
39 #define M_PI 3.1415926
40 #endif
41 
42 namespace angles {
43 
48 static inline double from_degrees(double degrees) {
49  return degrees * M_PI / 180.0;
50 }
51 
55 static inline double to_degrees(double radians) {
56  return radians * 180.0 / M_PI;
57 }
58 
59 
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);
68 }
69 
70 
78 static inline double normalize_angle(double angle) {
79  double a = normalize_angle_positive(angle);
80 
81  if (a > M_PI) {
82  a -= 2.0 * M_PI;
83  }
84 
85  return a;
86 }
87 
88 
101 static inline double shortest_angular_distance(double from, double to) {
102  return normalize_angle(to - from);
103 }
104 
114 static inline double two_pi_complement(double angle) {
115  //check input conditions
116  if (angle > 2 * M_PI || angle < -2.0 * M_PI) {
117  angle = fmod(angle, 2.0 * M_PI);
118  }
119 
120  if (angle < 0) {
121  return (2 * M_PI + angle);
122  } else if (angle > 0) {
123  return (-2 * M_PI + angle);
124  }
125 
126  return (2 * M_PI);
127 }
128 
140 static bool find_min_max_delta(double from, double left_limit,
141  double right_limit, double &result_min_delta, double &result_max_delta) {
142  double delta[4];
143 
144  delta[0] = shortest_angular_distance(from, left_limit);
145  delta[1] = shortest_angular_distance(from, right_limit);
146 
147  delta[2] = two_pi_complement(delta[0]);
148  delta[3] = two_pi_complement(delta[1]);
149 
150  if (fabs(delta[0]) < 1e-6) {
151  result_min_delta = delta[0];
152  result_max_delta = std::max<double>(delta[1], delta[3]);
153  return true;
154  }
155 
156  if (fabs(delta[1]) < 1e-6) {
157  result_max_delta = delta[1];
158  result_min_delta = std::min<double>(delta[0], delta[2]);
159  return true;
160  }
161 
162 
163  double delta_min = delta[0];
164  double delta_min_2pi = delta[2];
165 
166  if (delta[2] < delta_min) {
167  delta_min = delta[2];
168  delta_min_2pi = delta[0];
169  }
170 
171  double delta_max = delta[1];
172  double delta_max_2pi = delta[3];
173 
174  if (delta[3] > delta_max) {
175  delta_max = delta[3];
176  delta_max_2pi = delta[1];
177  }
178 
179 
180  // printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
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;
184 
185  if (left_limit == -M_PI && right_limit == M_PI) {
186  return true;
187  } else {
188  return false;
189  }
190  }
191 
192  result_min_delta = delta_min;
193  result_max_delta = delta_max;
194  return true;
195 }
196 
197 
215 static inline bool shortest_angular_distance_with_limits(double from, double to,
216  double left_limit, double right_limit, double &shortest_angle) {
217 
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,
223  max_delta);
224  double delta = shortest_angular_distance(from, to);
225  double delta_mod_2pi = two_pi_complement(delta);
226 
227 
228  if (flag) { //from position is within the limits
229  if (delta >= min_delta && delta <= max_delta) {
230  shortest_angle = delta;
231  return true;
232  } else if (delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta) {
233  shortest_angle = delta_mod_2pi;
234  return true;
235  } else { //to position is outside the limits
236  find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);
237 
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);
242  } else {
243  if (fabs(delta) < fabs(delta_mod_2pi)) {
244  shortest_angle = delta;
245  } else {
246  shortest_angle = delta_mod_2pi;
247  }
248  }
249 
250  return false;
251  }
252  } else { // from position is outside the limits
253  find_min_max_delta(to, left_limit, right_limit, min_delta_to, max_delta_to);
254 
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);
259  } else {
260  if (fabs(delta) < fabs(delta_mod_2pi)) {
261  shortest_angle = delta;
262  } else {
263  shortest_angle = delta_mod_2pi;
264  }
265  }
266 
267  return false;
268  }
269 
270  shortest_angle = delta;
271  return false;
272 }
273 }// namespace angles
Definition: angles.h:42