angles.h
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the Willow Garage nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef GEOMETRY_ANGLES_UTILS_H
#define GEOMETRY_ANGLES_UTILS_H
#include <algorithm>
#include <cmath>
namespace angles
{
static inline double from_degrees(double degrees)
{
return degrees * M_PI / 180.0;
}
static inline double to_degrees(double radians)
{
return radians * 180.0 / M_PI;
}
static inline double normalize_angle_positive(double angle)
{
return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
}
static inline double normalize_angle(double angle)
{
double a = normalize_angle_positive(angle);
if (a > M_PI)
a -= 2.0 *M_PI;
return a;
}
static inline double shortest_angular_distance(double from, double to)
{
return normalize_angle(to-from);
}
static inline double two_pi_complement(double angle)
{
//check input conditions
if (angle > 2*M_PI || angle < -2.0*M_PI)
angle = fmod(angle, 2.0*M_PI);
if(angle < 0)
return (2*M_PI+angle);
else if (angle > 0)
return (-2*M_PI+angle);
return(2*M_PI);
}
static bool find_min_max_delta(double from, double left_limit, double right_limit, double &result_min_delta, double &result_max_delta)
{
double delta[4];
delta[0] = shortest_angular_distance(from,left_limit);
delta[1] = shortest_angular_distance(from,right_limit);
delta[2] = two_pi_complement(delta[0]);
delta[3] = two_pi_complement(delta[1]);
if(fabs(delta[0]) < 1e-6)
{
result_min_delta = delta[0];
result_max_delta = std::max<double>(delta[1],delta[3]);
return true;
}
if(fabs(delta[1]) < 1e-6)
{
result_max_delta = delta[1];
result_min_delta = std::min<double>(delta[0],delta[2]);
return true;
}
double delta_min = delta[0];
double delta_min_2pi = delta[2];
if(delta[2] < delta_min)
{
delta_min = delta[2];
delta_min_2pi = delta[0];
}
double delta_max = delta[1];
double delta_max_2pi = delta[3];
if(delta[3] > delta_max)
{
delta_max = delta[3];
delta_max_2pi = delta[1];
}
// printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
if((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi))
{
result_min_delta = delta_max_2pi;
result_max_delta = delta_min_2pi;
if(left_limit == -M_PI && right_limit == M_PI)
return true;
else
return false;
}
result_min_delta = delta_min;
result_max_delta = delta_max;
return true;
}
static inline bool shortest_angular_distance_with_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
{
double min_delta = -2*M_PI;
double max_delta = 2*M_PI;
double min_delta_to = -2*M_PI;
double max_delta_to = 2*M_PI;
bool flag = find_min_max_delta(from,left_limit,right_limit,min_delta,max_delta);
double delta = shortest_angular_distance(from,to);
double delta_mod_2pi = two_pi_complement(delta);
if(flag)//from position is within the limits
{
if(delta >= min_delta && delta <= max_delta)
{
shortest_angle = delta;
return true;
}
else if(delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta)
{
shortest_angle = delta_mod_2pi;
return true;
}
else //to position is outside the limits
{
find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
if(fabs(min_delta_to) < fabs(max_delta_to))
shortest_angle = std::max<double>(delta,delta_mod_2pi);
else if(fabs(min_delta_to) > fabs(max_delta_to))
shortest_angle = std::min<double>(delta,delta_mod_2pi);
else
{
if (fabs(delta) < fabs(delta_mod_2pi))
shortest_angle = delta;
else
shortest_angle = delta_mod_2pi;
}
return false;
}
}
else // from position is outside the limits
{
find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
if(fabs(min_delta) < fabs(max_delta))
shortest_angle = std::min<double>(delta,delta_mod_2pi);
else if (fabs(min_delta) > fabs(max_delta))
shortest_angle = std::max<double>(delta,delta_mod_2pi);
else
{
if (fabs(delta) < fabs(delta_mod_2pi))
shortest_angle = delta;
else
shortest_angle = delta_mod_2pi;
}
return false;
}
shortest_angle = delta;
return false;
}
}
#endif
angles
Definition: angles.h:41