Finding rotation matrix from two 3D vectors

Question

I'm trying to find the rotation matrix that maps one 3d vector to another with the same magnitude by rotating it around the origin. I'm doing it in python, but people on stack overflow don't seem to help. To start I have two methods: one to calculate an R matrix from two vectors and another to convert it to angles.

import numpy as np
import math
import matplotlib.pyplot as plt
class Rotation():
# convert R matrix to angles
def rotationMatrixToEulerAngles(R):
    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])

    singular = sy < 1e-6

    if  not singular :
        x = math.atan2(R[2,1] , R[2,2])
        y = math.atan2(-R[2,0], sy)
        z = math.atan2(R[1,0], R[0,0])
    else :
        x = math.atan2(-R[1,2], R[1,1])
        y = math.atan2(-R[2,0], sy)
        z = 0
    return np.array([math.degrees(x), math.degrees(y), math.degrees(z)])

# get R matrix from two vectors
def get_rot_matrix(A, B):
    assert A.shape == B.shape
    v = np.cross(A, B)
    s = np.linalg.norm(v)
    c = np.dot(A, B)
    vx = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) 
    r = np.eye(3) + vx + (np.dot(vx, vx) * (1-c)/(s**2))
    return r

I think the problem is with the calculation of the rotation matrix but I'm not sure. I based the get_rot_matrix() method off of this answer. I ran some tests and heres what I found:

For simple rotation (rotation about 1 axis) the angle is correct, but when I try to apply the R matrix to the original starting point I don't get the target ending point.

# --------------- SIMPLE ROTATION ---------------------
print("SIMPLE ROTATION")
create chart
fig = plt.figure(figsize=(8,8))
ax = fig.add_subplot(111, projection='3d')
add simple 90 degrees rotation to chart
ax.plot([0,0], [0,0],[-5,5]) # axis of rotation
ax.scatter(4,0,0) # start
ax.scatter(0,4,0) # end
find and print calculated rotation angle
start = np.array([4,0,0])
end = np.array([0,4,0])
R = Rotation.get_rot_matrix(start, end)
print("Angles: \t" + str(Rotation.rotationMatrixToEulerAngles(R)))
apply rotation matrix to start point
end_attempt = np.dot(R, start)
print("End attempt: \t" + end_attempt.str())
open chart
plt.title("Simple Rotation")
plt.xlabel("X")
plt.ylabel("Y")
plt.legend(["axis of Rotation", "start", "end"])
plt.show()

Output:

SIMPLE ROTATION
Angles:         [ 0. -0. 90.]
End attempt:    [ 0. 64.  0.]

As you can see the end attempt should be [0, 4, 0] not [0, 64, 0]. Heres the graph of the rotation I'm doing: simple_rot

