You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
292 lines
8.6 KiB
292 lines
8.6 KiB
// 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>
|
|
|
|
#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<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::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<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;
|
|
}
|
|
|
|
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<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 = base::kPiDouble / 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 * 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
|