Python 通过四元数旋转坐标系

rotating coordinate system via a quaternion

提问by Fancypants_MD

We have a gazillion spatial coordinates (x, y and z) representing atoms in 3d space, and I'm constructing a function that will translate these points to a new coordinate system. Shifting the coordinates to an arbitrary origin is simple, but I can't wrap my head around the next step: 3d point rotation calculations. In other words, I'm trying to translate the points from (x, y, z) to (x', y', z'), where x', y' and z' are in terms of i', j' and k', the new axis vectors I'm making with the help of the euclid python module.

我们有无数个空间坐标（x、y 和 z）表示 3d 空间中的原子，我正在构建一个函数，将这些点转换为新的坐标系。将坐标移动到任意原点很简单，但我无法理解下一步：3d 点旋转计算。换句话说，我试图将点从 (x, y, z) 转换为 (x', y', z')，其中 x', y' 和 z' 是根据 i', j'和 k'，我在euclid python 模块的帮助下制作的新轴向量。

I thinkall I need is a euclid quaternion to do this, i.e.

我认为我需要的只是一个欧几里得四元数来做到这一点，即

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

but to make THAT i believe I need a rotation axis vector and an angle of rotation. But I have no idea how to calculate these from i', j' and k'. This seems like a simple procedure to code from scratch, but I suspect something like this requires linear algebra to figure out on my own. Many thanks for a nudge in the right direction.

但要做到这一点，我相信我需要一个旋转轴矢量和一个旋转角度。但我不知道如何从 i'、j' 和 k' 计算这些。这似乎是一个从头开始编写代码的简单过程，但我怀疑这样的事情需要线性代数来自己解决。非常感谢您对正确方向的推动。

采纳答案by senderle

Using quaternions to represent rotation is not difficult from an algebraic point of view. Personally, I find it hard to reason visuallyabout quaternions, but the formulas involved in using them for rotations are quite simple. I'll provide a basic set of reference functions here.¹(See also this lovely answer by hosolmaz, in which he packages these together to create a handy Quaternion class.)

从代数的角度来看，使用四元数来表示旋转并不困难。就我个人而言，我发现很难从视觉上推理四元数，但使用它们进行旋转所涉及的公式非常简单。我将在这里提供一组基本的参考函数。¹（另请参阅hosolmaz 的这个可爱的答案，他将这些打包在一起以创建一个方便的 Quaternion 类。）

You can think of quaternions (for our purposes) as a scalar plus a 3-d vector -- abstractly, w + xi + yj + zk, here represented by a simple tuple (w, x, y, z). The space of 3-d rotations is represented in full by a sub-space of the quaternions, the space of unitquaternions, so you want to make sure that your quaternions are normalized. You can do so in just the way you would normalize any 4-vector (i.e. magnitude should be close to 1; if it isn't, scale down the values by the magnitude):

您可以将四元数（出于我们的目的）视为标量加上 3 维向量——抽象地说w + xi + yj + zk，这里由一个简单的元组 表示(w, x, y, z)。3-d 旋转空间完全由四元数的子空间表示，单位四元数的空间，所以你要确保你的四元数是归一化的。您可以按照对任何 4 向量进行归一化的方式来执行此操作（即幅度应接近 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

Please note that for simplicity, the following functions assume that quaternion values are already normalized. In practice, you'll need to renormalize them from time to time, but the best way to deal with that will depend on the problem domain. These functions provide just the very basics, for reference purposes only.

请注意，为简单起见，以下函数假设四元数值已经归一化。在实践中，您需要不时地重新规范化它们，但处理这种情况的最佳方法将取决于问题域。这些函数仅提供非常基础的功能，仅供参考。

Every rotation is represented by a unit quaternion, and concatenations of rotations correspond to multiplicationsof unit quaternions. The formula²for this is as follows:

每个旋转都由一个单位四元数表示，旋转的串联对应于单位四元数的乘法。为此，公式²如下：

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

To rotate a vectorby a quaternion, you need the quaternion's conjugate too. That's easy:

要将向量旋转四元数，您还需要四元数的共轭。这很容易：

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

Now quaternion-vector multiplication is as simple as converting a vector into a quaternion (by setting w = 0and leaving x, y, and zthe same) and then multiplying q * v * q_conjugate(q):

现在四元数-向量乘法很简单，只要矢量变换成四元数（通过设置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:]

Finally, you need to know how to convert from axis-angle rotations to quaternions. Also easy! It makes sense to "sanitize" input and output here by calling normalize.

最后，您需要知道如何将轴角旋转转换为四元数。也轻松！通过调用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

And back:

然后回来：

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

Here's a quick usage example. A sequence of 90-degree rotations about the x, y, and z axes will return a vector on the y axis to its original position. This code performs those rotations:

这是一个快速使用示例。围绕 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)

