mathutils.h

Go to the documentation of this file.
```00001 //-*-c++-*-
00002 #ifndef __mathutils_h__
00003 #define __mathutils_h__
00004
00005 #include <cmath>
00006 #include <stdexcept>
00007 #include <algorithm>
00008
00009 #ifdef PLATFORM_APERIOS
00010 //missing/broken isnan
00011 namespace std {
00012   inline bool isnan(float x) {
00013     const int EXP  = 0x7f800000;
00014     const int FRAC = 0x007fffff;
00015     const int y = *((int*)(&x));
00016     return ((y&EXP)==EXP && (y&FRAC)!=0);
00017   }
00018 }
00019 #endif
00020
00021 //! a variety of handy mathematical functions, many of which are templated
00022 namespace mathutils {
00023   //! euclidean distance of two points, see squareDistance()
00024   inline float distance(float x1, float y1, float x2, float y2) {
00025     return ::hypotf(x1-x2, y1-y2);
00026   }
00027
00028   //! euclidean distance of two points, see squareDistance()
00029   inline double distance(double x1, double y1, double x2, double y2) {
00030     return ::hypot(x1-x2, y1-y2);
00031   }
00032
00033   //! Clips @a n to a minimum of @a low or maximum of @a high.
00034   /*! If @a low and @a high are inverted, high is returned. */
00035   template <class num>
00036   inline num limitRange(num n, num low, num high) {
00037     if (n<=low) return low;
00038     if (n>=high) return high;
00039     return n;
00040   }
00041
00042   //! Returns the log base 2 of a number
00043   /*! This template implementation does a bit shifting method appropriate for
00044    *  integers.  Specializations are provided for float and double to use the 'real' log() */
00045   template <class num>
00046   inline num log2t(num x) {
00047     num ans=0;
00048     for(unsigned int mag=sizeof(num)*4; mag>0; mag/=2) {
00049       num y=x>>mag;
00050       if(y>0) {
00051         x=y;
00052         ans+=mag;
00053       }
00054     }
00055     return ans;
00056   }
00057   //! returns the log base 2 for a 'float' value
00058   template <>
00059   inline float log2t(float x) {
00060     return std::log(x)/(float)M_LN2;
00061   }
00062   //! returns the log base 2 for a 'double' value
00063   template <>
00064   inline double log2t(double x) {
00065     return std::log(x)/M_LN2;
00066   }
00067
00068   //! converts from degrees to radians
00069   inline double deg2rad(double x) {
00070     return x*M_PI/180;
00071   }
00072
00073   //! converts from radians to degrees
00074   inline double rad2deg(double x) {
00075     return x/M_PI*180;
00076   }
00077
00078   //! converts from degrees to radians
00079   inline float deg2rad(float x) {
00080     return x*static_cast<float>(M_PI)/180;
00081   }
00082
00083   //! converts from radians to degrees
00084   inline float rad2deg(float x) {
00085     return x/static_cast<float>(M_PI)*180;
00086   }
00087
00088   //! Will set a to be between (-pi,pi) (inclusive), just like atan2()
00089   /*! This is only efficient if we expect that the angle is already close to the range... otherwise use fmod...
00090    *  See also Measures.h, this normalize is identical to the AngSignPi::normalize() */
00091   template<class num>
00092   num normalizeAngle(num value) {
00093     const num Pi=static_cast<num>(M_PI);
00094     const num TwoPi=static_cast<num>(2*M_PI);
00095     if ( value > Pi ) {
00096       if ( value <= TwoPi ) {
00097         value -= TwoPi;
00098       } else { // use fmod only if necessary
00099         value = std::fmod(value,TwoPi);
00100         if( value > Pi )
00101           value -= TwoPi;
00102       }
00103     } else if ( value <= -Pi ) {
00104       if ( value >= -TwoPi ) {
00105         value += TwoPi;
00106       } else { // use fmod only if necessary
00107         value = std::fmod(value,TwoPi);
00108         if( value <= -Pi )
00109           value += TwoPi;
00110       }
00111     }
00112     return value;
00113   }
00114
00115   // Aperios doesn't provide a random(), only rand()...
00116   // For testing consistency, Aibo target models always use rand(), regardless of local/aperios platform.
00117 #if defined(TGT_ERS210) || defined(TGT_ERS220) || defined(TGT_ERS2xx) || defined(TGT_ERS7) || defined(PLATFORM_APERIOS)
00118
00119   //! Chooses a number in the range [min,max) (exclusive upper bound)
00120   inline float sampleRange(float min, float max) {
00121     return rand() / (float)RAND_MAX * (max-min) + min;
00122   }
00123   //! Chooses a number in the range [min,max) (exclusive upper bound)
00124   inline double sampleRange(double min, double max) {
00125     return rand() / (double)RAND_MAX * (max-min) + min;
00126   }
00127   //! Chooses a number in the range [min,max) (exclusive upper bound)
00128   template<class num>
00129   num sampleInteger(num min, num max) {
00130     return rand() % (max-min) + min;
00131   }
00132
00133 #else
00134
00135   //! Chooses a number in the range [min,max) (exclusive upper bound)
00136   inline float sampleRange(float min, float max) {
00137     return random() / (float)(1U<<31) * (max-min) + min;
00138   }
00139   //! Chooses a number in the range [min,max) (exclusive upper bound)
00140   inline double sampleRange(double min, double max) {
00141     return random() / (double)(1U<<31) * (max-min) + min;
00142   }
00143   //! Chooses a number in the range [min,max) (exclusive upper bound)
00144   template<class num>
00145   num sampleInteger(num min, num max) {
00146     return random() % (max-min) + min;
00147   }
00148
00149 #endif
00150
00151   //! Pass a collection of std::pair<value,weight> (map, vector, etc.), returns a value based on a weighted sample
00152   /*! Throws an exception if the collection is empty; if all weights are zero, performs a uniform sampling. */
00153   template<class C>
00154   typename C::value_type::first_type weightedPick(const C& c) {
00155     typedef typename C::value_type::first_type T;
00156     typedef typename C::value_type::second_type W;
00157     typedef std::pair<W,const T*> pair;
00158     if(c.size()==0)
00159       throw std::runtime_error("mathutils::weightedPick passed empty collection");
00160     std::vector<pair> activation;
00161     activation.reserve(c.size());
00162     W curAct=0;
00163     for(typename C::const_iterator it=c.begin(); it!=c.end(); ++it)
00164       activation.push_back(pair(curAct += it->second, &it->first));
00165     //ASSERTRETVAL(curAct>0, "mathutils::weightedPick given zero total weight",c.begin()->first);
00166     if(curAct<=0) {
00167       // all zero weight, just pick uniformly
00168       return *activation[sampleInteger<size_t>(0, activation.size())].second;
00169     }
00170     pair pick(sampleRange(0, curAct), NULL);
00171     typename std::vector<pair>::const_iterator it = std::upper_bound(activation.begin(),activation.end(),pick);
00172     return *it->second;
00173   }
00174
00175 }
00176
00177 #endif
```

 Tekkotsu v5.1CVS Generated Mon May 9 04:58:44 2016 by Doxygen 1.6.3