master
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
15extern "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. */
27static 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. */
35static 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. */
44static 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`. */
50static 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. */
60static 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. */
66static 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 . */
73static SIMD_NOINLINE simd_quath simd_quaternion(simd_half4x4 matrix);
74
75/*! @abstract The real (scalar) part of the quaternion `q`. */
76static 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`. */
81static 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`. */
86static 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. */
90static inline SIMD_CFUNC simd_half3 simd_axis(simd_quath q);
91
92/*! @abstract The sum of the quaternions `p` and `q`. */
93static inline SIMD_CFUNC simd_quath simd_add(simd_quath p, simd_quath q);
94
95/*! @abstract The difference of the quaternions `p` and `q`. */
96static inline SIMD_CFUNC simd_quath simd_sub(simd_quath p, simd_quath q);
97
98/*! @abstract The product of the quaternions `p` and `q`. */
99static 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`. */
102static inline SIMD_CFUNC simd_quath simd_mul(simd_quath q, _Float16 a);
103
104/*! @abstract The quaternion `q` scaled by the real value `a`. */
105static inline SIMD_CFUNC simd_quath simd_mul(_Float16 a, simd_quath q);
106
107/*! @abstract The conjugate of the quaternion `q`. */
108static inline SIMD_CFUNC simd_quath simd_conjugate(simd_quath q);
109
110/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
111static inline SIMD_CFUNC simd_quath simd_inverse(simd_quath q);
112
113/*! @abstract The negation (additive inverse) of the quaternion `q`. */
114static 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. */
118static inline SIMD_CFUNC _Float16 simd_dot(simd_quath p, simd_quath q);
119
120/*! @abstract The length of the quaternion `q`. */
121static inline SIMD_CFUNC _Float16 simd_length(simd_quath q);
122
123/*! @abstract The unit quaternion obtained by normalizing `q`. */
124static inline SIMD_CFUNC simd_quath simd_normalize(simd_quath q);
125
126/*! @abstract Rotates the vector `v` by the quaternion `q`. */
127static 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; */
146static 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. */
150static 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`. */
154static 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`. */
158static 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. */
166static 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. */
175static 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
181namespace 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
288extern "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. */
298static 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. */
303static inline SIMD_CFUNC simd_quath _simd_quaternion(simd_half3 imag, _Float16 real) {
304 return simd_quaternion(simd_make_half4(imag, real));
305}
306
307static 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
311static inline SIMD_CFUNC _Float16 simd_angle(simd_quath q) {
312 return 2*(_Float16)atan2f(simd_length(q.vector.xyz), q.vector.w);
313}
314
315static inline SIMD_CFUNC simd_half3 simd_axis(simd_quath q) {
316 return simd_normalize(q.vector.xyz);
317}
318
319static inline SIMD_CFUNC simd_quath simd_add(simd_quath p, simd_quath q) {
320 return simd_quaternion(p.vector + q.vector);
321}
322
323static inline SIMD_CFUNC simd_quath simd_sub(simd_quath p, simd_quath q) {
324 return simd_quaternion(p.vector - q.vector);
325}
326
327static 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
335static inline SIMD_CFUNC simd_quath simd_mul(simd_quath q, _Float16 a) {
336 return simd_quaternion(a * q.vector);
337}
338
339static inline SIMD_CFUNC simd_quath simd_mul(_Float16 a, simd_quath q) {
340 return simd_mul(q,a);
341}
342
343static inline SIMD_CFUNC simd_quath simd_conjugate(simd_quath q) {
344 return simd_quaternion(q.vector * (simd_half4){-1,-1,-1, 1});
345}
346
347static 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
351static inline SIMD_CFUNC simd_quath simd_negate(simd_quath q) {
352 return simd_quaternion(-q.vector);
353}
354
355static inline SIMD_CFUNC _Float16 simd_dot(simd_quath p, simd_quath q) {
356 return simd_dot(p.vector, q.vector);
357}
358
359static inline SIMD_CFUNC _Float16 simd_length(simd_quath q) {
360 return simd_length(q.vector);
361}
362
363static 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. */
378static 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
386static 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
396static 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
403static 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. */
422static 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
427static 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
459static 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
493static 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. */
532static 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. */
541static 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. */
552static 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
559static 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
565static 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. */
574static 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. */
583static 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
589static 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
595static 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. */
613static 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. */
621static 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. */
630static 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`. */
636static 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. */
646static 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. */
652static 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 . */
659static SIMD_NOINLINE simd_quatf simd_quaternion(simd_float4x4 matrix);
660
661/*! @abstract The real (scalar) part of the quaternion `q`. */
662static 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`. */
667static 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`. */
672static 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. */
676static inline SIMD_CFUNC simd_float3 simd_axis(simd_quatf q);
677
678/*! @abstract The sum of the quaternions `p` and `q`. */
679static inline SIMD_CFUNC simd_quatf simd_add(simd_quatf p, simd_quatf q);
680
681/*! @abstract The difference of the quaternions `p` and `q`. */
682static inline SIMD_CFUNC simd_quatf simd_sub(simd_quatf p, simd_quatf q);
683
684/*! @abstract The product of the quaternions `p` and `q`. */
685static 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`. */
688static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf q, float a);
689
690/*! @abstract The quaternion `q` scaled by the real value `a`. */
691static inline SIMD_CFUNC simd_quatf simd_mul(float a, simd_quatf q);
692
693/*! @abstract The conjugate of the quaternion `q`. */
694static inline SIMD_CFUNC simd_quatf simd_conjugate(simd_quatf q);
695
696/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
697static inline SIMD_CFUNC simd_quatf simd_inverse(simd_quatf q);
698
699/*! @abstract The negation (additive inverse) of the quaternion `q`. */
700static 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. */
704static inline SIMD_CFUNC float simd_dot(simd_quatf p, simd_quatf q);
705
706/*! @abstract The length of the quaternion `q`. */
707static inline SIMD_CFUNC float simd_length(simd_quatf q);
708
709/*! @abstract The unit quaternion obtained by normalizing `q`. */
710static inline SIMD_CFUNC simd_quatf simd_normalize(simd_quatf q);
711
712/*! @abstract Rotates the vector `v` by the quaternion `q`. */
713static 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; */
732static 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. */
736static 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`. */
740static 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`. */
744static 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. */
752static 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. */
761static 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
767namespace 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
874extern "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. */
884static 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. */
889static inline SIMD_CFUNC simd_quatf _simd_quaternion(simd_float3 imag, float real) {
890 return simd_quaternion(simd_make_float4(imag, real));
891}
892
893static 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
897static inline SIMD_CFUNC float simd_angle(simd_quatf q) {
898 return 2*atan2(simd_length(q.vector.xyz), q.vector.w);
899}
900
901static inline SIMD_CFUNC simd_float3 simd_axis(simd_quatf q) {
902 return simd_normalize(q.vector.xyz);
903}
904
905static inline SIMD_CFUNC simd_quatf simd_add(simd_quatf p, simd_quatf q) {
906 return simd_quaternion(p.vector + q.vector);
907}
908
909static inline SIMD_CFUNC simd_quatf simd_sub(simd_quatf p, simd_quatf q) {
910 return simd_quaternion(p.vector - q.vector);
911}
912
913static 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
921static inline SIMD_CFUNC simd_quatf simd_mul(simd_quatf q, float a) {
922 return simd_quaternion(a * q.vector);
923}
924
925static inline SIMD_CFUNC simd_quatf simd_mul(float a, simd_quatf q) {
926 return simd_mul(q,a);
927}
928
929static inline SIMD_CFUNC simd_quatf simd_conjugate(simd_quatf q) {
930 return simd_quaternion(q.vector * (simd_float4){-1,-1,-1, 1});
931}
932
933static 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
937static inline SIMD_CFUNC simd_quatf simd_negate(simd_quatf q) {
938 return simd_quaternion(-q.vector);
939}
940
941static inline SIMD_CFUNC float simd_dot(simd_quatf p, simd_quatf q) {
942 return simd_dot(p.vector, q.vector);
943}
944
945static inline SIMD_CFUNC float simd_length(simd_quatf q) {
946 return simd_length(q.vector);
947}
948
949static 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. */
964static 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
972static 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
982static 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
989static 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. */
1008static 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
1013static 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
1045static 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
1079static 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. */
1118static 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. */
1127static 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. */
1138static 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
1145static 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
1151static 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. */
1160static 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. */
1169static 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
1175static 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
1181static 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. */
1199static 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. */
1207static 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. */
1216static 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`. */
1222static 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. */
1232static 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. */
1238static 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 . */
1245static SIMD_NOINLINE simd_quatd simd_quaternion(simd_double4x4 matrix);
1246
1247/*! @abstract The real (scalar) part of the quaternion `q`. */
1248static 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`. */
1253static 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`. */
1258static 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. */
1262static inline SIMD_CFUNC simd_double3 simd_axis(simd_quatd q);
1263
1264/*! @abstract The sum of the quaternions `p` and `q`. */
1265static inline SIMD_CFUNC simd_quatd simd_add(simd_quatd p, simd_quatd q);
1266
1267/*! @abstract The difference of the quaternions `p` and `q`. */
1268static inline SIMD_CFUNC simd_quatd simd_sub(simd_quatd p, simd_quatd q);
1269
1270/*! @abstract The product of the quaternions `p` and `q`. */
1271static 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`. */
1274static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd q, double a);
1275
1276/*! @abstract The quaternion `q` scaled by the real value `a`. */
1277static inline SIMD_CFUNC simd_quatd simd_mul(double a, simd_quatd q);
1278
1279/*! @abstract The conjugate of the quaternion `q`. */
1280static inline SIMD_CFUNC simd_quatd simd_conjugate(simd_quatd q);
1281
1282/*! @abstract The (multiplicative) inverse of the quaternion `q`. */
1283static inline SIMD_CFUNC simd_quatd simd_inverse(simd_quatd q);
1284
1285/*! @abstract The negation (additive inverse) of the quaternion `q`. */
1286static 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. */
1290static inline SIMD_CFUNC double simd_dot(simd_quatd p, simd_quatd q);
1291
1292/*! @abstract The length of the quaternion `q`. */
1293static inline SIMD_CFUNC double simd_length(simd_quatd q);
1294
1295/*! @abstract The unit quaternion obtained by normalizing `q`. */
1296static inline SIMD_CFUNC simd_quatd simd_normalize(simd_quatd q);
1297
1298/*! @abstract Rotates the vector `v` by the quaternion `q`. */
1299static 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; */
1318static 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. */
1322static 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`. */
1326static 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`. */
1330static 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. */
1338static 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. */
1347static 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
1353namespace 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
1460extern "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. */
1470static 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. */
1475static inline SIMD_CFUNC simd_quatd _simd_quaternion(simd_double3 imag, double real) {
1476 return simd_quaternion(simd_make_double4(imag, real));
1477}
1478
1479static 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
1483static inline SIMD_CFUNC double simd_angle(simd_quatd q) {
1484 return 2*atan2(simd_length(q.vector.xyz), q.vector.w);
1485}
1486
1487static inline SIMD_CFUNC simd_double3 simd_axis(simd_quatd q) {
1488 return simd_normalize(q.vector.xyz);
1489}
1490
1491static inline SIMD_CFUNC simd_quatd simd_add(simd_quatd p, simd_quatd q) {
1492 return simd_quaternion(p.vector + q.vector);
1493}
1494
1495static inline SIMD_CFUNC simd_quatd simd_sub(simd_quatd p, simd_quatd q) {
1496 return simd_quaternion(p.vector - q.vector);
1497}
1498
1499static 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
1507static inline SIMD_CFUNC simd_quatd simd_mul(simd_quatd q, double a) {
1508 return simd_quaternion(a * q.vector);
1509}
1510
1511static inline SIMD_CFUNC simd_quatd simd_mul(double a, simd_quatd q) {
1512 return simd_mul(q,a);
1513}
1514
1515static inline SIMD_CFUNC simd_quatd simd_conjugate(simd_quatd q) {
1516 return simd_quaternion(q.vector * (simd_double4){-1,-1,-1, 1});
1517}
1518
1519static 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
1523static inline SIMD_CFUNC simd_quatd simd_negate(simd_quatd q) {
1524 return simd_quaternion(-q.vector);
1525}
1526
1527static inline SIMD_CFUNC double simd_dot(simd_quatd p, simd_quatd q) {
1528 return simd_dot(p.vector, q.vector);
1529}
1530
1531static inline SIMD_CFUNC double simd_length(simd_quatd q) {
1532 return simd_length(q.vector);
1533}
1534
1535static 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. */
1550static 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
1558static 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
1568static 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
1575static 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. */
1594static 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
1599static 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
1631static 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
1665static 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. */
1704static 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. */
1713static 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. */
1724static 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
1731static 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
1737static 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. */
1746static 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. */
1755static 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
1761static 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
1767static 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 */