如何正确地从欧拉角转换并乘四元数?

时间:2019-06-13 20:56:47

标签: c++ unity3d quaternions

我们正在尝试将欧拉(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;
}

0 个答案:

没有答案