寻找一个更好的方法来做四元数的差分

Question

我有一个四元数（4x1）和一个角速度矢量（3x1），我称这个函数来计算差分四元数，如在这个web中所解释的。代码如下所示：

float wx = w.at<float>(0); 
float wy = w.at<float>(1); 
float wz = w.at<float>(2); 
float qw = q.at<float>(3); //scalar component 
float qx = q.at<float>(0); 
float qy = q.at<float>(1); 
float qz = q.at<float>(2); 

q.at<float>(0) = 0.5f * (wx*qw + wy*qz - wz*qy); // qdiffx 
q.at<float>(1) = 0.5f * (wy*qw + wz*qx - wx*qz); // qdiffy 
q.at<float>(2) = 0.5f * (wz*qw + wx*qy - wy*qx); // qdiffz 
q.at<float>(3) = -0.5f * (wx*qx + wy*qy + wz*qz); // qdiffw 

所以现在我有存储在Q差分四元数，然后由我简单地加入这个差四元数更新四元数。

这种方法适用于预测刚性物体的运动还是有更好的方法来预测角速度的四元数？这工作，但我没有得到预期的结果。

Answer 1

有几件事情可能正在发生。你没有提到重新规范化该四元数。如果你不这样做，肯定会发生坏事。您也不会说在将三角形四元数组件添加到原始四元数之前，它们已经通过了dt的时间量。如果角速度是以弧度/秒为单位的，但你只是向前走几分之一秒，那么你会走得太远。然而，即便如此，由于您正在经历一段时间，试图假装它无限小，奇怪的事情将会发生，特别是如果您的时间步或角速度很大。

物理引擎ODE提供了从角速度更新物体旋转的选项，就好像它正在采取无限小步骤或使用有限大小的步骤进行更新。有限步骤更精确，但涉及一些触发。功能等等有点慢。可以看到相关的ODE源代码here, lines 300-321，代码查找delta-quaternion here, line 310。

float wMag = sqrt(wx*wx + wy*wy + wz*wz); 
float theta = 0.5f*wMag*dt; 
q[0] = cos(theta); // Scalar component 
float s = sinc(theta)*0.5f*dt; 
q[1] = wx * s; 
q[2] = wy * s; 
q[3] = wz * s; 

其中sinc(x)是：

if (fabs(x) < 1.0e-4) return (1.0) - x*x*(0.166666666666666666667); 
else return sin(x)/x; 

这可以让你避免除以零的问题，仍然是非常精确的。

然后，将四元数q预乘到现有的身体方向的四元数表示上。然后，重新正常化。

---

编辑 - 当这个公式来源于：

考虑初始四元数q0和最终四元q1与角速度w旋转为dt量的时间之后产生的。我们在这里所做的就是将角速度矢量改变成四元数，然后用四元数旋转第一个方向。四元数和角速度都是轴角表示的变化。 正文围绕单位轴[x,y,z]围绕theta的标准方向旋转将具有以下四元数的方向表示：q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z]。 身体是旋转theta/s围绕单位轴[x,y,z]将有角速度w=[theta*x theta*y theta*z]。因此，为了决定在dt秒内将发生多少旋转，我们首先提取角速度的大小：theta/s = sqrt(w[0]^2 + w[1]^2 + w[2]^2)。然后我们找到实际的角度乘以dt（为方便起见，同时除以2，将其转换为四元数）。由于我们需要对轴线[x y z]进行归一化，因此我们也除以theta。这就是sinc(theta)部分的来源。 （因为theta有一个额外的0.5*dt从它的规模，我们乘以返回）。当x很小时，sinc(x)函数只是使用函数的泰勒级数近似，因为它在数值上是稳定的，而且更准确。使用这个方便的功能的能力是为什么我们不仅仅是除以实际量值wMag。旋转速度不快的物体将具有非常小的角速度。由于我们预计这很常见，我们需要一个稳定的解决方案。我们最终得到的是一个四元数，表示旋转的单步时间步dt。

Answer 2

有速度以及如何增量表示旋转状态一个quaterniom（即整合旋转运动的微分方程）由角dphi（小矢量增量精度之间具有非常好的折衷一个梅托德其是向量角速度omega时间步长dt）。

四元数的旋转通过矢量的精确（和慢）方法：

void rotate_quaternion_by_vector_vec (double [] dphi, double [] q) { 
    double x = dphi[0]; 
    double y = dphi[1]; 
    double z = dphi[2]; 

    double r2 = x*x + y*y + z*z; 
    double norm = Math.sqrt(r2); 

    double halfAngle = norm * 0.5d; 
    double sa = Math.sin(halfAngle)/norm; // we normalize it here to save multiplications 
    double ca = Math.cos(halfAngle); 
    x*=sa; y*=sa; z*=sa; 

    double qx = q[0]; 
    double qy = q[1]; 
    double qz = q[2]; 
    double qw = q[3]; 

    q[0] = x*qw + y*qz - z*qy + ca*qx; 
    q[1] = -x*qz + y*qw + z*qx + ca*qy; 
    q[2] = x*qy - y*qx + z*qw + ca*qz; 
    q[3] = -x*qx - y*qy - z*qz + ca*qw; 
} 

