forked from apple/swift-numerics
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Quaternion+ElementaryFunctions.swift
498 lines (474 loc) · 18.8 KB
/
Quaternion+ElementaryFunctions.swift
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
//===--- Quaternion+ElementaryFunctions.swift -----------------*- swift -*-===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2019 - 2022 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See https://swift.org/LICENSE.txt for license information
// See https://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//
// (r + xi + yj + zk) is a common representation that is often seen for
// quaternions. However, when we want to expand the elementary functions of
// quaternions in terms of real operations it is almost always easier to view
// them as real part (r) and imaginary vector part (v),
// i.e: r + xi + yj + zk = r + v; and so we diverge a little from the
// representation that is used in the documentation in other files and use this
// notation of quaternions in (internal) comments of the following functions.
//
// Quaternionic elementary functions have many similarities with elementary
// functions of complex numbers and their definition in terms of real
// operations. Therefore, if you make a modification to one of the following
// functions, you should almost surely make a parallel modification to the same
// elementary function of complex numbers.
import RealModule
extension Quaternion: ElementaryFunctions {
// MARK: - exp-like functions
@inlinable
public static func exp(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the `Real` operations `exp`, `cos` and `sin` (`let θ = ||v||`):
//
// ```
// exp(r + v) = exp(r) exp(v)
// = exp(r) (cos(θ) + (v/θ) sin(θ))
// ```
//
// Note that naive evaluation of this expression in floating-point would be
// prone to premature overflow, since `cos` and `sin` both have magnitude
// less than 1 for most inputs (i.e. `exp(r)` may be infinity when
// `exp(r) cos(||v||)` would not be).
guard q.isFinite else { return q }
let (â, θ) = q.imaginary.unitAxisAndLength
let rotation = Quaternion(halfAngle: θ, unitAxis: â)
// If real < log(greatestFiniteMagnitude), then exp(real) does not overflow.
// To protect ourselves against sketchy log or exp implementations in
// an unknown host library, or slight rounding disagreements between
// the two, subtract one from the bound for a little safety margin.
guard q.real < RealType.log(.greatestFiniteMagnitude) - 1 else {
let halfScale = RealType.exp(q.real/2)
return rotation.multiplied(by: halfScale).multiplied(by: halfScale)
}
return rotation.multiplied(by: .exp(q.real))
}
@inlinable
public static func expMinusOne(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the `Real` operations `exp`, `cos` and `sin` (`let θ = ||v||`):
//
// ```
// exp(r + v) - 1 = exp(r) exp(v) - 1
// = exp(r) (cos(θ) + (v/θ) sin(θ)) - 1
// = exp(r) cos(θ) + exp(r) (v/θ) sin(θ) - 1
// = (exp(r) cos(θ) - 1) + exp(r) (v/θ) sin(θ)
// -------- u --------
// ```
//
// Note that the imaginary part is just the usual exp(x) sin(y);
// the only trick is computing the real part ("u"):
//
// ```
// u = exp(r) cos(θ) - 1
// = exp(r) cos(θ) - cos(θ) + cos(θ) - 1
// = (exp(r) - 1) cos(θ) + (cos(θ) - 1)
// = expMinusOne(r) cos(θ) + cosMinusOne(θ)
// ```
//
// See `expMinusOne` on complex numbers for error bounds.
guard q.isFinite else { return q }
let (â, θ) = q.imaginary.unitAxisAndLength
// If exp(q) is close to the overflow boundary, we don't need to
// worry about the "MinusOne" part of this function; we're just
// computing exp(q). (Even when θ is near a multiple of π/2,
// it can't be close enough to overcome the scaling from exp(r),
// so the -1 term is _always_ negligable).
guard q.real < RealType.log(.greatestFiniteMagnitude) - 1 else {
let halfScale = RealType.exp(q.real/2)
let rotation = Quaternion(halfAngle: θ, unitAxis: â)
return rotation.multiplied(by: halfScale).multiplied(by: halfScale)
}
return Quaternion(
real: RealType._mulAdd(.cos(θ), .expMinusOne(q.real), .cosMinusOne(θ)),
imaginary: â * .exp(q.real) * .sin(θ)
)
}
@inlinable
public static func cosh(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// trigonometric `Real` operations (`let θ = ||v||`):
//
// ```
// cosh(q) = (exp(q) + exp(-q)) / 2
// = cosh(r) cos(θ) + (v/θ) sinh(r) sin(θ)
// ```
//
// Like exp, cosh is entire, so we do not need to worry about where
// branch cuts fall. Also like exp, cancellation never occurs in the
// evaluation of the naive expression, so all we need to be careful
// about is the behavior near the overflow boundary.
//
// Fortunately, if |r| >= -log(ulpOfOne), cosh(r) and sinh(r) are
// both just exp(|r|)/2, and we already know how to compute that.
//
// This function and sinh should stay in sync; if you make a
// modification here, you should almost surely make a parallel
// modification to sinh below.
guard q.isFinite else { return q }
let (â, θ) = q.imaginary.unitAxisAndLength
guard q.real.magnitude < -RealType.log(.ulpOfOne) else {
let rotation = Quaternion(halfAngle: θ, unitAxis: â)
let firstScale = RealType.exp(q.real.magnitude/2)
return rotation.multiplied(by: firstScale).multiplied(by: firstScale/2)
}
return Quaternion(
real: .cosh(q.real) * .cos(θ),
imaginary: â * .sinh(q.real) * .sin(θ)
)
}
@inlinable
public static func sinh(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// trigonometric `Real` operations (`let θ = ||v||`):
//
// ```
// sinh(q) = (exp(q) - exp(-q)) / 2
// = sinh(r) cos(θ) + (v/θ) cosh(r) sin(θ)
// ```
guard q.isFinite else { return q }
let (â, θ) = q.imaginary.unitAxisAndLength
guard q.real.magnitude < -RealType.log(.ulpOfOne) else {
let rotation = Quaternion(halfAngle: θ, unitAxis: â)
let firstScale = RealType.exp(q.real.magnitude/2)
let secondScale = RealType(signOf: q.real, magnitudeOf: firstScale/2)
return rotation.multiplied(by: firstScale).multiplied(by: secondScale)
}
return Quaternion(
real: .sinh(q.real) * .cos(θ),
imaginary: â * .cosh(q.real) * .sin(θ)
)
}
@inlinable
public static func tanh(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// quaternionic `sinh` and `cosh` operations:
//
// ```
// tanh(q) = sinh(q) / cosh(q)
// ```
guard q.isFinite else { return q }
// Note that when |r| is larger than -log(.ulpOfOne),
// sinh(r + v) == ±cosh(r + v), so tanh(r + v) is just ±1.
guard q.real.magnitude < -RealType.log(.ulpOfOne) else {
return Quaternion(
real: RealType(signOf: q.real, magnitudeOf: 1),
imaginary:
RealType(signOf: q.components.x, magnitudeOf: 0),
RealType(signOf: q.components.y, magnitudeOf: 0),
RealType(signOf: q.components.z, magnitudeOf: 0)
)
}
return sinh(q) / cosh(q)
}
@inlinable
public static func cos(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// quaternionic `cosh` operations (`let θ = ||v||`):
//
// ```
// cos(q) = cosh(q * (v/θ)))
// ```
let (â,_) = q.imaginary.unitAxisAndLength
let p = Quaternion(imaginary: â)
return cosh(q * p)
}
@inlinable
public static func sin(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// quaternionic `sinh` operations (`let θ = ||v||`):
//
// ```
// sin(q) = -(v/θ) * sinh(q * (v/θ)))
// ```
let (â,_) = q.imaginary.unitAxisAndLength
let p = Quaternion(imaginary: â)
return -p * sinh(q * p)
}
@inlinable
public static func tan(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// quaternionic `tanh` operations (`let θ = ||v||`):
//
// ```
// tan(q) = -(v/θ) * tanh(q * (v/θ)))
// ```
let (â,_) = q.imaginary.unitAxisAndLength
let p = Quaternion(imaginary: â)
return -p * tanh(q * p)
}
// MARK: - log-like functions
@inlinable
public static func log(_ q: Quaternion) -> Quaternion {
// If q is zero or infinite, the phase is undefined, so the result is
// the single exceptional value.
guard q.isFinite && !q.isZero else { return .infinity }
// Having eliminated non-finite values and zero, the imaginary part is
// straightforward:
let (â, θ) = q.imaginary.unitAxisAndLength
let argument = RealType.atan2(y: θ, x: q.real)
let imaginary = â * argument
// The real part of the result is trickier and we employ the same approach
// as we did for the complex numbers logarithm to improve the relative error
// bounds (`Complex.log`). There you may also find a lot more details to
// the following approach.
//
// To handle very large arguments without overflow, _rescale the problem.
// This is done by finding whichever part has greater magnitude, and
// dividing through by it.
let u = max(q.real.magnitude, θ)
let v = min(q.real.magnitude, θ)
// Now expand out log |w|:
//
// log |w| = log(u² + v²)/2
// = log(u + log(onePlus: (u/v)²))/2
//
// This handles overflow well, because log(u) is finite for every finite u,
// and we have 0 ≤ v/u ≤ 1. Unfortunately, it does not handle all points
// close to the unit circle so well, as cancellation might occur.
//
// We are not trying for sub-ulp accuracy, just a good relative error
// bound, so for our purposes it suffices to have log u dominate the
// result:
if u >= 1 || u >= RealType._mulAdd(u,u,v*v) {
let r = v / u
return Quaternion(real: .log(u) + .log(onePlus: r*r)/2, imaginary: imaginary)
}
// Here we're in the tricky case; cancellation is likely to occur.
// Instead of the factorization used above, we will want to evaluate
// log(onePlus: u² + v² - 1)/2. This all boils down to accurately
// evaluating u² + v² - 1.
let (a,b) = Augmented.product(u, u)
let (c,d) = Augmented.product(v, v)
var (s,e) = Augmented.sum(large: -1, small: a)
// Now we are ready to assemble the result. If cancellation happens,
// then |c| > |e| > |b| > |d|, so this assembly order is safe.
s = (s + c) + e + b + d
return Quaternion(real: .log(onePlus: s)/2, imaginary: imaginary)
}
@inlinable
public static func log(onePlus q: Quaternion) -> Quaternion {
// If either |r| or ||v||₁ is bounded away from the origin, we don't need
// any extra precision, and can just literally compute log(1+z). Note
// that this includes part of the sphere |1+q| = 1 where log(onePlus:)
// vanishes (where r <= -0.5), but on this portion of the sphere 1+r
// is always exact by Sterbenz' lemma, so as long as log( ) produces
// a good result, log(1+q) will too.
guard 2*q.real.magnitude < 1 && q.imaginary.oneNorm < 1 else {
return log(.one + q)
}
// q is in (±0.5, ±1), so we need to evaluate more carefully.
// The imaginary part is straightforward:
let argument = (.one + q).halfAngle
let (â,_) = q.imaginary.unitAxisAndLength
let imaginary = â * argument
// For the real part, we _could_ use the same approach that we do for
// log( ), but we'd need an extra-precise (1+r)², which can potentially
// be quite painful to calculate. Instead, we can use an approach that
// NevinBR suggested on the Swift forums for complex numbers:
//
// Re(log(1+q)) = (log(1+q) + log(1+q̅)) / 2
// = log((1+q)(1+q̅)) / 2
// = log(1 + q + q̅ + qq̅) / 2
// = log(1 + 2r + r² + v²)) / 2
// = log(1 + (2+r)r + v²)) / 2
// = log(1 + (2+r)r + x² + y² + z²)) / 2
// = log(onePlus: (2+r)r + x² + y² + z²) / 2
//
// So now we need to evaluate (2+r)r + x² + y² + z² accurately.
// To do this, we employ augmented arithmetic
// (2+r)r + x² + y² + z²
// --↓--
let rp2 = Augmented.sum(large: 2, small: q.real) // Known that 2 > |r|
var (head, δ) = Augmented.product(q.real, rp2.head)
var tail = δ
// head + x² + y² + z²
// ----↓----
let x² = Augmented.product(q.imaginary.x, q.imaginary.x)
(head, δ) = Augmented.sum(head, x².head)
tail += (δ + x².tail)
// head + y² + z²
// ----↓----
let y² = Augmented.product(q.imaginary.y, q.imaginary.y)
(head, δ) = Augmented.sum(head, y².head)
tail += (δ + y².tail)
// head + z²
// ----↓----
let z² = Augmented.product(q.imaginary.z, q.imaginary.z)
(head, δ) = Augmented.sum(head, z².head)
tail += (δ + z².tail)
let s = (head + tail).addingProduct(q.real, rp2.tail)
return Quaternion(real: .log(onePlus: s)/2, imaginary: imaginary)
}
@inlinable
public static func acos(_ q: Quaternion) -> Quaternion {
let (â, θ) = (sqrt(1+q).conjugate * sqrt(1-q)).imaginary.unitAxisAndLength
return Quaternion(
real: 2*RealType.atan2(y: sqrt(1-q).real, x: sqrt(1+q).real),
imaginary: â * RealType.asinh(θ)
)
}
@inlinable
public static func asin(_ q: Quaternion) -> Quaternion {
let (â, θ) = (sqrt(1-q).conjugate * sqrt(1+q)).imaginary.unitAxisAndLength
return Quaternion(
real: RealType.atan2(y: q.real, x: (sqrt(1-q) * sqrt(1+q)).real),
imaginary: â * RealType.asinh(θ)
)
}
@inlinable
public static func atan(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the quaternionic `atanh` operation (`let θ = ||v||`):
//
// ```
// atan(q) = -(v/θ) * atanh(q * (v/θ))
// ```
let (â, _) = q.imaginary.unitAxisAndLength
let p = Quaternion(imaginary: â)
return -p * .atanh(q * p)
}
@inlinable
public static func acosh(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the quaternionic `acos` operation (`let θ = ||v||`):
//
// ```
// acosh(q) = (v/θ) * acos(q)
// ```
let (â,_) = q.imaginary.unitAxisAndLength
let p = Quaternion(imaginary: â)
return p * acos(q)
}
@inlinable
public static func asinh(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the quaternionic `asin` operation (`let θ = ||v||`):
//
// ```
// sin(q) = -(v/θ) * asin(q * (v/θ)))
// ```
let (â,_) = q.imaginary.unitAxisAndLength
let p = Quaternion(imaginary: â)
return -p * .asin(q * p)
}
@inlinable
public static func atanh(_ q: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the quaternionic `log` operation:
//
// ```
// atanh(q) = (log(1 + q) - log(1 - q))/2
// = (log(onePlus: q) - log(onePlus: -q))/2
// ```
(log(onePlus: q) - log(onePlus:-q))/2
}
// MARK: - pow-like functions
@inlinable
public static func pow(_ q: Quaternion, _ p: Quaternion) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the quaternionic `exp` and `log` operations:
//
// ```
// pow(q, p) = exp(log(pow(q, p)))
// = exp(p * log(q))
// ```
guard !q.isZero else { return .zero }
return exp(p * log(q))
}
@inlinable
public static func pow(_ q: Quaternion, _ n: Int) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the quaternionic `exp` and `log` operations:
//
// ```
// pow(q, n) = exp(log(pow(q, n)))
// = exp(log(q) * n)
// ```
guard !q.isZero else { return .zero }
// TODO: this implementation is not quite correct, because n may be
// rounded in conversion to RealType. This only effects very extreme
// cases, so we'll leave it alone for now.
return exp(log(q).multiplied(by: RealType(n)))
}
@inlinable
public static func sqrt(_ q: Quaternion) -> Quaternion<RealType> {
let lengthSquared = q.lengthSquared
if lengthSquared.isNormal {
// If |q|^2 doesn't overflow, then define s and t by (`let θ = ||v||`):
//
// s = sqrt((|q|+|r|) / 2)
// t = θ/2s
//
// If r is positive, the result is just w = (s, (v/θ) * t). If r is negative,
// the result is (|t|, (v/θ) * copysign(s, θ)) instead.
let (â, θ) = q.imaginary.unitAxisAndLength
let norm: RealType = .sqrt(lengthSquared)
let s: RealType = .sqrt((norm + q.real.magnitude) / 2)
let t: RealType = θ / (2*s)
if q.real.sign == .plus {
return Quaternion(
real: s,
imaginary: â * t)
} else {
return Quaternion(
real: t.magnitude,
imaginary: â * RealType(signOf: θ, magnitudeOf: s)
)
}
}
// Handle edge cases:
guard !q.isZero else {
return Quaternion(
real: 0,
imaginary:
RealType(signOf: q.components.x, magnitudeOf: 0),
RealType(signOf: q.components.y, magnitudeOf: 0),
RealType(signOf: q.components.z, magnitudeOf: 0)
)
}
guard q.isFinite else { return q }
// q is finite but badly-scaled. Rescale and replay by factoring out
// the larger of r and v.
let scale = q.magnitude
return Quaternion.sqrt(q.divided(by: scale)).multiplied(by: .sqrt(scale))
}
@inlinable
public static func root(_ q: Quaternion, _ n: Int) -> Quaternion {
// Mathematically, this operation can be expanded in terms of
// the quaternionic `exp` and `log` operations:
//
// ```
// root(q, n) = q^(1/n) = exp(log(q^(1/n)))
// = exp(log(q) / n)
// ```
guard !q.isZero else { return .zero }
// TODO: this implementation is not quite correct, because n may be
// rounded in conversion to RealType. This only effects very extreme
// cases, so we'll leave it alone for now.
return exp(log(q).divided(by: RealType(n)))
}
}
extension SIMD3 where Scalar: FloatingPoint {
/// Returns the normalized axis and the length of this vector.
@_alwaysEmitIntoClient
fileprivate var unitAxisAndLength: (Self, Scalar) {
if self == .zero {
return (SIMD3(
Scalar(signOf: x, magnitudeOf: 0),
Scalar(signOf: y, magnitudeOf: 0),
Scalar(signOf: z, magnitudeOf: 0)
), .zero)
}
return (self/length, length)
}
}