Xiangfei Qian | 35d4bd4 | 2020-05-29 16:35:09 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Copyright 2020 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 | #ifndef SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_ |
| 18 | #define SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_ |
| 19 | |
| 20 | #include <array> |
| 21 | #include <cassert> |
| 22 | #include <cmath> |
| 23 | #include <iosfwd> |
| 24 | |
| 25 | template <class VType> |
| 26 | class Matrix4x4 { |
| 27 | private: |
| 28 | VType m[4][4]; |
| 29 | |
| 30 | public: |
| 31 | typedef Matrix4x4<VType> Self; |
| 32 | typedef VType BaseType; |
| 33 | typedef std::array<VType, 4> MVector; |
| 34 | |
| 35 | // Initialize the matrix to 0 |
| 36 | Matrix4x4() { |
| 37 | m[0][3] = m[0][2] = m[0][1] = m[0][0] = VType(); |
| 38 | m[1][3] = m[1][2] = m[1][1] = m[1][0] = VType(); |
| 39 | m[2][3] = m[2][2] = m[2][1] = m[2][0] = VType(); |
| 40 | m[3][3] = m[3][2] = m[3][1] = m[3][0] = VType(); |
| 41 | } |
| 42 | |
| 43 | // Explicitly set every element on construction |
| 44 | Matrix4x4(const VType& m00, const VType& m01, const VType& m02, const VType& m03, |
| 45 | const VType& m10, const VType& m11, const VType& m12, const VType& m13, |
| 46 | const VType& m20, const VType& m21, const VType& m22, const VType& m23, |
| 47 | const VType& m30, const VType& m31, const VType& m32, const VType& m33) { |
| 48 | m[0][0] = m00; |
| 49 | m[0][1] = m01; |
| 50 | m[0][2] = m02; |
| 51 | m[0][3] = m03; |
| 52 | |
| 53 | m[1][0] = m10; |
| 54 | m[1][1] = m11; |
| 55 | m[1][2] = m12; |
| 56 | m[1][3] = m13; |
| 57 | |
| 58 | m[2][0] = m20; |
| 59 | m[2][1] = m21; |
| 60 | m[2][2] = m22; |
| 61 | m[2][3] = m23; |
| 62 | |
| 63 | m[3][0] = m30; |
| 64 | m[3][1] = m31; |
| 65 | m[3][2] = m32; |
| 66 | m[3][3] = m33; |
| 67 | } |
| 68 | |
| 69 | // Casting constructor |
| 70 | template <class VType2> |
| 71 | static Matrix4x4 cast(const Matrix4x4<VType2>& mb) { |
| 72 | return Matrix4x4(static_cast<VType>(mb(0, 0)), static_cast<VType>(mb(0, 1)), |
| 73 | static_cast<VType>(mb(0, 2)), static_cast<VType>(mb(0, 3)), |
| 74 | static_cast<VType>(mb(1, 0)), static_cast<VType>(mb(1, 1)), |
| 75 | static_cast<VType>(mb(1, 2)), static_cast<VType>(mb(1, 3)), |
| 76 | static_cast<VType>(mb(2, 0)), static_cast<VType>(mb(2, 1)), |
| 77 | static_cast<VType>(mb(2, 2)), static_cast<VType>(mb(2, 3)), |
| 78 | static_cast<VType>(mb(3, 0)), static_cast<VType>(mb(3, 1)), |
| 79 | static_cast<VType>(mb(3, 2)), static_cast<VType>(mb(3, 3))); |
| 80 | } |
| 81 | |
| 82 | // Change the value of all the coefficients of the matrix |
| 83 | inline Matrix4x4& set(const VType& m00, const VType& m01, const VType& m02, const VType& m03, |
| 84 | const VType& m10, const VType& m11, const VType& m12, const VType& m13, |
| 85 | const VType& m20, const VType& m21, const VType& m22, const VType& m23, |
| 86 | const VType& m30, const VType& m31, const VType& m32, const VType& m33) { |
| 87 | m[0][0] = m00; |
| 88 | m[0][1] = m01; |
| 89 | m[0][2] = m02; |
| 90 | m[0][3] = m03; |
| 91 | |
| 92 | m[1][0] = m10; |
| 93 | m[1][1] = m11; |
| 94 | m[1][2] = m12; |
| 95 | m[1][3] = m13; |
| 96 | |
| 97 | m[2][0] = m20; |
| 98 | m[2][1] = m21; |
| 99 | m[2][2] = m22; |
| 100 | m[2][3] = m23; |
| 101 | |
| 102 | m[3][0] = m30; |
| 103 | m[3][1] = m31; |
| 104 | m[3][2] = m32; |
| 105 | m[3][3] = m33; |
| 106 | return (*this); |
| 107 | } |
| 108 | |
| 109 | // Matrix addition |
| 110 | inline Matrix4x4& operator+=(const Matrix4x4& addFrom) { |
| 111 | m[0][0] += addFrom.m[0][0]; |
| 112 | m[0][1] += addFrom.m[0][1]; |
| 113 | m[0][2] += addFrom.m[0][2]; |
| 114 | m[0][3] += addFrom.m[0][3]; |
| 115 | |
| 116 | m[1][0] += addFrom.m[1][0]; |
| 117 | m[1][1] += addFrom.m[1][1]; |
| 118 | m[1][2] += addFrom.m[1][2]; |
| 119 | m[1][3] += addFrom.m[1][3]; |
| 120 | |
| 121 | m[2][0] += addFrom.m[2][0]; |
| 122 | m[2][1] += addFrom.m[2][1]; |
| 123 | m[2][2] += addFrom.m[2][2]; |
| 124 | m[2][3] += addFrom.m[2][3]; |
| 125 | |
| 126 | m[3][0] += addFrom.m[3][0]; |
| 127 | m[3][1] += addFrom.m[3][1]; |
| 128 | m[3][2] += addFrom.m[3][2]; |
| 129 | m[3][3] += addFrom.m[3][3]; |
| 130 | return (*this); |
| 131 | } |
| 132 | |
| 133 | // Matrix subtration |
| 134 | inline Matrix4x4& operator-=(const Matrix4x4& subFrom) { |
| 135 | m[0][0] -= subFrom.m[0][0]; |
| 136 | m[0][1] -= subFrom.m[0][1]; |
| 137 | m[0][2] -= subFrom.m[0][2]; |
| 138 | m[0][3] -= subFrom.m[0][3]; |
| 139 | |
| 140 | m[1][0] -= subFrom.m[1][0]; |
| 141 | m[1][1] -= subFrom.m[1][1]; |
| 142 | m[1][2] -= subFrom.m[1][2]; |
| 143 | m[1][3] -= subFrom.m[1][3]; |
| 144 | |
| 145 | m[2][0] -= subFrom.m[2][0]; |
| 146 | m[2][1] -= subFrom.m[2][1]; |
| 147 | m[2][2] -= subFrom.m[2][2]; |
| 148 | m[2][3] -= subFrom.m[2][3]; |
| 149 | |
| 150 | m[3][0] -= subFrom.m[3][0]; |
| 151 | m[3][1] -= subFrom.m[3][1]; |
| 152 | m[3][2] -= subFrom.m[3][2]; |
| 153 | m[3][3] -= subFrom.m[3][3]; |
| 154 | return (*this); |
| 155 | } |
| 156 | |
| 157 | // Matrix multiplication by a scalar |
| 158 | inline Matrix4x4& operator*=(const VType& k) { |
| 159 | m[0][0] *= k; |
| 160 | m[0][1] *= k; |
| 161 | m[0][2] *= k; |
| 162 | m[0][3] *= k; |
| 163 | |
| 164 | m[1][0] *= k; |
| 165 | m[1][1] *= k; |
| 166 | m[1][2] *= k; |
| 167 | m[1][3] *= k; |
| 168 | |
| 169 | m[2][0] *= k; |
| 170 | m[2][1] *= k; |
| 171 | m[2][2] *= k; |
| 172 | m[2][3] *= k; |
| 173 | |
| 174 | m[3][0] *= k; |
| 175 | m[3][1] *= k; |
| 176 | m[3][2] *= k; |
| 177 | m[3][3] *= k; |
| 178 | return (*this); |
| 179 | } |
| 180 | |
| 181 | // Matrix addition |
| 182 | inline Matrix4x4 operator+(const Matrix4x4& mb) const { return Matrix4x4(*this) += mb; } |
| 183 | |
| 184 | // Matrix subtraction |
| 185 | inline Matrix4x4 operator-(const Matrix4x4& mb) const { return Matrix4x4(*this) -= mb; } |
| 186 | |
| 187 | // Change the sign of all the coefficients in the matrix |
| 188 | friend inline Matrix4x4 operator-(const Matrix4x4& vb) { |
| 189 | return Matrix4x4(-vb.m[0][0], -vb.m[0][1], -vb.m[0][2], -vb.m[0][3], -vb.m[1][0], |
| 190 | -vb.m[1][1], -vb.m[1][2], -vb.m[1][3], -vb.m[2][0], -vb.m[2][1], |
| 191 | -vb.m[2][2], -vb.m[2][3], -vb.m[3][0], -vb.m[3][1], -vb.m[3][2], |
| 192 | -vb.m[3][3]); |
| 193 | } |
| 194 | |
| 195 | // Matrix multiplication by a scalar |
| 196 | inline Matrix4x4 operator*(const VType& k) const { return Matrix4x4(*this) *= k; } |
| 197 | |
| 198 | // Multiplication by a scaler |
| 199 | friend inline Matrix4x4 operator*(const VType& k, const Matrix4x4& mb) { |
| 200 | return Matrix4x4(mb) * k; |
| 201 | } |
| 202 | |
| 203 | // Matrix multiplication |
| 204 | friend Matrix4x4 operator*(const Matrix4x4& a, const Matrix4x4& b) { |
| 205 | return Matrix4x4::fromCols(a * b.col(0), a * b.col(1), a * b.col(2), a * b.col(3)); |
| 206 | } |
| 207 | |
| 208 | // Multiplication of a matrix by a vector |
| 209 | friend MVector operator*(const Matrix4x4& a, const MVector& b) { |
| 210 | return MVector{dotProd(a.row(0), b), dotProd(a.row(1), b), dotProd(a.row(2), b), |
| 211 | dotProd(a.row(3), b)}; |
| 212 | } |
| 213 | |
| 214 | // Return the trace of the matrix |
| 215 | inline VType trace() const { return m[0][0] + m[1][1] + m[2][2] + m[3][3]; } |
| 216 | |
| 217 | // Return a pointer to the data array for interface with other libraries |
| 218 | // like opencv |
| 219 | VType* data() { return reinterpret_cast<VType*>(m); } |
| 220 | const VType* data() const { return reinterpret_cast<const VType*>(m); } |
| 221 | |
| 222 | // Return matrix element (i,j) with 0<=i<=3 0<=j<=3 |
| 223 | inline VType& operator()(const int i, const int j) { |
| 224 | assert(i >= 0); |
| 225 | assert(i < 4); |
| 226 | assert(j >= 0); |
| 227 | assert(j < 4); |
| 228 | return m[i][j]; |
| 229 | } |
| 230 | |
| 231 | inline VType operator()(const int i, const int j) const { |
| 232 | assert(i >= 0); |
| 233 | assert(i < 4); |
| 234 | assert(j >= 0); |
| 235 | assert(j < 4); |
| 236 | return m[i][j]; |
| 237 | } |
| 238 | |
| 239 | // Return matrix element (i/4,i%4) with 0<=i<=15 (access concatenated rows). |
| 240 | inline VType& operator[](const int i) { |
| 241 | assert(i >= 0); |
| 242 | assert(i < 16); |
| 243 | return reinterpret_cast<VType*>(m)[i]; |
| 244 | } |
| 245 | inline VType operator[](const int i) const { |
| 246 | assert(i >= 0); |
| 247 | assert(i < 16); |
| 248 | return reinterpret_cast<const VType*>(m)[i]; |
| 249 | } |
| 250 | |
| 251 | // Return the transposed matrix |
| 252 | inline Matrix4x4 transpose() const { |
| 253 | return Matrix4x4(m[0][0], m[1][0], m[2][0], m[3][0], m[0][1], m[1][1], m[2][1], m[3][1], |
| 254 | m[0][2], m[1][2], m[2][2], m[3][2], m[0][3], m[1][3], m[2][3], m[3][3]); |
| 255 | } |
| 256 | |
| 257 | // Returns the transpose of the matrix of the cofactors. |
| 258 | // (Useful for inversion for example.) |
| 259 | inline Matrix4x4 comatrixTransposed() const { |
| 260 | const auto cof = [this](unsigned int row, unsigned int col) { |
| 261 | unsigned int r0 = (row + 1) % 4; |
| 262 | unsigned int r1 = (row + 2) % 4; |
| 263 | unsigned int r2 = (row + 3) % 4; |
| 264 | unsigned int c0 = (col + 1) % 4; |
| 265 | unsigned int c1 = (col + 2) % 4; |
| 266 | unsigned int c2 = (col + 3) % 4; |
| 267 | |
| 268 | VType minor = m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) - |
| 269 | m[r1][c0] * (m[r0][c1] * m[r2][c2] - m[r2][c1] * m[r0][c2]) + |
| 270 | m[r2][c0] * (m[r0][c1] * m[r1][c2] - m[r1][c1] * m[r0][c2]); |
| 271 | return (row + col) & 1 ? -minor : minor; |
| 272 | }; |
| 273 | |
| 274 | // Transpose |
| 275 | return Matrix4x4(cof(0, 0), cof(1, 0), cof(2, 0), cof(3, 0), cof(0, 1), cof(1, 1), |
| 276 | cof(2, 1), cof(3, 1), cof(0, 2), cof(1, 2), cof(2, 2), cof(3, 2), |
| 277 | cof(0, 3), cof(1, 3), cof(2, 3), cof(3, 3)); |
| 278 | } |
| 279 | |
| 280 | // Return dot production of two the vectors |
| 281 | static inline VType dotProd(const MVector& lhs, const MVector& rhs) { |
| 282 | return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2] + lhs[3] * rhs[3]; |
| 283 | } |
| 284 | |
| 285 | // Return the 4D vector at row i |
| 286 | inline MVector row(const int i) const { |
| 287 | assert(i >= 0); |
| 288 | assert(i < 4); |
| 289 | return MVector{m[i][0], m[i][1], m[i][2], m[i][3]}; |
| 290 | } |
| 291 | |
| 292 | // Return the 4D vector at col i |
| 293 | inline MVector col(const int i) const { |
| 294 | assert(i >= 0); |
| 295 | assert(i < 4); |
| 296 | return MVector{m[0][i], m[1][i], m[2][i], m[3][i]}; |
| 297 | } |
| 298 | |
| 299 | // Create a matrix from 4 row vectors |
| 300 | static inline Matrix4x4 fromRows(const MVector& v1, const MVector& v2, const MVector& v3, |
| 301 | const MVector& v4) { |
| 302 | return Matrix4x4(v1[0], v1[1], v1[2], v1[3], v2[0], v2[1], v2[2], v2[3], v3[0], v3[1], |
| 303 | v3[2], v3[3], v4[0], v4[1], v4[2], v4[3]); |
| 304 | } |
| 305 | |
| 306 | // Create a matrix from 3 column vectors |
| 307 | static inline Matrix4x4 fromCols(const MVector& v1, const MVector& v2, const MVector& v3, |
| 308 | const MVector& v4) { |
| 309 | return Matrix4x4(v1[0], v2[0], v3[0], v4[0], v1[1], v2[1], v3[1], v4[1], v1[2], v2[2], |
| 310 | v3[2], v4[2], v1[3], v2[3], v3[3], v4[3]); |
| 311 | } |
| 312 | |
| 313 | // Set the vector in row i to be v1 |
| 314 | void setRow(int i, const MVector& v1) { |
| 315 | assert(i >= 0); |
| 316 | assert(i < 4); |
| 317 | m[i][0] = v1[0]; |
| 318 | m[i][1] = v1[1]; |
| 319 | m[i][2] = v1[2]; |
| 320 | m[i][3] = v1[3]; |
| 321 | } |
| 322 | |
| 323 | // Set the vector in column i to be v1 |
| 324 | void setCol(int i, const MVector& v1) { |
| 325 | assert(i >= 0); |
| 326 | assert(i < 4); |
| 327 | m[0][i] = v1[0]; |
| 328 | m[1][i] = v1[1]; |
| 329 | m[2][i] = v1[2]; |
| 330 | m[3][i] = v1[3]; |
| 331 | } |
| 332 | |
| 333 | // Return the identity matrix |
| 334 | static inline Matrix4x4 identity() { |
| 335 | return Matrix4x4(VType(1), VType(), VType(), VType(), VType(), VType(1), VType(), VType(), |
| 336 | VType(), VType(), VType(1), VType(), VType(), VType(), VType(), VType(1)); |
| 337 | } |
| 338 | |
| 339 | // Return a matrix full of zeros |
| 340 | static inline Matrix4x4 zero() { return Matrix4x4(); } |
| 341 | |
| 342 | // Return a diagonal matrix with the coefficients in v |
| 343 | static inline Matrix4x4 diagonal(const MVector& v) { |
| 344 | return Matrix4x4(v[0], VType(), VType(), VType(), // |
| 345 | VType(), v[1], VType(), VType(), // |
| 346 | VType(), VType(), v[2], VType(), // |
| 347 | VType(), VType(), VType(), v[3]); |
| 348 | } |
| 349 | |
| 350 | // Return the matrix vvT |
| 351 | static Matrix4x4 sym4(const MVector& v) { |
| 352 | return Matrix4x4(v[0] * v[0], v[0] * v[1], v[0] * v[2], v[0] * v[3], v[1] * v[0], |
| 353 | v[1] * v[1], v[1] * v[2], v[1] * v[3], v[2] * v[0], v[2] * v[1], |
| 354 | v[2] * v[2], v[2] * v[3], v[3] * v[0], v[3] * v[1], v[3] * v[2], |
| 355 | v[3] * v[3]); |
| 356 | } |
| 357 | |
| 358 | // Return the Frobenius norm of the matrix: sqrt(sum(aij^2)) |
| 359 | VType frobeniusNorm() const { |
| 360 | VType sum = VType(); |
| 361 | for (int i = 0; i < 4; i++) { |
| 362 | for (int j = 0; j < 4; j++) { |
| 363 | sum += m[i][j] * m[i][j]; |
| 364 | } |
| 365 | } |
| 366 | return std::sqrt(sum); |
| 367 | } |
| 368 | |
| 369 | // Return true is one of the elements of the matrix is NaN |
| 370 | bool isNaN() const { |
| 371 | for (int i = 0; i < 4; ++i) { |
| 372 | for (int j = 0; j < 4; ++j) { |
| 373 | if (isnan(m[i][j])) { |
| 374 | return true; |
| 375 | } |
| 376 | } |
| 377 | } |
| 378 | return false; |
| 379 | } |
| 380 | |
| 381 | friend bool operator==(const Matrix4x4& a, const Matrix4x4& b) { |
| 382 | return a.m[0][0] == b.m[0][0] && a.m[0][1] == b.m[0][1] && a.m[0][2] == b.m[0][2] && |
| 383 | a.m[0][3] == b.m[0][3] && a.m[1][0] == b.m[1][0] && a.m[1][1] == b.m[1][1] && |
| 384 | a.m[1][2] == b.m[1][2] && a.m[1][3] == b.m[1][3] && a.m[2][0] == b.m[2][0] && |
| 385 | a.m[2][1] == b.m[2][1] && a.m[2][2] == b.m[2][2] && a.m[2][3] == b.m[2][3] && |
| 386 | a.m[3][0] == b.m[3][0] && a.m[3][1] == b.m[3][1] && a.m[3][2] == b.m[3][2] && |
| 387 | a.m[3][3] == b.m[3][3]; |
| 388 | } |
| 389 | |
| 390 | friend bool operator!=(const Matrix4x4& a, const Matrix4x4& b) { return !(a == b); } |
| 391 | }; |
| 392 | |
| 393 | typedef Matrix4x4<int> Matrix4x4I; |
| 394 | typedef Matrix4x4<float> Matrix4x4F; |
| 395 | typedef Matrix4x4<double> Matrix4x4D; |
| 396 | |
| 397 | #endif // #ifndef SURROUND_VIEW_SERVICE_IMPL_MATRIX4X4_H_ |