ui/gfx/geometry/matrix3_f.cc

   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       static_cast<float>((data_[M11] * data_[M22] - data_[M12] * data_[M21]) /
 109           determinant),
 110       static_cast<float>((data_[M02] * data_[M21] - data_[M01] * data_[M22]) /
 111           determinant),
 112       static_cast<float>((data_[M01] * data_[M12] - data_[M02] * data_[M11]) /
 113           determinant),
 114       static_cast<float>((data_[M12] * data_[M20] - data_[M10] * data_[M22]) /
 115           determinant),
 116       static_cast<float>((data_[M00] * data_[M22] - data_[M02] * data_[M20]) /
 117           determinant),
 118       static_cast<float>((data_[M02] * data_[M10] - data_[M00] * data_[M12]) /
 119           determinant),
 120       static_cast<float>((data_[M10] * data_[M21] - data_[M11] * data_[M20]) /
 121           determinant),
 122       static_cast<float>((data_[M01] * data_[M20] - data_[M00] * data_[M21]) /
 123           determinant),
 124       static_cast<float>((data_[M00] * data_[M11] - data_[M01] * data_[M10]) /
 125           determinant));
 126   return inverse;
 127 }
 128
 129 float Matrix3F::Determinant() const {
 130   return static_cast<float>(Determinant3x3(data_));
 131 }
 132
 133 Vector3dF Matrix3F::SolveEigenproblem(Matrix3F* eigenvectors) const {
 134   // The matrix must be symmetric.
 135   const float epsilon = std::numeric_limits<float>::epsilon();
 136   if (std::abs(data_[M01] - data_[M10]) > epsilon ||
 137       std::abs(data_[M02] - data_[M20]) > epsilon ||
 138       std::abs(data_[M12] - data_[M21]) > epsilon) {
 139     NOTREACHED();
 140     return Vector3dF();
 141   }
 142
 143   float eigenvalues[3];
 144   float p =
 145       data_[M01] * data_[M01] +
 146       data_[M02] * data_[M02] +
 147       data_[M12] * data_[M12];
 148
 149   bool diagonal = std::abs(p) < epsilon;
 150   if (diagonal) {
 151     eigenvalues[0] = data_[M00];
 152     eigenvalues[1] = data_[M11];
 153     eigenvalues[2] = data_[M22];
 154   } else {
 155     float q = Trace() / 3.0f;
 156     p = (data_[M00] - q) * (data_[M00] - q) +
 157         (data_[M11] - q) * (data_[M11] - q) +
 158         (data_[M22] - q) * (data_[M22] - q) +
 159         2 * p;
 160     p = std::sqrt(p / 6);
 161
 162     // The computation below puts B as (A - qI) / p, where A is *this.
 163     Matrix3F matrix_b(*this);
 164     matrix_b.data_[M00] -= q;
 165     matrix_b.data_[M11] -= q;
 166     matrix_b.data_[M22] -= q;
 167     for (int i = 0; i < M_END; ++i)
 168       matrix_b.data_[i] /= p;
 169
 170     double half_det_b = Determinant3x3(matrix_b.data_) / 2.0;
 171     // half_det_b should be in <-1, 1>, but beware of rounding error.
 172     double phi = 0.0f;
 173     if (half_det_b <= -1.0)
 174       phi = M_PI / 3;
 175     else if (half_det_b < 1.0)
 176       phi = acos(half_det_b) / 3;
 177
 178     eigenvalues[0] = q + 2 * p * static_cast<float>(cos(phi));
 179     eigenvalues[2] = q + 2 * p *
 180         static_cast<float>(cos(phi + 2.0 * M_PI / 3.0));
 181     eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2];
 182   }
 183
 184   // Put eigenvalues in the descending order.
 185   int indices[3] = {0, 1, 2};
 186   if (eigenvalues[2] > eigenvalues[1]) {
 187     std::swap(eigenvalues[2], eigenvalues[1]);
 188     std::swap(indices[2], indices[1]);
 189   }
 190
 191   if (eigenvalues[1] > eigenvalues[0]) {
 192     std::swap(eigenvalues[1], eigenvalues[0]);
 193     std::swap(indices[1], indices[0]);
 194   }
 195
 196   if (eigenvalues[2] > eigenvalues[1]) {
 197     std::swap(eigenvalues[2], eigenvalues[1]);
 198     std::swap(indices[2], indices[1]);
 199   }
 200
 201   if (eigenvectors != NULL && diagonal) {
 202     // Eigenvectors are e-vectors, just need to be sorted accordingly.
 203     *eigenvectors = Zeros();
 204     for (int i = 0; i < 3; ++i)
 205       eigenvectors->set(indices[i], i, 1.0f);
 206   } else if (eigenvectors != NULL) {
 207     // Consult the following for a detailed discussion:
 208     // Joachim Kopp
 209     // Numerical diagonalization of hermitian 3x3 matrices
 210     // arXiv.org preprint: physics/0610206
 211     // Int. J. Mod. Phys. C19 (2008) 523-548
 212
 213     // TODO(motek): expand to handle correctly negative and multiple
 214     // eigenvalues.
 215     for (int i = 0; i < 3; ++i) {
 216       float l = eigenvalues[i];
 217       // B = A - l * I
 218       Matrix3F matrix_b(*this);
 219       matrix_b.data_[M00] -= l;
 220       matrix_b.data_[M11] -= l;
 221       matrix_b.data_[M22] -= l;
 222       Vector3dF e1 = CrossProduct(matrix_b.get_column(0),
 223                                   matrix_b.get_column(1));
 224       Vector3dF e2 = CrossProduct(matrix_b.get_column(1),
 225                                   matrix_b.get_column(2));
 226       Vector3dF e3 = CrossProduct(matrix_b.get_column(2),
 227                                   matrix_b.get_column(0));
 228
 229       // e1, e2 and e3 should point in the same direction.
 230       if (DotProduct(e1, e2) < 0)
 231         e2 = -e2;
 232
 233       if (DotProduct(e1, e3) < 0)
 234         e3 = -e3;
 235
 236       Vector3dF eigvec = e1 + e2 + e3;
 237       // Normalize.
 238       eigvec.Scale(1.0f / eigvec.Length());
 239       eigenvectors->set_column(i, eigvec);
 240     }
 241   }
 242
 243   return Vector3dF(eigenvalues[0], eigenvalues[1], eigenvalues[2]);
 244 }
 245
 246 }  // namespace gfx