fixed some issues
added new pose/turn detections new helper classes define-flags for libEigen
This commit is contained in:
219
math/Floatingpoint.h
Normal file
219
math/Floatingpoint.h
Normal file
@@ -0,0 +1,219 @@
|
||||
#ifndef FLOATINGPOINT_H
|
||||
#define FLOATINGPOINT_H
|
||||
|
||||
#include "../Assertions.h"
|
||||
|
||||
template <typename T> struct FP {
|
||||
|
||||
T val;
|
||||
|
||||
public:
|
||||
|
||||
FP(int val) : val(val) {;}
|
||||
FP(size_t val) : val(val) {;}
|
||||
FP(float val) : val(val) {;}
|
||||
FP(double val) : val(val) {;}
|
||||
|
||||
FP(const FP<float>& o) : val(o.val) {;}
|
||||
FP(const FP<double>& o) : val(o.val) {;}
|
||||
|
||||
FP(const volatile FP<float>& o) : val(o.val) {;}
|
||||
FP(const volatile FP<double>& o) : val(o.val) {;}
|
||||
|
||||
//template <typename U> explicit FP(const FP<U>& o) : val(o.val) {;}
|
||||
|
||||
//operator FP<float>() const {return FP<float>(val);}
|
||||
operator FP<double>() const {return FP<double>(val);}
|
||||
|
||||
operator float() const {return static_cast<float>(val);}
|
||||
operator double() const {return static_cast<double>(val);}
|
||||
|
||||
|
||||
FP& operator += (const FP o) {
|
||||
checkAddSub(val, o.val);
|
||||
val += o.val;
|
||||
return *this;
|
||||
}
|
||||
|
||||
FP& operator -= (const FP o) {
|
||||
checkAddSub(val, o.val);
|
||||
val -= o.val;
|
||||
return *this;
|
||||
}
|
||||
|
||||
FP& operator *= (const FP o) {
|
||||
val *= o.val;
|
||||
return *this;
|
||||
}
|
||||
|
||||
FP& operator /= (const FP o) {
|
||||
val /= o.val;
|
||||
return *this;
|
||||
}
|
||||
|
||||
/** -------- X -------- */
|
||||
|
||||
FP operator - () const {
|
||||
return FP(-val);
|
||||
}
|
||||
|
||||
FP operator + (const FP o) const {
|
||||
checkAddSub(val, o.val);
|
||||
return FP(val+o.val);
|
||||
}
|
||||
|
||||
FP operator - (const FP o) const {
|
||||
checkAddSub(val, o.val);
|
||||
return FP(val-o.val);
|
||||
}
|
||||
|
||||
FP operator * (const FP o) const {
|
||||
return FP(val*o.val);
|
||||
}
|
||||
|
||||
FP operator / (const FP o) const {
|
||||
return FP(val/o.val);
|
||||
}
|
||||
|
||||
FP& operator = (const FP o) {
|
||||
this->val = o;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
volatile FP& operator = (const FP& o) volatile {
|
||||
this->val = o;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool operator == (const FP o) const {
|
||||
return val == o.val;
|
||||
}
|
||||
|
||||
bool operator != (const FP o) const {
|
||||
return val != o.val;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
friend std::ostream& operator << (std::ostream& stream, const FP<T>& fp) {
|
||||
stream << fp.val;
|
||||
return stream;
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
static inline void checkAddSub(const T a, const T b) {
|
||||
const int x = expDiff(a,b);
|
||||
Assert::isTrue(x <= 8, "invalid precision for +/-");
|
||||
}
|
||||
|
||||
static inline int expDiff(const T a, const T b) {
|
||||
int x, y;
|
||||
frexp(a, &x);
|
||||
frexp(b, &y);
|
||||
const int d = (std::max(x,y) - std::min(x,y)) / 3; // 3 exponents in binary per 10 in decimal?
|
||||
return d;
|
||||
}
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
inline FP<double> operator + (const FP<float>& a, const FP<double>& b) {return FP<double>(a.val + b.val);}
|
||||
inline FP<double> operator + (const FP<double>& a, const FP<float>& b) {return FP<double>(a.val + b.val);}
|
||||
inline FP<float> operator + (const FP<float>& a, const float b) {return a + FP<float>(b);}
|
||||
inline FP<float> operator + (const float a, const FP<float>& b) {return FP<float>(a) + b;}
|
||||
inline FP<double> operator + (const FP<float>& a, const double b) {return a + FP<double>(b);}
|
||||
inline FP<double> operator + (const double a, const FP<float>& b) {return FP<double>(a) + b;}
|
||||
inline FP<double> operator + (const FP<double>& a, const float b) {return a + FP<double>(b);}
|
||||
inline FP<double> operator + (const float a, const FP<double>& b) {return FP<double>(a) + b;}
|
||||
inline FP<double> operator + (const FP<double>& a, const double b) {return a + FP<double>(b);}
|
||||
inline FP<double> operator + (const double a, const FP<double>& b) {return FP<double>(a) + b;}
|
||||
|
||||
inline FP<double> operator - (const FP<float>& a, const FP<double>& b) {return FP<double>(a.val - b.val);}
|
||||
inline FP<double> operator - (const FP<double>& a, const FP<float>& b) {return FP<double>(a.val - b.val);}
|
||||
inline FP<float> operator - (const FP<float>& a, const float b) {return a - FP<float>(b);}
|
||||
inline FP<float> operator - (const float a, const FP<float>& b) {return FP<float>(a) - b;}
|
||||
inline FP<double> operator - (const FP<float>& a, const double b) {return a - FP<double>(b);}
|
||||
inline FP<double> operator - (const double a, const FP<float>& b) {return FP<double>(a) - b;}
|
||||
inline FP<double> operator - (const FP<double>& a, const float b) {return a - FP<double>(b);}
|
||||
inline FP<double> operator - (const float a, const FP<double>& b) {return FP<double>(a) - b;}
|
||||
inline FP<double> operator - (const FP<double>& a, const double b) {return a - FP<double>(b);}
|
||||
inline FP<double> operator - (const double a, const FP<double>& b) {return FP<double>(a) - b;}
|
||||
|
||||
inline FP<double> operator / (const FP<float>& a, const FP<double>& b) {return FP<double>(a.val / b.val);}
|
||||
inline FP<double> operator / (const FP<double>& a, const FP<float>& b) {return FP<double>(a.val / b.val);}
|
||||
inline FP<float> operator / (const FP<float>& a, const float b) {return a / FP<float>(b);}
|
||||
inline FP<float> operator / (const float a, const FP<float>& b) {return FP<float>(a) / b;}
|
||||
inline FP<double> operator / (const FP<float>& a, const double b) {return a / FP<double>(b);}
|
||||
inline FP<double> operator / (const double a, const FP<float>& b) {return FP<double>(a) / b;}
|
||||
inline FP<double> operator / (const FP<double>& a, const float b) {return a / FP<double>(b);}
|
||||
inline FP<double> operator / (const float a, const FP<double>& b) {return FP<double>(a) / b;}
|
||||
inline FP<double> operator / (const FP<double>& a, const double b) {return a / FP<double>(b);}
|
||||
inline FP<double> operator / (const double a, const FP<double>& b) {return FP<double>(a) / b;}
|
||||
|
||||
inline FP<double> operator * (const FP<float>& a, const FP<double>& b) {return FP<double>(a.val * b.val);}
|
||||
inline FP<double> operator * (const FP<double>& a, const FP<float>& b) {return FP<double>(a.val * b.val);}
|
||||
inline FP<float> operator * (const FP<float>& a, const float b) {return a * FP<float>(b);}
|
||||
inline FP<float> operator * (const float a, const FP<float>& b) {return FP<float>(a) * b;}
|
||||
inline FP<double> operator * (const FP<float>& a, const double b) {return a * FP<double>(b);}
|
||||
inline FP<double> operator * (const double a, const FP<float>& b) {return FP<double>(a) * b;}
|
||||
inline FP<double> operator * (const FP<double>& a, const float b) {return a * FP<double>(b);}
|
||||
inline FP<double> operator * (const float a, const FP<double>& b) {return FP<double>(a) * b;}
|
||||
inline FP<double> operator * (const FP<double>& a, const double b) {return a * FP<double>(b);}
|
||||
inline FP<double> operator * (const double a, const FP<double>& b) {return FP<double>(a) * b;}
|
||||
|
||||
inline FP<float> operator / (const FP<float>& a, const int b) {return a / FP<float>(b);}
|
||||
inline FP<float> operator / (const FP<float>& a, const size_t b) {return a / FP<float>(b);}
|
||||
|
||||
inline FP<double> operator / (const FP<double>& a, const int b) {return a / FP<double>(b);}
|
||||
inline FP<double> operator / (const FP<double>& a, const size_t b) {return a / FP<double>(b);}
|
||||
|
||||
|
||||
//template <typename T, typename U> FP<T> operator + (const FP<T>& a, const U b) {return a + FP<T>(b);}
|
||||
//template <typename T, typename U> FP<T> operator + (const U a, const FP<T>& b) {return FP<T>(a) + b;}
|
||||
|
||||
//template <typename T, typename U> FP<T> operator - (const FP<T>& a, const U b) {return a - FP<T>(b);}
|
||||
//template <typename T, typename U> FP<T> operator - (const U a, const FP<T>& b) {return FP<T>(a) - b;}
|
||||
|
||||
//template <typename T, typename U> FP<T> operator * (const FP<T>& a, const U b) {return a * FP<T>(b);}
|
||||
//template <typename T, typename U> FP<T> operator * (const U a, const FP<T>& b) {return FP<T>(a) * b;}
|
||||
|
||||
//template <typename T, typename U> FP<T> operator / (const FP<T>& a, const U b) {return a / FP<T>(b);}
|
||||
//template <typename T, typename U> FP<T> operator / (const U a, const FP<T>& b) {return FP<T>(a) / b;}
|
||||
|
||||
|
||||
|
||||
namespace std {
|
||||
template<class T> FP<T> pow (const FP<T>& x, const FP<T>& y) {
|
||||
return FP<T>(std::pow(x.val, y.val));
|
||||
}
|
||||
template<class T> FP<T> pow (const FP<T>& x, const T& y) {
|
||||
return FP<T>(std::pow(x.val, y));
|
||||
}
|
||||
template<class T> FP<T> sqrt (const FP<T> x) {
|
||||
return FP<T>(std::sqrt(x.val));
|
||||
}
|
||||
template<class T> bool isnan(FP<T> x) {
|
||||
return std::isnan(x.val);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef WITH_FP
|
||||
using FPFloat = FP<float>;
|
||||
using FPDouble = FP<double>;
|
||||
using FPDefault = FPFloat;
|
||||
#else
|
||||
using FPFloat = float;
|
||||
using FPDouble = double;
|
||||
using FPDefault = double;
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
#endif // FLOATINGPOINT_H
|
||||
Reference in New Issue
Block a user