Math.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2006-2011, SRI International (R)
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 #pragma once
00019 
00020 #ifndef __OpenKarto_Math_h__
00021 #define __OpenKarto_Math_h__
00022 
00023 #include <assert.h>
00024 #include <math.h>
00025 #include <float.h>
00026 #include <cstdlib>
00027 
00028 #include <OpenKarto/Types.h>
00029 
00030 namespace karto
00031 {
00032 
00034 
00035 
00043   const kt_double KT_PI         =  3.14159265358979323846;
00044   
00048   const kt_double KT_2PI        =  6.28318530717958647692;
00049   
00053   const kt_double KT_PI_2       =  1.57079632679489661923;
00054   
00058   const kt_double KT_PI_180     =  0.01745329251994329577;
00059   
00063   const kt_double KT_180_PI     = 57.29577951308232087685;
00064   
00071   const kt_double KT_TOLERANCE  = 1e-06;
00072   
00073   namespace math
00074   {    
00076 
00077 
00083     inline kt_double DegreesToRadians(kt_double degrees)
00084     {
00085       return degrees * KT_PI_180;
00086     }
00087     
00093     inline kt_double RadiansToDegrees(kt_double radians)
00094     {
00095       return radians * KT_180_PI;
00096     }
00097     
00103     template<typename T>
00104     inline T Square(T value)
00105     {
00106       return (value * value);
00107     }
00108     
00114     inline kt_double Round(kt_double value)
00115     {
00116       return value >= 0.0 ? floor(value + 0.5) : ceil(value - 0.5);
00117     }
00118     
00125     template<typename T>
00126     inline const T& Minimum(const T& value1, const T& value2)
00127     {
00128       return value1 < value2 ? value1 : value2;
00129     }
00130     
00137     template<typename T>
00138     inline const T& Maximum(const T& value1, const T& value2)
00139     {
00140       return value1 > value2 ? value1 : value2;
00141     }
00142     
00150     template<typename T> 
00151     inline const T& Clip(const T& n, const T& minValue, const T& maxValue)
00152     {
00153       return Minimum(Maximum(n, minValue), maxValue);
00154     }
00155     
00162     inline kt_bool DoubleEqual(kt_double a, kt_double b)
00163     {
00164       double delta = a - b; 
00165       return delta < 0.0 ? delta >= -KT_TOLERANCE : delta <= KT_TOLERANCE;
00166     }
00167     
00174     template<typename T>
00175     inline kt_bool IsUpTo(const T& value, const T& maximum)
00176     {
00177       return (value >= 0 && value < maximum);
00178     }
00179 
00180     //@cond EXCLUDE
00188     template<>
00189     inline kt_bool IsUpTo<kt_int32u>(const kt_int32u& value, const kt_int32u& maximum)
00190     {
00191       return (value < maximum);
00192     }
00193     //@endcond
00194     
00202     template<typename T>
00203     inline kt_bool InRange(const T& value, const T& a, const T& b)
00204     {
00205       return (value >= a && value <= b);
00206     }
00207     
00213     inline kt_double NormalizeAngle(kt_double angle)
00214     {
00215       while (angle < -KT_PI)
00216       {
00217         if (angle < -KT_2PI)
00218         {
00219           angle += (kt_int32u)(angle / -KT_2PI) * KT_2PI;
00220         }
00221         else
00222         {
00223           angle += KT_2PI;
00224         }
00225       }
00226       
00227       while (angle > KT_PI)
00228       {
00229         if (angle > KT_2PI)
00230         {
00231           angle -= (kt_int32u)(angle / KT_2PI) * KT_2PI;
00232         }
00233         else
00234         {
00235           angle -= KT_2PI;
00236         }
00237       }
00238       
00239       assert(math::InRange(angle, -KT_PI, KT_PI));
00240       
00241       return angle;
00242     }
00243     
00252     inline kt_double NormalizeAngleDifference(kt_double minuend, kt_double subtrahend)
00253     {
00254       while (minuend - subtrahend < -KT_PI)
00255       {
00256         minuend += KT_2PI;
00257       }
00258       
00259       while (minuend - subtrahend > KT_PI)
00260       {
00261         minuend -= KT_2PI;
00262       }
00263       
00264       return minuend;
00265     }
00266     
00274     template<class T>
00275     inline T AlignValue(size_t value, size_t alignValue = 8)
00276     {
00277       return static_cast<T> ((value + (alignValue - 1)) & ~(alignValue - 1));
00278     }
00279     
00285     template<class T>
00286     inline void Swap(T& x, T& y)
00287     {
00288       T temp = x;
00289       x = y;
00290       y = temp;
00291     }
00292 
00294   } // Math
00295     
00297 }
00298 
00299 #endif // __OpenKarto_Math_h__


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Mon Oct 6 2014 02:44:17