Rotation of 3D vector?

I have two vectors as Python li开发者_JAVA技巧sts and an angle. E.g.:

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

What is the best/easiest way to get the resulting vector when rotating the v vector around the axis?

The rotation should appear to be counter clockwise for an observer to whom the axis vector is pointing. This is called the right hand rule

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

Use scipy's Rotation.from_rotvec(). The argument is the rotation vector (a unit vector) multiplied by the rotation angle in rads.

from scipy.spatial.transform import Rotation
from numpy.linalg import norm


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

axis = axis / norm(axis)  # normalize the rotation vector first
rot = Rotation.from_rotvec(theta * axis)

new_v = rot.apply(v)  
print(new_v)    # results in [2.74911638 4.77180932 1.91629719]

There are several more ways to use Rotation based on what data you have about the rotation:

• from_quat Initialized from quaternions.

• from_dcm Initialized from direction cosine matrices.

• from_euler Initialized from Euler angles.

_{Off-topic note: One line code is not necessarily better code as implied by some users.}

I made a fairly complete library of 3D mathematics for Python{2,3}. It still does not use Cython, but relies heavily on the efficiency of numpy. You can find it here with pip:

python[3] -m pip install math3d

Or have a look at my gitweb http://git.automatics.dyndns.dk/?p=pymath3d.git and now also on github: https://github.com/mortlind/pymath3d .

Once installed, in python you may create the orientation object which can rotate vectors, or be part of transform objects. E.g. the following code snippet composes an orientation that represents a rotation of 1 rad around the axis [1,2,3], applies it to the vector [4,5,6], and prints the result:

import math3d as m3d
r = m3d.Orientation.new_axis_angle([1,2,3], 1)
v = m3d.Vector(4,5,6)
print(r * v)

The output would be

<Vector: (2.53727, 6.15234, 5.71935)>

This is more efficient, by a factor of approximately four, as far as I can time it, than the oneliner using scipy posted by B. M. above. However, it requires installation of my math3d package.

It can also be solved using quaternion theory:

def angle_axis_quat(theta, axis):
    """
    Given an angle and an axis, it returns a quaternion.
    """
    axis = np.array(axis) / np.linalg.norm(axis)
    return np.append([np.cos(theta/2)],np.sin(theta/2) * axis)

def mult_quat(q1, q2):
    """
    Quaternion multiplication.
    """
    q3 = np.copy(q1)
    q3[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]
    q3[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]
    q3[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]
    q3[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]
    return q3

def rotate_quat(quat, vect):
    """
    Rotate a vector with the rotation defined by a quaternion.
    """
    # Transfrom vect into an quaternion 
    vect = np.append([0],vect)
    # Normalize it
    norm_vect = np.linalg.norm(vect)
    vect = vect/norm_vect
    # Computes the conjugate of quat
    quat_ = np.append(quat[0],-quat[1:])
    # The result is given by: quat * vect * quat_
    res = mult_quat(quat, mult_quat(vect,quat_)) * norm_vect
    return res[1:]

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

print(rotate_quat(angle_axis_quat(theta, axis), v))
# [2.74911638 4.77180932 1.91629719]

Disclaimer: I am the author of this package

While special classes for rotations can be convenient, in some cases one needs rotation matrices (e.g. for working with other libraries like the affine_transform functions in scipy). To avoid everyone implementing their own little matrix generating functions, there exists a tiny pure python package which does nothing more than providing convenient rotation matrix generating functions. The package is on github (mgen) and can be installed via pip:

pip install mgen

Example usage copied from the readme:

import numpy as np
np.set_printoptions(suppress=True)

from mgen import rotation_around_axis
from mgen import rotation_from_angles
from mgen import rotation_around_x

matrix = rotation_from_angles([np.pi/2, 0, 0], 'XYX')
matrix.dot([0, 1, 0])
# array([0., 0., 1.])

matrix = rotation_around_axis([1, 0, 0], np.pi/2)
matrix.dot([0, 1, 0])
# array([0., 0., 1.])

matrix = rotation_around_x(np.pi/2)
matrix.dot([0, 1, 0])
# array([0., 0., 1.])

Note that the matrices are just regular numpy arrays, so no new data-structures are introduced when using this package.

Using pyquaternion is extremely simple; to install it (while still in python), run in your console:

import pip;
pip.main(['install','pyquaternion'])

Once installed:

  from pyquaternion import Quaternion
  v = [3,5,0]
  axis = [4,4,1]
  theta = 1.2 #radian
  rotated_v = Quaternion(axis=axis,angle=theta).rotate(v)

I needed to rotate a 3D model around one of the three axes {x, y, z} in which that model was embedded and this was the top result for a search of how to do this in numpy. I used the following simple function:

def rotate(X, theta, axis='x'):
  '''Rotate multidimensional array `X` `theta` degrees around axis `axis`'''
  c, s = np.cos(theta), np.sin(theta)
  if axis == 'x': return np.dot(X, np.array([
    [1.,  0,  0],
    [0 ,  c, -s],
    [0 ,  s,  c]
  ]))
  elif axis == 'y': return np.dot(X, np.array([
    [c,  0,  -s],
    [0,  1,   0],
    [s,  0,   c]
  ]))
  elif axis == 'z': return np.dot(X, np.array([
    [c, -s,  0 ],
    [s,  c,  0 ],
    [0,  0,  1.],
  ]))