1 /*
2 * Copyright (c) 2021-2022 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15
16 #include "base/geometry/matrix4.h"
17
18 namespace OHOS::Ace {
19 namespace {
20 constexpr int32_t MATRIX_LENGTH = Matrix4::DIMENSION * Matrix4::DIMENSION;
21 constexpr double ANGLE_UNIT = 0.017453f; // PI / 180
22
IsEqual(const double & left,const double & right)23 inline bool IsEqual(const double& left, const double& right)
24 {
25 constexpr double epsilon = 0.0001;
26 return NearEqual(left, right, epsilon);
27 }
28
29 } // namespace
30
CreateIdentity()31 Matrix4 Matrix4::CreateIdentity()
32 {
33 return Matrix4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
34 }
35
CreateTranslate(double x,double y,double z)36 Matrix4 Matrix4::CreateTranslate(double x, double y, double z)
37 {
38 return Matrix4(1.0f, 0.0f, 0.0f, x, 0.0f, 1.0f, 0.0f, y, 0.0f, 0.0f, 1.0f, z, 0.0f, 0.0f, 0.0f, 1.0f);
39 }
40
CreateScale(double x,double y,double z)41 Matrix4 Matrix4::CreateScale(double x, double y, double z)
42 {
43 return Matrix4(x, 0.0f, 0.0f, 0.0f, 0.0f, y, 0.0f, 0.0f, 0.0f, 0.0f, z, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
44 }
45
CreateRotate(double angle,double dx,double dy,double dz)46 Matrix4 Matrix4::CreateRotate(double angle, double dx, double dy, double dz)
47 {
48 // (x,y,z) need normalize
49 double sum = dx * dx + dy * dy + dz * dz;
50 if (NearZero(sum)) {
51 return Matrix4::CreateIdentity();
52 }
53
54 double x = dx / sqrt(sum);
55 double y = dy / sqrt(sum);
56 double z = dz / sqrt(sum);
57 double redian = static_cast<double>(angle * (M_PI / 180.0f));
58 double cosValue = cosf(redian);
59 double sinValue = sinf(redian);
60
61 return Matrix4(cosValue + (x * x * (1.0f - cosValue)), (x * y * (1.0f - cosValue)) - (z * sinValue),
62 (x * z * (1.0f - cosValue)) + (y * sinValue), 0.0f, (y * x * (1.0f - cosValue)) + (z * sinValue),
63 cosValue + (y * y * (1.0f - cosValue)), (y * z * (1.0f - cosValue)) - (x * sinValue), 0.0f,
64 (z * x * (1.0f - cosValue)) - (y * sinValue), (z * y * (1.0f - cosValue)) + (x * sinValue),
65 cosValue + (z * z * (1.0f - cosValue)), 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
66 }
67
CreateMatrix2D(double m00,double m10,double m01,double m11,double m03,double m13)68 Matrix4 Matrix4::CreateMatrix2D(double m00, double m10, double m01, double m11, double m03, double m13)
69 {
70 return Matrix4(m00, m01, 0.0f, m03, m10, m11, 0.0f, m13, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
71 }
72
CreateSkew(double x,double y)73 Matrix4 Matrix4::CreateSkew(double x, double y)
74 {
75 return Matrix4(1.0f, std::tan(x * ANGLE_UNIT), 0.0f, 0.0f, std::tan(y * ANGLE_UNIT), 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
76 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
77 }
78
CreateFactorSkew(double x,double y)79 Matrix4 Matrix4::CreateFactorSkew(double x, double y)
80 {
81 return Matrix4(1.0f, x, 0.0f, 0.0f, y, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
82 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
83 }
84
CreateFactorPerspective(double x,double y)85 Matrix4 Matrix4::CreateFactorPerspective(double x, double y)
86 {
87 return Matrix4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f,
88 1.0f, 0.0f, x, y, 0.0f, 1.0f);
89 }
90
CreatePerspective(double distance)91 Matrix4 Matrix4::CreatePerspective(double distance)
92 {
93 auto result = CreateIdentity();
94 if (GreatNotEqual(distance, 0.0f)) {
95 result.matrix4x4_[2][3] = -1.0f / distance;
96 }
97 return result;
98 }
99
Invert(const Matrix4 & matrix)100 Matrix4 Matrix4::Invert(const Matrix4& matrix)
101 {
102 Matrix4 inverted = CreateInvert(matrix);
103 double determinant = matrix(0, 0) * inverted(0, 0) + matrix(0, 1) * inverted(1, 0) + matrix(0, 2) * inverted(2, 0) +
104 matrix(0, 3) * inverted(3, 0);
105 if (!NearZero(determinant)) {
106 inverted = inverted * (1.0f / determinant);
107 } else {
108 inverted = CreateIdentity();
109 }
110
111 return inverted;
112 }
113
QuaternionToMatrix(double x,double y,double z,double w)114 Matrix4 Matrix4::QuaternionToMatrix(double x, double y, double z, double w)
115 {
116 double norm = std::sqrt(w * w + x * x + y * y + z * z);
117 if (LessOrEqual(norm, 0.0f)) {
118 return Matrix4();
119 }
120 w /= norm;
121 x /= norm;
122 y /= norm;
123 z /= norm;
124
125 // Quaternion to matrix operation wiki:reference/apis-arkui/js-apis-matrix4.md.
126 return Matrix4(1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z), 2.0 * (x * z + w * y), 0.0,
127 2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - w * x), 0.0,
128 2.0 * (x * z - w * y), 2.0 * (y * z + w * x), 1.0 - 2.0 * (x * x + y * y), 0.0,
129 0.0, 0.0, 0.0, 1.0);
130 }
131
Matrix4()132 Matrix4::Matrix4()
133 : Matrix4(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f)
134 {}
135
Matrix4(const Matrix4 & matrix)136 Matrix4::Matrix4(const Matrix4& matrix)
137 {
138 std::copy_n(&matrix.matrix4x4_[0][0], MATRIX_LENGTH, &matrix4x4_[0][0]);
139 }
140
Matrix4(double m00,double m01,double m02,double m03,double m10,double m11,double m12,double m13,double m20,double m21,double m22,double m23,double m30,double m31,double m32,double m33)141 Matrix4::Matrix4(double m00, double m01, double m02, double m03, double m10, double m11, double m12, double m13,
142 double m20, double m21, double m22, double m23, double m30, double m31, double m32, double m33)
143 {
144 matrix4x4_[0][0] = m00;
145 matrix4x4_[1][0] = m01;
146 matrix4x4_[2][0] = m02;
147 matrix4x4_[3][0] = m03;
148 matrix4x4_[0][1] = m10;
149 matrix4x4_[1][1] = m11;
150 matrix4x4_[2][1] = m12;
151 matrix4x4_[3][1] = m13;
152 matrix4x4_[0][2] = m20;
153 matrix4x4_[1][2] = m21;
154 matrix4x4_[2][2] = m22;
155 matrix4x4_[3][2] = m23;
156 matrix4x4_[0][3] = m30;
157 matrix4x4_[1][3] = m31;
158 matrix4x4_[2][3] = m32;
159 matrix4x4_[3][3] = m33;
160 }
161
SetScale(double x,double y,double z)162 void Matrix4::SetScale(double x, double y, double z)
163 {
164 // The 4X4 matrix scale index is [0][0], [1][1], [2][2], [3][3].
165 matrix4x4_[0][0] = x;
166 matrix4x4_[1][1] = y;
167 matrix4x4_[2][2] = z;
168 matrix4x4_[3][3] = 1.0f;
169 }
170
GetScaleX() const171 double Matrix4::GetScaleX() const
172 {
173 return matrix4x4_[0][0];
174 }
175
GetScaleY() const176 double Matrix4::GetScaleY() const
177 {
178 return matrix4x4_[1][1];
179 }
180
SetEntry(int32_t row,int32_t col,double value)181 void Matrix4::SetEntry(int32_t row, int32_t col, double value)
182 {
183 if ((row < 0 || row >= DIMENSION) || (col < 0 || col >= DIMENSION)) {
184 return;
185 }
186 matrix4x4_[row][col] = value;
187 }
188
IsIdentityMatrix() const189 bool Matrix4::IsIdentityMatrix() const
190 {
191 return *this == CreateIdentity();
192 }
193
Rotate(double angle,double dx,double dy,double dz)194 void Matrix4::Rotate(double angle, double dx, double dy, double dz)
195 {
196 Matrix4 transform = *this;
197 *this = transform * CreateRotate(angle, dx, dy, dz);
198 }
199
Count() const200 int32_t Matrix4::Count() const
201 {
202 return MATRIX_LENGTH;
203 }
204
CreateInvert(const Matrix4 & matrix)205 Matrix4 Matrix4::CreateInvert(const Matrix4& matrix)
206 {
207 return Matrix4(
208 matrix(1, 1) * matrix(2, 2) * matrix(3, 3) - matrix(1, 1) * matrix(2, 3) * matrix(3, 2) -
209 matrix(2, 1) * matrix(1, 2) * matrix(3, 3) + matrix(2, 1) * matrix(1, 3) * matrix(3, 2) +
210 matrix(3, 1) * matrix(1, 2) * matrix(2, 3) - matrix(3, 1) * matrix(1, 3) * matrix(2, 2),
211 -matrix(1, 0) * matrix(2, 2) * matrix(3, 3) + matrix(1, 0) * matrix(2, 3) * matrix(3, 2) +
212 matrix(2, 0) * matrix(1, 2) * matrix(3, 3) - matrix(2, 0) * matrix(1, 3) * matrix(3, 2) -
213 matrix(3, 0) * matrix(1, 2) * matrix(2, 3) + matrix(3, 0) * matrix(1, 3) * matrix(2, 2),
214 matrix(1, 0) * matrix(2, 1) * matrix(3, 3) - matrix(1, 0) * matrix(2, 3) * matrix(3, 1) -
215 matrix(2, 0) * matrix(1, 1) * matrix(3, 3) + matrix(2, 0) * matrix(1, 3) * matrix(3, 1) +
216 matrix(3, 0) * matrix(1, 1) * matrix(2, 3) - matrix(3, 0) * matrix(1, 3) * matrix(2, 1),
217 -matrix(1, 0) * matrix(2, 1) * matrix(3, 2) + matrix(1, 0) * matrix(2, 2) * matrix(3, 1) +
218 matrix(2, 0) * matrix(1, 1) * matrix(3, 2) - matrix(2, 0) * matrix(1, 2) * matrix(3, 1) -
219 matrix(3, 0) * matrix(1, 1) * matrix(2, 2) + matrix(3, 0) * matrix(1, 2) * matrix(2, 1),
220 -matrix(0, 1) * matrix(2, 2) * matrix(3, 3) + matrix(0, 1) * matrix(2, 3) * matrix(3, 2) +
221 matrix(2, 1) * matrix(0, 2) * matrix(3, 3) - matrix(2, 1) * matrix(0, 3) * matrix(3, 2) -
222 matrix(3, 1) * matrix(0, 2) * matrix(2, 3) + matrix(3, 1) * matrix(0, 3) * matrix(2, 2),
223 matrix(0, 0) * matrix(2, 2) * matrix(3, 3) - matrix(0, 0) * matrix(2, 3) * matrix(3, 2) -
224 matrix(2, 0) * matrix(0, 2) * matrix(3, 3) + matrix(2, 0) * matrix(0, 3) * matrix(3, 2) +
225 matrix(3, 0) * matrix(0, 2) * matrix(2, 3) - matrix(3, 0) * matrix(0, 3) * matrix(2, 2),
226 -matrix(0, 0) * matrix(2, 1) * matrix(3, 3) + matrix(0, 0) * matrix(2, 3) * matrix(3, 1) +
227 matrix(2, 0) * matrix(0, 1) * matrix(3, 3) - matrix(2, 0) * matrix(0, 3) * matrix(3, 1) -
228 matrix(3, 0) * matrix(0, 1) * matrix(2, 3) + matrix(3, 0) * matrix(0, 3) * matrix(2, 1),
229 matrix(0, 0) * matrix(2, 1) * matrix(3, 2) - matrix(0, 0) * matrix(2, 2) * matrix(3, 1) -
230 matrix(2, 0) * matrix(0, 1) * matrix(3, 2) + matrix(2, 0) * matrix(0, 2) * matrix(3, 1) +
231 matrix(3, 0) * matrix(0, 1) * matrix(2, 2) - matrix(3, 0) * matrix(0, 2) * matrix(2, 1),
232 matrix(0, 1) * matrix(1, 2) * matrix(3, 3) - matrix(0, 1) * matrix(1, 3) * matrix(3, 2) -
233 matrix(1, 1) * matrix(0, 2) * matrix(3, 3) + matrix(1, 1) * matrix(0, 3) * matrix(3, 2) +
234 matrix(3, 1) * matrix(0, 2) * matrix(1, 3) - matrix(3, 1) * matrix(0, 3) * matrix(1, 2),
235 -matrix(0, 0) * matrix(1, 2) * matrix(3, 3) + matrix(0, 0) * matrix(1, 3) * matrix(3, 2) +
236 matrix(1, 0) * matrix(0, 2) * matrix(3, 3) - matrix(1, 0) * matrix(0, 3) * matrix(3, 2) -
237 matrix(3, 0) * matrix(0, 2) * matrix(1, 3) + matrix(3, 0) * matrix(0, 3) * matrix(1, 2),
238 matrix(0, 0) * matrix(1, 1) * matrix(3, 3) - matrix(0, 0) * matrix(1, 3) * matrix(3, 1) -
239 matrix(1, 0) * matrix(0, 1) * matrix(3, 3) + matrix(1, 0) * matrix(0, 3) * matrix(3, 1) +
240 matrix(3, 0) * matrix(0, 1) * matrix(1, 3) - matrix(3, 0) * matrix(0, 3) * matrix(1, 1),
241 -matrix(0, 0) * matrix(1, 1) * matrix(3, 2) + matrix(0, 0) * matrix(1, 2) * matrix(3, 1) +
242 matrix(1, 0) * matrix(0, 1) * matrix(3, 2) - matrix(1, 0) * matrix(0, 2) * matrix(3, 1) -
243 matrix(3, 0) * matrix(0, 1) * matrix(1, 2) + matrix(3, 0) * matrix(0, 2) * matrix(1, 1),
244 -matrix(0, 1) * matrix(1, 2) * matrix(2, 3) + matrix(0, 1) * matrix(1, 3) * matrix(2, 2) +
245 matrix(1, 1) * matrix(0, 2) * matrix(2, 3) - matrix(1, 1) * matrix(0, 3) * matrix(2, 2) -
246 matrix(2, 1) * matrix(0, 2) * matrix(1, 3) + matrix(2, 1) * matrix(0, 3) * matrix(1, 2),
247 matrix(0, 0) * matrix(1, 2) * matrix(2, 3) - matrix(0, 0) * matrix(1, 3) * matrix(2, 2) -
248 matrix(1, 0) * matrix(0, 2) * matrix(2, 3) + matrix(1, 0) * matrix(0, 3) * matrix(2, 2) +
249 matrix(2, 0) * matrix(0, 2) * matrix(1, 3) - matrix(2, 0) * matrix(0, 3) * matrix(1, 2),
250 -matrix(0, 0) * matrix(1, 1) * matrix(2, 3) + matrix(0, 0) * matrix(1, 3) * matrix(2, 1) +
251 matrix(1, 0) * matrix(0, 1) * matrix(2, 3) - matrix(1, 0) * matrix(0, 3) * matrix(2, 1) -
252 matrix(2, 0) * matrix(0, 1) * matrix(1, 3) + matrix(2, 0) * matrix(0, 3) * matrix(1, 1),
253 matrix(0, 0) * matrix(1, 1) * matrix(2, 2) - matrix(0, 0) * matrix(1, 2) * matrix(2, 1) -
254 matrix(1, 0) * matrix(0, 1) * matrix(2, 2) + matrix(1, 0) * matrix(0, 2) * matrix(2, 1) +
255 matrix(2, 0) * matrix(0, 1) * matrix(1, 2) - matrix(2, 0) * matrix(0, 2) * matrix(1, 1));
256 }
257
operator ==(const Matrix4 & matrix) const258 bool Matrix4::operator==(const Matrix4& matrix) const
259 {
260 return std::equal(&matrix4x4_[0][0], &matrix4x4_[0][0] + MATRIX_LENGTH, &matrix.matrix4x4_[0][0], IsEqual);
261 }
262
operator *(double num)263 Matrix4 Matrix4::operator*(double num)
264 {
265 Matrix4 ret(*this);
266 auto function = [num](double& v) { v *= num; };
267 auto it = &ret.matrix4x4_[0][0];
268 for (int32_t i = 0; i < MATRIX_LENGTH; ++it, ++i) {
269 function(*it);
270 }
271 return ret;
272 }
273
operator *(const Matrix4 & matrix)274 Matrix4 Matrix4::operator*(const Matrix4& matrix)
275 {
276 return Matrix4(
277 matrix4x4_[0][0] * matrix(0, 0) + matrix4x4_[1][0] * matrix(0, 1) + matrix4x4_[2][0] * matrix(0, 2) +
278 matrix4x4_[3][0] * matrix(0, 3),
279 matrix4x4_[0][0] * matrix(1, 0) + matrix4x4_[1][0] * matrix(1, 1) + matrix4x4_[2][0] * matrix(1, 2) +
280 matrix4x4_[3][0] * matrix(1, 3),
281 matrix4x4_[0][0] * matrix(2, 0) + matrix4x4_[1][0] * matrix(2, 1) + matrix4x4_[2][0] * matrix(2, 2) +
282 matrix4x4_[3][0] * matrix(2, 3),
283 matrix4x4_[0][0] * matrix(3, 0) + matrix4x4_[1][0] * matrix(3, 1) + matrix4x4_[2][0] * matrix(3, 2) +
284 matrix4x4_[3][0] * matrix(3, 3),
285 matrix4x4_[0][1] * matrix(0, 0) + matrix4x4_[1][1] * matrix(0, 1) + matrix4x4_[2][1] * matrix(0, 2) +
286 matrix4x4_[3][1] * matrix(0, 3),
287 matrix4x4_[0][1] * matrix(1, 0) + matrix4x4_[1][1] * matrix(1, 1) + matrix4x4_[2][1] * matrix(1, 2) +
288 matrix4x4_[3][1] * matrix(1, 3),
289 matrix4x4_[0][1] * matrix(2, 0) + matrix4x4_[1][1] * matrix(2, 1) + matrix4x4_[2][1] * matrix(2, 2) +
290 matrix4x4_[3][1] * matrix(2, 3),
291 matrix4x4_[0][1] * matrix(3, 0) + matrix4x4_[1][1] * matrix(3, 1) + matrix4x4_[2][1] * matrix(3, 2) +
292 matrix4x4_[3][1] * matrix(3, 3),
293 matrix4x4_[0][2] * matrix(0, 0) + matrix4x4_[1][2] * matrix(0, 1) + matrix4x4_[2][2] * matrix(0, 2) +
294 matrix4x4_[3][2] * matrix(0, 3),
295 matrix4x4_[0][2] * matrix(1, 0) + matrix4x4_[1][2] * matrix(1, 1) + matrix4x4_[2][2] * matrix(1, 2) +
296 matrix4x4_[3][2] * matrix(1, 3),
297 matrix4x4_[0][2] * matrix(2, 0) + matrix4x4_[1][2] * matrix(2, 1) + matrix4x4_[2][2] * matrix(2, 2) +
298 matrix4x4_[3][2] * matrix(2, 3),
299 matrix4x4_[0][2] * matrix(3, 0) + matrix4x4_[1][2] * matrix(3, 1) + matrix4x4_[2][2] * matrix(3, 2) +
300 matrix4x4_[3][2] * matrix(3, 3),
301 matrix4x4_[0][3] * matrix(0, 0) + matrix4x4_[1][3] * matrix(0, 1) + matrix4x4_[2][3] * matrix(0, 2) +
302 matrix4x4_[3][3] * matrix(0, 3),
303 matrix4x4_[0][3] * matrix(1, 0) + matrix4x4_[1][3] * matrix(1, 1) + matrix4x4_[2][3] * matrix(1, 2) +
304 matrix4x4_[3][3] * matrix(1, 3),
305 matrix4x4_[0][3] * matrix(2, 0) + matrix4x4_[1][3] * matrix(2, 1) + matrix4x4_[2][3] * matrix(2, 2) +
306 matrix4x4_[3][3] * matrix(2, 3),
307 matrix4x4_[0][3] * matrix(3, 0) + matrix4x4_[1][3] * matrix(3, 1) + matrix4x4_[2][3] * matrix(3, 2) +
308 matrix4x4_[3][3] * matrix(3, 3));
309 }
310
operator *(const Matrix4N & matrix) const311 Matrix4N Matrix4::operator*(const Matrix4N& matrix) const
312 {
313 int32_t columns = matrix.GetColNum();
314 Matrix4N matrix4n(columns);
315 for (auto i = 0; i < DIMENSION; i++) {
316 for (auto j = 0; j < columns; j++) {
317 double value = 0.0;
318 for (auto k = 0; k < DIMENSION; k++) {
319 value += matrix4x4_[i][k] * matrix[k][j];
320 }
321 matrix4n[i][j] = value;
322 }
323 }
324 return matrix4n;
325 }
326
operator *(const Point & point)327 Point Matrix4::operator*(const Point& point)
328 {
329 double x = point.GetX();
330 double y = point.GetY();
331 return Point(matrix4x4_[0][0] * x + matrix4x4_[1][0] * y + matrix4x4_[3][0],
332 matrix4x4_[0][1] * x + matrix4x4_[1][1] * y + matrix4x4_[3][1]);
333 }
334
operator =(const Matrix4 & matrix)335 Matrix4& Matrix4::operator=(const Matrix4& matrix)
336 {
337 if (this == &matrix) {
338 return *this;
339 }
340 std::copy_n(&matrix.matrix4x4_[0][0], MATRIX_LENGTH, &matrix4x4_[0][0]);
341 return *this;
342 }
343
operator [](int32_t index) const344 double Matrix4::operator[](int32_t index) const
345 {
346 if (index < 0 || index >= MATRIX_LENGTH) {
347 return 0.0f;
348 }
349 int32_t row = index / DIMENSION;
350 int32_t col = index % DIMENSION;
351 return matrix4x4_[row][col];
352 }
353
354 double Matrix4::operator()(int32_t row, int32_t col) const
__anon4eeb3e6e0402(int32_t index) 355 {
356 // Caller guarantee row and col in range of [0, 3].
357 return matrix4x4_[row][col];
358 }
359
360 double Matrix4::Determinant() const
__anon4eeb3e6e0502(int32_t index) 361 {
362 if (this->IsIdentityMatrix()) {
363 return 1.0;
364 }
365
366 double m00 = matrix4x4_[0][0];
367 double m01 = matrix4x4_[0][1];
368 double m02 = matrix4x4_[0][2];
369 double m03 = matrix4x4_[0][3];
370 double m10 = matrix4x4_[1][0];
371 double m11 = matrix4x4_[1][1];
372 double m12 = matrix4x4_[1][2];
373 double m13 = matrix4x4_[1][3];
374 double m20 = matrix4x4_[2][0];
375 double m21 = matrix4x4_[2][1];
376 double m22 = matrix4x4_[2][2];
377 double m23 = matrix4x4_[2][3];
378 double m30 = matrix4x4_[3][0];
379 double m31 = matrix4x4_[3][1];
380 double m32 = matrix4x4_[3][2];
381 double m33 = matrix4x4_[3][3];
382
383 double b00 = m00 * m11 - m01 * m10;
384 double b01 = m00 * m12 - m02 * m10;
385 double b02 = m00 * m13 - m03 * m10;
386 double b03 = m01 * m12 - m02 * m11;
387 double b04 = m01 * m13 - m03 * m11;
388 double b05 = m02 * m13 - m03 * m12;
389 double b06 = m20 * m31 - m21 * m30;
390 double b07 = m20 * m32 - m22 * m30;
391 double b08 = m20 * m33 - m23 * m30;
392 double b09 = m21 * m32 - m22 * m31;
393 double b10 = m21 * m33 - m23 * m31;
394 double b11 = m22 * m33 - m23 * m32;
395
396 return b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
397 }
398
399 void Matrix4::Transpose()
400 {
401 std::swap(matrix4x4_[0][1], matrix4x4_[1][0]);
402 std::swap(matrix4x4_[0][2], matrix4x4_[2][0]);
403 std::swap(matrix4x4_[0][3], matrix4x4_[3][0]);
404 std::swap(matrix4x4_[1][2], matrix4x4_[2][1]);
405 std::swap(matrix4x4_[1][3], matrix4x4_[3][1]);
406 std::swap(matrix4x4_[2][3], matrix4x4_[3][2]);
407 }
408
MapScalars(const double src[DIMENSION],double dst[DIMENSION]) const409 void Matrix4::MapScalars(const double src[DIMENSION], double dst[DIMENSION]) const
410 {
411 double storage[DIMENSION];
412
413 double* result = (src == dst) ? storage : dst;
414
415 for (int i = 0; i < DIMENSION; i++) {
416 double value = 0;
417 for (int j = 0; j < DIMENSION; j++) {
418 value += matrix4x4_[j][i] * src[j];
419 }
420 result[i] = value;
421 }
422
423 if (storage == result) {
424 std::copy_n(result, DIMENSION, dst);
425 }
426 }
427
ToString() const428 std::string Matrix4::ToString() const
429 {
430 std::string out;
431 for (auto& i : matrix4x4_) {
432 for (double j : i) {
433 out += std::to_string(j);
434 out += ",";
435 }
436 out += "\n";
437 }
438 return out;
439 }
440
Matrix4N(int32_t columns)441 Matrix4N::Matrix4N(int32_t columns) : columns_(columns)
442 {
443 matrix4n_.resize(DIMENSION, std::vector<double>(columns_, 0));
444 }
445
SetEntry(int32_t row,int32_t col,double value)446 bool Matrix4N::SetEntry(int32_t row, int32_t col, double value)
447 {
448 if (row < 0 || row >= DIMENSION || col < 0 || col >= columns_) {
449 return false;
450 }
451 matrix4n_[row][col] = value;
452 return true;
453 }
454
operator *(const MatrixN4 & matrix) const455 Matrix4 Matrix4N::operator*(const MatrixN4& matrix) const
456 {
457 auto matrix4 = Matrix4::CreateIdentity();
458 if (columns_ != matrix.GetRowNum()) {
459 return matrix4;
460 }
461 for (auto i = 0; i < DIMENSION; i++) {
462 for (auto j = 0; j < DIMENSION; j++) {
463 double value = 0.0;
464 for (auto k = 0; k < columns_; k++) {
465 value += matrix4n_[i][k] * matrix[k][j];
466 }
467 matrix4.SetEntry(i, j, value);
468 }
469 }
470 return matrix4;
471 }
472
Transpose() const473 MatrixN4 Matrix4N::Transpose() const
474 {
475 MatrixN4 matrix { columns_ };
476 for (auto i = 0; i < DIMENSION; i++) {
477 for (auto j = 0; j < columns_; j++) {
478 matrix[j][i] = matrix4n_[i][j];
479 }
480 }
481 return matrix;
482 }
483
MapScalars(const std::vector<double> & src) const484 std::vector<double> Matrix4N::MapScalars(const std::vector<double>& src) const
485 {
486 std::vector<double> value(DIMENSION, 0);
487 if (static_cast<int32_t>(src.size()) != columns_) {
488 return value;
489 }
490 for (int32_t i = 0; i < DIMENSION; i++) {
491 double item = 0.0;
492 for (int32_t j = 0; j < columns_; j++) {
493 item = item + matrix4n_[i][j] * src[j];
494 }
495 value[i] = item;
496 }
497 return value;
498 }
499
MapScalars(const std::vector<double> & src,std::vector<double> & result) const500 bool Matrix4N::MapScalars(const std::vector<double>& src, std::vector<double>& result) const
501 {
502 if (static_cast<int32_t>(src.size()) != columns_) {
503 return false;
504 }
505 result.resize(DIMENSION, 0);
506 for (int32_t i = 0; i < DIMENSION; i++) {
507 double item = 0.0;
508 for (int32_t j = 0; j < columns_; j++) {
509 item = item + matrix4n_[i][j] * src[j];
510 }
511 result[i] = item;
512 }
513 return true;
514 }
515
MatrixN4(int32_t rows)516 MatrixN4::MatrixN4(int32_t rows) : rows_(rows)
517 {
518 matrixn4_.resize(rows, std::vector<double>(DIMENSION, 0));
519 }
520
SetEntry(int32_t row,int32_t col,double value)521 bool MatrixN4::SetEntry(int32_t row, int32_t col, double value)
522 {
523 if (row < 0 || row >= rows_ || col < 0 || col >= DIMENSION) {
524 return false;
525 }
526 matrixn4_[row][col] = value;
527 return true;
528 }
529
Transpose() const530 Matrix4N MatrixN4::Transpose() const
531 {
532 Matrix4N matrix { rows_ };
533 for (auto i = 0; i < DIMENSION; i++) {
534 for (auto j = 0; j < rows_; j++) {
535 matrix[i][j] = matrixn4_[j][i];
536 }
537 }
538 return matrix;
539 }
540
MapScalars(const std::vector<double> & src) const541 std::vector<double> MatrixN4::MapScalars(const std::vector<double>& src) const
542 {
543 std::vector<double> value(rows_, 0);
544 if (static_cast<int32_t>(src.size()) != DIMENSION) {
545 return value;
546 }
547 for (int32_t i = 0; i < rows_; i++) {
548 double item = 0.0;
549 for (int32_t j = 0; j < DIMENSION; j++) {
550 item = item + matrixn4_[i][j] * src[j];
551 }
552 value[i] = item;
553 }
554 return value;
555 }
556 } // namespace OHOS::Ace
557