| // Copyright (c) 2013 The Chromium Authors. All rights reserved. |
| // Use of this source code is governed by a BSD-style license that can be |
| // found in the LICENSE file. |
| |
| #include "ui/gfx/geometry/matrix3_f.h" |
| |
| #include <algorithm> |
| #include <cmath> |
| #include <limits> |
| |
| #ifndef M_PI |
| #define M_PI 3.14159265358979323846 |
| #endif |
| |
| namespace { |
| |
| // This is only to make accessing indices self-explanatory. |
| enum MatrixCoordinates { |
| M00, |
| M01, |
| M02, |
| M10, |
| M11, |
| M12, |
| M20, |
| M21, |
| M22, |
| M_END |
| }; |
| |
| template<typename T> |
| double Determinant3x3(T data[M_END]) { |
| // This routine is separated from the Matrix3F::Determinant because in |
| // computing inverse we do want higher precision afforded by the explicit |
| // use of 'double'. |
| return |
| static_cast<double>(data[M00]) * ( |
| static_cast<double>(data[M11]) * data[M22] - |
| static_cast<double>(data[M12]) * data[M21]) + |
| static_cast<double>(data[M01]) * ( |
| static_cast<double>(data[M12]) * data[M20] - |
| static_cast<double>(data[M10]) * data[M22]) + |
| static_cast<double>(data[M02]) * ( |
| static_cast<double>(data[M10]) * data[M21] - |
| static_cast<double>(data[M11]) * data[M20]); |
| } |
| |
| } // namespace |
| |
| namespace gfx { |
| |
| Matrix3F::Matrix3F() { |
| } |
| |
| Matrix3F::~Matrix3F() { |
| } |
| |
| // static |
| Matrix3F Matrix3F::Zeros() { |
| Matrix3F matrix; |
| matrix.set(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f); |
| return matrix; |
| } |
| |
| // static |
| Matrix3F Matrix3F::Ones() { |
| Matrix3F matrix; |
| matrix.set(1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f); |
| return matrix; |
| } |
| |
| // static |
| Matrix3F Matrix3F::Identity() { |
| Matrix3F matrix; |
| matrix.set(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f); |
| return matrix; |
| } |
| |
| // static |
| Matrix3F Matrix3F::FromOuterProduct(const Vector3dF& a, const Vector3dF& bt) { |
| Matrix3F matrix; |
| matrix.set(a.x() * bt.x(), a.x() * bt.y(), a.x() * bt.z(), |
| a.y() * bt.x(), a.y() * bt.y(), a.y() * bt.z(), |
| a.z() * bt.x(), a.z() * bt.y(), a.z() * bt.z()); |
| return matrix; |
| } |
| |
| bool Matrix3F::IsEqual(const Matrix3F& rhs) const { |
| return 0 == memcmp(data_, rhs.data_, sizeof(data_)); |
| } |
| |
| bool Matrix3F::IsNear(const Matrix3F& rhs, float precision) const { |
| DCHECK(precision >= 0); |
| for (int i = 0; i < M_END; ++i) { |
| if (std::abs(data_[i] - rhs.data_[i]) > precision) |
| return false; |
| } |
| return true; |
| } |
| |
| Matrix3F Matrix3F::Inverse() const { |
| Matrix3F inverse = Matrix3F::Zeros(); |
| double determinant = Determinant3x3(data_); |
| if (std::numeric_limits<float>::epsilon() > std::abs(determinant)) |
| return inverse; // Singular matrix. Return Zeros(). |
| |
| inverse.set( |
| static_cast<float>((data_[M11] * data_[M22] - data_[M12] * data_[M21]) / |
| determinant), |
| static_cast<float>((data_[M02] * data_[M21] - data_[M01] * data_[M22]) / |
| determinant), |
| static_cast<float>((data_[M01] * data_[M12] - data_[M02] * data_[M11]) / |
| determinant), |
| static_cast<float>((data_[M12] * data_[M20] - data_[M10] * data_[M22]) / |
| determinant), |
| static_cast<float>((data_[M00] * data_[M22] - data_[M02] * data_[M20]) / |
| determinant), |
| static_cast<float>((data_[M02] * data_[M10] - data_[M00] * data_[M12]) / |
| determinant), |
| static_cast<float>((data_[M10] * data_[M21] - data_[M11] * data_[M20]) / |
| determinant), |
| static_cast<float>((data_[M01] * data_[M20] - data_[M00] * data_[M21]) / |
| determinant), |
| static_cast<float>((data_[M00] * data_[M11] - data_[M01] * data_[M10]) / |
| determinant)); |
| return inverse; |
| } |
| |
| float Matrix3F::Determinant() const { |
| return static_cast<float>(Determinant3x3(data_)); |
| } |
| |
| Vector3dF Matrix3F::SolveEigenproblem(Matrix3F* eigenvectors) const { |
| // The matrix must be symmetric. |
| const float epsilon = std::numeric_limits<float>::epsilon(); |
| if (std::abs(data_[M01] - data_[M10]) > epsilon || |
| std::abs(data_[M02] - data_[M20]) > epsilon || |
| std::abs(data_[M12] - data_[M21]) > epsilon) { |
| NOTREACHED(); |
| return Vector3dF(); |
| } |
| |
| float eigenvalues[3]; |
| float p = |
| data_[M01] * data_[M01] + |
| data_[M02] * data_[M02] + |
| data_[M12] * data_[M12]; |
| |
| bool diagonal = std::abs(p) < epsilon; |
| if (diagonal) { |
| eigenvalues[0] = data_[M00]; |
| eigenvalues[1] = data_[M11]; |
| eigenvalues[2] = data_[M22]; |
| } else { |
| float q = Trace() / 3.0f; |
| p = (data_[M00] - q) * (data_[M00] - q) + |
| (data_[M11] - q) * (data_[M11] - q) + |
| (data_[M22] - q) * (data_[M22] - q) + |
| 2 * p; |
| p = std::sqrt(p / 6); |
| |
| // The computation below puts B as (A - qI) / p, where A is *this. |
| Matrix3F matrix_b(*this); |
| matrix_b.data_[M00] -= q; |
| matrix_b.data_[M11] -= q; |
| matrix_b.data_[M22] -= q; |
| for (int i = 0; i < M_END; ++i) |
| matrix_b.data_[i] /= p; |
| |
| double half_det_b = Determinant3x3(matrix_b.data_) / 2.0; |
| // half_det_b should be in <-1, 1>, but beware of rounding error. |
| double phi = 0.0f; |
| if (half_det_b <= -1.0) |
| phi = M_PI / 3; |
| else if (half_det_b < 1.0) |
| phi = acos(half_det_b) / 3; |
| |
| eigenvalues[0] = q + 2 * p * static_cast<float>(cos(phi)); |
| eigenvalues[2] = q + 2 * p * |
| static_cast<float>(cos(phi + 2.0 * M_PI / 3.0)); |
| eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2]; |
| } |
| |
| // Put eigenvalues in the descending order. |
| int indices[3] = {0, 1, 2}; |
| if (eigenvalues[2] > eigenvalues[1]) { |
| std::swap(eigenvalues[2], eigenvalues[1]); |
| std::swap(indices[2], indices[1]); |
| } |
| |
| if (eigenvalues[1] > eigenvalues[0]) { |
| std::swap(eigenvalues[1], eigenvalues[0]); |
| std::swap(indices[1], indices[0]); |
| } |
| |
| if (eigenvalues[2] > eigenvalues[1]) { |
| std::swap(eigenvalues[2], eigenvalues[1]); |
| std::swap(indices[2], indices[1]); |
| } |
| |
| if (eigenvectors != NULL && diagonal) { |
| // Eigenvectors are e-vectors, just need to be sorted accordingly. |
| *eigenvectors = Zeros(); |
| for (int i = 0; i < 3; ++i) |
| eigenvectors->set(indices[i], i, 1.0f); |
| } else if (eigenvectors != NULL) { |
| // Consult the following for a detailed discussion: |
| // Joachim Kopp |
| // Numerical diagonalization of hermitian 3x3 matrices |
| // arXiv.org preprint: physics/0610206 |
| // Int. J. Mod. Phys. C19 (2008) 523-548 |
| |
| // TODO(motek): expand to handle correctly negative and multiple |
| // eigenvalues. |
| for (int i = 0; i < 3; ++i) { |
| float l = eigenvalues[i]; |
| // B = A - l * I |
| Matrix3F matrix_b(*this); |
| matrix_b.data_[M00] -= l; |
| matrix_b.data_[M11] -= l; |
| matrix_b.data_[M22] -= l; |
| Vector3dF e1 = CrossProduct(matrix_b.get_column(0), |
| matrix_b.get_column(1)); |
| Vector3dF e2 = CrossProduct(matrix_b.get_column(1), |
| matrix_b.get_column(2)); |
| Vector3dF e3 = CrossProduct(matrix_b.get_column(2), |
| matrix_b.get_column(0)); |
| |
| // e1, e2 and e3 should point in the same direction. |
| if (DotProduct(e1, e2) < 0) |
| e2 = -e2; |
| |
| if (DotProduct(e1, e3) < 0) |
| e3 = -e3; |
| |
| Vector3dF eigvec = e1 + e2 + e3; |
| // Normalize. |
| eigvec.Scale(1.0f / eigvec.Length()); |
| eigenvectors->set_column(i, eigvec); |
| } |
| } |
| |
| return Vector3dF(eigenvalues[0], eigenvalues[1], eigenvalues[2]); |
| } |
| |
| } // namespace gfx |