Efficient way to calculate a 3x3 rotation matrix from the rotation defined by two 3D Vectors

Question

While I've found 2 solutions to this, I was curious if there is well known method to perform this operation since it seems a fairly common task.

Here are the 2 obvious methods psudo-code...

Axis Angle

This is quite logical, but calls sin twice and cos once (in the angle calculation and axis angle to matrix conversion).

Matrix3x3 rotation_between_vectors_to_matrix(const Vector v1, const Vector v2)
{
    angle = v1.angle(v2);
    axis  = v1.cross(v2);

    /* maths for this is well known */
    Matrix3x3 matrix = axis_angle_to_matrix(axis, angle);

    return matrix;
}

Edit: The most straightforward function is a quite slow, however as has been pointed out in the replies here: calculating the angle can be avoided by getting angle_sin and angle_cos, from the axis length and v1,v2 dot product respectively.

Difference between two matrices

Here's another method I found which constructs two 3x3 matrices from the vectors and returns the difference.

However this is slower then axis/angle calculation which can be optimized (mentioned above).

Note. this assumes both vectors are normalized, matrix is column-major (OpenGL).

Matrix3x3 rotation_between_vectors_to_matrix(const Vector v1, const Vector v2)
{
    Matrix3x3 m1, m2;

    axis = v1.cross(v2);
    axis.normalize();

    /* construct 2 matrices */
    m1[0] = v1;
    m2[0] = v2;

    m1[1] = axis;
    m2[1] = axis;

    m1[2] = m1[1].cross(m1[0]);
    m2[2] = m2[1].cross(m2[0]);

    /* calculate the difference between m1 and m2 */
    m1.transpose();

    Matrix3x3 matrix = m2 * m1;

    return matrix;
}

Are there better ways to perform this calculation?

Edit: The purpose of this question is NOT to micro-optimize and benchmark each method. Instead - I was curious if there is some totally different and superior method which I didn't know about.

---

Note: I purposefully left out checks for the degenerate case for co-linear vectors (where the axis is zero length), to keep the examples simple.

Robalo

@legends2k, not as far as I know, I just found this myself while looking into better alternatives to the axis/angle method. (its well tested and got it working in degenerate cases too) – Robalo

Shoudn't you get the same result if you write m1.transpose(); matrix = m2 * m1; return matrix; ? Would save one transpose operation. – Ankylose

@ideasman42: Yeah, got it. Let A and B be the matrices, with some matrix X as the difference between them, then AX = B ⇒ X = A⁻¹B. Since pure rotation is represented by an orthogonal matrix, its transpose is its inverse. I think the 2nd transpose is redundant, if you transpose m1 and then multiply it with m2. – Ropedancer

@legends2k: In a rotation the rotation matrix comes first and the matrix/vector that is being rotated second. So it's XA = B⇒X = BA⁻¹ – Ankylose

@SpiderPig, your right, removed redundant transpose. – Robalo

@SpiderPig That depends on if you're having row vectors or column vectors. The OP is using column vectors, hence your form is appropriate. I gave the answer with that convention but the comment with row vector convention. Either ways, the second transpose was redundant. – Ropedancer

Answer 1

Both the methods you've posted can be optimised.

Method 1

Instead of using acos to find the angle between the two vectors, a better thing to do is to avoid finding the angle at all. How? The axis-angle formula by Rodrigues requires only sin θ, cos θ and 1 - cos θ, so finding the actual angle is redundant.

We know that v1 and v2 are unit vectors; v1 · v2 = |v1| |v2| cos θ since |v1| = |v2| = 1, v1 · v2 directly gives us cos θ, finding 1 - cos θ isn't costly. v1 × v2 = |v1| |v2| sin θ n = sin θ n, where n is a unit vector perpendicular to v1 and v2, finding |v1 × v2| the magnitude of the cross product would directly give sin θ.

Now that we've sin θ and cos θ, we can directly form the rotation matrix by using Rodrigues forumla; here's a simple implementation (though page claims to use Quaternion math, it's the Axis-Angle to Matrix conversion formula).

Method 2

After you've constructed two orthonormal frames as matrices, you can avoid the second transpose you do. Here's the proof.

Say A and B be the two matrices, since you want to rotate from A to B we need some matrix X which when multiplied with A will give B:

XA = B

X = BA⁻¹

This is all you need; when you pre-multiply X to A you'd get B. However, what you find is Y

Y = AB⁻¹

YB = A

Then you transpose Y to get Y⁻¹ i.e.

Y⁻¹YB = Y⁻¹A

B = Y⁻¹A

Instead of doing two inverses (transpose here), you can just do the above method which involves only one transpose.

I'd still say that without benchmarking the methods in their optimized forms, we cannot say method 2 is faster than method 1. So I'd really urge you to benchmark between the two methods (with some non-trivial load) and then draw a conclusion.

Ropedancer

Answer 2

The axis angle method is the fastest method, heres the C code I came up with for efficient axis/angle to 3x3 matrix conversion.

This checks for co-linear cases too.

Note: If you have your own math library, you can probably get rotation_between_vecs_to_mat3 working without any of the associated functions included for completeness.

#include <math.h>
#include <float.h>


/* -------------------------------------------------------------------- */
/* Math Lib declarations */

static void unit_m3(float m[3][3]);
static float dot_v3v3(const float a[3], const float b[3]);
static float normalize_v3(float n[3]);
static void cross_v3_v3v3(float r[3], const float a[3], const float b[3]);
static void mul_v3_v3fl(float r[3], const float a[3], float f);
static void ortho_v3_v3(float p[3], const float v[3]);
static void axis_angle_normalized_to_mat3_ex(
        float mat[3][3], const float axis[3],
        const float angle_sin, const float angle_cos);


/* -------------------------------------------------------------------- */
/* Main function */
void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float v2[3]);

