QuaternionBase<Derived>::setFromTwoVectors Uses 2 normalizations of input vectors, and waste 1 sqrt. The most optimal way (known to me) to implement this function , described http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors in pseudo code (input vectors is NOT unit) quat quat::fromtwovectors(vec3 u, vec3 v) { vec3 w = cross(u, v); quat q = quat(dot(u, v), w.x, w.y, w.z); q.w += q.magnitude(); return normalize(q); } Or version with explicit cache of (w.x*w.x +w.y*w.y +w.z*w.z) quat quat::fromtwovectors(vec3 u, vec3 v) { vec3 w = cross(u, v); quat q = quat(dot(u, v), w.x, w.y, w.z); float l=sqlength(w); q.w += sqrt(q.w*q.w+l); return q*(1/sqrt(q.w*q.w+l)); } It is about main algorithm branch, where input vectors is not opposite (180 deg). I guess its precision can be better, due to the fewer operations performed.
I'm willing to accept that, if you can provide code that checks for the border case without introducing more overhead than at the moment (only the check is relevant, IMO. Handling of the border cases should be rare enough). Perhaps, something like (-q.w*dummy_precision() > w.squaredNorm()) is sufficient for that (not tested!).
I suggest (in pseudocode) template<class Derived> template<typename Derived1, typename Derived2> inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b) { using std::max; using std::sqrt; Scalar dp = a.dot(b); Vector3 cp = a.cross(b); Scalar ls = cp.squaredNorm(); Scalar ws = dp*dp; // if dot == -1, vectors are nearly opposites // => accurately 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 a singular value problem if (ls < -ws*NumTraits<Scalar>::dummy_precision()) { Vector3 v0 = a.normalized(); Vector3 v1 = b.normalized(); Scalar c = v1.dot(v0); c = (max)(c,Scalar(-1)); Matrix<Scalar,2,3> m; m << v0.transpose(), v1.transpose(); JacobiSVD<Matrix<Scalar,2,3> > svd(m, ComputeFullV); Vector3 axis = svd.matrixV().col(2); Scalar w2 = (Scalar(1)+c)*Scalar(0.5); this->w() = sqrt(w2); this->vec() = axis * sqrt(Scalar(1) - w2); return derived(); } Scalar w = dp + sqrt(ws+ls); Scalar s = sqrt(w*w+ls); Scalar invs = Scalar(1)/s; this->vec() = cp * invs; this->w() = w * Scalar(0.5); return derived(); } Notes: 1. This version uses one multiplication instead addition in base version, but still avoid 1 sqrt and 2 multiplications in normalization. 2. Border case become other than in base version. And check sinus of angle compared to 1 * sqrt( NumTraits<Scalar>::dummy_precision() ). Is it bad? im not sure, i must test accuracy in this case. 3. Calculation of angle (in base version), in border case, based on cosinus can be very inaccurate. I should think how to handle it. Should i post another bug related to this issue?
Since ws > 0 your condition will never be true. Furthermore, it is crucial that the border case is not called for dp>0 and small ls, because that is the common (and stable) case of two vectors that are close to each other. Perhaps, dp<0 && ls < ws*NumTraits<Scalar>::dummy_precision() ? In the main branch invs has to be applied to the whole quaternion. For dp<0 and a small ls we could use the Taylor expansion of w = dp + sqrt(dp*dp + ls) ~=~ dp + -dp*(1+0.5*ls/(dp*dp)) == -0.5*ls/dp Also, for small ls (and dp<0) s = sqrt(ls+w*w) ~=~ sqrt(ls)*(1+1/2*w*w/ls) == sqrt(ls)*(1-1/8*ls/(dp*dp)) is basically just norm(cp), which means we could even avoid the whole SVD decomposition and just make a special case for ls==0 (within the border case of dp < 0) -- unless it is important around which axis the rotation occurs for near opposing vectors. You can test your implementation by running this in your build-directory: $ ./check.sh geo_quaternion
Thanks Christoph. I will spend more time for prototyping and testing, and try to present working code for both main branch and 180 deg branch. Anyway it is not urgent issue.
-- GitLab Migration Automatic Message -- This bug has been migrated to gitlab.com's GitLab instance and has been closed from further activity. You can subscribe and participate further through the new bug through this link to our GitLab instance: https://gitlab.com/libeigen/eigen/issues/989.