NumberManipulation.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, LABUST, UNIZG-FER
 *  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 LABUST 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 NUMBERMANIPULATION_HPP_
#define NUMBERMANIPULATION_HPP_

#define _USE_MATH_DEFINES
#include <cmath>
#include <labust/math/Limits.hpp>

namespace labust
{
  namespace math
  {
    //const double M_2PI = 2*M_PI;
    inline double wrapRad(double angle)
    {
      angle = fmod(angle,2*M_PI);
      if (angle > M_PI) return -2*M_PI + angle;
      if (angle <= -M_PI) return 2*M_PI + angle;
      return angle;
    }
    inline double wrapDeg(double angle)
    {
      angle = fmod(angle,360.);
      if (angle > 180) return -360. + angle;
      if (angle <= -180) return 360. + angle;
      return angle;
    }
    template <class type, typename precission>
    inline type coerce(type value, const Limit<precission>& limit)
    {
      if (value>limit.max) return limit.max;
      if (value<limit.min) return limit.min;
      return value;
    }
    template<class type>
    inline type coerce(type value, double min, double max)
    {
      if (value>max) return max;
      if (value<min) return min;
      return value;
    }
    template <class Iterator>
    inline double mean(const Iterator& first, const Iterator& last)
    {
        double sum(0);
        unsigned int size(0);
        for (Iterator cnt = first; cnt!=last; ++cnt,++size) sum += (*cnt);
        return (size)?(sum/size):0;
    }
    template <class Vector>
    inline double mean(const Vector& vec)
    {
        double sum(0);
        unsigned int size(0);
        for (typename Vector::const_iterator cnt = vec.begin(); cnt!=vec.end(); ++cnt,++size) sum += (*cnt);
        return (size)?(sum/size):0;
    }
    template<class Vector>
    inline double std2(const Vector& vec)
    {
      double sum(0),mean(labust::math::mean(vec));
      unsigned int size(0);
      for(typename Vector::const_iterator cnt = vec.begin(); cnt != vec.end(); ++cnt, ++size) sum+=std::pow(((*cnt) - mean),2);
      return size?std::sqrt(sum/size):-1;
    }
    struct unwrap
    {
        unwrap():anglek_1(0),index(0){};
        double operator()(double angle)
        {
                double u = angle - this->anglek_1;
                this->anglek_1=angle;

                if (u<=-M_PI)
                {
                        ++index;
                }
                else if (u>=M_PI) --index;

                return this->index*2*M_PI + angle;
        }

    private:
        double anglek_1;
        int index;
    };
  }
}
/* NUMBERMANIPULATION_HPP_ */
#endif