我有一个四元数（4x1）和一个角速度矢量（3x1），我调用一个函数来计算微分四元数，如本文所述。代码如下：
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中，然后我通过简单地添加这个微分四元数来更新四元数。
这种方法适用于预测刚性物体的运动吗？或者有更好的方法来预测具有角速度的四元数吗？这是有效的，但我没有得到预期的结果。
可能会发生一些事情。您没有提到重新规范四元数。如果你不这样做，坏事肯定会发生。您也没有说在将增量四元数分量添加到原始四元数之前，将其乘以经过dt的时间量。如果你的角速度是每秒弧度，但你只向前走了几分之一秒，你就会走得太远。然而，即便如此，由于你正在经历一段离散的时间，并试图假装它是无穷小的，奇怪的事情就会发生，尤其是当你的时间步长或角速度很大的时候。
物理引擎ODE提供了一个选项，可以根据物体的角速度更新物体的旋转，就好像它在走无穷小的一步，也可以使用有限大小的一步来更新。有限步要精确得多，但需要一些三角函数。函数，所以有点慢。相关的ODE源代码可以在这里看到，第300-321行，代码在这里找到德尔塔四元数，第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和在以角速度w旋转dt一定时间后产生的最终四元数q1。我们在这里所做的就是将角速度向量改变为四元数，然后将第一个方向旋转四元数。四元数和角速度都是轴角度表示的变化。由theta围绕单位轴[x,y,z]从其规范方向旋转的物体将具有其方向的以下四元数表示：q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z]。围绕单位轴[x,y,z]旋转theta/s的物体将具有角速度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。
在速度和精度之间有一种非常好的折衷方法如何通过角度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这样的慢函数。相反，您可以通过使用仅使用norm^2而不是norm表示的泰勒展开来近似sin和cos，从而在小角度下获得可观的速度增益和合理的精度（如果模拟的时间步长较小，则会出现这种情况）。
像这样通过向量旋转四元数的快速方法：
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次乘法的半o角来提高精度：
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更复杂的积分方案（如Runge-Kutta），则可能需要四元数的微分，而不是新的（更新的）四元数。
这可以在这里的代码中看到
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;
它可以被解释为旧的（未更新的）四元数与CCD_ 34（半角余弦）的乘积，CCD_。所以差分很简单：
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表示小角度，有时可以忽略（基本上它只是对量子进行重整化）。
从"指数映射"到四元数的简单转换。（指数映射等于角速度乘以deltaTime）。结果四元数是经过的deltaTime和角速度"w"的delta旋转。
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());
}