通过四元数旋转坐标系

Question

我们有很多很多的空间坐标（x, y 和 z），这些坐标代表了三维空间中的原子。我正在构建一个函数，用来把这些点转换到一个新的坐标系统。把坐标移动到一个任意的原点是比较简单的，但我对下一步有点搞不清楚：三维点的旋转计算。换句话说，我想把点从 (x, y, z) 转换到 (x', y', z')，其中 x'、y' 和 z' 是用我创建的新坐标轴 i'、j' 和 k' 来表示的，我是借助 euclid python 模块 来实现的。

我觉得我只需要一个 euclid 四元数来做到这一点，也就是：

>>> q * Vector3(x, y, z)
Vector3(x', y', z')

但是为了生成这个四元数，我认为我需要一个旋转轴向量和一个旋转角度。不过，我不知道如何从 i'、j' 和 k' 计算出这些。这看起来像是一个简单的编码过程，但我怀疑这需要一些线性代数的知识才能自己搞明白。非常感谢你能给我一点指引。

Answer 1

注意，矩阵的逆并不是那么简单！首先，所有的点（假设有n个点，n是你空间的维度）必须处于一般位置（也就是说，任何一个点不能仅仅通过其他点的线性组合来表示【注意：这看起来似乎是个简单的要求，但在数值线性代数的领域，这并不简单；最终是否存在这样的配置，还是要依赖于具体领域的知识】）。

另外，新旧点之间的“对应关系”可能并不完全准确（这时候你应该使用最好的方法来近似这个“真实对应关系”）。当你的库提供伪逆时，建议总是使用伪逆，而不是直接使用普通的逆。

伪逆的好处在于，你可以使用更多的点进行变换，这样就增加了至少有n个点处于一般位置的可能性。

这里有个例子，二维空间中将单位正方形逆时针旋转90度（显然这个方法在任何维度都适用），使用的是numpy：

In []: P=  matrix([[0, 0, 1, 1],
                   [0, 1, 1, 0]])
In []: Pn= matrix([[0, -1, -1, 0],
                   [0,  0,  1, 1]])
In []: T= Pn* pinv(P)
In []: (T* P).round()
Out[]:
matrix([[ 0., -1., -1.,  0.],
        [ 0.,  0.,  1.,  1.]])

附注：numpy的速度也很快。在我这台普通电脑上，处理100万个点：

In []: P= matrix(rand(2, 1e6))
In []: %timeit T* P
10 loops, best of 3: 37.7 ms per loop

Answer 2

这个问题和@senderle给出的答案对我的一个项目帮助很大。这个答案简洁明了，涵盖了大多数人可能需要进行的四元数计算的核心内容。

在我的项目中，我觉得每次都要为所有操作写单独的函数，并且一个个导入它们，实在是太麻烦了，所以我实现了一个面向对象的版本。

quaternion.py:

import numpy as np
from math import sin, cos, acos, sqrt

def normalize(v, tolerance=0.00001):
    mag2 = sum(n * n for n in v)
    if abs(mag2 - 1.0) > tolerance:
        mag = sqrt(mag2)
        v = tuple(n / mag for n in v)
    return np.array(v)

class Quaternion:

    def from_axisangle(theta, v):
        theta = theta
        v = normalize(v)

        new_quaternion = Quaternion()
        new_quaternion._axisangle_to_q(theta, v)
        return new_quaternion

    def from_value(value):
        new_quaternion = Quaternion()
        new_quaternion._val = value
        return new_quaternion

    def _axisangle_to_q(self, theta, v):
        x = v[0]
        y = v[1]
        z = v[2]

        w = cos(theta/2.)
        x = x * sin(theta/2.)
        y = y * sin(theta/2.)
        z = z * sin(theta/2.)

        self._val = np.array([w, x, y, z])

    def __mul__(self, b):

        if isinstance(b, Quaternion):
            return self._multiply_with_quaternion(b)
        elif isinstance(b, (list, tuple, np.ndarray)):
            if len(b) != 3:
                raise Exception(f"Input vector has invalid length {len(b)}")
            return self._multiply_with_vector(b)
        else:
            raise Exception(f"Multiplication with unknown type {type(b)}")

    def _multiply_with_quaternion(self, q2):
        w1, x1, y1, z1 = self._val
        w2, x2, y2, z2 = q2._val
        w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
        x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
        y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
        z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2

        result = Quaternion.from_value(np.array((w, x, y, z)))
        return result

    def _multiply_with_vector(self, v):
        q2 = Quaternion.from_value(np.append((0.0), v))
        return (self * q2 * self.get_conjugate())._val[1:]

    def get_conjugate(self):
        w, x, y, z = self._val
        result = Quaternion.from_value(np.array((w, -x, -y, -z)))
        return result

    def __repr__(self):
        theta, v = self.get_axisangle()
        return f"((%.6f; %.6f, %.6f, %.6f))"%(theta, v[0], v[1], v[2])

    def get_axisangle(self):
        w, v = self._val[0], self._val[1:]
        theta = acos(w) * 2.0

        return theta, normalize(v)

    def tolist(self):
        return self._val.tolist()

    def vector_norm(self):
        w, v = self.get_axisangle()
        return np.linalg.norm(v)

在这个版本中，你只需要使用重载的运算符，就可以进行四元数与四元数之间，或者四元数与向量之间的乘法运算。

from quaternion import Quaternion
import numpy as np

x_axis_unit = (1, 0, 0)
y_axis_unit = (0, 1, 0)
z_axis_unit = (0, 0, 1)

r1 = Quaternion.from_axisangle(np.pi / 2, x_axis_unit)
r2 = Quaternion.from_axisangle(np.pi / 2, y_axis_unit)
r3 = Quaternion.from_axisangle(np.pi / 2, z_axis_unit)

# Quaternion - vector multiplication
v = r1 * y_axis_unit
v = r2 * v
v = r3 * v

print(v)

# Quaternion - quaternion multiplication
r_total = r3 * r2 * r1
v = r_total * y_axis_unit

print(v)

我并不打算实现一个完整的四元数模块，所以这个版本主要是为了教学目的，就像@senderle的精彩回答一样。我希望这能帮助那些想要理解和尝试四元数新东西的人。

Answer 3

用四元数来表示旋转，从数学的角度来看并不难。个人来说，我觉得用视觉来理解四元数有点困难，但使用它们进行旋转的公式并不复杂。这里我会提供一组基本的参考函数。¹（另外，推荐看看hosolmaz的精彩回答，他把这些功能整合成了一个方便的四元数类。）

在这里，你可以把四元数想象成一个标量加上一个三维向量——抽象地说，就是 w + xi + yj + zk，这里用普通的元组 (w, x, y, z) 来表示。三维旋转的空间完全由四元数的一个子空间表示，这个子空间叫做单位四元数，所以你需要确保你的四元数是标准化的。你可以用和标准化任何四维向量一样的方法来做到这一点（也就是说，大小应该接近1；如果不是，就把值按大小缩小）：

def normalize(v, tolerance=0.00001):
    mag2 = sum(n * n for n in v)
    if abs(mag2 - 1.0) > tolerance:
        mag = sqrt(mag2)
        v = tuple(n / mag for n in v)
    return v

请注意，为了简单起见，以下函数假设四元数的值是已经标准化的。实际上，你需要不时地重新标准化它们，但处理这个问题的最佳方法取决于具体情况。这些函数仅提供最基本的参考。

每个旋转都由一个单位四元数表示，而多个旋转的组合对应于单位四元数的乘法。这个公式²如下：

def q_mult(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return w, x, y, z

要用四元数旋转一个向量，你还需要四元数的共轭：

def q_conjugate(q):
    w, x, y, z = q
    return (w, -x, -y, -z)

四元数和向量的乘法就是把你的向量转换成一个四元数（通过设置 w = 0，并保持 x、y 和 z 不变），然后计算 q * v * q_conjugate(q)：

def qv_mult(q1, v1):
    q2 = (0.0,) + v1
    return q_mult(q_mult(q1, q2), q_conjugate(q1))[1:]
    

最后，你需要知道如何将轴角旋转转换为四元数，反之亦然。这也出乎意料地简单。这里“清理”输入和输出是有意义的，可以调用 normalize。

def axisangle_to_q(v, theta):
    v = normalize(v)
    x, y, z = v
    theta /= 2
    w = cos(theta)
    x = x * sin(theta)
    y = y * sin(theta)
    z = z * sin(theta)
    return w, x, y, z

反向转换：

def q_to_axisangle(q):
    w, v = q[0], q[1:]
    theta = acos(w) * 2.0
    return normalize(v), theta

这里有个快速的使用示例。围绕 x、y 和 z 轴进行90度旋转的序列会将一个位于 y 轴的向量返回到它的原始位置。以下代码执行这些旋转：

x_axis_unit = (1, 0, 0)
y_axis_unit = (0, 1, 0)
z_axis_unit = (0, 0, 1)
r1 = axisangle_to_q(x_axis_unit, numpy.pi / 2)
r2 = axisangle_to_q(y_axis_unit, numpy.pi / 2)
r3 = axisangle_to_q(z_axis_unit, numpy.pi / 2)

v = qv_mult(r1, y_axis_unit)
v = qv_mult(r2, v)
v = qv_mult(r3, v)

print v
# output: (0.0, 1.0, 2.220446049250313e-16)

请记住，这个旋转序列不会将所有向量返回到同一个位置；例如，对于一个位于 x 轴的向量，它将对应于围绕 y 轴的90度旋转。（在这里可以想象一下右手法则；围绕 y 轴的正向旋转会将位于 x 轴的向量推向负 z 区域。）

v = qv_mult(r1, x_axis_unit)
v = qv_mult(r2, v)
v = qv_mult(r3, v)

print v
# output: (4.930380657631324e-32, 2.220446049250313e-16, -1.0)

如往常一样，如果你发现这里有任何问题，请告诉我。

---

^{1. 这些内容改编自一个 OpenGL 教程，存档在这里。}

^{2. 四元数乘法公式乍一看像个麻烦的乱麻，但推导其实很简单，虽然有点繁琐。用纸和笔，你可以这样表示两个四元数： w + xi + yj + zk。然后注意到 ii = jj = kk = -1；ij = k，jk = i，ki = j；以及 ji = -k，kj = -i，ik = -j。最后，乘以这两个四元数，分配出各个项并根据16次乘法的结果重新排列。这有助于说明为什么可以用四元数来表示旋转；最后六个恒等式遵循右手法则，创建了从 i 到 j 的旋转和围绕 k 的旋转之间的双射关系，依此类推。}

^{如果你这样做，你会发现恒等式 ii = jj = kk = -1 解释了 w 公式中的最后三项；jk = i 解释了 x 公式中的第三项；而 kj = -i 解释了 x 公式中的第四项。y 和 z 公式也是同样的道理。其余的项都是普通的标量乘法的例子。}