/*! @header
 *  This header defines functions for constructing and using quaternions.
 *  @copyright 2015-2016 Apple, Inc. All rights reserved.
 *  @unsorted                                                                 */

#ifndef SIMD_QUATERNIONS
#define SIMD_QUATERNIONS

#include <simd/base.h>
#if SIMD_COMPILER_HAS_REQUIRED_FEATURES
#include <simd/vector.h>
#include <simd/types.h>

#ifdef __cplusplus
extern "C" {
#endif
  
/*  MARK: - C and Objective-C float interfaces                                */

/*! @abstract Constructs a quaternion from four scalar values.
 *
 *  @param ix The first component of the imaginary (vector) part.
 *  @param iy The second component of the imaginary (vector) part.
 *  @param iz The third component of the imaginary (vector) part.
 *
 *  @param r The real (scalar) part.                                          */
static inline SIMD_CFUNC simd_quatf simd_quaternion(float ix, float iy, float iz, float r) {
  return (simd_quatf){ { ix, iy, iz, r } };
}
  
/*! @abstract Constructs a quaternion from an array of four scalars.
 *
 *  @discussion Note that the imaginary part of the quaternion comes from 
 *  array elements 0, 1, and 2, and the real part comes from element 3.       */
static inline SIMD_NONCONST simd_quatf simd_quaternion(const float xyzr[4]) {
  return (simd_quatf){ *(const simd_packed_float4 *)xyzr };
}
  
/*! @abstract Constructs a quaternion from a four-element vector.
 *
 *  @discussion Note that the imaginary (vector) part of the quaternion comes
 *  from lanes 0, 1, and 2 of the vector, and the real (scalar) part comes from
 *  lane 3.                                                                   */
static inline SIMD_CFUNC simd_quatf simd_quaternion(simd_float4 xyzr) {
  return (simd_quatf){ xyzr };
}
  
/*! @abstract Constructs a quaternion that rotates by `angle` radians about
 *  `axis`.                                                                   */
static inline SIMD_CFUNC simd_quatf simd_quaternion(float angle, simd_float3 axis);
  
/*! @abstract Construct a quaternion that rotates from one vector to another.
 *
 *  @param from A normalized three-element vector.
 *  @param to A normalized three-element vector.
 *
 *  @discussion The rotation axis is `simd_cross(from, to)`. If `from` and
 *  `to` point in opposite directions (to within machine precision), an
 *  arbitrary rotation axis is chosen, and the angle is pi radians.           */
static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float3 from, simd_float3 to);

/*! @abstract Construct a quaternion from a 3x3 rotation `matrix`.
 *
 *  @discussion If `matrix` is not orthogonal with determinant 1, the result
 *  is undefined.                                                             */
static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float3x3 matrix);

/*! @abstract Construct a quaternion from a 4x4 rotation `matrix`.
 *
 *  @discussion The last row and column of the matrix are ignored. This
 *  function is equivalent to calling simd_quaternion with the upper-left 3x3
 *  submatrix                .                                                */
static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float4x4 matrix);
  
/*! @abstract The real (scalar) part of the quaternion `q`.                   */
static inline SIMD_CFUNC float simd_real(simd_quatf q) {
  return q.vector.w;
}
  
/*! @abstract The imaginary (vector) part of the quaternion `q`.              */
static inline SIMD_CFUNC simd_float3 simd_imag(simd_quatf q) {
  return q.vector.xyz;
}
  
/*! @abstract The angle (in radians) of rotation represented by `q`.          */
static inline SIMD_CFUNC float simd_angle(simd_quatf q);
  
/*! @abstract The normalized axis (a 3-element vector) around which the
 *  action of the quaternion `q` rotates.                                     */
static inline SIMD_CFUNC simd_float3 simd_axis(simd_quatf q);
  
/*! @abstract The sum of the quaternions `p` and `q`.                         */
static inline SIMD_CFUNC simd_quatf simd_add(simd_quatf p, simd_quatf q);
  
/*! @abstract The difference of the quaternions `p` and `q`.                  */
static inline SIMD_CFUNC simd_quatf simd_sub(simd_quatf p, simd_quatf q);
  
/*! @abstract The product of the quaternions `p` and `q`.                     */
static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf p, simd_quatf q);
  
/*! @abstract The quaternion `q` scaled by the real value `a`.                */
static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf q, float a);
  
/*! @abstract The quaternion `q` scaled by the real value `a`.                */
static inline SIMD_CFUNC simd_quatf simd_mul(float a, simd_quatf q);
  
/*! @abstract The conjugate of the quaternion `q`.                            */
static inline SIMD_CFUNC simd_quatf simd_conjugate(simd_quatf q);
  
/*! @abstract The (multiplicative) inverse of the quaternion `q`.             */
static inline SIMD_CFUNC simd_quatf simd_inverse(simd_quatf q);
  
/*! @abstract The negation (additive inverse) of the quaternion `q`.          */
static inline SIMD_CFUNC simd_quatf simd_negate(simd_quatf q);
  
/*! @abstract The dot product of the quaternions `p` and `q` interpreted as
 *  four-dimensional vectors.                                                 */
static inline SIMD_CFUNC float simd_dot(simd_quatf p, simd_quatf q);
  
/*! @abstract The length of the quaternion `q`.                               */
static inline SIMD_CFUNC float simd_length(simd_quatf q);
  
/*! @abstract The unit quaternion obtained by normalizing `q`.                */
static inline SIMD_CFUNC simd_quatf simd_normalize(simd_quatf q);
  
/*! @abstract Rotates the vector `v` by the quaternion `q`.                   */
static inline SIMD_CFUNC simd_float3 simd_act(simd_quatf q, simd_float3 v);
  
/*! @abstract Logarithm of the quaternion `q`.
 *  @discussion Do not call this function directly; use `log(q)` instead.
 *
 *  We can write a quaternion `q` in the form: `r(cos(t) + sin(t)v)` where
 *  `r` is the length of `q`, `t` is an angle, and `v` is a unit 3-vector.
 *  The logarithm of `q` is `log(r) + tv`, just like the logarithm of the
 *  complex number `r*(cos(t) + i sin(t))` is `log(r) + it`.
 *
 *  Note that this function is not robust against poorly-scaled non-unit
 *  quaternions, because it is primarily used for spline interpolation of
 *  unit quaternions. If you need to compute a robust logarithm of general
 *  quaternions, you can use the following approach:
 *
 *    scale = simd_reduce_max(simd_abs(q.vector));
 *    logq = log(simd_recip(scale)*q);
 *    logq.real += log(scale);
 *    return logq;                                                            */
static SIMD_NOINLINE simd_quatf __tg_log(simd_quatf q);
    
/*! @abstract Inverse of `log( )`; the exponential map on quaternions.
 *  @discussion Do not call this function directly; use `exp(q)` instead.     */
static SIMD_NOINLINE simd_quatf __tg_exp(simd_quatf q);
  
/*! @abstract Spherical linear interpolation along the shortest arc between
 *  quaternions `q0` and `q1`.                                                */
static SIMD_NOINLINE simd_quatf simd_slerp(simd_quatf q0, simd_quatf q1, float t);

/*! @abstract Spherical linear interpolation along the longest arc between
 *  quaternions `q0` and `q1`.                                                */
