mathutil.hpp

Go to the documentation of this file.

 
#ifndef INCLUDE_IMUNANO33_MATHUTIL_HPP_
#define INCLUDE_IMUNANO33_MATHUTIL_HPP_
 
#ifdef IMUNANO33_EMBED
#include <float.h>
#include <math.h>
#else
#include <algorithm>
#include <cmath>
#include <limits>
#endif
 
#ifdef IMUNANO33_EMBED
#include "imunano33/sv_embed.hpp"
#else
#include "imunano33/simplevectors.hpp"
#endif
#include "imunano33/unit.hpp"
 
namespace imunano33 {
#ifdef IMUNANO33_EMBED
using Vector3D =
    svector::EmbVec3D; 
#else
using std::fabs;
using svector::Vector3D;
#endif
 
class MathUtil {
public:
  static bool nearZero(const num_t num) { return nearZero(num, NEAR_ZERO); }
 
  static bool nearZero(const num_t num, const num_t tol) {
    return fabs(num) < tol;
  }
 
  static bool nearZero(const Vector3D &vec) { return nearZero(vec, NEAR_ZERO); }
 
  static bool nearZero(const Vector3D &vec, const num_t tol) {
#ifdef IMUNANO33_EMBED
    return fabs(vec.x) < tol && fabs(vec.y) < tol && fabs(vec.z) < tol;
#else
    return std::none_of(vec.begin(), vec.end(), [tol](const num_t &el) {
      return std::fabs(el) >= tol;
    });
#endif
  }
 
  static bool nearEq(const num_t num1, const num_t num2) {
    return nearEq(num1, num2, NEAR_ZERO);
  }
 
  static bool nearEq(const num_t num1, const num_t num2, const num_t tol) {
    return fabs(num1 - num2) < tol;
  }
 
  template <typename T> static T clamp(const T &num, const T &lo, const T &hi) {
    return num < lo ? lo : num > hi ? hi : num;
  }
 
private:
#ifdef IMUNANO33_EMBED
  static constexpr num_t NEAR_ZERO = FLT_EPSILON;
#else
  static constexpr num_t NEAR_ZERO = std::numeric_limits<num_t>::epsilon();
#endif
};
 
} // namespace imunano33
 
#endif