Code of the Week #26: quaternion

OpenFOAM
1

#201913

在动网格操作中，需要对网格进行平移或旋转，比如 src/dynamicMesh/meshMotion/solidBodyMotion/linearOscillation/linearOscillation.C 中有这么一段

 83 Foam::septernion
 84 Foam::solidBodyMotionFunctions::linearOscillation::transformation() const
 85 {
 86     scalar t = time_.value();
 87 
 88     septernion TR(calcPosition(t), quaternion::I);
 89 
 90     Info<< "solidBodyMotionFunctions::linearOscillation::transformation(): "
 91         << "Time = " << t << " transformation: " << TR << endl;
 92 
 93     return TR;
 94 }
 95 
 96 
 97 Foam::septernion
 98 Foam::solidBodyMotionFunctions::linearOscillation::velocity() const
 99 {
100     scalar t = time_.value();
101     scalar dt = time_.deltaT().value();
102 
103     septernion TV
104     (
105         (calcPosition(t + dt) - calcPosition(t))/dt,
106         quaternion::zero
107     );
108 
109     return TV;
110 }

septernion 是什么东东？找到 septernion.H 里面讲了：

这是一个联合了平移的向量和旋转的四元数 (quaternion) 的一个向量，因此有 7 个分量。septernion 可以用一个向量和一个四元数构造：

         //- Construct given a translation vector and rotation quaternion
         inline septernion(const vector& t, const quaternion& r);

而我们要关注旋转方面的操作，除了在 CFD 上用，计算机游戏里面的很多旋转也是四元数完成的。为什么要用四元数呢？大家都知道旋转矩阵，使用欧拉角可完成旋转。欧拉角使用最简单的在 x，y，z 轴上的各旋转角度，取值范围 [0,\,2\pi] ，一般使用 roll，pitch，yaw 来表示这些分量的旋转值。需要注意的是，这里的旋转是针对世界坐标系说的，这意味着第一次的旋转不会影响第二、三次的转轴。欧拉角容易出现的问题是

  • 不易在任意方向的旋转轴插值；
  • 万向节死锁；
  • 旋转的次序无法确定。

quaternion 是四元数，什么是 quaternion? 简单地说，一个四元数由一个实数和三个虚数组成：

q = a+b\mathbf{i}+c\mathbf{j}+d\mathbf{k}

其中，

\mathbf{i}^2 = \mathbf{j}^2 =\mathbf{k}^2 = \mathbf{ijk}=-1

这套写法由数学家 Hamilton 创建。
也可以写成向量形式 (有序)

q=[a,b,c,d]

在 OpenFOAM 中，用到网格 (点) 旋转变换的，都使用四元数来操作完成。在这四个数里面，实数部分 a 代表旋转角度， (bi, cj, dk) 是旋转轴向量。

        //- Scalar part of the quaternion ( = cos(theta/2) for rotation) 旋转角度
        scalar w_;

        //- Vector part of the quaternion ( = axis of rotation) 旋转轴向量
        vector v_;

任意向量 \vec{v} 沿着以单位向量定义的旋转轴 \vec{u} 旋转 \theta 角之后的 \vec{v}' 可以使用四元数乘法来获得

\vec{v}'=qvq^*=qvq^{-1}

式中， v=[0,\vec{v}] ， q=\left[\cos\dfrac{\theta}{2},\sin\dfrac{\theta}{2}\vec{u}\right] 。

四元数的详细解释我推荐文献 [1]。

具体的实现可以参考 src/OpenFOAM/fields/Fields/transformField/transformField.C 文件，这是对场进行操作，对于网格点的位置，可以理解为向量场，

void Foam::transform
(
    vectorField& rtf,
    const quaternion& q,
    const vectorField& tf
)
{
    tensor t = q.R();
    TFOR_ALL_F_OP_FUNC_S_F(vector, rtf, =, transform, tensor, t, vector, tf)
}
...
Foam::tmp<Foam::vectorField> Foam::transform
(
    const quaternion& q, // 四元数
    const vectorField& tf // 向量场 (网格坐标点也是向量场)
)
{
    tmp<vectorField > tranf(new vectorField(tf.size()));// tmp 向量场
    transform(tranf(), q, tf); // 变换 tf 到新的向量场 tranf, 上一个函数
    return tranf; // 回传新的向量场
}

