Quaternion to Euler Angles conversion snippet

QuaternionToEulerAngles.c

#include <math.h>

struct Quaternion
{
  double w, x, y, z;
};

struct EulerAngles
{
  double roll, pitch, yaw;
};

struct EulerAngles ToEulerAngles(struct Quaternion q)
{
  struct EulerAngles angles;

  // roll (x-axis rotation)
  double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
  double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
  angles.roll = atan2(sinr_cosp, cosr_cosp);

  // pitch (y-axis rotation)
  double sinp = 2 * (q.w * q.y - q.z * q.x);
  if (fabs(sinp) >= 1)
    angles.pitch = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
  else
    angles.pitch = asin(sinp);

  // yaw (z-axis rotation)
  double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
  double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
  angles.yaw = atan2(siny_cosp, cosy_cosp);

  return angles;
}