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
00017 class MathUtil {
00018 public:
00019
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
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
00041         static inline float mod2pi(float ref, float v) { return ref + mod2pi(v-ref); }
00042
00043 // lousy approximation of arctan function, but good enough for our purposes (about 4 degrees)
00044   static inline double fast_atan2(double y, double x) {
00045     double coeff_1 = M_PI/4;
00046     double coeff_2 = 3*coeff_1;
00047     double abs_y = fabs(y)+1e-10;      // kludge to prevent 0/0 condition
00048
00049     double angle;
00050
00051     if (x >= 0) {
00052       double r = (x - abs_y) / (x + abs_y);
00053       angle = coeff_1 - coeff_1 * r;
00054     } else {
00055       double r = (x + abs_y) / (abs_y - x);
00056       angle = coeff_2 - coeff_1 * r;
00057     }
00058
00059     if (y < 0)
00060       return -angle;     // negate if in quad III or IV
00061     else
00062       return angle;
00063   }
00064
00065 };
00066
00067 } // namespace
00068
00069 #endif
```

apriltags
Author(s): Mitchell Wills
autogenerated on Thu Aug 27 2015 12:23:28