Как правильно конвертировать из углов Эйлера и умножить кватернионы? - PullRequest
Новая
       25

Как правильно конвертировать из углов Эйлера и умножить кватернионы?

1 голос
/ 13 июня 2019

Мы пытаемся преобразовать инкрементное вращение локального пространства в Euler (X, Y, Z) = Pitch Yaw Roll в абсолютное вращение мирового пространства Quaternion.

Мы делаем это, преобразовывая каждое добавочное вращение в кватернион и накапливая (посредством умножения) вращения кватернионов, чтобы получить кватернион пространства мира.

Однако наши повороты показывают, что объект вращается вокруг осей мирового пространства, а не локальных осей объекта.

Я слежу за этим циклом псевдокода и c ++ и построил то же самое в Unity3D (который работает правильно, поскольку операции Quaternion уже предусмотрены).

У кого-нибудь есть указания относительно того, что здесь происходит не так? 

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;
}
...