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>
ToDeg(FT f)16 static inline FT ToDeg(FT f) {
17 return f * static_cast<FT>(180.0 / M_PI);
18 }
19
20 template <typename FT>
ToRad(FT f)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>
NormalizePeriodicRange(T x,T lo,T hi)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
Round(const vec2 & v)67 static inline vec2i Round(const vec2& v) {
68 return vec2i(roundf(v.x()), roundf(v.y()));
69 }
70
Scale(const vec2i & v,float scale)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>
ConvertRange(T x,T lba,T uba,T lbb,T ubb)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>
MapPoint(const vec2 & pt,const R1 & from,const R2 & to)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>
Square(const T & x)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>
RandomInRange(const Eigen::MatrixBase<Derived1> & lo,const Eigen::MatrixBase<Derived2> & hi)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>
RandomRange(T x)143 T RandomRange(T x) {
144 return RandomInRange(-x, x);
145 }
146
147 template <typename T>
Clamp(T x,T lo,T hi)148 T Clamp(T x, T lo, T hi) {
149 return std::min(std::max(x, lo), hi);
150 }
151
ScaleMatrix(const vec2 & scale_xy)152 inline mat3 ScaleMatrix(const vec2& scale_xy) {
153 return mat3(Eigen::Scaling(scale_xy[0], scale_xy[1], 1.0f));
154 }
155
TranslationMatrix(const vec2 & translation)156 inline mat3 TranslationMatrix(const vec2& translation) {
157 return mat3(Eigen::Translation2f(translation));
158 }
159
TranslationMatrix(const vec3 & translation)160 inline mat4 TranslationMatrix(const vec3& translation) {
161 return mat4(Eigen::Translation3f(translation));
162 }
163
TransformPoint(const mat3 & m,const vec2 & p)164 inline vec2 TransformPoint(const mat3& m, const vec2& p) {
165 return m.linear() * p + m.translation();
166 }
167
TransformVector(const mat3 & m,const vec2 & p)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