static SIMD_NOINLINE simd_quatf simd_slerp_longest(simd_quatf q0, simd_quatf q1, float t);

/*! @abstract Interpolate between quaternions along a spherical cubic spline.
 *
 *  @discussion The function interpolates between q1 and q2. q0 is the left
 *  endpoint of the previous interval, and q3 is the right endpoint of the next
 *  interval. Use this function to smoothly interpolate between a sequence of
 *  rotations.                                                                */
static SIMD_NOINLINE simd_quatf simd_spline(simd_quatf q0, simd_quatf q1, simd_quatf q2, simd_quatf q3, float t);

/*! @abstract Spherical cubic Bezier interpolation between quaternions.
 *
 *  @discussion The function treats q0 ... q3 as control points and uses slerp
 *  in place of lerp in the De Castlejeau algorithm. The endpoints of
 *  interpolation are thus q0 and q3, and the curve will not generally pass
 *  through q1 or q2. Note that the convex hull property of "standard" Bezier
 *  curve does not hold on the sphere.                                        */
static SIMD_NOINLINE simd_quatf simd_bezier(simd_quatf q0, simd_quatf q1, simd_quatf q2, simd_quatf q3, float t);
  
#ifdef __cplusplus
} /* extern "C" */
/*  MARK: - C++ float interfaces                                              */

namespace simd {
  struct quatf : ::simd_quatf {
    /*! @abstract The identity quaternion.                                    */
    quatf( ) : ::simd_quatf(::simd_quaternion((float4){0,0,0,1})) { }
    
    /*! @abstract Constructs a C++ quaternion from a C quaternion.            */
    quatf(::simd_quatf q) : ::simd_quatf(q) { }
    
    /*! @abstract Constructs a quaternion from components.                    */
    quatf(float ix, float iy, float iz, float r) : ::simd_quatf(::simd_quaternion(ix, iy, iz, r)) { }
    
    /*! @abstract Constructs a quaternion from an array of scalars.           */
    quatf(const float xyzr[4]) : ::simd_quatf(::simd_quaternion(xyzr)) { }
    
    /*! @abstract Constructs a quaternion from a vector.                      */
    quatf(float4 xyzr) : ::simd_quatf(::simd_quaternion(xyzr)) { }
    
    /*! @abstract Quaternion representing rotation about `axis` by `angle` 
     *  radians.                                                              */
    quatf(float angle, float3 axis) : ::simd_quatf(::simd_quaternion(angle, axis)) { }
    
    /*! @abstract Quaternion that rotates `from` into `to`.                   */
    quatf(float3 from, float3 to) : ::simd_quatf(::simd_quaternion(from, to)) { }
    
    /*! @abstract Constructs a quaternion from a rotation matrix.             */
    quatf(::simd_float3x3 matrix) : ::simd_quatf(::simd_quaternion(matrix)) { }
    
    /*! @abstract Constructs a quaternion from a rotation matrix.             */
    quatf(::simd_float4x4 matrix) : ::simd_quatf(::simd_quaternion(matrix)) { }
  
    /*! @abstract The real (scalar) part of the quaternion.                   */
    float real(void) const { return ::simd_real(*this); }
    
    /*! @abstract The imaginary (vector) part of the quaternion.              */
    float3 imag(void) const { return ::simd_imag(*this); }
    
    /*! @abstract The angle the quaternion rotates by.                        */
    float angle(void) const { return ::simd_angle(*this); }
    
    /*! @abstract The axis the quaternion rotates about.                      */
    float3 axis(void) const { return ::simd_axis(*this); }
    
    /*! @abstract The length of the quaternion.                               */
    float length(void) const { return ::simd_length(*this); }
    
    /*! @abstract Act on the vector `v` by rotation.                          */
    float3  operator()(const ::simd_float3 v) const { return ::simd_act(*this, v); }
  };
  
  static SIMD_CPPFUNC quatf operator+(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_add(p, q); }
  static SIMD_CPPFUNC quatf operator-(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_sub(p, q); }
  static SIMD_CPPFUNC quatf operator-(const ::simd_quatf p) { return ::simd_negate(p); }
  static SIMD_CPPFUNC quatf operator*(const float r, const ::simd_quatf p) { return ::simd_mul(r, p); }
  static SIMD_CPPFUNC quatf operator*(const ::simd_quatf p, const float r) { return ::simd_mul(p, r); }
  static SIMD_CPPFUNC quatf operator*(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_mul(p, q); }
  static SIMD_CPPFUNC quatf operator/(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_mul(p, ::simd_inverse(q)); }
  static SIMD_INLINE SIMD_NODEBUG quatf operator+=(quatf &p, const ::simd_quatf q) { return p = p+q; }
  static SIMD_INLINE SIMD_NODEBUG quatf operator-=(quatf &p, const ::simd_quatf q) { return p = p-q; }
  static SIMD_INLINE SIMD_NODEBUG quatf operator*=(quatf &p, const float r) { return p = p*r; }
  static SIMD_INLINE SIMD_NODEBUG quatf operator*=(quatf &p, const ::simd_quatf q) { return p = p*q; }
  static SIMD_INLINE SIMD_NODEBUG quatf operator/=(quatf &p, const ::simd_quatf q) { return p = p/q; }
  
  /*! @abstract The conjugate of the quaternion `q`.                          */
  static SIMD_CPPFUNC quatf conjugate(const ::simd_quatf p) { return ::simd_conjugate(p); }
  
  /*! @abstract The (multiplicative) inverse of the quaternion `q`.           */
  static SIMD_CPPFUNC quatf inverse(const ::simd_quatf p) { return ::simd_inverse(p); }

  /*! @abstract The dot product of the quaternions `p` and `q` interpreted as
   *  four-dimensional vectors.                                               */
  static SIMD_CPPFUNC float dot(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_dot(p, q); }
  
  /*! @abstract The unit quaternion obtained by normalizing `q`.              */
  static SIMD_CPPFUNC quatf normalize(const ::simd_quatf p) { return ::simd_normalize(p); }

  /*! @abstract logarithm of the quaternion `q`.                              */
  static SIMD_CPPFUNC quatf log(const ::simd_quatf q) { return ::__tg_log(q); }

  /*! @abstract exponential map of quaterion `q`.                             */
  static SIMD_CPPFUNC quatf exp(const ::simd_quatf q) { return ::__tg_exp(q); }
  
  /*! @abstract Spherical linear interpolation along the shortest arc between
   *  quaternions `q0` and `q1`.                                              */
  static SIMD_CPPFUNC quatf slerp(const ::simd_quatf p0, const ::simd_quatf p1, float t) { return ::simd_slerp(p0, p1, t); }
  
  /*! @abstract Spherical linear interpolation along the longest arc between
   *  quaternions `q0` and `q1`.                                              */
  static SIMD_CPPFUNC quatf slerp_longest(const ::simd_quatf p0, const ::simd_quatf p1, float t) { return ::simd_slerp_longest(p0, p1, t); }
  
  /*! @abstract Interpolate between quaternions along a spherical cubic spline.
   *
   *  @discussion The function interpolates between q1 and q2. q0 is the left
   *  endpoint of the previous interval, and q3 is the right endpoint of the next
   *  interval. Use this function to smoothly interpolate between a sequence of
   *  rotations.                                                              */
  static SIMD_CPPFUNC quatf spline(const ::simd_quatf p0, const ::simd_quatf p1, const ::simd_quatf p2, const ::simd_quatf p3, float t) { return ::simd_spline(p0, p1, p2, p3, t); }
  