R() 是四元数类的一个函数，其操作位于 src/foam/primitives/quaternion/quaternionI.H

inline Foam::tensor Foam::quaternion::R() const
{
    scalar w2 = sqr(w());
    scalar x2 = sqr(v().x());
    scalar y2 = sqr(v().y());
    scalar z2 = sqr(v().z());

    scalar txy = 2*v().x()*v().y();
    scalar twz = 2*w()*v().z();
    scalar txz = 2*v().x()*v().z();
    scalar twy = 2*w()*v().y();
    scalar tyz = 2*v().y()*v().z();
    scalar twx = 2*w()*v().x();

    return tensor
    (
        w2 + x2 - y2 - z2,  txy - twz,          txz + twy,
        txy + twz,          w2 - x2 + y2 - z2,  tyz - twx,
        txz - twy,          tyz + twx,          w2 - x2 - y2 + z2
    );
}

TFOR_ALL_F_OP_FUNC_S_F 是一个宏，定义在文件
src/foam/fields/Fields/Field/FieldM.H

#define TFOR_ALL_F_OP_FUNC_S_F(typeF1, f1, OP, FUNC, typeS, s, typeF2, f2)  \
                                                                            \
    /* check the two fields have same Field<Type> mesh */                   \
    checkFields(f1, f2, "f1 " #OP " " #FUNC "(s, f2)");                     \
                                                                            \
    /* set access to f1, f2 and f3 at end of each field */                  \
    List_ACCESS(typeF1, f1, f1P);                                           \
    List_CONST_ACCESS(typeF2, f2, f2P);                                     \
                                                                            \
    /* loop through fields performing f1 OP1 f2 OP2 f3 */                   \
    List_FOR_ALL(f1, i)                                                     \
        List_ELEM(f1, f1P, i) OP FUNC((s), List_ELEM(f2, f2P, i));          \
    List_END_FOR_ALL                                                        \

List_FOR_ALL 和 List_END_FOR_ALL 宏定义 src/foam/containers/Lists/List/ListLoopM.H

#ifdef vectorMachine

// Element access looping using [] for vector machines

#define List_FOR_ALL(f, i)                      \
        const label _n##i = (f).size();\
        for (label i=0; i<_n##i; i++)  \
        {

#define List_END_FOR_ALL  }

// Provide current element
#define List_CELEM(f, fp, i)  (fp[i])

// Provide current element
#define List_ELEM(f, fp, i)  (fp[i])

#define List_ACCESS(type, f, fp) \
    type* const __restrict__ fp = (f).begin()

#define List_CONST_ACCESS(type, f, fp) \
    const type* const __restrict__ fp = (f).begin()

#else

// Pointer looping for scalar machines

#define List_FOR_ALL(f, i)                      \
        label i = (f).size();          \
        while (i--)                             \
        {                                       \

#define List_END_FOR_ALL  }

// Provide current element without incrementing pointer
#define List_CELEM(f, fp, i)  (*fp)

// Provide current element and increment pointer
#define List_ELEM(f, fp, i)  (*fp++)

#define List_ACCESS(type, f, fp) \
    type* __restrict__ fp = (f).begin()

#define List_CONST_ACCESS(type, f, fp) \
    const type* __restrict__ fp = (f).begin()

#endif

把宏用其定义的字段替代，就可以得到实际执行的程序段了。
大家可以尝试把一段写下来。

    TFOR_ALL_F_OP_FUNC_S_F(vector, rtf, =, transform, tensor, t, vector, tf)

(本帖程序段主要来自于 foam-extend-3.1)

参考文献

  1. 四元数与三维旋转, https://krasjet.github.io/quaternion/quaternion.pdf
  2. 【Unity技巧】四元数（Quaternion）和旋转, https://blog.csdn.net/candycat1992/article/details/41254799
  3. What are quaternions, and how do you visualize them? A story of four dimensions.
  4. Quaternions and 3d rotation, explained interactively
  5. Quaternions and spatial rotation
4

试一下：

checkFields(f1, f2, "f1=transform(s, f2)");

vector* const __restrict__ f1P = f1.begin();
const vector* const __restrict__ f2P=f2.begin();

register const label _ni = f1.size();
for (register label i=0; i<_ni; i++)
{
    (*f1P++)=transform(s,*f2P++);
}