geo.hpp

Go to the documentation of this file.

/** \file geo.hpp
 * \author Jared R. Males
 * \brief Utilities for working with angles
 * \ingroup gen_math_files
 *
 */
 
//***********************************************************************//
// Copyright 2015, 2016, 2017, 2018 Jared R. Males (jaredmales@gmail.com)
//
// This file is part of mxlib.
//
// mxlib is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// mxlib is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with mxlib.  If not, see <http://www.gnu.org/licenses/>.
//***********************************************************************//
 
#ifndef math_geo_hpp
#define math_geo_hpp
 
#include <vector>
 
#include <cmath>
 
#include "constants.hpp"
 
namespace mx
{
namespace math
{
 
/// Calculate the semi-latus rectum of a <a href="http://en.wikipedia.org/wiki/Conic_section">conic section</a>
/**
 * \ingroup geo
 */
#define semilatrect( a, e )                                                                                            \
    ( e == 0.0 ? a : ( e == 1.0 ? 2. * a : ( e < 1. ? a * ( 1 - e * e ) : a * ( e * e - 1 ) ) ) )
 
/// Calculate the focal parameter of a <a href="http://en.wikipedia.org/wiki/Conic_section">conic section</a>
/**
 * \ingroup geo
 */
#define focus( a, e )                                                                                                  \
    ( e == 0.0 ? 1e34 : ( e == 1.0 ? 2. * a : ( e < 1. ? a * ( 1 - e * e ) / e : a * ( e * e - 1 ) / e ) ) )
 
/// Calculate the semi-major axis of a <a href="http://en.wikipedia.org/wiki/Conic_section">conic section</a>, given the
/// focal parameter and the eccentricity
/**
 * \ingroup geo
 */
#define semimaj( p, e ) ( e == 1.0 ? 1e34 : ( e < 1 ? p * e / ( 1 - e * e ) : p * e / ( e * e - 1 ) ) )
 
/// Calculate the eccentricity of a <a href="http://en.wikipedia.org/wiki/Conic_section">conic section</a> given the
/// semi-major axis and the focal parameter
/**
 * \ingroup geo
 */
#define eccent( a, p )                                                                                                 \
    ( a == 0.0 ? 1e34                                                                                                  \
               : ( p >= 1e9 ? 0.0                                                                                      \
                            : ( p > 0 ? ( -p / ( 2 * a ) + 0.5 * std::sqrt( p * p / ( a * a ) + 4 ) )                  \
                                      : ( p / ( 2 * a ) + 0.5 * std::sqrt( p * p / ( a * a ) + 4 ) ) ) ) )
 
/// Type specifying angles in radians
/**
 * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
 */
struct radians;
 
/// Type specifying angles in degrees
/**
 * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
 */
struct degrees;
 
/// Type holding constants related to angle calculations in degrees
template <typename degrad, typename realT>
struct degradT;
 
/// Type holding constants related to angle calculations in degrees
/**
 * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
 */
template <typename _realT>
struct degradT<degrees, _realT>
{
    typedef _realT realT;
    static constexpr realT scale =
        static_cast<realT>( 180 ) / pi<realT>(); // Scale factor to convert from radians to this unit.
    static constexpr realT degrees = 1;
    static constexpr realT radians = pi<realT>() / static_cast<realT>( 180 );
    static constexpr realT full = static_cast<realT>( 360.0 );
    static constexpr realT half = static_cast<realT>( 180.0 );
};
 
template <typename realT>
using degreesT = degradT<degrees, realT>;
 
/// Type holding constants related to angle calculations in radians
/**
 * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
 */
template <typename _realT>
struct degradT<radians, _realT>
{
    typedef _realT realT;
    static constexpr realT scale = 1; // Scale factor to convert from radians to this unit.
    static constexpr realT degrees = static_cast<realT>( 180 ) / pi<realT>();
    static constexpr realT radians = 1;
    static constexpr realT full = two_pi<realT>();
    static constexpr realT half = pi<realT>();
};
 
template <typename realT>
using radiansT = degradT<radians, realT>;
 
/// Convert from degrees to radians
/**
 *
 * \param q is the angle to convert
 *
 * \return the angle q converted to radians
 *
 * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
 *
 * \ingroup geo
 */
template <typename realT>
realT dtor( realT q )
{
    return q * degradT<degrees, realT>::radians;
}
 
/// Convert from radians to degrees
/**
 *
 * \param q is the angle to convert
 *
 * \return the angle q converted to degrees
 *
 * \ingroup geo
 */
template <typename realT>
realT rtod( realT q )
{
    return q * degradT<radians, realT>::degrees;
}
 
/// Calculate the angle modulo full-circle, normalizing to a positive value.
/** The output will be betweeen 0 and 360 (or 0 and 2pi).
 *
 * \returns the value of q normalized to 0 <= q < 360[2pi]
 *
 * \tparam degrad controls whether this is in degrees (0, default) or radians (1)
 * \tparam realT is the type in which to do arithmetic
 *
 * \ingroup geo
 */
template <class angleT>
typename angleT::realT angleMod( typename angleT::realT q /**< [in] the angle */ )
{
    static_assert( std::is_floating_point<typename angleT::realT>::value,
                   "angleMod: angleT::realT must be floating point" );
 
    q = fmod( q, angleT::full );
 
    if( q < 0 )
        q += angleT::full;
 
    return q;
}
 
/// Calculate the difference between two angles, correctly across 0/360.
/** Calculates \f$ dq = q2- q1 \f$, but accounts for crossing 0/360.  This implies
 * that \f$ dq \le 180 \f$.
 *
 *
 * \returns the difference of q2 and q1
 *
 * \tparam angleT controls whether this is in degrees (0, default) or radians (1)
 * \tparam realT is the type in which to do arithmetic
 *
 * \test Verify compilation and calculations of math::angleDiff \ref tests_math_geo_angleDiff "[test doc]"
 *
 * \ingroup geo
 */
template <class angleT>
typename angleT::realT angleDiff( typename angleT::realT q1, ///< [in] angle to subtract from q2.
                                  typename angleT::realT q2  ///< [in] angle to subtract q1 from.
)
{
    static_assert( std::is_floating_point<typename angleT::realT>::value, "angleDiff: realT must be floating point" );
 
    typename angleT::realT dq = q2 - q1;
 
    if( std::abs( dq ) > angleT::half )
    {
        if( dq < 0 )
        {
            dq = dq + static_cast<typename angleT::realT>( 2 ) * angleT::half;
        }
        else
        {
            dq = dq - static_cast<typename angleT::realT>( 2 ) * angleT::half;
        }
    }
 
    return dq;
}
 
/// Calculate the mean of a set of angles, correctly across 0/360.
/** Calculates the mean by decomposing into vectors and averaging the components. This accounts for crossing 0/360.
 *
 * \returns the mean angle
 *
 * \tparam angleT is the angle type, either radians<realT> or degrees<realT>.  angleT::realT is the type in which to do
 * arithmetic.
 *
 * \ingroup geo
 */
template <class angleT>
typename angleT::realT
angleMean( const std::vector<typename angleT::realT> &q /**< [in] vector of angles to average.*/ )
{
    static_assert( std::is_floating_point<typename angleT::realT>::value, "angleMean: realT must be floating point" );
 
    typename angleT::realT s = 0;
    typename angleT::realT c = 0;
 
    for( int i = 0; i < q.size(); ++i )
    {
        s += sin( q[i] / angleT::scale );
        c += cos( q[i] / angleT::scale );
    }
 
    s /= q.size();
    c /= q.size();
 
    return atan2( s, c ) * angleT::scale;
}
 
/// Make a vector of angles continuous, fixing the 0/360 crossing.
/** The vector is modified so it is continuous.
 *
 *
 * \tparam degrad controls whether angles are degrees (false) or radians (true)
 * \tparam realT is the type in which to do arithmetic
 *
 *  \ingroup geo
 */
template <int degrad = 0, typename realT>
int continueAngles( std::vector<realT> &angles, ///< [in] the vector of angles
                    realT threshold = 0.75 ///< [in] [optional] the fraction of a full circle at which to consider a
                                           ///< difference in angle discontinuous.
)
{
    realT full;
 
    if( degrad )
        full = two_pi<realT>();
    else
        full = static_cast<realT>( 360 );
 
    threshold *= full;
 
    realT adj = 0;
 
    if( fabs( angles[1] - angles[0] ) > threshold )
    {
        if( angles[1] > angles[0] )
            adj = -full;
        else
            adj = full;
 
        angles[1] += adj;
    }
 
    if( angles.size() == 2 )
        return 0;
 
    for( int i = 2; i < angles.size(); ++i )
    {
        angles[i] += adj;
 
        if( fabs( angles[i] - angles[i - 1] ) > threshold )
        {
            if( angles[i] > angles[i - 1] )
                adj += -full;
            else
                adj += full;
 
            angles[i] += adj;
        }
    }
 
    return 0;
}
 
/// Rotate a point about the origin.
/** The rotation is counter-clockwise for positive angles.
 *
 * \tparam realT a real floating point type
 *
 *  \ingroup geo
 */
template <typename realT>
void rotatePoint( realT &x0, ///< [in.out] the x-coordinate of the point to rotate.  On exit contains the rotated value.
                  realT &y0, ///< [in.out] the y-coordinate of the point to rotate.  On exit contains the rotated value.
                  realT angle ///< [in] the angle by which to rotate [radians]
)
{
    realT x1;
    realT y1;
 
    realT cq = cos( angle );
    realT sq = sin( angle );
 
    x1 = x0 * cq - y0 * sq;
    y1 = x0 * sq + y0 * cq;
 
    x0 = x1;
    y0 = y1;
}
 
} // namespace math
} // namespace mx
 
#endif // math_geo_hpp