  /*! @abstract Spherical cubic Bezier interpolation between quaternions.
   *
   *  @discussion The function treats q0 ... q3 as control points and uses slerp
   *  in place of lerp in the De Castlejeau algorithm. The endpoints of
   *  interpolation are thus q0 and q3, and the curve will not generally pass
   *  through q1 or q2. Note that the convex hull property of "standard" Bezier
   *  curve does not hold on the sphere.                                      */
  static SIMD_CPPFUNC quatf bezier(const ::simd_quatf p0, const ::simd_quatf p1, const ::simd_quatf p2, const ::simd_quatf p3, float t) { return ::simd_bezier(p0, p1, p2, p3, t); }
}

extern "C" {
#endif /* __cplusplus */
  
/*  MARK: - float implementations                                             */

#include <simd/math.h>
#include <simd/geometry.h>
  
/*  tg_promote is implementation gobbledygook that enables the compile-time
 *  dispatching in tgmath.h to work its magic.                                */
static simd_quatf __attribute__((__overloadable__)) __tg_promote(simd_quatf);
  
/*! @abstract Constructs a quaternion from imaginary and real parts.
 *  @discussion This function is hidden behind an underscore to avoid confusion
 *  with the angle-axis constructor.                                          */
static inline SIMD_CFUNC simd_quatf _simd_quaternion(simd_float3 imag, float real) {
  return simd_quaternion(simd_make_float4(imag, real));
}
  
static inline SIMD_CFUNC simd_quatf simd_quaternion(float angle, simd_float3 axis) {
  return _simd_quaternion(sin(angle/2) * axis, cos(angle/2));
}
  
static inline SIMD_CFUNC float simd_angle(simd_quatf q) {
  return 2*atan2(simd_length(q.vector.xyz), q.vector.w);
}
  
static inline SIMD_CFUNC simd_float3 simd_axis(simd_quatf q) {
  return simd_normalize(q.vector.xyz);
}
  
static inline SIMD_CFUNC simd_quatf simd_add(simd_quatf p, simd_quatf q) {
  return simd_quaternion(p.vector + q.vector);
}
  
static inline SIMD_CFUNC simd_quatf simd_sub(simd_quatf p, simd_quatf q) {
  return simd_quaternion(p.vector - q.vector);
}

static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf p, simd_quatf q) {
  #pragma STDC FP_CONTRACT ON
  return simd_quaternion((p.vector.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
                          p.vector.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5)) +
                         (p.vector.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6) +
                          p.vector.w * q.vector));
}

static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf q, float a) {
  return simd_quaternion(a * q.vector);
}

static inline SIMD_CFUNC simd_quatf simd_mul(float a, simd_quatf q) {
  return simd_mul(q,a);
}
  
static inline SIMD_CFUNC simd_quatf simd_conjugate(simd_quatf q) {
  return simd_quaternion(q.vector * (simd_float4){-1,-1,-1, 1});
}
  
static inline SIMD_CFUNC simd_quatf simd_inverse(simd_quatf q) {
  return simd_quaternion(simd_conjugate(q).vector * simd_recip(simd_length_squared(q.vector)));
}
  
static inline SIMD_CFUNC simd_quatf simd_negate(simd_quatf q) {
  return simd_quaternion(-q.vector);
}
  
static inline SIMD_CFUNC float simd_dot(simd_quatf p, simd_quatf q) {
  return simd_dot(p.vector, q.vector);
}
  
static inline SIMD_CFUNC float simd_length(simd_quatf q) {
  return simd_length(q.vector);
}
  
static inline SIMD_CFUNC simd_quatf simd_normalize(simd_quatf q) {
  float length_squared = simd_length_squared(q.vector);
  if (length_squared == 0) {
    return simd_quaternion((simd_float4){0,0,0,1});
  }
  return simd_quaternion(q.vector * simd_rsqrt(length_squared));
}

#if defined __arm__ || defined __arm64__
/*! @abstract Multiplies the vector `v` by the quaternion `q`.
 *  
 *  @discussion This IS NOT the action of `q` on `v` (i.e. this is not rotation
 *  by `q`. That operation is provided by `simd_act(q, v)`. This function is an
 *  implementation detail and you should not call it directly. It may be
 *  removed or modified in future versions of the simd module.                */
static inline SIMD_CFUNC simd_quatf _simd_mul_vq(simd_float3 v, simd_quatf q) {
  #pragma STDC FP_CONTRACT ON
  return simd_quaternion(v.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
                         v.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5) +
                         v.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6));
}
#endif
  
static inline SIMD_CFUNC simd_float3 simd_act(simd_quatf q, simd_float3 v) {
#if defined __arm__ || defined __arm64__
  return simd_mul(q, _simd_mul_vq(v, simd_conjugate(q))).vector.xyz;
#else
  #pragma STDC FP_CONTRACT ON
  simd_float3 t = 2*simd_cross(simd_imag(q),v);
  return v + simd_real(q)*t + simd_cross(simd_imag(q), t);
#endif
}

static SIMD_NOINLINE simd_quatf __tg_log(simd_quatf q) {
  float real = __tg_log(simd_length_squared(q.vector))/2;
  if (simd_equal(simd_imag(q), 0)) return _simd_quaternion(0, real);
  simd_float3 imag = __tg_acos(simd_real(q)/simd_length(q)) * simd_normalize(simd_imag(q));
  return _simd_quaternion(imag, real);
}
  
static SIMD_NOINLINE simd_quatf __tg_exp(simd_quatf q) {
  //  angle is actually *twice* the angle of the rotation corresponding to
  //  the resulting quaternion, which is why we don't simply use the (angle,
  //  axis) constructor to generate `unit`.
  float angle = simd_length(simd_imag(q));
  if (angle == 0) return _simd_quaternion(0, exp(simd_real(q)));
  simd_float3 axis = simd_normalize(simd_imag(q));
  simd_quatf unit = _simd_quaternion(sin(angle)*axis, cosf(angle));
  return simd_mul(exp(simd_real(q)), unit);
}
 
