YDLIDAR X2 SDK  V1.4.1
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 
35 #ifndef GEOMETRY_ANGLES_UTILS_H
36 #define GEOMETRY_ANGLES_UTILS_H
37 
38 #include <algorithm>
39 #include <cmath>
40 
41 namespace angles
42 {
43 
48  static inline double from_degrees(double degrees)
49  {
50  return degrees * M_PI / 180.0;
51  }
52 
56  static inline double to_degrees(double radians)
57  {
58  return radians * 180.0 / M_PI;
59  }
60 
61 
68  static inline double normalize_angle_positive(double angle)
69  {
70  return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
71  }
72 
73 
81  static inline double normalize_angle(double angle)
82  {
83  double a = normalize_angle_positive(angle);
84  if (a > M_PI)
85  a -= 2.0 *M_PI;
86  return a;
87  }
88 
89 
102  static inline double shortest_angular_distance(double from, double to)
103  {
104  return normalize_angle(to-from);
105  }
106 
116  static inline double two_pi_complement(double angle)
117  {
118  //check input conditions
119  if (angle > 2*M_PI || angle < -2.0*M_PI)
120  angle = fmod(angle, 2.0*M_PI);
121  if(angle < 0)
122  return (2*M_PI+angle);
123  else if (angle > 0)
124  return (-2*M_PI+angle);
125 
126  return(2*M_PI);
127  }
128 
140  static bool find_min_max_delta(double from, double left_limit, double right_limit, double &result_min_delta, double &result_max_delta)
141  {
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  {
152  result_min_delta = delta[0];
153  result_max_delta = std::max<double>(delta[1],delta[3]);
154  return true;
155  }
156 
157  if(fabs(delta[1]) < 1e-6)
158  {
159  result_max_delta = delta[1];
160  result_min_delta = std::min<double>(delta[0],delta[2]);
161  return true;
162  }
163 
164 
165  double delta_min = delta[0];
166  double delta_min_2pi = delta[2];
167  if(delta[2] < delta_min)
168  {
169  delta_min = delta[2];
170  delta_min_2pi = delta[0];
171  }
172 
173  double delta_max = delta[1];
174  double delta_max_2pi = delta[3];
175  if(delta[3] > delta_max)
176  {
177  delta_max = delta[3];
178  delta_max_2pi = delta[1];
179  }
180 
181 
182  // printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
183  if((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi))
184  {
185  result_min_delta = delta_max_2pi;
186  result_max_delta = delta_min_2pi;
187  if(left_limit == -M_PI && right_limit == M_PI)
188  return true;
189  else
190  return false;
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, double left_limit, double right_limit, double &shortest_angle)
216  {
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,max_delta);
223  double delta = shortest_angular_distance(from,to);
224  double delta_mod_2pi = two_pi_complement(delta);
225 
226 
227  if(flag)//from position is within the limits
228  {
229  if(delta >= min_delta && delta <= max_delta)
230  {
231  shortest_angle = delta;
232  return true;
233  }
234  else if(delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta)
235  {
236  shortest_angle = delta_mod_2pi;
237  return true;
238  }
239  else //to position is outside the limits
240  {
241  find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
242  if(fabs(min_delta_to) < fabs(max_delta_to))
243  shortest_angle = std::max<double>(delta,delta_mod_2pi);
244  else if(fabs(min_delta_to) > fabs(max_delta_to))
245  shortest_angle = std::min<double>(delta,delta_mod_2pi);
246  else
247  {
248  if (fabs(delta) < fabs(delta_mod_2pi))
249  shortest_angle = delta;
250  else
251  shortest_angle = delta_mod_2pi;
252  }
253  return false;
254  }
255  }
256  else // from position is outside the limits
257  {
258  find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
259 
260  if(fabs(min_delta) < fabs(max_delta))
261  shortest_angle = std::min<double>(delta,delta_mod_2pi);
262  else if (fabs(min_delta) > fabs(max_delta))
263  shortest_angle = std::max<double>(delta,delta_mod_2pi);
264  else
265  {
266  if (fabs(delta) < fabs(delta_mod_2pi))
267  shortest_angle = delta;
268  else
269  shortest_angle = delta_mod_2pi;
270  }
271  return false;
272  }
273 
274  shortest_angle = delta;
275  return false;
276  }
277 }
278 
279 #endif
Definition: angles.h:41