/**
 * Calculate a rotation matrix from 2 normalized vectors.
 *
 * v1 and v2 must be unit length.
 */
void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float v2[3])
{
    float axis[3];
    /* avoid calculating the angle */
    float angle_sin;
    float angle_cos;

    cross_v3_v3v3(axis, v1, v2);

    angle_sin = normalize_v3(axis);
    angle_cos = dot_v3v3(v1, v2);

    if (angle_sin > FLT_EPSILON) {
axis_calc:
        axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos);
    }
    else {
        /* Degenerate (co-linear) vectors */
        if (angle_cos > 0.0f) {
            /* Same vectors, zero rotation... */
            unit_m3(m);
        }
        else {
            /* Colinear but opposed vectors, 180 rotation... */
            ortho_v3_v3(axis, v1);
            normalize_v3(axis);
            angle_sin =  0.0f;  /* sin(M_PI) */
            angle_cos = -1.0f;  /* cos(M_PI) */
            goto axis_calc;
        }
    }
}


/* -------------------------------------------------------------------- */
/* Math Lib */

static void unit_m3(float m[3][3])
{
    m[0][0] = m[1][1] = m[2][2] = 1.0;
    m[0][1] = m[0][2] = 0.0;
    m[1][0] = m[1][2] = 0.0;
    m[2][0] = m[2][1] = 0.0;
}

static float dot_v3v3(const float a[3], const float b[3])
{
    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

static void cross_v3_v3v3(float r[3], const float a[3], const float b[3])
{
    r[0] = a[1] * b[2] - a[2] * b[1];
    r[1] = a[2] * b[0] - a[0] * b[2];
    r[2] = a[0] * b[1] - a[1] * b[0];
}

static void mul_v3_v3fl(float r[3], const float a[3], float f)
{
    r[0] = a[0] * f;
    r[1] = a[1] * f;
    r[2] = a[2] * f;
}

static float normalize_v3_v3(float r[3], const float a[3])
{
    float d = dot_v3v3(a, a);

    if (d > 1.0e-35f) {
        d = sqrtf(d);
        mul_v3_v3fl(r, a, 1.0f / d);
    }
    else {
        d = r[0] = r[1] = r[2] = 0.0f;
    }

    return d;
}

static float normalize_v3(float n[3])
{
    return normalize_v3_v3(n, n);
}

static int axis_dominant_v3_single(const float vec[3])
{
    const float x = fabsf(vec[0]);
    const float y = fabsf(vec[1]);
    const float z = fabsf(vec[2]);
    return ((x > y) ?
           ((x > z) ? 0 : 2) :
           ((y > z) ? 1 : 2));
}

static void ortho_v3_v3(float p[3], const float v[3])
{
    const int axis = axis_dominant_v3_single(v);

    switch (axis) {
        case 0:
            p[0] = -v[1] - v[2];
            p[1] =  v[0];
            p[2] =  v[0];
            break;
        case 1:
            p[0] =  v[1];
            p[1] = -v[0] - v[2];
            p[2] =  v[1];
            break;
        case 2:
            p[0] =  v[2];
            p[1] =  v[2];
            p[2] = -v[0] - v[1];
            break;
    }
}

/* axis must be unit length */
static void axis_angle_normalized_to_mat3_ex(
        float mat[3][3], const float axis[3],
        const float angle_sin, const float angle_cos)
{
    float nsi[3], ico;
    float n_00, n_01, n_11, n_02, n_12, n_22;

    ico = (1.0f - angle_cos);
    nsi[0] = axis[0] * angle_sin;
    nsi[1] = axis[1] * angle_sin;
    nsi[2] = axis[2] * angle_sin;

    n_00 = (axis[0] * axis[0]) * ico;
    n_01 = (axis[0] * axis[1]) * ico;
    n_11 = (axis[1] * axis[1]) * ico;
    n_02 = (axis[0] * axis[2]) * ico;
    n_12 = (axis[1] * axis[2]) * ico;
    n_22 = (axis[2] * axis[2]) * ico;

    mat[0][0] = n_00 + angle_cos;
    mat[0][1] = n_01 + nsi[2];
    mat[0][2] = n_02 - nsi[1];
    mat[1][0] = n_01 - nsi[2];
    mat[1][1] = n_11 + angle_cos;
    mat[1][2] = n_12 + nsi[0];
    mat[2][0] = n_02 + nsi[1];
    mat[2][1] = n_12 - nsi[0];
    mat[2][2] = n_22 + angle_cos;
}

Robalo

Your answer is very helpful for me. But could you explain what is happening in the co-linear case when you have 180 degree rotation? I see that you create a new axis using the ortho_v3_v3() function, but how is this axis chosen? I don't understand the axis_dominant_v3_single() function, as well as the switch block. Any insight would be great, thanks. – Colporteur

I mean, can't you just rotate around any axis which is perpedicular to v1 ? – Colporteur

For the 180 degree rotation you can rotate around any perpendicular axis. – Robalo

Yes, I am wondering what your code does: you have this function which talks about "dominant axis"? – Colporteur

If you have questions about how to get an orthogonal axis, it would make more sense to ask a separate question about this. – Robalo

It's not really about an orthogonal axis in general. It's more about your x>y, x>z conditions, and the switch block. – Colporteur