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