Math.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2010 SRI International
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #ifndef OPEN_KARTO_MATH_H
00019 #define OPEN_KARTO_MATH_H
00020 
00021 #include <assert.h>
00022 #include <math.h>
00023 #include <limits>
00024 
00025 #include <open_karto/Types.h>
00026 
00027 namespace karto
00028 {
00032   const kt_double KT_PI         =  3.14159265358979323846;  // The value of PI
00033   const kt_double KT_2PI        =  6.28318530717958647692;  // 2 * PI
00034   const kt_double KT_PI_2       =  1.57079632679489661923;  // PI / 2
00035   const kt_double KT_PI_180     =  0.01745329251994329577;  // PI / 180
00036   const kt_double KT_180_PI     = 57.29577951308232087685;  // 180 / PI
00037 
00041   const kt_double KT_TOLERANCE  = 1e-06;
00042 
00047   const kt_int32s INVALID_SCAN = std::numeric_limits<kt_int32s>::max();
00048 
00049   namespace math
00050   {
00056     inline kt_double DegreesToRadians(kt_double degrees)
00057     {
00058       return degrees * KT_PI_180;
00059     }
00060 
00066     inline kt_double RadiansToDegrees(kt_double radians)
00067     {
00068       return radians * KT_180_PI;
00069     }
00070 
00076     template<typename T>
00077     inline T Square(T value)
00078     {
00079       return (value * value);
00080     }
00081 
00087     inline kt_double Round(kt_double value)
00088     {
00089       return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5);
00090     }
00091 
00098     template<typename T>
00099     inline const T& Minimum(const T& value1, const T& value2)
00100     {
00101       return value1 < value2 ? value1 : value2;
00102     }
00103 
00110     template<typename T>
00111     inline const T& Maximum(const T& value1, const T& value2)
00112     {
00113       return value1 > value2 ? value1 : value2;
00114     }
00115 
00123     template<typename T>
00124     inline const T& Clip(const T& n, const T& minValue, const T& maxValue)
00125     {
00126       return Minimum(Maximum(n, minValue), maxValue);
00127     }
00128 
00135     inline kt_bool DoubleEqual(kt_double a, kt_double b)
00136     {
00137       double delta = a - b;
00138       return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE;
00139     }
00140 
00146     template<typename T>
00147     inline kt_bool IsUpTo(const T& value, const T& maximum)
00148     {
00149       return (value >= 0 && value < maximum);
00150     }
00151 
00158     template<>
00159     inline kt_bool IsUpTo<kt_int32u>(const kt_int32u& value, const kt_int32u& maximum)
00160     {
00161       return (value < maximum);
00162     }
00163 
00164 
00171     template<typename T>
00172     inline kt_bool InRange(const T& value, const T& a, const T& b)
00173     {
00174       return (value >= a && value <= b);
00175     }
00176 
00182     inline kt_double NormalizeAngle(kt_double angle)
00183     {
00184       while (angle < -KT_PI)
00185       {
00186         if (angle < -KT_2PI)
00187         {
00188           angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI;
00189         }
00190         else
00191         {
00192           angle += KT_2PI;
00193         }
00194       }
00195 
00196       while (angle > KT_PI)
00197       {
00198         if (angle > KT_2PI)
00199         {
00200           angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI;
00201         }
00202         else
00203         {
00204           angle -= KT_2PI;
00205         }
00206       }
00207 
00208       assert(math::InRange(angle, -KT_PI, KT_PI));
00209 
00210       return angle;
00211     }
00212 
00221     inline kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend)
00222     {
00223       while (minuend - subtrahend < -KT_PI)
00224       {
00225         minuend += KT_2PI;
00226       }
00227 
00228       while (minuend - subtrahend > KT_PI)
00229       {
00230         minuend -= KT_2PI;
00231       }
00232 
00233       return minuend;
00234     }
00235 
00243     template<class T>
00244     inline T AlignValue(size_t value, size_t alignValue = 8)
00245     {
00246       return static_cast<T> ((value + (alignValue - 1)) & ~(alignValue - 1));
00247     }
00248   }  // namespace math
00249 
00250 }  // namespace karto
00251 
00252 #endif  // OPEN_KARTO_MATH_H


open_karto
Author(s):
autogenerated on Thu Jun 6 2019 21:02:56