1e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#ifndef ANDROID_DVR_NUMERIC_H_ 2e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#define ANDROID_DVR_NUMERIC_H_ 3e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 4e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#include <cmath> 5e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#include <limits> 6e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#include <random> 7e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#include <type_traits> 8e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 9e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#include <private/dvr/eigen.h> 10e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#include <private/dvr/types.h> 11e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 12e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkonamespace android { 13e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkonamespace dvr { 14e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 15e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename FT> 16e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkostatic inline FT ToDeg(FT f) { 17e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return f * static_cast<FT>(180.0 / M_PI); 18e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 19e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 20e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename FT> 21e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkostatic inline FT ToRad(FT f) { 22e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return f * static_cast<FT>(M_PI / 180.0); 23e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 24e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 25e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// Adjusts `x` to the periodic range `[lo, hi]` (to normalize angle values 26e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// for example). 27e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 28e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT NormalizePeriodicRange(T x, T lo, T hi) { 29e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko T range_size = hi - lo; 30e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 31e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko while (x < lo) { 32e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko x += range_size; 33e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 34e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 35e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko while (x > hi) { 36e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko x -= range_size; 37e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 38e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 39e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return x; 40e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 41e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 42e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// Normalizes a measurement in radians. 43e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// @param x the angle to be normalized 44e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// @param centre the point around which to normalize the range 45e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// @return the value of x, normalized to the range [centre - 180, centre + 180] 46e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 47e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT NormalizeDegrees(T x, T centre = static_cast<T>(180.0)) { 48e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return NormalizePeriodicRange(x, centre - static_cast<T>(180.0), 49e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko centre + static_cast<T>(180.0)); 50e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 51e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 52e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// Normalizes a measurement in radians. 53e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// @param x the angle to be normalized 54e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// @param centre the point around which to normalize the range 55e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// @return the value of x, normalized to the range 56e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// [centre - M_PI, centre + M_PI] 57e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// @remark the centre parameter is to make it possible to specify a different 58e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// periodic range. This is useful if you are planning on comparing two 59e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// angles close to 0 or M_PI, so that one might not accidentally end 60e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// up on the other side of the range 61e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 62e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT NormalizeRadians(T x, T centre = static_cast<T>(M_PI)) { 63e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return NormalizePeriodicRange(x, centre - static_cast<T>(M_PI), 64e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko centre + static_cast<T>(M_PI)); 65e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 66e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 67e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkostatic inline vec2i Round(const vec2& v) { 68e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return vec2i(roundf(v.x()), roundf(v.y())); 69e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 70e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 71e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkostatic inline vec2i Scale(const vec2i& v, float scale) { 72e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return vec2i(roundf(static_cast<float>(v.x()) * scale), 73e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko roundf(static_cast<float>(v.y()) * scale)); 74e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 75e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 76e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko// Re-maps `x` from `[lba,uba]` to `[lbb,ubb]`. 77e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 78e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT ConvertRange(T x, T lba, T uba, T lbb, T ubb) { 79e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return (((x - lba) * (ubb - lbb)) / (uba - lba)) + lbb; 80e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 81e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 82e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename R1, typename R2> 83e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkostatic inline vec2 MapPoint(const vec2& pt, const R1& from, const R2& to) { 84e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko vec2 normalized((pt - vec2(from.p1)).array() / vec2(from.GetSize()).array()); 85e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return (normalized * vec2(to.GetSize())) + vec2(to.p1); 86e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 87e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 88e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 89e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkoinline bool IsZero(const T& v, 90e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko const T& tol = std::numeric_limits<T>::epsilon()) { 91e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return std::abs(v) <= tol; 92e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 93e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 94e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 95e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkoinline bool IsEqual(const T& a, const T& b, 96e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko const T& tol = std::numeric_limits<T>::epsilon()) { 97e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return std::abs(b - a) <= tol; 98e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 99e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 100e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 101e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT Square(const T& x) { 102e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return x * x; 103e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 104e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 105e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 106e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT RandomInRange(T lo, T hi, 107e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko typename 108e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko std::enable_if<std::is_floating_point<T>::value>::type* = 0) { 109e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko std::random_device rd; 110e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko std::mt19937 gen(rd()); 111e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko std::uniform_real_distribution<T> distro(lo, hi); 112e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return distro(gen); 113e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 114e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 115e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 116e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT RandomInRange(T lo, T hi, 117e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko typename 118e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko std::enable_if<std::is_integral<T>::value>::type* = 0) { 119e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko std::random_device rd; 120e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko std::mt19937 gen(rd()); 121e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko std::uniform_int_distribution<T> distro(lo, hi); 122e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return distro(gen); 123e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 124e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 125e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename Derived1, typename Derived2> 126e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoDerived1 RandomInRange( 127e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko const Eigen::MatrixBase<Derived1>& lo, 128e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko const Eigen::MatrixBase<Derived2>& hi) { 1297245fb74d86dce3c07c5341020d25d24065d1c0dMiao Wang EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Derived1, Derived2); 130e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 1317245fb74d86dce3c07c5341020d25d24065d1c0dMiao Wang Derived1 result = Eigen::MatrixBase<Derived1>::Zero(); 132e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 133e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko for (int row = 0; row < result.rows(); ++row) { 134e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko for (int col = 0; col < result.cols(); ++col) { 135e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko result(row, col) = RandomInRange(lo(row, col), hi(row, col)); 136e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 137e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko } 138e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 139e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return result; 140e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 141e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 142e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 143e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT RandomRange(T x) { 144e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return RandomInRange(-x, x); 145e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 146e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 147e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkotemplate <typename T> 148e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex VakulenkoT Clamp(T x, T lo, T hi) { 149e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return std::min(std::max(x, lo), hi); 150e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 151e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 152e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkoinline mat3 ScaleMatrix(const vec2& scale_xy) { 153e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return mat3(Eigen::Scaling(scale_xy[0], scale_xy[1], 1.0f)); 154e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 155e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 156e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkoinline mat3 TranslationMatrix(const vec2& translation) { 157e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return mat3(Eigen::Translation2f(translation)); 158e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 159e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 160e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkoinline mat4 TranslationMatrix(const vec3& translation) { 161e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return mat4(Eigen::Translation3f(translation)); 162e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 163e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 164e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkoinline vec2 TransformPoint(const mat3& m, const vec2& p) { 165e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return m.linear() * p + m.translation(); 166e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 167e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 168e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenkoinline vec2 TransformVector(const mat3& m, const vec2& p) { 169e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko return m.linear() * p; 170e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} 171e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 172e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} // namespace dvr 173e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko} // namespace android 174e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko 175e4eec20f6263f4a42ae462456f60ea6c4518bb0aAlex Vakulenko#endif // ANDROID_DVR_NUMERIC_H_ 176