Могу ли я получить вращение из этого матричного алгоритма

Question

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

matrix[0][0] = cos(pitch)*cos(yaw);
matrix[0][2] = cos(pitch)*sin(yaw);
matrix[0][1] = sin(pitch);
matrix[0][3] = 0;
matrix[1][0] = -cos(roll)*sin(pitch)*cos(yaw)+sin(roll)*sin(yaw);
matrix[1][1] = cos(roll)*cos(pitch);
matrix[1][2] = -cos(roll)*sin(pitch)*sin(yaw)-sin(roll)*cos(yaw);
matrix[1][3] = 0;
matrix[2][0] = -sin(roll)*sin(pitch)*cos(yaw)-cos(roll)*sin(yaw);
matrix[2][1] = sin(roll)*cos(pitch);
matrix[2][2] = cos(roll)*cos(yaw)-sin(roll)*sin(pitch)*sin(yaw);
matrix[2][3] = 0;
matrix[3][0] = 0;
matrix[3][1] = 0;
matrix[3][2] = 0;
matrix[3][3] = 1;

изменить: я использовал этот код, но он не работает:

D3DXMATRIX matrix2 = D3DXMATRIX(matrix);
yaw   = atan2(matrix2._13, matrix2._11);
pitch = asin(matrix2._12);
roll  = atan2(matrix2._32, matrix2._22);

Редактировать 3: это моя старая функция, она работает, но не 100%

VECTOR getAnglefromMatrix(float* m)
{
    float pitch, yaw, roll;

    D3DXMATRIX matrix = D3DXMATRIX(m);

    if (matrix._11 == 1.0f)
    {
        yaw = todegree(atan2f(matrix._13, matrix._34));
        pitch = 0;
        roll = 0;
    }
    else if (matrix._11 == -1.0f)
    {
        yaw = todegree(atan2f(matrix._13, matrix._34));
        pitch = 0;
        roll = 0;
    }
    else
    {

        yaw = todegree(atan2(-matrix._31,matrix._11));
        pitch = todegree(asin(matrix._21));
        roll = todegree(atan2(-matrix._23,matrix._22));
    }

    return vector3d(yaw,pitch,roll);
}

я что-то не так сделал?

Answer 1

Мы можем просмотреть записи и найти те, которые подходят для обратных вычислений:

matrix[0][0] = cos(pitch)*cos(yaw);
matrix[0][2] = cos(pitch)*sin(yaw);

Это дает нам:

yaw = atan2(matrix[0][2], matrix[0][0])

Для шага:

matrix[0][1] = sin(pitch);

Это дает нам:

pitch = asin(matrix[0][1])  //this assumes pitch is between -pi/2 and +pi/2

Затем

matrix[1][1] = cos(roll)*cos(pitch);
matrix[2][1] = sin(roll)*cos(pitch);

Это дает нам:

roll = atan2(matrix[2][1], matrix[1][1])

Я пропустил случаи, когда аргументы atan2 станет нулем. Я уверен, что вы можете понять их. В этих случаях углы Эйлера не определяются однозначно.