1 // Copyright (c) 2013 The Chromium Authors. All rights reserved.
2 // Use of this source code is governed by a BSD-style license that can be
3 // found in the LICENSE file.
4
5 #include "ui/gfx/geometry/matrix3_f.h"
6
7 #include <algorithm>
8 #include <cmath>
9 #include <limits>
10
11 #include "base/numerics/math_constants.h"
12 #include "base/strings/stringprintf.h"
13
14 namespace {
15
16 // This is only to make accessing indices self-explanatory.
17 enum MatrixCoordinates {
18 M00,
19 M01,
20 M02,
21 M10,
22 M11,
23 M12,
24 M20,
25 M21,
26 M22,
27 M_END
28 };
29
30 template<typename T>
Determinant3x3(T data[M_END])31 double Determinant3x3(T data[M_END]) {
32 // This routine is separated from the Matrix3F::Determinant because in
33 // computing inverse we do want higher precision afforded by the explicit
34 // use of 'double'.
35 return
36 static_cast<double>(data[M00]) * (
37 static_cast<double>(data[M11]) * data[M22] -
38 static_cast<double>(data[M12]) * data[M21]) +
39 static_cast<double>(data[M01]) * (
40 static_cast<double>(data[M12]) * data[M20] -
41 static_cast<double>(data[M10]) * data[M22]) +
42 static_cast<double>(data[M02]) * (
43 static_cast<double>(data[M10]) * data[M21] -
44 static_cast<double>(data[M11]) * data[M20]);
45 }
46
47 } // namespace
48
49 namespace gfx {
50
Matrix3F()51 Matrix3F::Matrix3F() {
52 }
53
~Matrix3F()54 Matrix3F::~Matrix3F() {
55 }
56
57 // static
Zeros()58 Matrix3F Matrix3F::Zeros() {
59 Matrix3F matrix;
60 matrix.set(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
61 return matrix;
62 }
63
64 // static
Ones()65 Matrix3F Matrix3F::Ones() {
66 Matrix3F matrix;
67 matrix.set(1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f);
68 return matrix;
69 }
70
71 // static
Identity()72 Matrix3F Matrix3F::Identity() {
73 Matrix3F matrix;
74 matrix.set(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
75 return matrix;
76 }
77
78 // static
FromOuterProduct(const Vector3dF & a,const Vector3dF & bt)79 Matrix3F Matrix3F::FromOuterProduct(const Vector3dF& a, const Vector3dF& bt) {
80 Matrix3F matrix;
81 matrix.set(a.x() * bt.x(), a.x() * bt.y(), a.x() * bt.z(),
82 a.y() * bt.x(), a.y() * bt.y(), a.y() * bt.z(),
83 a.z() * bt.x(), a.z() * bt.y(), a.z() * bt.z());
84 return matrix;
85 }
86
IsEqual(const Matrix3F & rhs) const87 bool Matrix3F::IsEqual(const Matrix3F& rhs) const {
88 return 0 == memcmp(data_, rhs.data_, sizeof(data_));
89 }
90
IsNear(const Matrix3F & rhs,float precision) const91 bool Matrix3F::IsNear(const Matrix3F& rhs, float precision) const {
92 DCHECK(precision >= 0);
93 for (int i = 0; i < M_END; ++i) {
94 if (std::abs(data_[i] - rhs.data_[i]) > precision)
95 return false;
96 }
97 return true;
98 }
99
Add(const Matrix3F & rhs) const100 Matrix3F Matrix3F::Add(const Matrix3F& rhs) const {
101 Matrix3F result;
102 for (int i = 0; i < M_END; ++i)
103 result.data_[i] = data_[i] + rhs.data_[i];
104 return result;
105 }
106
Subtract(const Matrix3F & rhs) const107 Matrix3F Matrix3F::Subtract(const Matrix3F& rhs) const {
108 Matrix3F result;
109 for (int i = 0; i < M_END; ++i)
110 result.data_[i] = data_[i] - rhs.data_[i];
111 return result;
112 }
113
Inverse() const114 Matrix3F Matrix3F::Inverse() const {
115 Matrix3F inverse = Matrix3F::Zeros();
116 double determinant = Determinant3x3(data_);
117 if (std::numeric_limits<float>::epsilon() > std::abs(determinant))
118 return inverse; // Singular matrix. Return Zeros().
119
120 inverse.set(
121 static_cast<float>((data_[M11] * data_[M22] - data_[M12] * data_[M21]) /
122 determinant),
123 static_cast<float>((data_[M02] * data_[M21] - data_[M01] * data_[M22]) /
124 determinant),
125 static_cast<float>((data_[M01] * data_[M12] - data_[M02] * data_[M11]) /
126 determinant),
127 static_cast<float>((data_[M12] * data_[M20] - data_[M10] * data_[M22]) /
128 determinant),
129 static_cast<float>((data_[M00] * data_[M22] - data_[M02] * data_[M20]) /
130 determinant),
131 static_cast<float>((data_[M02] * data_[M10] - data_[M00] * data_[M12]) /
132 determinant),
133 static_cast<float>((data_[M10] * data_[M21] - data_[M11] * data_[M20]) /
134 determinant),
135 static_cast<float>((data_[M01] * data_[M20] - data_[M00] * data_[M21]) /
136 determinant),
137 static_cast<float>((data_[M00] * data_[M11] - data_[M01] * data_[M10]) /
138 determinant));
139 return inverse;
140 }
141
Transpose() const142 Matrix3F Matrix3F::Transpose() const {
143 Matrix3F transpose;
144 transpose.set(data_[M00], data_[M10], data_[M20], data_[M01], data_[M11],
145 data_[M21], data_[M02], data_[M12], data_[M22]);
146 return transpose;
147 }
148
Determinant() const149 float Matrix3F::Determinant() const {
150 return static_cast<float>(Determinant3x3(data_));
151 }
152
SolveEigenproblem(Matrix3F * eigenvectors) const153 Vector3dF Matrix3F::SolveEigenproblem(Matrix3F* eigenvectors) const {
154 // The matrix must be symmetric.
155 const float epsilon = std::numeric_limits<float>::epsilon();
156 if (std::abs(data_[M01] - data_[M10]) > epsilon ||
157 std::abs(data_[M02] - data_[M20]) > epsilon ||
158 std::abs(data_[M12] - data_[M21]) > epsilon) {
159 NOTREACHED();
160 return Vector3dF();
161 }
162
163 float eigenvalues[3];
164 float p =
165 data_[M01] * data_[M01] +
166 data_[M02] * data_[M02] +
167 data_[M12] * data_[M12];
168
169 bool diagonal = std::abs(p) < epsilon;
170 if (diagonal) {
171 eigenvalues[0] = data_[M00];
172 eigenvalues[1] = data_[M11];
173 eigenvalues[2] = data_[M22];
174 } else {
175 float q = Trace() / 3.0f;
176 p = (data_[M00] - q) * (data_[M00] - q) +
177 (data_[M11] - q) * (data_[M11] - q) +
178 (data_[M22] - q) * (data_[M22] - q) +
179 2 * p;
180 p = std::sqrt(p / 6);
181
182 // The computation below puts B as (A - qI) / p, where A is *this.
183 Matrix3F matrix_b(*this);
184 matrix_b.data_[M00] -= q;
185 matrix_b.data_[M11] -= q;
186 matrix_b.data_[M22] -= q;
187 for (int i = 0; i < M_END; ++i)
188 matrix_b.data_[i] /= p;
189
190 double half_det_b = Determinant3x3(matrix_b.data_) / 2.0;
191 // half_det_b should be in <-1, 1>, but beware of rounding error.
192 double phi = 0.0f;
193 if (half_det_b <= -1.0)
194 phi = base::kPiDouble / 3;
195 else if (half_det_b < 1.0)
196 phi = acos(half_det_b) / 3;
197
198 eigenvalues[0] = q + 2 * p * static_cast<float>(cos(phi));
199 eigenvalues[2] =
200 q + 2 * p * static_cast<float>(cos(phi + 2.0 * base::kPiDouble / 3.0));
201 eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2];
202 }
203
204 // Put eigenvalues in the descending order.
205 int indices[3] = {0, 1, 2};
206 if (eigenvalues[2] > eigenvalues[1]) {
207 std::swap(eigenvalues[2], eigenvalues[1]);
208 std::swap(indices[2], indices[1]);
209 }
210
211 if (eigenvalues[1] > eigenvalues[0]) {
212 std::swap(eigenvalues[1], eigenvalues[0]);
213 std::swap(indices[1], indices[0]);
214 }
215
216 if (eigenvalues[2] > eigenvalues[1]) {
217 std::swap(eigenvalues[2], eigenvalues[1]);
218 std::swap(indices[2], indices[1]);
219 }
220
221 if (eigenvectors != NULL && diagonal) {
222 // Eigenvectors are e-vectors, just need to be sorted accordingly.
223 *eigenvectors = Zeros();
224 for (int i = 0; i < 3; ++i)
225 eigenvectors->set(indices[i], i, 1.0f);
226 } else if (eigenvectors != NULL) {
227 // Consult the following for a detailed discussion:
228 // Joachim Kopp
229 // Numerical diagonalization of hermitian 3x3 matrices
230 // arXiv.org preprint: physics/0610206
231 // Int. J. Mod. Phys. C19 (2008) 523-548
232
233 // TODO(motek): expand to handle correctly negative and multiple
234 // eigenvalues.
235 for (int i = 0; i < 3; ++i) {
236 float l = eigenvalues[i];
237 // B = A - l * I
238 Matrix3F matrix_b(*this);
239 matrix_b.data_[M00] -= l;
240 matrix_b.data_[M11] -= l;
241 matrix_b.data_[M22] -= l;
242 Vector3dF e1 = CrossProduct(matrix_b.get_column(0),
243 matrix_b.get_column(1));
244 Vector3dF e2 = CrossProduct(matrix_b.get_column(1),
245 matrix_b.get_column(2));
246 Vector3dF e3 = CrossProduct(matrix_b.get_column(2),
247 matrix_b.get_column(0));
248
249 // e1, e2 and e3 should point in the same direction.
250 if (DotProduct(e1, e2) < 0)
251 e2 = -e2;
252
253 if (DotProduct(e1, e3) < 0)
254 e3 = -e3;
255
256 Vector3dF eigvec = e1 + e2 + e3;
257 // Normalize.
258 eigvec.Scale(1.0f / eigvec.Length());
259 eigenvectors->set_column(i, eigvec);
260 }
261 }
262
263 return Vector3dF(eigenvalues[0], eigenvalues[1], eigenvalues[2]);
264 }
265
MatrixProduct(const Matrix3F & lhs,const Matrix3F & rhs)266 Matrix3F MatrixProduct(const Matrix3F& lhs, const Matrix3F& rhs) {
267 Matrix3F result = Matrix3F::Zeros();
268 for (int i = 0; i < 3; i++) {
269 for (int j = 0; j < 3; j++) {
270 result.set(i, j, DotProduct(lhs.get_row(i), rhs.get_column(j)));
271 }
272 }
273 return result;
274 }
275
MatrixProduct(const Matrix3F & lhs,const Vector3dF & rhs)276 Vector3dF MatrixProduct(const Matrix3F& lhs, const Vector3dF& rhs) {
277 return Vector3dF(DotProduct(lhs.get_row(0), rhs),
278 DotProduct(lhs.get_row(1), rhs),
279 DotProduct(lhs.get_row(2), rhs));
280 }
281
ToString() const282 std::string Matrix3F::ToString() const {
283 return base::StringPrintf(
284 "[[%+0.4f, %+0.4f, %+0.4f],"
285 " [%+0.4f, %+0.4f, %+0.4f],"
286 " [%+0.4f, %+0.4f, %+0.4f]]",
287 data_[M00], data_[M01], data_[M02], data_[M10], data_[M11], data_[M12],
288 data_[M20], data_[M21], data_[M22]);
289 }
290
291 } // namespace gfx
292