QuaternionFromMatrix function

Quaternion QuaternionFromMatrix(Matrix4 mat)

Quaternion from rotation matrix (upper-left 3×3 of mat).

Implementation

Quaternion QuaternionFromMatrix(Matrix4 mat) {
  final trace = mat[0] + mat[5] + mat[10];
  if (trace > 0) {
    final s = 0.5 / math.sqrt(trace + 1.0);
    return Quaternion(
      (mat[6] - mat[9]) * s,
      (mat[8] - mat[2]) * s,
      (mat[1] - mat[4]) * s,
      0.25 / s,
    );
  } else if (mat[0] > mat[5] && mat[0] > mat[10]) {
    final s = 2.0 * math.sqrt(1.0 + mat[0] - mat[5] - mat[10]);
    return Quaternion(
      0.25 * s,
      (mat[4] + mat[1]) / s,
      (mat[8] + mat[2]) / s,
      (mat[6] - mat[9]) / s,
    );
  } else if (mat[5] > mat[10]) {
    final s = 2.0 * math.sqrt(1.0 + mat[5] - mat[0] - mat[10]);
    return Quaternion(
      (mat[4] + mat[1]) / s,
      0.25 * s,
      (mat[9] + mat[6]) / s,
      (mat[8] - mat[2]) / s,
    );
  } else {
    final s = 2.0 * math.sqrt(1.0 + mat[10] - mat[0] - mat[5]);
    return Quaternion(
      (mat[8] + mat[2]) / s,
      (mat[9] + mat[6]) / s,
      0.25 * s,
      (mat[1] - mat[4]) / s,
    );
  }
}