OLD | NEW |
| (Empty) |
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/matrix3_f.h" | |
6 | |
7 #include <algorithm> | |
8 #include <cmath> | |
9 #include <limits> | |
10 | |
11 #ifndef M_PI | |
12 #define M_PI 3.14159265358979323846 | |
13 #endif | |
14 | |
15 namespace { | |
16 | |
17 // This is only to make accessing indices self-explanatory. | |
18 enum MatrixCoordinates { | |
19 M00, | |
20 M01, | |
21 M02, | |
22 M10, | |
23 M11, | |
24 M12, | |
25 M20, | |
26 M21, | |
27 M22, | |
28 M_END | |
29 }; | |
30 | |
31 template<typename T> | |
32 double Determinant3x3(T data[M_END]) { | |
33 // This routine is separated from the Matrix3F::Determinant because in | |
34 // computing inverse we do want higher precision afforded by the explicit | |
35 // use of 'double'. | |
36 return | |
37 static_cast<double>(data[M00]) * ( | |
38 static_cast<double>(data[M11]) * data[M22] - | |
39 static_cast<double>(data[M12]) * data[M21]) + | |
40 static_cast<double>(data[M01]) * ( | |
41 static_cast<double>(data[M12]) * data[M20] - | |
42 static_cast<double>(data[M10]) * data[M22]) + | |
43 static_cast<double>(data[M02]) * ( | |
44 static_cast<double>(data[M10]) * data[M21] - | |
45 static_cast<double>(data[M11]) * data[M20]); | |
46 } | |
47 | |
48 } // namespace | |
49 | |
50 namespace gfx { | |
51 | |
52 Matrix3F::Matrix3F() { | |
53 } | |
54 | |
55 Matrix3F::~Matrix3F() { | |
56 } | |
57 | |
58 // static | |
59 Matrix3F Matrix3F::Zeros() { | |
60 Matrix3F matrix; | |
61 matrix.set(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); | |
62 return matrix; | |
63 } | |
64 | |
65 // static | |
66 Matrix3F Matrix3F::Ones() { | |
67 Matrix3F matrix; | |
68 matrix.set(1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f); | |
69 return matrix; | |
70 } | |
71 | |
72 // static | |
73 Matrix3F Matrix3F::Identity() { | |
74 Matrix3F matrix; | |
75 matrix.set(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f); | |
76 return matrix; | |
77 } | |
78 | |
79 // static | |
80 Matrix3F Matrix3F::FromOuterProduct(const Vector3dF& a, const Vector3dF& bt) { | |
81 Matrix3F matrix; | |
82 matrix.set(a.x() * bt.x(), a.x() * bt.y(), a.x() * bt.z(), | |
83 a.y() * bt.x(), a.y() * bt.y(), a.y() * bt.z(), | |
84 a.z() * bt.x(), a.z() * bt.y(), a.z() * bt.z()); | |
85 return matrix; | |
86 } | |
87 | |
88 bool Matrix3F::IsEqual(const Matrix3F& rhs) const { | |
89 return 0 == memcmp(data_, rhs.data_, sizeof(data_)); | |
90 } | |
91 | |
92 bool Matrix3F::IsNear(const Matrix3F& rhs, float precision) const { | |
93 DCHECK(precision >= 0); | |
94 for (int i = 0; i < M_END; ++i) { | |
95 if (std::abs(data_[i] - rhs.data_[i]) > precision) | |
96 return false; | |
97 } | |
98 return true; | |
99 } | |
100 | |
101 Matrix3F Matrix3F::Inverse() const { | |
102 Matrix3F inverse = Matrix3F::Zeros(); | |
103 double determinant = Determinant3x3(data_); | |
104 if (std::numeric_limits<float>::epsilon() > std::abs(determinant)) | |
105 return inverse; // Singular matrix. Return Zeros(). | |
106 | |
107 inverse.set( | |
108 (data_[M11] * data_[M22] - data_[M12] * data_[M21]) / determinant, | |
109 (data_[M02] * data_[M21] - data_[M01] * data_[M22]) / determinant, | |
110 (data_[M01] * data_[M12] - data_[M02] * data_[M11]) / determinant, | |
111 (data_[M12] * data_[M20] - data_[M10] * data_[M22]) / determinant, | |
112 (data_[M00] * data_[M22] - data_[M02] * data_[M20]) / determinant, | |
113 (data_[M02] * data_[M10] - data_[M00] * data_[M12]) / determinant, | |
114 (data_[M10] * data_[M21] - data_[M11] * data_[M20]) / determinant, | |
115 (data_[M01] * data_[M20] - data_[M00] * data_[M21]) / determinant, | |
116 (data_[M00] * data_[M11] - data_[M01] * data_[M10]) / determinant); | |
117 return inverse; | |
118 } | |
119 | |
120 float Matrix3F::Determinant() const { | |
121 return static_cast<float>(Determinant3x3(data_)); | |
122 } | |
123 | |
124 Vector3dF Matrix3F::SolveEigenproblem(Matrix3F* eigenvectors) const { | |
125 // The matrix must be symmetric. | |
126 const float epsilon = std::numeric_limits<float>::epsilon(); | |
127 if (std::abs(data_[M01] - data_[M10]) > epsilon || | |
128 std::abs(data_[M02] - data_[M20]) > epsilon || | |
129 std::abs(data_[M12] - data_[M21]) > epsilon) { | |
130 NOTREACHED(); | |
131 return Vector3dF(); | |
132 } | |
133 | |
134 float eigenvalues[3]; | |
135 float p = | |
136 data_[M01] * data_[M01] + | |
137 data_[M02] * data_[M02] + | |
138 data_[M12] * data_[M12]; | |
139 | |
140 bool diagonal = std::abs(p) < epsilon; | |
141 if (diagonal) { | |
142 eigenvalues[0] = data_[M00]; | |
143 eigenvalues[1] = data_[M11]; | |
144 eigenvalues[2] = data_[M22]; | |
145 } else { | |
146 float q = Trace() / 3.0f; | |
147 p = (data_[M00] - q) * (data_[M00] - q) + | |
148 (data_[M11] - q) * (data_[M11] - q) + | |
149 (data_[M22] - q) * (data_[M22] - q) + | |
150 2 * p; | |
151 p = std::sqrt(p / 6); | |
152 | |
153 // The computation below puts B as (A - qI) / p, where A is *this. | |
154 Matrix3F matrix_b(*this); | |
155 matrix_b.data_[M00] -= q; | |
156 matrix_b.data_[M11] -= q; | |
157 matrix_b.data_[M22] -= q; | |
158 for (int i = 0; i < M_END; ++i) | |
159 matrix_b.data_[i] /= p; | |
160 | |
161 double half_det_b = Determinant3x3(matrix_b.data_) / 2.0; | |
162 // half_det_b should be in <-1, 1>, but beware of rounding error. | |
163 double phi = 0.0f; | |
164 if (half_det_b <= -1.0) | |
165 phi = M_PI / 3; | |
166 else if (half_det_b < 1.0) | |
167 phi = acos(half_det_b) / 3; | |
168 | |
169 eigenvalues[0] = q + 2 * p * static_cast<float>(cos(phi)); | |
170 eigenvalues[2] = q + 2 * p * | |
171 static_cast<float>(cos(phi + 2.0 * M_PI / 3.0)); | |
172 eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2]; | |
173 } | |
174 | |
175 // Put eigenvalues in the descending order. | |
176 int indices[3] = {0, 1, 2}; | |
177 if (eigenvalues[2] > eigenvalues[1]) { | |
178 std::swap(eigenvalues[2], eigenvalues[1]); | |
179 std::swap(indices[2], indices[1]); | |
180 } | |
181 | |
182 if (eigenvalues[1] > eigenvalues[0]) { | |
183 std::swap(eigenvalues[1], eigenvalues[0]); | |
184 std::swap(indices[1], indices[0]); | |
185 } | |
186 | |
187 if (eigenvalues[2] > eigenvalues[1]) { | |
188 std::swap(eigenvalues[2], eigenvalues[1]); | |
189 std::swap(indices[2], indices[1]); | |
190 } | |
191 | |
192 if (eigenvectors != NULL && diagonal) { | |
193 // Eigenvectors are e-vectors, just need to be sorted accordingly. | |
194 *eigenvectors = Zeros(); | |
195 for (int i = 0; i < 3; ++i) | |
196 eigenvectors->set(indices[i], i, 1.0f); | |
197 } else if (eigenvectors != NULL) { | |
198 // Consult the following for a detailed discussion: | |
199 // Joachim Kopp | |
200 // Numerical diagonalization of hermitian 3x3 matrices | |
201 // arXiv.org preprint: physics/0610206 | |
202 // Int. J. Mod. Phys. C19 (2008) 523-548 | |
203 | |
204 // TODO(motek): expand to handle correctly negative and multiple | |
205 // eigenvalues. | |
206 for (int i = 0; i < 3; ++i) { | |
207 float l = eigenvalues[i]; | |
208 // B = A - l * I | |
209 Matrix3F matrix_b(*this); | |
210 matrix_b.data_[M00] -= l; | |
211 matrix_b.data_[M11] -= l; | |
212 matrix_b.data_[M22] -= l; | |
213 Vector3dF e1 = CrossProduct(matrix_b.get_column(0), | |
214 matrix_b.get_column(1)); | |
215 Vector3dF e2 = CrossProduct(matrix_b.get_column(1), | |
216 matrix_b.get_column(2)); | |
217 Vector3dF e3 = CrossProduct(matrix_b.get_column(2), | |
218 matrix_b.get_column(0)); | |
219 | |
220 // e1, e2 and e3 should point in the same direction. | |
221 if (DotProduct(e1, e2) < 0) | |
222 e2 = -e2; | |
223 | |
224 if (DotProduct(e1, e3) < 0) | |
225 e3 = -e3; | |
226 | |
227 Vector3dF eigvec = e1 + e2 + e3; | |
228 // Normalize. | |
229 eigvec.Scale(1.0f / eigvec.Length()); | |
230 eigenvectors->set_column(i, eigvec); | |
231 } | |
232 } | |
233 | |
234 return Vector3dF(eigenvalues[0], eigenvalues[1], eigenvalues[2]); | |
235 } | |
236 | |
237 } // namespace gfx | |
OLD | NEW |