quaternionToDegreesXYZ static method

void quaternionToDegreesXYZ(
  1. List<double> q,
  2. List<double> result
)

Implementation

static void quaternionToDegreesXYZ(List<double> q, List<double> result) {
  var qx = q[0];
  var qy = q[1];
  var qz = q[2];
  var qw = q[3];
  var qw2 = qw * qw;
  var qx2 = qx * qx;
  var qy2 = qy * qy;
  var qz2 = qz * qz;
  var test = qx * qy + qz * qw;
  var unit = qw2 + qx2 + qy2 + qz2;
  var conv = 180 / pi;

  if (test > 0.49999 * unit) {
    result[0] = 0;
    result[1] = 2 * atan2(qx, qw) * conv;
    result[2] = 90;
    return;
  }
  if (test < -0.49999 * unit) {
    result[0] = 0;
    result[1] = -2 * atan2(qx, qw) * conv;
    result[2] = -90;
    return;
  }

  result[0] = roundTo3Places(
      atan2(2 * qx * qw - 2 * qy * qz, 1 - 2 * qx2 - 2 * qz2) * conv);
  result[1] = roundTo3Places(
      atan2(2 * qy * qw - 2 * qx * qz, 1 - 2 * qy2 - 2 * qz2) * conv);
  result[2] = roundTo3Places(asin(2 * qx * qy + 2 * qz * qw) * conv);
}