/*! @abstract Implementation detail of the `simd_quaternion(from, to)`
 *  initializer.
 *
 *  @discussion Computes the quaternion rotation `from` to `to` if they are
 *  separated by less than 90 degrees. Not numerically stable for larger
 *  angles. This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static inline SIMD_CFUNC simd_quatf _simd_quaternion_reduced(simd_float3 from, simd_float3 to) {
  simd_float3 half = simd_normalize(from + to);
  return _simd_quaternion(simd_cross(from, half), simd_dot(from, half));
}

static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float3 from, simd_float3 to) {
  
  //  If the angle between from and to is not too big, we can compute the
  //  rotation accurately using a simple implementation.
  if (simd_dot(from, to) >= 0) {
    return _simd_quaternion_reduced(from, to);
  }
  
  //  Because from and to are more than 90 degrees apart, we compute the
  //  rotation in two stages (from -> half), (half -> to) to preserve numerical
  //  accuracy.
  simd_float3 half = simd_normalize(from) + simd_normalize(to);
  
  if (simd_length_squared(half) <= 0x1p-46f) {
    //  half is nearly zero, so from and to point in nearly opposite directions
    //  and the rotation is numerically underspecified. Pick an axis orthogonal
    //  to the vectors, and use an angle of pi radians.
    simd_float3 abs_from = simd_abs(from);
    if (abs_from.x <= abs_from.y && abs_from.x <= abs_from.z)
      return _simd_quaternion(simd_normalize(simd_cross(from, (simd_float3){1,0,0})), 0.f);
    else if (abs_from.y <= abs_from.z)
      return _simd_quaternion(simd_normalize(simd_cross(from, (simd_float3){0,1,0})), 0.f);
    else
      return _simd_quaternion(simd_normalize(simd_cross(from, (simd_float3){0,0,1})), 0.f);
  }

  //  Compute the two-step rotation.                         */
  half = simd_normalize(half);
  return simd_mul(_simd_quaternion_reduced(from, half),
                  _simd_quaternion_reduced(half, to));
}

static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float3x3 matrix) {
  const simd_float3 *mat = matrix.columns;
  float trace = mat[0][0] + mat[1][1] + mat[2][2];
  if (trace >= 0.0) {
    float r = 2*sqrt(1 + trace);
    float rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
                           rinv*(mat[2][0] - mat[0][2]),
                           rinv*(mat[0][1] - mat[1][0]),
                           r/4);
  } else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
    float r = 2*sqrt(1 - mat[1][1] - mat[2][2] + mat[0][0]);
    float rinv = simd_recip(r);
    return simd_quaternion(r/4,
                           rinv*(mat[0][1] + mat[1][0]),
                           rinv*(mat[0][2] + mat[2][0]),
                           rinv*(mat[1][2] - mat[2][1]));
  } else if (mat[1][1] >= mat[2][2]) {
    float r = 2*sqrt(1 - mat[0][0] - mat[2][2] + mat[1][1]);
    float rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
                           r/4,
                           rinv*(mat[1][2] + mat[2][1]),
                           rinv*(mat[2][0] - mat[0][2]));
  } else {
    float r = 2*sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
    float rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
                           rinv*(mat[1][2] + mat[2][1]),
                           r/4,
                           rinv*(mat[0][1] - mat[1][0]));
  }
}
  
static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float4x4 matrix) {
  const simd_float4 *mat = matrix.columns;
  float trace = mat[0][0] + mat[1][1] + mat[2][2];
  if (trace >= 0.0) {
    float r = 2*sqrt(1 + trace);
    float rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
                           rinv*(mat[2][0] - mat[0][2]),
                           rinv*(mat[0][1] - mat[1][0]),
                           r/4);
  } else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
    float r = 2*sqrt(1 - mat[1][1] - mat[2][2] + mat[0][0]);
    float rinv = simd_recip(r);
    return simd_quaternion(r/4,
                           rinv*(mat[0][1] + mat[1][0]),
                           rinv*(mat[0][2] + mat[2][0]),
                           rinv*(mat[1][2] - mat[2][1]));
  } else if (mat[1][1] >= mat[2][2]) {
    float r = 2*sqrt(1 - mat[0][0] - mat[2][2] + mat[1][1]);
    float rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
                           r/4,
                           rinv*(mat[1][2] + mat[2][1]),
                           rinv*(mat[2][0] - mat[0][2]));
  } else {
    float r = 2*sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
    float rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
                           rinv*(mat[1][2] + mat[2][1]),
                           r/4,
                           rinv*(mat[0][1] - mat[1][0]));
  }
}
  
/*! @abstract The angle between p and q interpreted as 4-dimensional vectors.
 *
 *  @discussion This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static SIMD_NOINLINE float _simd_angle(simd_quatf p, simd_quatf q) {
  return 2*atan2(simd_length(p.vector - q.vector), simd_length(p.vector + q.vector));
}
  
/*! @abstract sin(x)/x.
 *
 *  @discussion This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static SIMD_CFUNC float _simd_sinc(float x) {
  if (x == 0) return 1;
  return sin(x)/x;
}
 
/*! @abstract Spherical lerp between q0 and q1.
 *
 *  @discussion This function may interpolate along either the longer or
 *  shorter path between q0 and q1; it is used as an implementation detail
 *  in `simd_slerp` and `simd_slerp_longest`; you should use those functions
 *  instead of calling this directly.                                         */
static SIMD_NOINLINE simd_quatf _simd_slerp_internal(simd_quatf q0, simd_quatf q1, float t) {
  float s = 1 - t;
  float a = _simd_angle(q0, q1);
  float r = simd_recip(_simd_sinc(a));
  return simd_normalize(simd_quaternion(_simd_sinc(s*a)*r*s*q0.vector + _simd_sinc(t*a)*r*t*q1.vector));
}
  
static SIMD_NOINLINE simd_quatf simd_slerp(simd_quatf q0, simd_quatf q1, float t) {
  if (simd_dot(q0, q1) >= 0)
    return _simd_slerp_internal(q0, q1, t);
  return _simd_slerp_internal(q0, simd_negate(q1), t);
}

static SIMD_NOINLINE simd_quatf simd_slerp_longest(simd_quatf q0, simd_quatf q1, float t) {
  if (simd_dot(q0, q1) >= 0)
    return _simd_slerp_internal(q0, simd_negate(q1), t);
  return _simd_slerp_internal(q0, q1, t);
}
  
/*! @discussion This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static SIMD_NOINLINE simd_quatf _simd_intermediate(simd_quatf q0, simd_quatf q1, simd_quatf q2) {
  simd_quatf p0 = __tg_log(simd_mul(q0, simd_inverse(q1)));
  simd_quatf p2 = __tg_log(simd_mul(q2, simd_inverse(q1)));
  return simd_normalize(simd_mul(q1, __tg_exp(simd_mul(-0.25, simd_add(p0,p2)))));
}

/*! @discussion This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static SIMD_NOINLINE simd_quatf _simd_squad(simd_quatf q0, simd_quatf qa, simd_quatf qb, simd_quatf q1, float t) {
  simd_quatf r0 = _simd_slerp_internal(q0, q1, t);
  simd_quatf r1 = _simd_slerp_internal(qa, qb, t);
  return _simd_slerp_internal(r0, r1, 2*t*(1 - t));
}
  
static SIMD_NOINLINE simd_quatf simd_spline(simd_quatf q0, simd_quatf q1, simd_quatf q2, simd_quatf q3, float t) {
  simd_quatf qa = _simd_intermediate(q0, q1, q2);
  simd_quatf qb = _simd_intermediate(q1, q2, q3);
  return _simd_squad(q1, qa, qb, q2, t);
}
  
static SIMD_NOINLINE simd_quatf simd_bezier(simd_quatf q0, simd_quatf q1, simd_quatf q2, simd_quatf q3, float t) {
  simd_quatf q01 = _simd_slerp_internal(q0, q1, t);
  simd_quatf q12 = _simd_slerp_internal(q1, q2, t);
  simd_quatf q23 = _simd_slerp_internal(q2, q3, t);
  simd_quatf q012 = _simd_slerp_internal(q01, q12, t);
  simd_quatf q123 = _simd_slerp_internal(q12, q23, t);
  return _simd_slerp_internal(q012, q123, t);
}

/*  MARK: - C and Objective-C double interfaces                                */

