matrix3_f.cc revision 5d1f7b1de12d16ceb2c938c56701a3e8bfa558f7
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
15namespace {
16
17// This is only to make accessing indices self-explanatory.
18enum MatrixCoordinates {
19  M00,
20  M01,
21  M02,
22  M10,
23  M11,
24  M12,
25  M20,
26  M21,
27  M22,
28  M_END
29};
30
31template<typename T>
32double 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
50namespace gfx {
51
52Matrix3F::Matrix3F() {
53}
54
55Matrix3F::~Matrix3F() {
56}
57
58// static
59Matrix3F 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
66Matrix3F 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
73Matrix3F 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
80Matrix3F 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
88bool Matrix3F::IsEqual(const Matrix3F& rhs) const {
89  return 0 == memcmp(data_, rhs.data_, sizeof(data_));
90}
91
92bool 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
101Matrix3F 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
120float Matrix3F::Determinant() const {
121  return static_cast<float>(Determinant3x3(data_));
122}
123
124Vector3dF 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