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/matrix3.h"
17
18 namespace OHOS::Ace {
SetEntry(int32_t row,int32_t col,double value)19 void Matrix3::SetEntry(int32_t row, int32_t col, double value)
20 {
21 if ((row < 0 || row >= DIMENSION) || (col < 0 || col >= DIMENSION)) {
22 return;
23 }
24 matrix3X3_[row][col] = value;
25 }
26
Invert(Matrix3 & matrix) const27 bool Matrix3::Invert(Matrix3& matrix) const
28 {
29 static const double diff = 1e-20;
30 double val1 = matrix3X3_[0][0] * matrix3X3_[1][1] * matrix3X3_[2][2];
31 double val2 = matrix3X3_[0][0] * matrix3X3_[1][2] * matrix3X3_[2][1];
32 double val3 = matrix3X3_[1][0] * matrix3X3_[0][1] * matrix3X3_[2][2];
33 double val4 = matrix3X3_[1][0] * matrix3X3_[0][2] * matrix3X3_[2][1];
34 double val5 = matrix3X3_[2][0] * matrix3X3_[0][1] * matrix3X3_[1][2];
35 double val6 = matrix3X3_[2][0] * matrix3X3_[0][2] * matrix3X3_[1][1];
36 double detA = val1 - val2 - val3 + val4 + val5 - val6;
37 if (NearZero(detA, diff)) {
38 return false;
39 }
40 detA = 1.0 / detA;
41 // a11a22 - a12a21
42 matrix[0][0] = matrix3X3_[1][1] * matrix3X3_[2][2] - matrix3X3_[1][2] * matrix3X3_[2][1];
43 // a20a21 - a01a22
44 matrix[0][1] = matrix3X3_[0][2] * matrix3X3_[2][1] - matrix3X3_[0][1] * matrix3X3_[2][2];
45 // a01a12 - a02a11
46 matrix[0][2] = matrix3X3_[0][1] * matrix3X3_[1][2] - matrix3X3_[0][2] * matrix3X3_[1][1];
47 // a12a20 - a10a22
48 matrix[1][0] = matrix3X3_[1][2] * matrix3X3_[2][0] - matrix3X3_[1][0] * matrix3X3_[2][2];
49 // a00a22 - a02a20
50 matrix[1][1] = matrix3X3_[0][0] * matrix3X3_[2][2] - matrix3X3_[0][2] * matrix3X3_[2][0];
51 // a10a02 - a00a12
52 matrix[1][2] = matrix3X3_[1][0] * matrix3X3_[0][2] - matrix3X3_[0][0] * matrix3X3_[1][2];
53 // a10a21 - a11a20
54 matrix[2][0] = matrix3X3_[1][0] * matrix3X3_[2][1] - matrix3X3_[1][1] * matrix3X3_[2][0];
55 // a01a20 - a00a21
56 matrix[2][1] = matrix3X3_[0][1] * matrix3X3_[2][0] - matrix3X3_[0][0] * matrix3X3_[2][1];
57 // a00a11 - a10a01
58 matrix[2][2] = matrix3X3_[0][0] * matrix3X3_[1][1] - matrix3X3_[1][0] * matrix3X3_[0][1];
59 // invert
60 matrix* detA;
61 return true;
62 }
63
operator *(const Matrix3N & matrix) const64 Matrix3N Matrix3::operator*(const Matrix3N& matrix) const
65 {
66 int32_t columns = matrix.GetColNum();
67 Matrix3N Matrix3n { columns };
68 for (auto i = 0; i < DIMENSION; i++) {
69 for (auto j = 0; j < columns; j++) {
70 double value = 0.0;
71 for (auto k = 0; k < DIMENSION; k++) {
72 value += matrix3X3_[i][k] * matrix[k][j];
73 }
74 Matrix3n[i][j] = value;
75 }
76 }
77 return Matrix3n;
78 }
79
Transpose() const80 Matrix3 Matrix3::Transpose() const
81 {
82 Matrix3 matrix;
83 for (auto i = 0; i < DIMENSION; i++) {
84 for (auto j = 0; j < DIMENSION; j++) {
85 matrix[j][i] = matrix3X3_[i][j];
86 }
87 }
88 return matrix;
89 }
90
MapScalars(const std::vector<double> & src) const91 std::vector<double> Matrix3::MapScalars(const std::vector<double>& src) const
92 {
93 std::vector<double> value(DIMENSION, 0);
94 if (static_cast<int32_t>(src.size()) != DIMENSION) {
95 return value;
96 }
97 for (int32_t i = 0; i < DIMENSION; i++) {
98 double item = 0.0;
99 for (int32_t j = 0; j < DIMENSION; j++) {
100 item = item + matrix3X3_[i][j] * src[j];
101 }
102 value[i] = item;
103 }
104 return value;
105 }
106
MapScalars(const std::vector<double> & src,std::vector<double> & result) const107 bool Matrix3::MapScalars(const std::vector<double>& src, std::vector<double>& result) const
108 {
109 if (static_cast<int32_t>(src.size()) != DIMENSION) {
110 return false;
111 }
112 result.resize(DIMENSION, 0);
113 for (int32_t i = 0; i < DIMENSION; i++) {
114 double item = 0.0;
115 for (int32_t j = 0; j < DIMENSION; j++) {
116 item = item + matrix3X3_[i][j] * src[j];
117 }
118 result[i] = item;
119 }
120 return true;
121 }
122
Matrix3N(int32_t columns)123 Matrix3N::Matrix3N(int32_t columns) : columns_(columns)
124 {
125 Matrix3n_.resize(DIMENSION, std::vector<double>(columns_, 0));
126 }
127
SetEntry(int32_t row,int32_t col,double value)128 bool Matrix3N::SetEntry(int32_t row, int32_t col, double value)
129 {
130 if (row >= DIMENSION || col >= columns_) {
131 return false;
132 }
133 Matrix3n_[row][col] = value;
134 return true;
135 }
136
operator *(const MatrixN3 & matrix) const137 Matrix3 Matrix3N::operator*(const MatrixN3& matrix) const
138 {
139 Matrix3 Matrix3;
140 if (columns_ != matrix.GetRowNum()) {
141 return Matrix3;
142 }
143 for (auto i = 0; i < DIMENSION; i++) {
144 for (auto j = 0; j < DIMENSION; j++) {
145 double value = 0.0;
146 for (auto k = 0; k < columns_; k++) {
147 value += Matrix3n_[i][k] * matrix[k][j];
148 }
149 Matrix3[i][j] = value;
150 }
151 }
152 return Matrix3;
153 }
154
Transpose() const155 MatrixN3 Matrix3N::Transpose() const
156 {
157 MatrixN3 matrix { columns_ };
158 for (auto i = 0; i < DIMENSION; i++) {
159 for (auto j = 0; j < columns_; j++) {
160 matrix[j][i] = Matrix3n_[i][j];
161 }
162 }
163 return matrix;
164 }
165
MapScalars(const std::vector<double> & src) const166 std::vector<double> Matrix3N::MapScalars(const std::vector<double>& src) const
167 {
168 std::vector<double> value(DIMENSION, 0);
169 if (static_cast<int32_t>(src.size()) != columns_) {
170 return value;
171 }
172 for (int32_t i = 0; i < DIMENSION; i++) {
173 double item = 0.0;
174 for (int32_t j = 0; j < columns_; j++) {
175 item = item + Matrix3n_[i][j] * src[j];
176 }
177 value[i] = item;
178 }
179 return value;
180 }
181
MapScalars(const std::vector<double> & src,std::vector<double> & result) const182 bool Matrix3N::MapScalars(const std::vector<double>& src, std::vector<double>& result) const
183 {
184 if (static_cast<int32_t>(src.size()) != columns_) {
185 return false;
186 }
187 result.resize(DIMENSION, 0);
188 for (int32_t i = 0; i < DIMENSION; i++) {
189 double item = 0.0;
190 for (int32_t j = 0; j < columns_; j++) {
191 item = item + Matrix3n_[i][j] * src[j];
192 }
193 result[i] = item;
194 }
195 return true;
196 }
197
MatrixN3(int32_t rows)198 MatrixN3::MatrixN3(int32_t rows) : rows_(rows)
199 {
200 Matrixn3_.resize(rows, std::vector<double>(DIMENSION, 0));
201 }
202
SetEntry(int32_t row,int32_t col,double value)203 bool MatrixN3::SetEntry(int32_t row, int32_t col, double value)
204 {
205 if (row >= rows_ || col >= DIMENSION) {
206 return false;
207 }
208 Matrixn3_[row][col] = value;
209 return true;
210 }
211
Transpose() const212 Matrix3N MatrixN3::Transpose() const
213 {
214 Matrix3N matrix { rows_ };
215 for (auto i = 0; i < DIMENSION; i++) {
216 for (auto j = 0; j < rows_; j++) {
217 matrix[i][j] = Matrixn3_[j][i];
218 }
219 }
220 return matrix;
221 }
222
MapScalars(const std::vector<double> & src) const223 std::vector<double> MatrixN3::MapScalars(const std::vector<double>& src) const
224 {
225 std::vector<double> value(rows_, 0);
226 if (static_cast<int32_t>(src.size()) != DIMENSION) {
227 return value;
228 }
229 for (int32_t i = 0; i < rows_; i++) {
230 double item = 0.0;
231 for (int32_t j = 0; j < DIMENSION; j++) {
232 item = item + Matrixn3_[i][j] * src[j];
233 }
234 value[i] = item;
235 }
236 return value;
237 }
238 } // namespace OHOS::Ace
239