Math.h

Go to the documentation of this file.

/*
 * Copyright 2010 SRI International
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#ifndef OPEN_KARTO_MATH_H
#define OPEN_KARTO_MATH_H

#include <assert.h>
#include <math.h>
#include <limits>

#include <open_karto/Types.h>

namespace karto
{
  const kt_double KT_PI         =  3.14159265358979323846;  // The value of PI
  const kt_double KT_2PI        =  6.28318530717958647692;  // 2 * PI
  const kt_double KT_PI_2       =  1.57079632679489661923;  // PI / 2
  const kt_double KT_PI_180     =  0.01745329251994329577;  // PI / 180
  const kt_double KT_180_PI     = 57.29577951308232087685;  // 180 / PI

  const kt_double KT_TOLERANCE  = 1e-06;

  const kt_int32s INVALID_SCAN = std::numeric_limits<kt_int32s>::max();

  namespace math
  {
    inline kt_double DegreesToRadians(kt_double degrees)
    {
      return degrees * KT_PI_180;
    }

    inline kt_double RadiansToDegrees(kt_double radians)
    {
      return radians * KT_180_PI;
    }

    template<typename T>
    inline T Square(T value)
    {
      return (value * value);
    }

    inline kt_double Round(kt_double value)
    {
      return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5);
    }

    template<typename T>
    inline const T& Minimum(const T& value1, const T& value2)
    {
      return value1 < value2 ? value1 : value2;
    }

    template<typename T>
    inline const T& Maximum(const T& value1, const T& value2)
    {
      return value1 > value2 ? value1 : value2;
    }

    template<typename T>
    inline const T& Clip(const T& n, const T& minValue, const T& maxValue)
    {
      return Minimum(Maximum(n, minValue), maxValue);
    }

    inline kt_bool DoubleEqual(kt_double a, kt_double b)
    {
      double delta = a - b;
      return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE;
    }

    template<typename T>
    inline kt_bool IsUpTo(const T& value, const T& maximum)
    {
      return (value >= 0 && value < maximum);
    }

    template<>
    inline kt_bool IsUpTo<kt_int32u>(const kt_int32u& value, const kt_int32u& maximum)
    {
      return (value < maximum);
    }


    template<typename T>
    inline kt_bool InRange(const T& value, const T& a, const T& b)
    {
      return (value >= a && value <= b);
    }

    inline kt_double NormalizeAngle(kt_double angle)
    {
      while (angle < -KT_PI)
      {
        if (angle < -KT_2PI)
        {
          angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI;
        }
        else
        {
          angle += KT_2PI;
        }
      }

      while (angle > KT_PI)
      {
        if (angle > KT_2PI)
        {
          angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI;
        }
        else
        {
          angle -= KT_2PI;
        }
      }

      assert(math::InRange(angle, -KT_PI, KT_PI));

      return angle;
    }

    inline kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend)
    {
      while (minuend - subtrahend < -KT_PI)
      {
        minuend += KT_2PI;
      }

      while (minuend - subtrahend > KT_PI)
      {
        minuend -= KT_2PI;
      }

      return minuend;
    }

    template<class T>
    inline T AlignValue(size_t value, size_t alignValue = 8)
    {
      return static_cast<T> ((value + (alignValue - 1)) & ~(alignValue - 1));
    }
  }  // namespace math

}  // namespace karto

#endif  // OPEN_KARTO_MATH_H