1 | /* |
2 | Copyright 2018 Google Inc. All Rights Reserved. |
3 | |
4 | Licensed under the Apache License, Version 2.0 (the "License"); |
5 | you may not use this file except in compliance with the License. |
6 | You may obtain a copy of the License at |
7 | |
8 | http://www.apache.org/licenses/LICENSE-2.0 |
9 | |
10 | Unless required by applicable law or agreed to in writing, software |
11 | distributed under the License is distributed on an "AS-IS" BASIS, |
12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
13 | See the License for the specific language governing permissions and |
14 | limitations under the License. |
15 | */ |
16 | |
17 | #ifndef RESONANCE_AUDIO_BASE_MISC_MATH_H_ |
18 | #define RESONANCE_AUDIO_BASE_MISC_MATH_H_ |
19 | |
20 | #ifndef _USE_MATH_DEFINES |
21 | #define _USE_MATH_DEFINES // Enable MSVC math constants (e.g., M_PI). |
22 | #endif // _USE_MATH_DEFINES |
23 | |
24 | #include <algorithm> |
25 | #include <cmath> |
26 | #include <cstdint> |
27 | #include <limits> |
28 | #include <utility> |
29 | #include <vector> |
30 | |
31 | #include "base/integral_types.h" |
32 | #include "Eigen/Dense" |
33 | #include "base/constants_and_types.h" |
34 | #include "base/logging.h" |
35 | |
36 | namespace vraudio { |
37 | |
38 | class WorldPosition : public Eigen::Matrix<float, 3, 1, Eigen::DontAlign> { |
39 | public: |
40 | // Inherits all constructors with 1-or-more arguments. Necessary because |
41 | // MSVC12 doesn't support inheriting constructors. |
42 | template <typename Arg1, typename... Args> |
43 | WorldPosition(const Arg1& arg1, Args&&... args) |
44 | : Matrix(arg1, std::forward<Args>(args)...) {} |
45 | |
46 | // Constructs a zero vector. |
47 | WorldPosition(); |
48 | |
49 | // Returns True if other |WorldPosition| differs by at least |kEpsilonFloat|. |
50 | bool operator!=(const WorldPosition& other) const { |
51 | return std::abs(x: this->x() - other.x()) > kEpsilonFloat || |
52 | std::abs(x: this->y() - other.y()) > kEpsilonFloat || |
53 | std::abs(x: this->z() - other.z()) > kEpsilonFloat; |
54 | } |
55 | }; |
56 | |
57 | class WorldRotation : public Eigen::Quaternion<float, Eigen::DontAlign> { |
58 | public: |
59 | // Inherits all constructors with 1-or-more arguments. Necessary because |
60 | // MSVC12 doesn't support inheriting constructors. |
61 | template <typename Arg1, typename... Args> |
62 | WorldRotation(const Arg1& arg1, Args&&... args) |
63 | : Quaternion(arg1, std::forward<Args>(args)...) {} |
64 | |
65 | // Constructs an identity rotation. |
66 | WorldRotation(); |
67 | |
68 | // Returns the shortest arc between two |WorldRotation|s in radians. |
69 | float AngularDifferenceRad(const WorldRotation& other) const { |
70 | const Quaternion difference = this->inverse() * other; |
71 | return static_cast<float>(Eigen::AngleAxisf(difference).angle()); |
72 | } |
73 | }; |
74 | |
75 | typedef Eigen::AngleAxis<float> AngleAxisf; |
76 | |
77 | typedef WorldPosition AudioPosition; |
78 | |
79 | typedef WorldRotation AudioRotation; |
80 | |
81 | // Converts |world_position| into an equivalent audio space position. |
82 | // The world space follows the typical CG coordinate system convention: |
83 | // Positive x points right, positive y points up, negative z points forward. |
84 | // The audio space follows the ambiX coordinate system convention that is |
85 | // commonly accepted in literature [http://goo.gl/XdYNm9]: |
86 | // Positive x points forward, negative y points right, positive z points up. |
87 | // Positions in both world space and audio space are in meters. |
88 | // |
89 | // @param world_position 3D position in world space. |
90 | // @param audio_position Output 3D position in audio space. |
91 | inline void ConvertAudioFromWorldPosition(const WorldPosition& world_position, |
92 | AudioPosition* audio_position) { |
93 | DCHECK(audio_position); |
94 | (*audio_position)(0) = -world_position[2]; |
95 | (*audio_position)(1) = -world_position[0]; |
96 | (*audio_position)(2) = world_position[1]; |
97 | } |
98 | |
99 | // Converts |audio_position| into an equivalent world space position. |
100 | // The world space follows the typical CG coordinate system convention: |
101 | // Positive x points right, positive y points up, negative z points forward. |
102 | // The audio space follows the ambiX coordinate system convention that is |
103 | // commonly accepted in literature [http://goo.gl/XdYNm9]: |
104 | // Positive x points forward, negative y points right, positive z points up. |
105 | // Positions in both world space and audio space are in meters. |
106 | // |
107 | // @param audio_position 3D position in audio space. |
108 | // @param world_position Output 3D position in world space. |
109 | inline void ConvertWorldFromAudioPosition(const AudioPosition& audio_position, |
110 | AudioPosition* world_position) { |
111 | DCHECK(world_position); |
112 | (*world_position)(0) = -audio_position[1]; |
113 | (*world_position)(1) = audio_position[2]; |
114 | (*world_position)(2) = -audio_position[0]; |
115 | } |
116 | |
117 | // Converts |world_rotation| into an equivalent audio space rotation. |
118 | // The world space follows the typical CG coordinate system convention: |
119 | // Positive x points right, positive y points up, negative z points forward. |
120 | // The audio space follows the ambiX coordinate system convention that is |
121 | // commonly accepted in literature [http://goo.gl/XdYNm9]: |
122 | // Positive x points forward, negative y points right, positive z points up. |
123 | // Positions in both world space and audio space are in meters. |
124 | // |
125 | // @param world_rotation 3D rotation in world space. |
126 | // @param audio_rotation Output 3D rotation in audio space. |
127 | inline void ConvertAudioFromWorldRotation(const WorldRotation& world_rotation, |
128 | AudioRotation* audio_rotation) { |
129 | DCHECK(audio_rotation); |
130 | audio_rotation->w() = world_rotation.w(); |
131 | audio_rotation->x() = -world_rotation.x(); |
132 | audio_rotation->y() = world_rotation.y(); |
133 | audio_rotation->z() = -world_rotation.z(); |
134 | } |
135 | |
136 | // Returns the relative direction vector |from_position| and |to_position| by |
137 | // rotating the relative position vector with respect to |from_rotation|. |
138 | // |
139 | // @param from_position Origin position of the direction. |
140 | // @param from_rotation Origin orientation of the direction. |
141 | // @param to_position Target position of the direction. |
142 | // @param relative_direction Relative direction vector (not normalized). |
143 | inline void GetRelativeDirection(const WorldPosition& from_position, |
144 | const WorldRotation& from_rotation, |
145 | const WorldPosition& to_position, |
146 | WorldPosition* relative_direction) { |
147 | DCHECK(relative_direction); |
148 | *relative_direction = |
149 | from_rotation.conjugate() * (to_position - from_position); |
150 | } |
151 | |
152 | // Returns the closest relative position in an axis-aligned bounding box to the |
153 | // given |relative_position|. |
154 | // |
155 | // @param position Input position relative to the center of the bounding box. |
156 | // @param aabb_dimensions Bounding box dimensions. |
157 | // @return aabb bounded position. |
158 | inline void GetClosestPositionInAabb(const WorldPosition& relative_position, |
159 | const WorldPosition& aabb_dimensions, |
160 | WorldPosition* closest_position) { |
161 | DCHECK(closest_position); |
162 | const WorldPosition aabb_offset = 0.5f * aabb_dimensions; |
163 | (*closest_position)[0] = |
164 | std::min(a: std::max(a: relative_position[0], b: -aabb_offset[0]), b: aabb_offset[0]); |
165 | (*closest_position)[1] = |
166 | std::min(a: std::max(a: relative_position[1], b: -aabb_offset[1]), b: aabb_offset[1]); |
167 | (*closest_position)[2] = |
168 | std::min(a: std::max(a: relative_position[2], b: -aabb_offset[2]), b: aabb_offset[2]); |
169 | } |
170 | |
171 | // Returns true if given world |position| is in given axis-aligned bounding box. |
172 | // |
173 | // @param position Position to be tested. |
174 | // @param aabb_center Bounding box center. |
175 | // @param aabb_dimensions Bounding box dimensions. |
176 | // @return True if |position| is within bounding box, false otherwise. |
177 | inline bool IsPositionInAabb(const WorldPosition& position, |
178 | const WorldPosition& aabb_center, |
179 | const WorldPosition& aabb_dimensions) { |
180 | return std::abs(x: position[0] - aabb_center[0]) <= 0.5f * aabb_dimensions[0] && |
181 | std::abs(x: position[1] - aabb_center[1]) <= 0.5f * aabb_dimensions[1] && |
182 | std::abs(x: position[2] - aabb_center[2]) <= 0.5f * aabb_dimensions[2]; |
183 | } |
184 | |
185 | // Returns true if an integer overflow occurred during the calculation of |
186 | // x = a * b. |
187 | // |
188 | // @param a First multiplicand. |
189 | // @param b Second multiplicand. |
190 | // @param x Product. |
191 | // @return True if integer overflow occurred, false otherwise. |
192 | template <typename T> |
193 | inline bool DoesIntegerMultiplicationOverflow(T a, T b, T x) { |
194 | // Detects an integer overflow occurs by inverting the multiplication and |
195 | // testing for x / a != b. |
196 | return a == 0 ? false : (x / a != b); |
197 | } |
198 | |
199 | // Returns true if an integer overflow occurred during the calculation of |
200 | // a + b. |
201 | // |
202 | // @param a First summand. |
203 | // @param b Second summand. |
204 | // @return True if integer overflow occurred, false otherwise. |
205 | template <typename T> |
206 | inline bool DoesIntegerAdditionOverflow(T a, T b) { |
207 | T x = a + b; |
208 | return x < b; |
209 | } |
210 | |
211 | // Safely converts an int to a size_t. |
212 | // |
213 | // @param i Integer input. |
214 | // @param x Size_t output. |
215 | // @return True if integer overflow occurred, false otherwise. |
216 | inline bool DoesIntSafelyConvertToSizeT(int i, size_t* x) { |
217 | if (i < 0) { |
218 | return false; |
219 | } |
220 | *x = static_cast<size_t>(i); |
221 | return true; |
222 | } |
223 | |
224 | // Safely converts a size_t to an int. |
225 | // |
226 | // @param i Size_t input. |
227 | // @param x Integer output. |
228 | // @return True if integer overflow occurred, false otherwise. |
229 | inline bool DoesSizeTSafelyConvertToInt(size_t i, int* x) { |
230 | if (i > static_cast<size_t>(std::numeric_limits<int>::max())) { |
231 | return false; |
232 | } |
233 | *x = static_cast<int>(i); |
234 | return true; |
235 | } |
236 | |
237 | // Finds the greatest common divisor between two integer values using the |
238 | // Euclidean algorithm. Always returns a positive integer. |
239 | // |
240 | // @param a First of the two integer values. |
241 | // @param b second of the two integer values. |
242 | // @return The greatest common divisor of the two integer values. |
243 | inline int FindGcd(int a, int b) { |
244 | a = std::abs(x: a); |
245 | b = std::abs(x: b); |
246 | int temp_value = 0; |
247 | while (b != 0) { |
248 | temp_value = b; |
249 | b = a % b; |
250 | a = temp_value; |
251 | } |
252 | return a; |
253 | } |
254 | |
255 | // Finds the next power of two from an integer. This method works with values |
256 | // representable by unsigned 32 bit integers. |
257 | // |
258 | // @param input Integer value. |
259 | // @return The next power of two from |input|. |
260 | inline size_t NextPowTwo(size_t input) { |
261 | // Ensure the value fits in a uint32_t. |
262 | DCHECK_LT(static_cast<uint64_t>(input), |
263 | static_cast<uint64_t>(std::numeric_limits<uint32_t>::max())); |
264 | uint32_t number = static_cast<uint32_t>(--input); |
265 | number |= number >> 1; // Take care of 2 bit numbers. |
266 | number |= number >> 2; // Take care of 4 bit numbers. |
267 | number |= number >> 4; // Take care of 8 bit numbers. |
268 | number |= number >> 8; // Take care of 16 bit numbers. |
269 | number |= number >> 16; // Take care of 32 bit numbers. |
270 | number++; |
271 | return static_cast<size_t>(number); |
272 | } |
273 | |
274 | // Returns the factorial (!) of x. If x < 0, it returns 0. |
275 | inline float Factorial(int x) { |
276 | if (x < 0) return 0.0f; |
277 | float result = 1.0f; |
278 | for (; x > 0; --x) result *= static_cast<float>(x); |
279 | return result; |
280 | } |
281 | |
282 | // Returns the double factorial (!!) of x. |
283 | // For odd x: 1 * 3 * 5 * ... * (x - 2) * x |
284 | // For even x: 2 * 4 * 6 * ... * (x - 2) * x |
285 | // If x < 0, it returns 0. |
286 | inline float DoubleFactorial(int x) { |
287 | if (x < 0) return 0.0f; |
288 | float result = 1.0f; |
289 | for (; x > 0; x -= 2) result *= static_cast<float>(x); |
290 | return result; |
291 | } |
292 | |
293 | // This is a *safe* alternative to std::equal function as a workaround in order |
294 | // to avoid MSVC compiler warning C4996 for unchecked iterators (see |
295 | // https://msdn.microsoft.com/en-us/library/aa985965.aspx). |
296 | // Also note that, an STL equivalent of this function was introduced in C++14 to |
297 | // be replaced with this implementation (see version (5) in |
298 | // http://en.cppreference.com/w/cpp/algorithm/equal). |
299 | template <typename Iterator> |
300 | inline bool EqualSafe(const Iterator& lhs_begin, const Iterator& lhs_end, |
301 | const Iterator& rhs_begin, const Iterator& rhs_end) { |
302 | auto lhs_itr = lhs_begin; |
303 | auto rhs_itr = rhs_begin; |
304 | while (lhs_itr != lhs_end && rhs_itr != rhs_end) { |
305 | if (*lhs_itr != *rhs_itr) { |
306 | return false; |
307 | } |
308 | ++lhs_itr; |
309 | ++rhs_itr; |
310 | } |
311 | return lhs_itr == lhs_end && rhs_itr == rhs_end; |
312 | } |
313 | |
314 | // Fast reciprocal of square-root. See: https://goo.gl/fqvstz for details. |
315 | // |
316 | // @param input The number to be inverse rooted. |
317 | // @return An approximation of the reciprocal square root of |input|. |
318 | inline float FastReciprocalSqrt(float input) { |
319 | const float kThreeHalfs = 1.5f; |
320 | const uint32_t kMagicNumber = 0x5f3759df; |
321 | |
322 | // Approximate a logarithm by aliasing to an integer. |
323 | uint32_t integer; |
324 | memcpy(dest: &integer, src: &input, n: sizeof(float)); |
325 | integer = kMagicNumber - (integer >> 1); |
326 | float approximation; |
327 | memcpy(dest: &approximation, src: &integer, n: sizeof(float)); |
328 | const float half_input = input * 0.5f; |
329 | // One iteration of Newton's method. |
330 | return approximation * |
331 | (kThreeHalfs - (half_input * approximation * approximation)); |
332 | } |
333 | |
334 | // Finds the best-fitting line to a given set of 2D points by minimizing the |
335 | // sum of the squares of the vertical (along y-axis) offsets. The slope and |
336 | // intercept of the fitted line are recorded, as well as the coefficient of |
337 | // determination, which gives the quality of the fitting. |
338 | // See http://mathworld.wolfram.com/LeastSquaresFitting.html for how to compute |
339 | // these values. |
340 | // |
341 | // @param x_array Array of the x coordinates of the points. |
342 | // @param y_array Array of the y coordinates of the points. |
343 | // @param slope Output slope of the fitted line. |
344 | // @param intercept Output slope of the fitted line. |
345 | // @param r_squared Coefficient of determination. |
346 | // @return False if the fitting fails. |
347 | bool LinearLeastSquareFitting(const std::vector<float>& x_array, |
348 | const std::vector<float>& y_array, float* slope, |
349 | float* intercept, float* r_squared); |
350 | |
351 | // Computes |base|^|exp|, where |exp| is a *non-negative* integer, with the |
352 | // squared exponentiation (a.k.a double-and-add) method. |
353 | // When T is a floating point type, this has the same semantics as pow(), but |
354 | // is much faster. |
355 | // T can also be any integral type, in which case computations will be |
356 | // performed in the value domain of this integral type, and overflow semantics |
357 | // will be those of T. |
358 | // You can also use any type for which operator*= is defined. |
359 | // See : |
360 | |
361 | // This method is reproduced here so vraudio classes don't need to depend on |
362 | // //util/math/mathutil.h |
363 | // |
364 | // @tparam base Input to the exponent function. Any type for which *= is |
365 | // defined. |
366 | // @param exp Integer exponent, must be greater than or equal to zero. |
367 | // @return |base|^|exp|. |
368 | template <typename T> |
369 | static inline T IntegerPow(T base, int exp) { |
370 | DCHECK_GE(exp, 0); |
371 | T result = static_cast<T>(1); |
372 | while (true) { |
373 | if (exp & 1) { |
374 | result *= base; |
375 | } |
376 | exp >>= 1; |
377 | if (!exp) break; |
378 | base *= base; |
379 | } |
380 | return result; |
381 | } |
382 | |
383 | } // namespace vraudio |
384 | |
385 | #endif // RESONANCE_AUDIO_BASE_MISC_MATH_H_ |
386 | |