/*! @abstract Constructs a quaternion from four scalar values.
 *
 *  @param ix The first component of the imaginary (vector) part.
 *  @param iy The second component of the imaginary (vector) part.
 *  @param iz The third component of the imaginary (vector) part.
 *
 *  @param r The real (scalar) part.                                          */
static inline SIMD_CFUNC simd_quatd simd_quaternion(double ix, double iy, double iz, double r) {
  return (simd_quatd){ { ix, iy, iz, r } };
}
  
/*! @abstract Constructs a quaternion from an array of four scalars.
 *
 *  @discussion Note that the imaginary part of the quaternion comes from 
 *  array elements 0, 1, and 2, and the real part comes from element 3.       */
static inline SIMD_NONCONST simd_quatd simd_quaternion(const double xyzr[4]) {
  return (simd_quatd){ *(const simd_packed_double4 *)xyzr };
}
  
/*! @abstract Constructs a quaternion from a four-element vector.
 *
 *  @discussion Note that the imaginary (vector) part of the quaternion comes
 *  from lanes 0, 1, and 2 of the vector, and the real (scalar) part comes from
 *  lane 3.                                                                   */
static inline SIMD_CFUNC simd_quatd simd_quaternion(simd_double4 xyzr) {
  return (simd_quatd){ xyzr };
}
  
/*! @abstract Constructs a quaternion that rotates by `angle` radians about
 *  `axis`.                                                                   */
static inline SIMD_CFUNC simd_quatd simd_quaternion(double angle, simd_double3 axis);
  
/*! @abstract Construct a quaternion that rotates from one vector to another.
 *
 *  @param from A normalized three-element vector.
 *  @param to A normalized three-element vector.
 *
 *  @discussion The rotation axis is `simd_cross(from, to)`. If `from` and
 *  `to` point in opposite directions (to within machine precision), an
 *  arbitrary rotation axis is chosen, and the angle is pi radians.           */
static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double3 from, simd_double3 to);

/*! @abstract Construct a quaternion from a 3x3 rotation `matrix`.
 *
 *  @discussion If `matrix` is not orthogonal with determinant 1, the result
 *  is undefined.                                                             */
static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double3x3 matrix);

/*! @abstract Construct a quaternion from a 4x4 rotation `matrix`.
 *
 *  @discussion The last row and column of the matrix are ignored. This
 *  function is equivalent to calling simd_quaternion with the upper-left 3x3
 *  submatrix                .                                                */
static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double4x4 matrix);
  
/*! @abstract The real (scalar) part of the quaternion `q`.                   */
static inline SIMD_CFUNC double simd_real(simd_quatd q) {
  return q.vector.w;
}
  
/*! @abstract The imaginary (vector) part of the quaternion `q`.              */
static inline SIMD_CFUNC simd_double3 simd_imag(simd_quatd q) {
  return q.vector.xyz;
}
  
/*! @abstract The angle (in radians) of rotation represented by `q`.          */
static inline SIMD_CFUNC double simd_angle(simd_quatd q);
  
/*! @abstract The normalized axis (a 3-element vector) around which the
 *  action of the quaternion `q` rotates.                                     */
static inline SIMD_CFUNC simd_double3 simd_axis(simd_quatd q);
  
/*! @abstract The sum of the quaternions `p` and `q`.                         */
static inline SIMD_CFUNC simd_quatd simd_add(simd_quatd p, simd_quatd q);
  
/*! @abstract The difference of the quaternions `p` and `q`.                  */
static inline SIMD_CFUNC simd_quatd simd_sub(simd_quatd p, simd_quatd q);
  
/*! @abstract The product of the quaternions `p` and `q`.                     */
static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd p, simd_quatd q);
  
/*! @abstract The quaternion `q` scaled by the real value `a`.                */
static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd q, double a);
  
/*! @abstract The quaternion `q` scaled by the real value `a`.                */
static inline SIMD_CFUNC simd_quatd simd_mul(double a, simd_quatd q);
  
/*! @abstract The conjugate of the quaternion `q`.                            */
static inline SIMD_CFUNC simd_quatd simd_conjugate(simd_quatd q);
  
/*! @abstract The (multiplicative) inverse of the quaternion `q`.             */
static inline SIMD_CFUNC simd_quatd simd_inverse(simd_quatd q);
  
/*! @abstract The negation (additive inverse) of the quaternion `q`.          */
static inline SIMD_CFUNC simd_quatd simd_negate(simd_quatd q);
  
/*! @abstract The dot product of the quaternions `p` and `q` interpreted as
 *  four-dimensional vectors.                                                 */
static inline SIMD_CFUNC double simd_dot(simd_quatd p, simd_quatd q);
  
/*! @abstract The length of the quaternion `q`.                               */
static inline SIMD_CFUNC double simd_length(simd_quatd q);
  
/*! @abstract The unit quaternion obtained by normalizing `q`.                */
static inline SIMD_CFUNC simd_quatd simd_normalize(simd_quatd q);
  
/*! @abstract Rotates the vector `v` by the quaternion `q`.                   */
static inline SIMD_CFUNC simd_double3 simd_act(simd_quatd q, simd_double3 v);
  
/*! @abstract Logarithm of the quaternion `q`.
 *  @discussion Do not call this function directly; use `log(q)` instead.
 *
 *  We can write a quaternion `q` in the form: `r(cos(t) + sin(t)v)` where
 *  `r` is the length of `q`, `t` is an angle, and `v` is a unit 3-vector.
 *  The logarithm of `q` is `log(r) + tv`, just like the logarithm of the
 *  complex number `r*(cos(t) + i sin(t))` is `log(r) + it`.
 *
 *  Note that this function is not robust against poorly-scaled non-unit
 *  quaternions, because it is primarily used for spline interpolation of
 *  unit quaternions. If you need to compute a robust logarithm of general
 *  quaternions, you can use the following approach:
 *
 *    scale = simd_reduce_max(simd_abs(q.vector));
 *    logq = log(simd_recip(scale)*q);
 *    logq.real += log(scale);
 *    return logq;                                                            */
static SIMD_NOINLINE simd_quatd __tg_log(simd_quatd q);
    
/*! @abstract Inverse of `log( )`; the exponential map on quaternions.
 *  @discussion Do not call this function directly; use `exp(q)` instead.     */
static SIMD_NOINLINE simd_quatd __tg_exp(simd_quatd q);
  
/*! @abstract Spherical linear interpolation along the shortest arc between
 *  quaternions `q0` and `q1`.                                                */
static SIMD_NOINLINE simd_quatd simd_slerp(simd_quatd q0, simd_quatd q1, double t);

/*! @abstract Spherical linear interpolation along the longest arc between
 *  quaternions `q0` and `q1`.                                                */
static SIMD_NOINLINE simd_quatd simd_slerp_longest(simd_quatd q0, simd_quatd q1, double t);

/*! @abstract Interpolate between quaternions along a spherical cubic spline.
 *
 *  @discussion The function interpolates between q1 and q2. q0 is the left
 *  endpoint of the previous interval, and q3 is the right endpoint of the next
 *  interval. Use this function to smoothly interpolate between a sequence of
 *  rotations.                                                                */
