blob: 1420ee539dcaea053e008826bf1f1ff058773ef9 [file] [log] [blame]
 // 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 #include #include #include "base/numerics/math_constants.h" #include "base/strings/stringprintf.h" namespace { // This is only to make accessing indices self-explanatory. enum MatrixCoordinates { M00, M01, M02, M10, M11, M12, M20, M21, M22, M_END }; template 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(data[M00]) * ( static_cast(data[M11]) * data[M22] - static_cast(data[M12]) * data[M21]) + static_cast(data[M01]) * ( static_cast(data[M12]) * data[M20] - static_cast(data[M10]) * data[M22]) + static_cast(data[M02]) * ( static_cast(data[M10]) * data[M21] - static_cast(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::Add(const Matrix3F& rhs) const { Matrix3F result; for (int i = 0; i < M_END; ++i) result.data_[i] = data_[i] + rhs.data_[i]; return result; } Matrix3F Matrix3F::Subtract(const Matrix3F& rhs) const { Matrix3F result; for (int i = 0; i < M_END; ++i) result.data_[i] = data_[i] - rhs.data_[i]; return result; } Matrix3F Matrix3F::Inverse() const { Matrix3F inverse = Matrix3F::Zeros(); double determinant = Determinant3x3(data_); if (std::numeric_limits::epsilon() > std::abs(determinant)) return inverse; // Singular matrix. Return Zeros(). inverse.set( static_cast((data_[M11] * data_[M22] - data_[M12] * data_[M21]) / determinant), static_cast((data_[M02] * data_[M21] - data_[M01] * data_[M22]) / determinant), static_cast((data_[M01] * data_[M12] - data_[M02] * data_[M11]) / determinant), static_cast((data_[M12] * data_[M20] - data_[M10] * data_[M22]) / determinant), static_cast((data_[M00] * data_[M22] - data_[M02] * data_[M20]) / determinant), static_cast((data_[M02] * data_[M10] - data_[M00] * data_[M12]) / determinant), static_cast((data_[M10] * data_[M21] - data_[M11] * data_[M20]) / determinant), static_cast((data_[M01] * data_[M20] - data_[M00] * data_[M21]) / determinant), static_cast((data_[M00] * data_[M11] - data_[M01] * data_[M10]) / determinant)); return inverse; } Matrix3F Matrix3F::Transpose() const { Matrix3F transpose; transpose.set(data_[M00], data_[M10], data_[M20], data_[M01], data_[M11], data_[M21], data_[M02], data_[M12], data_[M22]); return transpose; } float Matrix3F::Determinant() const { return static_cast(Determinant3x3(data_)); } Vector3dF Matrix3F::SolveEigenproblem(Matrix3F* eigenvectors) const { // The matrix must be symmetric. const float epsilon = std::numeric_limits::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 = base::kPiDouble / 3; else if (half_det_b < 1.0) phi = acos(half_det_b) / 3; eigenvalues[0] = q + 2 * p * static_cast(cos(phi)); eigenvalues[2] = q + 2 * p * static_cast(cos(phi + 2.0 * base::kPiDouble / 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]); } Matrix3F MatrixProduct(const Matrix3F& lhs, const Matrix3F& rhs) { Matrix3F result = Matrix3F::Zeros(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { result.set(i, j, DotProduct(lhs.get_row(i), rhs.get_column(j))); } } return result; } Vector3dF MatrixProduct(const Matrix3F& lhs, const Vector3dF& rhs) { return Vector3dF(DotProduct(lhs.get_row(0), rhs), DotProduct(lhs.get_row(1), rhs), DotProduct(lhs.get_row(2), rhs)); } std::string Matrix3F::ToString() const { return base::StringPrintf( "[[%+0.4f, %+0.4f, %+0.4f]," " [%+0.4f, %+0.4f, %+0.4f]," " [%+0.4f, %+0.4f, %+0.4f]]", data_[M00], data_[M01], data_[M02], data_[M10], data_[M11], data_[M12], data_[M20], data_[M21], data_[M22]); } } // namespace gfx