C++ (Cpp) FLOAT_RMAT_OF_AXIS_ANGLEの例

コード例 #1

ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi

void test1234(void)
{
  struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)};

  struct FloatVect3 uz = { 0., 0., 1.};
  struct FloatRMat r_yaw;
  FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);

  struct FloatVect3 uy = { 0., 1., 0.};
  struct FloatRMat r_pitch;
  FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);

  struct FloatVect3 ux = { 1., 0., 0.};
  struct FloatRMat r_roll;
  FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);

  struct FloatRMat r_tmp;
  float_rmat_comp(&r_tmp, &r_yaw, &r_roll);

  struct FloatRMat r_att;
  float_rmat_comp(&r_att, &r_tmp, &r_pitch);
  DISPLAY_FLOAT_RMAT("r_att_ref  ", r_att);

  float_rmat_of_eulers_312(&r_att, &eul);
  DISPLAY_FLOAT_RMAT("r_att312  ", r_att);

}

コード例 #2

ファイル: test_algebra.c プロジェクト: byrddev/paparazzi

void test1234(void) {
    struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)};

    struct FloatVect3 uz = { 0., 0., 1.};
    struct FloatMat33 r_yaw;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);

    struct FloatVect3 uy = { 0., 1., 0.};
    struct FloatMat33 r_pitch;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);

    struct FloatVect3 ux = { 1., 0., 0.};
    struct FloatMat33 r_roll;
    FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);

    struct FloatMat33 r_tmp;
    FLOAT_RMAT_COMP(r_tmp, r_yaw, r_roll);

    struct FloatMat33 r_att;
    FLOAT_RMAT_COMP(r_att, r_tmp, r_pitch);
    DISPLAY_FLOAT_RMAT("r_att_ref  ", r_att);

    FLOAT_RMAT_OF_EULERS_312(r_att, eul);
    DISPLAY_FLOAT_RMAT("r_att312  ", r_att);




}

コード例 #3

ファイル: test_algebra.c プロジェクト: EwoudSmeur/paparazzi

void test_of_axis_angle(void)
{

  struct FloatVect3 axis = { 0., 1., 0.};
  FLOAT_VECT3_NORMALIZE(axis);
  DISPLAY_FLOAT_VECT3("axis", axis);
  const float angle = RadOfDeg(30.);
  printf("angle %f\n", DegOfRad(angle));

  struct FloatQuat my_q;
  FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle);
  DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q);

  struct FloatRMat my_r1;
  float_rmat_of_quat(&my_r1, &my_q);
  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1);
  DISPLAY_FLOAT_RMAT("rmat1", my_r1);

  struct FloatRMat my_r;
  FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle);
  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r);
  DISPLAY_FLOAT_RMAT("rmat", my_r);

  printf("\n");

  struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.};

  struct FloatVect3 uz = { 0., 0., 1.};
  struct FloatRMat r_yaw;
  FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);

  struct FloatVect3 uy = { 0., 1., 0.};
  struct FloatRMat r_pitch;
  FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);

  struct FloatVect3 ux = { 1., 0., 0.};
  struct FloatRMat r_roll;
  FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);

  struct FloatRMat r_yaw_pitch;
  float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch);

  struct FloatRMat r_yaw_pitch_roll;
  float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll);

  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll);
  DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll);

  DISPLAY_FLOAT_EULERS_DEG("eul", eul);
  struct FloatRMat rmat1;
  float_rmat_of_eulers(&rmat1, &eul);
  DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);
  DISPLAY_FLOAT_RMAT("rmat1", rmat1);

}