Trouble Implementing Rodrigues' rotation formula in C++

Question

I'm trying to implement a function that takes two geometry vectors in 3D space and returns a rotation matrix that rotates the first vector to the second vector. My function currently uses Rodrigues' rotation formula to create a matrix, but my implementation of this formula gives the wrong answer for some inputs. I checked the math by hand for one test that gave an incorrect result, and my work gave the same result.

Here is the code for my function:

Matrix3d rotation_matrix(Vector3d vector0, Vector3d vector1)
{
    vector0.normalize();
    vector1.normalize();
    // vector orthogonal to both inputs
    Vector3d u = vector0.cross(vector1);
    if (!u.norm())
    {
        if (vector0 == vector1)
            return Matrix3d::Identity();
        // return rotation matrix that represents 180 degree rotation
        Matrix3d m1;
        m1 << -1, 0, 0,
               0,-1, 0,
               0, 0, 1;
        return m1;
    }

    /* For the angle between both inputs:
     * 1) The sine is the magnitude of their cross product.
     * 2) The cosine equals their dot product.
     */
    // sine must be calculated using original cross product
    double sine = u.norm();
    double cosine = vector0.dot(vector1);
    u.normalize();

    double ux = u[0];
    double uy = u[1];
    double uz = u[2];
    Matrix3d cross_product_matrix;
    cross_product_matrix << 0, -uz, uy,
                            uz,   0,-ux,
                           -uy,  ux,  0;

    Matrix3d part1 = Matrix3d::Identity();
    Matrix3d part2 = cross_product_matrix * sine;
    Matrix3d part3 = cross_product_matrix*cross_product_matrix * (1 - cosine);
    return part1 + part2 + part3;
}

I use the Eigen C++ library for linear algebra (available here): http://eigen.tuxfamily.org/index.php?title=Main_Page

Any help is appreciated. Thanks.

Answer 1

A one liner version consists in using Eigen's Quaternion:

return Matrix3d(Quaterniond::FromTwoVectors(v0,v1));