Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MathUtil.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #ifndef MATHUTIL_H
00004 #define MATHUTIL_H
00005 
00006 #include <cmath>
00007 #include <cfloat>
00008 #include <cstdlib>
00009 #include <ostream>
00010 #include <utility>
00011 
00012 namespace AprilTags {
00013 
00014 std::ostream& operator<<(std::ostream &os, const std::pair<float,float> &pt);
00015 
00016 //! Miscellaneous math utilities and fast exp functions.
00017 class MathUtil {
00018 public:
00019   
00020   //! Returns the square of a value.
00021   static inline float square(float x) { return x*x; }
00022   
00023   static inline float distance2D(const std::pair<float,float> &p0, const std::pair<float,float> &p1) {
00024     float dx = p0.first - p1.first;
00025     float dy = p0.second - p1.second;
00026     return std::sqrt(dx*dx + dy*dy);
00027   }
00028   
00029   //! Returns a result in [-Pi, Pi]
00030   static inline float mod2pi(float vin) {
00031     const float twopi = 2 * (float)M_PI;
00032     const float twopi_inv = 1.f / (2.f * (float)M_PI);
00033     float absv = std::abs(vin);
00034     float q = absv*twopi_inv + 0.5f;
00035     int qi = (int) q;
00036     float r = absv - qi*twopi;
00037     return (vin<0) ? -r : r;
00038   }
00039   
00040   //! Returns a value of v wrapped such that ref and v differ by no more than +/- Pi
00041   static inline float mod2pi(float ref, float v) { return ref + mod2pi(v-ref); }
00042   
00043 };
00044 
00045 } // namespace
00046 
00047 #endif

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