3D矢量的旋转？

Question

我有两个向量作为 Python 列表和一个角度。例如：

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

当围绕轴旋转 v 向量时，获得结果向量的最佳/最简单方法是什么？

对于轴矢量指向的观察者来说，旋转应该是逆时针的。这称为right hand rule

Answer 1

使用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]

Answer 2

单线，具有 numpy/scipy 函数。

我们使用以下内容：

设a为沿axis的单位向量，即a = axis/norm(axis)
A = I × a 是与 a 相关的斜对称矩阵，即单位矩阵与 a

的叉积>

那么M = exp(θ A)就是旋转矩阵。

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) 计算指数的泰勒级数：
\sum_{k=0}^{20} \frac{1}{k!} (θ A)^k ，所以它的时间很昂贵，但可读且安全。 如果您要做的旋转很少但向量很多，这可能是一个好方法。

Answer 3

我只是想提一下，如果需要速度，将 unutbu 的代码包装在 scipy 的 weave.inline 中并将已经存在的矩阵作为参数传递会导致运行时间减少 20 倍。

代码（在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]])

时间安排：

>>> 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]

Answer 4

这是一种使用速度极快的四元数的优雅方法；我可以使用适当的矢量化 numpy 数组计算每秒 1000 万次旋转。它依赖于 numpy 找到的四元数扩展 here。

四元数理论： 四元数是一个具有一个实维和 3 个虚维的数字，通常写为q = w + xi + yj + zk，其中“i”、“j”、“k”是虚维。正如单位复数“c”可以用c=exp(i * theta) 表示所有2d 旋转一样，单位四元数“q”可以用q=exp(p) 表示所有3d 旋转，其中“p”是由轴和角度设置的纯虚数四元数.

我们首先将您的轴和角度转换为四元数，其虚数由您的旋转轴给出，其大小由旋转角度的一半给出，以弧度表示。 4个元素向量(w, x, y, z)构造如下：

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)

首先，为要旋转的向量vector 和旋转轴rot_axis 构造一个实部w=0 的4 个元素的numpy 数组。然后通过归一化然后乘以所需角度theta 的一半来构造轴角度表示。请参阅here 了解为什么需要半角。

现在使用库创建四元数v和qlog，并通过取指数得到单位旋转四元数q。

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

最后通过下面的操作计算向量的旋转。

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

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

现在只需丢弃真实元素，您就有了旋转矢量！

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

请注意，如果您必须通过多次连续旋转来旋转向量，则此方法特别有效，因为四元数积可以计算为 q = q1 * q2 * q3 * q4 * ... * qn 然后向量仅在最后使用 v' = q * v * conj(q) 旋转 'q'。

此方法为您提供轴角 3d 旋转运算符之间的无缝转换，只需通过exp 和log 函数（是的log(q) 只返回轴角表示！）。有关四元数乘法等如何工作的进一步说明，请参阅here

Answer 5

看看http://vpython.org/contents/docs/visual/VisualIntro.html。

它提供了一个vector 类，该类有一个方法A.rotate(theta,B)。如果您不想调用A 上的方法，它还提供了一个帮助函数rotate(A,theta,B)。

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

Answer 6

我为 Python 制作了一个相当完整的 3D 数学库{2,3}。它仍然不使用 Cython，但严重依赖 numpy 的效率。你可以在这里用 pip 找到它：

python[3] -m pip install math3d

或者看看我的 gitweb http://git.automatics.dyndns.dk/?p=pymath3d.git，现在也在 github 上：https://github.com/mortlind/pymath3d。

安装后，您可以在 python 中创建可以旋转矢量的方向对象，或者成为变换对象的一部分。例如。以下代码 sn-p 组成一个方向，表示围绕轴 [1,2,3] 旋转 1 rad，将其应用于向量 [4,5,6]，并打印结果：

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

输出将是

<Vector: (2.53727, 6.15234, 5.71935)>

据我所知，这比上面 B. M. 发布的使用 scipy 的 oneliner 效率高大约四倍。但是，它需要安装我的 math3d 包。

Answer 7

使用 scipy 的 Rotation.from_rotvec()。参数是旋转向量（单位向量）乘以旋转角度（以弧度为单位）。

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]

根据您拥有的关于轮换的数据，还有几种使用 Rotation 的方法：

• from_quat 从四元数初始化。

• from_dcm 从方向余弦矩阵初始化。

• from_euler 从欧拉角初始化。

---

_{题外话：一行代码不一定某些用户暗示的更好的代码。}

Answer 8

也可以用四元数理论求解：

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]

Answer 9

免责声明：我是这个包的作者

虽然旋转的特殊类可能很方便，但在某些情况下需要旋转矩阵（例如，与其他库一起使用，如 scipy 中的 affine_transform 函数）。为了避免每个人都实现自己的小矩阵生成函数，有一个很小的纯 python 包，它只是提供方便的旋转矩阵生成函数。包在github上（mgen），可以通过pip安装：

pip install mgen

从自述文件中复制的示例用法：

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.])

请注意，矩阵只是常规的 numpy 数组，因此在使用此包时不会引入新的数据结构。

Answer 10

使用pyquaternion非常简单；要安装它（仍然在 python 中），请在控制台中运行：

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

安装后：

  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)

Answer 11

我需要围绕嵌入该模型的三个轴 {x,y,z} 之一旋转 3D 模型，这是在 numpy.xml 中搜索如何执行此操作的最佳结果。我使用了以下简单功能：

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.],
  ]))