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
18namespace OHOS::Ace {
19namespace {
20constexpr int32_t MATRIX_LENGTH = Matrix4::DIMENSION * Matrix4::DIMENSION;
21constexpr double ANGLE_UNIT = 0.017453f; // PI / 180
22
23inline 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
31Matrix4 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
36Matrix4 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
41Matrix4 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
46Matrix4 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
68Matrix4 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
73Matrix4 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
79Matrix4 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
85Matrix4 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
91Matrix4 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
100Matrix4 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
114Matrix4 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
132Matrix4::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
136Matrix4::Matrix4(const Matrix4& matrix)
137{
138    std::copy_n(&matrix.matrix4x4_[0][0], MATRIX_LENGTH, &matrix4x4_[0][0]);
139}
140
141Matrix4::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
162void 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
171double Matrix4::GetScaleX() const
172{
173    return matrix4x4_[0][0];
174}
175
176double Matrix4::GetScaleY() const
177{
178    return matrix4x4_[1][1];
179}
180
181void 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
189bool Matrix4::IsIdentityMatrix() const
190{
191    return *this == CreateIdentity();
192}
193
194void 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
200int32_t Matrix4::Count() const
201{
202    return MATRIX_LENGTH;
203}
204
205Matrix4 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
258bool 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
263Matrix4 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
274Matrix4 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
311Matrix4N 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
327Point 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
335Matrix4& 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
344double 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
354double Matrix4::operator()(int32_t row, int32_t col) const
355{
356    // Caller guarantee row and col in range of [0, 3].
357    return matrix4x4_[row][col];
358}
359
360double Matrix4::Determinant() const
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
399void 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
409void 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
428std::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
441Matrix4N::Matrix4N(int32_t columns) : columns_(columns)
442{
443    matrix4n_.resize(DIMENSION, std::vector<double>(columns_, 0));
444}
445
446bool 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
455Matrix4 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
473MatrixN4 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
484std::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
500bool 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
516MatrixN4::MatrixN4(int32_t rows) : rows_(rows)
517{
518    matrixn4_.resize(rows, std::vector<double>(DIMENSION, 0));
519}
520
521bool 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
530Matrix4N 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
541std::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