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 238
