Squashed 'third_party/eigen/' content from commit 61d72f6
Change-Id: Iccc90fa0b55ab44037f018046d2fcffd90d9d025
git-subtree-dir: third_party/eigen
git-subtree-split: 61d72f6383cfa842868c53e30e087b0258177257
diff --git a/bench/eig33.cpp b/bench/eig33.cpp
new file mode 100644
index 0000000..1608b99
--- /dev/null
+++ b/bench/eig33.cpp
@@ -0,0 +1,196 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+// The computeRoots function included in this is based on materials
+// covered by the following copyright and license:
+//
+// Geometric Tools, LLC
+// Copyright (c) 1998-2010
+// Distributed under the Boost Software License, Version 1.0.
+//
+// Permission is hereby granted, free of charge, to any person or organization
+// obtaining a copy of the software and accompanying documentation covered by
+// this license (the "Software") to use, reproduce, display, distribute,
+// execute, and transmit the Software, and to prepare derivative works of the
+// Software, and to permit third-parties to whom the Software is furnished to
+// do so, all subject to the following:
+//
+// The copyright notices in the Software and this entire statement, including
+// the above license grant, this restriction and the following disclaimer,
+// must be included in all copies of the Software, in whole or in part, and
+// all derivative works of the Software, unless such copies or derivative
+// works are solely in the form of machine-executable object code generated by
+// a source language processor.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+// DEALINGS IN THE SOFTWARE.
+
+#include <iostream>
+#include <Eigen/Core>
+#include <Eigen/Eigenvalues>
+#include <Eigen/Geometry>
+#include <bench/BenchTimer.h>
+
+using namespace Eigen;
+using namespace std;
+
+template<typename Matrix, typename Roots>
+inline void computeRoots(const Matrix& m, Roots& roots)
+{
+ typedef typename Matrix::Scalar Scalar;
+ const Scalar s_inv3 = 1.0/3.0;
+ const Scalar s_sqrt3 = internal::sqrt(Scalar(3.0));
+
+ // The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
+ // eigenvalues are the roots to this equation, all guaranteed to be
+ // real-valued, because the matrix is symmetric.
+ 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);
+ 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);
+ Scalar c2 = m(0,0) + m(1,1) + m(2,2);
+
+ // Construct the parameters used in classifying the roots of the equation
+ // and in solving the equation for the roots in closed form.
+ Scalar c2_over_3 = c2*s_inv3;
+ Scalar a_over_3 = (c1 - c2*c2_over_3)*s_inv3;
+ if (a_over_3 > Scalar(0))
+ a_over_3 = Scalar(0);
+
+ Scalar half_b = Scalar(0.5)*(c0 + c2_over_3*(Scalar(2)*c2_over_3*c2_over_3 - c1));
+
+ Scalar q = half_b*half_b + a_over_3*a_over_3*a_over_3;
+ if (q > Scalar(0))
+ q = Scalar(0);
+
+ // Compute the eigenvalues by solving for the roots of the polynomial.
+ Scalar rho = internal::sqrt(-a_over_3);
+ Scalar theta = std::atan2(internal::sqrt(-q),half_b)*s_inv3;
+ Scalar cos_theta = internal::cos(theta);
+ Scalar sin_theta = internal::sin(theta);
+ roots(0) = c2_over_3 + Scalar(2)*rho*cos_theta;
+ roots(1) = c2_over_3 - rho*(cos_theta + s_sqrt3*sin_theta);
+ roots(2) = c2_over_3 - rho*(cos_theta - s_sqrt3*sin_theta);
+
+ // Sort in increasing order.
+ if (roots(0) >= roots(1))
+ std::swap(roots(0),roots(1));
+ if (roots(1) >= roots(2))
+ {
+ std::swap(roots(1),roots(2));
+ if (roots(0) >= roots(1))
+ std::swap(roots(0),roots(1));
+ }
+}
+
+template<typename Matrix, typename Vector>
+void eigen33(const Matrix& mat, Matrix& evecs, Vector& evals)
+{
+ typedef typename Matrix::Scalar Scalar;
+ // Scale the matrix so its entries are in [-1,1]. The scaling is applied
+ // only when at least one matrix entry has magnitude larger than 1.
+
+ Scalar scale = mat.cwiseAbs()/*.template triangularView<Lower>()*/.maxCoeff();
+ scale = std::max(scale,Scalar(1));
+ Matrix scaledMat = mat / scale;
+
+ // Compute the eigenvalues
+// scaledMat.setZero();
+ computeRoots(scaledMat,evals);
+
+ // compute the eigen vectors
+ // **here we assume 3 differents eigenvalues**
+
+ // "optimized version" which appears to be slower with gcc!
+// Vector base;
+// Scalar alpha, beta;
+// base << scaledMat(1,0) * scaledMat(2,1),
+// scaledMat(1,0) * scaledMat(2,0),
+// -scaledMat(1,0) * scaledMat(1,0);
+// for(int k=0; k<2; ++k)
+// {
+// alpha = scaledMat(0,0) - evals(k);
+// beta = scaledMat(1,1) - evals(k);
+// evecs.col(k) = (base + Vector(-beta*scaledMat(2,0), -alpha*scaledMat(2,1), alpha*beta)).normalized();
+// }
+// evecs.col(2) = evecs.col(0).cross(evecs.col(1)).normalized();
+
+// // naive version
+// Matrix tmp;
+// tmp = scaledMat;
+// tmp.diagonal().array() -= evals(0);
+// evecs.col(0) = tmp.row(0).cross(tmp.row(1)).normalized();
+//
+// tmp = scaledMat;
+// tmp.diagonal().array() -= evals(1);
+// evecs.col(1) = tmp.row(0).cross(tmp.row(1)).normalized();
+//
+// tmp = scaledMat;
+// tmp.diagonal().array() -= evals(2);
+// evecs.col(2) = tmp.row(0).cross(tmp.row(1)).normalized();
+
+ // a more stable version:
+ if((evals(2)-evals(0))<=Eigen::NumTraits<Scalar>::epsilon())
+ {
+ evecs.setIdentity();
+ }
+ else
+ {
+ Matrix tmp;
+ tmp = scaledMat;
+ tmp.diagonal ().array () -= evals (2);
+ evecs.col (2) = tmp.row (0).cross (tmp.row (1)).normalized ();
+
+ tmp = scaledMat;
+ tmp.diagonal ().array () -= evals (1);
+ evecs.col(1) = tmp.row (0).cross(tmp.row (1));
+ Scalar n1 = evecs.col(1).norm();
+ if(n1<=Eigen::NumTraits<Scalar>::epsilon())
+ evecs.col(1) = evecs.col(2).unitOrthogonal();
+ else
+ evecs.col(1) /= n1;
+
+ // make sure that evecs[1] is orthogonal to evecs[2]
+ evecs.col(1) = evecs.col(2).cross(evecs.col(1).cross(evecs.col(2))).normalized();
+ evecs.col(0) = evecs.col(2).cross(evecs.col(1));
+ }
+
+ // Rescale back to the original size.
+ evals *= scale;
+}
+
+int main()
+{
+ BenchTimer t;
+ int tries = 10;
+ int rep = 400000;
+ typedef Matrix3f Mat;
+ typedef Vector3f Vec;
+ Mat A = Mat::Random(3,3);
+ A = A.adjoint() * A;
+
+ SelfAdjointEigenSolver<Mat> eig(A);
+ BENCH(t, tries, rep, eig.compute(A));
+ std::cout << "Eigen: " << t.best() << "s\n";
+
+ Mat evecs;
+ Vec evals;
+ BENCH(t, tries, rep, eigen33(A,evecs,evals));
+ std::cout << "Direct: " << t.best() << "s\n\n";
+
+ std::cerr << "Eigenvalue/eigenvector diffs:\n";
+ std::cerr << (evals - eig.eigenvalues()).transpose() << "\n";
+ for(int k=0;k<3;++k)
+ if(evecs.col(k).dot(eig.eigenvectors().col(k))<0)
+ evecs.col(k) = -evecs.col(k);
+ std::cerr << evecs - eig.eigenvectors() << "\n\n";
+}