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
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:
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
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:
Note that the above functions returns tuples, which are immutable. If you want mutable results, just return lists (
(..)$\to$[..]) instead.