c++ - 更快的四元数 vector 乘法不起作用

Question

我的数学库需要一个更快的四元数 vector 乘法例程。现在我正在使用规范的 v' = qv(q^-1)，它产生的结果与将 vector 乘以由四元数构成的矩阵相同，所以我相信它是正确性。

到目前为止，我已经实现了 3 种“更快”的替代方法:

#1，我不知道我从哪里得到这个:

v' = (q.xyz * 2 * dot(q.xyz, v)) + (v * (q.w*q.w - dot(q.xyz, q.zyx))) + (cross(q.xyz, v) * q.w * w)

实现为:

vec3 rotateVector(const quat& q, const vec3& v)
{
    vec3 u(q.x, q.y, q.z);
    float s = q.w;

    return vec3(u * 2.0f * vec3::dot(u, v))
    + (v * (s*s - vec3::dot(u, u)))
    + (vec3::cross(u, v) * s * 2.0f);
}

#2，由 this fine blog 提供

t = 2 * cross(q.xyz, v);
v' = v + q.w * t + cross(q.xyz, t);

实现为:

__m128 rotateVector(__m128 q, __m128 v)
{
    __m128 temp = _mm_mul_ps(vec4::cross(q, v), _mm_set1_ps(2.0f));
    return _mm_add_ps(
        _mm_add_ps(v, _mm_mul_ps(_mm_shuffle_ps(q, q, _MM_SHUFFLE(3, 3, 3, 3)), temp)),
        vec4::cross(q, temp));
}

#3，来自众多来源，

v' = v + 2.0 * cross(cross(v, q.xyz) + q.w * v, q.xyz);

实现为:

__m128 rotateVector(__m128 q, __m128 v)
{
    //return v + 2.0 * cross(cross(v, q.xyz) + q.w * v, q.xyz);
    return _mm_add_ps(v,
        _mm_mul_ps(_mm_set1_ps(2.0f),
            vec4::cross(
                _mm_add_ps(
                    _mm_mul_ps(_mm_shuffle_ps(q, q, _MM_SHUFFLE(3, 3, 3, 3)), v),
                    vec4::cross(v, q)),
                q)));
}

所有这 3 个都会产生不正确的结果。然而，我注意到了一些有趣的模式。首先，#1 和#2 产生相同的结果。如果所述矩阵被转置，#3 产生的结果与我将 vector 乘以派生矩阵得到的结果相同(我偶然发现了这一点，之前我的 quat-to-matrix 代码假定行主矩阵，这是不正确的)。

我的四元数的数据存储定义为:

union
{
    __m128 data;
    struct { float x, y, z, w; };
    float f[4];
};

是我的实现有缺陷，还是我遗漏了什么？

Answer 1

主要问题，如果你想通过四元数旋转 3d vector ，你需要计算旋转矩阵的所有 9 个标量。在您的示例中，旋转矩阵的计算是隐式的。计算顺序可能不是最优的。

如果你从四元数和乘法 vector 生成 3x3 矩阵，你应该有相同数量的算术运算(@see code at bottom)。

我推荐的。

1. 尝试生成 3x3 矩阵并将 vector 相乘，测量速度并与之前的进行比较。

2. 分析显式解决方案，并尝试针对自定义架构进行优化。

3. 尝试实现替代四元数乘法，并从方程 q*v*q' 导出乘法。

//============ 替代乘法伪代码

 /**
      alternative way of quaternion multiplication,
      can speedup multiplication for some systems (PDA for example)
      http://mathforum.org/library/drmath/view/51464.html
      http://www.lboro.ac.uk/departments/ma/gallery/quat/src/quat.ps
      in provided code by url's many bugs, have to be rewriten.
    */
    inline xxquaternion mul_alt( const xxquaternion& q) const {
        float t0 = (x-y)*(q.y-q.x);
        float t1 = (w+z)*(q.w+q.z);
        float t2 = (w-z)*(q.y+q.x);
        float t3 = (x+y)*(q.w-q.z);
        float t4 = (x-z)*(q.z-q.y);
        float t5 = (x+z)*(q.z+q.y);
        float t6 = (w+y)*(q.w-q.x);
        float t7 = (w-y)*(q.w+q.x);

        float t8 = t5 + t6 + t7;
        float t9 = (t4 + t8)*0.5;
        return xxquaternion ( t3+t9-t6,
                              t2+t9-t7,
                              t1+t9-t8,
                              t0+t9-t5 );
        // 9 multiplications    27  addidtions    8 variables
        // but of couse we can clean 4 variables
/*
        float r = w, i = z, j = y, k =x;
        float br = q.w,  bi = q.z, bj = q.y, bk =q.x;
        float t0 = (k-j)*(bj-bk);
        float t1 = (r+i)*(br+bi);
        float t2 = (r-i)*(bj+bk);
        float t3 = (k+j)*(br-bi);
        float t4 = (k-i)*(bi-bj);
        float t5 = (k+i)*(bi+bj);
        float t6 = (r+j)*(br-bk);
        float t7 = (r-j)*(br+bk);
        float t8 = t5 + t6 + t7;
        float t9 = (t4 + t8)*0.5;
        float rr = t0+t9-t5;
        float ri = t1+t9-t8;
        float rj = t2+t9-t7;
        float rk = t3+t9-t6;
        return xxquaternion ( rk, rj, ri, rr );
*/
    }

//============ 显式 vector 旋转变体

/**
      rotate vector by quaternion
    */
    inline vector3 rotate(const vector3& v)const{
        xxquaternion q(v.x * w + v.z * y - v.y * z,
                       v.y * w + v.x * z - v.z * x,
                       v.z * w + v.y * x - v.x * y,
                       v.x * x + v.y * y + v.z * z);

        return vector3(w * q.x + x * q.w + y * q.z - z * q.y,
                       w * q.y + y * q.w + z * q.x - x * q.z,
                       w * q.z + z * q.w + x * q.y - y * q.x)*( 1.0f/norm() );

        // 29  multiplications, 20 addidtions, 4 variables
        // 5 
       /*
        // refrence implementation  
        xxquaternion r = (*this)*xxquaternion(v.x, v.y, v.z, 0)*this->inverted();
        return vector3( r.x, r.y, r.z ); 
       */

       /*
        // alternative implementation
        float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
        x2 = q.x + q.x; y2 = q.y + q.y; z2 = q.z + q.z;

        xx = q.x * x2;   xy = q.x * y2;   xz = q.x * z2;
        yy = q.y * y2;   yz = q.y * z2;   zz = q.z * z2;
        wx = q.w * x2;   wy = q.w * y2;   wz = q.w * z2;

        return vector3( v.x  - v.x * (yy + zz) + v.y * (xy - wz) + v.z * (xz + wy),
                        v.y  + v.x * (xy + wz) - v.y * (xx + zz) + v.z * (yz - wx),
                        v.z  + v.x * (xz - wy) + v.y * (yz + wx) - v.z * (xx + yy) )*( 1.0f/norm() );
        // 18 multiplications, 21 addidtions, 12 variables
       */
    };

祝你好运。