From 90f1e24579b8fa0605f63b2c9bfec5d023ca23ee Mon Sep 17 00:00:00 2001
From: Gael Guennebaud <g.gael@free.fr>
Date: Mon, 6 Jul 2009 10:35:20 +0200
Subject: [PATCH] significantly improve the accuracy of setFromTwoVectors
 (fixes #21)

---
 Eigen/Geometry                  |  1 +
 Eigen/src/Geometry/Quaternion.h | 24 ++++++++++++++----------
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/Eigen/Geometry b/Eigen/Geometry
index 7860d8fca..8698dd1a5 100644
--- a/Eigen/Geometry
+++ b/Eigen/Geometry
@@ -6,6 +6,7 @@
 #include "src/Core/util/DisableMSVCWarnings.h"
 
 #include "Array"
+#include "QR"
 #include <limits>
 
 #ifndef M_PI
diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index f2ebece9a..088dff9f2 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -360,18 +360,22 @@ inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBas
   Vector3 v1 = b.normalized();
   Scalar c = v0.dot(v1);
 
-  // if dot == 1, vectors are the same
-  if (ei_isApprox(c,Scalar(1)))
-  {
-    // set to identity
-    this->w() = 1; this->vec().setZero();
-    return *this;
-  }
-  // if dot == -1, vectors are opposites
+  // if dot == -1, vectors are nearly opposites
+  // => accuraletly compute the rotation axis by computing the
+  //    intersection of the two planes. This is done by solving:
+  //       x^T v0 = 0
+  //       x^T v1 = 0
+  //    under the constraint:
+  //       ||x|| = 1
+  //    which yields an eigenvalue problem (or a SVD)
   if (ei_isApprox(c,Scalar(-1)))
   {
-    this->vec() = v0.unitOrthogonal();
-    this->w() = 0;
+    c = std::max<Scalar>(c,-1);
+    SelfAdjointEigenSolver<Matrix<Scalar,3,3> > eig(v0 * v0.transpose() + v1 * v1.transpose());
+    Vector3 axis = eig.eigenvectors().col(0);
+    Scalar w2 = (Scalar(1)+c)*Scalar(0.5);
+    this->w() = ei_sqrt(w2);
+    this->vec() = axis * ei_sqrt(Scalar(1) - w2);
     return *this;
   }