For complex rotation (rotation about multiple axis') I know that the end attempt is not correct, but I am not sure if the angles are correct. I don't really know how to check if they are!

# --------------- COMPLEX ROTATION ---------------------
print("\nCOMPLEX ROTATION")
create chart
fig = plt.figure(figsize=(8,8))
ax = fig.add_subplot(111, projection='3d')
add complex rotation to chart
ax.scatter(0,0,0) # origin
ax.scatter(4,3,2) # start
pythag theroem to find end point on y axis
length = math.sqrt(sum(a**2 for a in (1,4,2)))
ax.scatter(0,length,0) # end
find and print calculated rotation angle
start = np.array([4,3,2])
end = np.array([0,length,0])
R = Rotation.get_rot_matrix(start, end)
print("Angles: \t" + str(Rotation.rotationMatrixToEulerAngles(R)))
apply rotation matrix to start point
end_attempt = np.dot(R, start)
print("End attempt: \t" + end_attempt.str())
open chart
plt.title("Complex Rotation")
plt.xlabel("X")
plt.ylabel("Y")
plt.legend(["origin", "start", "end"])
plt.show()

Output

COMPLEX ROTATION
Angles:         [-68.82925422 -13.3540114   58.57878746]
End attempt:    [5.32907052e-15 1.32894695e+02 2.66453526e-15]

Heres the graph: complex_rot

Answer 1

Rotating a vector $A$ to a vector $B$ having the same magnitude as $A$, is straight forward and not ambiguous at all.

The rotation you request is about an axis passing through the origin.

The unit vector along the axis lies in the plane passing through the origin, and having a normal vector equal to the vector $\vec{AB}$. There is an infinite number of such vectors. However the axis that minimizes the angle of rotation is the one that is along the cross product between $A$ and $B$. Hence, we'll select our axis of rotation to be the unit vector $a$

$ a = \dfrac{ A \times B }{\| A \times B \| }$

The angle of rotation $\theta$ is given by

$ \theta = \sin^{-1} \left( \dfrac{ \| A \times B \| }{ \| A \|^2 } \right) $

Now all you have to do is use the well-known Rodrigues' rotation matrix formula to compute your rotation matrix $R$

$R = {a a}^T + (I - {a a}^T ) \cos(\theta) + S_a \sin(\theta) $

where

$ S_a = \begin{bmatrix} 0 && - a_z && a_y \\ a_z && 0 && -a_x \\ -a_y && a_x && 0 \end{bmatrix} $

That's it!!

Now to verify the above, suppose you have vector $A = (1, -4, 8) $ and $B = (4, -8, 1) $, then you can carry out the above computations to end up with the matrix $R$, and then pre-multiply $A$ with the matrix $R$, and you should have

$ B = R A $

Answer 2

To rotate vector $\vec{a} = (a_x, a_y, a_z)$ to vector $\vec{b} = (b_x, b_y, b_z)$ when $\lVert\vec{a}\rVert = \lVert \vec{b} \rVert$, we need to rotate $\vec{a}$ around unit axis vector $\hat{n}$ by angle $\theta$, such that $$\begin{aligned} \hat{a} &= \frac{\vec{a}}{\left\lVert\vec{a}\right\rVert} = \frac{\vec{a}}{\sqrt{\vec{a} \cdot \vec{a}}} \\ \hat{b} &= \frac{\vec{b}}{\left\lVert\vec{a}\right\rVert} = \frac{\vec{b}}{\sqrt{\vec{b} \cdot \vec{b}}} \\ \hat{n} &= \frac{\vec{n}}{\left\lVert\vec{n}\right\rVert} = \frac{\vec{n}}{\sqrt{\vec{n} \cdot \vec{n}}} \\ \vec{n} &= \vec{a} \times \vec{b} \\ \sin \theta &= \left\lVert \hat{a} \times \hat{b} \right\rVert \quad \text{(not needed in this case)} \\ \cos \theta &= \hat{a} \cdot \hat{b} \\ \end{aligned}$$

Python implementation:

from math import sqrt, cos, sin, acos
def unit_axis_angle(a, b):
    an = sqrt(a[0]a[0] + a[1]a[1] + a[2]a[2])
    bn = sqrt(b[0]b[0] + b[1]b[1] + b[2]b[2])
    ax, ay, az = a[0]/an, a[1]/an, a[2]/an
    bx, by, bz = b[0]/bn, b[1]/bn, b[2]/bn
    nx, ny, nz = aybz-azby, azbx-axbz, axby-aybx
    nn = sqrt(nxnx + nyny + nznz)
    return (nx/nn, ny/nn, nz/nn), acos(axbx + ayby + azbz)

The returned axis and angle we can convert to a rotation matrix trivially. If our arrays describe column vectors, and we multiply matrices and vectors with matrix on the left, $$\mathbf{R} \vec{a} = \vec{c} \quad \iff \quad \begin{bmatrix} R_{11} & R_{12} & R_{13} \ R_{21} & R_{22} & R_{23} \ R_{31} & R_{32} & R_{33} \ \end{bmatrix} \begin{bmatrix} a_x \ a_y \ a_z \end{bmatrix} = \begin{bmatrix} c_x \ c_y \ c_z \end{bmatrix}$$ then

def rotation_matrix(axis, angle):
    ax, ay, az = axis[0], axis[1], axis[2]
    s = sin(angle)
    c = cos(angle)
    u = 1 - c
    return ( ( ax*ax*u + c,    ax*ay*u - az*s, ax*az*u + ay*s ),
             ( ay*ax*u + az*s, ay*ay*u + c,    ay*az*u - ax*s ),
             ( az*ax*u - ay*s, az*ay*u + ax*s, az*az*u + c    ) )
def multiply(matrix, vector):
    return ( matrix[0][0]vector[0] + matrix[0][1]vector[1] + matrix[0][2]vector[2],
             matrix[1][0]vector[0] + matrix[1][1]vector[1] + matrix[1][2]vector[2],
             matrix[2][0]vector[0] + matrix[2][1]vector[1] + matrix[2][2]*vector[2] )

Nothing beats an unit test. So, let's generate a couple of random vectors, scale the second to the same length as the first, and verify that when applied to the first vector, it yields the second vector, within rounding error:

from random import Random
rng = Random()
a = (rng.uniform(-5,5), rng.uniform(-5,5), rng.uniform(-5,5))
b = (rng.uniform(-5,5), rng.uniform(-5,5), rng.uniform(-5,5))
s = sqrt((a[0]a[0] + a[1]a[1] + a[2]a[2]) / (b[0]b[0] + b[1]b[1] + b[2]b[2]))
b = (b[0]s, b[1]s, b[2]*s)
axis, angle = unit_axis_angle(a, b)
c = multiply(rotation_matrix(axis, angle), a)
print("a = (%9.6f, %9.6f, %9.6f)" % a)
print("b = (%9.6f, %9.6f, %9.6f)" % b)
print("c = (%9.6f, %9.6f, %9.6f)" % c)

Note that the above functions returns tuples, which are immutable. If you want mutable results, just return lists ((..) $\to$ [..]) instead.