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