1 /*
2 * Copyright (c) 2021 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 #include <algorithm>
19 #include <cmath>
20
21 #include "base/utils/utils.h"
22
23 namespace OHOS::Ace {
24 namespace {
25 constexpr int32_t MATRIX_LENGTH = Matrix4::DIMENSION * Matrix4::DIMENSION;
26 constexpr double ANGLE_UNIT = 0.017453f; // PI / 180
27
IsEqual(const double & left,const double & right)28 inline bool IsEqual(const double& left, const double& right)
29 {
30 return NearEqual(left, right);
31 }
32
33 } // namespace
34
CreateIdentity()35 Matrix4 Matrix4::CreateIdentity()
36 {
37 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);
38 }
39
CreateTranslate(double x,double y,double z)40 Matrix4 Matrix4::CreateTranslate(double x, double y, double z)
41 {
42 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);
43 }
44
CreateScale(double x,double y,double z)45 Matrix4 Matrix4::CreateScale(double x, double y, double z)
46 {
47 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);
48 }
49
CreateRotate(double angle,double dx,double dy,double dz)50 Matrix4 Matrix4::CreateRotate(double angle, double dx, double dy, double dz)
51 {
52 // (x,y,z) need normalize
53 double sum = dx * dx + dy * dy + dz * dz;
54 if (NearZero(sum)) {
55 return Matrix4::CreateIdentity();
56 }
57
58 double x = dx / sqrt(sum);
59 double y = dy / sqrt(sum);
60 double z = dz / sqrt(sum);
61 double redian = static_cast<double>(angle * (M_PI / 180.0f));
62 double cosValue = cosf(redian);
63 double sinValue = sinf(redian);
64
65 return Matrix4(cosValue + (x * x * (1.0f - cosValue)), (x * y * (1.0f - cosValue)) - (z * sinValue),
66 (x * z * (1.0f - cosValue)) + (y * sinValue), 0.0f, (y * x * (1.0f - cosValue)) + (z * sinValue),
67 cosValue + (y * y * (1.0f - cosValue)), (y * z * (1.0f - cosValue)) - (x * sinValue), 0.0f,
68 (z * x * (1.0f - cosValue)) - (y * sinValue), (z * y * (1.0f - cosValue)) + (x * sinValue),
69 cosValue + (z * z * (1.0f - cosValue)), 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
70 }
71
CreateMatrix2D(double m00,double m10,double m01,double m11,double m03,double m13)72 Matrix4 Matrix4::CreateMatrix2D(double m00, double m10, double m01, double m11, double m03, double m13)
73 {
74 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);
75 }
76
CreateSkew(double x,double y)77 Matrix4 Matrix4::CreateSkew(double x, double y)
78 {
79 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,
80 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f);
81 }
82
CreatePerspective(double distance)83 Matrix4 Matrix4::CreatePerspective(double distance)
84 {
85 auto result = CreateIdentity();
86 if (GreatNotEqual(distance, 0.0f)) {
87 result.matrix4x4_[2][3] = -1.0f / distance;
88 }
89 return result;
90 }
91
Invert(const Matrix4 & matrix)92 Matrix4 Matrix4::Invert(const Matrix4& matrix)
93 {
94 Matrix4 inverted = CreateInvert(matrix);
95 double determinant = matrix(0, 0) * inverted(0, 0) + matrix(0, 1) * inverted(1, 0) + matrix(0, 2) * inverted(2, 0) +
96 matrix(0, 3) * inverted(3, 0);
97
98 if (!NearZero(determinant)) {
99 inverted = inverted * (1.0f / determinant);
100 } else {
101 inverted = CreateIdentity();
102 }
103
104 return inverted;
105 }
106
Matrix4()107 Matrix4::Matrix4()
108 : 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)
109 {}
110
Matrix4(const Matrix4 & matrix)111 Matrix4::Matrix4(const Matrix4& matrix)
112 {
113 std::copy_n(&matrix.matrix4x4_[0][0], MATRIX_LENGTH, &matrix4x4_[0][0]);
114 }
115
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)116 Matrix4::Matrix4(double m00, double m01, double m02, double m03, double m10, double m11, double m12, double m13,
117 double m20, double m21, double m22, double m23, double m30, double m31, double m32, double m33)
118 {
119 matrix4x4_[0][0] = m00;
120 matrix4x4_[1][0] = m01;
121 matrix4x4_[2][0] = m02;
122 matrix4x4_[3][0] = m03;
123 matrix4x4_[0][1] = m10;
124 matrix4x4_[1][1] = m11;
125 matrix4x4_[2][1] = m12;
126 matrix4x4_[3][1] = m13;
127 matrix4x4_[0][2] = m20;
128 matrix4x4_[1][2] = m21;
129 matrix4x4_[2][2] = m22;
130 matrix4x4_[3][2] = m23;
131 matrix4x4_[0][3] = m30;
132 matrix4x4_[1][3] = m31;
133 matrix4x4_[2][3] = m32;
134 matrix4x4_[3][3] = m33;
135 }
136
SetScale(double x,double y,double z)137 void Matrix4::SetScale(double x, double y, double z)
138 {
139 // The 4X4 matrix scale index is [0][0], [1][1], [2][2], [3][3].
140 matrix4x4_[0][0] = x;
141 matrix4x4_[1][1] = y;
142 matrix4x4_[2][2] = z;
143 matrix4x4_[3][3] = 1.0f;
144 }
145
GetScaleX() const146 double Matrix4::GetScaleX() const
147 {
148 return matrix4x4_[0][0];
149 }
150
GetScaleY() const151 double Matrix4::GetScaleY() const
152 {
153 return matrix4x4_[1][1];
154 }
155
SetEntry(int32_t row,int32_t col,double value)156 void Matrix4::SetEntry(int32_t row, int32_t col, double value)
157 {
158 if ((row < 0 || row >= DIMENSION) || (col < 0 || col >= DIMENSION)) {
159 return;
160 }
161 matrix4x4_[row][col] = value;
162 }
163
IsIdentityMatrix() const164 bool Matrix4::IsIdentityMatrix() const
165 {
166 return *this == CreateIdentity();
167 }
168
Rotate(double angle,double dx,double dy,double dz)169 void Matrix4::Rotate(double angle, double dx, double dy, double dz)
170 {
171 Matrix4 transform = *this;
172 *this = transform * CreateRotate(angle, dx, dy, dz);
173 }
174
Count() const175 int32_t Matrix4::Count() const
176 {
177 return MATRIX_LENGTH;
178 }
179
CreateInvert(const Matrix4 & matrix)180 Matrix4 Matrix4::CreateInvert(const Matrix4& matrix)
181 {
182 return Matrix4(
183 matrix(1, 1) * matrix(2, 2) * matrix(3, 3) - matrix(1, 1) * matrix(2, 3) * matrix(3, 2) -
184 matrix(2, 1) * matrix(1, 2) * matrix(3, 3) + matrix(2, 1) * matrix(1, 3) * matrix(3, 2) +
185 matrix(3, 1) * matrix(1, 2) * matrix(2, 3) - matrix(3, 1) * matrix(1, 3) * matrix(2, 2),
186 -matrix(1, 0) * matrix(2, 2) * matrix(3, 3) + matrix(1, 0) * matrix(2, 3) * matrix(3, 2) +
187 matrix(2, 0) * matrix(1, 2) * matrix(3, 3) - matrix(2, 0) * matrix(1, 3) * matrix(3, 2) -
188 matrix(3, 0) * matrix(1, 2) * matrix(2, 3) + matrix(3, 0) * matrix(1, 3) * matrix(2, 2),
189 matrix(1, 0) * matrix(2, 1) * matrix(3, 3) - matrix(1, 0) * matrix(2, 3) * matrix(3, 1) -
190 matrix(2, 0) * matrix(1, 1) * matrix(3, 3) + matrix(2, 0) * matrix(1, 3) * matrix(3, 1) +
191 matrix(3, 0) * matrix(1, 1) * matrix(2, 3) - matrix(3, 0) * matrix(1, 3) * matrix(2, 1),
192 -matrix(1, 0) * matrix(2, 1) * matrix(3, 2) + matrix(1, 0) * matrix(2, 2) * matrix(3, 1) +
193 matrix(2, 0) * matrix(1, 1) * matrix(3, 2) - matrix(2, 0) * matrix(1, 2) * matrix(3, 1) -
194 matrix(3, 0) * matrix(1, 1) * matrix(2, 2) + matrix(3, 0) * matrix(1, 2) * matrix(2, 1),
195 -matrix(0, 1) * matrix(2, 2) * matrix(3, 3) + matrix(0, 1) * matrix(2, 3) * matrix(3, 2) +
196 matrix(2, 1) * matrix(0, 2) * matrix(3, 3) - matrix(2, 1) * matrix(0, 3) * matrix(3, 2) -
197 matrix(3, 1) * matrix(0, 2) * matrix(2, 3) + matrix(3, 1) * matrix(0, 3) * matrix(2, 2),
198 matrix(0, 0) * matrix(2, 2) * matrix(3, 3) - matrix(0, 0) * matrix(2, 3) * matrix(3, 2) -
199 matrix(2, 0) * matrix(0, 2) * matrix(3, 3) + matrix(2, 0) * matrix(0, 3) * matrix(3, 2) +
200 matrix(3, 0) * matrix(0, 2) * matrix(2, 3) - matrix(3, 0) * matrix(0, 3) * matrix(2, 2),
201 -matrix(0, 0) * matrix(2, 1) * matrix(3, 3) + matrix(0, 0) * matrix(2, 3) * matrix(3, 1) +
202 matrix(2, 0) * matrix(0, 1) * matrix(3, 3) - matrix(2, 0) * matrix(0, 3) * matrix(3, 1) -
203 matrix(3, 0) * matrix(0, 1) * matrix(2, 3) + matrix(3, 0) * matrix(0, 3) * matrix(2, 1),
204 matrix(0, 0) * matrix(2, 1) * matrix(3, 2) - matrix(0, 0) * matrix(2, 2) * matrix(3, 1) -
205 matrix(2, 0) * matrix(0, 1) * matrix(3, 2) + matrix(2, 0) * matrix(0, 2) * matrix(3, 1) +
206 matrix(3, 0) * matrix(0, 1) * matrix(2, 2) - matrix(3, 0) * matrix(0, 2) * matrix(2, 1),
207 matrix(0, 1) * matrix(1, 2) * matrix(3, 3) - matrix(0, 1) * matrix(1, 3) * matrix(3, 2) -
208 matrix(1, 1) * matrix(0, 2) * matrix(3, 3) + matrix(1, 1) * matrix(0, 3) * matrix(3, 2) +
209 matrix(3, 1) * matrix(0, 2) * matrix(1, 3) - matrix(3, 1) * matrix(0, 3) * matrix(1, 2),
210 -matrix(0, 0) * matrix(1, 2) * matrix(3, 3) + matrix(0, 0) * matrix(1, 3) * matrix(3, 2) +
211 matrix(1, 0) * matrix(0, 2) * matrix(3, 3) - matrix(1, 0) * matrix(0, 3) * matrix(3, 2) -
212 matrix(3, 0) * matrix(0, 2) * matrix(1, 3) + matrix(3, 0) * matrix(0, 3) * matrix(1, 2),
213 matrix(0, 0) * matrix(1, 1) * matrix(3, 3) - matrix(0, 0) * matrix(1, 3) * matrix(3, 1) -
214 matrix(1, 0) * matrix(0, 1) * matrix(3, 3) + matrix(1, 0) * matrix(0, 3) * matrix(3, 1) +
215 matrix(3, 0) * matrix(0, 1) * matrix(1, 3) - matrix(3, 0) * matrix(0, 3) * matrix(1, 1),
216 -matrix(0, 0) * matrix(1, 1) * matrix(3, 2) + matrix(0, 0) * matrix(1, 2) * matrix(3, 1) +
217 matrix(1, 0) * matrix(0, 1) * matrix(3, 2) - matrix(1, 0) * matrix(0, 2) * matrix(3, 1) -
218 matrix(3, 0) * matrix(0, 1) * matrix(1, 2) + matrix(3, 0) * matrix(0, 2) * matrix(1, 1),
219 -matrix(0, 1) * matrix(1, 2) * matrix(2, 3) + matrix(0, 1) * matrix(1, 3) * matrix(2, 2) +
220 matrix(1, 1) * matrix(0, 2) * matrix(2, 3) - matrix(1, 1) * matrix(0, 3) * matrix(2, 2) -
221 matrix(2, 1) * matrix(0, 2) * matrix(1, 3) + matrix(2, 1) * matrix(0, 3) * matrix(1, 2),
222 matrix(0, 0) * matrix(1, 2) * matrix(2, 3) - matrix(0, 0) * matrix(1, 3) * matrix(2, 2) -
223 matrix(1, 0) * matrix(0, 2) * matrix(2, 3) + matrix(1, 0) * matrix(0, 3) * matrix(2, 2) +
224 matrix(2, 0) * matrix(0, 2) * matrix(1, 3) - matrix(2, 0) * matrix(0, 3) * matrix(1, 2),
225 -matrix(0, 0) * matrix(1, 1) * matrix(2, 3) + matrix(0, 0) * matrix(1, 3) * matrix(2, 1) +
226 matrix(1, 0) * matrix(0, 1) * matrix(2, 3) - matrix(1, 0) * matrix(0, 3) * matrix(2, 1) -
227 matrix(2, 0) * matrix(0, 1) * matrix(1, 3) + matrix(2, 0) * matrix(0, 3) * matrix(1, 1),
228 matrix(0, 0) * matrix(1, 1) * matrix(2, 2) - matrix(0, 0) * matrix(1, 2) * matrix(2, 1) -
229 matrix(1, 0) * matrix(0, 1) * matrix(2, 2) + matrix(1, 0) * matrix(0, 2) * matrix(2, 1) +
230 matrix(2, 0) * matrix(0, 1) * matrix(1, 2) - matrix(2, 0) * matrix(0, 2) * matrix(1, 1));
231 }
232
operator ==(const Matrix4 & matrix) const233 bool Matrix4::operator==(const Matrix4& matrix) const
234 {
235 return std::equal(&matrix4x4_[0][0], &matrix4x4_[0][0] + MATRIX_LENGTH, &matrix.matrix4x4_[0][0], IsEqual);
236 }
237
operator *(double num)238 Matrix4 Matrix4::operator*(double num)
239 {
240 Matrix4 ret(*this);
241 std::for_each_n(&ret.matrix4x4_[0][0], MATRIX_LENGTH, [num](double& v) { v *= num; });
242 return ret;
243 }
244
operator *(const Matrix4 & matrix)245 Matrix4 Matrix4::operator*(const Matrix4& matrix)
246 {
247 return Matrix4(
248 matrix4x4_[0][0] * matrix(0, 0) + matrix4x4_[1][0] * matrix(0, 1) + matrix4x4_[2][0] * matrix(0, 2) +
249 matrix4x4_[3][0] * matrix(0, 3),
250 matrix4x4_[0][0] * matrix(1, 0) + matrix4x4_[1][0] * matrix(1, 1) + matrix4x4_[2][0] * matrix(1, 2) +
251 matrix4x4_[3][0] * matrix(1, 3),
252 matrix4x4_[0][0] * matrix(2, 0) + matrix4x4_[1][0] * matrix(2, 1) + matrix4x4_[2][0] * matrix(2, 2) +
253 matrix4x4_[3][0] * matrix(2, 3),
254 matrix4x4_[0][0] * matrix(3, 0) + matrix4x4_[1][0] * matrix(3, 1) + matrix4x4_[2][0] * matrix(3, 2) +
255 matrix4x4_[3][0] * matrix(3, 3),
256 matrix4x4_[0][1] * matrix(0, 0) + matrix4x4_[1][1] * matrix(0, 1) + matrix4x4_[2][1] * matrix(0, 2) +
257 matrix4x4_[3][1] * matrix(0, 3),
258 matrix4x4_[0][1] * matrix(1, 0) + matrix4x4_[1][1] * matrix(1, 1) + matrix4x4_[2][1] * matrix(1, 2) +
259 matrix4x4_[3][1] * matrix(1, 3),
260 matrix4x4_[0][1] * matrix(2, 0) + matrix4x4_[1][1] * matrix(2, 1) + matrix4x4_[2][1] * matrix(2, 2) +
261 matrix4x4_[3][1] * matrix(2, 3),
262 matrix4x4_[0][1] * matrix(3, 0) + matrix4x4_[1][1] * matrix(3, 1) + matrix4x4_[2][1] * matrix(3, 2) +
263 matrix4x4_[3][1] * matrix(3, 3),
264 matrix4x4_[0][2] * matrix(0, 0) + matrix4x4_[1][2] * matrix(0, 1) + matrix4x4_[2][2] * matrix(0, 2) +
265 matrix4x4_[3][2] * matrix(0, 3),
266 matrix4x4_[0][2] * matrix(1, 0) + matrix4x4_[1][2] * matrix(1, 1) + matrix4x4_[2][2] * matrix(1, 2) +
267 matrix4x4_[3][2] * matrix(1, 3),
268 matrix4x4_[0][2] * matrix(2, 0) + matrix4x4_[1][2] * matrix(2, 1) + matrix4x4_[2][2] * matrix(2, 2) +
269 matrix4x4_[3][2] * matrix(2, 3),
270 matrix4x4_[0][2] * matrix(3, 0) + matrix4x4_[1][2] * matrix(3, 1) + matrix4x4_[2][2] * matrix(3, 2) +
271 matrix4x4_[3][2] * matrix(3, 3),
272 matrix4x4_[0][3] * matrix(0, 0) + matrix4x4_[1][3] * matrix(0, 1) + matrix4x4_[2][3] * matrix(0, 2) +
273 matrix4x4_[3][3] * matrix(0, 3),
274 matrix4x4_[0][3] * matrix(1, 0) + matrix4x4_[1][3] * matrix(1, 1) + matrix4x4_[2][3] * matrix(1, 2) +
275 matrix4x4_[3][3] * matrix(1, 3),
276 matrix4x4_[0][3] * matrix(2, 0) + matrix4x4_[1][3] * matrix(2, 1) + matrix4x4_[2][3] * matrix(2, 2) +
277 matrix4x4_[3][3] * matrix(2, 3),
278 matrix4x4_[0][3] * matrix(3, 0) + matrix4x4_[1][3] * matrix(3, 1) + matrix4x4_[2][3] * matrix(3, 2) +
279 matrix4x4_[3][3] * matrix(3, 3));
280 }
281
operator *(const Matrix4N & matrix) const282 Matrix4N Matrix4::operator*(const Matrix4N& matrix) const
283 {
284 int32_t columns = matrix.GetColNum();
285 Matrix4N matrix4n { columns };
286 for (auto i = 0; i < DIMENSION; i++) {
287 for (auto j = 0; j < columns; j++) {
288 double value = 0.0;
289 for (auto k = 0; k < DIMENSION; k++) {
290 value += matrix4x4_[i][k] * matrix[k][j];
291 }
292 matrix4n[i][j] = value;
293 }
294 }
295 return matrix4n;
296 }
297
operator *(const Point & point)298 Point Matrix4::operator*(const Point& point)
299 {
300 double x = point.GetX();
301 double y = point.GetY();
302 return Point(matrix4x4_[0][0] * x + matrix4x4_[1][0] * y + matrix4x4_[3][0],
303 matrix4x4_[0][1] * x + matrix4x4_[1][1] * y + matrix4x4_[3][1]);
304 }
305
operator =(const Matrix4 & matrix)306 Matrix4& Matrix4::operator=(const Matrix4& matrix)
307 {
308 if (this == &matrix) {
309 return *this;
310 }
311 std::copy_n(&matrix.matrix4x4_[0][0], MATRIX_LENGTH, &matrix4x4_[0][0]);
312 return *this;
313 }
314
operator [](int32_t index) const315 double Matrix4::operator[](int32_t index) const
316 {
317 if (index < 0 || index >= MATRIX_LENGTH) {
318 return 0.0f;
319 }
320 int32_t row = index / DIMENSION;
321 int32_t col = index % DIMENSION;
322 return matrix4x4_[row][col];
323 }
324
operator ()(int32_t row,int32_t col) const325 double Matrix4::operator()(int32_t row, int32_t col) const
326 {
327 // Caller guarantee row and col in range of [0, 3].
328 return matrix4x4_[row][col];
329 }
330
Determinant() const331 double Matrix4::Determinant() const
332 {
333 if (this->IsIdentityMatrix()) {
334 return 1.0;
335 }
336
337 double m00 = matrix4x4_[0][0];
338 double m01 = matrix4x4_[0][1];
339 double m02 = matrix4x4_[0][2];
340 double m03 = matrix4x4_[0][3];
341 double m10 = matrix4x4_[1][0];
342 double m11 = matrix4x4_[1][1];
343 double m12 = matrix4x4_[1][2];
344 double m13 = matrix4x4_[1][3];
345 double m20 = matrix4x4_[2][0];
346 double m21 = matrix4x4_[2][1];
347 double m22 = matrix4x4_[2][2];
348 double m23 = matrix4x4_[2][3];
349 double m30 = matrix4x4_[3][0];
350 double m31 = matrix4x4_[3][1];
351 double m32 = matrix4x4_[3][2];
352 double m33 = matrix4x4_[3][3];
353
354 double b00 = m00 * m11 - m01 * m10;
355 double b01 = m00 * m12 - m02 * m10;
356 double b02 = m00 * m13 - m03 * m10;
357 double b03 = m01 * m12 - m02 * m11;
358 double b04 = m01 * m13 - m03 * m11;
359 double b05 = m02 * m13 - m03 * m12;
360 double b06 = m20 * m31 - m21 * m30;
361 double b07 = m20 * m32 - m22 * m30;
362 double b08 = m20 * m33 - m23 * m30;
363 double b09 = m21 * m32 - m22 * m31;
364 double b10 = m21 * m33 - m23 * m31;
365 double b11 = m22 * m33 - m23 * m32;
366
367 return b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
368 }
369
Transpose()370 void Matrix4::Transpose()
371 {
372 std::swap(matrix4x4_[0][1], matrix4x4_[1][0]);
373 std::swap(matrix4x4_[0][2], matrix4x4_[2][0]);
374 std::swap(matrix4x4_[0][3], matrix4x4_[3][0]);
375 std::swap(matrix4x4_[1][2], matrix4x4_[2][1]);
376 std::swap(matrix4x4_[1][3], matrix4x4_[3][1]);
377 std::swap(matrix4x4_[2][3], matrix4x4_[3][2]);
378 }
379
MapScalars(const double src[DIMENSION],double dst[DIMENSION]) const380 void Matrix4::MapScalars(const double src[DIMENSION], double dst[DIMENSION]) const
381 {
382 double storage[DIMENSION];
383
384 double* result = (src == dst) ? storage : dst;
385
386 for (int i = 0; i < DIMENSION; i++) {
387 double value = 0;
388 for (int j = 0; j < DIMENSION; j++) {
389 value += matrix4x4_[j][i] * src[j];
390 }
391 result[i] = value;
392 }
393
394 if (storage == result) {
395 std::copy_n(result, DIMENSION, dst);
396 }
397 }
398
ToString() const399 std::string Matrix4::ToString() const
400 {
401 std::string out;
402 for (auto& i : matrix4x4_) {
403 for (double j : i) {
404 out += std::to_string(j);
405 out += ",";
406 }
407 out += "\n";
408 }
409 return out;
410 }
411
Matrix4N(int32_t columns)412 Matrix4N::Matrix4N(int32_t columns) : columns_(columns)
413 {
414 matrix4n_.resize(DIMENSION, std::vector<double>(columns_, 0));
415 }
416
SetEntry(int32_t row,int32_t col,double value)417 bool Matrix4N::SetEntry(int32_t row, int32_t col, double value)
418 {
419 if (row >= DIMENSION || col >= columns_) {
420 return false;
421 }
422 matrix4n_[row][col] = value;
423 return true;
424 }
425
operator *(const MatrixN4 & matrix) const426 Matrix4 Matrix4N::operator*(const MatrixN4& matrix) const
427 {
428 auto matrix4 = Matrix4::CreateIdentity();
429 if (columns_ != matrix.GetRowNum()) {
430 return matrix4;
431 }
432 for (auto i = 0; i < DIMENSION; i++) {
433 for (auto j = 0; j < DIMENSION; j++) {
434 double value = 0.0;
435 for (auto k = 0; k < columns_; k++) {
436 value += matrix4n_[i][k] * matrix[k][j];
437 }
438 matrix4.SetEntry(i, j, value);
439 }
440 }
441 return matrix4;
442 }
443
Transpose() const444 MatrixN4 Matrix4N::Transpose() const
445 {
446 MatrixN4 matrix { columns_ };
447 for (auto i = 0; i < DIMENSION; i++) {
448 for (auto j = 0; j < columns_; j++) {
449 matrix[j][i] = matrix4n_[i][j];
450 }
451 }
452 return matrix;
453 }
454
MapScalars(const std::vector<double> & src) const455 std::vector<double> Matrix4N::MapScalars(const std::vector<double>& src) const
456 {
457 std::vector<double> value { DIMENSION, 0 };
458 if (static_cast<int32_t>(src.size()) != columns_) {
459 return value;
460 }
461 for (int32_t i = 0; i < DIMENSION; i++) {
462 double item = 0.0;
463 for (int32_t j = 0; j < columns_; j++) {
464 item = item + matrix4n_[i][j] * src[j];
465 }
466 value[i] = item;
467 }
468 return value;
469 }
470
MapScalars(const std::vector<double> & src,std::vector<double> & result) const471 bool Matrix4N::MapScalars(const std::vector<double>& src, std::vector<double>& result) const
472 {
473 if (static_cast<int32_t>(src.size()) != columns_) {
474 return false;
475 }
476 result.resize(DIMENSION, 0);
477 for (int32_t i = 0; i < DIMENSION; i++) {
478 double item = 0.0;
479 for (int32_t j = 0; j < columns_; j++) {
480 item = item + matrix4n_[i][j] * src[j];
481 }
482 result[i] = item;
483 }
484 return true;
485 }
486
MatrixN4(int32_t rows)487 MatrixN4::MatrixN4(int32_t rows) : rows_(rows)
488 {
489 matrixn4_.resize(rows, std::vector<double>(DIMENSION, 0));
490 }
491
SetEntry(int32_t row,int32_t col,double value)492 bool MatrixN4::SetEntry(int32_t row, int32_t col, double value)
493 {
494 if (row >= rows_ || col >= DIMENSION) {
495 return false;
496 }
497 matrixn4_[row][col] = value;
498 return true;
499 }
500
Transpose() const501 Matrix4N MatrixN4::Transpose() const
502 {
503 Matrix4N matrix { rows_ };
504 for (auto i = 0; i < DIMENSION; i++) {
505 for (auto j = 0; j < rows_; j++) {
506 matrix[i][j] = matrixn4_[j][i];
507 }
508 }
509 return matrix;
510 }
511
MapScalars(const std::vector<double> & src) const512 std::vector<double> MatrixN4::MapScalars(const std::vector<double>& src) const
513 {
514 std::vector<double> value { rows_, 0 };
515 if (static_cast<int32_t>(src.size()) != DIMENSION) {
516 return value;
517 }
518 for (int32_t i = 0; i < rows_; i++) {
519 double item = 0.0;
520 for (int32_t j = 0; j < DIMENSION; j++) {
521 item = item + matrixn4_[i][j] * src[j];
522 }
523 value[i] = item;
524 }
525 return value;
526 }
527 } // namespace OHOS::Ace
528