angles: angles: angles.h Source 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 ANGLES_H
00036 #define ANGLES_H
00037 
00038 #include <algorithm>
00039 #include <cmath>
00040 #include <stdio.h>
00041 
00042 namespace angles
00043 {
00044     
00049   static inline double from_degrees(double degrees)
00050   {
00051     return degrees * M_PI / 180.0;   
00052   }
00053     
00057   static inline double to_degrees(double radians)
00058   {
00059     return radians * 180.0 / M_PI;
00060   }
00061     
00062 
00069   static inline double normalize_angle_positive(double angle)
00070   {
00071     return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
00072   }
00073 
00074 
00082   static inline double normalize_angle(double angle)
00083   {
00084     double a = normalize_angle_positive(angle);
00085     if (a > M_PI)
00086       a -= 2.0 *M_PI;
00087     return a;
00088   }
00089 
00090     
00103   static inline double shortest_angular_distance(double from, double to)
00104   {
00105     double result = normalize_angle_positive(normalize_angle_positive(to) - normalize_angle_positive(from));
00106         
00107     if (result > M_PI)
00108       // If the result > 180,
00109       // It's shorter the other way.
00110       result = -(2.0*M_PI - result);
00111         
00112     return normalize_angle(result);
00113   }
00114 
00124   static inline double two_pi_complement(double angle)
00125   {
00126     //check input conditions
00127     if (angle > 2*M_PI || angle < -2.0*M_PI)
00128       angle = fmod(angle, 2.0*M_PI);    
00129     if(angle < 0)
00130       return (2*M_PI+angle);
00131     else if (angle > 0)
00132       return (-2*M_PI+angle);
00133 
00134     return(2*M_PI);
00135   }
00136 
00148   static bool find_min_max_delta(double from, double left_limit, double right_limit, double &result_min_delta, double &result_max_delta)
00149   {
00150     double delta[4];
00151 
00152     delta[0] = shortest_angular_distance(from,left_limit);
00153     delta[1] = shortest_angular_distance(from,right_limit);
00154 
00155     delta[2] = two_pi_complement(delta[0]);
00156     delta[3] = two_pi_complement(delta[1]);
00157 
00158     if(delta[0] == 0)
00159     {
00160       result_min_delta = delta[0];
00161       result_max_delta = std::max<double>(delta[1],delta[3]);
00162       return true;
00163     }
00164 
00165     if(delta[1] == 0)
00166     {
00167         result_max_delta = delta[1];
00168         result_min_delta = std::min<double>(delta[0],delta[2]);
00169         return true;
00170     }
00171 
00172 
00173     double delta_min = delta[0];
00174     double delta_min_2pi = delta[2];
00175     if(delta[2] < delta_min)
00176     {
00177       delta_min = delta[2];
00178       delta_min_2pi = delta[0];
00179     }
00180 
00181     double delta_max = delta[1];
00182     double delta_max_2pi = delta[3];
00183     if(delta[3] > delta_max)
00184     {
00185       delta_max = delta[3];
00186       delta_max_2pi = delta[1];
00187     }
00188 
00189 
00190     //    printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
00191     if((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi))
00192     {
00193       result_min_delta = delta_max_2pi;
00194       result_max_delta = delta_min_2pi;
00195       if(left_limit == -M_PI && right_limit == M_PI)
00196         return true;
00197       else
00198         return false;
00199     }
00200     result_min_delta = delta_min;
00201     result_max_delta = delta_max;
00202     return true;
00203   }
00204 
00205 
00223   static inline bool shortest_angular_distance_with_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
00224   {
00225 
00226     double min_delta = -2*M_PI;
00227     double max_delta = 2*M_PI;
00228     double min_delta_to = -2*M_PI;
00229     double max_delta_to = 2*M_PI;
00230     bool flag    = find_min_max_delta(from,left_limit,right_limit,min_delta,max_delta);
00231     double delta = shortest_angular_distance(from,to);
00232     double delta_mod_2pi  = two_pi_complement(delta);
00233 
00234 
00235     if(flag)//from position is within the limits
00236     {
00237       if(delta >= min_delta && delta <= max_delta)
00238       {
00239         shortest_angle = delta;
00240         return true;
00241       }
00242       else if(delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta)
00243       {
00244         shortest_angle = delta_mod_2pi;
00245         return true;
00246       }
00247       else //to position is outside the limits
00248       {
00249         find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
00250           if(fabs(min_delta_to) < fabs(max_delta_to))
00251             shortest_angle = std::max<double>(delta,delta_mod_2pi);
00252           else if(fabs(min_delta_to) > fabs(max_delta_to))
00253             shortest_angle =  std::min<double>(delta,delta_mod_2pi);
00254           else
00255           {
00256             if (fabs(delta) < fabs(delta_mod_2pi))
00257               shortest_angle = delta;
00258             else
00259               shortest_angle = delta_mod_2pi;
00260           }
00261           return false;
00262       }
00263     }
00264     else // from position is outside the limits
00265     {
00266         find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
00267 
00268           if(fabs(min_delta) < fabs(max_delta))
00269             shortest_angle = std::min<double>(delta,delta_mod_2pi);
00270           else if (fabs(min_delta) > fabs(max_delta))
00271             shortest_angle =  std::max<double>(delta,delta_mod_2pi);
00272           else
00273           {
00274             if (fabs(delta) < fabs(delta_mod_2pi))
00275               shortest_angle = delta;
00276             else
00277               shortest_angle = delta_mod_2pi;
00278           }
00279       return false;
00280     }
00281 
00282     shortest_angle = delta;
00283     return false;
00284   }
00285 
00286 
00287     
00288 
00289 
00290 }
00291 
00292 #endif