C++ (Cpp) DISPLAY_INT32_RMAT_AS_FLOAT Beispiele

Beispiel #1

Datei: test_algebra.c Projekt: EwoudSmeur/paparazzi

float test_INT32_QUAT_OF_RMAT(struct FloatEulers *eul_f, bool display)
{
  struct Int32Eulers eul321_i;
  EULERS_BFP_OF_REAL(eul321_i, (*eul_f));

  struct Int32Eulers eul312_i;
  EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
  if (display) { DISPLAY_INT32_EULERS("eul312_i", eul312_i); }

  struct FloatRMat rmat_f;
  float_rmat_of_eulers_321(&rmat_f, eul_f);
  if (display) { DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); }
  if (display) { DISPLAY_FLOAT_RMAT("rmat float", rmat_f); }

  struct Int32RMat rmat_i;
  int32_rmat_of_eulers_321(&rmat_i, &eul321_i);
  if (display) { DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); }
  if (display) { DISPLAY_INT32_RMAT("rmat int", rmat_i); }
  if (display) { DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); }

  struct FloatQuat qf;
  float_quat_of_rmat(&qf, &rmat_f);
  //FLOAT_QUAT_WRAP_SHORTEST(qf);
  if (display) { DISPLAY_FLOAT_QUAT("qf", qf); }

  struct Int32Quat qi;
  int32_quat_of_rmat(&qi, &rmat_i);
  //int32_quat_wrap_shortest(&qi);
  if (display) { DISPLAY_INT32_QUAT("qi", qi); }
  if (display) { DISPLAY_INT32_QUAT_2("qi", qi); }

  struct FloatQuat qif;
  QUAT_FLOAT_OF_BFP(qif, qi);

  // dot product of two quaternions is 1 if they represent same rotation
  float qi_dot_qf = qif.qi * qf.qi + qif.qx * qf.qx + qif.qy * qf.qy + qif.qz * qf.qz;
  float err_norm = fabs(fabs(qi_dot_qf) - 1.);

  if (display) { printf("err %f\n", err_norm); }
  if (display) { printf("\n"); }

  return err_norm;

}

Beispiel #2

Datei: test_algebra.c Projekt: byrddev/paparazzi

float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) {

    struct Int32Eulers eul312_i;
    EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
    if (display)  DISPLAY_INT32_EULERS("eul312_i", eul312_i);

    struct FloatRMat rmat_f;
    FLOAT_RMAT_OF_EULERS_312(rmat_f, (*eul_f));
    if (display) DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
    if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f);

    struct Int32RMat rmat_i;
    INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
    if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
    if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i);
    if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i);

    struct FloatQuat qf;
    FLOAT_QUAT_OF_RMAT(qf, rmat_f);
    FLOAT_QUAT_WRAP_SHORTEST(qf);
    if (display) DISPLAY_FLOAT_QUAT("qf", qf);

    struct Int32Quat qi;
    INT32_QUAT_OF_RMAT(qi, rmat_i);
    INT32_QUAT_WRAP_SHORTEST(qi);
    if (display) DISPLAY_INT32_QUAT("qi", qi);
    if (display) DISPLAY_INT32_QUAT_2("qi", qi);

    struct FloatQuat qif;
    QUAT_FLOAT_OF_BFP(qif, qi);
    struct FloatQuat qerr;
    QUAT_DIFF(qerr, qif, qf);

    float err_norm = FLOAT_QUAT_NORM(qerr);
    if (display) printf("err %f\n", err_norm);
    if (display) printf("\n");

    return err_norm;

}