3D矢量的旋转？

Question

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

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

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

对于轴向量指向的观察者,旋转应该是逆时针的.这被称为右手规则

Answer 1

使用Euler-Rodrigues公式:

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是沿轴的单位向量,即a =轴/范数(轴)
和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 (这里的代码)计算指数的泰勒系列:
\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的四元数扩展.

四元数理论:四元数是一个具有一个实数和三个虚数维的数字,通常写为q = w + xi + yj + zk"i","j","k"是虚数维.正如单位复数'c'可以表示所有2d旋转c=exp(i * theta),单位四元数'q'可以表示所有3d旋转q=exp(p),其中'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)

首先,构造4个元素的numpy阵列,对于要旋转的矢量vector和旋转轴,实部分w = 0 rot_axis.然后通过归一化然后乘以所需角度的一半来构造轴角度表示theta.请参阅此处了解为什么需要一半的角度.

现在创建四元数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'.

这种方法简单地通过exp和log函数(是的,log(q)只返回轴角表示!)为您提供轴角<---> 3d旋转操作符之间的无缝转换.有关四元数乘法等的工作原理的进一步说明,请参见此处

Answer 5

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

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

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

Answer 6

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

我为Python {2,3}创建了一个相当完整的3D数学库.它仍然没有使用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中创建可以旋转矢量的方向对象,或者成为变换对象的一部分.例如,下面的代码片段组成一个方向,表示围绕轴[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)>

就使用BM上面发布的scipy的oneliner而言,这比我使用它的时间大约四倍更高效.但是,它需要安装我的math3d包.