的问题是，你必须计算像cos, sin, sqrt慢的函数。相反，如果通过taylor扩展来近似sin和cos，则只需使用norm^2而不是norm即可获得相当可观的速度增益和合理的小角度精度（这种情况下，如果模拟的时间步长合理小）。

像通过矢量四元数的旋转这个快速方法：

void rotate_quaternion_by_vector_Fast (double [] dphi, double [] q) { 
    double x = dphi[0]; 
    double y = dphi[1]; 
    double z = dphi[2]; 

    double r2 = x*x + y*y + z*z; 

    // derived from second order taylor expansion 
    // often this is accuracy is sufficient 
    final double c3 = 1.0d/(6 * 2*2*2)  ; // evaulated in compile time 
    final double c2 = 1.0d/(2 * 2*2)   ; // evaulated in compile time 
    double sa = 0.5d - c3*r2    ; 
    double ca = 1 - c2*r2    ; 

    x*=sa; 
    y*=sa; 
    z*=sa; 

    double qx = q[0]; 
    double qy = q[1]; 
    double qz = q[2]; 
    double qw = q[3]; 

    q[0] = x*qw + y*qz - z*qy + ca*qx; 
    q[1] = -x*qz + y*qw + z*qx + ca*qy; 
    q[2] = x*qy - y*qx + z*qw + ca*qz; 
    q[3] = -x*qx - y*qy - z*qz + ca*qw; 

} 

，你可以通过做一半Ø角度提高准确性其中5个乘法：

final double c3 = 1.0d/(6.0 *4*4*4 ) ; // evaulated in compile time 
    final double c2 = 1.0d/(2.0 *4*4 ) ; // evaulated in compile time 
    double sa_ = 0.25d - c3*r2   ; 
    double ca_ = 1  - c2*r2   ; 
    double ca = ca_*ca_ - sa_*sa_*r2  ; 
    double sa = 2*ca_*sa_     ; 

或者更准确的另一个分裂角为一半：

final double c3 = 1.0d/(6 *8*8*8); // evaulated in compile time 
    final double c2 = 1.0d/(2 *8*8 ); // evaulated in compile time 
    double sa = ( 0.125d - c3*r2)  ; 
    double ca = 1  - c2*r2  ; 
    double ca_ = ca*ca - sa*sa*r2; 
    double sa_ = 2*ca*sa; 
     ca = ca_*ca_ - sa_*sa_*r2; 
     sa = 2*ca_*sa_; 

---

注：如果您使用更复杂的集成方案不仅仅是verlet的（如龙格 - 库塔），你可能会需要四元的差别，而不是新的（更新）四元数。

这是可能在这里

q[0] = x*qw + y*qz - z*qy + ca*qx; 
    q[1] = -x*qz + y*qw + z*qx + ca*qy; 
    q[2] = x*qy - y*qx + z*qw + ca*qz; 
    q[3] = -x*qx - y*qy - z*qz + ca*qw; 

的代码，看看它可以通过ca（半角的余弦），这是approximativelly ca ~ 1对于小角度和被解释为老（不更新）四元数的乘法添加其余的（一些交叉相互作用）。因此，简单地差：

dq[0] = x*qw + y*qz - z*qy + (1-ca)*qx; 
    dq[1] = -x*qz + y*qw + z*qx + (1-ca)*qy; 
    dq[2] = x*qy - y*qx + z*qw + (1-ca)*qz; 
    dq[3] = -x*qx - y*qy - z*qz + (1-ca)*qw; 

其中对于小角度长期(1 - ca) ~ 0，有时甚至可以忽略不计（基本上它只是重新归一化的quternion）。

Answer 3

从“指数映射”到四元数的简单转换。 （指数图等于角速度乘以deltaTime）。结果四元数是传递的deltaTime和角速度“w”的增量旋转。

Vector3 em = w*deltaTime; // exponential map 
{ 
Quaternion q; 
Vector3 ha = em/2.0; // vector of half angle 

double l = ha.norm(); 
if(l > 0) 
{ 
    double ss = sin(l)/l; 
    q = Quaternion(cos(l), ha.x()*ss, ha.y()*ss, ha.z()*ss); 
}else{ 
    // if l is too small, its norm can be equal 0 but norm_inf greater 0 
    q = Quaternion(1.0, ha.x(), ha.y(), ha.z()); 
}