angles.h
Go to the documentation of this file.
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  const double result = fmod(angle, 2.0*M_PI);
71  if(result < 0) return result + 2.0*M_PI;
72  return result;
73  }
74 
75 
83  static inline double normalize_angle(double angle)
84  {
85  const double result = fmod(angle + M_PI, 2.0*M_PI);
86  if(result <= 0.0) return result + M_PI;
87  return result - M_PI;
88  }
89 
90 
103  static inline double shortest_angular_distance(double from, double to)
104  {
105  return normalize_angle(to-from);
106  }
107 
117  static inline double two_pi_complement(double angle)
118  {
119  //check input conditions
120  if (angle > 2*M_PI || angle < -2.0*M_PI)
121  angle = fmod(angle, 2.0*M_PI);
122  if(angle < 0)
123  return (2*M_PI+angle);
124  else if (angle > 0)
125  return (-2*M_PI+angle);
126 
127  return(2*M_PI);
128  }
129 
141  static bool find_min_max_delta(double from, double left_limit, double right_limit, double &result_min_delta, double &result_max_delta)
142  {
143  double delta[4];
144 
145  delta[0] = shortest_angular_distance(from,left_limit);
146  delta[1] = shortest_angular_distance(from,right_limit);
147 
148  delta[2] = two_pi_complement(delta[0]);
149  delta[3] = two_pi_complement(delta[1]);
150 
151  if(delta[0] == 0)
152  {
153  result_min_delta = delta[0];
154  result_max_delta = std::max<double>(delta[1],delta[3]);
155  return true;
156  }
157 
158  if(delta[1] == 0)
159  {
160  result_max_delta = delta[1];
161  result_min_delta = std::min<double>(delta[0],delta[2]);
162  return true;
163  }
164 
165 
166  double delta_min = delta[0];
167  double delta_min_2pi = delta[2];
168  if(delta[2] < delta_min)
169  {
170  delta_min = delta[2];
171  delta_min_2pi = delta[0];
172  }
173 
174  double delta_max = delta[1];
175  double delta_max_2pi = delta[3];
176  if(delta[3] > delta_max)
177  {
178  delta_max = delta[3];
179  delta_max_2pi = delta[1];
180  }
181 
182 
183  // printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
184  if((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi))
185  {
186  result_min_delta = delta_max_2pi;
187  result_max_delta = delta_min_2pi;
188  if(left_limit == -M_PI && right_limit == M_PI)
189  return true;
190  else
191  return false;
192  }
193  result_min_delta = delta_min;
194  result_max_delta = delta_max;
195  return true;
196  }
197 
198 
222  static inline bool shortest_angular_distance_with_large_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
223  {
224  // Shortest steps in the two directions
225  double delta = shortest_angular_distance(from, to);
226  double delta_2pi = two_pi_complement(delta);
227 
228  // "sort" distances so that delta is shorter than delta_2pi
229  if(std::fabs(delta) > std::fabs(delta_2pi))
230  std::swap(delta, delta_2pi);
231 
232  if(left_limit > right_limit) {
233  // If limits are something like [PI/2 , -PI/2] it actually means that we
234  // want rotations to be in the interval [-PI,PI/2] U [PI/2,PI], ie, the
235  // half unit circle not containing the 0. This is already gracefully
236  // handled by shortest_angular_distance_with_limits, and therefore this
237  // function should not be called at all. However, if one has limits that
238  // are larger than PI, the same rationale behind shortest_angular_distance_with_limits
239  // does not hold, ie, M_PI+x should not be directly equal to -M_PI+x.
240  // In this case, the correct way of getting the shortest solution is to
241  // properly set the limits, eg, by saying that the interval is either
242  // [PI/2, 3*PI/2] or [-3*M_PI/2, -M_PI/2]. For this reason, here we
243  // return false by default.
244  shortest_angle = delta;
245  return false;
246  }
247 
248  // Check in which direction we should turn (clockwise or counter-clockwise).
249 
250  // start by trying with the shortest angle (delta).
251  double to2 = from + delta;
252  if(left_limit <= to2 && to2 <= right_limit) {
253  // we can move in this direction: return success if the "from" angle is inside limits
254  shortest_angle = delta;
255  return left_limit <= from && from <= right_limit;
256  }
257 
258  // delta is not ok, try to move in the other direction (using its complement)
259  to2 = from + delta_2pi;
260  if(left_limit <= to2 && to2 <= right_limit) {
261  // we can move in this direction: return success if the "from" angle is inside limits
262  shortest_angle = delta_2pi;
263  return left_limit <= from && from <= right_limit;
264  }
265 
266  // nothing works: we always go outside limits
267  shortest_angle = delta; // at least give some "coherent" result
268  return false;
269  }
270 
271 
289  static inline bool shortest_angular_distance_with_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
290  {
291 
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;
296  bool flag = find_min_max_delta(from,left_limit,right_limit,min_delta,max_delta);
297  double delta = shortest_angular_distance(from,to);
298  double delta_mod_2pi = two_pi_complement(delta);
299 
300 
301  if(flag)//from position is within the limits
302  {
303  if(delta >= min_delta && delta <= max_delta)
304  {
305  shortest_angle = delta;
306  return true;
307  }
308  else if(delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta)
309  {
310  shortest_angle = delta_mod_2pi;
311  return true;
312  }
313  else //to position is outside the limits
314  {
315  find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
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);
320  else
321  {
322  if (fabs(delta) < fabs(delta_mod_2pi))
323  shortest_angle = delta;
324  else
325  shortest_angle = delta_mod_2pi;
326  }
327  return false;
328  }
329  }
330  else // from position is outside the limits
331  {
332  find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
333 
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);
338  else
339  {
340  if (fabs(delta) < fabs(delta_mod_2pi))
341  shortest_angle = delta;
342  else
343  shortest_angle = delta_mod_2pi;
344  }
345  return false;
346  }
347 
348  shortest_angle = delta;
349  return false;
350  }
351 }
352 
353 #endif
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...
Definition: angles.h:289
static double shortest_angular_distance(double from, double to)
shortest_angular_distance
Definition: angles.h:103
static double normalize_angle(double angle)
normalize
Definition: angles.h:83
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...
Definition: angles.h:222
Definition: angles.h:41
static double normalize_angle_positive(double angle)
normalize_angle_positive
Definition: angles.h:68
static double from_degrees(double degrees)
Convert degrees to radians.
Definition: angles.h:48
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. ...
Definition: angles.h:117
static double to_degrees(double radians)
Convert radians to degrees.
Definition: angles.h:56
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".
Definition: angles.h:141


angles
Author(s): John Hsu , Tully Foote
autogenerated on Mon Feb 28 2022 21:35:12