Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 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 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 17 | #ifndef UI_TMATHELPERS_H_ |
| 18 | #define UI_TMATHELPERS_H_ |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 19 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 20 | #include <math.h> |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 21 | #include <stdint.h> |
| 22 | #include <sys/types.h> |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 23 | |
| 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 Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 41 | |
| 42 | #define PURE __attribute__((pure)) |
| 43 | |
| 44 | namespace android { |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 45 | namespace details { |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 46 | // ------------------------------------------------------------------------------------- |
| 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 | |
| 59 | namespace matrix { |
| 60 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 61 | inline constexpr int transpose(int v) { return v; } |
| 62 | inline constexpr float transpose(float v) { return v; } |
| 63 | inline constexpr double transpose(double v) { return v; } |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 64 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 65 | inline constexpr int trace(int v) { return v; } |
| 66 | inline constexpr float trace(float v) { return v; } |
| 67 | inline constexpr double trace(double v) { return v; } |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 68 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 69 | /* |
| 70 | * Matrix inversion |
| 71 | */ |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 72 | template<typename MATRIX> |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 73 | MATRIX PURE gaussJordanInverse(const MATRIX& src) { |
| 74 | typedef typename MATRIX::value_type T; |
| 75 | static constexpr unsigned int N = MATRIX::NUM_ROWS; |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 76 | MATRIX tmp(src); |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 77 | MATRIX inverted(1); |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 78 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 79 | 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 Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 86 | swap = j; |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 87 | t = t2; |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 88 | } |
| 89 | } |
| 90 | |
| 91 | if (swap != i) { |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 92 | // swap columns. |
| 93 | std::swap(tmp[i], tmp[swap]); |
| 94 | std::swap(inverted[i], inverted[swap]); |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 95 | } |
| 96 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 97 | 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 Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 101 | } |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 102 | |
| 103 | // Factor out the lower triangle |
| 104 | for (size_t j = 0; j < N; ++j) { |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 105 | if (j != i) { |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 106 | const T t = tmp[j][i]; |
| 107 | for (size_t k = 0; k < N; ++k) { |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 108 | tmp[j][k] -= tmp[i][k] * t; |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 109 | inverted[j][k] -= inverted[i][k] * t; |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 110 | } |
| 111 | } |
| 112 | } |
| 113 | } |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 114 | |
| 115 | return inverted; |
| 116 | } |
| 117 | |
| 118 | |
| 119 | //------------------------------------------------------------------------------ |
| 120 | // 2x2 matrix inverse is easy. |
| 121 | template <typename MATRIX> |
| 122 | MATRIX 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 |
| 155 | template <typename MATRIX> |
| 156 | MATRIX 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 | */ |
| 226 | template <typename MATRIX> |
| 227 | inline 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 Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 232 | } |
| 233 | |
| 234 | template<typename MATRIX_R, typename MATRIX_A, typename MATRIX_B> |
| 235 | MATRIX_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 Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 241 | 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 Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 247 | |
| 248 | MATRIX_R res(MATRIX_R::NO_INIT); |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 249 | for (size_t col = 0; col < MATRIX_R::NUM_COLS; ++col) { |
| 250 | res[col] = lhs * rhs[col]; |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 251 | } |
| 252 | return res; |
| 253 | } |
| 254 | |
| 255 | // transpose. this handles matrices of matrices |
| 256 | template <typename MATRIX> |
| 257 | MATRIX PURE transpose(const MATRIX& m) { |
| 258 | // for now we only handle square matrix transpose |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 259 | static_assert(MATRIX::NUM_COLS == MATRIX::NUM_ROWS, "transpose only supports square matrices"); |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 260 | MATRIX result(MATRIX::NO_INIT); |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 261 | 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 Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 266 | return result; |
| 267 | } |
| 268 | |
| 269 | // trace. this handles matrices of matrices |
| 270 | template <typename MATRIX> |
| 271 | typename MATRIX::value_type PURE trace(const MATRIX& m) { |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 272 | static_assert(MATRIX::NUM_COLS == MATRIX::NUM_ROWS, "trace only defined for square matrices"); |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 273 | typename MATRIX::value_type result(0); |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 274 | for (size_t col = 0; col < MATRIX::NUM_COLS; ++col) { |
| 275 | result += trace(m[col][col]); |
| 276 | } |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 277 | return result; |
| 278 | } |
| 279 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 280 | // diag. this handles matrices of matrices |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 281 | template <typename MATRIX> |
| 282 | typename MATRIX::col_type PURE diag(const MATRIX& m) { |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 283 | static_assert(MATRIX::NUM_COLS == MATRIX::NUM_ROWS, "diag only defined for square matrices"); |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 284 | typename MATRIX::col_type result(MATRIX::col_type::NO_INIT); |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 285 | for (size_t col = 0; col < MATRIX::NUM_COLS; ++col) { |
| 286 | result[col] = m[col][col]; |
| 287 | } |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 288 | return result; |
| 289 | } |
| 290 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 291 | //------------------------------------------------------------------------------ |
| 292 | // This is taken from the Imath MatrixAlgo code, and is identical to Eigen. |
| 293 | template <typename MATRIX> |
| 294 | TQuaternion<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 Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 335 | template <typename MATRIX> |
| 336 | String8 asString(const MATRIX& m) { |
| 337 | String8 s; |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 338 | for (size_t c = 0; c < MATRIX::col_size(); c++) { |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 339 | s.append("| "); |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 340 | for (size_t r = 0; r < MATRIX::row_size(); r++) { |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 341 | s.appendFormat("%7.2f ", m[r][c]); |
| 342 | } |
| 343 | s.append("|\n"); |
| 344 | } |
| 345 | return s; |
| 346 | } |
| 347 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 348 | } // namespace matrix |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 349 | |
| 350 | // ------------------------------------------------------------------------------------- |
Mathias Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 351 | |
| 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 | |
| 361 | template <template<typename T> class BASE, typename T> |
| 362 | class TMatProductOperators { |
| 363 | public: |
| 364 | // multiply by a scalar |
| 365 | BASE<T>& operator *= (T v) { |
| 366 | BASE<T>& lhs(static_cast< BASE<T>& >(*this)); |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 367 | for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) { |
| 368 | lhs[col] *= v; |
Mathias Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 369 | } |
| 370 | return lhs; |
| 371 | } |
| 372 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 373 | // 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 Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 381 | // divide by a scalar |
| 382 | BASE<T>& operator /= (T v) { |
| 383 | BASE<T>& lhs(static_cast< BASE<T>& >(*this)); |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 384 | for (size_t col = 0; col < BASE<T>::NUM_COLS; ++col) { |
| 385 | lhs[col] /= v; |
Mathias Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 386 | } |
| 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 Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 397 | /* |
| 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 | |
| 411 | template<template<typename U> class BASE, typename T> |
| 412 | class TMatSquareFunctions { |
| 413 | public: |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 414 | |
Mathias Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 415 | /* |
| 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 Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 422 | 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 Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 431 | }; |
| 432 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 433 | template<template<typename U> class BASE, typename T> |
| 434 | class TMatHelpers { |
| 435 | public: |
| 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 |
| 477 | template<template<typename U> class BASE, typename T> |
| 478 | class TMatTransform { |
| 479 | public: |
| 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 Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 592 | template <template<typename T> class BASE, typename T> |
| 593 | class TMatDebug { |
| 594 | public: |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 595 | 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 Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 621 | String8 asString() const { |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 622 | return matrix::asString(static_cast<const BASE<T>&>(*this)); |
Mathias Agopian | 1d4d8f9 | 2013-09-01 21:35:36 -0700 | [diff] [blame] | 623 | } |
| 624 | }; |
| 625 | |
| 626 | // ------------------------------------------------------------------------------------- |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 627 | } // namespace details |
| 628 | } // namespace android |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 629 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 630 | #undef LIKELY |
| 631 | #undef UNLIKELY |
Mathias Agopian | 595ea77 | 2013-08-21 23:10:41 -0700 | [diff] [blame] | 632 | #undef PURE |
| 633 | |
Romain Guy | 5d4bae7 | 2016-11-08 09:49:25 -0800 | [diff] [blame^] | 634 | #endif // UI_TMATHELPERS_H_ |