static SIMD_NOINLINE simd_quatd simd_spline(simd_quatd q0, simd_quatd q1, simd_quatd q2, simd_quatd q3, double t);

/*! @abstract Spherical cubic Bezier interpolation between quaternions.
 *
 *  @discussion The function treats q0 ... q3 as control points and uses slerp
 *  in place of lerp in the De Castlejeau algorithm. The endpoints of
 *  interpolation are thus q0 and q3, and the curve will not generally pass
 *  through q1 or q2. Note that the convex hull property of "standard" Bezier
 *  curve does not hold on the sphere.                                        */
static SIMD_NOINLINE simd_quatd simd_bezier(simd_quatd q0, simd_quatd q1, simd_quatd q2, simd_quatd q3, double t);
  
#ifdef __cplusplus
} /* extern "C" */
/*  MARK: - C++ double interfaces                                              */

namespace simd {
  struct quatd : ::simd_quatd {
    /*! @abstract The identity quaternion.                                    */
    quatd( ) : ::simd_quatd(::simd_quaternion((double4){0,0,0,1})) { }
    
    /*! @abstract Constructs a C++ quaternion from a C quaternion.            */
    quatd(::simd_quatd q) : ::simd_quatd(q) { }
    
    /*! @abstract Constructs a quaternion from components.                    */
    quatd(double ix, double iy, double iz, double r) : ::simd_quatd(::simd_quaternion(ix, iy, iz, r)) { }
    
    /*! @abstract Constructs a quaternion from an array of scalars.           */
    quatd(const double xyzr[4]) : ::simd_quatd(::simd_quaternion(xyzr)) { }
    
    /*! @abstract Constructs a quaternion from a vector.                      */
    quatd(double4 xyzr) : ::simd_quatd(::simd_quaternion(xyzr)) { }
    
    /*! @abstract Quaternion representing rotation about `axis` by `angle` 
     *  radians.                                                              */
    quatd(double angle, double3 axis) : ::simd_quatd(::simd_quaternion(angle, axis)) { }
    
    /*! @abstract Quaternion that rotates `from` into `to`.                   */
    quatd(double3 from, double3 to) : ::simd_quatd(::simd_quaternion(from, to)) { }
    
    /*! @abstract Constructs a quaternion from a rotation matrix.             */
    quatd(::simd_double3x3 matrix) : ::simd_quatd(::simd_quaternion(matrix)) { }
    
    /*! @abstract Constructs a quaternion from a rotation matrix.             */
    quatd(::simd_double4x4 matrix) : ::simd_quatd(::simd_quaternion(matrix)) { }
  
    /*! @abstract The real (scalar) part of the quaternion.                   */
    double real(void) const { return ::simd_real(*this); }
    
    /*! @abstract The imaginary (vector) part of the quaternion.              */
    double3 imag(void) const { return ::simd_imag(*this); }
    
    /*! @abstract The angle the quaternion rotates by.                        */
    double angle(void) const { return ::simd_angle(*this); }
    
    /*! @abstract The axis the quaternion rotates about.                      */
    double3 axis(void) const { return ::simd_axis(*this); }
    
    /*! @abstract The length of the quaternion.                               */
    double length(void) const { return ::simd_length(*this); }
    
    /*! @abstract Act on the vector `v` by rotation.                          */
    double3  operator()(const ::simd_double3 v) const { return ::simd_act(*this, v); }
  };
  
  static SIMD_CPPFUNC quatd operator+(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_add(p, q); }
  static SIMD_CPPFUNC quatd operator-(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_sub(p, q); }
  static SIMD_CPPFUNC quatd operator-(const ::simd_quatd p) { return ::simd_negate(p); }
  static SIMD_CPPFUNC quatd operator*(const double r, const ::simd_quatd p) { return ::simd_mul(r, p); }
  static SIMD_CPPFUNC quatd operator*(const ::simd_quatd p, const double r) { return ::simd_mul(p, r); }
  static SIMD_CPPFUNC quatd operator*(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_mul(p, q); }
  static SIMD_CPPFUNC quatd operator/(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_mul(p, ::simd_inverse(q)); }
  static SIMD_INLINE SIMD_NODEBUG quatd operator+=(quatd &p, const ::simd_quatd q) { return p = p+q; }
  static SIMD_INLINE SIMD_NODEBUG quatd operator-=(quatd &p, const ::simd_quatd q) { return p = p-q; }
  static SIMD_INLINE SIMD_NODEBUG quatd operator*=(quatd &p, const double r) { return p = p*r; }
  static SIMD_INLINE SIMD_NODEBUG quatd operator*=(quatd &p, const ::simd_quatd q) { return p = p*q; }
  static SIMD_INLINE SIMD_NODEBUG quatd operator/=(quatd &p, const ::simd_quatd q) { return p = p/q; }
  
  /*! @abstract The conjugate of the quaternion `q`.                          */
  static SIMD_CPPFUNC quatd conjugate(const ::simd_quatd p) { return ::simd_conjugate(p); }
  
  /*! @abstract The (multiplicative) inverse of the quaternion `q`.           */
  static SIMD_CPPFUNC quatd inverse(const ::simd_quatd p) { return ::simd_inverse(p); }

  /*! @abstract The dot product of the quaternions `p` and `q` interpreted as
   *  four-dimensional vectors.                                               */
  static SIMD_CPPFUNC double dot(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_dot(p, q); }
  
  /*! @abstract The unit quaternion obtained by normalizing `q`.              */
  static SIMD_CPPFUNC quatd normalize(const ::simd_quatd p) { return ::simd_normalize(p); }

  /*! @abstract logarithm of the quaternion `q`.                              */
  static SIMD_CPPFUNC quatd log(const ::simd_quatd q) { return ::__tg_log(q); }

  /*! @abstract exponential map of quaterion `q`.                             */
  static SIMD_CPPFUNC quatd exp(const ::simd_quatd q) { return ::__tg_exp(q); }
  
  /*! @abstract Spherical linear interpolation along the shortest arc between
   *  quaternions `q0` and `q1`.                                              */
  static SIMD_CPPFUNC quatd slerp(const ::simd_quatd p0, const ::simd_quatd p1, double t) { return ::simd_slerp(p0, p1, t); }
  
  /*! @abstract Spherical linear interpolation along the longest arc between
   *  quaternions `q0` and `q1`.                                              */
  static SIMD_CPPFUNC quatd slerp_longest(const ::simd_quatd p0, const ::simd_quatd p1, double t) { return ::simd_slerp_longest(p0, p1, t); }
  
  /*! @abstract Interpolate between quaternions along a spherical cubic spline.
   *
   *  @discussion The function interpolates between q1 and q2. q0 is the left
   *  endpoint of the previous interval, and q3 is the right endpoint of the next
   *  interval. Use this function to smoothly interpolate between a sequence of
   *  rotations.                                                              */
  static SIMD_CPPFUNC quatd spline(const ::simd_quatd p0, const ::simd_quatd p1, const ::simd_quatd p2, const ::simd_quatd p3, double t) { return ::simd_spline(p0, p1, p2, p3, t); }
  
