angles.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
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     //check input conditions
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     //    printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
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)//from position is within the limits
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 //to position is outside the limits
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 // from position is outside the limits
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


angles
Author(s): John Hsu
autogenerated on Sun Apr 16 2017 02:17:29