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
Matrix4x4()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
Matrix4x4(const VType & m00,const VType & m01,const VType & m02,const VType & m03,const VType & m10,const VType & m11,const VType & m12,const VType & m13,const VType & m20,const VType & m21,const VType & m22,const VType & m23,const VType & m30,const VType & m31,const VType & m32,const VType & m33)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>
cast(const Matrix4x4<VType2> & mb)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
set(const VType & m00,const VType & m01,const VType & m02,const VType & m03,const VType & m10,const VType & m11,const VType & m12,const VType & m13,const VType & m20,const VType & m21,const VType & m22,const VType & m23,const VType & m30,const VType & m31,const VType & m32,const VType & m33)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
trace()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
data()219     VType* data() { return reinterpret_cast<VType*>(m); }
data()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
operator()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 
operator()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
transpose()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.)
comatrixTransposed()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
dotProd(const MVector & lhs,const MVector & rhs)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
row(const int 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
col(const int 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
fromRows(const MVector & v1,const MVector & v2,const MVector & v3,const MVector & v4)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
fromCols(const MVector & v1,const MVector & v2,const MVector & v3,const MVector & v4)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
setRow(int i,const MVector & 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
setCol(int i,const MVector & 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
identity()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
zero()340     static inline Matrix4x4 zero() { return Matrix4x4(); }
341 
342     // Return a diagonal matrix with the coefficients in v
diagonal(const MVector & 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
sym4(const MVector & v)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))
frobeniusNorm()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
isNaN()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_
398