  /*! @abstract Spherical cubic Bezier interpolation between quaternions.
   *
   *  @discussion The function treats q0 ... q3 as control points and uses slerp
   *  in place of lerp in the De Castlejeau algorithm. The endpoints of
   *  interpolation are thus q0 and q3, and the curve will not generally pass
   *  through q1 or q2. Note that the convex hull property of "standard" Bezier
   *  curve does not hold on the sphere.                                      */
  static SIMD_CPPFUNC quatd bezier(const ::simd_quatd p0, const ::simd_quatd p1, const ::simd_quatd p2, const ::simd_quatd p3, double t) { return ::simd_bezier(p0, p1, p2, p3, t); }
}

extern "C" {
#endif /* __cplusplus */
  
/*  MARK: - double implementations                                             */

#include <simd/math.h>
#include <simd/geometry.h>
  
/*  tg_promote is implementation gobbledygook that enables the compile-time
 *  dispatching in tgmath.h to work its magic.                                */
static simd_quatd __attribute__((__overloadable__)) __tg_promote(simd_quatd);
  
/*! @abstract Constructs a quaternion from imaginary and real parts.
 *  @discussion This function is hidden behind an underscore to avoid confusion
 *  with the angle-axis constructor.                                          */
static inline SIMD_CFUNC simd_quatd _simd_quaternion(simd_double3 imag, double real) {
  return simd_quaternion(simd_make_double4(imag, real));
}
  
static inline SIMD_CFUNC simd_quatd simd_quaternion(double angle, simd_double3 axis) {
  return _simd_quaternion(sin(angle/2) * axis, cos(angle/2));
}
  
static inline SIMD_CFUNC double simd_angle(simd_quatd q) {
  return 2*atan2(simd_length(q.vector.xyz), q.vector.w);
}
  
static inline SIMD_CFUNC simd_double3 simd_axis(simd_quatd q) {
  return simd_normalize(q.vector.xyz);
}
  
static inline SIMD_CFUNC simd_quatd simd_add(simd_quatd p, simd_quatd q) {
  return simd_quaternion(p.vector + q.vector);
}
  
static inline SIMD_CFUNC simd_quatd simd_sub(simd_quatd p, simd_quatd q) {
  return simd_quaternion(p.vector - q.vector);
}

static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd p, simd_quatd q) {
  #pragma STDC FP_CONTRACT ON
  return simd_quaternion((p.vector.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
                          p.vector.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5)) +
                         (p.vector.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6) +
                          p.vector.w * q.vector));
}

static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd q, double a) {
  return simd_quaternion(a * q.vector);
}

static inline SIMD_CFUNC simd_quatd simd_mul(double a, simd_quatd q) {
  return simd_mul(q,a);
}
  
static inline SIMD_CFUNC simd_quatd simd_conjugate(simd_quatd q) {
  return simd_quaternion(q.vector * (simd_double4){-1,-1,-1, 1});
}
  
static inline SIMD_CFUNC simd_quatd simd_inverse(simd_quatd q) {
  return simd_quaternion(simd_conjugate(q).vector * simd_recip(simd_length_squared(q.vector)));
}
  
static inline SIMD_CFUNC simd_quatd simd_negate(simd_quatd q) {
  return simd_quaternion(-q.vector);
}
  
static inline SIMD_CFUNC double simd_dot(simd_quatd p, simd_quatd q) {
  return simd_dot(p.vector, q.vector);
}
  
static inline SIMD_CFUNC double simd_length(simd_quatd q) {
  return simd_length(q.vector);
}
  
static inline SIMD_CFUNC simd_quatd simd_normalize(simd_quatd q) {
  double length_squared = simd_length_squared(q.vector);
  if (length_squared == 0) {
    return simd_quaternion((simd_double4){0,0,0,1});
  }
  return simd_quaternion(q.vector * simd_rsqrt(length_squared));
}

#if defined __arm__ || defined __arm64__
/*! @abstract Multiplies the vector `v` by the quaternion `q`.
 *  
 *  @discussion This IS NOT the action of `q` on `v` (i.e. this is not rotation
 *  by `q`. That operation is provided by `simd_act(q, v)`. This function is an
 *  implementation detail and you should not call it directly. It may be
 *  removed or modified in future versions of the simd module.                */
static inline SIMD_CFUNC simd_quatd _simd_mul_vq(simd_double3 v, simd_quatd q) {
  #pragma STDC FP_CONTRACT ON
  return simd_quaternion(v.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
                         v.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5) +
                         v.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6));
}
#endif
  
static inline SIMD_CFUNC simd_double3 simd_act(simd_quatd q, simd_double3 v) {
#if defined __arm__ || defined __arm64__
  return simd_mul(q, _simd_mul_vq(v, simd_conjugate(q))).vector.xyz;
#else
  #pragma STDC FP_CONTRACT ON
  simd_double3 t = 2*simd_cross(simd_imag(q),v);
  return v + simd_real(q)*t + simd_cross(simd_imag(q), t);
#endif
}

static SIMD_NOINLINE simd_quatd __tg_log(simd_quatd q) {
  double real = __tg_log(simd_length_squared(q.vector))/2;
  if (simd_equal(simd_imag(q), 0)) return _simd_quaternion(0, real);
  simd_double3 imag = __tg_acos(simd_real(q)/simd_length(q)) * simd_normalize(simd_imag(q));
  return _simd_quaternion(imag, real);
}
  
static SIMD_NOINLINE simd_quatd __tg_exp(simd_quatd q) {
  //  angle is actually *twice* the angle of the rotation corresponding to
  //  the resulting quaternion, which is why we don't simply use the (angle,
  //  axis) constructor to generate `unit`.
  double angle = simd_length(simd_imag(q));
  if (angle == 0) return _simd_quaternion(0, exp(simd_real(q)));
  simd_double3 axis = simd_normalize(simd_imag(q));
  simd_quatd unit = _simd_quaternion(sin(angle)*axis, cosf(angle));
  return simd_mul(exp(simd_real(q)), unit);
}
 
/*! @abstract Implementation detail of the `simd_quaternion(from, to)`
 *  initializer.
 *
 *  @discussion Computes the quaternion rotation `from` to `to` if they are
 *  separated by less than 90 degrees. Not numerically stable for larger
 *  angles. This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static inline SIMD_CFUNC simd_quatd _simd_quaternion_reduced(simd_double3 from, simd_double3 to) {
  simd_double3 half = simd_normalize(from + to);
  return _simd_quaternion(simd_cross(from, half), simd_dot(from, half));
}

static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double3 from, simd_double3 to) {
  
  //  If the angle between from and to is not too big, we can compute the
  //  rotation accurately using a simple implementation.
  if (simd_dot(from, to) >= 0) {
    return _simd_quaternion_reduced(from, to);
  }
  
  //  Because from and to are more than 90 degrees apart, we compute the
  //  rotation in two stages (from -> half), (half -> to) to preserve numerical
  //  accuracy.
  simd_double3 half = simd_normalize(from) + simd_normalize(to);
  
  if (simd_length_squared(half) <= 0x1p-104) {
    //  half is nearly zero, so from and to point in nearly opposite directions
    //  and the rotation is numerically underspecified. Pick an axis orthogonal
    //  to the vectors, and use an angle of pi radians.
    simd_double3 abs_from = simd_abs(from);
    if (abs_from.x <= abs_from.y && abs_from.x <= abs_from.z)
      return _simd_quaternion(simd_normalize(simd_cross(from, (simd_double3){1,0,0})), 0.f);
    else if (abs_from.y <= abs_from.z)
      return _simd_quaternion(simd_normalize(simd_cross(from, (simd_double3){0,1,0})), 0.f);
    else
      return _simd_quaternion(simd_normalize(simd_cross(from, (simd_double3){0,0,1})), 0.f);
  }

  //  Compute the two-step rotation.                         */
  half = simd_normalize(half);
  return simd_mul(_simd_quaternion_reduced(from, half),
                  _simd_quaternion_reduced(half, to));
}

