rtcMath.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcMathUtil.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-03-20 11:37:03 -0700 (Fri, 20 Mar 2009) $
00016  * changed by .: $Author: wg75pal $
00017  * revision ...: $Revision: 106 $
00018  */
00019 #ifndef RTC_MATHUTIL_H
00020 #define RTC_MATHUTIL_H
00021 
00022 //== INCLUDES ==================================================================
00023 #include <cmath>
00024 #include <cstdlib>
00025 #include <cstring>
00026 #include <iostream>
00027 #include <iomanip>
00028 #include <sstream>
00029 
00030 // rtc
00031 #include <rtc/rtcException.h>
00032 #include <rtc/rtcStringTools.h>
00033 
00034 //#define MATMATH_CHECK_BOUNDS 1
00035 
00037 #define EPS_       1E-5
00038 
00040 #define D2R       0.0174532925199432957692
00041 
00043 #define R2D       57.295779513082320876
00044 
00046 #define LN2       0.693147180559945309417
00047 #define LN10      2.30258509299404568402
00048 #define PI        3.14159265358979323846
00049 #define TWOPI     6.28318530717958647692
00050 #define PI_2      1.57079632679489661923
00051 #define PI_4      0.785398163397448309616
00052 #define SQRT2     1.41421356237309504880
00053 #define SQRT1_2   0.707106781186547524401
00054 
00055 //== NAMESPACES ================================================================
00056 namespace rtc {
00057 
00059 template <class T> inline T rtc_clamp(const T x, const T a, const T b) {
00060   if (x < a) return a;
00061   else return (x>b)?b:x;
00062 }
00063 
00065 template <class T> inline bool rtc_within(const T x, const T a, const T b) {
00066   return (x>=a && x<=b);
00067 }
00068 
00070 template <class T> inline T rtc_sign(const T x) {
00071   return ((x != T(0))? x/fabs(x) : T(1));
00072 }
00073 
00075 template <class T> inline int rtc_trunc(const T x) {
00076   return (int)x;
00077 }
00078 
00080 template <class T> inline int rtc_round(const T x) {
00081   if (x >= 0)
00082     return (int)(x + T(0.5));
00083   else
00084     return (int)(x - T(0.5));
00085 }
00086 
00087 template <class T> inline       T rtc_lerp(const T& t0, const T& t1, float a) {
00088   return t0 * (1.0f - a) + t1 * a;
00089 }
00090 
00092 template <class T> inline T rtc_pythag(T a, T b) {
00093   T absa=fabs(a); T absb=fabs(b);
00094   if (absa > absb) return absa*sqrt(1.0+(absb/absa)*(absb/absa));
00095   else return (absb==T(0) ? T(0) : absb*sqrt(T(1)+(absa/absb)*(absa/absb)));
00096 }
00097 
00099 template <class T> inline T rtc_sqr(const T x) { return x*x; }
00100 
00102 template <class T> inline T rtc_cube(const T x) { return x*x*x; }
00103 
00105 template <class T> inline T rtc_quart(const T x) { return x*x*x*x; }
00106 
00108 template <class T> inline T rtc_quint(const T x) { return x*x*x*x*x; }
00109 
00111 template <class T> inline T rtc_cuberoot(const T x) { return cube(sqrt(x)); }
00112 
00114 template <class T> inline T rtc_max(const T x1, const T x2) {
00115   return (x1>x2)?x1:x2;
00116 }
00117 
00119 template <class T> inline T rtc_max(const T x1, const T x2, const T x3) {
00120   return rtc_max(rtc_max(x1,x2),x3);
00121 }
00122 
00124 template <class T> inline T rtc_min(const T x1, const T x2) {
00125   return (x1<x2)?x1:x2;
00126 }
00127 
00129 template <class T> inline T rtc_min(const T x1, const T x2, const T x3) {
00130   return rtc_min(rtc_min(x1,x2),x3);
00131 }
00132 
00134 template <class T> inline void rtc_swap(T& x1, T& x2) {
00135   T temp = x1; x1 = x2; x2 = temp;
00136 }
00137 
00139 template <class T> inline int rtc_compare(const void * a, const void * b) {
00140   if (*(T*)a < *(T*)b) return -1;
00141   else if (*(T*)a == *(T*)b) return 0;
00142   else return 1;
00143 }
00144 
00145 // random number functions
00146 
00148 template <class T> inline T rtc_normal_rand(T mean = T(0), T stdev = T(1)) {
00149   const double norm = 1.0/(RAND_MAX + 1.0);
00150   double u = 1.0 - std::rand()*norm;
00151   double v = std::rand()*norm;
00152   double z = std::sqrt(-2.0*log(u))*std::cos(2.0*M_PI*v);
00153   return T(mean + stdev*z);
00154 }
00155 
00156 /*
00158 template <class T, int M> Vec<T,M> multivariateGauss(const Vec<T,M>& mean = Vec<T,M>(0),
00159                                                      const SMat<T,M>& cov = SMat<T,M>(1)) {
00160   Vec<T,M> v;
00161   SMat<T,M> S(cov);
00162   int n=S.choleskyDecomp();
00163   assert(n==0);
00164   S.transpose();
00165   v.normalRand();
00166   v = S*v;
00167   v += mean;
00168   return v;
00169 }
00170 */
00171 
00173 template <class T> inline T rtc_uniform_rand(T lower = T(0), T upper = T(1)) {
00174   return lower + T(double(upper - lower)*std::rand()/(RAND_MAX+1.0));
00175 }
00176 
00178 void seedRand(void);
00179 
00181 inline double rtc_abs(const double& x) { return std::fabs(x); }
00182 inline float rtc_abs(const float& x) { return (float)std::fabs(static_cast<double>(x)); }
00183 inline int rtc_abs(const int& x) { return std::abs(x); }
00184 inline long int rtc_abs(const long int& x) { return std::labs(x); }
00185 #ifndef _WIN32
00186 inline long long int rtc_abs(const long long int& x) { return std::llabs(x); }
00187 #endif
00188 
00190 template <class T> inline T rtc_sqrt(const T& x) {
00191   return static_cast<T>(std::sqrt(static_cast<double>(x)));
00192 }
00193 
00195 template <class T, class U> inline T rtc_pow(const T& x, const U& y) {
00196   return static_cast<T>(std::pow(static_cast<double>(x),static_cast<double>(y)));
00197 }
00198 
00200 template<> inline float rtc_pow<float,int>(const float &x, const int &y)
00201 {
00202   float ret=1;
00203   for (int i=0;i<y;i++) ret*=x;
00204   return ret;
00205 }
00206 
00208 template <class T> inline T rtc_normalize_theta(T theta)
00209 {
00210   int multiplier;
00211 
00212   if (theta >= -T(PI) && theta < T(PI))
00213     return theta;
00214 
00215   multiplier = (int)(theta / T(TWOPI));
00216   theta = theta - multiplier*T(TWOPI);
00217   if (theta >= T(PI))
00218     theta -= T(TWOPI);
00219   if (theta < -T(PI))
00220     theta += T(TWOPI);
00221 
00222   return theta;
00223 }
00224 
00225 template <class T> inline T rtc_safe_acos(T costheta)
00226 {
00227   if (costheta>T(1)) costheta=T(1);
00228   if (costheta<T(-1)) costheta=T(-1);
00229   return (T)acos(double(costheta));
00230 }
00231 
00233 template <class T> inline void rtc_cartesian_to_spherical(const T& x, const T& y, const T& z, T& r, T& theta, T& phi)
00234 {
00235   r = sqrt(rtc_sqr(x)+rtc_sqr(y)+rtc_sqr(z));
00236   if(r<EPS_) {
00237     r = theta = phi = 0;
00238   }
00239   else {
00240     theta = atan2(y,x);
00241     phi = acos(z/r);
00242   }
00243 }
00244 
00246 template <class T> inline void rtc_spherical_to_cartesian(const T& r, const T& theta, const T& phi, T& x, T& y, T& z)
00247 {
00248   float ct = cos(theta);
00249   float st = sin(theta);
00250   float sp = sin(phi);
00251   float cp = cos(phi);
00252   x = r*ct*sp;
00253   y = r*st*sp;
00254   z = r*cp;
00255 }
00256 
00258 template <class T> inline void rtc_normalize_spherical(T& theta, T& phi)
00259 {
00260   int multiplier;
00261 
00262   // normalize phi
00263   if (phi < T(0) || phi > T(PI)) {
00264     multiplier = (int)(phi / T(TWOPI));
00265     phi = phi - multiplier*T(TWOPI);
00266     if (phi > T(PI)) {
00267       phi = T(TWOPI)-phi;
00268       theta += T(PI);
00269     }
00270     else if (phi < T(0)) {
00271       phi*=-1;
00272       theta += T(PI);
00273     }
00274   }
00275 
00276   // normalize theta
00277   theta = rtc_normalize_theta(theta);
00278 }
00279 
00281 inline float rtc_sin(float x) { return static_cast<float>(std::sin(x)); }
00283 inline double rtc_sin(double x) { return std::sin(x); }
00285 inline float rtc_cos(float x) { return static_cast<float>(std::cos(x)); }
00287 inline double rtc_cos(double x) { return std::cos(x); }
00289 inline float rtc_tan(float x) { return static_cast<float>(std::tan(x)); }
00291 inline double rtc_tan(double x) { return std::tan(x); }
00293 inline float rtc_asin(float x) { return static_cast<float>(std::asin(x)); }
00295 inline double rtc_asin(double x) { return std::asin(x); }
00297 inline float rtc_acos(float x) { return static_cast<float>(std::acos(x)); }
00299 inline double rtc_acos(double x) { return std::acos(x); }
00301 inline float rtc_atan(float x) { return static_cast<float>(std::atan(x)); }
00303 inline double rtc_atan(double x) { return std::atan(x); }
00305 inline float rtc_atan2(float y,float x) { return static_cast<float>(std::atan2(y,x)); }
00307 inline double rtc_atan2(double y,double x) { return std::atan2(y,x); }
00308 
00309 //==============================================================================
00310 } // namespace rtc
00311 //==============================================================================
00312 #endif // RTC_MATHUTIL_H defined
00313 //==============================================================================
00314 


rtc
Author(s): Benjamin Pitzer
autogenerated on Thu Jan 2 2014 11:04:53