C++ (Cpp) FLOAT_VECT3_NORMALIZE Beispiele

Beispiel #1

Datei: test_algebra.c Projekt: 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);

}

Beispiel #2

Datei: test_algebra.c Projekt: byrddev/paparazzi

float test_quat(void) {
    struct FloatVect3 u = { 1., 2., 3.};
    FLOAT_VECT3_NORMALIZE(u);
    float angle = RadOfDeg(30.);

    struct FloatQuat q;
    FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle);
    DISPLAY_FLOAT_QUAT("q ", q);

    struct FloatQuat iq;
    FLOAT_QUAT_INVERT(iq, q);
    DISPLAY_FLOAT_QUAT("iq", iq);

    struct FloatQuat q1;
    FLOAT_QUAT_COMP(q1, q, iq);
    DISPLAY_FLOAT_QUAT("q1", q1);

    struct FloatQuat q2;
    FLOAT_QUAT_COMP(q2, q, iq);
    DISPLAY_FLOAT_QUAT("q2", q2);

    struct FloatQuat qe;
    QUAT_EXPLEMENTARY(qe, q);
    DISPLAY_FLOAT_QUAT("qe", qe);

    struct FloatVect3 a = { 2., 1., 3.};
    DISPLAY_FLOAT_VECT3("a ", a);
    struct FloatVect3 a1;
    FLOAT_QUAT_VMULT(a1, q, a);
    DISPLAY_FLOAT_VECT3("a1", a1);

    struct FloatVect3 a2;
    FLOAT_QUAT_VMULT(a2, qe, a);
    DISPLAY_FLOAT_VECT3("a2", a2);





    return 0.;

}