static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double3x3 matrix) {
  const simd_double3 *mat = matrix.columns;
  double trace = mat[0][0] + mat[1][1] + mat[2][2];
  if (trace >= 0.0) {
    double r = 2*sqrt(1 + trace);
    double rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
                           rinv*(mat[2][0] - mat[0][2]),
                           rinv*(mat[0][1] - mat[1][0]),
                           r/4);
  } else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
    double r = 2*sqrt(1 - mat[1][1] - mat[2][2] + mat[0][0]);
    double rinv = simd_recip(r);
    return simd_quaternion(r/4,
                           rinv*(mat[0][1] + mat[1][0]),
                           rinv*(mat[0][2] + mat[2][0]),
                           rinv*(mat[1][2] - mat[2][1]));
  } else if (mat[1][1] >= mat[2][2]) {
    double r = 2*sqrt(1 - mat[0][0] - mat[2][2] + mat[1][1]);
    double rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
                           r/4,
                           rinv*(mat[1][2] + mat[2][1]),
                           rinv*(mat[2][0] - mat[0][2]));
  } else {
    double r = 2*sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
    double rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
                           rinv*(mat[1][2] + mat[2][1]),
                           r/4,
                           rinv*(mat[0][1] - mat[1][0]));
  }
}
  
static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double4x4 matrix) {
  const simd_double4 *mat = matrix.columns;
  double trace = mat[0][0] + mat[1][1] + mat[2][2];
  if (trace >= 0.0) {
    double r = 2*sqrt(1 + trace);
    double rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
                           rinv*(mat[2][0] - mat[0][2]),
                           rinv*(mat[0][1] - mat[1][0]),
                           r/4);
  } else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
    double r = 2*sqrt(1 - mat[1][1] - mat[2][2] + mat[0][0]);
    double rinv = simd_recip(r);
    return simd_quaternion(r/4,
                           rinv*(mat[0][1] + mat[1][0]),
                           rinv*(mat[0][2] + mat[2][0]),
                           rinv*(mat[1][2] - mat[2][1]));
  } else if (mat[1][1] >= mat[2][2]) {
    double r = 2*sqrt(1 - mat[0][0] - mat[2][2] + mat[1][1]);
    double rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
                           r/4,
                           rinv*(mat[1][2] + mat[2][1]),
                           rinv*(mat[2][0] - mat[0][2]));
  } else {
    double r = 2*sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
    double rinv = simd_recip(r);
    return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
                           rinv*(mat[1][2] + mat[2][1]),
                           r/4,
                           rinv*(mat[0][1] - mat[1][0]));
  }
}
  
/*! @abstract The angle between p and q interpreted as 4-dimensional vectors.
 *
 *  @discussion This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static SIMD_NOINLINE double _simd_angle(simd_quatd p, simd_quatd q) {
  return 2*atan2(simd_length(p.vector - q.vector), simd_length(p.vector + q.vector));
}
  
/*! @abstract sin(x)/x.
 *
 *  @discussion This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static SIMD_CFUNC double _simd_sinc(double x) {
  if (x == 0) return 1;
  return sin(x)/x;
}
 
/*! @abstract Spherical lerp between q0 and q1.
 *
 *  @discussion This function may interpolate along either the longer or
 *  shorter path between q0 and q1; it is used as an implementation detail
 *  in `simd_slerp` and `simd_slerp_longest`; you should use those functions
 *  instead of calling this directly.                                         */
static SIMD_NOINLINE simd_quatd _simd_slerp_internal(simd_quatd q0, simd_quatd q1, double t) {
  double s = 1 - t;
  double a = _simd_angle(q0, q1);
  double r = simd_recip(_simd_sinc(a));
  return simd_normalize(simd_quaternion(_simd_sinc(s*a)*r*s*q0.vector + _simd_sinc(t*a)*r*t*q1.vector));
}
  
static SIMD_NOINLINE simd_quatd simd_slerp(simd_quatd q0, simd_quatd q1, double t) {
  if (simd_dot(q0, q1) >= 0)
    return _simd_slerp_internal(q0, q1, t);
  return _simd_slerp_internal(q0, simd_negate(q1), t);
}

static SIMD_NOINLINE simd_quatd simd_slerp_longest(simd_quatd q0, simd_quatd q1, double t) {
  if (simd_dot(q0, q1) >= 0)
    return _simd_slerp_internal(q0, simd_negate(q1), t);
  return _simd_slerp_internal(q0, q1, t);
}
  
/*! @discussion This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static SIMD_NOINLINE simd_quatd _simd_intermediate(simd_quatd q0, simd_quatd q1, simd_quatd q2) {
  simd_quatd p0 = __tg_log(simd_mul(q0, simd_inverse(q1)));
  simd_quatd p2 = __tg_log(simd_mul(q2, simd_inverse(q1)));
  return simd_normalize(simd_mul(q1, __tg_exp(simd_mul(-0.25, simd_add(p0,p2)))));
}

/*! @discussion This function is an implementation detail and you should not
 *  call it directly. It may be removed or modified in future versions of the
 *  simd module.                                                              */
static SIMD_NOINLINE simd_quatd _simd_squad(simd_quatd q0, simd_quatd qa, simd_quatd qb, simd_quatd q1, double t) {
  simd_quatd r0 = _simd_slerp_internal(q0, q1, t);
  simd_quatd r1 = _simd_slerp_internal(qa, qb, t);
  return _simd_slerp_internal(r0, r1, 2*t*(1 - t));
}
  
static SIMD_NOINLINE simd_quatd simd_spline(simd_quatd q0, simd_quatd q1, simd_quatd q2, simd_quatd q3, double t) {
  simd_quatd qa = _simd_intermediate(q0, q1, q2);
  simd_quatd qb = _simd_intermediate(q1, q2, q3);
  return _simd_squad(q1, qa, qb, q2, t);
}
  
static SIMD_NOINLINE simd_quatd simd_bezier(simd_quatd q0, simd_quatd q1, simd_quatd q2, simd_quatd q3, double t) {
  simd_quatd q01 = _simd_slerp_internal(q0, q1, t);
  simd_quatd q12 = _simd_slerp_internal(q1, q2, t);
  simd_quatd q23 = _simd_slerp_internal(q2, q3, t);
  simd_quatd q012 = _simd_slerp_internal(q01, q12, t);
  simd_quatd q123 = _simd_slerp_internal(q12, q23, t);
  return _simd_slerp_internal(q012, q123, t);
}

#ifdef __cplusplus
}      /* extern "C"  */
#endif /* __cplusplus */
#endif /* SIMD_COMPILER_HAS_REQUIRED_FEATURES */
#endif /* SIMD_QUATERNIONS */