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