1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
5//
6// This Source Code Form is subject to the terms of the Mozilla
7// Public License v. 2.0. If a copy of the MPL was not distributed
8// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
9
10// The computeRoots function included in this is based on materials
11// covered by the following copyright and license:
12//
13// Geometric Tools, LLC
14// Copyright (c) 1998-2010
15// Distributed under the Boost Software License, Version 1.0.
16//
17// Permission is hereby granted, free of charge, to any person or organization
18// obtaining a copy of the software and accompanying documentation covered by
19// this license (the "Software") to use, reproduce, display, distribute,
20// execute, and transmit the Software, and to prepare derivative works of the
21// Software, and to permit third-parties to whom the Software is furnished to
22// do so, all subject to the following:
23//
24// The copyright notices in the Software and this entire statement, including
25// the above license grant, this restriction and the following disclaimer,
26// must be included in all copies of the Software, in whole or in part, and
27// all derivative works of the Software, unless such copies or derivative
28// works are solely in the form of machine-executable object code generated by
29// a source language processor.
30//
31// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
32// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
33// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
34// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
35// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
36// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
37// DEALINGS IN THE SOFTWARE.
38
39#include <iostream>
40#include <Eigen/Core>
41#include <Eigen/Eigenvalues>
42#include <Eigen/Geometry>
43#include <bench/BenchTimer.h>
44
45using namespace Eigen;
46using namespace std;
47
48template<typename Matrix, typename Roots>
49inline void computeRoots(const Matrix& m, Roots& roots)
50{
51  typedef typename Matrix::Scalar Scalar;
52  const Scalar s_inv3 = 1.0/3.0;
53  const Scalar s_sqrt3 = internal::sqrt(Scalar(3.0));
54
55  // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0.  The
56  // eigenvalues are the roots to this equation, all guaranteed to be
57  // real-valued, because the matrix is symmetric.
58  Scalar c0 = m(0,0)*m(1,1)*m(2,2) + Scalar(2)*m(0,1)*m(0,2)*m(1,2) - m(0,0)*m(1,2)*m(1,2) - m(1,1)*m(0,2)*m(0,2) - m(2,2)*m(0,1)*m(0,1);
59  Scalar c1 = m(0,0)*m(1,1) - m(0,1)*m(0,1) + m(0,0)*m(2,2) - m(0,2)*m(0,2) + m(1,1)*m(2,2) - m(1,2)*m(1,2);
60  Scalar c2 = m(0,0) + m(1,1) + m(2,2);
61
62  // Construct the parameters used in classifying the roots of the equation
63  // and in solving the equation for the roots in closed form.
64  Scalar c2_over_3 = c2*s_inv3;
65  Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
66  if (a_over_3 > Scalar(0))
67    a_over_3 = Scalar(0);
68
69  Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));
70
71  Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
72  if (q > Scalar(0))
73    q = Scalar(0);
74
75  // Compute the eigenvalues by solving for the roots of the polynomial.
76  Scalar rho = internal::sqrt(-a_over_3);
77  Scalar theta = std::atan2(internal::sqrt(-q),half_b)*s_inv3;
78  Scalar cos_theta = internal::cos(theta);
79  Scalar sin_theta = internal::sin(theta);
80  roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta;
81  roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
82  roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);
83
84  // Sort in increasing order.
85  if (roots(0) >= roots(1))
86    std::swap(roots(0),roots(1));
87  if (roots(1) >= roots(2))
88  {
89    std::swap(roots(1),roots(2));
90    if (roots(0) >= roots(1))
91      std::swap(roots(0),roots(1));
92  }
93}
94
95template<typename Matrix, typename Vector>
96void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals)
97{
98  typedef typename Matrix::Scalar Scalar;
99  // Scale the matrix so its entries are in [-1,1].  The scaling is applied
100  // only when at least one matrix entry has magnitude larger than 1.
101
102  Scalar scale = mat.cwiseAbs()/*.template triangularView<Lower>()*/.maxCoeff();
103  scale = std::max(scale,Scalar(1));
104  Matrix scaledMat = mat / scale;
105
106  // Compute the eigenvalues
107//   scaledMat.setZero();
108  computeRoots(scaledMat,evals);
109
110  // compute the eigen vectors
111  // **here we assume 3 differents eigenvalues**
112
113  // "optimized version" which appears to be slower with gcc!
114//     Vector base;
115//     Scalar alpha, beta;
116//     base <<   scaledMat(1,0) * scaledMat(2,1),
117//               scaledMat(1,0) * scaledMat(2,0),
118//              -scaledMat(1,0) * scaledMat(1,0);
119//     for(int k=0; k<2; ++k)
120//     {
121//       alpha = scaledMat(0,0) - evals(k);
122//       beta  = scaledMat(1,1) - evals(k);
123//       evecs.col(k) = (base + Vector(-beta*scaledMat(2,0), -alpha*scaledMat(2,1), alpha*beta)).normalized();
124//     }
125//     evecs.col(2) = evecs.col(0).cross(evecs.col(1)).normalized();
126
127//   // naive version
128//   Matrix tmp;
129//   tmp = scaledMat;
130//   tmp.diagonal().array() -= evals(0);
131//   evecs.col(0) = tmp.row(0).cross(tmp.row(1)).normalized();
132//
133//   tmp = scaledMat;
134//   tmp.diagonal().array() -= evals(1);
135//   evecs.col(1) = tmp.row(0).cross(tmp.row(1)).normalized();
136//
137//   tmp = scaledMat;
138//   tmp.diagonal().array() -= evals(2);
139//   evecs.col(2) = tmp.row(0).cross(tmp.row(1)).normalized();
140
141  // a more stable version:
142  if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon())
143  {
144    evecs.setIdentity();
145  }
146  else
147  {
148    Matrix tmp;
149    tmp = scaledMat;
150    tmp.diagonal ().array () -= evals (2);
151    evecs.col (2) = tmp.row (0).cross (tmp.row (1)).normalized ();
152
153    tmp = scaledMat;
154    tmp.diagonal ().array () -= evals (1);
155    evecs.col(1) = tmp.row (0).cross(tmp.row (1));
156    Scalar n1 = evecs.col(1).norm();
157    if(n1<=Eigen::NumTraits<Scalar>::epsilon())
158      evecs.col(1) = evecs.col(2).unitOrthogonal();
159    else
160      evecs.col(1) /= n1;
161
162    // make sure that evecs[1] is orthogonal to evecs[2]
163    evecs.col(1) = evecs.col(2).cross(evecs.col(1).cross(evecs.col(2))).normalized();
164    evecs.col(0) = evecs.col(2).cross(evecs.col(1));
165  }
166
167  // Rescale back to the original size.
168  evals *= scale;
169}
170
171int main()
172{
173  BenchTimer t;
174  int tries = 10;
175  int rep = 400000;
176  typedef Matrix3f Mat;
177  typedef Vector3f Vec;
178  Mat A = Mat::Random(3,3);
179  A = A.adjoint() * A;
180
181  SelfAdjointEigenSolver<Mat> eig(A);
182  BENCH(t, tries, rep, eig.compute(A));
183  std::cout << "Eigen:  " << t.best() << "s\n";
184
185  Mat evecs;
186  Vec evals;
187  BENCH(t, tries, rep, eigen33(A,evecs,evals));
188  std::cout << "Direct: " << t.best() << "s\n\n";
189
190  std::cerr << "Eigenvalue/eigenvector diffs:\n";
191  std::cerr << (evals - eig.eigenvalues()).transpose() << "\n";
192  for(int k=0;k<3;++k)
193    if(evecs.col(k).dot(eig.eigenvectors().col(k))<0)
194      evecs.col(k) = -evecs.col(k);
195  std::cerr << evecs - eig.eigenvectors() << "\n\n";
196}
197