我们正在尝试将欧拉(X,Y,Z)=俯仰偏航角的局部空间增量旋转转换为绝对世界空间旋转四元数。
我们通过将每个增量旋转转换为四元数并累积(通过乘法)四元数旋转以提供世界空间四元数来做到这一点。
但是,我们的结果旋转显示对象绕世界空间轴而不是局部对象轴旋转。
我正在使用此伪代码循环和c ++,并已在Unity3D中构建了相同的代码(由于提供了四元数运算,因此可以正常工作)。
有人对这里出了什么问题有任何指示吗?
Quaternion qacc;
Quaternion q1;
loop
{
quacc = quacc * q1.degreeToQuaternion(xRot, yRot, zRot);
}
...
void Quaternion::degreeToQuaternion( double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
yaw = yaw * M_PI / 180.;
pitch = pitch * M_PI / 180.;
roll = roll * M_PI / 180.;
radianToQuaternion(yaw, pitch, roll);
}
void Quaternion::radianToQuaternion( double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
this->w = cy * cp * cr + sy * sp * sr;
this->x = cy * cp * sr - sy * sp * cr;
this->y = sy * cp * sr + cy * sp * cr;
this->z = sy * cp * cr - cy * sp * sr;
}
Quaternion operator * (Quaternion q0, Quaternion q1)
{
Quaternion q2;
double mag;
q2.w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z;
q2.x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y;
q2.y = q0.w * q1.y - q0.x * q1.z + q0.y * q1.w + q0.z * q1.x;
q2.z = q0.w * q1.z + q0.x * q1.y - q0.y * q1.x + q0.z * q1.w;
mag = sqrt (q2.w * q2.w + q2.x * q2.x + q2.y * q2.y + q2.z * q2.z);
q2.w /= mag;
q2.x /= mag;
q2.y /= mag;
q2.z /= mag;
return q2;
}