Rotation of 3D vector?

Using the Euler-Rodrigues formula:

import numpy as np
import math

def rotation_matrix(axis, theta):
    """
    Return the rotation matrix associated with counterclockwise rotation about
    the given axis by theta radians.
    """
    axis = np.asarray(axis)
    axis = axis / math.sqrt(np.dot(axis, axis))
    a = math.cos(theta / 2.0)
    b, c, d = -axis * math.sin(theta / 2.0)
    aa, bb, cc, dd = a * a, b * b, c * c, d * d
    bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d
    return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)],
                     [2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)],
                     [2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]])

v = [3, 5, 0]
axis = [4, 4, 1]
theta = 1.2 

print(np.dot(rotation_matrix(axis, theta), v)) 
# [ 2.74911638  4.77180932  1.91629719]

A one-liner, with numpy/scipy functions.

We use the following:

let a be the unit vector along axis, i.e. a = axis/norm(axis)
and A = I × a be the skew-symmetric matrix associated to a, i.e. the cross product of the identity matrix with a

then M = exp(θ A) is the rotation matrix.

from numpy import cross, eye, dot
from scipy.linalg import expm, norm

def M(axis, theta):
    return expm(cross(eye(3), axis/norm(axis)*theta))

v, axis, theta = [3,5,0], [4,4,1], 1.2
M0 = M(axis, theta)

print(dot(M0,v))
# [ 2.74911638  4.77180932  1.91629719]

expm (code here) computes the taylor series of the exponential:
\sum_{k=0}^{20} \frac{1}{k!} (θ A)^k , so it's time expensive, but readable and secure. It can be a good way if you have few rotations to do but a lot of vectors.

I just wanted to mention that if speed is required, wrapping unutbu's code in scipy's weave.inline and passing an already existing matrix as a parameter yields a 20-fold decrease in the running time.

The code (in rotation_matrix_test.py):

import numpy as np
import timeit

from math import cos, sin, sqrt
import numpy.random as nr

from scipy import weave

def rotation_matrix_weave(axis, theta, mat = None):
    if mat == None:
        mat = np.eye(3,3)

    support = "#include <math.h>"
    code = """
        double x = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]);
        double a = cos(theta / 2.0);
        double b = -(axis[0] / x) * sin(theta / 2.0);
        double c = -(axis[1] / x) * sin(theta / 2.0);
        double d = -(axis[2] / x) * sin(theta / 2.0);

        mat[0] = a*a + b*b - c*c - d*d;
        mat[1] = 2 * (b*c - a*d);
        mat[2] = 2 * (b*d + a*c);

        mat[3*1 + 0] = 2*(b*c+a*d);
        mat[3*1 + 1] = a*a+c*c-b*b-d*d;
        mat[3*1 + 2] = 2*(c*d-a*b);

        mat[3*2 + 0] = 2*(b*d-a*c);
        mat[3*2 + 1] = 2*(c*d+a*b);
        mat[3*2 + 2] = a*a+d*d-b*b-c*c;
    """

    weave.inline(code, ['axis', 'theta', 'mat'], support_code = support, libraries = ['m'])

    return mat

def rotation_matrix_numpy(axis, theta):
    mat = np.eye(3,3)
    axis = axis/sqrt(np.dot(axis, axis))
    a = cos(theta/2.)
    b, c, d = -axis*sin(theta/2.)

    return np.array([[a*a+b*b-c*c-d*d, 2*(b*c-a*d), 2*(b*d+a*c)],
                  [2*(b*c+a*d), a*a+c*c-b*b-d*d, 2*(c*d-a*b)],
                  [2*(b*d-a*c), 2*(c*d+a*b), a*a+d*d-b*b-c*c]])

The timing:

>>> import timeit
>>> 
>>> setup = """
... import numpy as np
... import numpy.random as nr
... 
... from rotation_matrix_test import rotation_matrix_weave
... from rotation_matrix_test import rotation_matrix_numpy
... 
... mat1 = np.eye(3,3)
... theta = nr.random()
... axis = nr.random(3)
... """
>>> 
>>> timeit.repeat("rotation_matrix_weave(axis, theta, mat1)", setup=setup, number=100000)
[0.36641597747802734, 0.34883809089660645, 0.3459300994873047]
>>> timeit.repeat("rotation_matrix_numpy(axis, theta)", setup=setup, number=100000)
[7.180983066558838, 7.172032117843628, 7.180462837219238]

Here is an elegant method using quaternions that are blazingly fast; I can calculate 10 million rotations per second with appropriately vectorised numpy arrays. It relies on the quaternion extension to numpy found here.

Quaternion Theory: A quaternion is a number with one real and 3 imaginary dimensions usually written as q = w + xi + yj + zk where 'i', 'j', 'k' are imaginary dimensions. Just as a unit complex number 'c' can represent all 2d rotations by c=exp(i * theta), a unit quaternion 'q' can represent all 3d rotations by q=exp(p), where 'p' is a pure imaginary quaternion set by your axis and angle.

We start by converting your axis and angle to a quaternion whose imaginary dimensions are given by your axis of rotation, and whose magnitude is given by half the angle of rotation in radians. The 4 element vectors (w, x, y, z) are constructed as follows:

import numpy as np
import quaternion as quat

v = [3,5,0]
axis = [4,4,1]
theta = 1.2 #radian

vector = np.array([0.] + v)
rot_axis = np.array([0.] + axis)
axis_angle = (theta*0.5) * rot_axis/np.linalg.norm(rot_axis)

First, a numpy array of 4 elements is constructed with the real component w=0 for both the vector to be rotated vector and the rotation axis rot_axis. The axis angle representation is then constructed by normalizing then multiplying by half the desired angle theta. See here for why half the angle is required.

Now create the quaternions v and qlog using the library, and get the unit rotation quaternion q by taking the exponential.

vec = quat.quaternion(*v)
qlog = quat.quaternion(*axis_angle)
q = np.exp(qlog)

Finally, the rotation of the vector is calculated by the following operation.

v_prime = q * vec * np.conjugate(q)

print(v_prime) # quaternion(0.0, 2.7491163, 4.7718093, 1.9162971)

Now just discard the real element and you have your rotated vector!

v_prime_vec = v_prime.imag # [2.74911638 4.77180932 1.91629719] as a numpy array

Note that this method is particularly efficient if you have to rotate a vector through many sequential rotations, as the quaternion product can just be calculated as q = q1 * q2 * q3 * q4 * ... * qn and then the vector is only rotated by 'q' at the very end using v' = q * v * conj(q).

This method gives you a seamless transformation between axis angle <---> 3d rotation operator simply by exp and log functions (yes log(q) just returns the axis-angle representation!). For further clarification of how quaternion multiplication etc. work, see here

Take a look at http://vpython.org/contents/docs/visual/VisualIntro.html.

It provides a vector class which has a method A.rotate(theta,B). It also provides a helper function rotate(A,theta,B) if you don't want to call the method on A.

http://vpython.org/contents/docs/visual/vector.html