Does Eigen have a function to get the shortest arc quaternion between 2 vectors?

i.e.

Code: Select all

q = shortest_arc( v0, v1 );
assert( v1 == q*v0 );


I can do it with something like:

Code: Select all

Vector3f axis = v0.cross(v1);
float angle = std::acos( v0.dot( v1 ) );
q = Quaternionf( AngleAxisf( angle, axis ) );


But can have problems when v0 is close/parallel to v1.

I pulled this from my old game programming gems book and ported it to Eigen syntax:

Code: Select all

Quaternionf RotationArc( Vector3f v0, Vector3f v1 )
{
    v0.normalize();
    v1.normalize();
    Vector3f c = v0.cross( v1 );
    float   d = v0.dot( v1 );
    float   s = std::sqrt( (1+d)*2 );
    return Quaternionf( s/2, c.x() / s, c.y() / s, c.z() / s );
}


However this still will fail when v0 = -v1.

So we can "improve" it a little more with:

Code: Select all

Quaternionf RotationArc( Vector3f v0, Vector3f v1 )
{
    v0.normalize();
    v1.normalize();
    float   d = v0.dot( v1 );
    float   s = std::sqrt( (1+d)*2 );
    if ( s < std::numeric_limits<float>::epsilon() )
    {
        // Generate an axis
        Vector3f a = Vector3f::UnitX().cross( v0 );
        // pick another if collinear
        if ( a.squaredNorm() < std::numeric_limits<float>::epsilon() )
        {
            a = Vector3f::UnitY().cross( v0 );
        }
        a.normalize();
        return Quaternionf( AngleAxisf( boost::math::constants::pi<float>() , a ) );
    }
    else
    {
        Vector3 i = v0.cross( v1 ) / s;
        return Quaternionf( s / 2, i.x(), i.y(), i.z() );
    }

}


This is quickly getting slow ugly and hairy.

Does Eigen have this utility? This seems like a fairly common task with a good deal of edge cases.

q.setFromTwoVectors(v0,v1);


The doc says:

/** Sets \c *this to be a quaternion representing a rotation between
* the two arbitrary vectors \a a and \a b. In other words, the built
* rotation represent a rotation sending the line of direction \a a
* to the line of direction \a b, both lines passing through the origin.
*
* \returns a reference to \c *this.
*
* Note that the two input vectors do \b not have to be normalized, and
* do not need to have the same norm.
*/

Ah wonderful I searched pretty hard but missed this thank you!

EDIT:

Looking for it again I still can't find it in the docs anywhere.

http://eigen.tuxfamily.org/dox/classEig ... mbers.html

Also seems kind of a shame to have to do 2phase initialization i.e.

Quaternion q;
q.setFromTwoVectors( v0, v1 );

Still THANKS

yes, doxygen has some difficulties with Eigen's c++ code