GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-11 11:19:02 +08:00
Code Issues Packages Projects Releases Wiki Activity

significantly improve the accuracy of setFromTwoVectors (fixes #21)

Browse Source
This commit is contained in:
Gael Guennebaud 2009-07-06 10:35:20 +02:00
parent ecc4f07af5
commit 90f1e24579
2 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Eigen/Geometry
View File

@ -6,6 +6,7 @@
#include "src/Core/util/DisableMSVCWarnings.h" #include "src/Core/util/DisableMSVCWarnings.h"
#include "Array" #include "Array"
#include "QR"
#include <limits> #include <limits>
#ifndef M_PI #ifndef M_PI

24
Eigen/src/Geometry/Quaternion.h
View File

@ -360,18 +360,22 @@ inline Quaternion<Scalar>& Quaternion<Scalar>::setFromTwoVectors(const MatrixBas
Vector3 v1 = b.normalized(); Vector3 v1 = b.normalized();
Scalar c = v0.dot(v1); Scalar c = v0.dot(v1);
// if dot == 1, vectors are the same // if dot == -1, vectors are nearly opposites
if (ei_isApprox(c,Scalar(1))) // => accuraletly compute the rotation axis by computing the
{ // intersection of the two planes. This is done by solving:
// set to identity // x^T v0 = 0
this->w() = 1; this->vec().setZero(); // x^T v1 = 0
return *this; // under the constraint:
} // ||x|| = 1
// if dot == -1, vectors are opposites // which yields an eigenvalue problem (or a SVD)
if (ei_isApprox(c,Scalar(-1))) if (ei_isApprox(c,Scalar(-1)))
{ {
this->vec() = v0.unitOrthogonal(); c = std::max<Scalar>(c,-1);
this->w() = 0; 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; return *this;
} }