Meg's Mirrors

zig

1	/*! @header
2	* This header defines functions for constructing and using quaternions.
3	* @copyright 2015-2016 Apple, Inc. All rights reserved.
4	* @unsorted */
5
6	#ifndef SIMD_QUATERNIONS
7	#define SIMD_QUATERNIONS
8
9	#include <simd/base.h>
10	#if SIMD_COMPILER_HAS_REQUIRED_FEATURES
11	#include <simd/vector.h>
12	#include <simd/types.h>
13
14	#ifdef __cplusplus
15	extern "C" {
16	#endif
17
18	/* MARK: - C and Objective-C _Float16 interfaces */
19
20	/*! @abstract Constructs a quaternion from four scalar values.
21	*
22	* @param ix The first component of the imaginary (vector) part.
23	* @param iy The second component of the imaginary (vector) part.
24	* @param iz The third component of the imaginary (vector) part.
25	*
26	* @param r The real (scalar) part. */
27	static inline SIMD_CFUNC simd_quath simd_quaternion(_Float16 ix, _Float16 iy, _Float16 iz, _Float16 r) {
28	return (simd_quath){ { ix, iy, iz, r } };
29	}
30
31	/*! @abstract Constructs a quaternion from an array of four scalars.
32	*
33	* @discussion Note that the imaginary part of the quaternion comes from
34	* array elements 0, 1, and 2, and the real part comes from element 3. */
35	static inline SIMD_NONCONST simd_quath simd_quaternion(const _Float16 xyzr[4]) {
36	return (simd_quath){ *(const simd_packed_half4 *)xyzr };
37	}
38
39	/*! @abstract Constructs a quaternion from a four-element vector.
40	*
41	* @discussion Note that the imaginary (vector) part of the quaternion comes
42	* from lanes 0, 1, and 2 of the vector, and the real (scalar) part comes from
43	* lane 3. */
44	static inline SIMD_CFUNC simd_quath simd_quaternion(simd_half4 xyzr) {
45	return (simd_quath){ xyzr };
46	}
47
48	/*! @abstract Constructs a quaternion that rotates by `angle` radians about
49	* `axis`. */
50	static inline SIMD_CFUNC simd_quath simd_quaternion(_Float16 angle, simd_half3 axis);
51
52	/*! @abstract Construct a quaternion that rotates from one vector to another.
53	*
54	* @param from A normalized three-element vector.
55	* @param to A normalized three-element vector.
56	*
57	* @discussion The rotation axis is `simd_cross(from, to)`. If `from` and
58	* `to` point in opposite directions (to within machine precision), an
59	* arbitrary rotation axis is chosen, and the angle is pi radians. */
60	static SIMD_NOINLINE simd_quath simd_quaternion(simd_half3 from, simd_half3 to);
61
62	/*! @abstract Construct a quaternion from a 3x3 rotation `matrix`.
63	*
64	* @discussion If `matrix` is not orthogonal with determinant 1, the result
65	* is undefined. */
66	static SIMD_NOINLINE simd_quath simd_quaternion(simd_half3x3 matrix);
67
68	/*! @abstract Construct a quaternion from a 4x4 rotation `matrix`.
69	*
70	* @discussion The last row and column of the matrix are ignored. This
71	* function is equivalent to calling simd_quaternion with the upper-left 3x3
72	* submatrix . */
73	static SIMD_NOINLINE simd_quath simd_quaternion(simd_half4x4 matrix);
74
75	/*! @abstract The real (scalar) part of the quaternion `q`. */
76	static inline SIMD_CFUNC _Float16 simd_real(simd_quath q) {
77	return q.vector.w;
78	}
79
80	/*! @abstract The imaginary (vector) part of the quaternion `q`. */
81	static inline SIMD_CFUNC simd_half3 simd_imag(simd_quath q) {
82	return q.vector.xyz;
83	}
84
85	/*! @abstract The angle (in radians) of rotation represented by `q`. */
86	static inline SIMD_CFUNC _Float16 simd_angle(simd_quath q);
87
88	/*! @abstract The normalized axis (a 3-element vector) around which the
89	* action of the quaternion `q` rotates. */
90	static inline SIMD_CFUNC simd_half3 simd_axis(simd_quath q);
91
92	/*! @abstract The sum of the quaternions `p` and `q`. */
93	static inline SIMD_CFUNC simd_quath simd_add(simd_quath p, simd_quath q);
94
95	/*! @abstract The difference of the quaternions `p` and `q`. */
96	static inline SIMD_CFUNC simd_quath simd_sub(simd_quath p, simd_quath q);
97
98	/*! @abstract The product of the quaternions `p` and `q`. */
99	static inline SIMD_CFUNC simd_quath simd_mul(simd_quath p, simd_quath q);
100
101	/*! @abstract The quaternion `q` scaled by the real value `a`. */
102	static inline SIMD_CFUNC simd_quath simd_mul(simd_quath q, _Float16 a);
103
104	/*! @abstract The quaternion `q` scaled by the real value `a`. */
105	static inline SIMD_CFUNC simd_quath simd_mul(_Float16 a, simd_quath q);
106
107	/*! @abstract The conjugate of the quaternion `q`. */
108	static inline SIMD_CFUNC simd_quath simd_conjugate(simd_quath q);
109
110	/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
111	static inline SIMD_CFUNC simd_quath simd_inverse(simd_quath q);
112
113	/*! @abstract The negation (additive inverse) of the quaternion `q`. */
114	static inline SIMD_CFUNC simd_quath simd_negate(simd_quath q);
115
116	/*! @abstract The dot product of the quaternions `p` and `q` interpreted as
117	* four-dimensional vectors. */
118	static inline SIMD_CFUNC _Float16 simd_dot(simd_quath p, simd_quath q);
119
120	/*! @abstract The length of the quaternion `q`. */
121	static inline SIMD_CFUNC _Float16 simd_length(simd_quath q);
122
123	/*! @abstract The unit quaternion obtained by normalizing `q`. */
124	static inline SIMD_CFUNC simd_quath simd_normalize(simd_quath q);
125
126	/*! @abstract Rotates the vector `v` by the quaternion `q`. */
127	static inline SIMD_CFUNC simd_half3 simd_act(simd_quath q, simd_half3 v);
128
129	/*! @abstract Logarithm of the quaternion `q`.
130	* @discussion Do not call this function directly; use `log(q)` instead.
131	*
132	* We can write a quaternion `q` in the form: `r(cos(t) + sin(t)v)` where
133	* `r` is the length of `q`, `t` is an angle, and `v` is a unit 3-vector.
134	* The logarithm of `q` is `log(r) + tv`, just like the logarithm of the
135	* complex number `r*(cos(t) + i sin(t))` is `log(r) + it`.
136	*
137	* Note that this function is not robust against poorly-scaled non-unit
138	* quaternions, because it is primarily used for spline interpolation of
139	* unit quaternions. If you need to compute a robust logarithm of general
140	* quaternions, you can use the following approach:
141	*
142	* scale = simd_reduce_max(simd_abs(q.vector));
143	* logq = log(simd_recip(scale)*q);
144	* logq.real += log(scale);
145	* return logq; */
146	static SIMD_NOINLINE simd_quath __tg_log(simd_quath q);
147
148	/*! @abstract Inverse of `log( )`; the exponential map on quaternions.
149	* @discussion Do not call this function directly; use `exp(q)` instead. */
150	static SIMD_NOINLINE simd_quath __tg_exp(simd_quath q);
151
152	/*! @abstract Spherical linear interpolation along the shortest arc between
153	* quaternions `q0` and `q1`. */
154	static SIMD_NOINLINE simd_quath simd_slerp(simd_quath q0, simd_quath q1, _Float16 t);
155
156	/*! @abstract Spherical linear interpolation along the longest arc between
157	* quaternions `q0` and `q1`. */
158	static SIMD_NOINLINE simd_quath simd_slerp_longest(simd_quath q0, simd_quath q1, _Float16 t);
159
160	/*! @abstract Interpolate between quaternions along a spherical cubic spline.
161	*
162	* @discussion The function interpolates between q1 and q2. q0 is the left
163	* endpoint of the previous interval, and q3 is the right endpoint of the next
164	* interval. Use this function to smoothly interpolate between a sequence of
165	* rotations. */
166	static SIMD_NOINLINE simd_quath simd_spline(simd_quath q0, simd_quath q1, simd_quath q2, simd_quath q3, _Float16 t);
167
168	/*! @abstract Spherical cubic Bezier interpolation between quaternions.
169	*
170	* @discussion The function treats q0 ... q3 as control points and uses slerp
171	* in place of lerp in the De Castlejeau algorithm. The endpoints of
172	* interpolation are thus q0 and q3, and the curve will not generally pass
173	* through q1 or q2. Note that the convex hull property of "standard" Bezier
174	* curve does not hold on the sphere. */
175	static SIMD_NOINLINE simd_quath simd_bezier(simd_quath q0, simd_quath q1, simd_quath q2, simd_quath q3, _Float16 t);
176
177	#ifdef __cplusplus
178	} /* extern "C" */
179	/* MARK: - C++ _Float16 interfaces */
180
181	namespace simd {
182	struct quath : ::simd_quath {
183	/*! @abstract The identity quaternion. */
184	quath( ) : ::simd_quath(::simd_quaternion((half4){0,0,0,1})) { }
185
186	/*! @abstract Constructs a C++ quaternion from a C quaternion. */
187	quath(::simd_quath q) : ::simd_quath(q) { }
188
189	/*! @abstract Constructs a quaternion from components. */
190	quath(_Float16 ix, _Float16 iy, _Float16 iz, _Float16 r) : ::simd_quath(::simd_quaternion(ix, iy, iz, r)) { }
191
192	/*! @abstract Constructs a quaternion from an array of scalars. */
193	quath(const _Float16 xyzr[4]) : ::simd_quath(::simd_quaternion(xyzr)) { }
194
195	/*! @abstract Constructs a quaternion from a vector. */
196	quath(half4 xyzr) : ::simd_quath(::simd_quaternion(xyzr)) { }
197
198	/*! @abstract Quaternion representing rotation about `axis` by `angle`
199	* radians. */
200	quath(_Float16 angle, half3 axis) : ::simd_quath(::simd_quaternion(angle, axis)) { }
201
202	/*! @abstract Quaternion that rotates `from` into `to`. */
203	quath(half3 from, half3 to) : ::simd_quath(::simd_quaternion(from, to)) { }
204
205	/*! @abstract Constructs a quaternion from a rotation matrix. */
206	quath(::simd_half3x3 matrix) : ::simd_quath(::simd_quaternion(matrix)) { }
207
208	/*! @abstract Constructs a quaternion from a rotation matrix. */
209	quath(::simd_half4x4 matrix) : ::simd_quath(::simd_quaternion(matrix)) { }
210
211	/*! @abstract The real (scalar) part of the quaternion. */
212	_Float16 real(void) const { return ::simd_real(*this); }
213
214	/*! @abstract The imaginary (vector) part of the quaternion. */
215	half3 imag(void) const { return ::simd_imag(*this); }
216
217	/*! @abstract The angle the quaternion rotates by. */
218	_Float16 angle(void) const { return ::simd_angle(*this); }
219
220	/*! @abstract The axis the quaternion rotates about. */
221	half3 axis(void) const { return ::simd_axis(*this); }
222
223	/*! @abstract The length of the quaternion. */
224	_Float16 length(void) const { return ::simd_length(*this); }
225
226	/*! @abstract Act on the vector `v` by rotation. */
227	half3 operator()(const ::simd_half3 v) const { return ::simd_act(*this, v); }
228	};
229
230	static SIMD_CPPFUNC quath operator+(const ::simd_quath p, const ::simd_quath q) { return ::simd_add(p, q); }
231	static SIMD_CPPFUNC quath operator-(const ::simd_quath p, const ::simd_quath q) { return ::simd_sub(p, q); }
232	static SIMD_CPPFUNC quath operator-(const ::simd_quath p) { return ::simd_negate(p); }
233	static SIMD_CPPFUNC quath operator*(const _Float16 r, const ::simd_quath p) { return ::simd_mul(r, p); }
234	static SIMD_CPPFUNC quath operator*(const ::simd_quath p, const _Float16 r) { return ::simd_mul(p, r); }
235	static SIMD_CPPFUNC quath operator*(const ::simd_quath p, const ::simd_quath q) { return ::simd_mul(p, q); }
236	static SIMD_CPPFUNC quath operator/(const ::simd_quath p, const ::simd_quath q) { return ::simd_mul(p, ::simd_inverse(q)); }
237	static SIMD_INLINE SIMD_NODEBUG quath operator+=(quath &p, const ::simd_quath q) { return p = p+q; }
238	static SIMD_INLINE SIMD_NODEBUG quath operator-=(quath &p, const ::simd_quath q) { return p = p-q; }
239	static SIMD_INLINE SIMD_NODEBUG quath operator*=(quath &p, const _Float16 r) { return p = p*r; }
240	static SIMD_INLINE SIMD_NODEBUG quath operator*=(quath &p, const ::simd_quath q) { return p = p*q; }
241	static SIMD_INLINE SIMD_NODEBUG quath operator/=(quath &p, const ::simd_quath q) { return p = p/q; }
242
243	/*! @abstract The conjugate of the quaternion `q`. */
244	static SIMD_CPPFUNC quath conjugate(const ::simd_quath p) { return ::simd_conjugate(p); }
245
246	/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
247	static SIMD_CPPFUNC quath inverse(const ::simd_quath p) { return ::simd_inverse(p); }
248
249	/*! @abstract The dot product of the quaternions `p` and `q` interpreted as
250	* four-dimensional vectors. */
251	static SIMD_CPPFUNC _Float16 dot(const ::simd_quath p, const ::simd_quath q) { return ::simd_dot(p, q); }
252
253	/*! @abstract The unit quaternion obtained by normalizing `q`. */
254	static SIMD_CPPFUNC quath normalize(const ::simd_quath p) { return ::simd_normalize(p); }
255
256	/*! @abstract logarithm of the quaternion `q`. */
257	static SIMD_CPPFUNC quath log(const ::simd_quath q) { return ::__tg_log(q); }
258
259	/*! @abstract exponential map of quaterion `q`. */
260	static SIMD_CPPFUNC quath exp(const ::simd_quath q) { return ::__tg_exp(q); }
261
262	/*! @abstract Spherical linear interpolation along the shortest arc between
263	* quaternions `q0` and `q1`. */
264	static SIMD_CPPFUNC quath slerp(const ::simd_quath p0, const ::simd_quath p1, _Float16 t) { return ::simd_slerp(p0, p1, t); }
265
266	/*! @abstract Spherical linear interpolation along the longest arc between
267	* quaternions `q0` and `q1`. */
268	static SIMD_CPPFUNC quath slerp_longest(const ::simd_quath p0, const ::simd_quath p1, _Float16 t) { return ::simd_slerp_longest(p0, p1, t); }
269
270	/*! @abstract Interpolate between quaternions along a spherical cubic spline.
271	*
272	* @discussion The function interpolates between q1 and q2. q0 is the left
273	* endpoint of the previous interval, and q3 is the right endpoint of the next
274	* interval. Use this function to smoothly interpolate between a sequence of
275	* rotations. */
276	static SIMD_CPPFUNC quath spline(const ::simd_quath p0, const ::simd_quath p1, const ::simd_quath p2, const ::simd_quath p3, _Float16 t) { return ::simd_spline(p0, p1, p2, p3, t); }
277
278	/*! @abstract Spherical cubic Bezier interpolation between quaternions.
279	*
280	* @discussion The function treats q0 ... q3 as control points and uses slerp
281	* in place of lerp in the De Castlejeau algorithm. The endpoints of
282	* interpolation are thus q0 and q3, and the curve will not generally pass
283	* through q1 or q2. Note that the convex hull property of "standard" Bezier
284	* curve does not hold on the sphere. */
285	static SIMD_CPPFUNC quath bezier(const ::simd_quath p0, const ::simd_quath p1, const ::simd_quath p2, const ::simd_quath p3, _Float16 t) { return ::simd_bezier(p0, p1, p2, p3, t); }
286	}
287
288	extern "C" {
289	#endif /* __cplusplus */
290
291	/* MARK: - _Float16 implementations */
292
293	#include <simd/math.h>
294	#include <simd/geometry.h>
295
296	/* tg_promote is implementation gobbledygook that enables the compile-time
297	* dispatching in tgmath.h to work its magic. */
298	static simd_quath __attribute__((__overloadable__)) __tg_promote(simd_quath);
299
300	/*! @abstract Constructs a quaternion from imaginary and real parts.
301	* @discussion This function is hidden behind an underscore to avoid confusion
302	* with the angle-axis constructor. */
303	static inline SIMD_CFUNC simd_quath _simd_quaternion(simd_half3 imag, _Float16 real) {
304	return simd_quaternion(simd_make_half4(imag, real));
305	}
306
307	static inline SIMD_CFUNC simd_quath simd_quaternion(_Float16 angle, simd_half3 axis) {
308	return _simd_quaternion((_Float16)sinf(angle/2) * axis, (_Float16)cosf(angle/2));
309	}
310
311	static inline SIMD_CFUNC _Float16 simd_angle(simd_quath q) {
312	return 2*(_Float16)atan2f(simd_length(q.vector.xyz), q.vector.w);
313	}
314
315	static inline SIMD_CFUNC simd_half3 simd_axis(simd_quath q) {
316	return simd_normalize(q.vector.xyz);
317	}
318
319	static inline SIMD_CFUNC simd_quath simd_add(simd_quath p, simd_quath q) {
320	return simd_quaternion(p.vector + q.vector);
321	}
322
323	static inline SIMD_CFUNC simd_quath simd_sub(simd_quath p, simd_quath q) {
324	return simd_quaternion(p.vector - q.vector);
325	}
326
327	static inline SIMD_CFUNC simd_quath simd_mul(simd_quath p, simd_quath q) {
328	#pragma STDC FP_CONTRACT ON
329	return simd_quaternion((p.vector.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
330	p.vector.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5)) +
331	(p.vector.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6) +
332	p.vector.w * q.vector));
333	}
334
335	static inline SIMD_CFUNC simd_quath simd_mul(simd_quath q, _Float16 a) {
336	return simd_quaternion(a * q.vector);
337	}
338
339	static inline SIMD_CFUNC simd_quath simd_mul(_Float16 a, simd_quath q) {
340	return simd_mul(q,a);
341	}
342
343	static inline SIMD_CFUNC simd_quath simd_conjugate(simd_quath q) {
344	return simd_quaternion(q.vector * (simd_half4){-1,-1,-1, 1});
345	}
346
347	static inline SIMD_CFUNC simd_quath simd_inverse(simd_quath q) {
348	return simd_quaternion(simd_conjugate(q).vector * simd_recip(simd_length_squared(q.vector)));
349	}
350
351	static inline SIMD_CFUNC simd_quath simd_negate(simd_quath q) {
352	return simd_quaternion(-q.vector);
353	}
354
355	static inline SIMD_CFUNC _Float16 simd_dot(simd_quath p, simd_quath q) {
356	return simd_dot(p.vector, q.vector);
357	}
358
359	static inline SIMD_CFUNC _Float16 simd_length(simd_quath q) {
360	return simd_length(q.vector);
361	}
362
363	static inline SIMD_CFUNC simd_quath simd_normalize(simd_quath q) {
364	_Float16 length_squared = simd_length_squared(q.vector);
365	if (length_squared == 0) {
366	return simd_quaternion((simd_half4){0,0,0,1});
367	}
368	return simd_quaternion(q.vector * simd_rsqrt(length_squared));
369	}
370
371	#if defined __arm__ || defined __arm64__ || defined __aarch64__
372	/*! @abstract Multiplies the vector `v` by the quaternion `q`.
373	*
374	* @discussion This IS NOT the action of `q` on `v` (i.e. this is not rotation
375	* by `q`. That operation is provided by `simd_act(q, v)`. This function is an
376	* implementation detail and you should not call it directly. It may be
377	* removed or modified in future versions of the simd module. */
378	static inline SIMD_CFUNC simd_quath _simd_mul_vq(simd_half3 v, simd_quath q) {
379	#pragma STDC FP_CONTRACT ON
380	return simd_quaternion(v.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
381	v.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5) +
382	v.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6));
383	}
384	#endif
385
386	static inline SIMD_CFUNC simd_half3 simd_act(simd_quath q, simd_half3 v) {
387	#if defined __arm__ || defined __arm64__ || defined __aarch64__
388	return simd_mul(q, _simd_mul_vq(v, simd_conjugate(q))).vector.xyz;
389	#else
390	#pragma STDC FP_CONTRACT ON
391	simd_half3 t = 2*simd_cross(simd_imag(q),v);
392	return v + simd_real(q)*t + simd_cross(simd_imag(q), t);
393	#endif
394	}
395
396	static SIMD_NOINLINE simd_quath __tg_log(simd_quath q) {
397	_Float16 real = (_Float16)logf(simd_length_squared(q.vector))/2;
398	if (simd_equal(simd_imag(q), 0)) return _simd_quaternion(0, real);
399	simd_half3 imag = (_Float16)acosf(simd_real(q)/simd_length(q)) * simd_normalize(simd_imag(q));
400	return _simd_quaternion(imag, real);
401	}
402
403	static SIMD_NOINLINE simd_quath __tg_exp(simd_quath q) {
404	// angle is actually *twice* the angle of the rotation corresponding to
405	// the resulting quaternion, which is why we don't simply use the (angle,
406	// axis) constructor to generate `unit`.
407	_Float16 angle = simd_length(simd_imag(q));
408	if (angle == 0) return _simd_quaternion((simd_half3)0, (_Float16)expf(simd_real(q)));
409	simd_half3 axis = simd_normalize(simd_imag(q));
410	simd_quath unit = _simd_quaternion((_Float16)sinf(angle)*axis, (_Float16)cosf(angle));
411	return simd_mul((_Float16)expf(simd_real(q)), unit);
412	}
413
414	/*! @abstract Implementation detail of the `simd_quaternion(from, to)`
415	* initializer.
416	*
417	* @discussion Computes the quaternion rotation `from` to `to` if they are
418	* separated by less than 90 degrees. Not numerically stable for larger
419	* angles. This function is an implementation detail and you should not
420	* call it directly. It may be removed or modified in future versions of the
421	* simd module. */
422	static inline SIMD_CFUNC simd_quath _simd_quaternion_reduced(simd_half3 from, simd_half3 to) {
423	simd_half3 half = simd_normalize(from + to);
424	return _simd_quaternion(simd_cross(from, half), simd_dot(from, half));
425	}
426
427	static SIMD_NOINLINE simd_quath simd_quaternion(simd_half3 from, simd_half3 to) {
428
429	// If the angle between from and to is not too big, we can compute the
430	// rotation accurately using a simple implementation.
431	if (simd_dot(from, to) >= 0) {
432	return _simd_quaternion_reduced(from, to);
433	}
434
435	// Because from and to are more than 90 degrees apart, we compute the
436	// rotation in two stages (from -> half), (half -> to) to preserve numerical
437	// accuracy.
438	simd_half3 half = simd_normalize(from) + simd_normalize(to);
439
440	if (simd_length_squared(half) <= 0x1p-46f) {
441	// half is nearly zero, so from and to point in nearly opposite directions
442	// and the rotation is numerically underspecified. Pick an axis orthogonal
443	// to the vectors, and use an angle of pi radians.
444	simd_half3 abs_from = simd_abs(from);
445	if (abs_from.x <= abs_from.y && abs_from.x <= abs_from.z)
446	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_half3){1,0,0})), 0.f);
447	else if (abs_from.y <= abs_from.z)
448	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_half3){0,1,0})), 0.f);
449	else
450	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_half3){0,0,1})), 0.f);
451	}
452
453	// Compute the two-step rotation. */
454	half = simd_normalize(half);
455	return simd_mul(_simd_quaternion_reduced(from, half),
456	_simd_quaternion_reduced(half, to));
457	}
458
459	static SIMD_NOINLINE simd_quath simd_quaternion(simd_half3x3 matrix) {
460	const simd_half3 *mat = matrix.columns;
461	_Float16 trace = mat[0][0] + mat[1][1] + mat[2][2];
462	if (trace >= 0.0) {
463	_Float16 r = 2*__sqrtf16(1 + trace);
464	_Float16 rinv = simd_recip(r);
465	return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
466	rinv*(mat[2][0] - mat[0][2]),
467	rinv*(mat[0][1] - mat[1][0]),
468	r/4);
469	} else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
470	_Float16 r = 2*__sqrtf16(1 - mat[1][1] - mat[2][2] + mat[0][0]);
471	_Float16 rinv = simd_recip(r);
472	return simd_quaternion(r/4,
473	rinv*(mat[0][1] + mat[1][0]),
474	rinv*(mat[0][2] + mat[2][0]),
475	rinv*(mat[1][2] - mat[2][1]));
476	} else if (mat[1][1] >= mat[2][2]) {
477	_Float16 r = 2*__sqrtf16(1 - mat[0][0] - mat[2][2] + mat[1][1]);
478	_Float16 rinv = simd_recip(r);
479	return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
480	r/4,
481	rinv*(mat[1][2] + mat[2][1]),
482	rinv*(mat[2][0] - mat[0][2]));
483	} else {
484	_Float16 r = 2*__sqrtf16(1 - mat[0][0] - mat[1][1] + mat[2][2]);
485	_Float16 rinv = simd_recip(r);
486	return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
487	rinv*(mat[1][2] + mat[2][1]),
488	r/4,
489	rinv*(mat[0][1] - mat[1][0]));
490	}
491	}
492
493	static SIMD_NOINLINE simd_quath simd_quaternion(simd_half4x4 matrix) {
494	const simd_half4 *mat = matrix.columns;
495	_Float16 trace = mat[0][0] + mat[1][1] + mat[2][2];
496	if (trace >= 0.0) {
497	_Float16 r = 2*__sqrtf16(1 + trace);
498	_Float16 rinv = simd_recip(r);
499	return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
500	rinv*(mat[2][0] - mat[0][2]),
501	rinv*(mat[0][1] - mat[1][0]),
502	r/4);
503	} else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
504	_Float16 r = 2*__sqrtf16(1 - mat[1][1] - mat[2][2] + mat[0][0]);
505	_Float16 rinv = simd_recip(r);
506	return simd_quaternion(r/4,
507	rinv*(mat[0][1] + mat[1][0]),
508	rinv*(mat[0][2] + mat[2][0]),
509	rinv*(mat[1][2] - mat[2][1]));
510	} else if (mat[1][1] >= mat[2][2]) {
511	_Float16 r = 2*__sqrtf16(1 - mat[0][0] - mat[2][2] + mat[1][1]);
512	_Float16 rinv = simd_recip(r);
513	return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
514	r/4,
515	rinv*(mat[1][2] + mat[2][1]),
516	rinv*(mat[2][0] - mat[0][2]));
517	} else {
518	_Float16 r = 2*__sqrtf16(1 - mat[0][0] - mat[1][1] + mat[2][2]);
519	_Float16 rinv = simd_recip(r);
520	return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
521	rinv*(mat[1][2] + mat[2][1]),
522	r/4,
523	rinv*(mat[0][1] - mat[1][0]));
524	}
525	}
526
527	/*! @abstract The angle between p and q interpreted as 4-dimensional vectors.
528	*
529	* @discussion This function is an implementation detail and you should not
530	* call it directly. It may be removed or modified in future versions of the
531	* simd module. */
532	static SIMD_NOINLINE _Float16 _simd_angle(simd_quath p, simd_quath q) {
533	return 2*(_Float16)atan2f(simd_length(p.vector - q.vector), simd_length(p.vector + q.vector));
534	}
535
536	/*! @abstract sin(x)/x.
537	*
538	* @discussion This function is an implementation detail and you should not
539	* call it directly. It may be removed or modified in future versions of the
540	* simd module. */
541	static SIMD_CFUNC _Float16 _simd_sinc(_Float16 x) {
542	if (x == 0) return 1;
543	return (_Float16)sinf(x)/x;
544	}
545
546	/*! @abstract Spherical lerp between q0 and q1.
547	*
548	* @discussion This function may interpolate along either the longer or
549	* shorter path between q0 and q1; it is used as an implementation detail
550	* in `simd_slerp` and `simd_slerp_longest`; you should use those functions
551	* instead of calling this directly. */
552	static SIMD_NOINLINE simd_quath _simd_slerp_internal(simd_quath q0, simd_quath q1, _Float16 t) {
553	_Float16 s = 1 - t;
554	_Float16 a = _simd_angle(q0, q1);
555	_Float16 r = simd_recip(_simd_sinc(a));
556	return simd_normalize(simd_quaternion(_simd_sinc(s*a)*r*s*q0.vector + _simd_sinc(t*a)*r*t*q1.vector));
557	}
558
559	static SIMD_NOINLINE simd_quath simd_slerp(simd_quath q0, simd_quath q1, _Float16 t) {
560	if (simd_dot(q0, q1) >= 0)
561	return _simd_slerp_internal(q0, q1, t);
562	return _simd_slerp_internal(q0, simd_negate(q1), t);
563	}
564
565	static SIMD_NOINLINE simd_quath simd_slerp_longest(simd_quath q0, simd_quath q1, _Float16 t) {
566	if (simd_dot(q0, q1) >= 0)
567	return _simd_slerp_internal(q0, simd_negate(q1), t);
568	return _simd_slerp_internal(q0, q1, t);
569	}
570
571	/*! @discussion This function is an implementation detail and you should not
572	* call it directly. It may be removed or modified in future versions of the
573	* simd module. */
574	static SIMD_NOINLINE simd_quath _simd_intermediate(simd_quath q0, simd_quath q1, simd_quath q2) {
575	simd_quath p0 = __tg_log(simd_mul(q0, simd_inverse(q1)));
576	simd_quath p2 = __tg_log(simd_mul(q2, simd_inverse(q1)));
577	return simd_normalize(simd_mul(q1, __tg_exp(simd_mul(-0.25, simd_add(p0,p2)))));
578	}
579
580	/*! @discussion This function is an implementation detail and you should not
581	* call it directly. It may be removed or modified in future versions of the
582	* simd module. */
583	static SIMD_NOINLINE simd_quath _simd_squad(simd_quath q0, simd_quath qa, simd_quath qb, simd_quath q1, _Float16 t) {
584	simd_quath r0 = _simd_slerp_internal(q0, q1, t);
585	simd_quath r1 = _simd_slerp_internal(qa, qb, t);
586	return _simd_slerp_internal(r0, r1, 2*t*(1 - t));
587	}
588
589	static SIMD_NOINLINE simd_quath simd_spline(simd_quath q0, simd_quath q1, simd_quath q2, simd_quath q3, _Float16 t) {
590	simd_quath qa = _simd_intermediate(q0, q1, q2);
591	simd_quath qb = _simd_intermediate(q1, q2, q3);
592	return _simd_squad(q1, qa, qb, q2, t);
593	}
594
595	static SIMD_NOINLINE simd_quath simd_bezier(simd_quath q0, simd_quath q1, simd_quath q2, simd_quath q3, _Float16 t) {
596	simd_quath q01 = _simd_slerp_internal(q0, q1, t);
597	simd_quath q12 = _simd_slerp_internal(q1, q2, t);
598	simd_quath q23 = _simd_slerp_internal(q2, q3, t);
599	simd_quath q012 = _simd_slerp_internal(q01, q12, t);
600	simd_quath q123 = _simd_slerp_internal(q12, q23, t);
601	return _simd_slerp_internal(q012, q123, t);
602	}
603
604	/* MARK: - C and Objective-C float interfaces */
605
606	/*! @abstract Constructs a quaternion from four scalar values.
607	*
608	* @param ix The first component of the imaginary (vector) part.
609	* @param iy The second component of the imaginary (vector) part.
610	* @param iz The third component of the imaginary (vector) part.
611	*
612	* @param r The real (scalar) part. */
613	static inline SIMD_CFUNC simd_quatf simd_quaternion(float ix, float iy, float iz, float r) {
614	return (simd_quatf){ { ix, iy, iz, r } };
615	}
616
617	/*! @abstract Constructs a quaternion from an array of four scalars.
618	*
619	* @discussion Note that the imaginary part of the quaternion comes from
620	* array elements 0, 1, and 2, and the real part comes from element 3. */
621	static inline SIMD_NONCONST simd_quatf simd_quaternion(const float xyzr[4]) {
622	return (simd_quatf){ *(const simd_packed_float4 *)xyzr };
623	}
624
625	/*! @abstract Constructs a quaternion from a four-element vector.
626	*
627	* @discussion Note that the imaginary (vector) part of the quaternion comes
628	* from lanes 0, 1, and 2 of the vector, and the real (scalar) part comes from
629	* lane 3. */
630	static inline SIMD_CFUNC simd_quatf simd_quaternion(simd_float4 xyzr) {
631	return (simd_quatf){ xyzr };
632	}
633
634	/*! @abstract Constructs a quaternion that rotates by `angle` radians about
635	* `axis`. */
636	static inline SIMD_CFUNC simd_quatf simd_quaternion(float angle, simd_float3 axis);
637
638	/*! @abstract Construct a quaternion that rotates from one vector to another.
639	*
640	* @param from A normalized three-element vector.
641	* @param to A normalized three-element vector.
642	*
643	* @discussion The rotation axis is `simd_cross(from, to)`. If `from` and
644	* `to` point in opposite directions (to within machine precision), an
645	* arbitrary rotation axis is chosen, and the angle is pi radians. */
646	static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float3 from, simd_float3 to);
647
648	/*! @abstract Construct a quaternion from a 3x3 rotation `matrix`.
649	*
650	* @discussion If `matrix` is not orthogonal with determinant 1, the result
651	* is undefined. */
652	static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float3x3 matrix);
653
654	/*! @abstract Construct a quaternion from a 4x4 rotation `matrix`.
655	*
656	* @discussion The last row and column of the matrix are ignored. This
657	* function is equivalent to calling simd_quaternion with the upper-left 3x3
658	* submatrix . */
659	static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float4x4 matrix);
660
661	/*! @abstract The real (scalar) part of the quaternion `q`. */
662	static inline SIMD_CFUNC float simd_real(simd_quatf q) {
663	return q.vector.w;
664	}
665
666	/*! @abstract The imaginary (vector) part of the quaternion `q`. */
667	static inline SIMD_CFUNC simd_float3 simd_imag(simd_quatf q) {
668	return q.vector.xyz;
669	}
670
671	/*! @abstract The angle (in radians) of rotation represented by `q`. */
672	static inline SIMD_CFUNC float simd_angle(simd_quatf q);
673
674	/*! @abstract The normalized axis (a 3-element vector) around which the
675	* action of the quaternion `q` rotates. */
676	static inline SIMD_CFUNC simd_float3 simd_axis(simd_quatf q);
677
678	/*! @abstract The sum of the quaternions `p` and `q`. */
679	static inline SIMD_CFUNC simd_quatf simd_add(simd_quatf p, simd_quatf q);
680
681	/*! @abstract The difference of the quaternions `p` and `q`. */
682	static inline SIMD_CFUNC simd_quatf simd_sub(simd_quatf p, simd_quatf q);
683
684	/*! @abstract The product of the quaternions `p` and `q`. */
685	static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf p, simd_quatf q);
686
687	/*! @abstract The quaternion `q` scaled by the real value `a`. */
688	static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf q, float a);
689
690	/*! @abstract The quaternion `q` scaled by the real value `a`. */
691	static inline SIMD_CFUNC simd_quatf simd_mul(float a, simd_quatf q);
692
693	/*! @abstract The conjugate of the quaternion `q`. */
694	static inline SIMD_CFUNC simd_quatf simd_conjugate(simd_quatf q);
695
696	/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
697	static inline SIMD_CFUNC simd_quatf simd_inverse(simd_quatf q);
698
699	/*! @abstract The negation (additive inverse) of the quaternion `q`. */
700	static inline SIMD_CFUNC simd_quatf simd_negate(simd_quatf q);
701
702	/*! @abstract The dot product of the quaternions `p` and `q` interpreted as
703	* four-dimensional vectors. */
704	static inline SIMD_CFUNC float simd_dot(simd_quatf p, simd_quatf q);
705
706	/*! @abstract The length of the quaternion `q`. */
707	static inline SIMD_CFUNC float simd_length(simd_quatf q);
708
709	/*! @abstract The unit quaternion obtained by normalizing `q`. */
710	static inline SIMD_CFUNC simd_quatf simd_normalize(simd_quatf q);
711
712	/*! @abstract Rotates the vector `v` by the quaternion `q`. */
713	static inline SIMD_CFUNC simd_float3 simd_act(simd_quatf q, simd_float3 v);
714
715	/*! @abstract Logarithm of the quaternion `q`.
716	* @discussion Do not call this function directly; use `log(q)` instead.
717	*
718	* We can write a quaternion `q` in the form: `r(cos(t) + sin(t)v)` where
719	* `r` is the length of `q`, `t` is an angle, and `v` is a unit 3-vector.
720	* The logarithm of `q` is `log(r) + tv`, just like the logarithm of the
721	* complex number `r*(cos(t) + i sin(t))` is `log(r) + it`.
722	*
723	* Note that this function is not robust against poorly-scaled non-unit
724	* quaternions, because it is primarily used for spline interpolation of
725	* unit quaternions. If you need to compute a robust logarithm of general
726	* quaternions, you can use the following approach:
727	*
728	* scale = simd_reduce_max(simd_abs(q.vector));
729	* logq = log(simd_recip(scale)*q);
730	* logq.real += log(scale);
731	* return logq; */
732	static SIMD_NOINLINE simd_quatf __tg_log(simd_quatf q);
733
734	/*! @abstract Inverse of `log( )`; the exponential map on quaternions.
735	* @discussion Do not call this function directly; use `exp(q)` instead. */
736	static SIMD_NOINLINE simd_quatf __tg_exp(simd_quatf q);
737
738	/*! @abstract Spherical linear interpolation along the shortest arc between
739	* quaternions `q0` and `q1`. */
740	static SIMD_NOINLINE simd_quatf simd_slerp(simd_quatf q0, simd_quatf q1, float t);
741
742	/*! @abstract Spherical linear interpolation along the longest arc between
743	* quaternions `q0` and `q1`. */
744	static SIMD_NOINLINE simd_quatf simd_slerp_longest(simd_quatf q0, simd_quatf q1, float t);
745
746	/*! @abstract Interpolate between quaternions along a spherical cubic spline.
747	*
748	* @discussion The function interpolates between q1 and q2. q0 is the left
749	* endpoint of the previous interval, and q3 is the right endpoint of the next
750	* interval. Use this function to smoothly interpolate between a sequence of
751	* rotations. */
752	static SIMD_NOINLINE simd_quatf simd_spline(simd_quatf q0, simd_quatf q1, simd_quatf q2, simd_quatf q3, float t);
753
754	/*! @abstract Spherical cubic Bezier interpolation between quaternions.
755	*
756	* @discussion The function treats q0 ... q3 as control points and uses slerp
757	* in place of lerp in the De Castlejeau algorithm. The endpoints of
758	* interpolation are thus q0 and q3, and the curve will not generally pass
759	* through q1 or q2. Note that the convex hull property of "standard" Bezier
760	* curve does not hold on the sphere. */
761	static SIMD_NOINLINE simd_quatf simd_bezier(simd_quatf q0, simd_quatf q1, simd_quatf q2, simd_quatf q3, float t);
762
763	#ifdef __cplusplus
764	} /* extern "C" */
765	/* MARK: - C++ float interfaces */
766
767	namespace simd {
768	struct quatf : ::simd_quatf {
769	/*! @abstract The identity quaternion. */
770	quatf( ) : ::simd_quatf(::simd_quaternion((float4){0,0,0,1})) { }
771
772	/*! @abstract Constructs a C++ quaternion from a C quaternion. */
773	quatf(::simd_quatf q) : ::simd_quatf(q) { }
774
775	/*! @abstract Constructs a quaternion from components. */
776	quatf(float ix, float iy, float iz, float r) : ::simd_quatf(::simd_quaternion(ix, iy, iz, r)) { }
777
778	/*! @abstract Constructs a quaternion from an array of scalars. */
779	quatf(const float xyzr[4]) : ::simd_quatf(::simd_quaternion(xyzr)) { }
780
781	/*! @abstract Constructs a quaternion from a vector. */
782	quatf(float4 xyzr) : ::simd_quatf(::simd_quaternion(xyzr)) { }
783
784	/*! @abstract Quaternion representing rotation about `axis` by `angle`
785	* radians. */
786	quatf(float angle, float3 axis) : ::simd_quatf(::simd_quaternion(angle, axis)) { }
787
788	/*! @abstract Quaternion that rotates `from` into `to`. */
789	quatf(float3 from, float3 to) : ::simd_quatf(::simd_quaternion(from, to)) { }
790
791	/*! @abstract Constructs a quaternion from a rotation matrix. */
792	quatf(::simd_float3x3 matrix) : ::simd_quatf(::simd_quaternion(matrix)) { }
793
794	/*! @abstract Constructs a quaternion from a rotation matrix. */
795	quatf(::simd_float4x4 matrix) : ::simd_quatf(::simd_quaternion(matrix)) { }
796
797	/*! @abstract The real (scalar) part of the quaternion. */
798	float real(void) const { return ::simd_real(*this); }
799
800	/*! @abstract The imaginary (vector) part of the quaternion. */
801	float3 imag(void) const { return ::simd_imag(*this); }
802
803	/*! @abstract The angle the quaternion rotates by. */
804	float angle(void) const { return ::simd_angle(*this); }
805
806	/*! @abstract The axis the quaternion rotates about. */
807	float3 axis(void) const { return ::simd_axis(*this); }
808
809	/*! @abstract The length of the quaternion. */
810	float length(void) const { return ::simd_length(*this); }
811
812	/*! @abstract Act on the vector `v` by rotation. */
813	float3 operator()(const ::simd_float3 v) const { return ::simd_act(*this, v); }
814	};
815
816	static SIMD_CPPFUNC quatf operator+(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_add(p, q); }
817	static SIMD_CPPFUNC quatf operator-(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_sub(p, q); }
818	static SIMD_CPPFUNC quatf operator-(const ::simd_quatf p) { return ::simd_negate(p); }
819	static SIMD_CPPFUNC quatf operator*(const float r, const ::simd_quatf p) { return ::simd_mul(r, p); }
820	static SIMD_CPPFUNC quatf operator*(const ::simd_quatf p, const float r) { return ::simd_mul(p, r); }
821	static SIMD_CPPFUNC quatf operator*(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_mul(p, q); }
822	static SIMD_CPPFUNC quatf operator/(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_mul(p, ::simd_inverse(q)); }
823	static SIMD_INLINE SIMD_NODEBUG quatf operator+=(quatf &p, const ::simd_quatf q) { return p = p+q; }
824	static SIMD_INLINE SIMD_NODEBUG quatf operator-=(quatf &p, const ::simd_quatf q) { return p = p-q; }
825	static SIMD_INLINE SIMD_NODEBUG quatf operator*=(quatf &p, const float r) { return p = p*r; }
826	static SIMD_INLINE SIMD_NODEBUG quatf operator*=(quatf &p, const ::simd_quatf q) { return p = p*q; }
827	static SIMD_INLINE SIMD_NODEBUG quatf operator/=(quatf &p, const ::simd_quatf q) { return p = p/q; }
828
829	/*! @abstract The conjugate of the quaternion `q`. */
830	static SIMD_CPPFUNC quatf conjugate(const ::simd_quatf p) { return ::simd_conjugate(p); }
831
832	/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
833	static SIMD_CPPFUNC quatf inverse(const ::simd_quatf p) { return ::simd_inverse(p); }
834
835	/*! @abstract The dot product of the quaternions `p` and `q` interpreted as
836	* four-dimensional vectors. */
837	static SIMD_CPPFUNC float dot(const ::simd_quatf p, const ::simd_quatf q) { return ::simd_dot(p, q); }
838
839	/*! @abstract The unit quaternion obtained by normalizing `q`. */
840	static SIMD_CPPFUNC quatf normalize(const ::simd_quatf p) { return ::simd_normalize(p); }
841
842	/*! @abstract logarithm of the quaternion `q`. */
843	static SIMD_CPPFUNC quatf log(const ::simd_quatf q) { return ::__tg_log(q); }
844
845	/*! @abstract exponential map of quaterion `q`. */
846	static SIMD_CPPFUNC quatf exp(const ::simd_quatf q) { return ::__tg_exp(q); }
847
848	/*! @abstract Spherical linear interpolation along the shortest arc between
849	* quaternions `q0` and `q1`. */
850	static SIMD_CPPFUNC quatf slerp(const ::simd_quatf p0, const ::simd_quatf p1, float t) { return ::simd_slerp(p0, p1, t); }
851
852	/*! @abstract Spherical linear interpolation along the longest arc between
853	* quaternions `q0` and `q1`. */
854	static SIMD_CPPFUNC quatf slerp_longest(const ::simd_quatf p0, const ::simd_quatf p1, float t) { return ::simd_slerp_longest(p0, p1, t); }
855
856	/*! @abstract Interpolate between quaternions along a spherical cubic spline.
857	*
858	* @discussion The function interpolates between q1 and q2. q0 is the left
859	* endpoint of the previous interval, and q3 is the right endpoint of the next
860	* interval. Use this function to smoothly interpolate between a sequence of
861	* rotations. */
862	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); }
863
864	/*! @abstract Spherical cubic Bezier interpolation between quaternions.
865	*
866	* @discussion The function treats q0 ... q3 as control points and uses slerp
867	* in place of lerp in the De Castlejeau algorithm. The endpoints of
868	* interpolation are thus q0 and q3, and the curve will not generally pass
869	* through q1 or q2. Note that the convex hull property of "standard" Bezier
870	* curve does not hold on the sphere. */
871	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); }
872	}
873
874	extern "C" {
875	#endif /* __cplusplus */
876
877	/* MARK: - float implementations */
878
879	#include <simd/math.h>
880	#include <simd/geometry.h>
881
882	/* tg_promote is implementation gobbledygook that enables the compile-time
883	* dispatching in tgmath.h to work its magic. */
884	static simd_quatf __attribute__((__overloadable__)) __tg_promote(simd_quatf);
885
886	/*! @abstract Constructs a quaternion from imaginary and real parts.
887	* @discussion This function is hidden behind an underscore to avoid confusion
888	* with the angle-axis constructor. */
889	static inline SIMD_CFUNC simd_quatf _simd_quaternion(simd_float3 imag, float real) {
890	return simd_quaternion(simd_make_float4(imag, real));
891	}
892
893	static inline SIMD_CFUNC simd_quatf simd_quaternion(float angle, simd_float3 axis) {
894	return _simd_quaternion(sin(angle/2) * axis, cos(angle/2));
895	}
896
897	static inline SIMD_CFUNC float simd_angle(simd_quatf q) {
898	return 2*atan2(simd_length(q.vector.xyz), q.vector.w);
899	}
900
901	static inline SIMD_CFUNC simd_float3 simd_axis(simd_quatf q) {
902	return simd_normalize(q.vector.xyz);
903	}
904
905	static inline SIMD_CFUNC simd_quatf simd_add(simd_quatf p, simd_quatf q) {
906	return simd_quaternion(p.vector + q.vector);
907	}
908
909	static inline SIMD_CFUNC simd_quatf simd_sub(simd_quatf p, simd_quatf q) {
910	return simd_quaternion(p.vector - q.vector);
911	}
912
913	static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf p, simd_quatf q) {
914	#pragma STDC FP_CONTRACT ON
915	return simd_quaternion((p.vector.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
916	p.vector.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5)) +
917	(p.vector.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6) +
918	p.vector.w * q.vector));
919	}
920
921	static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf q, float a) {
922	return simd_quaternion(a * q.vector);
923	}
924
925	static inline SIMD_CFUNC simd_quatf simd_mul(float a, simd_quatf q) {
926	return simd_mul(q,a);
927	}
928
929	static inline SIMD_CFUNC simd_quatf simd_conjugate(simd_quatf q) {
930	return simd_quaternion(q.vector * (simd_float4){-1,-1,-1, 1});
931	}
932
933	static inline SIMD_CFUNC simd_quatf simd_inverse(simd_quatf q) {
934	return simd_quaternion(simd_conjugate(q).vector * simd_recip(simd_length_squared(q.vector)));
935	}
936
937	static inline SIMD_CFUNC simd_quatf simd_negate(simd_quatf q) {
938	return simd_quaternion(-q.vector);
939	}
940
941	static inline SIMD_CFUNC float simd_dot(simd_quatf p, simd_quatf q) {
942	return simd_dot(p.vector, q.vector);
943	}
944
945	static inline SIMD_CFUNC float simd_length(simd_quatf q) {
946	return simd_length(q.vector);
947	}
948
949	static inline SIMD_CFUNC simd_quatf simd_normalize(simd_quatf q) {
950	float length_squared = simd_length_squared(q.vector);
951	if (length_squared == 0) {
952	return simd_quaternion((simd_float4){0,0,0,1});
953	}
954	return simd_quaternion(q.vector * simd_rsqrt(length_squared));
955	}
956
957	#if defined __arm__ || defined __arm64__ || defined __aarch64__
958	/*! @abstract Multiplies the vector `v` by the quaternion `q`.
959	*
960	* @discussion This IS NOT the action of `q` on `v` (i.e. this is not rotation
961	* by `q`. That operation is provided by `simd_act(q, v)`. This function is an
962	* implementation detail and you should not call it directly. It may be
963	* removed or modified in future versions of the simd module. */
964	static inline SIMD_CFUNC simd_quatf _simd_mul_vq(simd_float3 v, simd_quatf q) {
965	#pragma STDC FP_CONTRACT ON
966	return simd_quaternion(v.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
967	v.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5) +
968	v.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6));
969	}
970	#endif
971
972	static inline SIMD_CFUNC simd_float3 simd_act(simd_quatf q, simd_float3 v) {
973	#if defined __arm__ || defined __arm64__ || defined __aarch64__
974	return simd_mul(q, _simd_mul_vq(v, simd_conjugate(q))).vector.xyz;
975	#else
976	#pragma STDC FP_CONTRACT ON
977	simd_float3 t = 2*simd_cross(simd_imag(q),v);
978	return v + simd_real(q)*t + simd_cross(simd_imag(q), t);
979	#endif
980	}
981
982	static SIMD_NOINLINE simd_quatf __tg_log(simd_quatf q) {
983	float real = log(simd_length_squared(q.vector))/2;
984	if (simd_equal(simd_imag(q), 0)) return _simd_quaternion(0, real);
985	simd_float3 imag = acos(simd_real(q)/simd_length(q)) * simd_normalize(simd_imag(q));
986	return _simd_quaternion(imag, real);
987	}
988
989	static SIMD_NOINLINE simd_quatf __tg_exp(simd_quatf q) {
990	// angle is actually *twice* the angle of the rotation corresponding to
991	// the resulting quaternion, which is why we don't simply use the (angle,
992	// axis) constructor to generate `unit`.
993	float angle = simd_length(simd_imag(q));
994	if (angle == 0) return _simd_quaternion((simd_float3)0, exp(simd_real(q)));
995	simd_float3 axis = simd_normalize(simd_imag(q));
996	simd_quatf unit = _simd_quaternion(sin(angle)*axis, cos(angle));
997	return simd_mul(exp(simd_real(q)), unit);
998	}
999
1000	/*! @abstract Implementation detail of the `simd_quaternion(from, to)`
1001	* initializer.
1002	*
1003	* @discussion Computes the quaternion rotation `from` to `to` if they are
1004	* separated by less than 90 degrees. Not numerically stable for larger
1005	* angles. This function is an implementation detail and you should not
1006	* call it directly. It may be removed or modified in future versions of the
1007	* simd module. */
1008	static inline SIMD_CFUNC simd_quatf _simd_quaternion_reduced(simd_float3 from, simd_float3 to) {
1009	simd_float3 half = simd_normalize(from + to);
1010	return _simd_quaternion(simd_cross(from, half), simd_dot(from, half));
1011	}
1012
1013	static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float3 from, simd_float3 to) {
1014
1015	// If the angle between from and to is not too big, we can compute the
1016	// rotation accurately using a simple implementation.
1017	if (simd_dot(from, to) >= 0) {
1018	return _simd_quaternion_reduced(from, to);
1019	}
1020
1021	// Because from and to are more than 90 degrees apart, we compute the
1022	// rotation in two stages (from -> half), (half -> to) to preserve numerical
1023	// accuracy.
1024	simd_float3 half = simd_normalize(from) + simd_normalize(to);
1025
1026	if (simd_length_squared(half) <= 0x1p-46f) {
1027	// half is nearly zero, so from and to point in nearly opposite directions
1028	// and the rotation is numerically underspecified. Pick an axis orthogonal
1029	// to the vectors, and use an angle of pi radians.
1030	simd_float3 abs_from = simd_abs(from);
1031	if (abs_from.x <= abs_from.y && abs_from.x <= abs_from.z)
1032	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_float3){1,0,0})), 0.f);
1033	else if (abs_from.y <= abs_from.z)
1034	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_float3){0,1,0})), 0.f);
1035	else
1036	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_float3){0,0,1})), 0.f);
1037	}
1038
1039	// Compute the two-step rotation. */
1040	half = simd_normalize(half);
1041	return simd_mul(_simd_quaternion_reduced(from, half),
1042	_simd_quaternion_reduced(half, to));
1043	}
1044
1045	static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float3x3 matrix) {
1046	const simd_float3 *mat = matrix.columns;
1047	float trace = mat[0][0] + mat[1][1] + mat[2][2];
1048	if (trace >= 0.0) {
1049	float r = 2*sqrt(1 + trace);
1050	float rinv = simd_recip(r);
1051	return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
1052	rinv*(mat[2][0] - mat[0][2]),
1053	rinv*(mat[0][1] - mat[1][0]),
1054	r/4);
1055	} else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
1056	float r = 2*sqrt(1 - mat[1][1] - mat[2][2] + mat[0][0]);
1057	float rinv = simd_recip(r);
1058	return simd_quaternion(r/4,
1059	rinv*(mat[0][1] + mat[1][0]),
1060	rinv*(mat[0][2] + mat[2][0]),
1061	rinv*(mat[1][2] - mat[2][1]));
1062	} else if (mat[1][1] >= mat[2][2]) {
1063	float r = 2*sqrt(1 - mat[0][0] - mat[2][2] + mat[1][1]);
1064	float rinv = simd_recip(r);
1065	return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
1066	r/4,
1067	rinv*(mat[1][2] + mat[2][1]),
1068	rinv*(mat[2][0] - mat[0][2]));
1069	} else {
1070	float r = 2*sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
1071	float rinv = simd_recip(r);
1072	return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
1073	rinv*(mat[1][2] + mat[2][1]),
1074	r/4,
1075	rinv*(mat[0][1] - mat[1][0]));
1076	}
1077	}
1078
1079	static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float4x4 matrix) {
1080	const simd_float4 *mat = matrix.columns;
1081	float trace = mat[0][0] + mat[1][1] + mat[2][2];
1082	if (trace >= 0.0) {
1083	float r = 2*sqrt(1 + trace);
1084	float rinv = simd_recip(r);
1085	return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
1086	rinv*(mat[2][0] - mat[0][2]),
1087	rinv*(mat[0][1] - mat[1][0]),
1088	r/4);
1089	} else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
1090	float r = 2*sqrt(1 - mat[1][1] - mat[2][2] + mat[0][0]);
1091	float rinv = simd_recip(r);
1092	return simd_quaternion(r/4,
1093	rinv*(mat[0][1] + mat[1][0]),
1094	rinv*(mat[0][2] + mat[2][0]),
1095	rinv*(mat[1][2] - mat[2][1]));
1096	} else if (mat[1][1] >= mat[2][2]) {
1097	float r = 2*sqrt(1 - mat[0][0] - mat[2][2] + mat[1][1]);
1098	float rinv = simd_recip(r);
1099	return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
1100	r/4,
1101	rinv*(mat[1][2] + mat[2][1]),
1102	rinv*(mat[2][0] - mat[0][2]));
1103	} else {
1104	float r = 2*sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
1105	float rinv = simd_recip(r);
1106	return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
1107	rinv*(mat[1][2] + mat[2][1]),
1108	r/4,
1109	rinv*(mat[0][1] - mat[1][0]));
1110	}
1111	}
1112
1113	/*! @abstract The angle between p and q interpreted as 4-dimensional vectors.
1114	*
1115	* @discussion This function is an implementation detail and you should not
1116	* call it directly. It may be removed or modified in future versions of the
1117	* simd module. */
1118	static SIMD_NOINLINE float _simd_angle(simd_quatf p, simd_quatf q) {
1119	return 2*atan2(simd_length(p.vector - q.vector), simd_length(p.vector + q.vector));
1120	}
1121
1122	/*! @abstract sin(x)/x.
1123	*
1124	* @discussion This function is an implementation detail and you should not
1125	* call it directly. It may be removed or modified in future versions of the
1126	* simd module. */
1127	static SIMD_CFUNC float _simd_sinc(float x) {
1128	if (x == 0) return 1;
1129	return sin(x)/x;
1130	}
1131
1132	/*! @abstract Spherical lerp between q0 and q1.
1133	*
1134	* @discussion This function may interpolate along either the longer or
1135	* shorter path between q0 and q1; it is used as an implementation detail
1136	* in `simd_slerp` and `simd_slerp_longest`; you should use those functions
1137	* instead of calling this directly. */
1138	static SIMD_NOINLINE simd_quatf _simd_slerp_internal(simd_quatf q0, simd_quatf q1, float t) {
1139	float s = 1 - t;
1140	float a = _simd_angle(q0, q1);
1141	float r = simd_recip(_simd_sinc(a));
1142	return simd_normalize(simd_quaternion(_simd_sinc(s*a)*r*s*q0.vector + _simd_sinc(t*a)*r*t*q1.vector));
1143	}
1144
1145	static SIMD_NOINLINE simd_quatf simd_slerp(simd_quatf q0, simd_quatf q1, float t) {
1146	if (simd_dot(q0, q1) >= 0)
1147	return _simd_slerp_internal(q0, q1, t);
1148	return _simd_slerp_internal(q0, simd_negate(q1), t);
1149	}
1150
1151	static SIMD_NOINLINE simd_quatf simd_slerp_longest(simd_quatf q0, simd_quatf q1, float t) {
1152	if (simd_dot(q0, q1) >= 0)
1153	return _simd_slerp_internal(q0, simd_negate(q1), t);
1154	return _simd_slerp_internal(q0, q1, t);
1155	}
1156
1157	/*! @discussion This function is an implementation detail and you should not
1158	* call it directly. It may be removed or modified in future versions of the
1159	* simd module. */
1160	static SIMD_NOINLINE simd_quatf _simd_intermediate(simd_quatf q0, simd_quatf q1, simd_quatf q2) {
1161	simd_quatf p0 = __tg_log(simd_mul(q0, simd_inverse(q1)));
1162	simd_quatf p2 = __tg_log(simd_mul(q2, simd_inverse(q1)));
1163	return simd_normalize(simd_mul(q1, __tg_exp(simd_mul(-0.25, simd_add(p0,p2)))));
1164	}
1165
1166	/*! @discussion This function is an implementation detail and you should not
1167	* call it directly. It may be removed or modified in future versions of the
1168	* simd module. */
1169	static SIMD_NOINLINE simd_quatf _simd_squad(simd_quatf q0, simd_quatf qa, simd_quatf qb, simd_quatf q1, float t) {
1170	simd_quatf r0 = _simd_slerp_internal(q0, q1, t);
1171	simd_quatf r1 = _simd_slerp_internal(qa, qb, t);
1172	return _simd_slerp_internal(r0, r1, 2*t*(1 - t));
1173	}
1174
1175	static SIMD_NOINLINE simd_quatf simd_spline(simd_quatf q0, simd_quatf q1, simd_quatf q2, simd_quatf q3, float t) {
1176	simd_quatf qa = _simd_intermediate(q0, q1, q2);
1177	simd_quatf qb = _simd_intermediate(q1, q2, q3);
1178	return _simd_squad(q1, qa, qb, q2, t);
1179	}
1180
1181	static SIMD_NOINLINE simd_quatf simd_bezier(simd_quatf q0, simd_quatf q1, simd_quatf q2, simd_quatf q3, float t) {
1182	simd_quatf q01 = _simd_slerp_internal(q0, q1, t);
1183	simd_quatf q12 = _simd_slerp_internal(q1, q2, t);
1184	simd_quatf q23 = _simd_slerp_internal(q2, q3, t);
1185	simd_quatf q012 = _simd_slerp_internal(q01, q12, t);
1186	simd_quatf q123 = _simd_slerp_internal(q12, q23, t);
1187	return _simd_slerp_internal(q012, q123, t);
1188	}
1189
1190	/* MARK: - C and Objective-C double interfaces */
1191
1192	/*! @abstract Constructs a quaternion from four scalar values.
1193	*
1194	* @param ix The first component of the imaginary (vector) part.
1195	* @param iy The second component of the imaginary (vector) part.
1196	* @param iz The third component of the imaginary (vector) part.
1197	*
1198	* @param r The real (scalar) part. */
1199	static inline SIMD_CFUNC simd_quatd simd_quaternion(double ix, double iy, double iz, double r) {
1200	return (simd_quatd){ { ix, iy, iz, r } };
1201	}
1202
1203	/*! @abstract Constructs a quaternion from an array of four scalars.
1204	*
1205	* @discussion Note that the imaginary part of the quaternion comes from
1206	* array elements 0, 1, and 2, and the real part comes from element 3. */
1207	static inline SIMD_NONCONST simd_quatd simd_quaternion(const double xyzr[4]) {
1208	return (simd_quatd){ *(const simd_packed_double4 *)xyzr };
1209	}
1210
1211	/*! @abstract Constructs a quaternion from a four-element vector.
1212	*
1213	* @discussion Note that the imaginary (vector) part of the quaternion comes
1214	* from lanes 0, 1, and 2 of the vector, and the real (scalar) part comes from
1215	* lane 3. */
1216	static inline SIMD_CFUNC simd_quatd simd_quaternion(simd_double4 xyzr) {
1217	return (simd_quatd){ xyzr };
1218	}
1219
1220	/*! @abstract Constructs a quaternion that rotates by `angle` radians about
1221	* `axis`. */
1222	static inline SIMD_CFUNC simd_quatd simd_quaternion(double angle, simd_double3 axis);
1223
1224	/*! @abstract Construct a quaternion that rotates from one vector to another.
1225	*
1226	* @param from A normalized three-element vector.
1227	* @param to A normalized three-element vector.
1228	*
1229	* @discussion The rotation axis is `simd_cross(from, to)`. If `from` and
1230	* `to` point in opposite directions (to within machine precision), an
1231	* arbitrary rotation axis is chosen, and the angle is pi radians. */
1232	static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double3 from, simd_double3 to);
1233
1234	/*! @abstract Construct a quaternion from a 3x3 rotation `matrix`.
1235	*
1236	* @discussion If `matrix` is not orthogonal with determinant 1, the result
1237	* is undefined. */
1238	static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double3x3 matrix);
1239
1240	/*! @abstract Construct a quaternion from a 4x4 rotation `matrix`.
1241	*
1242	* @discussion The last row and column of the matrix are ignored. This
1243	* function is equivalent to calling simd_quaternion with the upper-left 3x3
1244	* submatrix . */
1245	static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double4x4 matrix);
1246
1247	/*! @abstract The real (scalar) part of the quaternion `q`. */
1248	static inline SIMD_CFUNC double simd_real(simd_quatd q) {
1249	return q.vector.w;
1250	}
1251
1252	/*! @abstract The imaginary (vector) part of the quaternion `q`. */
1253	static inline SIMD_CFUNC simd_double3 simd_imag(simd_quatd q) {
1254	return q.vector.xyz;
1255	}
1256
1257	/*! @abstract The angle (in radians) of rotation represented by `q`. */
1258	static inline SIMD_CFUNC double simd_angle(simd_quatd q);
1259
1260	/*! @abstract The normalized axis (a 3-element vector) around which the
1261	* action of the quaternion `q` rotates. */
1262	static inline SIMD_CFUNC simd_double3 simd_axis(simd_quatd q);
1263
1264	/*! @abstract The sum of the quaternions `p` and `q`. */
1265	static inline SIMD_CFUNC simd_quatd simd_add(simd_quatd p, simd_quatd q);
1266
1267	/*! @abstract The difference of the quaternions `p` and `q`. */
1268	static inline SIMD_CFUNC simd_quatd simd_sub(simd_quatd p, simd_quatd q);
1269
1270	/*! @abstract The product of the quaternions `p` and `q`. */
1271	static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd p, simd_quatd q);
1272
1273	/*! @abstract The quaternion `q` scaled by the real value `a`. */
1274	static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd q, double a);
1275
1276	/*! @abstract The quaternion `q` scaled by the real value `a`. */
1277	static inline SIMD_CFUNC simd_quatd simd_mul(double a, simd_quatd q);
1278
1279	/*! @abstract The conjugate of the quaternion `q`. */
1280	static inline SIMD_CFUNC simd_quatd simd_conjugate(simd_quatd q);
1281
1282	/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
1283	static inline SIMD_CFUNC simd_quatd simd_inverse(simd_quatd q);
1284
1285	/*! @abstract The negation (additive inverse) of the quaternion `q`. */
1286	static inline SIMD_CFUNC simd_quatd simd_negate(simd_quatd q);
1287
1288	/*! @abstract The dot product of the quaternions `p` and `q` interpreted as
1289	* four-dimensional vectors. */
1290	static inline SIMD_CFUNC double simd_dot(simd_quatd p, simd_quatd q);
1291
1292	/*! @abstract The length of the quaternion `q`. */
1293	static inline SIMD_CFUNC double simd_length(simd_quatd q);
1294
1295	/*! @abstract The unit quaternion obtained by normalizing `q`. */
1296	static inline SIMD_CFUNC simd_quatd simd_normalize(simd_quatd q);
1297
1298	/*! @abstract Rotates the vector `v` by the quaternion `q`. */
1299	static inline SIMD_CFUNC simd_double3 simd_act(simd_quatd q, simd_double3 v);
1300
1301	/*! @abstract Logarithm of the quaternion `q`.
1302	* @discussion Do not call this function directly; use `log(q)` instead.
1303	*
1304	* We can write a quaternion `q` in the form: `r(cos(t) + sin(t)v)` where
1305	* `r` is the length of `q`, `t` is an angle, and `v` is a unit 3-vector.
1306	* The logarithm of `q` is `log(r) + tv`, just like the logarithm of the
1307	* complex number `r*(cos(t) + i sin(t))` is `log(r) + it`.
1308	*
1309	* Note that this function is not robust against poorly-scaled non-unit
1310	* quaternions, because it is primarily used for spline interpolation of
1311	* unit quaternions. If you need to compute a robust logarithm of general
1312	* quaternions, you can use the following approach:
1313	*
1314	* scale = simd_reduce_max(simd_abs(q.vector));
1315	* logq = log(simd_recip(scale)*q);
1316	* logq.real += log(scale);
1317	* return logq; */
1318	static SIMD_NOINLINE simd_quatd __tg_log(simd_quatd q);
1319
1320	/*! @abstract Inverse of `log( )`; the exponential map on quaternions.
1321	* @discussion Do not call this function directly; use `exp(q)` instead. */
1322	static SIMD_NOINLINE simd_quatd __tg_exp(simd_quatd q);
1323
1324	/*! @abstract Spherical linear interpolation along the shortest arc between
1325	* quaternions `q0` and `q1`. */
1326	static SIMD_NOINLINE simd_quatd simd_slerp(simd_quatd q0, simd_quatd q1, double t);
1327
1328	/*! @abstract Spherical linear interpolation along the longest arc between
1329	* quaternions `q0` and `q1`. */
1330	static SIMD_NOINLINE simd_quatd simd_slerp_longest(simd_quatd q0, simd_quatd q1, double t);
1331
1332	/*! @abstract Interpolate between quaternions along a spherical cubic spline.
1333	*
1334	* @discussion The function interpolates between q1 and q2. q0 is the left
1335	* endpoint of the previous interval, and q3 is the right endpoint of the next
1336	* interval. Use this function to smoothly interpolate between a sequence of
1337	* rotations. */
1338	static SIMD_NOINLINE simd_quatd simd_spline(simd_quatd q0, simd_quatd q1, simd_quatd q2, simd_quatd q3, double t);
1339
1340	/*! @abstract Spherical cubic Bezier interpolation between quaternions.
1341	*
1342	* @discussion The function treats q0 ... q3 as control points and uses slerp
1343	* in place of lerp in the De Castlejeau algorithm. The endpoints of
1344	* interpolation are thus q0 and q3, and the curve will not generally pass
1345	* through q1 or q2. Note that the convex hull property of "standard" Bezier
1346	* curve does not hold on the sphere. */
1347	static SIMD_NOINLINE simd_quatd simd_bezier(simd_quatd q0, simd_quatd q1, simd_quatd q2, simd_quatd q3, double t);
1348
1349	#ifdef __cplusplus
1350	} /* extern "C" */
1351	/* MARK: - C++ double interfaces */
1352
1353	namespace simd {
1354	struct quatd : ::simd_quatd {
1355	/*! @abstract The identity quaternion. */
1356	quatd( ) : ::simd_quatd(::simd_quaternion((double4){0,0,0,1})) { }
1357
1358	/*! @abstract Constructs a C++ quaternion from a C quaternion. */
1359	quatd(::simd_quatd q) : ::simd_quatd(q) { }
1360
1361	/*! @abstract Constructs a quaternion from components. */
1362	quatd(double ix, double iy, double iz, double r) : ::simd_quatd(::simd_quaternion(ix, iy, iz, r)) { }
1363
1364	/*! @abstract Constructs a quaternion from an array of scalars. */
1365	quatd(const double xyzr[4]) : ::simd_quatd(::simd_quaternion(xyzr)) { }
1366
1367	/*! @abstract Constructs a quaternion from a vector. */
1368	quatd(double4 xyzr) : ::simd_quatd(::simd_quaternion(xyzr)) { }
1369
1370	/*! @abstract Quaternion representing rotation about `axis` by `angle`
1371	* radians. */
1372	quatd(double angle, double3 axis) : ::simd_quatd(::simd_quaternion(angle, axis)) { }
1373
1374	/*! @abstract Quaternion that rotates `from` into `to`. */
1375	quatd(double3 from, double3 to) : ::simd_quatd(::simd_quaternion(from, to)) { }
1376
1377	/*! @abstract Constructs a quaternion from a rotation matrix. */
1378	quatd(::simd_double3x3 matrix) : ::simd_quatd(::simd_quaternion(matrix)) { }
1379
1380	/*! @abstract Constructs a quaternion from a rotation matrix. */
1381	quatd(::simd_double4x4 matrix) : ::simd_quatd(::simd_quaternion(matrix)) { }
1382
1383	/*! @abstract The real (scalar) part of the quaternion. */
1384	double real(void) const { return ::simd_real(*this); }
1385
1386	/*! @abstract The imaginary (vector) part of the quaternion. */
1387	double3 imag(void) const { return ::simd_imag(*this); }
1388
1389	/*! @abstract The angle the quaternion rotates by. */
1390	double angle(void) const { return ::simd_angle(*this); }
1391
1392	/*! @abstract The axis the quaternion rotates about. */
1393	double3 axis(void) const { return ::simd_axis(*this); }
1394
1395	/*! @abstract The length of the quaternion. */
1396	double length(void) const { return ::simd_length(*this); }
1397
1398	/*! @abstract Act on the vector `v` by rotation. */
1399	double3 operator()(const ::simd_double3 v) const { return ::simd_act(*this, v); }
1400	};
1401
1402	static SIMD_CPPFUNC quatd operator+(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_add(p, q); }
1403	static SIMD_CPPFUNC quatd operator-(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_sub(p, q); }
1404	static SIMD_CPPFUNC quatd operator-(const ::simd_quatd p) { return ::simd_negate(p); }
1405	static SIMD_CPPFUNC quatd operator*(const double r, const ::simd_quatd p) { return ::simd_mul(r, p); }
1406	static SIMD_CPPFUNC quatd operator*(const ::simd_quatd p, const double r) { return ::simd_mul(p, r); }
1407	static SIMD_CPPFUNC quatd operator*(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_mul(p, q); }
1408	static SIMD_CPPFUNC quatd operator/(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_mul(p, ::simd_inverse(q)); }
1409	static SIMD_INLINE SIMD_NODEBUG quatd operator+=(quatd &p, const ::simd_quatd q) { return p = p+q; }
1410	static SIMD_INLINE SIMD_NODEBUG quatd operator-=(quatd &p, const ::simd_quatd q) { return p = p-q; }
1411	static SIMD_INLINE SIMD_NODEBUG quatd operator*=(quatd &p, const double r) { return p = p*r; }
1412	static SIMD_INLINE SIMD_NODEBUG quatd operator*=(quatd &p, const ::simd_quatd q) { return p = p*q; }
1413	static SIMD_INLINE SIMD_NODEBUG quatd operator/=(quatd &p, const ::simd_quatd q) { return p = p/q; }
1414
1415	/*! @abstract The conjugate of the quaternion `q`. */
1416	static SIMD_CPPFUNC quatd conjugate(const ::simd_quatd p) { return ::simd_conjugate(p); }
1417
1418	/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
1419	static SIMD_CPPFUNC quatd inverse(const ::simd_quatd p) { return ::simd_inverse(p); }
1420
1421	/*! @abstract The dot product of the quaternions `p` and `q` interpreted as
1422	* four-dimensional vectors. */
1423	static SIMD_CPPFUNC double dot(const ::simd_quatd p, const ::simd_quatd q) { return ::simd_dot(p, q); }
1424
1425	/*! @abstract The unit quaternion obtained by normalizing `q`. */
1426	static SIMD_CPPFUNC quatd normalize(const ::simd_quatd p) { return ::simd_normalize(p); }
1427
1428	/*! @abstract logarithm of the quaternion `q`. */
1429	static SIMD_CPPFUNC quatd log(const ::simd_quatd q) { return ::__tg_log(q); }
1430
1431	/*! @abstract exponential map of quaterion `q`. */
1432	static SIMD_CPPFUNC quatd exp(const ::simd_quatd q) { return ::__tg_exp(q); }
1433
1434	/*! @abstract Spherical linear interpolation along the shortest arc between
1435	* quaternions `q0` and `q1`. */
1436	static SIMD_CPPFUNC quatd slerp(const ::simd_quatd p0, const ::simd_quatd p1, double t) { return ::simd_slerp(p0, p1, t); }
1437
1438	/*! @abstract Spherical linear interpolation along the longest arc between
1439	* quaternions `q0` and `q1`. */
1440	static SIMD_CPPFUNC quatd slerp_longest(const ::simd_quatd p0, const ::simd_quatd p1, double t) { return ::simd_slerp_longest(p0, p1, t); }
1441
1442	/*! @abstract Interpolate between quaternions along a spherical cubic spline.
1443	*
1444	* @discussion The function interpolates between q1 and q2. q0 is the left
1445	* endpoint of the previous interval, and q3 is the right endpoint of the next
1446	* interval. Use this function to smoothly interpolate between a sequence of
1447	* rotations. */
1448	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); }
1449
1450	/*! @abstract Spherical cubic Bezier interpolation between quaternions.
1451	*
1452	* @discussion The function treats q0 ... q3 as control points and uses slerp
1453	* in place of lerp in the De Castlejeau algorithm. The endpoints of
1454	* interpolation are thus q0 and q3, and the curve will not generally pass
1455	* through q1 or q2. Note that the convex hull property of "standard" Bezier
1456	* curve does not hold on the sphere. */
1457	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); }
1458	}
1459
1460	extern "C" {
1461	#endif /* __cplusplus */
1462
1463	/* MARK: - double implementations */
1464
1465	#include <simd/math.h>
1466	#include <simd/geometry.h>
1467
1468	/* tg_promote is implementation gobbledygook that enables the compile-time
1469	* dispatching in tgmath.h to work its magic. */
1470	static simd_quatd __attribute__((__overloadable__)) __tg_promote(simd_quatd);
1471
1472	/*! @abstract Constructs a quaternion from imaginary and real parts.
1473	* @discussion This function is hidden behind an underscore to avoid confusion
1474	* with the angle-axis constructor. */
1475	static inline SIMD_CFUNC simd_quatd _simd_quaternion(simd_double3 imag, double real) {
1476	return simd_quaternion(simd_make_double4(imag, real));
1477	}
1478
1479	static inline SIMD_CFUNC simd_quatd simd_quaternion(double angle, simd_double3 axis) {
1480	return _simd_quaternion(sin(angle/2) * axis, cos(angle/2));
1481	}
1482
1483	static inline SIMD_CFUNC double simd_angle(simd_quatd q) {
1484	return 2*atan2(simd_length(q.vector.xyz), q.vector.w);
1485	}
1486
1487	static inline SIMD_CFUNC simd_double3 simd_axis(simd_quatd q) {
1488	return simd_normalize(q.vector.xyz);
1489	}
1490
1491	static inline SIMD_CFUNC simd_quatd simd_add(simd_quatd p, simd_quatd q) {
1492	return simd_quaternion(p.vector + q.vector);
1493	}
1494
1495	static inline SIMD_CFUNC simd_quatd simd_sub(simd_quatd p, simd_quatd q) {
1496	return simd_quaternion(p.vector - q.vector);
1497	}
1498
1499	static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd p, simd_quatd q) {
1500	#pragma STDC FP_CONTRACT ON
1501	return simd_quaternion((p.vector.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
1502	p.vector.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5)) +
1503	(p.vector.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6) +
1504	p.vector.w * q.vector));
1505	}
1506
1507	static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd q, double a) {
1508	return simd_quaternion(a * q.vector);
1509	}
1510
1511	static inline SIMD_CFUNC simd_quatd simd_mul(double a, simd_quatd q) {
1512	return simd_mul(q,a);
1513	}
1514
1515	static inline SIMD_CFUNC simd_quatd simd_conjugate(simd_quatd q) {
1516	return simd_quaternion(q.vector * (simd_double4){-1,-1,-1, 1});
1517	}
1518
1519	static inline SIMD_CFUNC simd_quatd simd_inverse(simd_quatd q) {
1520	return simd_quaternion(simd_conjugate(q).vector * simd_recip(simd_length_squared(q.vector)));
1521	}
1522
1523	static inline SIMD_CFUNC simd_quatd simd_negate(simd_quatd q) {
1524	return simd_quaternion(-q.vector);
1525	}
1526
1527	static inline SIMD_CFUNC double simd_dot(simd_quatd p, simd_quatd q) {
1528	return simd_dot(p.vector, q.vector);
1529	}
1530
1531	static inline SIMD_CFUNC double simd_length(simd_quatd q) {
1532	return simd_length(q.vector);
1533	}
1534
1535	static inline SIMD_CFUNC simd_quatd simd_normalize(simd_quatd q) {
1536	double length_squared = simd_length_squared(q.vector);
1537	if (length_squared == 0) {
1538	return simd_quaternion((simd_double4){0,0,0,1});
1539	}
1540	return simd_quaternion(q.vector * simd_rsqrt(length_squared));
1541	}
1542
1543	#if defined __arm__ || defined __arm64__ || defined __aarch64__
1544	/*! @abstract Multiplies the vector `v` by the quaternion `q`.
1545	*
1546	* @discussion This IS NOT the action of `q` on `v` (i.e. this is not rotation
1547	* by `q`. That operation is provided by `simd_act(q, v)`. This function is an
1548	* implementation detail and you should not call it directly. It may be
1549	* removed or modified in future versions of the simd module. */
1550	static inline SIMD_CFUNC simd_quatd _simd_mul_vq(simd_double3 v, simd_quatd q) {
1551	#pragma STDC FP_CONTRACT ON
1552	return simd_quaternion(v.x * __builtin_shufflevector(q.vector, -q.vector, 3,6,1,4) +
1553	v.y * __builtin_shufflevector(q.vector, -q.vector, 2,3,4,5) +
1554	v.z * __builtin_shufflevector(q.vector, -q.vector, 5,0,3,6));
1555	}
1556	#endif
1557
1558	static inline SIMD_CFUNC simd_double3 simd_act(simd_quatd q, simd_double3 v) {
1559	#if defined __arm__ || defined __arm64__ || defined __aarch64__
1560	return simd_mul(q, _simd_mul_vq(v, simd_conjugate(q))).vector.xyz;
1561	#else
1562	#pragma STDC FP_CONTRACT ON
1563	simd_double3 t = 2*simd_cross(simd_imag(q),v);
1564	return v + simd_real(q)*t + simd_cross(simd_imag(q), t);
1565	#endif
1566	}
1567
1568	static SIMD_NOINLINE simd_quatd __tg_log(simd_quatd q) {
1569	double real = log(simd_length_squared(q.vector))/2;
1570	if (simd_equal(simd_imag(q), 0)) return _simd_quaternion(0, real);
1571	simd_double3 imag = acos(simd_real(q)/simd_length(q)) * simd_normalize(simd_imag(q));
1572	return _simd_quaternion(imag, real);
1573	}
1574
1575	static SIMD_NOINLINE simd_quatd __tg_exp(simd_quatd q) {
1576	// angle is actually *twice* the angle of the rotation corresponding to
1577	// the resulting quaternion, which is why we don't simply use the (angle,
1578	// axis) constructor to generate `unit`.
1579	double angle = simd_length(simd_imag(q));
1580	if (angle == 0) return _simd_quaternion((simd_double3)0, exp(simd_real(q)));
1581	simd_double3 axis = simd_normalize(simd_imag(q));
1582	simd_quatd unit = _simd_quaternion(sin(angle)*axis, cos(angle));
1583	return simd_mul(exp(simd_real(q)), unit);
1584	}
1585
1586	/*! @abstract Implementation detail of the `simd_quaternion(from, to)`
1587	* initializer.
1588	*
1589	* @discussion Computes the quaternion rotation `from` to `to` if they are
1590	* separated by less than 90 degrees. Not numerically stable for larger
1591	* angles. This function is an implementation detail and you should not
1592	* call it directly. It may be removed or modified in future versions of the
1593	* simd module. */
1594	static inline SIMD_CFUNC simd_quatd _simd_quaternion_reduced(simd_double3 from, simd_double3 to) {
1595	simd_double3 half = simd_normalize(from + to);
1596	return _simd_quaternion(simd_cross(from, half), simd_dot(from, half));
1597	}
1598
1599	static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double3 from, simd_double3 to) {
1600
1601	// If the angle between from and to is not too big, we can compute the
1602	// rotation accurately using a simple implementation.
1603	if (simd_dot(from, to) >= 0) {
1604	return _simd_quaternion_reduced(from, to);
1605	}
1606
1607	// Because from and to are more than 90 degrees apart, we compute the
1608	// rotation in two stages (from -> half), (half -> to) to preserve numerical
1609	// accuracy.
1610	simd_double3 half = simd_normalize(from) + simd_normalize(to);
1611
1612	if (simd_length_squared(half) <= 0x1p-104) {
1613	// half is nearly zero, so from and to point in nearly opposite directions
1614	// and the rotation is numerically underspecified. Pick an axis orthogonal
1615	// to the vectors, and use an angle of pi radians.
1616	simd_double3 abs_from = simd_abs(from);
1617	if (abs_from.x <= abs_from.y && abs_from.x <= abs_from.z)
1618	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_double3){1,0,0})), 0.f);
1619	else if (abs_from.y <= abs_from.z)
1620	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_double3){0,1,0})), 0.f);
1621	else
1622	return _simd_quaternion(simd_normalize(simd_cross(from, (simd_double3){0,0,1})), 0.f);
1623	}
1624
1625	// Compute the two-step rotation. */
1626	half = simd_normalize(half);
1627	return simd_mul(_simd_quaternion_reduced(from, half),
1628	_simd_quaternion_reduced(half, to));
1629	}
1630
1631	static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double3x3 matrix) {
1632	const simd_double3 *mat = matrix.columns;
1633	double trace = mat[0][0] + mat[1][1] + mat[2][2];
1634	if (trace >= 0.0) {
1635	double r = 2*sqrt(1 + trace);
1636	double rinv = simd_recip(r);
1637	return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
1638	rinv*(mat[2][0] - mat[0][2]),
1639	rinv*(mat[0][1] - mat[1][0]),
1640	r/4);
1641	} else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
1642	double r = 2*sqrt(1 - mat[1][1] - mat[2][2] + mat[0][0]);
1643	double rinv = simd_recip(r);
1644	return simd_quaternion(r/4,
1645	rinv*(mat[0][1] + mat[1][0]),
1646	rinv*(mat[0][2] + mat[2][0]),
1647	rinv*(mat[1][2] - mat[2][1]));
1648	} else if (mat[1][1] >= mat[2][2]) {
1649	double r = 2*sqrt(1 - mat[0][0] - mat[2][2] + mat[1][1]);
1650	double rinv = simd_recip(r);
1651	return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
1652	r/4,
1653	rinv*(mat[1][2] + mat[2][1]),
1654	rinv*(mat[2][0] - mat[0][2]));
1655	} else {
1656	double r = 2*sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
1657	double rinv = simd_recip(r);
1658	return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
1659	rinv*(mat[1][2] + mat[2][1]),
1660	r/4,
1661	rinv*(mat[0][1] - mat[1][0]));
1662	}
1663	}
1664
1665	static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double4x4 matrix) {
1666	const simd_double4 *mat = matrix.columns;
1667	double trace = mat[0][0] + mat[1][1] + mat[2][2];
1668	if (trace >= 0.0) {
1669	double r = 2*sqrt(1 + trace);
1670	double rinv = simd_recip(r);
1671	return simd_quaternion(rinv*(mat[1][2] - mat[2][1]),
1672	rinv*(mat[2][0] - mat[0][2]),
1673	rinv*(mat[0][1] - mat[1][0]),
1674	r/4);
1675	} else if (mat[0][0] >= mat[1][1] && mat[0][0] >= mat[2][2]) {
1676	double r = 2*sqrt(1 - mat[1][1] - mat[2][2] + mat[0][0]);
1677	double rinv = simd_recip(r);
1678	return simd_quaternion(r/4,
1679	rinv*(mat[0][1] + mat[1][0]),
1680	rinv*(mat[0][2] + mat[2][0]),
1681	rinv*(mat[1][2] - mat[2][1]));
1682	} else if (mat[1][1] >= mat[2][2]) {
1683	double r = 2*sqrt(1 - mat[0][0] - mat[2][2] + mat[1][1]);
1684	double rinv = simd_recip(r);
1685	return simd_quaternion(rinv*(mat[0][1] + mat[1][0]),
1686	r/4,
1687	rinv*(mat[1][2] + mat[2][1]),
1688	rinv*(mat[2][0] - mat[0][2]));
1689	} else {
1690	double r = 2*sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]);
1691	double rinv = simd_recip(r);
1692	return simd_quaternion(rinv*(mat[0][2] + mat[2][0]),
1693	rinv*(mat[1][2] + mat[2][1]),
1694	r/4,
1695	rinv*(mat[0][1] - mat[1][0]));
1696	}
1697	}
1698
1699	/*! @abstract The angle between p and q interpreted as 4-dimensional vectors.
1700	*
1701	* @discussion This function is an implementation detail and you should not
1702	* call it directly. It may be removed or modified in future versions of the
1703	* simd module. */
1704	static SIMD_NOINLINE double _simd_angle(simd_quatd p, simd_quatd q) {
1705	return 2*atan2(simd_length(p.vector - q.vector), simd_length(p.vector + q.vector));
1706	}
1707
1708	/*! @abstract sin(x)/x.
1709	*
1710	* @discussion This function is an implementation detail and you should not
1711	* call it directly. It may be removed or modified in future versions of the
1712	* simd module. */
1713	static SIMD_CFUNC double _simd_sinc(double x) {
1714	if (x == 0) return 1;
1715	return sin(x)/x;
1716	}
1717
1718	/*! @abstract Spherical lerp between q0 and q1.
1719	*
1720	* @discussion This function may interpolate along either the longer or
1721	* shorter path between q0 and q1; it is used as an implementation detail
1722	* in `simd_slerp` and `simd_slerp_longest`; you should use those functions
1723	* instead of calling this directly. */
1724	static SIMD_NOINLINE simd_quatd _simd_slerp_internal(simd_quatd q0, simd_quatd q1, double t) {
1725	double s = 1 - t;
1726	double a = _simd_angle(q0, q1);
1727	double r = simd_recip(_simd_sinc(a));
1728	return simd_normalize(simd_quaternion(_simd_sinc(s*a)*r*s*q0.vector + _simd_sinc(t*a)*r*t*q1.vector));
1729	}
1730
1731	static SIMD_NOINLINE simd_quatd simd_slerp(simd_quatd q0, simd_quatd q1, double t) {
1732	if (simd_dot(q0, q1) >= 0)
1733	return _simd_slerp_internal(q0, q1, t);
1734	return _simd_slerp_internal(q0, simd_negate(q1), t);
1735	}
1736
1737	static SIMD_NOINLINE simd_quatd simd_slerp_longest(simd_quatd q0, simd_quatd q1, double t) {
1738	if (simd_dot(q0, q1) >= 0)
1739	return _simd_slerp_internal(q0, simd_negate(q1), t);
1740	return _simd_slerp_internal(q0, q1, t);
1741	}
1742
1743	/*! @discussion This function is an implementation detail and you should not
1744	* call it directly. It may be removed or modified in future versions of the
1745	* simd module. */
1746	static SIMD_NOINLINE simd_quatd _simd_intermediate(simd_quatd q0, simd_quatd q1, simd_quatd q2) {
1747	simd_quatd p0 = __tg_log(simd_mul(q0, simd_inverse(q1)));
1748	simd_quatd p2 = __tg_log(simd_mul(q2, simd_inverse(q1)));
1749	return simd_normalize(simd_mul(q1, __tg_exp(simd_mul(-0.25, simd_add(p0,p2)))));
1750	}
1751
1752	/*! @discussion This function is an implementation detail and you should not
1753	* call it directly. It may be removed or modified in future versions of the
1754	* simd module. */
1755	static SIMD_NOINLINE simd_quatd _simd_squad(simd_quatd q0, simd_quatd qa, simd_quatd qb, simd_quatd q1, double t) {
1756	simd_quatd r0 = _simd_slerp_internal(q0, q1, t);
1757	simd_quatd r1 = _simd_slerp_internal(qa, qb, t);
1758	return _simd_slerp_internal(r0, r1, 2*t*(1 - t));
1759	}
1760
1761	static SIMD_NOINLINE simd_quatd simd_spline(simd_quatd q0, simd_quatd q1, simd_quatd q2, simd_quatd q3, double t) {
1762	simd_quatd qa = _simd_intermediate(q0, q1, q2);
1763	simd_quatd qb = _simd_intermediate(q1, q2, q3);
1764	return _simd_squad(q1, qa, qb, q2, t);
1765	}
1766
1767	static SIMD_NOINLINE simd_quatd simd_bezier(simd_quatd q0, simd_quatd q1, simd_quatd q2, simd_quatd q3, double t) {
1768	simd_quatd q01 = _simd_slerp_internal(q0, q1, t);
1769	simd_quatd q12 = _simd_slerp_internal(q1, q2, t);
1770	simd_quatd q23 = _simd_slerp_internal(q2, q3, t);
1771	simd_quatd q012 = _simd_slerp_internal(q01, q12, t);
1772	simd_quatd q123 = _simd_slerp_internal(q12, q23, t);
1773	return _simd_slerp_internal(q012, q123, t);
1774	}
1775
1776	#ifdef __cplusplus
1777	} /* extern "C" */
1778	#endif /* __cplusplus */
1779	#endif /* SIMD_COMPILER_HAS_REQUIRED_FEATURES */
1780	#endif /* SIMD_QUATERNIONS */