Блог

Мне нужна функция, которая, учитывая рыскание, тангаж и крен, может создавать векторы спереди (или смотреть на них), вправо и вверх в «мировых координатах».

В моем конкретном мировом пространстве, начиная с начала координат (0,0,0), X положительно отклоняется влево, Z положительно отклоняется от наблюдателя / источника, а Y положительно увеличивается.

Например, дано… (углы в градусах)

• рыскание = 0, тангаж = 0, крен = 0, ожидаемый результат равен:

  • спереди = (0.0,0.0,1.0)
  • справа = (-1.0,0.0,0.0)
  • вверх = (0.0,1.0,0.0)

• рыскание = 90, тангаж = 0, крен = 0, ожидаемый результат равен:

  • спереди = (1.0,0.0,0.0)
  • справа = (0,0,0.0,1.0)
  • вверх = (0.0,1.0,0.0)

• рыскание = 0, тангаж = 90, крен = 0, ожидаемый результат равен:

  • спереди = (0.0,1.0,0.0)
  • справа = (-1.0,0.0,0.0)
  • up = (0.0,0.0,-1.0)

• рыскание = 0, тангаж = 0, крен = 90, ожидаемый результат равен:

  • спереди = (0.0,0.0,1.0)
  • справа = (0.0,1.0,0.0)
  • вверх = (1.0,0.0,0.0)

Язык, на котором я работаю, — C , и я с удовольствием использую glm для решения этой проблемы, если это имеет наибольший смысл. Если я смогу добраться туда с помощью кватерниона, меня тоже устроит это решение, поскольку я нашел другие руководства, в которых описывается, как получить кватернион из углов Эйлера.

Вот полный рабочий пример. Это не очень похоже на C . Вы, вероятно, захотите использовать класс real matrix, но он должен быть в порядке для демонстрационных целей. Одна вещь, которая не ясна из вашего вопроса, — это порядок вращения, но это можно легко изменить.

 #include <iostream>
#include <cmath>
#include <cstdlib>

typedef float Float;
typedef Float Axis[3];
typedef Axis Axes[3];

static void copy(const Axes amp;from,Axes amp;to)
{
  for (size_t i=0; i!=3;   i) {
    for (size_t j=0; j!=3;   j) {
      to[i][j] = from[i][j];
    }
  }
}

static void mul(Axes amp;mat,Axes amp;b)
{
  Axes resu<
  for (size_t i=0; i!=3;   i) {
    for (size_t j=0; j!=3;   j) {
      Float sum = 0;
      for (size_t k=0; k!=3;   k) {
        sum  = mat[i][k]*b[k][j];
      }
      result[i][j] = sum;
    }
  }
  copy(result,mat);
}

static void getAxes(Axes amp;result,Float yaw,Float pitch,Float roll)
{
  Float x = -pitch;
  Float y = yaw;
  Float z = -roll;
  Axes matX = {
    {1,     0,     0 },
    {0, cos(x),sin(x)},
    {0,-sin(x),cos(x)}
  };
  Axes matY = {
    {cos(y),0,-sin(y)},
    {     0,1,      0},
    {sin(y),0, cos(y)}
  };
  Axes matZ = {
    { cos(z),sin(z),0},
    {-sin(z),cos(z),0},
    {      0,     0,1}
  };
  Axes axes = {
    {1,0,0},
    {0,1,0},
    {0,0,1}
  };

  mul(axes,matX);
  mul(axes,matY);
  mul(axes,matZ);

  copy(axes,result);
}


static void showAxis(const char *desc,const Axis amp;axis,Float sign)
{
  std::cout << "  " << desc << " = (";
  for (size_t i=0; i!=3;   i) {
    if (i!=0) {
      std::cout << ",";
    }
    std::cout << axis[i]*sign;
  }
  std::cout << ")n";
}

static void showAxes(const char *desc,Axes amp;axes)
{
  std::cout << desc << ":n";
  showAxis("front",axes[2],1);
  showAxis("right",axes[0],-1);
  showAxis("up",axes[1],1);
}

int main(int,char**)
{
  Axes axes;
  std::cout.setf(std::ios::fixed);
  std::cout.precision(1);
  getAxes(axes,0,0,0);
  showAxes("yaw=0, pitch=0, roll=0",axes);
  getAxes(axes,M_PI/2,0,0);
  showAxes("yaw=90, pitch=0, roll=0",axes);
  getAxes(axes,0,M_PI/2,0);
  showAxes("yaw=0, pitch=90, roll=0",axes);
  getAxes(axes,0,0,M_PI/2);
  showAxes("yaw=0, pitch=0, roll=90",axes);
  return 0;
}