Keep in mind that this sequence of rotations won't return allvectors to the same position; for example, for a vector on the x axis, it will correspond to a 90 degree rotation about the y axis. (Keep the right-hand-rule in mind here; a positive rotation about the y axis pushes a vector on the x axis into the negativez region.)

请记住，此旋转序列不会将所有向量返回到相同位置；例如，对于 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)

As always, please let me know if you find any problems here.

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

---

---

^{1. These are adapted from an OpenGL tutorial archived here.}

^{1. 这些改编自此处存档的 OpenGL 教程。}

^{2. The quaternion multiplication formula looks like a rat's nest, but the derivation is simple (if tedious). Just note first that ii = jj = kk = -1; then that ij = k, jk = i, ki = j; and finally that ji = -k, kj = -i, ik = -j. Then multiply the two quaternions, distributing out the terms and rearranging them based on the results of each of the 16 multiplications. This also helps to illustrate whyyou can use quaternions to represent rotation; the last six identities follow the right-hand rule, creating bijections between rotations fromito jand rotations aroundk, and so on.}

^{2. 四元数乘法公式看似老鼠窝，但推导很简单（如果繁琐的话）。首先请注意ii = jj = kk = -1; 那么ij = k, jk = i, ki = j; 最后是ji = -k，kj = -i，ik = -j。然后将两个四元数相乘，分配项并根据 16 次乘法中的每一次的结果重新排列它们。这也有助于说明为什么可以使用四元数来表示旋转；最后六个恒等式遵循右手定则，在从itoj旋转和围绕旋转之间创建双射k，依此类推。}

回答by eat

Note that the inversion of matrix is not that trivial at all! Firstly, all n (where n is the dimension of your space) points must be in general position (i.e. no individual point can be expressed as a linear combination of rest of the points [caveat: this may seem to be a simple requirement indeed, but in the realm of numerical linear algebra, it's nontrivial; final decison wheter such configuration really exist or not, will eventually be based on the 'actual domain' specific knowledge]).

请注意，矩阵的求逆根本不是那么简单！首先，所有 n（其中 n 是您的空间的维度）点必须处于一般位置（即，没有单个点可以表示为其余点的线性组合 [警告：这似乎确实是一个简单的要求，但在数值线性代数领域，这是不平凡的；最终决定这种配置是否真的存在，最终将基于“实际领域”特定知识]）。

Also the 'correspondence' of the new and old points may not be exact (and then you should utilize the best possible approximator of the 'true correspondence', i.e.:). Pseudo inverse (instead of trying to utilize the plain inverse) is recommend allways when your lib provides it.

此外，新旧点的“对应关系”可能不准确（然后您应该使用“真实对应关系”的最佳近似值，即：）。当您的库提供伪逆（而不是尝试使用普通逆）时，始终建议使用它。

The pseudo inverse has the advantage that you'll be able to use more points for your transformation, hence increasing the probability that at least n points will be in general position.

伪逆的优点是您可以使用更多点进行变换，从而增加至少 n 个点处于一般位置的可能性。

Here is an example, rotation of unit square 90 deg. ccw in 2D (but obviously this determination works in any dim), with numpy:

这是一个示例，单位正方形旋转 90 度。ccw 在 2D（但显然这个决定适用于任何暗淡），具有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.]])

P.S. numpyis also fast. Transformation of 1 million points in my modest computer:

PSnumpy也快。100万点在我普通电脑上的转化：

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

回答by osolmaz

This question and the answer given by @senderle really helped me with one of my projects. The answer is minimal and covers the core of most quaternion computations that one might need to perform.

这个问题和@senderle 给出的答案真的帮助我完成了我的一个项目。答案是最小的，涵盖了人们可能需要执行的大多数四元数计算的核心。

For my own project, I found it tedious to have separate functions for all the operations and import them one by one every time I need one, so I implemented an object oriented version.

对于我自己的项目，我发现为所有操作都设置单独的函数并在每次需要时一个一个地导入它们很乏味，所以我实现了一个面向对象的版本。

quaternion.py:

四元数.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)

In this version, one can just use the overloaded operators for quaternion-quaternion and quaternion-vector multiplication

在这个版本中，可以只使用重载运算符进行四元数-四元数和四元数-向量乘法

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)

I did not intend to implement a full-fledged quaternion module, so this is again for instructional purposes, as in @senderle's great answer. I hope this helps out to those who want to understand and try out new things with quaternions.

我不打算实现一个成熟的四元数模块，所以这又是出于教学目的，就像@senderle 的好答案一样。我希望这对那些想要理解和尝试四元数新事物的人有所帮助。