blob: 9daca0827da181dc00cb9fe7285a6a70c9e7aa47 [file] [log] [blame]
Mathias Agopian595ea772013-08-21 23:10:41 -07001/*
2 * Copyright 2013 The Android Open Source Project
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
Romain Guy5d4bae72016-11-08 09:49:25 -080017#ifndef UI_TMATHELPERS_H_
18#define UI_TMATHELPERS_H_
Mathias Agopian595ea772013-08-21 23:10:41 -070019
Romain Guy5d4bae72016-11-08 09:49:25 -080020#include <math.h>
Mathias Agopian595ea772013-08-21 23:10:41 -070021#include <stdint.h>
22#include <sys/types.h>
Romain Guy5d4bae72016-11-08 09:49:25 -080023
24#include <cmath>
25#include <exception>
26#include <iomanip>
27#include <stdexcept>
28
29#include <ui/quat.h>
30#include <ui/TVecHelpers.h>
31
32#include <utils/String8.h>
33
34#ifdef __cplusplus
35# define LIKELY( exp ) (__builtin_expect( !!(exp), true ))
36# define UNLIKELY( exp ) (__builtin_expect( !!(exp), false ))
37#else
38# define LIKELY( exp ) (__builtin_expect( !!(exp), 1 ))
39# define UNLIKELY( exp ) (__builtin_expect( !!(exp), 0 ))
40#endif
Mathias Agopian595ea772013-08-21 23:10:41 -070041
42#define PURE __attribute__((pure))
43
44namespace android {
Romain Guy5d4bae72016-11-08 09:49:25 -080045namespace details {
Mathias Agopian595ea772013-08-21 23:10:41 -070046// -------------------------------------------------------------------------------------
47
48/*
49 * No user serviceable parts here.
50 *
51 * Don't use this file directly, instead include ui/mat*.h
52 */
53
54
55/*
56 * Matrix utilities
57 */
58
59namespace matrix {
60
Romain Guy5d4bae72016-11-08 09:49:25 -080061inline constexpr int transpose(int v) { return v; }
62inline constexpr float transpose(float v) { return v; }
63inline constexpr double transpose(double v) { return v; }
Mathias Agopian595ea772013-08-21 23:10:41 -070064
Romain Guy5d4bae72016-11-08 09:49:25 -080065inline constexpr int trace(int v) { return v; }
66inline constexpr float trace(float v) { return v; }
67inline constexpr double trace(double v) { return v; }
Mathias Agopian595ea772013-08-21 23:10:41 -070068
Romain Guy5d4bae72016-11-08 09:49:25 -080069/*
70 * Matrix inversion
71 */
Mathias Agopian595ea772013-08-21 23:10:41 -070072template<typename MATRIX>
Romain Guy5d4bae72016-11-08 09:49:25 -080073MATRIX PURE gaussJordanInverse(const MATRIX& src) {
74 typedef typename MATRIX::value_type T;
75 static constexpr unsigned int N = MATRIX::NUM_ROWS;
Mathias Agopian595ea772013-08-21 23:10:41 -070076 MATRIX tmp(src);
Romain Guy5d4bae72016-11-08 09:49:25 -080077 MATRIX inverted(1);
Mathias Agopian595ea772013-08-21 23:10:41 -070078
Romain Guy5d4bae72016-11-08 09:49:25 -080079 for (size_t i = 0; i < N; ++i) {
80 // look for largest element in i'th column
81 size_t swap = i;
82 T t = std::abs(tmp[i][i]);
83 for (size_t j = i + 1; j < N; ++j) {
84 const T t2 = std::abs(tmp[j][i]);
85 if (t2 > t) {
Mathias Agopian595ea772013-08-21 23:10:41 -070086 swap = j;
Romain Guy5d4bae72016-11-08 09:49:25 -080087 t = t2;
Mathias Agopian595ea772013-08-21 23:10:41 -070088 }
89 }
90
91 if (swap != i) {
Romain Guy5d4bae72016-11-08 09:49:25 -080092 // swap columns.
93 std::swap(tmp[i], tmp[swap]);
94 std::swap(inverted[i], inverted[swap]);
Mathias Agopian595ea772013-08-21 23:10:41 -070095 }
96
Romain Guy5d4bae72016-11-08 09:49:25 -080097 const T denom(tmp[i][i]);
98 for (size_t k = 0; k < N; ++k) {
99 tmp[i][k] /= denom;
100 inverted[i][k] /= denom;
Mathias Agopian595ea772013-08-21 23:10:41 -0700101 }
Romain Guy5d4bae72016-11-08 09:49:25 -0800102
103 // Factor out the lower triangle
104 for (size_t j = 0; j < N; ++j) {
Mathias Agopian595ea772013-08-21 23:10:41 -0700105 if (j != i) {
Romain Guy5d4bae72016-11-08 09:49:25 -0800106 const T t = tmp[j][i];
107 for (size_t k = 0; k < N; ++k) {
Mathias Agopian595ea772013-08-21 23:10:41 -0700108 tmp[j][k] -= tmp[i][k] * t;
Romain Guy5d4bae72016-11-08 09:49:25 -0800109 inverted[j][k] -= inverted[i][k] * t;
Mathias Agopian595ea772013-08-21 23:10:41 -0700110 }
111 }
112 }
113 }
Romain Guy5d4bae72016-11-08 09:49:25 -0800114
115 return inverted;
116}
117
118
119//------------------------------------------------------------------------------
120// 2x2 matrix inverse is easy.
121template <typename MATRIX>
122MATRIX PURE fastInverse2(const MATRIX& x) {
123 typedef typename MATRIX::value_type T;
124
125 // Assuming the input matrix is:
126 // | a b |
127 // | c d |
128 //
129 // The analytic inverse is
130 // | d -b |
131 // | -c a | / (a d - b c)
132 //
133 // Importantly, our matrices are column-major!
134
135 MATRIX inverted(MATRIX::NO_INIT);
136
137 const T a = x[0][0];
138 const T c = x[0][1];
139 const T b = x[1][0];
140 const T d = x[1][1];
141
142 const T det((a * d) - (b * c));
143 inverted[0][0] = d / det;
144 inverted[0][1] = -c / det;
145 inverted[1][0] = -b / det;
146 inverted[1][1] = a / det;
147 return inverted;
148}
149
150
151//------------------------------------------------------------------------------
152// From the Wikipedia article on matrix inversion's section on fast 3x3
153// matrix inversion:
154// http://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3.C3.973_matrices
155template <typename MATRIX>
156MATRIX PURE fastInverse3(const MATRIX& x) {
157 typedef typename MATRIX::value_type T;
158
159 // Assuming the input matrix is:
160 // | a b c |
161 // | d e f |
162 // | g h i |
163 //
164 // The analytic inverse is
165 // | A B C |^T
166 // | D E F |
167 // | G H I | / determinant
168 //
169 // Which is
170 // | A D G |
171 // | B E H |
172 // | C F I | / determinant
173 //
174 // Where:
175 // A = (ei - fh), B = (fg - di), C = (dh - eg)
176 // D = (ch - bi), E = (ai - cg), F = (bg - ah)
177 // G = (bf - ce), H = (cd - af), I = (ae - bd)
178 //
179 // and the determinant is a*A + b*B + c*C (The rule of Sarrus)
180 //
181 // Importantly, our matrices are column-major!
182
183 MATRIX inverted(MATRIX::NO_INIT);
184
185 const T a = x[0][0];
186 const T b = x[1][0];
187 const T c = x[2][0];
188 const T d = x[0][1];
189 const T e = x[1][1];
190 const T f = x[2][1];
191 const T g = x[0][2];
192 const T h = x[1][2];
193 const T i = x[2][2];
194
195 // Do the full analytic inverse
196 const T A = e * i - f * h;
197 const T B = f * g - d * i;
198 const T C = d * h - e * g;
199 inverted[0][0] = A; // A
200 inverted[0][1] = B; // B
201 inverted[0][2] = C; // C
202 inverted[1][0] = c * h - b * i; // D
203 inverted[1][1] = a * i - c * g; // E
204 inverted[1][2] = b * g - a * h; // F
205 inverted[2][0] = b * f - c * e; // G
206 inverted[2][1] = c * d - a * f; // H
207 inverted[2][2] = a * e - b * d; // I
208
209 const T det(a * A + b * B + c * C);
210 for (size_t col = 0; col < 3; ++col) {
211 for (size_t row = 0; row < 3; ++row) {
212 inverted[col][row] /= det;
213 }
214 }
215
216 return inverted;
217}
218
219
220/**
221 * Inversion function which switches on the matrix size.
222 * @warning This function assumes the matrix is invertible. The result is
223 * undefined if it is not. It is the responsibility of the caller to
224 * make sure the matrix is not singular.
225 */
226template <typename MATRIX>
227inline constexpr MATRIX PURE inverse(const MATRIX& matrix) {
228 static_assert(MATRIX::NUM_ROWS == MATRIX::NUM_COLS, "only square matrices can be inverted");
229 return (MATRIX::NUM_ROWS == 2) ? fastInverse2<MATRIX>(matrix) :
230 ((MATRIX::NUM_ROWS == 3) ? fastInverse3<MATRIX>(matrix) :
231 gaussJordanInverse<MATRIX>(matrix));
Mathias Agopian595ea772013-08-21 23:10:41 -0700232}
233
234template<typename MATRIX_R, typename MATRIX_A, typename MATRIX_B>
235MATRIX_R PURE multiply(const MATRIX_A& lhs, const MATRIX_B& rhs) {
236 // pre-requisite:
237 // lhs : D columns, R rows
238 // rhs : C columns, D rows
239 // res : C columns, R rows
240
Romain Guy5d4bae72016-11-08 09:49:25 -0800241 static_assert(MATRIX_A::NUM_COLS == MATRIX_B::NUM_ROWS,
242 "matrices can't be multiplied. invalid dimensions.");
243 static_assert(MATRIX_R::NUM_COLS == MATRIX_B::NUM_COLS,
244 "invalid dimension of matrix multiply result.");
245 static_assert(MATRIX_R::NUM_ROWS == MATRIX_A::NUM_ROWS,
246 "invalid dimension of matrix multiply result.");
Mathias Agopian595ea772013-08-21 23:10:41 -0700247
248 MATRIX_R res(MATRIX_R::NO_INIT);
Romain Guy5d4bae72016-11-08 09:49:25 -0800249 for (size_t col = 0; col < MATRIX_R::NUM_COLS; ++col) {
250 res[col] = lhs * rhs[col];
Mathias Agopian595ea772013-08-21 23:10:41 -0700251 }
252 return res;
253}
254
255// transpose. this handles matrices of matrices
256template <typename MATRIX>
257MATRIX PURE transpose(const MATRIX& m) {
258 // for now we only handle square matrix transpose
Romain Guy5d4bae72016-11-08 09:49:25 -0800259 static_assert(MATRIX::NUM_COLS == MATRIX::NUM_ROWS, "transpose only supports square matrices");
Mathias Agopian595ea772013-08-21 23:10:41 -0700260 MATRIX result(MATRIX::NO_INIT);
Romain Guy5d4bae72016-11-08 09:49:25 -0800261 for (size_t col = 0; col < MATRIX::NUM_COLS; ++col) {
262 for (size_t row = 0; row < MATRIX::NUM_ROWS; ++row) {
263 result[col][row] = transpose(m[row][col]);
264 }
265 }
Mathias Agopian595ea772013-08-21 23:10:41 -0700266 return result;
267}
268
269// trace. this handles matrices of matrices
270template <typename MATRIX>
271typename MATRIX::value_type PURE trace(const MATRIX& m) {
Romain Guy5d4bae72016-11-08 09:49:25 -0800272 static_assert(MATRIX::NUM_COLS == MATRIX::NUM_ROWS, "trace only defined for square matrices");
Mathias Agopian595ea772013-08-21 23:10:41 -0700273 typename MATRIX::value_type result(0);
Romain Guy5d4bae72016-11-08 09:49:25 -0800274 for (size_t col = 0; col < MATRIX::NUM_COLS; ++col) {
275 result += trace(m[col][col]);
276 }
Mathias Agopian595ea772013-08-21 23:10:41 -0700277 return result;
278}
279
Romain Guy5d4bae72016-11-08 09:49:25 -0800280// diag. this handles matrices of matrices
Mathias Agopian595ea772013-08-21 23:10:41 -0700281template <typename MATRIX>
282typename MATRIX::col_type PURE diag(const MATRIX& m) {
Romain Guy5d4bae72016-11-08 09:49:25 -0800283 static_assert(MATRIX::NUM_COLS == MATRIX::NUM_ROWS, "diag only defined for square matrices");
Mathias Agopian595ea772013-08-21 23:10:41 -0700284 typename MATRIX::col_type result(MATRIX::col_type::NO_INIT);
Romain Guy5d4bae72016-11-08 09:49:25 -0800285 for (size_t col = 0; col < MATRIX::NUM_COLS; ++col) {
286 result[col] = m[col][col];
287 }
Mathias Agopian595ea772013-08-21 23:10:41 -0700288 return result;
289}
290
Romain Guy5d4bae72016-11-08 09:49:25 -0800291//------------------------------------------------------------------------------
292// This is taken from the Imath MatrixAlgo code, and is identical to Eigen.
293template <typename MATRIX>
294TQuaternion<typename MATRIX::value_type> extractQuat(const MATRIX& mat) {
295 typedef typename MATRIX::value_type T;
296
297 TQuaternion<T> quat(TQuaternion<T>::NO_INIT);
298
299 // Compute the trace to see if it is positive or not.
300 const T trace = mat[0][0] + mat[1][1] + mat[2][2];
301
302 // check the sign of the trace
303 if (LIKELY(trace > 0)) {
304 // trace is positive
305 T s = std::sqrt(trace + 1);
306 quat.w = T(0.5) * s;
307 s = T(0.5) / s;
308 quat.x = (mat[1][2] - mat[2][1]) * s;
309 quat.y = (mat[2][0] - mat[0][2]) * s;
310 quat.z = (mat[0][1] - mat[1][0]) * s;
311 } else {
312 // trace is negative
313
314 // Find the index of the greatest diagonal
315 size_t i = 0;
316 if (mat[1][1] > mat[0][0]) { i = 1; }
317 if (mat[2][2] > mat[i][i]) { i = 2; }
318
319 // Get the next indices: (n+1)%3
320 static constexpr size_t next_ijk[3] = { 1, 2, 0 };
321 size_t j = next_ijk[i];
322 size_t k = next_ijk[j];
323 T s = std::sqrt((mat[i][i] - (mat[j][j] + mat[k][k])) + 1);
324 quat[i] = T(0.5) * s;
325 if (s != 0) {
326 s = T(0.5) / s;
327 }
328 quat.w = (mat[j][k] - mat[k][j]) * s;
329 quat[j] = (mat[i][j] + mat[j][i]) * s;
330 quat[k] = (mat[i][k] + mat[k][i]) * s;
331 }
332 return quat;
333}
334
Mathias Agopian595ea772013-08-21 23:10:41 -0700335template <typename MATRIX>
336String8 asString(const MATRIX& m) {
337 String8 s;
Romain Guy5d4bae72016-11-08 09:49:25 -0800338 for (size_t c = 0; c < MATRIX::col_size(); c++) {
Mathias Agopian595ea772013-08-21 23:10:41 -0700339 s.append("| ");
Romain Guy5d4bae72016-11-08 09:49:25 -0800340 for (size_t r = 0; r < MATRIX::row_size(); r++) {
Mathias Agopian595ea772013-08-21 23:10:41 -0700341 s.appendFormat("%7.2f ", m[r][c]);
342 }
343 s.append("|\n");
344 }
345 return s;
346}
347
Romain Guy5d4bae72016-11-08 09:49:25 -0800348} // namespace matrix
Mathias Agopian595ea772013-08-21 23:10:41 -0700349
350// -------------------------------------------------------------------------------------
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700351
352/*
353 * TMatProductOperators implements basic arithmetic and basic compound assignments
354 * operators on a vector of type BASE<T>.
355 *
356 * BASE only needs to implement operator[] and size().
357 * By simply inheriting from TMatProductOperators<BASE, T> BASE will automatically
358 * get all the functionality here.
359 */
360
361template <template<typename T> class BASE, typename T>
362class TMatProductOperators {
363public:
364 // multiply by a scalar
365 BASE<T>& operator *= (T v) {
366 BASE<T>& lhs(static_cast< BASE<T>& >(*this));
Romain Guy5d4bae72016-11-08 09:49:25 -0800367 for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
368 lhs[col] *= v;
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700369 }
370 return lhs;
371 }
372
Romain Guy5d4bae72016-11-08 09:49:25 -0800373 // matrix *= matrix
374 template<typename U>
375 const BASE<T>& operator *= (const BASE<U>& rhs) {
376 BASE<T>& lhs(static_cast< BASE<T>& >(*this));
377 lhs = matrix::multiply<BASE<T> >(lhs, rhs);
378 return lhs;
379 }
380
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700381 // divide by a scalar
382 BASE<T>& operator /= (T v) {
383 BASE<T>& lhs(static_cast< BASE<T>& >(*this));
Romain Guy5d4bae72016-11-08 09:49:25 -0800384 for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
385 lhs[col] /= v;
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700386 }
387 return lhs;
388 }
389
390 // matrix * matrix, result is a matrix of the same type than the lhs matrix
391 template<typename U>
392 friend BASE<T> PURE operator *(const BASE<T>& lhs, const BASE<U>& rhs) {
393 return matrix::multiply<BASE<T> >(lhs, rhs);
394 }
395};
396
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700397/*
398 * TMatSquareFunctions implements functions on a matrix of type BASE<T>.
399 *
400 * BASE only needs to implement:
401 * - operator[]
402 * - col_type
403 * - row_type
404 * - COL_SIZE
405 * - ROW_SIZE
406 *
407 * By simply inheriting from TMatSquareFunctions<BASE, T> BASE will automatically
408 * get all the functionality here.
409 */
410
411template<template<typename U> class BASE, typename T>
412class TMatSquareFunctions {
413public:
Romain Guy5d4bae72016-11-08 09:49:25 -0800414
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700415 /*
416 * NOTE: the functions below ARE NOT member methods. They are friend functions
417 * with they definition inlined with their declaration. This makes these
418 * template functions available to the compiler when (and only when) this class
419 * is instantiated, at which point they're only templated on the 2nd parameter
420 * (the first one, BASE<T> being known).
421 */
Romain Guy5d4bae72016-11-08 09:49:25 -0800422 friend inline BASE<T> PURE inverse(const BASE<T>& matrix) {
423 return matrix::inverse(matrix);
424 }
425 friend inline constexpr BASE<T> PURE transpose(const BASE<T>& m) {
426 return matrix::transpose(m);
427 }
428 friend inline constexpr T PURE trace(const BASE<T>& m) {
429 return matrix::trace(m);
430 }
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700431};
432
Romain Guy5d4bae72016-11-08 09:49:25 -0800433template<template<typename U> class BASE, typename T>
434class TMatHelpers {
435public:
436 constexpr inline size_t getColumnSize() const { return BASE<T>::COL_SIZE; }
437 constexpr inline size_t getRowSize() const { return BASE<T>::ROW_SIZE; }
438 constexpr inline size_t getColumnCount() const { return BASE<T>::NUM_COLS; }
439 constexpr inline size_t getRowCount() const { return BASE<T>::NUM_ROWS; }
440 constexpr inline size_t size() const { return BASE<T>::ROW_SIZE; } // for TVec*<>
441
442 // array access
443 constexpr T const* asArray() const {
444 return &static_cast<BASE<T> const &>(*this)[0][0];
445 }
446
447 // element access
448 inline constexpr T const& operator()(size_t row, size_t col) const {
449 return static_cast<BASE<T> const &>(*this)[col][row];
450 }
451
452 inline T& operator()(size_t row, size_t col) {
453 return static_cast<BASE<T>&>(*this)[col][row];
454 }
455
456 template <typename VEC>
457 static BASE<T> translate(const VEC& t) {
458 BASE<T> r;
459 r[BASE<T>::NUM_COLS-1] = t;
460 return r;
461 }
462
463 template <typename VEC>
464 static constexpr BASE<T> scale(const VEC& s) {
465 return BASE<T>(s);
466 }
467
468 friend inline BASE<T> PURE abs(BASE<T> m) {
469 for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
470 m[col] = abs(m[col]);
471 }
472 return m;
473 }
474};
475
476// functions for 3x3 and 4x4 matrices
477template<template<typename U> class BASE, typename T>
478class TMatTransform {
479public:
480 inline constexpr TMatTransform() {
481 static_assert(BASE<T>::NUM_ROWS == 3 || BASE<T>::NUM_ROWS == 4, "3x3 or 4x4 matrices only");
482 }
483
484 template <typename A, typename VEC>
485 static BASE<T> rotate(A radian, const VEC& about) {
486 BASE<T> r;
487 T c = std::cos(radian);
488 T s = std::sin(radian);
489 if (about.x == 1 && about.y == 0 && about.z == 0) {
490 r[1][1] = c; r[2][2] = c;
491 r[1][2] = s; r[2][1] = -s;
492 } else if (about.x == 0 && about.y == 1 && about.z == 0) {
493 r[0][0] = c; r[2][2] = c;
494 r[2][0] = s; r[0][2] = -s;
495 } else if (about.x == 0 && about.y == 0 && about.z == 1) {
496 r[0][0] = c; r[1][1] = c;
497 r[0][1] = s; r[1][0] = -s;
498 } else {
499 VEC nabout = normalize(about);
500 typename VEC::value_type x = nabout.x;
501 typename VEC::value_type y = nabout.y;
502 typename VEC::value_type z = nabout.z;
503 T nc = 1 - c;
504 T xy = x * y;
505 T yz = y * z;
506 T zx = z * x;
507 T xs = x * s;
508 T ys = y * s;
509 T zs = z * s;
510 r[0][0] = x*x*nc + c; r[1][0] = xy*nc - zs; r[2][0] = zx*nc + ys;
511 r[0][1] = xy*nc + zs; r[1][1] = y*y*nc + c; r[2][1] = yz*nc - xs;
512 r[0][2] = zx*nc - ys; r[1][2] = yz*nc + xs; r[2][2] = z*z*nc + c;
513
514 // Clamp results to -1, 1.
515 for (size_t col = 0; col < 3; ++col) {
516 for (size_t row = 0; row < 3; ++row) {
517 r[col][row] = std::min(std::max(r[col][row], T(-1)), T(1));
518 }
519 }
520 }
521 return r;
522 }
523
524 /**
525 * Create a matrix from euler angles using YPR around YXZ respectively
526 * @param yaw about Y axis
527 * @param pitch about X axis
528 * @param roll about Z axis
529 */
530 template <
531 typename Y, typename P, typename R,
532 typename = typename std::enable_if<std::is_arithmetic<Y>::value >::type,
533 typename = typename std::enable_if<std::is_arithmetic<P>::value >::type,
534 typename = typename std::enable_if<std::is_arithmetic<R>::value >::type
535 >
536 static BASE<T> eulerYXZ(Y yaw, P pitch, R roll) {
537 return eulerZYX(roll, pitch, yaw);
538 }
539
540 /**
541 * Create a matrix from euler angles using YPR around ZYX respectively
542 * @param roll about X axis
543 * @param pitch about Y axis
544 * @param yaw about Z axis
545 *
546 * The euler angles are applied in ZYX order. i.e: a vector is first rotated
547 * about X (roll) then Y (pitch) and then Z (yaw).
548 */
549 template <
550 typename Y, typename P, typename R,
551 typename = typename std::enable_if<std::is_arithmetic<Y>::value >::type,
552 typename = typename std::enable_if<std::is_arithmetic<P>::value >::type,
553 typename = typename std::enable_if<std::is_arithmetic<R>::value >::type
554 >
555 static BASE<T> eulerZYX(Y yaw, P pitch, R roll) {
556 BASE<T> r;
557 T cy = std::cos(yaw);
558 T sy = std::sin(yaw);
559 T cp = std::cos(pitch);
560 T sp = std::sin(pitch);
561 T cr = std::cos(roll);
562 T sr = std::sin(roll);
563 T cc = cr * cy;
564 T cs = cr * sy;
565 T sc = sr * cy;
566 T ss = sr * sy;
567 r[0][0] = cp * cy;
568 r[0][1] = cp * sy;
569 r[0][2] = -sp;
570 r[1][0] = sp * sc - cs;
571 r[1][1] = sp * ss + cc;
572 r[1][2] = cp * sr;
573 r[2][0] = sp * cc + ss;
574 r[2][1] = sp * cs - sc;
575 r[2][2] = cp * cr;
576
577 // Clamp results to -1, 1.
578 for (size_t col = 0; col < 3; ++col) {
579 for (size_t row = 0; row < 3; ++row) {
580 r[col][row] = std::min(std::max(r[col][row], T(-1)), T(1));
581 }
582 }
583 return r;
584 }
585
586 TQuaternion<T> toQuaternion() const {
587 return matrix::extractQuat(static_cast<const BASE<T>&>(*this));
588 }
589};
590
591
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700592template <template<typename T> class BASE, typename T>
593class TMatDebug {
594public:
Romain Guy5d4bae72016-11-08 09:49:25 -0800595 friend std::ostream& operator<<(std::ostream& stream, const BASE<T>& m) {
596 for (size_t row = 0; row < BASE<T>::NUM_ROWS; ++row) {
597 if (row != 0) {
598 stream << std::endl;
599 }
600 if (row == 0) {
601 stream << "/ ";
602 } else if (row == BASE<T>::NUM_ROWS-1) {
603 stream << "\\ ";
604 } else {
605 stream << "| ";
606 }
607 for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) {
608 stream << std::setw(10) << std::to_string(m[col][row]);
609 }
610 if (row == 0) {
611 stream << " \\";
612 } else if (row == BASE<T>::NUM_ROWS-1) {
613 stream << " /";
614 } else {
615 stream << " |";
616 }
617 }
618 return stream;
619 }
620
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700621 String8 asString() const {
Romain Guy5d4bae72016-11-08 09:49:25 -0800622 return matrix::asString(static_cast<const BASE<T>&>(*this));
Mathias Agopian1d4d8f92013-09-01 21:35:36 -0700623 }
624};
625
626// -------------------------------------------------------------------------------------
Romain Guy5d4bae72016-11-08 09:49:25 -0800627} // namespace details
628} // namespace android
Mathias Agopian595ea772013-08-21 23:10:41 -0700629
Romain Guy5d4bae72016-11-08 09:49:25 -0800630#undef LIKELY
631#undef UNLIKELY
Mathias Agopian595ea772013-08-21 23:10:41 -0700632#undef PURE
633
Romain Guy5d4bae72016-11-08 09:49:25 -0800634#endif // UI_TMATHELPERS_H_