Umwandlung von Euler zu Quaternion und wieder zu Euler

[Marcel B. (cable545)]

Hallo,

ich wandel mit folgendem Code Euler-Winkel in ein Gesamt-Quaternion um:

void eul_to_quat(float* roll, float* pitch, float* yaw, QUATERNION* q)
{
  c_roll = cos(*roll / 2);
  c_pitch = cos(*pitch / 2);
  c_yaw = cos(*yaw / 2);
  
  s_roll = sin(*roll / 2);
  s_pitch = sin(*pitch / 2);
  s_yaw = sin(*yaw / 2);
  
  c_yaw_pitch = c_yaw * c_pitch;
  s_yaw_pitch = s_yaw * s_pitch;

  q->a = c_yaw_pitch * c_roll + s_yaw_pitch * s_roll;
  q->b = c_yaw_pitch * s_roll - s_yaw_pitch * c_roll;
  q->c = c_yaw * s_pitch * c_roll + s_yaw * c_pitch * s_roll;
  q->d = s_yaw * c_pitch * c_roll - c_yaw * s_pitch * s_roll;
}

Nun möchte ich das berechnete Gesamt-Quaternion mit folgendem Code 
wieder in Euler-Winkel umrechnen:

uint8 quat_to_eul(float* roll, float* pitch, float* yaw, QUATERNION* q)
{
  QUATERNION q_norm;
  float a_2, b_2, c_2, d_2; 
  
  if(!norm_q(q, &q_norm))
    return FALSE;
  
  a_2 = q_norm.a * q_norm.a;
  b_2 = q_norm.b * q_norm.b;
  c_2 = q_norm.c * q_norm.c;
  d_2 = q_norm.d * q_norm.d;

  *yaw   = atan2(2 * (q_norm.b * q_norm.c + q_norm.a * q_norm.d), a_2 + b_2 - c_2 - d_2);
  *pitch = asin(2 * (q_norm.a * q_norm.c - q_norm.b * q_norm.d));
  *roll  = atan2(2 * (q_norm.c * q_norm.d + q_norm.a * q_norm.b), a_2 - b_2 - c_2 + d_2);
  
  return TRUE;
}

Wenn ich nun die Euler-Winkel x(phi) = 0, y(teta) = 0, z(psi) = 179° 
wähle bekomme ich nach der Transformation in Quaternion und wieder 
zurück zu Euler-Winkeln die gleichen Werte raus. Wenn ich jedoch für psi 
181° oder größer wähle sind die Werte nicht mehr korrekt. Das gleiche 
gillt für den phi Winkel. Wird dieser größer als 179° gewählt, wird er 
nicht mehr korrekt von Quaternion zu Euler zurück gerechnet.

Bei einem teta(nicken) Winkel von 90° und größer hab ich das gleiche 
Problem. Wobei hier wahrscheinlich der Gimbal Lock das Problem ist.

Vielleicht zur Info. Als Rotationsvorschrift habe ich die ZYX Vorschrift 
gewählt. Das heißt zuerst wird um die Z-Achse gedreht(Gier-Winkel), dann 
um die Y-Achse(Nick-Winkel) und dann um die X-Achse(Roll-Winkel).

Die Frage ist, wie gehe ich mit diesen falschen Umrechnungen um? Gibt es 
Grenzwerte die ich bei der Umrechnung gesondert behandeln muss? Ich 
stehe da gerade ordentlich auf dem Schlauch.