Home | History | Annotate | Download | only in geometry 
      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/geometry/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