12a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)// Copyright (c) 2013 The Chromium Authors. All rights reserved.
22a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)// Use of this source code is governed by a BSD-style license that can be
32a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)// found in the LICENSE file.
42a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
55d1f7b1de12d16ceb2c938c56701a3e8bfa558f7Torne (Richard Coles)#include "ui/gfx/geometry/matrix3_f.h"
62a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
72a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)#include <algorithm>
82a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)#include <cmath>
92a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)#include <limits>
102a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
112a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)#ifndef M_PI
122a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)#define M_PI 3.14159265358979323846
132a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)#endif
142a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
152a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)namespace {
162a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
172a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)// This is only to make accessing indices self-explanatory.
182a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)enum MatrixCoordinates {
192a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M00,
202a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M01,
212a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M02,
222a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M10,
232a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M11,
242a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M12,
252a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M20,
262a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M21,
272a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M22,
282a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  M_END
292a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)};
302a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
312a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)template<typename T>
322a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)double Determinant3x3(T data[M_END]) {
332a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  // This routine is separated from the Matrix3F::Determinant because in
342a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  // computing inverse we do want higher precision afforded by the explicit
352a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  // use of 'double'.
362a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return
372a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      static_cast<double>(data[M00]) * (
382a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)          static_cast<double>(data[M11]) * data[M22] -
392a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)          static_cast<double>(data[M12]) * data[M21]) +
402a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      static_cast<double>(data[M01]) * (
412a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)          static_cast<double>(data[M12]) * data[M20] -
422a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)          static_cast<double>(data[M10]) * data[M22]) +
432a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      static_cast<double>(data[M02]) * (
442a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)          static_cast<double>(data[M10]) * data[M21] -
452a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)          static_cast<double>(data[M11]) * data[M20]);
462a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
472a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
482a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}  // namespace
492a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
502a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)namespace gfx {
512a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
522a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)Matrix3F::Matrix3F() {
532a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
542a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
552a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)Matrix3F::~Matrix3F() {
562a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
572a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
582a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)// static
592a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)Matrix3F Matrix3F::Zeros() {
602a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  Matrix3F matrix;
612a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  matrix.set(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
622a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return matrix;
632a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
642a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
652a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)// static
662a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)Matrix3F Matrix3F::Ones() {
672a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  Matrix3F matrix;
682a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  matrix.set(1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f);
692a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return matrix;
702a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
712a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
722a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)// static
732a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)Matrix3F Matrix3F::Identity() {
742a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  Matrix3F matrix;
752a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  matrix.set(1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f);
762a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return matrix;
772a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
782a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
792a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)// static
802a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)Matrix3F Matrix3F::FromOuterProduct(const Vector3dF& a, const Vector3dF& bt) {
812a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  Matrix3F matrix;
822a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  matrix.set(a.x() * bt.x(), a.x() * bt.y(), a.x() * bt.z(),
832a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)             a.y() * bt.x(), a.y() * bt.y(), a.y() * bt.z(),
842a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)             a.z() * bt.x(), a.z() * bt.y(), a.z() * bt.z());
852a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return matrix;
862a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
872a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
882a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)bool Matrix3F::IsEqual(const Matrix3F& rhs) const {
892a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return 0 == memcmp(data_, rhs.data_, sizeof(data_));
902a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
912a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
922a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)bool Matrix3F::IsNear(const Matrix3F& rhs, float precision) const {
932a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  DCHECK(precision >= 0);
942a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  for (int i = 0; i < M_END; ++i) {
952a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    if (std::abs(data_[i] - rhs.data_[i]) > precision)
962a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      return false;
972a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  }
982a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return true;
992a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
1002a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1012a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)Matrix3F Matrix3F::Inverse() const {
1022a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  Matrix3F inverse = Matrix3F::Zeros();
1032a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  double determinant = Determinant3x3(data_);
1042a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  if (std::numeric_limits<float>::epsilon() > std::abs(determinant))
1052a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    return inverse;  // Singular matrix. Return Zeros().
1062a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1072a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  inverse.set(
1082a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M11] * data_[M22] - data_[M12] * data_[M21]) / determinant,
1092a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M02] * data_[M21] - data_[M01] * data_[M22]) / determinant,
1102a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M01] * data_[M12] - data_[M02] * data_[M11]) / determinant,
1112a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M12] * data_[M20] - data_[M10] * data_[M22]) / determinant,
1122a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M00] * data_[M22] - data_[M02] * data_[M20]) / determinant,
1132a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M02] * data_[M10] - data_[M00] * data_[M12]) / determinant,
1142a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M10] * data_[M21] - data_[M11] * data_[M20]) / determinant,
1152a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M01] * data_[M20] - data_[M00] * data_[M21]) / determinant,
1162a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      (data_[M00] * data_[M11] - data_[M01] * data_[M10]) / determinant);
1172a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return inverse;
1182a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
1192a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1202a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)float Matrix3F::Determinant() const {
1212a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return static_cast<float>(Determinant3x3(data_));
1222a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
1232a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1242a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)Vector3dF Matrix3F::SolveEigenproblem(Matrix3F* eigenvectors) const {
1252a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  // The matrix must be symmetric.
1262a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  const float epsilon = std::numeric_limits<float>::epsilon();
1272a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  if (std::abs(data_[M01] - data_[M10]) > epsilon ||
128c2db58bd994c04d98e4ee2cd7565b71548655fe3Ben Murdoch      std::abs(data_[M02] - data_[M20]) > epsilon ||
1292a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      std::abs(data_[M12] - data_[M21]) > epsilon) {
1302a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    NOTREACHED();
1312a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    return Vector3dF();
1322a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  }
1332a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1342a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  float eigenvalues[3];
1352a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  float p =
1362a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      data_[M01] * data_[M01] +
1372a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      data_[M02] * data_[M02] +
1382a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      data_[M12] * data_[M12];
1392a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1402a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  bool diagonal = std::abs(p) < epsilon;
1412a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  if (diagonal) {
1422a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    eigenvalues[0] = data_[M00];
1432a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    eigenvalues[1] = data_[M11];
1442a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    eigenvalues[2] = data_[M22];
1452a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  } else {
1462a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    float q = Trace() / 3.0f;
1472a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    p = (data_[M00] - q) * (data_[M00] - q) +
1482a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)        (data_[M11] - q) * (data_[M11] - q) +
1492a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)        (data_[M22] - q) * (data_[M22] - q) +
1502a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)        2 * p;
1512a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    p = std::sqrt(p / 6);
1522a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1532a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // The computation below puts B as (A - qI) / p, where A is *this.
1542a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    Matrix3F matrix_b(*this);
1552a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    matrix_b.data_[M00] -= q;
1562a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    matrix_b.data_[M11] -= q;
1572a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    matrix_b.data_[M22] -= q;
1582a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    for (int i = 0; i < M_END; ++i)
1592a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      matrix_b.data_[i] /= p;
1602a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1612a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    double half_det_b = Determinant3x3(matrix_b.data_) / 2.0;
1622a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // half_det_b should be in <-1, 1>, but beware of rounding error.
1632a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    double phi = 0.0f;
1642a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    if (half_det_b <= -1.0)
1652a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      phi = M_PI / 3;
1662a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    else if (half_det_b < 1.0)
1672a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      phi = acos(half_det_b) / 3;
1682a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1692a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    eigenvalues[0] = q + 2 * p * static_cast<float>(cos(phi));
1702a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    eigenvalues[2] = q + 2 * p *
1712a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)        static_cast<float>(cos(phi + 2.0 * M_PI / 3.0));
1722a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2];
1732a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  }
1742a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1752a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  // Put eigenvalues in the descending order.
1762a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  int indices[3] = {0, 1, 2};
1772a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  if (eigenvalues[2] > eigenvalues[1]) {
1782a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    std::swap(eigenvalues[2], eigenvalues[1]);
1792a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    std::swap(indices[2], indices[1]);
1802a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  }
1812a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1822a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  if (eigenvalues[1] > eigenvalues[0]) {
1832a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    std::swap(eigenvalues[1], eigenvalues[0]);
1842a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    std::swap(indices[1], indices[0]);
1852a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  }
1862a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1872a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  if (eigenvalues[2] > eigenvalues[1]) {
1882a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    std::swap(eigenvalues[2], eigenvalues[1]);
1892a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    std::swap(indices[2], indices[1]);
1902a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  }
1912a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
1922a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  if (eigenvectors != NULL && diagonal) {
1932a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // Eigenvectors are e-vectors, just need to be sorted accordingly.
1942a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    *eigenvectors = Zeros();
1952a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    for (int i = 0; i < 3; ++i)
1962a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      eigenvectors->set(indices[i], i, 1.0f);
1972a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  } else if (eigenvectors != NULL) {
1982a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // Consult the following for a detailed discussion:
1992a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // Joachim Kopp
2002a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // Numerical diagonalization of hermitian 3x3 matrices
2012a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // arXiv.org preprint: physics/0610206
2022a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // Int. J. Mod. Phys. C19 (2008) 523-548
2032a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
2042a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // TODO(motek): expand to handle correctly negative and multiple
2052a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    // eigenvalues.
2062a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    for (int i = 0; i < 3; ++i) {
2072a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      float l = eigenvalues[i];
2082a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      // B = A - l * I
2092a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      Matrix3F matrix_b(*this);
2102a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      matrix_b.data_[M00] -= l;
2112a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      matrix_b.data_[M11] -= l;
2122a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      matrix_b.data_[M22] -= l;
2132a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      Vector3dF e1 = CrossProduct(matrix_b.get_column(0),
2142a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)                                  matrix_b.get_column(1));
2152a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      Vector3dF e2 = CrossProduct(matrix_b.get_column(1),
2162a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)                                  matrix_b.get_column(2));
2172a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      Vector3dF e3 = CrossProduct(matrix_b.get_column(2),
2182a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)                                  matrix_b.get_column(0));
2192a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
2202a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      // e1, e2 and e3 should point in the same direction.
2212a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      if (DotProduct(e1, e2) < 0)
2222a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)        e2 = -e2;
2232a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
2242a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      if (DotProduct(e1, e3) < 0)
2252a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)        e3 = -e3;
2262a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
2272a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      Vector3dF eigvec = e1 + e2 + e3;
2282a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      // Normalize.
2292a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      eigvec.Scale(1.0f / eigvec.Length());
2302a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)      eigenvectors->set_column(i, eigvec);
2312a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)    }
2322a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  }
2332a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
2342a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)  return Vector3dF(eigenvalues[0], eigenvalues[1], eigenvalues[2]);
2352a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}
2362a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)
2372a99a7e74a7f215066514fe81d2bfa6639d9edddTorne (Richard Coles)}  // namespace gfx
238