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