alice_physics/math.rs
1//! Deterministic Fixed-Point Mathematics - SIMD Optimized Edition
2//!
3//! > "God does not play dice with floating point numbers."
4//!
5//! # Overview
6//!
7//! This module provides bit-exact arithmetic that produces identical results
8//! on x86, ARM, WASM, or any other platform. No IEEE 754 floating point.
9//!
10//! # Features
11//!
12//! - **`simd` feature**: exposes the `*_simd` / `dot_batch_4` API surface and
13//! `SIMD_WIDTH`. As of 1.2.0 every one of these is **scalar-equivalent**:
14//! SSE2/AVX2 have no 128-bit multiply and cannot propagate the lo→hi carry a
15//! Fix128 add needs, so the intrinsic paths were never faster than the
16//! ADC chain LLVM already emits. The feature is kept so downstream code can
17//! target a future AVX-512 / NEON batch path without an API change.
18//! - **Determinism**: All operations produce bit-identical results across platforms
19//! - **Zero-allocation**: Hot paths use no heap allocation
20//!
21//! # Types
22//!
23//! - `Fix128` (I64F64): 128-bit fixed-point with 64 integer bits, 64 fractional bits
24//! - `Vec3Fix`: 3D vector using Fix128 components (SIMD-accelerated dot product)
25//! - `QuatFix`: Quaternion using Fix128 components
26//! - `Mat3Fix`: 3x3 matrix for inertia tensors
27//!
28//! # Precision
29//!
30//! - Range: ±9.2 × 10^18 (meters)
31//! - Precision: ~5.4 × 10^-20 (meters)
32//! - From subatomic particles to galactic scales with uniform precision
33
34use core::cmp::Ordering;
35use core::ops::{Add, Div, Mul, Neg, Sub};
36
37// ============================================================================
38// Fix128 (I64F64) - 128-bit Fixed-Point Number
39// ============================================================================
40
41/// 128-bit fixed-point number (64 integer bits, 64 fractional bits)
42///
43/// Internal representation: `value = raw / 2^64`
44///
45/// This provides:
46/// - Range: ±9.2 × 10^18
47/// - Precision: ~5.4 × 10^-20
48/// - Bit-exact arithmetic across all platforms
49#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Hash)]
50#[repr(C)]
51pub struct Fix128 {
52 /// Raw 128-bit value (stored as two i64s for portability)
53 /// Interpretation: (hi << 64) | lo as a signed 128-bit integer
54 /// The decimal point is between hi and lo
55 pub hi: i64, // Integer part (signed)
56 /// Fractional part (64-bit unsigned)
57 pub lo: u64, // Fractional part
58}
59
60impl Fix128 {
61 /// Zero constant
62 pub const ZERO: Self = Self { hi: 0, lo: 0 };
63
64 /// One constant (1.0)
65 pub const ONE: Self = Self { hi: 1, lo: 0 };
66
67 /// Negative one (-1.0)
68 pub const NEG_ONE: Self = Self { hi: -1, lo: 0 };
69
70 /// Pi (π) - precomputed to full precision
71 /// π ≈ 3.14159265358979323846...
72 pub const PI: Self = Self {
73 hi: 3,
74 lo: 0x243F6A8885A308D3, // Fractional part of π
75 };
76
77 /// Half Pi (π/2)
78 pub const HALF_PI: Self = Self {
79 hi: 1,
80 lo: 0x921FB54442D18469, // Fractional part of π/2
81 };
82
83 /// Two Pi (2π)
84 pub const TWO_PI: Self = Self {
85 hi: 6,
86 lo: 0x487ED5110B4611A6, // Fractional part of 2π
87 };
88
89 /// Create from integer
90 #[inline]
91 #[must_use]
92 pub const fn from_int(n: i64) -> Self {
93 Self { hi: n, lo: 0 }
94 }
95
96 /// Create from raw parts (hi = integer, lo = fraction)
97 #[inline]
98 #[must_use]
99 pub const fn from_raw(hi: i64, lo: u64) -> Self {
100 Self { hi, lo }
101 }
102
103 /// Create from f64 (for initialization only, not deterministic!)
104 #[must_use]
105 pub fn from_f64(f: f64) -> Self {
106 let hi = f as i64; // truncation toward zero
107 let frac = f - (hi as f64);
108 let abs_frac = frac.abs();
109 let lo = (abs_frac * (1u128 << 64) as f64) as u64;
110 if f < 0.0 && lo != 0 {
111 Self {
112 hi: hi - 1,
113 lo: (!lo).wrapping_add(1),
114 }
115 } else {
116 Self { hi, lo }
117 }
118 }
119
120 /// Convert to f64 (for debugging only, not deterministic!)
121 #[must_use]
122 pub fn to_f64(self) -> f64 {
123 self.hi as f64 + (self.lo as f64 / (1u128 << 64) as f64)
124 }
125
126 /// Create from f32 (for SDF bridge, not deterministic!)
127 #[must_use]
128 pub fn from_f32(f: f32) -> Self {
129 Self::from_f64(f as f64)
130 }
131
132 /// Convert to f32 (for SDF bridge, not deterministic!)
133 #[must_use]
134 pub fn to_f32(self) -> f32 {
135 self.to_f64() as f32
136 }
137
138 /// Create from fraction (numerator / denominator)
139 #[must_use]
140 pub fn from_ratio(num: i64, denom: i64) -> Self {
141 if denom == 0 {
142 return Self::ZERO;
143 }
144
145 let neg = (num < 0) != (denom < 0);
146 let num = num.unsigned_abs() as u128;
147 let denom = denom.unsigned_abs() as u128;
148
149 // Compute (num << 64) / denom
150 let scaled = (num << 64) / denom;
151 let hi = (scaled >> 64) as i64;
152 let lo = scaled as u64;
153
154 if neg {
155 Self { hi, lo }.neg()
156 } else {
157 Self { hi, lo }
158 }
159 }
160
161 /// Absolute value
162 ///
163 /// # Claims
164 /// - `|x|` for every value except the most negative one
165 /// - saturating: `|-2^63| = 2^63` is not representable, so `abs` of the most
166 /// negative value (`hi == i64::MIN`, `lo == 0`) is the largest representable
167 /// value instead of wrapping back to itself; the result is never negative
168 /// (AUD-A-S1W5-028, AUD-A-S3W3-010, AUD-A-S5W2-017)
169 #[inline]
170 #[must_use]
171 pub fn abs(self) -> Self {
172 if self.hi == i64::MIN && self.lo == 0 {
173 Self {
174 hi: i64::MAX,
175 lo: u64::MAX,
176 }
177 } else if self.hi < 0 || (self.hi == 0 && self.lo == 0) {
178 self.neg()
179 } else {
180 self
181 }
182 }
183
184 /// Check if negative
185 #[inline]
186 #[must_use]
187 pub const fn is_negative(self) -> bool {
188 self.hi < 0
189 }
190
191 /// Check if zero
192 #[inline]
193 #[must_use]
194 pub const fn is_zero(self) -> bool {
195 self.hi == 0 && self.lo == 0
196 }
197
198 /// Floor (round toward negative infinity)
199 #[inline]
200 #[must_use]
201 pub const fn floor(self) -> Self {
202 Self { hi: self.hi, lo: 0 }
203 }
204
205 /// Ceiling (round toward positive infinity)
206 ///
207 /// # Claims
208 /// - an integer value is returned unchanged; otherwise the next integer up
209 /// - saturating: above `i64::MAX` (integer part `i64::MAX` with a fraction)
210 /// the next integer is not representable, and the result is the largest
211 /// representable value, which is still `>= self` (AUD-A-S1W5-028)
212 #[inline]
213 #[must_use]
214 pub const fn ceil(self) -> Self {
215 if self.lo == 0 {
216 self
217 } else if self.hi == i64::MAX {
218 Self {
219 hi: i64::MAX,
220 lo: u64::MAX,
221 }
222 } else {
223 Self {
224 hi: self.hi + 1,
225 lo: 0,
226 }
227 }
228 }
229
230 /// Square root, exact floor: `floor(sqrt(self))` in I64F64.
231 ///
232 /// Restoring digit-recurrence (one result bit per step) on the 192-bit
233 /// radicand `self << 64`, 96 fixed steps, so the result is the largest
234 /// `r` with `r * r <= self` — bit-identical on every platform and to the
235 /// pre-1.2.0 Newton-Raphson implementation (verified over 20 000 samples
236 /// plus edge values in `tests::sqrt_matches_newton_reference`).
237 ///
238 /// Before 1.2.0 this ran 64 Newton iterations, each calling the 64-step
239 /// long division, ≈ 4 096 inner steps per call (≈ 10 µs); 3-5 iterations
240 /// had already converged. The recurrence needs 96 shift/compare/subtract
241 /// steps (≈ 24× faster, see `benches/physics_bench.rs::fix128_sqrt`).
242 ///
243 /// Negative input returns `ZERO` (deterministic, no NaN).
244 ///
245 /// Deterministic: fixed number of iterations, integer-only.
246 #[must_use]
247 pub fn sqrt(self) -> Self {
248 if self.is_negative() || self.is_zero() {
249 return Self::ZERO;
250 }
251
252 // Radicand N = (hi:lo) << 64 as a 192-bit integer; result = isqrt(N),
253 // which is sqrt(value) * 2^64 = the I64F64 encoding of sqrt(value).
254 // Process 2 radicand bits per step from the top: the first 64 steps
255 // consume the 128 bits of (hi:lo), the remaining 32 steps consume the
256 // 64 appended zero bits.
257 let n = ((self.hi as u128) << 64) | (self.lo as u128);
258 let mut rem: u128 = 0;
259 let mut root: u128 = 0;
260 let mut i = 0u32;
261 while i < 96 {
262 // Next two radicand bits (MSB first). rem < 2*root + 1 < 2^97
263 // before the shift, so rem << 2 | bits < 2^99 never overflows u128.
264 let bits = if i < 64 { (n >> (126 - 2 * i)) & 3 } else { 0 };
265 rem = (rem << 2) | bits;
266 let trial = (root << 2) | 1;
267 if rem >= trial {
268 rem -= trial;
269 root = (root << 1) | 1;
270 } else {
271 root <<= 1;
272 }
273 i += 1;
274 }
275
276 Self {
277 hi: (root >> 64) as i64,
278 lo: root as u64,
279 }
280 }
281
282 /// Divide by 2 (bit shift, exact)
283 #[inline]
284 #[must_use]
285 pub const fn half(self) -> Self {
286 let hi = self.hi >> 1;
287 let lo = (self.lo >> 1) | ((self.hi as u64 & 1) << 63);
288 Self { hi, lo }
289 }
290
291 /// Arithmetic right shift of the full 128-bit value by `i` bits (exact `self / 2^i`,
292 /// rounding toward negative infinity).
293 ///
294 /// The single source of truth for the CORDIC `x >> i` / `y >> i` micro-rotations
295 /// (`cordic_sin_cos` and `cordic_atan`). Before 1.1.1 `cordic_atan` had its own copy
296 /// that dropped the bits carried from `hi` into `lo`, so `ONE >> 1` evaluated to `0`
297 /// instead of `0.5` and every `atan` / `atan2` result was off by up to ~0.17 rad.
298 #[inline]
299 #[must_use]
300 pub const fn shr_bits(self, i: u32) -> Self {
301 if i == 0 {
302 return self;
303 }
304 if i >= 128 {
305 let hi = self.hi >> 63;
306 return Self { hi, lo: hi as u64 };
307 }
308 if i >= 64 {
309 // hi の下位ビットが lo に降りる、hi は符号のみ残る
310 let lo = (self.hi >> (i - 64)) as u64;
311 return Self {
312 hi: self.hi >> 63,
313 lo,
314 };
315 }
316 Self {
317 hi: self.hi >> i,
318 lo: (self.lo >> i) | ((self.hi as u64) << (64 - i)),
319 }
320 }
321
322 /// Multiply by 2 (bit shift, exact)
323 #[inline]
324 #[must_use]
325 pub const fn double(self) -> Self {
326 let hi = (self.hi << 1) | ((self.lo >> 63) as i64);
327 let lo = self.lo << 1;
328 Self { hi, lo }
329 }
330
331 /// Sine using CORDIC algorithm (deterministic)
332 ///
333 /// Input should be in range [-π, π] for best precision
334 #[must_use]
335 pub fn sin(self) -> Self {
336 cordic_sin_cos(self).0
337 }
338
339 /// Cosine using CORDIC algorithm (deterministic)
340 ///
341 /// Input should be in range [-π, π] for best precision
342 #[must_use]
343 pub fn cos(self) -> Self {
344 cordic_sin_cos(self).1
345 }
346
347 /// Simultaneous sin and cos (more efficient)
348 #[must_use]
349 pub fn sin_cos(self) -> (Self, Self) {
350 cordic_sin_cos(self)
351 }
352
353 /// Arctangent using CORDIC (deterministic)
354 #[must_use]
355 pub fn atan(self) -> Self {
356 cordic_atan(self)
357 }
358
359 /// Arctangent2 (deterministic)
360 #[must_use]
361 pub fn atan2(y: Self, x: Self) -> Self {
362 cordic_atan2(y, x)
363 }
364
365 // ========================================================================
366 // SIMD-Accelerated Operations (x86_64 only)
367 // ========================================================================
368
369 /// Addition through the `simd` feature's API surface — **scalar-equivalent**.
370 ///
371 /// Bit-identical to `self + rhs`. SSE2's `_mm_add_epi64` adds the two
372 /// 64-bit lanes independently and cannot carry from `lo` into `hi`, so a
373 /// Fix128 add has no profitable SSE2 form; LLVM already emits `add`/`adc`
374 /// for the scalar path. Before 1.2.0 this function loaded both operands
375 /// into `__m128i` registers and then discarded them (`let _ = (a, b)`),
376 /// i.e. it was scalar with dead intrinsics.
377 ///
378 /// # Safety
379 ///
380 /// No preconditions; kept `unsafe` + `#[target_feature(enable = "sse2")]`
381 /// for signature compatibility with existing callers.
382 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
383 #[inline]
384 #[target_feature(enable = "sse2")]
385 pub unsafe fn add_simd(self, rhs: Self) -> Self {
386 self + rhs
387 }
388
389 /// Subtraction through the `simd` feature's API surface — **scalar-equivalent**
390 /// (see [`Self::add_simd`]).
391 ///
392 /// # Safety
393 ///
394 /// No preconditions; kept `unsafe` + `#[target_feature(enable = "sse2")]`
395 /// for signature compatibility with existing callers.
396 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
397 #[inline]
398 #[target_feature(enable = "sse2")]
399 pub unsafe fn sub_simd(self, rhs: Self) -> Self {
400 self - rhs
401 }
402}
403
404impl Add for Fix128 {
405 type Output = Self;
406
407 #[inline]
408 fn add(self, rhs: Self) -> Self {
409 let (lo, carry) = self.lo.overflowing_add(rhs.lo);
410 let hi = self.hi.wrapping_add(rhs.hi).wrapping_add(carry as i64);
411 Self { hi, lo }
412 }
413}
414
415impl Sub for Fix128 {
416 type Output = Self;
417
418 #[inline]
419 fn sub(self, rhs: Self) -> Self {
420 let (lo, borrow) = self.lo.overflowing_sub(rhs.lo);
421 let hi = self.hi.wrapping_sub(rhs.hi).wrapping_sub(borrow as i64);
422 Self { hi, lo }
423 }
424}
425
426/// Fixed-point multiplication: the 256-bit product of the two Q64.64
427/// values, keeping bits `[192:64]`.
428///
429/// # Overflow and rounding
430///
431/// - **Wrapping**: if the mathematical product does not fit the ±2^63
432/// integer range the high bits are discarded — the result wraps modulo
433/// 2^128 of the raw two's-complement representation. There is no
434/// saturation and no panic, in debug builds too (every intermediate uses
435/// `wrapping_*`). Callers that need range safety must check operands
436/// beforehand; the engine's own hot paths keep magnitudes far below the
437// LIMITATION(COV-NUM-076): There is no saturation and no panic, in debug builds too
438/// limit.
439/// - **Truncation**: fractional bits below 2^-64 are dropped, which is a
440/// floor toward −∞ on the two's-complement bit pattern (e.g.
441/// `-2^-64 * 0.5 == -2^-64`, not `0`).
442///
443/// Both properties are part of the determinism contract: the same
444/// operands yield the same bits on every platform.
445impl Mul for Fix128 {
446 type Output = Self;
447
448 fn mul(self, rhs: Self) -> Self {
449 // 128x128 -> 256 bit multiplication, take middle 128 bits
450 //
451 // self = (hi_a << 64) + lo_a
452 // rhs = (hi_b << 64) + lo_b
453 //
454 // product = hi_a*hi_b << 128 + (hi_a*lo_b + lo_a*hi_b) << 64 + lo_a*lo_b
455 //
456 // We want bits [192:64] of the 256-bit result (middle 128 bits)
457
458 let a_hi = self.hi as i128;
459 let a_lo = self.lo as u128;
460 let b_hi = rhs.hi as i128;
461 let b_lo = rhs.lo as u128;
462
463 // lo * lo (unsigned)
464 let ll = a_lo.wrapping_mul(b_lo);
465
466 // hi * lo and lo * hi (mixed sign)
467 let hl = a_hi.wrapping_mul(b_lo as i128);
468 let lh = (a_lo as i128).wrapping_mul(b_hi);
469
470 // hi * hi (signed)
471 let hh = a_hi.wrapping_mul(b_hi);
472
473 // Combine: we want (hh << 64) + hl + lh + (ll >> 64)
474 let ll_hi = (ll >> 64) as i128;
475
476 let mid = hl.wrapping_add(lh).wrapping_add(ll_hi);
477 let mid_lo = mid as u64;
478 let mid_hi = (mid >> 64) as i64;
479
480 let hi = (hh as i64).wrapping_add(mid_hi);
481
482 Self { hi, lo: mid_lo }
483 }
484}
485
486impl Vec3Fix {
487 /// 各成分を `s` 倍し、**どれか 1 成分でも範囲外なら `None`** を返す
488 ///
489 /// [`Fix128::checked_mul`] の `Vec3Fix` 版 発散検出の投入点
490 /// (積分 `position += velocity*dt` と速度導出) で使う
491 #[must_use]
492 pub fn checked_scale(self, s: Fix128) -> Option<Self> {
493 Some(Self {
494 x: self.x.checked_mul(s)?,
495 y: self.y.checked_mul(s)?,
496 z: self.z.checked_mul(s)?,
497 })
498 }
499}
500
501impl Fix128 {
502 /// [`Mul`] と同じ積を計算し、**範囲外なら `None`** を返す
503 ///
504 /// # なぜ必要か (WM-01 / doctrine B-12)
505 ///
506 /// `Mul` は全段 `wrapping_*` なので **範囲外の積が silent に巻き戻る**
507 /// 実測 (2026-09-30): `2⁴⁰ × 2⁴⁰` は exact `1.21e24` だが、Q64.64 では
508 /// `hh = 2⁸⁰` で `2⁸⁰ mod 2⁶⁴ = 0` なので **`hi` が厳密に 0** になる
509 ///
510 /// ⇒ `position += velocity * dt` が 0 加算になり、`update_velocities` が
511 /// `velocity = (position − prev_position) * inv_dt` で速度も 0 に上書きする
512 /// ⇒ **「原点で完全に静止した自己整合な状態」** に落ちる
513 /// ⚠️ **静止は物理的に妥当なのでどの不変条件でも red にならない**ため、
514 /// 「値が大きい / 値が飛んだ」を見る検出器では原理的に捕まえられない
515 ///
516 /// ⚠️ **`Mul` 自体の挙動は変えない** — 既存の決定論 golden と bit 互換を
517 /// 保つため、検出が要る経路だけが本 method を使う
518 /// (`PhysicsWorld::step` の積分と速度導出、`PhysicsWorld::overflow_detected`)
519 #[must_use]
520 pub fn checked_mul(self, rhs: Self) -> Option<Self> {
521 let a_hi = self.hi as i128;
522 let a_lo = self.lo as u128;
523 let b_hi = rhs.hi as i128;
524 let b_lo = rhs.lo as u128;
525
526 // `Mul` と同じ 256 bit 積の中央 128 bit を作る (ここは wrapping で良い
527 // — 落とす上位 bit があるかは下の try_from で判定する)
528 let ll = a_lo.wrapping_mul(b_lo);
529 let hl = a_hi.wrapping_mul(b_lo as i128);
530 let lh = (a_lo as i128).wrapping_mul(b_hi);
531 let hh = a_hi.wrapping_mul(b_hi);
532 let ll_hi = (ll >> 64) as i128;
533
534 // 中央の和 `hl + lh + ll_hi` は i128 を 1 bit 越えうる (例: a_hi = i64::MIN と
535 // b = −2⁻⁶⁴ の組は真の積が ≈ 0.5 でも和が −2¹²⁷ を下回る) wrap した値と
536 // 2¹²⁸ 単位の桁上がり `carry` に分けて厳密に持ち、範囲は最後に 1 回だけ判定する
537 let mut carry: i128 = 0;
538 let (mid, o1) = hl.overflowing_add(lh);
539 if o1 {
540 carry += if lh > 0 { 1 } else { -1 };
541 }
542 let (mid, o2) = mid.overflowing_add(ll_hi);
543 if o2 {
544 carry += 1; // ll_hi ≥ 0
545 }
546 let mid_lo = mid as u64;
547
548 // 整数部は `hh + floor(真の中央の和 / 2⁶⁴)` = `hh + (mid >> 64) + carry·2⁶⁴`
549 // 収まるかは**和**で判定する (`hh` 単独では判定しない: 負の値は整数部が
550 // floor なので `hh` が i64 を越えても後ろの項が打ち消すことがある)
551 // |hh| ≤ 2¹²⁶、|mid >> 64| ≤ 2⁶³、|carry·2⁶⁴| ≤ 2⁶⁵ なので i128 で溢れない
552 let hi = i64::try_from(hh + (mid >> 64) + (carry << 64)).ok()?;
553
554 Some(Self { hi, lo: mid_lo })
555 }
556
557 /// [`Add`] と同じ和を計算し、**範囲外 (`|a + b| ≥ 2⁶³`) なら `None`** を返す
558 ///
559 /// `Add` は wrap するので、範囲の検査が要る経路だけが本 method を使う
560 /// (`Add` 自体の挙動は変えない)
561 #[inline]
562 #[must_use]
563 pub fn checked_add(self, rhs: Self) -> Option<Self> {
564 let a = ((self.hi as i128) << 64) | (self.lo as i128);
565 let b = ((rhs.hi as i128) << 64) | (rhs.lo as i128);
566 let s = a.checked_add(b)?;
567 Some(Self::from_raw((s >> 64) as i64, s as u64))
568 }
569
570 /// `self^exponent` for `self > 0` and `exponent ≥ 0` (real exponent).
571 ///
572 /// Integer part of the exponent by repeated multiplication, fractional
573 /// part by its binary expansion `base^f = Π base^(1/2^k)` over the first 24
574 /// bits using successive [`Fix128::sqrt`] calls — pure integer arithmetic,
575 /// bit-identical on every platform, relative error ≲ |ln base| · 2⁻²⁴ plus
576 /// sqrt truncation (measured ≤ 1e-6 relative for results ≥ 1e-9 over
577 /// `base ∈ [1e-3, 1e3]`, `exponent ∈ [0, 8]`,
578 /// `math::tests::powf_pos_matches_f64_reference`; smaller results hit the
579 /// 2⁻⁶⁴ absolute resolution).
580 /// `self ≤ 0` or a negative exponent returns `ZERO` (no NaN; callers that
581 /// need those cases handle them explicitly).
582 ///
583 /// # Integer part of the exponent
584 ///
585 /// Exponents whose integer part is at most 64 use left-to-right repeated
586 /// multiplication; above 64 the integer part uses exponentiation by
587 /// squaring. The seam exists to keep results **bit-identical to 1.5.0 and
588 /// earlier wherever those versions were already correct**: fixed-point
589 /// multiplication truncates and is therefore not associative, so the two
590 /// orders differ in the low bits for a base that is not exactly
591 /// representable. Before 1.6.0 the integer part was silently capped at 64
592 /// (`1.01^100` returned `1.01^64` = 1.89 rather than 2.705), so only the
593 /// `n ≤ 64` range has bits worth preserving; above it the result was wrong
594 /// and the faster O(log n) order is used instead of an O(n) loop.
595 #[must_use]
596 pub fn powf_pos(self, exponent: Self) -> Self {
597 if self <= Self::ZERO || exponent.is_negative() {
598 return Self::ZERO;
599 }
600 // Integer part (`exponent.hi` ≥ 0 here, checked above). Two
601 // multiplication orders on purpose, split at 64:
602 //
603 // - `n ≤ 64` — left-to-right repeated multiplication, the order
604 // this function has always used. Fixed-point multiplication
605 // truncates, so it is **not associative**: reassociating the
606 // product changes the low bits (measured: up to 18 raw units for
607 // a non-dyadic base at `n ≤ 64`, and exactly zero for a base that
608 // is representable without truncation, such as the 2 that
609 // [`Fix128::exp`] passes). Bit-exact reproducibility across
610 // versions is a headline property of this crate, so the order is
611 // preserved over the whole range where it was already correct.
612 // - `n > 64` — exponentiation by squaring. This range used to be
613 // silently capped (`exponent.hi.min(64)`), so `1.01^100` returned
614 // `1.01^64` = 1.89 instead of 2.705: a plainly wrong value that no
615 // caller can have a legitimate dependency on, which leaves the
616 // order free here. Squaring keeps it O(log n), so a large exponent
617 // cannot turn into an unbounded loop.
618 let n = exponent.hi as u64;
619 let mut r = Self::ONE;
620 if n <= 64 {
621 for _ in 0..n {
622 r = r * self;
623 }
624 } else {
625 let mut n = n;
626 let mut base = self;
627 while n > 0 {
628 if n & 1 != 0 {
629 r = r * base;
630 }
631 n >>= 1;
632 if n > 0 {
633 base = base * base;
634 }
635 }
636 }
637 let mut frac_bits = exponent.lo;
638 let mut root = self;
639 for _ in 0..24 {
640 root = root.sqrt(); // base^(1/2), base^(1/4), …
641 if frac_bits & (1u64 << 63) != 0 {
642 r = r * root;
643 }
644 frac_bits <<= 1;
645 }
646 r
647 }
648
649 /// Deterministic `e^self` via `2^(x·log₂e)` ([`Fix128::powf_pos`] with base 2;
650 /// negative arguments as `1 / e^|x|`). Relative error ≲ 1e-6 for
651 /// `|x| ≤ 40` (`math::tests::exp_matches_f64_reference`); `x < −44` is
652 /// below the 2⁻⁶⁴ resolution and returns `ZERO`, and from `x ≥ 63 · ln 2`
653 /// (≈ 43.668, where `e^x` reaches 2⁶³) it saturates at the representable
654 /// maximum instead of wrapping.
655 #[must_use]
656 pub fn exp(self) -> Self {
657 // log₂(e) = 1.442 695 040 888 963 4 (raw pair, exact to 2⁻⁶⁴)
658 const LOG2_E: Fix128 = Fix128 {
659 hi: 1,
660 lo: 8_166_282_121_979_092_992,
661 };
662 if self.is_negative() {
663 if self.hi < -44 {
664 return Self::ZERO;
665 }
666 let pos = Self::from_int(2).powf_pos(self.neg() * LOG2_E);
667 if pos.is_zero() {
668 return Self::ZERO;
669 }
670 return Self::ONE / pos;
671 }
672 // e^x fits Fix128 while x · log₂e < 63 (x < 43.668); past that it
673 // saturates. `self.hi >= 44` keeps the product itself in range.
674 if self.hi >= 44 {
675 return Self::from_raw(i64::MAX, u64::MAX);
676 }
677 let power = self * LOG2_E;
678 if power.hi >= 63 {
679 return Self::from_raw(i64::MAX, u64::MAX);
680 }
681 Self::from_int(2).powf_pos(power)
682 }
683
684 /// Deterministic natural log via range reduction to `[1, 2)` + atanh
685 /// series (`ln(m) = 2·atanh((m−1)/(m+1))`, 16 terms). Non-positive input
686 /// returns `ZERO` (no panic, no NaN representation in `Fix128`). Moved
687 /// here from a private `turbulence.rs` helper of the same algorithm
688 /// (canonical source: one `ln`, not one per caller module).
689 #[must_use]
690 pub fn ln(self) -> Self {
691 if self <= Self::ZERO {
692 return Self::ZERO;
693 }
694 // ln 2 = 0.693 147 180 559 945 3 (raw pair, exact to 2⁻⁶⁴)
695 const LN2: Fix128 = Fix128 {
696 hi: 0,
697 lo: 0xB172_17F7_D1CF_79AC,
698 };
699 let mut m = self;
700 let mut k: i64 = 0;
701 while m >= Self::from_int(2) {
702 m = m.half();
703 k += 1;
704 }
705 while m < Self::ONE {
706 m = m.double();
707 k -= 1;
708 }
709 let t = (m - Self::ONE) / (m + Self::ONE);
710 let t2 = t * t;
711 let mut term = t;
712 let mut sum = Self::ZERO;
713 for n in 0..16u32 {
714 let denom = Self::from_int(i64::from(2 * n + 1));
715 sum = sum + term / denom;
716 term = term * t2;
717 }
718 Self::from_int(k) * LN2 + sum.double()
719 }
720
721 /// Checked division: `None` when `rhs == 0`, otherwise `Some(self / rhs)`.
722 ///
723 /// The `Div` operator returns `ZERO` for a zero divisor (deterministic, no
724 /// panic, no NaN) which is convenient in solver hot paths but hides bugs
725 /// in caller code that never expected a zero — use this in validation and
726 /// setup paths. Bit-identical to `/` for every non-zero divisor.
727 #[inline]
728 #[must_use]
729 pub fn checked_div(self, rhs: Self) -> Option<Self> {
730 if rhs.is_zero() {
731 None
732 } else {
733 Some(self / rhs)
734 }
735 }
736}
737
738/// Fixed-point division, truncating toward zero on the raw bit pattern.
739///
740/// ⚠️ **Not the same direction as [`Mul`], which floors toward
741/// −∞.** This operator takes the magnitudes apart, divides them and reapplies
742/// the sign, so the result is symmetric about zero: `(-a) / b == -(a / b)`
743/// exactly. `Mul` is not — `(-a) * b` is one raw unit below `-(a * b)`
744/// whenever the product has a remainder. Measured on `a = 1/3`, `b = 1/7`.
745///
746/// The difference matters wherever the *direction* of the bias decides an
747/// outcome rather than its size: a contraction ratio read as `> 1` or `= 1`,
748/// a residual that either reaches an exact fixed point or keeps drifting, a
749/// quadrature weight that is one unit light on one side of zero and not the
750/// other. Division is the safer of the two there, because the sign cannot
751/// change which way the error leans.
752///
753/// # Division by zero
754///
755/// Returns `ZERO` (documented contract since 1.2.0; the behaviour predates
756/// it). This keeps the solver deterministic and panic-free — a zero inverse
757/// mass, a degenerate normal or a zero `dt` never poisons a lockstep peer with
758/// NaN — but it also means a caller bug is silently masked. Use
759/// [`Fix128::checked_div`] where a zero divisor is an error. A `Result`-based
760/// operator is planned for 2.0 (`PhysicsError` is not `#[non_exhaustive]`,
761/// so a new variant cannot be added in 1.x).
762impl Div for Fix128 {
763 type Output = Self;
764
765 fn div(self, rhs: Self) -> Self {
766 if rhs.is_zero() {
767 return Self::ZERO;
768 }
769
770 let neg = self.is_negative() != rhs.is_negative();
771
772 let a = if self.is_negative() { self.neg() } else { self };
773 let b = if rhs.is_negative() { rhs.neg() } else { rhs };
774
775 let a_full = ((a.hi as u128) << 64) | (a.lo as u128);
776 let b_full = ((b.hi as u128) << 64) | (b.lo as u128);
777
778 if b_full == 0 {
779 return Self::ZERO;
780 }
781
782 // I64F64 division: result = (a_full << 64) / b_full
783 // Since (a_full << 64) overflows u128, split into two parts:
784 // result_hi = a_full / b_full (integer part)
785 // result_lo = fractional part via 64-step long division
786
787 let quot_hi = a_full / b_full;
788 let rem = a_full % b_full;
789
790 // Compute fractional 64 bits via bit-by-bit long division
791 // This avoids the (rem << 64) overflow that broke the old implementation
792 let mut r = rem;
793 let mut quot_lo: u64 = 0;
794 for i in (0..64).rev() {
795 let overflow_bit = r >> 127;
796 r <<= 1;
797 if overflow_bit != 0 || r >= b_full {
798 r = r.wrapping_sub(b_full);
799 quot_lo |= 1u64 << i;
800 }
801 }
802
803 let result = Self {
804 hi: quot_hi as i64,
805 lo: quot_lo,
806 };
807
808 if neg {
809 result.neg()
810 } else {
811 result
812 }
813 }
814}
815
816impl Neg for Fix128 {
817 type Output = Self;
818
819 #[inline]
820 fn neg(self) -> Self {
821 // Two's complement negation
822 let (lo, carry) = (!self.lo).overflowing_add(1);
823 let hi = (!self.hi).wrapping_add(carry as i64);
824 Self { hi, lo }
825 }
826}
827
828impl PartialOrd for Fix128 {
829 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
830 Some(self.cmp(other))
831 }
832}
833
834impl Ord for Fix128 {
835 fn cmp(&self, other: &Self) -> Ordering {
836 match self.hi.cmp(&other.hi) {
837 Ordering::Equal => self.lo.cmp(&other.lo),
838 ord => ord,
839 }
840 }
841}
842
843impl core::fmt::Display for Fix128 {
844 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
845 let val = self.to_f64();
846 write!(f, "{val:.4}")
847 }
848}
849
850impl From<i64> for Fix128 {
851 #[inline]
852 fn from(n: i64) -> Self {
853 Self::from_int(n)
854 }
855}
856
857impl From<i32> for Fix128 {
858 #[inline]
859 fn from(n: i32) -> Self {
860 Self::from_int(n as i64)
861 }
862}
863
864// ============================================================================
865// CORDIC Algorithm (Coordinate Rotation Digital Computer)
866// ============================================================================
867
868/// CORDIC angles table (arctan(2^-i) in Fix128 format)
869/// Precomputed for 64 iterations
870const CORDIC_ANGLES: [Fix128; 64] = compute_cordic_angles();
871
872/// CORDIC gain constant K = prod(cos(arctan(2^-i)), i=0..47) ≈ 0.60725293500888133
873const CORDIC_K: Fix128 = Fix128 {
874 hi: 0,
875 lo: 0x9B74EDA8435E5DD7, // 0.60725293500888133 * 2^64
876};
877
878/// Compute CORDIC angle table at compile time
879///
880/// Each entry is arctan(2^-i) in Fix128 format (hi=0, lo=fractional).
881/// Values precomputed to full 64-bit fractional precision.
882const fn compute_cordic_angles() -> [Fix128; 64] {
883 // arctan(2^-i) * 2^64 for i = 0..63, precomputed with arbitrary-precision math.
884 // arctan(2^0) = pi/4 = 0.7853981633974483...
885 // arctan(2^-1) = 0.4636476090008061...
886 // arctan(2^-2) = 0.2449786631268641...
887 // For large i (>= ~15), arctan(2^-i) ≈ 2^-i to full precision.
888 const LO: [u64; 64] = [
889 0xC90FDAA22168C000, // i=0: arctan(1) = pi/4
890 0x76B19C1586ED4000, // i=1: arctan(1/2) = 0.46364760900...
891 0x3EB6EBF25901BA00, // i=2: arctan(1/4) = 0.24497866312...
892 0x1FD5BA9AAC2F6E00, // i=3: arctan(1/8) = 0.12435499454...
893 0x0FFAADDB967EF500, // i=4: arctan(1/16) = 0.06241880999...
894 0x07FF556EEA5D8940, // i=5: arctan(1/32) = 0.03123983343...
895 0x03FFEAAB776E5360, // i=6: arctan(1/64) = 0.01562372862...
896 0x01FFFD555BBBA970, // i=7: arctan(1/128) = 0.00781234106...
897 0x00FFFFAAAADDDDB8, // i=8: arctan(1/256)
898 0x007FFFF55556EEF0, // i=9: arctan(1/512)
899 0x003FFFFEAAAAB778, // i=10: arctan(1/1024)
900 0x001FFFFFD55555BC, // i=11: arctan(1/2048)
901 0x000FFFFFFAAAAAAE, // i=12: arctan(1/4096)
902 0x0007FFFFFF555555, // i=13: arctan(1/8192)
903 0x0003FFFFFFEAAAAA, // i=14: arctan(1/16384)
904 0x0001FFFFFFFD5555, // i=15: arctan(1/32768)
905 0x0000FFFFFFFFAAAA, // i=16
906 0x00007FFFFFFFF555, // i=17
907 0x00003FFFFFFFFEAA, // i=18
908 0x00001FFFFFFFFFD5, // i=19
909 0x00000FFFFFFFFFFA, // i=20
910 0x000007FFFFFFFFFF, // i=21
911 0x000003FFFFFFFFFF, // i=22
912 0x000001FFFFFFFFFF, // i=23
913 0x000000FFFFFFFFFF, // i=24
914 0x0000007FFFFFFFFF, // i=25
915 0x0000003FFFFFFFFF, // i=26
916 0x0000002000000000, // i=27
917 0x0000001000000000, // i=28
918 0x0000000800000000, // i=29
919 0x0000000400000000, // i=30
920 0x0000000200000000, // i=31
921 0x0000000100000000, // i=32
922 0x0000000080000000, // i=33
923 0x0000000040000000, // i=34
924 0x0000000020000000, // i=35
925 0x0000000010000000, // i=36
926 0x0000000008000000, // i=37
927 0x0000000004000000, // i=38
928 0x0000000002000000, // i=39
929 0x0000000001000000, // i=40
930 0x0000000000800000, // i=41
931 0x0000000000400000, // i=42
932 0x0000000000200000, // i=43
933 0x0000000000100000, // i=44
934 0x0000000000080000, // i=45
935 0x0000000000040000, // i=46
936 0x0000000000020000, // i=47
937 0x0000000000010000, // i=48
938 0x0000000000008000, // i=49
939 0x0000000000004000, // i=50
940 0x0000000000002000, // i=51
941 0x0000000000001000, // i=52
942 0x0000000000000800, // i=53
943 0x0000000000000400, // i=54
944 0x0000000000000200, // i=55
945 0x0000000000000100, // i=56
946 0x0000000000000080, // i=57
947 0x0000000000000040, // i=58
948 0x0000000000000020, // i=59
949 0x0000000000000010, // i=60
950 0x0000000000000008, // i=61
951 0x0000000000000004, // i=62
952 0x0000000000000002, // i=63
953 ];
954 let mut angles = [Fix128::ZERO; 64];
955 let mut i = 0;
956 while i < 64 {
957 angles[i] = Fix128 { hi: 0, lo: LO[i] };
958 i += 1;
959 }
960 angles
961}
962
963/// CORDIC sine and cosine (deterministic, 48 iterations)
964fn cordic_sin_cos(angle: Fix128) -> (Fix128, Fix128) {
965 // Step 1: O(1) modular reduction to [-π, π]
966 let mut theta = angle;
967 if theta > Fix128::PI || theta < Fix128::PI.neg() {
968 // k = floor((theta + π) / 2π)
969 let shifted = theta + Fix128::PI;
970 let k = shifted / Fix128::TWO_PI;
971 let k_int = Fix128::from_int(k.hi);
972 theta = theta - Fix128::TWO_PI * k_int;
973 // Clamp to handle edge cases
974 if theta > Fix128::PI {
975 theta = theta - Fix128::TWO_PI;
976 } else if theta < Fix128::PI.neg() {
977 theta = theta + Fix128::TWO_PI;
978 }
979 }
980
981 // Step 2: Quadrant reduction to [-π/2, π/2] (CORDIC convergence range)
982 // For |θ| > π/2, use: sin(θ) = sin(π-θ), cos(θ) = -cos(π-θ)
983 let negate_cos = if theta > Fix128::HALF_PI {
984 theta = Fix128::PI - theta;
985 true
986 } else if theta < Fix128::HALF_PI.neg() {
987 theta = Fix128::PI.neg() - theta;
988 true
989 } else {
990 false
991 };
992
993 // Initialize: start at (K, 0) and rotate by theta
994 let mut x = CORDIC_K;
995 let mut y = Fix128::ZERO;
996 let mut z = theta;
997
998 // CORDIC iterations
999 for (i, &angle) in CORDIC_ANGLES.iter().enumerate().take(48) {
1000 let d = if z.is_negative() { -1i64 } else { 1i64 };
1001
1002 let x_shift = x.shr_bits(i as u32);
1003 let y_shift = y.shr_bits(i as u32);
1004
1005 let new_x;
1006 let new_y;
1007
1008 if d > 0 {
1009 new_x = x - y_shift;
1010 new_y = y + x_shift;
1011 z = z - angle;
1012 } else {
1013 new_x = x + y_shift;
1014 new_y = y - x_shift;
1015 z = z + angle;
1016 }
1017
1018 x = new_x;
1019 y = new_y;
1020 }
1021
1022 // Apply quadrant correction
1023 if negate_cos {
1024 (y, x.neg()) // sin preserved, cos negated
1025 } else {
1026 (y, x) // sin, cos
1027 }
1028}
1029
1030/// CORDIC arctangent (deterministic)
1031fn cordic_atan(v: Fix128) -> Fix128 {
1032 // atan(v) using CORDIC in vectoring mode
1033 let mut x = Fix128::ONE;
1034 let mut y = v;
1035 let mut z = Fix128::ZERO;
1036
1037 for (i, &angle) in CORDIC_ANGLES.iter().enumerate().take(48) {
1038 let d = if y.is_negative() { 1i64 } else { -1i64 };
1039
1040 let x_shift = x.shr_bits(i as u32);
1041 let y_shift = y.shr_bits(i as u32);
1042
1043 if d > 0 {
1044 x = x - y_shift;
1045 y = y + x_shift;
1046 z = z - angle;
1047 } else {
1048 x = x + y_shift;
1049 y = y - x_shift;
1050 z = z + angle;
1051 }
1052 }
1053
1054 z
1055}
1056
1057/// CORDIC atan2 (deterministic)
1058fn cordic_atan2(y: Fix128, x: Fix128) -> Fix128 {
1059 if x.is_zero() && y.is_zero() {
1060 return Fix128::ZERO;
1061 }
1062
1063 if x.is_zero() {
1064 return if y.is_negative() {
1065 Fix128::HALF_PI.neg()
1066 } else {
1067 Fix128::HALF_PI
1068 };
1069 }
1070
1071 // Keep the CORDIC argument in [−1, 1]: for |y| > |x| use the complement
1072 // atan2(y, x) = ±π/2 − atan(x / y). Before 1.2.0 `y / x` was formed
1073 // unconditionally, and with |x| a few ulp it overflowed the 64-bit
1074 // integer part (1 / 2⁻⁶³ = 2⁶³), so atan2(1, 2 ulp) returned −0.755
1075 // instead of π/2 — visible as a wrong-way quaternion slerp between
1076 // nearly opposite rotations.
1077 if y.abs() > x.abs() {
1078 let complement = cordic_atan(x / y);
1079 return if y.is_negative() {
1080 Fix128::HALF_PI.neg() - complement
1081 } else {
1082 Fix128::HALF_PI - complement
1083 };
1084 }
1085
1086 let ratio = y / x;
1087 let base_atan = cordic_atan(ratio);
1088
1089 if x.is_negative() {
1090 if y.is_negative() {
1091 base_atan - Fix128::PI
1092 } else {
1093 base_atan + Fix128::PI
1094 }
1095 } else {
1096 base_atan
1097 }
1098}
1099
1100// ============================================================================
1101// Vec3Fix - 3D Vector with Fixed-Point Components
1102// ============================================================================
1103
1104/// 3D vector using Fix128 components
1105#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
1106#[repr(C)]
1107pub struct Vec3Fix {
1108 /// X component
1109 pub x: Fix128,
1110 /// Y component
1111 pub y: Fix128,
1112 /// Z component
1113 pub z: Fix128,
1114}
1115
1116impl Vec3Fix {
1117 /// Zero vector
1118 pub const ZERO: Self = Self {
1119 x: Fix128::ZERO,
1120 y: Fix128::ZERO,
1121 z: Fix128::ZERO,
1122 };
1123
1124 /// Unit X vector
1125 pub const UNIT_X: Self = Self {
1126 x: Fix128::ONE,
1127 y: Fix128::ZERO,
1128 z: Fix128::ZERO,
1129 };
1130
1131 /// Unit Y vector
1132 pub const UNIT_Y: Self = Self {
1133 x: Fix128::ZERO,
1134 y: Fix128::ONE,
1135 z: Fix128::ZERO,
1136 };
1137
1138 /// Unit Z vector
1139 pub const UNIT_Z: Self = Self {
1140 x: Fix128::ZERO,
1141 y: Fix128::ZERO,
1142 z: Fix128::ONE,
1143 };
1144
1145 /// Create new vector
1146 #[inline]
1147 #[must_use]
1148 pub const fn new(x: Fix128, y: Fix128, z: Fix128) -> Self {
1149 Self { x, y, z }
1150 }
1151
1152 /// Create from integers
1153 #[inline]
1154 #[must_use]
1155 pub const fn from_int(x: i64, y: i64, z: i64) -> Self {
1156 Self {
1157 x: Fix128::from_int(x),
1158 y: Fix128::from_int(y),
1159 z: Fix128::from_int(z),
1160 }
1161 }
1162
1163 /// Create from f32 components (for SDF bridge)
1164 /// Create from f32 components (for SDF bridge, not deterministic!)
1165 #[inline]
1166 #[must_use]
1167 pub fn from_f32(x: f32, y: f32, z: f32) -> Self {
1168 Self {
1169 x: Fix128::from_f32(x),
1170 y: Fix128::from_f32(y),
1171 z: Fix128::from_f32(z),
1172 }
1173 }
1174
1175 /// Convert to f32 tuple (for SDF bridge)
1176 #[inline]
1177 #[must_use]
1178 pub fn to_f32(self) -> (f32, f32, f32) {
1179 (self.x.to_f32(), self.y.to_f32(), self.z.to_f32())
1180 }
1181
1182 /// Dot product
1183 #[inline(always)]
1184 #[must_use]
1185 pub fn dot(self, rhs: Self) -> Fix128 {
1186 self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
1187 }
1188
1189 /// Cross product
1190 #[inline(always)]
1191 #[must_use]
1192 pub fn cross(self, rhs: Self) -> Self {
1193 Self {
1194 x: self.y * rhs.z - self.z * rhs.y,
1195 y: self.z * rhs.x - self.x * rhs.z,
1196 z: self.x * rhs.y - self.y * rhs.x,
1197 }
1198 }
1199
1200 /// Squared length (no sqrt)
1201 #[inline(always)]
1202 #[must_use]
1203 pub fn length_squared(self) -> Fix128 {
1204 self.dot(self)
1205 }
1206
1207 /// Length (magnitude)
1208 #[inline(always)]
1209 #[must_use]
1210 pub fn length(self) -> Fix128 {
1211 self.length_squared().sqrt()
1212 }
1213
1214 /// The same direction with its largest component brought into `[2^-24, 2^24]`,
1215 /// for functions that use only the direction of their argument (shape
1216 /// support mappings).
1217 ///
1218 /// # Claims
1219 /// - a direction whose largest component is already in `[2^-24, 2^24]`, or
1220 /// the zero vector, is returned unchanged (bit for bit)
1221 /// - otherwise every component is shifted by the same number of bits so the
1222 /// largest lies in `[1, 2)`: an exact power-of-two scaling, so the direction
1223 /// is unchanged and squares of the components neither underflow below
1224 /// `2^-64` (`|d| < ~2.3e-10`) nor overflow the integer range (`|d| > ~3e9`)
1225 /// (AUD-A-S3W3-018, AUD-A-S4W3-001, AUD-A-S5W2-007, AUD-A-S5W3-002,
1226 /// AUD-A-S4W3-002, AUD-A-S5W3-003, AUD-A-S6W1-007)
1227 #[must_use]
1228 pub(crate) fn rescaled_direction(self) -> Self {
1229 let raw = |f: Fix128| ((f.hi as i128) << 64) | (f.lo as i128);
1230 let back = |r: i128| Fix128::from_raw((r >> 64) as i64, r as u64);
1231 let (x, y, z) = (raw(self.x), raw(self.y), raw(self.z));
1232 let m = x.unsigned_abs().max(y.unsigned_abs()).max(z.unsigned_abs());
1233 if m == 0 {
1234 return self;
1235 }
1236 // bit 64 is 1.0; keep [2^-24, 2^24] = bits 40..=88 untouched
1237 let msb = 127 - m.leading_zeros() as i32;
1238 if (40..=88).contains(&msb) {
1239 return self;
1240 }
1241 let shift = 64 - msb;
1242 let scale = |r: i128| if shift > 0 { r << shift } else { r >> (-shift) };
1243 Vec3Fix::new(back(scale(x)), back(scale(y)), back(scale(z)))
1244 }
1245
1246 /// `self` with its `x` and `z` scaled up by the same power of two `2ᵏ`
1247 /// (exactly), so that the larger lies in `[1, 2)`, when it is below `2⁻⁸`;
1248 /// `y` is kept, and an `x`, `z` not below `2⁻⁸` is returned as is. For a
1249 /// support mapping that normalizes the `xz` part of its direction (a
1250 /// cylinder's rim, a cone's base): `x² + z²` keeps multiples of `2⁻⁶⁴`
1251 /// only, so an `xz` part of size `m` loses `2⁻⁶⁴ / m²` of its length, and
1252 /// the rim point as much of the radius. A direction near the axis (a
1253 /// contact near a rim, its normal mostly along the axis) has a short `xz`
1254 /// part: at `m ≈ 2⁻²⁰` the rim point was `2⁻²⁴` of the radius off, outside
1255 /// the solid. Below `2⁻⁸` that loss would exceed `2⁻⁴⁸`.
1256 ///
1257 /// An `xz` part no larger than the rounding of the turn into the body frame
1258 /// is returned as zero (the mapping then takes its point on the axis, as it
1259 /// did when `x² + z²` came out zero). `self` is `q* d q` (two quaternion
1260 /// products); with `u = 2⁻⁶⁴`, a direction `d` along the axis comes out with
1261 /// `|x|, |z|` below:
1262 ///
1263 /// - `12u`: each product is truncated by less than `u`; a component of
1264 /// `q* d` sums at most 4 of them (`< 4u`), and one of `(q* d) q` sums 4
1265 /// products of those with components of `q` (`Σ|qᵢ| ≤ 2`) and truncates 4
1266 /// more, whatever the length of `d`;
1267 /// - `2⁻⁴⁴·|d|`: the quaternion is not the turn meant. `from_axis_angle`
1268 /// takes the half angle's sine and cosine by CORDIC, off by up to
1269 /// `2⁻⁴⁷` (the largest error over 200001 angles in `[−π, π]` against
1270 /// `f64`), so each component is within `2⁻⁴⁷` and the turn within
1271 /// `2·2·2⁻⁴⁷ = 2⁻⁴⁵` rad; an angle and an axis given in `f64` add half an
1272 /// ulp each (below `2⁻⁵⁰` rad together). The direction `(1, 0, 0)` onto a
1273 /// cylinder turned by `π/2` about `Z` came out with `x ≈ 2⁻⁴⁶·⁴·|d|`,
1274 /// which scaled up gave a rim point.
1275 ///
1276 /// The bound is `2⁻⁴²·max(1, |d|∞)` (`|d|∞` the largest component of
1277 /// `self`), 4 times the sum. Taking the axis point below it costs little:
1278 /// the rim point's support value along `d` exceeds the axis point's by at
1279 /// most `radius·√2·2⁻⁴²`, below the `2⁻³²` tolerance of the queries for a
1280 /// radius up to `2⁹`.
1281 #[must_use]
1282 pub(crate) fn rescaled_xz(self) -> Self {
1283 let raw = |f: Fix128| ((f.hi as i128) << 64) | (f.lo as i128);
1284 let back = |r: i128| Fix128::from_raw((r >> 64) as i64, r as u64);
1285 let (x, y, z) = (raw(self.x), raw(self.y), raw(self.z));
1286 let m = x.unsigned_abs().max(z.unsigned_abs());
1287 // bit 64 is 1.0: the bound 2⁻⁴²·max(1, |d|∞)
1288 let big = m.max(y.unsigned_abs()).max(1u128 << 64);
1289 if m <= big >> 42 {
1290 return Vec3Fix::new(Fix128::ZERO, self.y, Fix128::ZERO);
1291 }
1292 // 2⁻⁸ is bit 56
1293 let msb = 127 - m.leading_zeros() as i32;
1294 if msb >= 56 {
1295 return self;
1296 }
1297 let shift = 64 - msb;
1298 Vec3Fix::new(back(x << shift), self.y, back(z << shift))
1299 }
1300
1301 /// Normalize to unit length.
1302 ///
1303 /// Returns `Self::ZERO` for zero-length vectors. Use [`Self::try_normalize`]
1304 /// when you need to distinguish a zero-length input from a valid unit vector.
1305 #[inline(always)]
1306 #[must_use]
1307 pub fn normalize(self) -> Self {
1308 let len = self.length();
1309 if len.is_zero() {
1310 Self::ZERO
1311 } else {
1312 self / len
1313 }
1314 }
1315
1316 /// Try to normalize, returning `None` for zero-length vectors.
1317 #[inline(always)]
1318 #[must_use]
1319 pub fn try_normalize(self) -> Option<Self> {
1320 let len = self.length();
1321 if len.is_zero() {
1322 None
1323 } else {
1324 Some(self / len)
1325 }
1326 }
1327
1328 /// Normalize and return both the unit vector and the original length.
1329 ///
1330 /// Avoids double sqrt when both normalized direction and distance are needed.
1331 /// Returns `(Self::ZERO, Fix128::ZERO)` if the vector is zero-length.
1332 #[inline(always)]
1333 #[must_use]
1334 pub fn normalize_with_length(self) -> (Self, Fix128) {
1335 let len = self.length();
1336 if len.is_zero() {
1337 (Self::ZERO, Fix128::ZERO)
1338 } else {
1339 let inv_len = Fix128::ONE / len;
1340 (self * inv_len, len)
1341 }
1342 }
1343
1344 // ------------------------------------------------------------------------
1345 // 範囲を検査する版
1346 //
1347 // `dot` / `length_squared` は各成分の積と和が wrap する (mod 2¹²⁸) ので、
1348 // |v| ≥ 2³¹·⁵ ≈ 3.04e9 で `length` は 0、`normalize` は ZERO を返す
1349 // (`tests/analytic_coordinate_range.rs`) 既存の関数は決定論の固定値と
1350 // bit 互換を保つため変えず、範囲の検査が要る経路だけが以下を使う
1351 // ------------------------------------------------------------------------
1352
1353 /// 成分ごとの [`Fix128::checked_add`] (`+` と同じ和、どれかの成分が
1354 /// `|·| ≥ 2⁶³` なら `None`)
1355 ///
1356 /// # Claims
1357 /// - `Some` の時は `self + rhs` と bit 一致する
1358 /// - 位置の加算のように wrap が反対側の端への移動になる経路だけが使う
1359 #[inline]
1360 #[must_use]
1361 pub fn checked_add(self, rhs: Self) -> Option<Self> {
1362 Some(Self {
1363 x: self.x.checked_add(rhs.x)?,
1364 y: self.y.checked_add(rhs.y)?,
1365 z: self.z.checked_add(rhs.z)?,
1366 })
1367 }
1368
1369 /// [`Self::dot`] と同じ値を計算し、**積か和のどれかが範囲外なら `None`** を返す
1370 ///
1371 /// # Claims
1372 /// - `Some` の時は `dot` と bit 一致する (同じ積を同じ順で足す)
1373 /// - 3 つの積のどれか、または途中の和が `|·| ≥ 2⁶³` なら `None`
1374 #[must_use]
1375 pub fn checked_dot(self, rhs: Self) -> Option<Fix128> {
1376 let x = self.x.checked_mul(rhs.x)?;
1377 let y = self.y.checked_mul(rhs.y)?;
1378 let z = self.z.checked_mul(rhs.z)?;
1379 x.checked_add(y)?.checked_add(z)
1380 }
1381
1382 /// [`Self::length_squared`] の範囲検査版 (`|v|² ≥ 2⁶³` で `None`)
1383 ///
1384 /// `Some` の時は `length_squared` と bit 一致する
1385 #[must_use]
1386 pub fn checked_length_squared(self) -> Option<Fix128> {
1387 self.checked_dot(self)
1388 }
1389
1390 /// [`Self::length`] の範囲検査版 (`|v| ≥ 2³¹·⁵` で `None`)
1391 ///
1392 /// `Some` の時は `length` と bit 一致する 範囲外でも長さが要るなら
1393 /// [`Self::checked_length_scaled`] を使う
1394 #[must_use]
1395 pub fn checked_length(self) -> Option<Fix128> {
1396 Some(self.checked_length_squared()?.sqrt())
1397 }
1398
1399 /// [`Self::try_normalize`] の範囲検査版
1400 ///
1401 /// 長さ 0 と `|v| ≥ 2³¹·⁵` (長さの 2 乗が範囲外) のどちらも `None`
1402 /// `Some` の時は `try_normalize` と bit 一致する 範囲外でも向きが要るなら
1403 /// [`Self::try_normalize_scaled`] を使う
1404 #[must_use]
1405 pub fn checked_normalize(self) -> Option<Self> {
1406 let len = self.checked_length()?;
1407 if len.is_zero() {
1408 None
1409 } else {
1410 Some(self / len)
1411 }
1412 }
1413
1414 /// 最大成分を `[2²⁹, 2³⁰)` に寄せた同じ向きのベクトルと、その倍率の指数 `k`
1415 /// (`self ≈ scaled · 2ᵏ`) 零ベクトルは `(ZERO, 0)`
1416 ///
1417 /// 寄せた後は 2 乗の和が `3·2⁶⁰` 未満なので `length_squared` が wrap しない
1418 /// 縮める時 (`k > 0`) は各成分の絶対値の下位 `k` bit を切り捨てる (符号は
1419 /// 保つので向きは原点対称)、広げる時 (`k < 0`) は厳密
1420 fn split_pow2(self) -> (Self, i32) {
1421 let raw = |f: Fix128| ((f.hi as i128) << 64) | (f.lo as i128);
1422 let (x, y, z) = (raw(self.x), raw(self.y), raw(self.z));
1423 let m = x.unsigned_abs().max(y.unsigned_abs()).max(z.unsigned_abs());
1424 if m == 0 {
1425 return (Self::ZERO, 0);
1426 }
1427 // bit 64 が 1.0、bit 93 が 2²⁹
1428 let k = (127 - m.leading_zeros() as i32) - 93;
1429 let scale = |r: i128| -> Fix128 {
1430 let a = r.unsigned_abs();
1431 let s = if k >= 0 { a >> k } else { a << (-k) };
1432 // s < 2⁹⁴ なので i128 に収まる
1433 let v = if r < 0 { -(s as i128) } else { s as i128 };
1434 Fix128::from_raw((v >> 64) as i64, v as u64)
1435 };
1436 (Self::new(scale(x), scale(y), scale(z)), k)
1437 }
1438
1439 /// 2 乗が範囲外になる大きさでも正しい長さ 結果自体が `≥ 2⁶³` (表せない)
1440 /// 時だけ `None`
1441 ///
1442 /// # Claims
1443 /// - `|v|² < 2⁶³` (`length_squared` が wrap しない範囲) では **`length` と
1444 /// bit 一致する** (同じ式をそのまま使う)
1445 /// - それより大きい時は、最大成分を 2 の冪で `[2²⁹, 2³⁰)` に寄せてから
1446 /// 2 乗するので wrap せず、相対誤差は約 `2⁻⁹²` (成分の切り捨てと
1447 /// `sqrt` の floor)
1448 /// - 結果が `≥ 2⁶³` (成分が `2⁶²` 級) なら `None`
1449 #[must_use]
1450 pub fn checked_length_scaled(self) -> Option<Fix128> {
1451 if let Some(len) = self.checked_length() {
1452 return Some(len);
1453 }
1454 let (s, k) = self.split_pow2();
1455 // ここに来るのは |v| ≥ 2³¹·⁵ の時だけなので k ≥ 1
1456 let ls = s.length();
1457 let raw = ((ls.hi as i128) << 64) | (ls.lo as i128);
1458 // ls < 2³¹ なので raw ≥ 0、raw << k < 2¹²⁷ が要る
1459 if k < 0 || raw.leading_zeros() <= k as u32 {
1460 return None;
1461 }
1462 let r = raw << k;
1463 Some(Fix128::from_raw((r >> 64) as i64, r as u64))
1464 }
1465
1466 /// 2 乗が範囲外になる大きさでも向きを返す正規化 零ベクトルだけ `None`
1467 ///
1468 /// # Claims
1469 /// - `try_normalize` が `Some` を返す入力では **`try_normalize` と bit 一致**
1470 /// - `try_normalize` が範囲外 (`|v| ≥ 2³¹·⁵`) や長さの 2 乗の下位桁あふれ
1471 /// (`|v| < 約 2⁻³²`) で `None` を返す非零ベクトルには、最大成分を
1472 /// `[2²⁹, 2³⁰)` に寄せたベクトルを正規化して返す
1473 #[must_use]
1474 pub fn try_normalize_scaled(self) -> Option<Self> {
1475 if let Some(n) = self.checked_normalize() {
1476 return Some(n);
1477 }
1478 let (s, _) = self.split_pow2();
1479 if s == Self::ZERO {
1480 return None;
1481 }
1482 // 最大成分 ∈ [2²⁹, 2³⁰) なので長さは 0 にならず、2 乗も wrap しない
1483 Some(s / s.length())
1484 }
1485
1486 /// Scale by scalar
1487 #[inline]
1488 #[must_use]
1489 pub fn scale(self, s: Fix128) -> Self {
1490 Self {
1491 x: self.x * s,
1492 y: self.y * s,
1493 z: self.z * s,
1494 }
1495 }
1496
1497 // ========================================================================
1498 // SIMD-Accelerated Operations
1499 // ========================================================================
1500
1501 /// SIMD-accelerated dot product — SSE2 inner implementation.
1502 ///
1503 /// # Determinism guarantee
1504 ///
1505 /// Dot product on the `simd` feature's x86_64 path — **scalar-equivalent**.
1506 ///
1507 /// Fix128 multiplication is 128-bit integer arithmetic (`u128`/`i128`); no
1508 /// SSE2/AVX2 instruction performs it, and the two Fix128 additions cannot
1509 /// use `_mm_add_epi64` because it does not carry from `lo` into `hi` (see
1510 /// the comment in the body). The result is bit-exact to `dot()` because it
1511 /// *is* `dot()`. Kept as the dispatch target of `dot_simd` so a future
1512 /// AVX-512 / NEON batch path can land without an API change.
1513 ///
1514 /// # Safety
1515 ///
1516 /// No preconditions; `#[target_feature(enable = "sse2")]` is always
1517 /// satisfied on x86_64.
1518 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
1519 #[target_feature(enable = "sse2")]
1520 unsafe fn dot_simd_sse2(self, rhs: Self) -> Fix128 {
1521 // --- Step 1: scalar Fix128 multiplications (128-bit, bit-exact) ------
1522 // SSE2 has no 128-bit multiply, so the multiply stays scalar.
1523 let px = self.x * rhs.x;
1524 let py = self.y * rhs.y;
1525 let pz = self.z * rhs.z;
1526
1527 // --- Step 2: Scalar Fix128 addition chain (px + py + pz) -------------
1528 //
1529 // SSE2's _mm_add_epi64 performs lane-independent 64-bit adds but cannot
1530 // propagate the carry from lo to hi that Fix128 addition requires.
1531 // A correct SIMD path would need to detect per-lane overflow, extract it,
1532 // and feed it into the hi lane — more instructions than the scalar path.
1533 //
1534 // Therefore we use the direct scalar carry chain which is already well
1535 // optimized by LLVM into ADC (add-with-carry) on x86_64.
1536 let ab = px + py;
1537 ab + pz
1538 }
1539
1540 /// SIMD-accelerated dot product — safe public entry point.
1541 ///
1542 /// On `x86_64` with the `simd` feature enabled this dispatches to the SSE2
1543 /// path (`dot_simd_sse2`). On every other platform it falls back to the
1544 /// scalar `dot()`. The result is **bit-exact identical** to `dot()` on every
1545 /// platform.
1546 #[inline]
1547 #[must_use]
1548 pub fn dot_simd(self, rhs: Self) -> Fix128 {
1549 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
1550 {
1551 // SAFETY: SSE2 is part of the x86_64 baseline ABI; all x86_64 CPUs
1552 // support it unconditionally.
1553 unsafe { self.dot_simd_sse2(rhs) }
1554 }
1555 #[cfg(not(all(feature = "simd", target_arch = "x86_64")))]
1556 {
1557 self.dot(rhs)
1558 }
1559 }
1560
1561 /// Squared length using SIMD-accelerated dot product.
1562 ///
1563 /// Equivalent to `length_squared()` but dispatches through `dot_simd`.
1564 /// Result is **bit-exact identical** to `length_squared()`.
1565 #[inline]
1566 #[must_use]
1567 pub fn length_squared_simd(self) -> Fix128 {
1568 self.dot_simd(self)
1569 }
1570
1571 /// Cross product through the `simd` feature's API surface —
1572 /// **scalar-equivalent**, bit-identical to `cross()` (see the module docs).
1573 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
1574 #[inline]
1575 pub fn cross_simd(self, rhs: Self) -> Self {
1576 // Cross product: (a.y*b.z - a.z*b.y, a.z*b.x - a.x*b.z, a.x*b.y - a.y*b.x)
1577 // Compute all 6 products first, then subtract
1578 let yz = self.y * rhs.z;
1579 let zy = self.z * rhs.y;
1580 let zx = self.z * rhs.x;
1581 let xz = self.x * rhs.z;
1582 let xy = self.x * rhs.y;
1583 let yx = self.y * rhs.x;
1584
1585 Self {
1586 x: yz - zy,
1587 y: zx - xz,
1588 z: xy - yx,
1589 }
1590 }
1591
1592 /// Batch dot product for 4 vector pairs — **scalar-equivalent** (four
1593 /// `dot()` calls, bit-identical). The batch shape is the API a future
1594 /// AVX-512 / NEON path would fill in.
1595 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
1596 pub fn dot_batch_4(a: [Self; 4], b: [Self; 4]) -> [Fix128; 4] {
1597 // Process all 4 pairs
1598 [
1599 a[0].dot(b[0]),
1600 a[1].dot(b[1]),
1601 a[2].dot(b[2]),
1602 a[3].dot(b[3]),
1603 ]
1604 }
1605}
1606
1607impl Add for Vec3Fix {
1608 type Output = Self;
1609
1610 #[inline]
1611 fn add(self, rhs: Self) -> Self {
1612 Self {
1613 x: self.x + rhs.x,
1614 y: self.y + rhs.y,
1615 z: self.z + rhs.z,
1616 }
1617 }
1618}
1619
1620impl Sub for Vec3Fix {
1621 type Output = Self;
1622
1623 #[inline]
1624 fn sub(self, rhs: Self) -> Self {
1625 Self {
1626 x: self.x - rhs.x,
1627 y: self.y - rhs.y,
1628 z: self.z - rhs.z,
1629 }
1630 }
1631}
1632
1633impl Mul<Fix128> for Vec3Fix {
1634 type Output = Self;
1635
1636 #[inline]
1637 fn mul(self, rhs: Fix128) -> Self {
1638 self.scale(rhs)
1639 }
1640}
1641
1642impl Div<Fix128> for Vec3Fix {
1643 type Output = Self;
1644
1645 #[inline]
1646 fn div(self, rhs: Fix128) -> Self {
1647 Self {
1648 x: self.x / rhs,
1649 y: self.y / rhs,
1650 z: self.z / rhs,
1651 }
1652 }
1653}
1654
1655impl Neg for Vec3Fix {
1656 type Output = Self;
1657
1658 #[inline]
1659 fn neg(self) -> Self {
1660 Self {
1661 x: -self.x,
1662 y: -self.y,
1663 z: -self.z,
1664 }
1665 }
1666}
1667
1668impl core::fmt::Display for Vec3Fix {
1669 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1670 write!(f, "({}, {}, {})", self.x, self.y, self.z)
1671 }
1672}
1673
1674impl From<[Fix128; 3]> for Vec3Fix {
1675 #[inline]
1676 fn from(arr: [Fix128; 3]) -> Self {
1677 Self::new(arr[0], arr[1], arr[2])
1678 }
1679}
1680
1681impl From<Vec3Fix> for [Fix128; 3] {
1682 #[inline]
1683 fn from(v: Vec3Fix) -> Self {
1684 [v.x, v.y, v.z]
1685 }
1686}
1687
1688// ============================================================================
1689// QuatFix - Quaternion with Fixed-Point Components
1690// ============================================================================
1691
1692/// Quaternion using Fix128 components (for rotations)
1693///
1694/// Stored as (x, y, z, w) where w is the scalar part
1695#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
1696#[repr(C)]
1697pub struct QuatFix {
1698 /// X component (imaginary i)
1699 pub x: Fix128,
1700 /// Y component (imaginary j)
1701 pub y: Fix128,
1702 /// Z component (imaginary k)
1703 pub z: Fix128,
1704 /// W component (scalar/real part)
1705 pub w: Fix128,
1706}
1707
1708impl QuatFix {
1709 /// Identity quaternion (no rotation)
1710 pub const IDENTITY: Self = Self {
1711 x: Fix128::ZERO,
1712 y: Fix128::ZERO,
1713 z: Fix128::ZERO,
1714 w: Fix128::ONE,
1715 };
1716
1717 /// Create new quaternion
1718 #[inline]
1719 #[must_use]
1720 pub const fn new(x: Fix128, y: Fix128, z: Fix128, w: Fix128) -> Self {
1721 Self { x, y, z, w }
1722 }
1723
1724 /// Create from axis-angle representation
1725 #[must_use]
1726 pub fn from_axis_angle(axis: Vec3Fix, angle: Fix128) -> Self {
1727 let half_angle = angle.half();
1728 let (sin_ha, cos_ha) = half_angle.sin_cos();
1729 let axis_norm = axis.normalize();
1730 // no axis, no rotation: (0, 0, 0, cos) is not a unit quaternion and
1731 // would scale whatever it rotates by cos^2 (AUD-A-S1W5-023)
1732 if axis_norm == Vec3Fix::ZERO {
1733 return Self::IDENTITY;
1734 }
1735
1736 Self {
1737 x: axis_norm.x * sin_ha,
1738 y: axis_norm.y * sin_ha,
1739 z: axis_norm.z * sin_ha,
1740 w: cos_ha,
1741 }
1742 }
1743
1744 /// Quaternion multiplication (composition of rotations)
1745 #[allow(clippy::should_implement_trait)]
1746 #[must_use]
1747 pub fn mul(self, rhs: Self) -> Self {
1748 Self {
1749 x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
1750 y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
1751 z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w,
1752 w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
1753 }
1754 }
1755
1756 /// Conjugate (inverse for unit quaternions)
1757 #[inline]
1758 #[must_use]
1759 pub fn conjugate(self) -> Self {
1760 Self {
1761 x: -self.x,
1762 y: -self.y,
1763 z: -self.z,
1764 w: self.w,
1765 }
1766 }
1767
1768 /// Squared magnitude
1769 #[inline]
1770 #[must_use]
1771 pub fn length_squared(self) -> Fix128 {
1772 self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w
1773 }
1774
1775 /// Magnitude
1776 #[inline(always)]
1777 #[must_use]
1778 pub fn length(self) -> Fix128 {
1779 self.length_squared().sqrt()
1780 }
1781
1782 /// This rotation as a unit quaternion, for code that applies a stored
1783 /// rotation: a quaternion whose squared length is within `2^-32` of one
1784 /// is returned unchanged (bit for bit), any other is normalized, and the
1785 /// zero quaternion becomes the identity.
1786 ///
1787 /// `q v q*` scales by `|q|^2`, so applying a non-unit quaternion resizes
1788 /// what it rotates (AUD-A-S4W1-004, AUD-A-S5W3-004, AUD-A-S5W3-014).
1789 #[must_use]
1790 pub(crate) fn unit_rotation(self) -> Self {
1791 let tol = Fix128::from_raw(0, 1 << 32);
1792 if (self.length_squared() - Fix128::ONE).abs() <= tol {
1793 self
1794 } else {
1795 self.normalize()
1796 }
1797 }
1798
1799 /// Normalize to unit quaternion (reciprocal: 1 division + 4 multiplications)
1800 #[inline(always)]
1801 #[must_use]
1802 pub fn normalize(self) -> Self {
1803 let len = self.length();
1804 if len.is_zero() {
1805 Self::IDENTITY
1806 } else {
1807 let inv_len = Fix128::ONE / len;
1808 Self {
1809 x: self.x * inv_len,
1810 y: self.y * inv_len,
1811 z: self.z * inv_len,
1812 w: self.w * inv_len,
1813 }
1814 }
1815 }
1816
1817 /// Rotate a vector by this quaternion
1818 #[must_use]
1819 pub fn rotate_vec(self, v: Vec3Fix) -> Vec3Fix {
1820 // q * v * q^-1
1821 let qv = Self::new(v.x, v.y, v.z, Fix128::ZERO);
1822 let result = self.mul(qv).mul(self.conjugate());
1823 Vec3Fix::new(result.x, result.y, result.z)
1824 }
1825}
1826
1827impl core::fmt::Display for QuatFix {
1828 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1829 write!(f, "({}, {}, {}, {})", self.x, self.y, self.z, self.w)
1830 }
1831}
1832
1833impl From<[Fix128; 4]> for QuatFix {
1834 #[inline]
1835 fn from(arr: [Fix128; 4]) -> Self {
1836 Self::new(arr[0], arr[1], arr[2], arr[3])
1837 }
1838}
1839
1840impl From<QuatFix> for [Fix128; 4] {
1841 #[inline]
1842 fn from(q: QuatFix) -> Self {
1843 [q.x, q.y, q.z, q.w]
1844 }
1845}
1846
1847// ============================================================================
1848// 3x3 Matrix (Inertia Tensor)
1849// ============================================================================
1850
1851/// 3x3 Matrix for inertia tensors and rotations
1852#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
1853#[repr(C)]
1854pub struct Mat3Fix {
1855 /// Column 0
1856 pub col0: Vec3Fix,
1857 /// Column 1
1858 pub col1: Vec3Fix,
1859 /// Column 2
1860 pub col2: Vec3Fix,
1861}
1862
1863/// Why [`Mat3Fix::polar_rotation`] could not produce a rotation.
1864///
1865/// The three refusals and the two iteration outcomes are separate variants on
1866/// purpose: **the caller's next action is different for each**, and collapsing
1867/// them into one `None` was measured to mislead. The same crate already learned
1868/// this for the conjugate gradient, where `pᵀKp ≤ 0` was reported uniformly as
1869/// "under-constrained" until it was split into a null space (add constraints) and
1870/// a rounding floor (revisit the preconditioner or the tolerance). During this type's own
1871/// development a budget that was merely too small was read as a degeneracy and
1872/// cost a round of diagnosis, which is the same failure one layer up.
1873#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1874#[non_exhaustive]
1875pub enum PolarError {
1876 /// `det F ≤ 0`: the deformation has turned the element inside out.
1877 ///
1878 /// There is no rotation factor — the nearest orthogonal matrix is a reflection
1879 /// with `det = −1`. **Fix the mesh or the deformation**; a larger budget or a
1880 /// lower floor will not help. Returned whatever `det_floor` was given, so a
1881 /// caller passing a negative floor still cannot receive a reflection.
1882 Inverted,
1883 /// `det F ≤ det_floor`: too close to singular for the caller's policy.
1884 ///
1885 /// **Revisit the floor, or the element that produced this gradient.**
1886 Degenerate,
1887 /// An intermediate iterate was exactly singular, so it could not be inverted.
1888 ///
1889 /// Distinct from [`Self::Degenerate`] because the *input* passed the floor:
1890 /// what failed is the path, not the data. **Lower the floor's tolerance for
1891 /// ill-conditioning, or inspect the gradient.**
1892 SingularStep {
1893 /// Steps completed before the singular iterate.
1894 steps: u32,
1895 },
1896 /// `max_iters` was exhausted while the change was still above the settling
1897 /// bound.
1898 ///
1899 /// ⚠️ **The input is fine — raise the budget.** Measured requirements run to 21
1900 /// steps for a gradient with one singular value four decades from one, so a
1901 /// budget of 16 is too small for the general case; see
1902 /// [`Mat3Fix::polar_rotation`] for the table.
1903 NotConverged {
1904 /// Steps performed, equal to the `max_iters` that was given.
1905 steps: u32,
1906 },
1907}
1908
1909/// A 256-bit two's-complement integer `(high, low)`, used by
1910/// [`Mat3Fix::inverse`] to sum products exactly.
1911type Wide = (i128, u128);
1912
1913/// `floor(a·b / 2⁶⁴)` in raw units, exact: the full product of the two Q64.64
1914/// values, of which [`Mul`] keeps the low 128 bits.
1915fn wide_mul_floor(a: Fix128, b: Fix128) -> Wide {
1916 const LOW: u128 = u64::MAX as u128;
1917 let ra = ((a.hi as i128) << 64) | a.lo as i128;
1918 let rb = ((b.hi as i128) << 64) | b.lo as i128;
1919 let (ua, ub) = (ra.unsigned_abs(), rb.unsigned_abs());
1920 let (a1, a0) = (ua >> 64, ua & LOW);
1921 let (b1, b0) = (ub >> 64, ub & LOW);
1922 // |a|, |b| ≤ 2¹²⁷, so the magnitude is below 2²⁵⁴ and `hi` cannot overflow
1923 let (mid, mid_carry) = (a0 * b1).overflowing_add(a1 * b0);
1924 let (lo, lo_carry) = (a0 * b0).overflowing_add(mid << 64);
1925 let hi = a1 * b1 + (mid >> 64) + ((mid_carry as u128) << 64) + lo_carry as u128;
1926 let p = if (ra < 0) != (rb < 0) {
1927 wide_neg((hi as i128, lo))
1928 } else {
1929 (hi as i128, lo)
1930 };
1931 // Arithmetic shift right by 64 bits: floor on the two's-complement value
1932 (p.0 >> 64, ((p.0 as u128) << 64) | (p.1 >> 64))
1933}
1934
1935fn wide_add(x: Wide, y: Wide) -> Wide {
1936 let (lo, carry) = x.1.overflowing_add(y.1);
1937 (x.0.wrapping_add(y.0).wrapping_add(carry as i128), lo)
1938}
1939
1940fn wide_neg(x: Wide) -> Wide {
1941 wide_add((!x.0, !x.1), (0, 1))
1942}
1943
1944/// The value as a [`Fix128`], or `None` when it is outside `[-2⁶³, 2⁶³)`.
1945fn wide_fit(x: Wide) -> Option<Fix128> {
1946 (x.0 == (x.1 as i128) >> 127).then(|| Fix128::from_raw((x.1 >> 64) as i64, x.1 as u64))
1947}
1948
1949/// `p·q − r·s` with the products summed exactly; `None` when the difference
1950/// does not fit. Bit-identical to `p * q - r * s` whenever it fits.
1951fn wide_cofactor(p: Fix128, q: Fix128, r: Fix128, s: Fix128) -> Option<Fix128> {
1952 wide_fit(wide_add(
1953 wide_mul_floor(p, q),
1954 wide_neg(wide_mul_floor(r, s)),
1955 ))
1956}
1957
1958impl Mat3Fix {
1959 /// Identity matrix
1960 pub const IDENTITY: Self = Self {
1961 col0: Vec3Fix::UNIT_X,
1962 col1: Vec3Fix::UNIT_Y,
1963 col2: Vec3Fix::UNIT_Z,
1964 };
1965
1966 /// Zero matrix
1967 pub const ZERO: Self = Self {
1968 col0: Vec3Fix::ZERO,
1969 col1: Vec3Fix::ZERO,
1970 col2: Vec3Fix::ZERO,
1971 };
1972
1973 /// Create from columns
1974 #[inline]
1975 #[must_use]
1976 pub const fn from_cols(col0: Vec3Fix, col1: Vec3Fix, col2: Vec3Fix) -> Self {
1977 Self { col0, col1, col2 }
1978 }
1979
1980 /// Create diagonal matrix
1981 #[inline]
1982 #[must_use]
1983 pub const fn diagonal(x: Fix128, y: Fix128, z: Fix128) -> Self {
1984 Self {
1985 col0: Vec3Fix::new(x, Fix128::ZERO, Fix128::ZERO),
1986 col1: Vec3Fix::new(Fix128::ZERO, y, Fix128::ZERO),
1987 col2: Vec3Fix::new(Fix128::ZERO, Fix128::ZERO, z),
1988 }
1989 }
1990
1991 /// Matrix-vector multiplication
1992 #[inline]
1993 #[must_use]
1994 pub fn mul_vec(self, v: Vec3Fix) -> Vec3Fix {
1995 Vec3Fix::new(
1996 self.col0.x * v.x + self.col1.x * v.y + self.col2.x * v.z,
1997 self.col0.y * v.x + self.col1.y * v.y + self.col2.y * v.z,
1998 self.col0.z * v.x + self.col1.z * v.y + self.col2.z * v.z,
1999 )
2000 }
2001
2002 /// Transpose
2003 #[inline]
2004 #[must_use]
2005 pub const fn transpose(self) -> Self {
2006 Self {
2007 col0: Vec3Fix::new(self.col0.x, self.col1.x, self.col2.x),
2008 col1: Vec3Fix::new(self.col0.y, self.col1.y, self.col2.y),
2009 col2: Vec3Fix::new(self.col0.z, self.col1.z, self.col2.z),
2010 }
2011 }
2012
2013 /// Scale all elements
2014 #[inline]
2015 #[must_use]
2016 pub fn scale(self, s: Fix128) -> Self {
2017 Self {
2018 col0: self.col0.scale(s),
2019 col1: self.col1.scale(s),
2020 col2: self.col2.scale(s),
2021 }
2022 }
2023
2024 /// Matrix-matrix multiplication (self * rhs)
2025 #[inline]
2026 #[must_use]
2027 pub fn mul_mat(self, rhs: Self) -> Self {
2028 Self {
2029 col0: self.mul_vec(rhs.col0),
2030 col1: self.mul_vec(rhs.col1),
2031 col2: self.mul_vec(rhs.col2),
2032 }
2033 }
2034
2035 /// Determinant
2036 #[inline]
2037 #[must_use]
2038 pub fn determinant(self) -> Fix128 {
2039 self.col0.x * (self.col1.y * self.col2.z - self.col1.z * self.col2.y)
2040 - self.col1.x * (self.col0.y * self.col2.z - self.col0.z * self.col2.y)
2041 + self.col2.x * (self.col0.y * self.col1.z - self.col0.z * self.col1.y)
2042 }
2043
2044 /// Inverse matrix.
2045 ///
2046 /// Returns `None` if the matrix is singular **or if the inverse cannot be
2047 /// computed inside the Q64.64 range**: a cofactor, the determinant, `1 / det`
2048 /// (`|det| ≤ 2⁻⁶³`) or one of the entries `cofactor / det` does not fit.
2049 /// Earlier versions wrapped in the out-of-range cases and returned `Some` with flipped
2050 /// signs (e.g. `diag(2²¹, 2²¹, 2²¹)`, whose determinant is `2⁶³`).
2051 ///
2052 /// The products inside each cofactor and inside the determinant are summed
2053 /// exactly (256-bit), so a single product beyond `2⁶³` that cancels out does
2054 /// not make the result `None`: in that case the wrapping arithmetic was
2055 /// already exact modulo `2¹²⁸`, and the result is bit-identical to the
2056 /// unchecked formula, as it is for every input whose intermediates fit.
2057 ///
2058 /// A matrix whose entries are all small (largest below `1/2`) and whose
2059 /// determinant is below `2⁻³²` is scaled up by a power of two first, so its
2060 /// determinant is not quantised away (`diag(1e-5, 1e-5, 1e-5)` had a relative
2061 /// error of 5e-5, and below `2⁻⁶³` the result was `None` although the inverse
2062 /// fits): `A⁻¹ = 2ᵏ (2ᵏ A)⁻¹`, both scalings exact. Every other matrix gets the
2063 /// unscaled result, bit for bit. `k` follows the largest entry, so the gain
2064 /// is for matrices whose entries are of similar size: one with entries many
2065 /// orders apart (`diag(3·2⁻⁶⁴, 2⁻³³, 3·2⁻⁶⁴)`) still has its determinant
2066 /// quantised and can be off by a large relative error (12.5 % there), where
2067 /// it used to be `None`.
2068 #[must_use]
2069 pub fn inverse(self) -> Option<Self> {
2070 let direct = self.inverse_and_det();
2071 if let Some((inv, det)) = direct {
2072 if det.abs() >= Fix128::from_raw(0, 1u64 << 32) {
2073 return Some(inv);
2074 }
2075 }
2076 let largest = [self.col0, self.col1, self.col2]
2077 .iter()
2078 .flat_map(|v| [v.x.abs(), v.y.abs(), v.z.abs()])
2079 .fold(Fix128::ZERO, |m, x| if x > m { x } else { m });
2080 let half = Fix128::from_raw(0, 1u64 << 63);
2081 if largest.is_zero() || largest >= half {
2082 return direct.map(|(inv, _)| inv);
2083 }
2084 // the shift that brings `largest` into [1/2, 1), at most 62
2085 let mut k = 0u32;
2086 let mut scaled = largest;
2087 while scaled < half && k < 62 {
2088 scaled = scaled + scaled;
2089 k += 1;
2090 }
2091 let up = Fix128::from_int(1i64 << k);
2092 let scale = |v: Vec3Fix| Vec3Fix::new(v.x * up, v.y * up, v.z * up);
2093 let (inv, _) = Self {
2094 col0: scale(self.col0),
2095 col1: scale(self.col1),
2096 col2: scale(self.col2),
2097 }
2098 .inverse_and_det()?;
2099 let back = |v: Vec3Fix| -> Option<Vec3Fix> {
2100 Some(Vec3Fix::new(
2101 v.x.checked_mul(up)?,
2102 v.y.checked_mul(up)?,
2103 v.z.checked_mul(up)?,
2104 ))
2105 };
2106 Some(Self {
2107 col0: back(inv.col0)?,
2108 col1: back(inv.col1)?,
2109 col2: back(inv.col2)?,
2110 })
2111 }
2112
2113 /// [`Self::inverse`] without the rescaling, with the determinant it used.
2114 fn inverse_and_det(self) -> Option<(Self, Fix128)> {
2115 let (a, b, c) = (self.col0, self.col1, self.col2);
2116
2117 // Cofactor matrix transposed (adjugate)
2118 let c00 = wide_cofactor(b.y, c.z, b.z, c.y)?;
2119 let c01 = wide_cofactor(a.z, c.y, a.y, c.z)?;
2120 let c02 = wide_cofactor(a.y, b.z, a.z, b.y)?;
2121
2122 let c10 = wide_cofactor(b.z, c.x, b.x, c.z)?;
2123 let c11 = wide_cofactor(a.x, c.z, a.z, c.x)?;
2124 let c12 = wide_cofactor(a.z, b.x, a.x, b.z)?;
2125
2126 let c20 = wide_cofactor(b.x, c.y, b.y, c.x)?;
2127 let c21 = wide_cofactor(a.y, c.x, a.x, c.y)?;
2128 let c22 = wide_cofactor(a.x, b.y, a.y, b.x)?;
2129
2130 // Same expansion as `determinant`, summed exactly
2131 let minor = wide_cofactor(a.y, c.z, a.z, c.y)?;
2132 let det = wide_fit(wide_add(
2133 wide_add(
2134 wide_mul_floor(a.x, c00),
2135 wide_neg(wide_mul_floor(b.x, minor)),
2136 ),
2137 wide_mul_floor(c.x, c02),
2138 ))?;
2139 if det.is_zero() {
2140 return None;
2141 }
2142 // `1 / det` is `2¹²⁸ / raw` in raw units, which reaches `2¹²⁷` for
2143 // `|raw| ≤ 2` (`|det| ≤ 2⁻⁶³`) and would wrap inside the division
2144 let det_raw = ((det.hi as i128) << 64) | det.lo as i128;
2145 if det_raw.unsigned_abs() <= 2 {
2146 return None;
2147 }
2148 let inv_det = Fix128::ONE / det;
2149 let e = |x: Fix128| wide_fit(wide_mul_floor(x, inv_det));
2150
2151 Some((
2152 Self {
2153 col0: Vec3Fix::new(e(c00)?, e(c01)?, e(c02)?),
2154 col1: Vec3Fix::new(e(c10)?, e(c11)?, e(c12)?),
2155 col2: Vec3Fix::new(e(c20)?, e(c21)?, e(c22)?),
2156 },
2157 det,
2158 ))
2159 }
2160
2161 /// Sum of two matrices, component by component.
2162 #[inline]
2163 fn add_mat(self, rhs: Self) -> Self {
2164 Self {
2165 col0: self.col0 + rhs.col0,
2166 col1: self.col1 + rhs.col1,
2167 col2: self.col2 + rhs.col2,
2168 }
2169 }
2170
2171 /// Smallest power of two at or above a **positive** `v`.
2172 ///
2173 /// Only used to pick an exact divisor for the polar iteration, which needs the
2174 /// divisor within a factor of two of `v` and nothing more, so when doubling
2175 /// the leading bit would leave the range the leading bit itself is returned
2176 /// instead of failing. `Fix128::ONE` for a non-positive `v`, which
2177 /// [`Self::polar_rotation`] has already rejected before it gets here.
2178 fn ceil_power_of_two(v: Fix128) -> Fix128 {
2179 if v <= Fix128::ZERO {
2180 return Fix128::ONE;
2181 }
2182 let raw = ((v.hi as u64 as u128) << 64) | v.lo as u128;
2183 let bits = 128 - raw.leading_zeros();
2184 let floor_pow = 1u128 << (bits - 1);
2185 let pow = if raw == floor_pow || bits >= 127 {
2186 floor_pow
2187 } else {
2188 floor_pow << 1
2189 };
2190 Fix128 {
2191 hi: (pow >> 64) as i64,
2192 lo: pow as u64,
2193 }
2194 }
2195
2196 /// Largest absolute component, as a scale for this matrix.
2197 ///
2198 /// Useful for turning an intrinsic, scale-free criterion into an absolute one
2199 /// at the call site: a determinant scales as the **cube** of the matrix, so
2200 /// `‖F‖³` is the quantity a determinant floor has to be expressed against.
2201 /// See [`Self::polar_rotation`].
2202 #[must_use]
2203 pub fn max_abs_component(self) -> Fix128 {
2204 let mut worst = Fix128::ZERO;
2205 for c in [self.col0, self.col1, self.col2] {
2206 for v in [c.x, c.y, c.z] {
2207 let a = v.abs();
2208 if a > worst {
2209 worst = a;
2210 }
2211 }
2212 }
2213 worst
2214 }
2215
2216 /// Rotation factor `R` of the polar decomposition `F = R·U`, where `U` is
2217 /// symmetric positive definite.
2218 ///
2219 /// This is the piece a co-rotational formulation needs: the rotation a
2220 /// deformation gradient contains, separated from the stretch, so an element's
2221 /// small-strain stiffness can be rotated into the deformed frame instead of
2222 /// reading a large rotation as a strain.
2223 ///
2224 /// # Method
2225 ///
2226 /// Higham's Newton iteration for the orthogonal polar factor,
2227 ///
2228 /// ```text
2229 /// R₀ = F, R_{k+1} = ½ (R_k + R_k⁻ᵀ)
2230 /// ```
2231 ///
2232 /// which needs only [`Self::inverse`], [`Self::transpose`], addition and a
2233 /// halving — no square root, no trigonometry, no eigensolver. Every operation
2234 /// is one [`Fix128`] performs exactly, so two runs on different targets
2235 /// produce bit-identical rotations and nothing here can reach a platform
2236 /// `libm`. Nothing allocates, so this is available in `no_std` builds.
2237 ///
2238 /// # Convergence, measured
2239 ///
2240 /// Quadratic near the answer — the number of correct bits doubles per step —
2241 /// and linear while a singular value is far from one, where the iteration
2242 /// halves the distance per step. Measured change per step for a general
2243 /// gradient:
2244 ///
2245 /// ```text
2246 /// 4.17e-1 → 8.01e-2 → 3.20e-3 → 5.12e-6 → 1.31e-11 → 0
2247 /// ```
2248 ///
2249 /// Measured step counts to the settling bound
2250 /// (`tests/analytic_polar_decomposition.rs` prints the first four; the rest were
2251 /// measured by raising `max_iters` until the call succeeded):
2252 ///
2253 /// | `F` | steps |
2254 /// |---|---|
2255 /// | simple shear `γ = 0.4` | 6 |
2256 /// | `diag(1.5, 0.8, 1.1)` | 7 |
2257 /// | rotation ∘ `diag(1.3, 0.9, 1.05)` | 7 |
2258 /// | a general full matrix | 7 |
2259 /// | `diag(1, 1, 0.1)` | 9 |
2260 /// | `diag(1e3, 1e3, 1e2)` | 9 |
2261 /// | `diag(4, 1, 0.25)` | 10 |
2262 /// | `diag(1e-3, 1e-3, 1e-4)` | 10 |
2263 /// | `diag(1, 1, 0.01)` | 13 |
2264 /// | `diag(1, 1, 1e-4)` | 19 |
2265 ///
2266 /// So **`max_iters = 32` covers everything measured with margin, and 16 is not
2267 /// enough** — the flattest gradient the intrinsic floor still accepts needs 19.
2268 ///
2269 /// The two uniformly scaled rows are the point of the normalisation below:
2270 /// `diag(1e3, 1e3, 1e2)` and `diag(1e-3, 1e-3, 1e-4)` are the same shape six
2271 /// decades apart and both finish in 9 or 10 steps, so **the budget a caller has
2272 /// to pass does not depend on the units its field is expressed in**. What the
2273 /// count does depend on is the *spread*: `diag(1, 1, 1e-4)` is slow because one
2274 /// singular value is four decades from the others, and no uniform scaling can
2275 /// fix that.
2276 ///
2277 /// An exactly representable rotation is returned *unchanged*: its largest
2278 /// component lies in `[1/√3, 1]` so the divisor is exactly one, and for
2279 /// orthogonal `F`, `F⁻ᵀ = F`, so the first step is `½(F + F)` with no
2280 /// rounding.
2281 ///
2282 /// # Why the iteration stops on a change of a few bits, not on no change
2283 ///
2284 /// ⚠️ **There is in general no matrix this iteration leaves unchanged.**
2285 /// [`Fix128`] multiplication **truncates** rather than rounding to nearest, so
2286 /// the rounded map carries a systematic bias of a fraction of a unit in the
2287 /// last place per step, and the iterate keeps creeping after it has converged
2288 /// to everything the representation can hold. Measured on a rotation about x
2289 /// built from `3/5` and `4/5` (whose determinant is `1 + 819·2⁻⁶⁴`, not
2290 /// exactly one), tracked for 24 steps:
2291 ///
2292 /// ```text
2293 /// step 1: col1.y lo=…730314 col1.z lo=…641784
2294 /// step 4: col1.y lo=…730312 col1.z lo=…641785
2295 /// step 9: col1.y lo=…730310 col1.z lo=…641786
2296 /// step 14: col1.y lo=…730309 col1.z lo=…641787 ← no fixed point, no cycle
2297 /// ```
2298 ///
2299 /// So `next == r` is not a reachable stopping condition, and neither is a
2300 /// period-two cycle: both were tried and both burned the whole budget, which
2301 /// the caller then received as a failure to decompose. The drift is **1 unit
2302 /// in the last place per one or two steps**; the iteration stops at a change of
2303 /// **4**, which is above the drift with margin and far below anything a caller
2304 /// of a rotation matrix can observe.
2305 ///
2306 /// Rounding to nearest would not remove the need for this rule — it would make
2307 /// the bias unbiased rather than systematic, and the map would still be a
2308 /// discretisation with no guaranteed fixed point.
2309 ///
2310 /// The bound is scale free because the iterate is normalised: `‖r‖ ≈ 1`, so
2311 /// "four units in the last place" is a relative quantity, not the absolute
2312 /// threshold that always breaks at some other problem size.
2313 ///
2314 /// # Why `det_floor` is a parameter and not a constant
2315 ///
2316 /// A matrix does not know the scale of the field it came from, so a floor
2317 /// written *inside* this function would be an absolute threshold — the shape
2318 /// that always breaks at some problem size. The policy belongs to the caller:
2319 /// a finite-element assembly derives it from the element volume, and a caller
2320 /// that only wants the intrinsic numerical criterion passes
2321 ///
2322 /// ```text
2323 /// det_floor = ε · ‖F‖³ (Self::max_abs_component gives ‖F‖)
2324 /// ```
2325 ///
2326 /// which is scale free, because both sides scale as the cube of the matrix.
2327 ///
2328 /// # Returns `None`
2329 ///
2330 /// - When `det F ≤ 0`, **whatever `det_floor` is given**. A non-positive
2331 /// determinant means the deformation has turned the element inside out, and
2332 /// the nearest orthogonal matrix is then a *reflection* with `det = −1`.
2333 /// Returning it would silently flip the sign of every stress component, so
2334 /// this rejection is not delegated to the caller's floor: a caller that
2335 /// passes a negative `det_floor` still cannot get a reflection back.
2336 /// - When `det F ≤ det_floor`.
2337 /// - When `max_iters` is exhausted, or an intermediate matrix is exactly
2338 /// singular.
2339 ///
2340 /// # Errors
2341 ///
2342 /// One of the four [`PolarError`] variants, which are kept separate because the
2343 /// caller's next action differs for each: [`PolarError::Inverted`] and
2344 /// [`PolarError::Degenerate`] are about the data, [`PolarError::NotConverged`]
2345 /// is about the budget, and [`PolarError::SingularStep`] is about the path.
2346 pub fn polar_rotation(self, det_floor: Fix128, max_iters: u32) -> Result<Self, PolarError> {
2347 self.polar_rotation_steps(det_floor, max_iters)
2348 .map(|(r, _)| r)
2349 }
2350
2351 /// [`Self::polar_rotation`], also reporting how many iterations it took.
2352 ///
2353 /// Crate-internal so the step count can be pinned by a test without becoming
2354 /// part of the published surface.
2355 pub(crate) fn polar_rotation_steps(
2356 self,
2357 det_floor: Fix128,
2358 max_iters: u32,
2359 ) -> Result<(Self, u32), PolarError> {
2360 let det = self.determinant();
2361 // Unconditional, and checked before the caller's floor: a reflection must
2362 // not be reachable through `det_floor`, however that floor was chosen.
2363 if det <= Fix128::ZERO {
2364 return Err(PolarError::Inverted);
2365 }
2366 if det <= det_floor {
2367 return Err(PolarError::Degenerate);
2368 }
2369
2370 // Normalise by a **power of two** at or above ‖F‖ before iterating.
2371 //
2372 // The polar factor is invariant under a positive scaling (`F/s = R·(U/s)`
2373 // and `U/s` is still symmetric positive definite), but the *iteration
2374 // count* is not: Higham's iteration is linear while the singular values
2375 // are far from one, halving the largest per step. Measured, `diag(1, 1, 0.1)`
2376 // needs 9 steps and the six-decades-smaller `diag(1e-3, 1e-3, 1e-4)` needs
2377 // 10 — without normalisation the second did not converge in 16 at all, so a
2378 // caller's budget would have depended on the units its field is expressed
2379 // in.
2380 //
2381 // Dividing by ‖F‖ itself would fix the step count and **break
2382 // idempotence**: the iteration settles on a fixed point of the rounded
2383 // map, which for `diag(1.5, 0.8, 1.1)` is `diag(1 − 2⁻⁶⁴, …)`, and
2384 // dividing *that* by its own largest component lands on the neighbouring
2385 // fixed point `I`. Rounding the divisor up to a power of two avoids it:
2386 // every column of an orthogonal matrix is a unit vector, so its largest
2387 // component is at least `1/√3 ≈ 0.577` and at most `1`, which rounds up to
2388 // exactly one — a rotation is therefore never rescaled, and feeding one
2389 // back in returns it bit for bit.
2390 // `det > 0` already rules out a zero matrix, so the scale is positive.
2391 let divisor = Self::ceil_power_of_two(self.max_abs_component());
2392 let mut r = if divisor == Fix128::ONE {
2393 self
2394 } else {
2395 Self {
2396 col0: self.col0 / divisor,
2397 col1: self.col1 / divisor,
2398 col2: self.col2 / divisor,
2399 }
2400 };
2401 /// Change at which the iteration is considered settled, in units of
2402 /// `2⁻⁶⁴`. The measured drift of the truncating map is one per one or two
2403 /// steps, so four is above it with margin; the iterate is normalised to
2404 /// `‖r‖ ≈ 1`, which is what makes a bit count a relative quantity.
2405 const SETTLED: Fix128 = Fix128 { hi: 0, lo: 4 };
2406
2407 let half = Fix128 { hi: 0, lo: 1 << 63 }; // exactly 0.5
2408 let mut steps = 0_u32;
2409 while steps < max_iters {
2410 let inverse = match r.inverse() {
2411 Some(i) => i,
2412 None => return Err(PolarError::SingularStep { steps }),
2413 };
2414 let next = r.add_mat(inverse.transpose()).scale(half);
2415 steps += 1;
2416 let change = next.add_mat(r.scale(Fix128::NEG_ONE)).max_abs_component();
2417 r = next;
2418 if change <= SETTLED {
2419 return Ok((r, steps));
2420 }
2421 }
2422 Err(PolarError::NotConverged { steps })
2423 }
2424}
2425
2426// ============================================================================
2427// Runtime SIMD Width Detection
2428// ============================================================================
2429
2430/// Returns the SIMD width for the current build target.
2431///
2432/// Dispatches at compile time based on enabled features and target architecture:
2433/// - AVX2 (`x86_64)`: 8 lanes (256-bit / 32-bit float)
2434/// - SSE2 / no-AVX2 (`x86_64` without avx2): 4 lanes (128-bit)
2435/// - NEON (aarch64): 4 lanes (128-bit)
2436/// - Other targets with `simd` (wasm32, ...): 1, no SIMD path, the scalar code runs
2437/// - Scalar fallback (no `simd` feature): 1
2438///
2439/// Use the [`SIMD_WIDTH`] constant for a zero-cost compile-time value.
2440#[inline(always)]
2441#[must_use]
2442pub const fn simd_width() -> usize {
2443 #[cfg(all(feature = "simd", target_arch = "x86_64", target_feature = "avx2"))]
2444 {
2445 8
2446 }
2447 #[cfg(all(feature = "simd", target_arch = "x86_64", not(target_feature = "avx2")))]
2448 {
2449 4
2450 }
2451 #[cfg(all(feature = "simd", target_arch = "aarch64"))]
2452 {
2453 4
2454 }
2455 // no SIMD path for other targets (wasm32, ...): the scalar code runs
2456 #[cfg(all(
2457 feature = "simd",
2458 not(any(target_arch = "x86_64", target_arch = "aarch64"))
2459 ))]
2460 {
2461 1
2462 }
2463 #[cfg(not(feature = "simd"))]
2464 {
2465 1
2466 }
2467}
2468
2469/// Compile-time SIMD lane width for the current build target.
2470///
2471/// Equals `simd_width()`. Use this for array sizes and loop-unroll factors.
2472pub const SIMD_WIDTH: usize = simd_width();
2473
2474// ============================================================================
2475// Branchless Helpers
2476// ============================================================================
2477
2478/// Branchless select: returns `a` if `condition` is true, `b` otherwise.
2479///
2480/// Uses a bitwise mask approach identical to CMOV — no branch, no pipeline flush.
2481/// The result is bit-exact identical to `if condition { a } else { b }`.
2482///
2483/// # Safety
2484///
2485/// This is always safe. The mask is constructed from `-(condition as i64)`,
2486/// which is `0xFFFFFFFF_FFFFFFFF` when true and `0x00000000_00000000` when false.
2487/// Bitwise AND/OR then selects the correct limbs without any conditional instruction.
2488#[inline(always)]
2489#[must_use]
2490pub(crate) const fn select_fix128(condition: bool, a: Fix128, b: Fix128) -> Fix128 {
2491 // mask = 0xFFFF...FFFF when condition is true, 0x0000...0000 when false
2492 let mask = -(condition as i64) as u64;
2493 let inv_mask = !mask;
2494
2495 // Select hi (signed i64): cast to u64 for bitwise ops, cast back
2496 let hi = ((a.hi as u64 & mask) | (b.hi as u64 & inv_mask)) as i64;
2497 // Select lo (unsigned u64): direct bitwise ops
2498 let lo = (a.lo & mask) | (b.lo & inv_mask);
2499
2500 Fix128 { hi, lo }
2501}
2502
2503/// Branchless select for `Vec3Fix`.
2504///
2505/// Returns `a` if `condition` is true, `b` otherwise.
2506/// Applies `select_fix128` component-wise.
2507#[inline(always)]
2508#[must_use]
2509pub(crate) const fn select_vec3(condition: bool, a: Vec3Fix, b: Vec3Fix) -> Vec3Fix {
2510 Vec3Fix {
2511 x: select_fix128(condition, a.x, b.x),
2512 y: select_fix128(condition, a.y, b.y),
2513 z: select_fix128(condition, a.z, b.z),
2514 }
2515}
2516
2517impl core::ops::Mul<Self> for Mat3Fix {
2518 type Output = Self;
2519
2520 fn mul(self, rhs: Self) -> Self {
2521 Self {
2522 col0: self.mul_vec(rhs.col0),
2523 col1: self.mul_vec(rhs.col1),
2524 col2: self.mul_vec(rhs.col2),
2525 }
2526 }
2527}
2528
2529#[cfg(test)]
2530mod tests {
2531 use super::*;
2532 #[cfg(not(feature = "std"))]
2533 use alloc::{format, vec::Vec};
2534
2535 /// `rescaled_direction`: untouched inside `[2^-24, 2^24]`, an exact
2536 /// power-of-two scaling to a largest component in `[1, 2)` outside it.
2537 #[test]
2538 fn rescaled_direction_keeps_the_window_and_scales_exactly_outside_it() {
2539 let v = |x: i64, y: i64, z: i64| {
2540 Vec3Fix::new(
2541 Fix128::from_int(x),
2542 Fix128::from_int(y),
2543 Fix128::from_int(z),
2544 )
2545 };
2546 // inside the window: bit for bit the same, at both ends and in between
2547 for d in [
2548 v(3, -4, 5),
2549 Vec3Fix::new(Fix128::from_raw(0, 1 << 40), Fix128::ZERO, Fix128::ZERO), // 2^-24
2550 Vec3Fix::new(Fix128::from_raw(1 << 24, 0), Fix128::ZERO, Fix128::ZERO), // 2^24
2551 Vec3Fix::new(Fix128::from_ratio(1, 1_000_000), Fix128::ZERO, Fix128::ZERO),
2552 ] {
2553 assert_eq!(d.rescaled_direction(), d);
2554 }
2555 assert_eq!(Vec3Fix::ZERO.rescaled_direction(), Vec3Fix::ZERO);
2556 // tiny: (3, -4, 1) * 2^-40 -> (3, -4, 1) / 4, exact (largest 1.0 in [1, 2))
2557 let tiny = Vec3Fix::new(
2558 Fix128::from_raw(0, 3 << 24),
2559 -Fix128::from_raw(0, 4 << 24),
2560 Fix128::from_raw(0, 1 << 24),
2561 );
2562 assert_eq!(
2563 tiny.rescaled_direction(),
2564 Vec3Fix::new(
2565 Fix128::from_ratio(3, 4),
2566 -Fix128::ONE,
2567 Fix128::from_ratio(1, 4)
2568 )
2569 );
2570 // huge: (2^40, 2^39, 0) -> (1, 1/2, 0), exact
2571 let huge = Vec3Fix::new(
2572 Fix128::from_raw(1 << 40, 0),
2573 Fix128::from_raw(1 << 39, 0),
2574 Fix128::ZERO,
2575 );
2576 assert_eq!(
2577 huge.rescaled_direction(),
2578 Vec3Fix::new(Fix128::ONE, Fix128::from_ratio(1, 2), Fix128::ZERO)
2579 );
2580 }
2581
2582 /// `unit_rotation`: a unit quaternion (as produced by `from_axis_angle`,
2583 /// not exactly of length one) is kept bit for bit, a non-unit one is
2584 /// normalized, the zero quaternion is the identity.
2585 #[test]
2586 fn unit_rotation_keeps_unit_inputs_and_normalizes_the_rest() {
2587 let q = QuatFix::from_axis_angle(
2588 Vec3Fix::new(Fix128::ONE, Fix128::from_int(2), Fix128::from_int(3)),
2589 Fix128::from_ratio(7, 10),
2590 );
2591 assert_eq!(q.unit_rotation(), q);
2592 assert_eq!(QuatFix::IDENTITY.unit_rotation(), QuatFix::IDENTITY);
2593 let two = QuatFix::new(
2594 Fix128::ZERO,
2595 Fix128::ZERO,
2596 Fix128::ZERO,
2597 Fix128::from_int(2),
2598 );
2599 assert_eq!(two.unit_rotation(), QuatFix::IDENTITY);
2600 let zero = QuatFix::new(Fix128::ZERO, Fix128::ZERO, Fix128::ZERO, Fix128::ZERO);
2601 assert_eq!(zero.unit_rotation(), QuatFix::IDENTITY);
2602 }
2603
2604 #[test]
2605 #[allow(clippy::disallowed_methods)] // f64::powf is the oracle
2606 fn powf_pos_matches_f64_reference() {
2607 let mut worst = 0.0f64;
2608 for bi in 0..40 {
2609 let b = 1e-3 * 1e6f64.powf(bi as f64 / 39.0); // 1e-3 .. 1e3
2610 for ei in 0..=32 {
2611 let e = 8.0 * ei as f64 / 32.0;
2612 let got = Fix128::from_f64(b).powf_pos(Fix128::from_f64(e)).to_f64();
2613 let want = b.powf(e);
2614 if want < 1e-9 || want > 1e15 {
2615 continue; // Fix128 has 2⁻⁶⁴ absolute resolution: below 1e-9 the
2616 // relative error is dominated by truncation
2617 }
2618 let rel = ((got - want) / want).abs();
2619 worst = worst.max(rel);
2620 assert!(rel < 1e-6, "{b}^{e} = {got} vs {want} (rel {rel:e})");
2621 }
2622 }
2623 assert!(worst > 0.0, "sweep ran");
2624 // exact cases
2625 assert_eq!(
2626 Fix128::from_int(2).powf_pos(Fix128::from_int(10)),
2627 Fix128::from_int(1024)
2628 );
2629 assert_eq!(
2630 Fix128::from_int(9).powf_pos(Fix128::from_ratio(1, 2)),
2631 Fix128::from_int(3)
2632 );
2633 assert_eq!(Fix128::from_int(5).powf_pos(Fix128::ZERO), Fix128::ONE);
2634 assert_eq!(Fix128::ZERO.powf_pos(Fix128::ONE), Fix128::ZERO);
2635 assert_eq!(Fix128::from_int(-2).powf_pos(Fix128::ONE), Fix128::ZERO);
2636 assert_eq!(
2637 Fix128::from_int(2).powf_pos(Fix128::from_int(-1)),
2638 Fix128::ZERO
2639 );
2640 }
2641
2642 #[test]
2643 #[allow(clippy::disallowed_methods)] // f64::exp is the oracle
2644 fn exp_matches_f64_reference() {
2645 for i in 0..=400 {
2646 let x = -40.0 + 80.0 * f64::from(i) / 400.0;
2647 let got = Fix128::from_f64(x).exp().to_f64();
2648 let want = x.exp();
2649 if want < 1e-9 {
2650 continue; // below the 2⁻⁶⁴ resolution floor in relative terms
2651 }
2652 let rel = ((got - want) / want).abs();
2653 assert!(rel < 2e-6, "exp({x}) = {got} vs {want} (rel {rel:e})");
2654 }
2655 assert_eq!(Fix128::ZERO.exp(), Fix128::ONE);
2656 assert!(Fix128::from_int(-50).exp().is_zero());
2657 assert_eq!(
2658 Fix128::from_int(50).exp(),
2659 Fix128::from_raw(i64::MAX, u64::MAX)
2660 );
2661 // exp(1) = e to 1e-6
2662 assert!((Fix128::ONE.exp().to_f64() - core::f64::consts::E).abs() < 3e-6);
2663 }
2664
2665 #[test]
2666 fn div_by_zero_is_zero_and_checked_div_is_none() {
2667 let x = Fix128::from_int(7);
2668 assert_eq!(x / Fix128::ZERO, Fix128::ZERO);
2669 assert_eq!(Fix128::ZERO / Fix128::ZERO, Fix128::ZERO);
2670 assert_eq!(x.checked_div(Fix128::ZERO), None);
2671 assert_eq!(
2672 x.checked_div(Fix128::from_int(2)),
2673 Some(Fix128::from_ratio(7, 2))
2674 );
2675 assert_eq!(
2676 Fix128::from_ratio(-1, 3).checked_div(Fix128::from_ratio(2, 7)),
2677 Some(Fix128::from_ratio(-1, 3) / Fix128::from_ratio(2, 7))
2678 );
2679 }
2680
2681 #[test]
2682 fn fix128_mul_wraps_on_integer_overflow() {
2683 // 2^62 * 4 = 2^64 → integer part wraps modulo 2^64 → 0.
2684 let a = Fix128::from_int(1 << 62);
2685 let b = Fix128::from_int(4);
2686 assert_eq!(a * b, Fix128::ZERO);
2687 // 2^62 * 3 = 3·2^62 = 2^63 + 2^62 → wraps to -2^63 + 2^62 = -2^62.
2688 assert_eq!(a * Fix128::from_int(3), Fix128::from_int(-(1 << 62)));
2689 }
2690
2691 #[test]
2692 fn fix128_mul_truncates_toward_negative_infinity() {
2693 // -2^-64 is the raw pattern {hi: -1, lo: u64::MAX}.
2694 let tiny_neg = Fix128::from_raw(-1, u64::MAX);
2695 let half = Fix128::from_ratio(1, 2);
2696 // Exact value -2^-65 is not representable; floor gives -2^-64.
2697 assert_eq!(tiny_neg * half, tiny_neg);
2698 // Positive counterpart truncates to 0.
2699 let tiny_pos = Fix128::from_raw(0, 1);
2700 assert_eq!(tiny_pos * half, Fix128::ZERO);
2701 }
2702
2703 #[test]
2704 fn test_fix128_basic_ops() {
2705 let a = Fix128::from_int(5);
2706 let b = Fix128::from_int(3);
2707
2708 let sum = a + b;
2709 assert_eq!(sum.hi, 8);
2710 assert_eq!(sum.lo, 0);
2711
2712 let diff = a - b;
2713 assert_eq!(diff.hi, 2);
2714 assert_eq!(diff.lo, 0);
2715 }
2716
2717 #[test]
2718 fn test_fix128_mul() {
2719 let a = Fix128::from_int(6);
2720 let b = Fix128::from_int(7);
2721 let product = a * b;
2722 assert_eq!(product.hi, 42);
2723 }
2724
2725 #[test]
2726 fn test_fix128_div() {
2727 let a = Fix128::from_int(42);
2728 let b = Fix128::from_int(6);
2729 let quot = a / b;
2730 assert_eq!(quot.hi, 7);
2731 }
2732
2733 #[test]
2734 fn test_fix128_neg() {
2735 let a = Fix128::from_int(5);
2736 let neg_a = -a;
2737 assert_eq!(neg_a.hi, -5);
2738
2739 let sum = a + neg_a;
2740 assert!(sum.is_zero());
2741 }
2742
2743 #[test]
2744 fn test_vec3_dot() {
2745 let a = Vec3Fix::from_int(1, 2, 3);
2746 let b = Vec3Fix::from_int(4, 5, 6);
2747 let dot = a.dot(b);
2748 // 1*4 + 2*5 + 3*6 = 4 + 10 + 18 = 32
2749 assert_eq!(dot.hi, 32);
2750 }
2751
2752 #[test]
2753 fn test_vec3_cross() {
2754 let a = Vec3Fix::UNIT_X;
2755 let b = Vec3Fix::UNIT_Y;
2756 let cross = a.cross(b);
2757 // X × Y = Z
2758 assert_eq!(cross.z.hi, 1);
2759 assert!(cross.x.is_zero());
2760 assert!(cross.y.is_zero());
2761 }
2762
2763 #[test]
2764 fn test_quat_identity() {
2765 let q = QuatFix::IDENTITY;
2766 let v = Vec3Fix::from_int(1, 2, 3);
2767 let rotated = q.rotate_vec(v);
2768
2769 assert_eq!(rotated.x.hi, 1);
2770 assert_eq!(rotated.y.hi, 2);
2771 assert_eq!(rotated.z.hi, 3);
2772 }
2773
2774 #[test]
2775 fn test_determinism() {
2776 // The same operations should always produce the same results
2777 let a = Fix128::from_raw(12345, 0xABCDEF0123456789);
2778 let b = Fix128::from_raw(67890, 0x9876543210FEDCBA);
2779
2780 let result1 = (a * b) + (a - b);
2781 let result2 = (a * b) + (a - b);
2782
2783 assert_eq!(result1.hi, result2.hi);
2784 assert_eq!(result1.lo, result2.lo);
2785 }
2786
2787 #[test]
2788 fn test_fix128_f32_roundtrip() {
2789 let a = Fix128::from_f32(core::f32::consts::PI);
2790 let back = a.to_f32();
2791 assert!((back - core::f32::consts::PI).abs() < 0.001);
2792
2793 let b = Fix128::from_f32(-7.5);
2794 let back_b = b.to_f32();
2795 assert!((back_b - (-7.5)).abs() < 0.001);
2796 }
2797
2798 #[test]
2799 fn test_vec3fix_f32_roundtrip() {
2800 let v = Vec3Fix::from_f32(1.0, -2.5, 3.75);
2801 let (x, y, z) = v.to_f32();
2802 assert!((x - 1.0).abs() < 0.001);
2803 assert!((y - (-2.5)).abs() < 0.001);
2804 assert!((z - 3.75).abs() < 0.001);
2805 }
2806
2807 // -----------------------------------------------------------------------
2808 // dot_simd / length_squared_simd bit-exactness tests
2809 // -----------------------------------------------------------------------
2810
2811 /// Verify dot_simd produces a bit-exact identical result to dot() for
2812 /// simple integer vectors.
2813 #[test]
2814 fn test_dot_simd_integer_vectors_bit_exact() {
2815 let a = Vec3Fix::from_int(1, 2, 3);
2816 let b = Vec3Fix::from_int(4, 5, 6);
2817 let scalar = a.dot(b);
2818 let simd = a.dot_simd(b);
2819 // 1*4 + 2*5 + 3*6 = 32
2820 assert_eq!(scalar.hi, 32, "scalar dot hi mismatch");
2821 assert_eq!(scalar.lo, 0, "scalar dot lo mismatch");
2822 assert_eq!(simd.hi, scalar.hi, "dot_simd hi != scalar hi");
2823 assert_eq!(simd.lo, scalar.lo, "dot_simd lo != scalar lo");
2824 }
2825
2826 /// Verify dot_simd with arbitrary fractional Fix128 values.
2827 #[test]
2828 fn test_dot_simd_fractional_bit_exact() {
2829 let ax = Fix128::from_raw(3, 0xABCD_EF01_2345_6789);
2830 let ay = Fix128::from_raw(-1, 0x1111_2222_3333_4444);
2831 let az = Fix128::from_raw(7, 0xFEDC_BA98_7654_3210);
2832
2833 let bx = Fix128::from_raw(2, 0x9876_5432_10FE_DCBA);
2834 let by = Fix128::from_raw(5, 0xAAAA_BBBB_CCCC_DDDD);
2835 let bz = Fix128::from_raw(-3, 0x0F0F_0F0F_0F0F_0F0F);
2836
2837 let a = Vec3Fix::new(ax, ay, az);
2838 let b = Vec3Fix::new(bx, by, bz);
2839
2840 let scalar = a.dot(b);
2841 let simd = a.dot_simd(b);
2842
2843 assert_eq!(
2844 simd.hi, scalar.hi,
2845 "dot_simd hi={:#018x} != scalar hi={:#018x}",
2846 simd.hi, scalar.hi
2847 );
2848 assert_eq!(
2849 simd.lo, scalar.lo,
2850 "dot_simd lo={:#018x} != scalar lo={:#018x}",
2851 simd.lo, scalar.lo
2852 );
2853 }
2854
2855 /// Verify dot_simd with negative components.
2856 #[test]
2857 fn test_dot_simd_negative_components_bit_exact() {
2858 let a = Vec3Fix::from_int(-3, 4, -5);
2859 let b = Vec3Fix::from_int(6, -7, 8);
2860 // -3*6 + 4*(-7) + (-5)*8 = -18 - 28 - 40 = -86
2861 let scalar = a.dot(b);
2862 let simd = a.dot_simd(b);
2863 assert_eq!(scalar.hi, -86, "scalar dot should be -86");
2864 assert_eq!(simd.hi, scalar.hi, "dot_simd hi mismatch on negatives");
2865 assert_eq!(simd.lo, scalar.lo, "dot_simd lo mismatch on negatives");
2866 }
2867
2868 /// Verify dot_simd of a zero vector is zero.
2869 #[test]
2870 fn test_dot_simd_zero_vector_bit_exact() {
2871 let a = Vec3Fix::ZERO;
2872 let b = Vec3Fix::from_int(100, 200, 300);
2873 let scalar = a.dot(b);
2874 let simd = a.dot_simd(b);
2875 assert!(scalar.is_zero(), "scalar dot with zero should be zero");
2876 assert_eq!(simd.hi, scalar.hi, "dot_simd hi mismatch (zero vector)");
2877 assert_eq!(simd.lo, scalar.lo, "dot_simd lo mismatch (zero vector)");
2878 }
2879
2880 /// Verify dot_simd self-dot (a . a) for unit vectors.
2881 #[test]
2882 fn test_dot_simd_unit_vectors_self_dot() {
2883 for unit in [Vec3Fix::UNIT_X, Vec3Fix::UNIT_Y, Vec3Fix::UNIT_Z] {
2884 let scalar = unit.dot(unit);
2885 let simd = unit.dot_simd(unit);
2886 assert_eq!(scalar.hi, 1, "unit self-dot should be 1");
2887 assert_eq!(scalar.lo, 0, "unit self-dot lo should be 0");
2888 assert_eq!(simd.hi, scalar.hi, "dot_simd hi mismatch on unit self-dot");
2889 assert_eq!(simd.lo, scalar.lo, "dot_simd lo mismatch on unit self-dot");
2890 }
2891 }
2892
2893 /// Verify length_squared_simd is bit-exact with length_squared.
2894 #[test]
2895 fn test_length_squared_simd_bit_exact() {
2896 let v = Vec3Fix::from_int(3, 4, 0);
2897 // 3^2 + 4^2 + 0^2 = 25
2898 let scalar = v.length_squared();
2899 let simd = v.length_squared_simd();
2900 assert_eq!(scalar.hi, 25, "length_squared should be 25");
2901 assert_eq!(simd.hi, scalar.hi, "length_squared_simd hi mismatch");
2902 assert_eq!(simd.lo, scalar.lo, "length_squared_simd lo mismatch");
2903 }
2904
2905 /// Verify length_squared_simd with fractional components.
2906 #[test]
2907 fn test_length_squared_simd_fractional_bit_exact() {
2908 let v = Vec3Fix::new(
2909 Fix128::from_raw(1, 0x8000_0000_0000_0000), // 1.5
2910 Fix128::from_raw(2, 0x0000_0000_0000_0000), // 2.0
2911 Fix128::from_raw(0, 0x8000_0000_0000_0000), // 0.5
2912 );
2913 let scalar = v.length_squared();
2914 let simd = v.length_squared_simd();
2915 // 1.5^2 + 2.0^2 + 0.5^2 = 2.25 + 4.0 + 0.25 = 6.5
2916 assert_eq!(
2917 simd.hi, scalar.hi,
2918 "length_squared_simd hi mismatch (fractional)"
2919 );
2920 assert_eq!(
2921 simd.lo, scalar.lo,
2922 "length_squared_simd lo mismatch (fractional)"
2923 );
2924 }
2925
2926 /// Exhaustive bit-exactness sweep: 16 pseudo-random raw Fix128 vectors.
2927 #[test]
2928 fn test_dot_simd_exhaustive_raw_sweep() {
2929 // Deterministic pseudo-random values (no RNG dependency).
2930 let raws: [(i64, u64); 8] = [
2931 (0, 0x0000_0000_0000_0001),
2932 (1, 0xFFFF_FFFF_FFFF_FFFF),
2933 (-1, 0x0000_0000_0000_0000),
2934 (42, 0x1234_5678_9ABC_DEF0),
2935 (-7, 0xDEAD_BEEF_CAFE_BABE),
2936 (100, 0xAAAA_AAAA_AAAA_AAAA),
2937 (-100, 0x5555_5555_5555_5555),
2938 (i64::MAX / 2, 0x8000_0000_0000_0000),
2939 ];
2940
2941 for &(ah, al) in &raws {
2942 for &(bh, bl) in &raws {
2943 let a = Vec3Fix::new(
2944 Fix128::from_raw(ah, al),
2945 Fix128::from_raw(bh, bl),
2946 Fix128::from_raw(ah.wrapping_add(bh), al ^ bl),
2947 );
2948 let b = Vec3Fix::new(
2949 Fix128::from_raw(bh, bl),
2950 Fix128::from_raw(ah, al),
2951 Fix128::from_raw(ah.wrapping_sub(bh), al.wrapping_add(bl)),
2952 );
2953 let scalar = a.dot(b);
2954 let simd = a.dot_simd(b);
2955 assert_eq!(
2956 simd.hi, scalar.hi,
2957 "sweep: dot_simd hi mismatch for a=({ah},{al:#x}) b=({bh},{bl:#x})"
2958 );
2959 assert_eq!(
2960 simd.lo, scalar.lo,
2961 "sweep: dot_simd lo mismatch for a=({ah},{al:#x}) b=({bh},{bl:#x})"
2962 );
2963 }
2964 }
2965 }
2966
2967 // ---- atan / atan2 (1.1.1: cordic_atan の shift 桁落ち修正) --------
2968
2969 fn close_f64(v: Fix128, expect: f64, tol: f64) -> bool {
2970 (v.to_f64() - expect).abs() <= tol
2971 }
2972
2973 #[test]
2974 fn shr_bits_is_exact_arithmetic_shift() {
2975 let one = Fix128::ONE;
2976 assert_eq!(one.shr_bits(0), one);
2977 assert_eq!(one.shr_bits(1), Fix128::from_ratio(1, 2)); // 1.1.1 以前の atan 版は 0 になっていた
2978 assert_eq!(one.shr_bits(3), Fix128::from_ratio(1, 8));
2979 assert_eq!(Fix128::from_int(-1).shr_bits(1), Fix128::from_ratio(-1, 2));
2980 assert_eq!(Fix128::from_int(-6).shr_bits(2), Fix128::from_ratio(-3, 2));
2981 let v = Fix128 {
2982 hi: 5,
2983 lo: 0x8000_0000_0000_0001,
2984 };
2985 assert_eq!(
2986 v.shr_bits(1),
2987 Fix128 {
2988 hi: 2,
2989 lo: 0xC000_0000_0000_0000
2990 }
2991 );
2992 assert_eq!(v.shr_bits(64), Fix128 { hi: 0, lo: 5 });
2993 assert_eq!(v.shr_bits(65), Fix128 { hi: 0, lo: 2 });
2994 assert_eq!(
2995 Fix128::from_int(-1).shr_bits(64),
2996 Fix128 {
2997 hi: -1,
2998 lo: u64::MAX
2999 }
3000 );
3001 assert_eq!(v.shr_bits(200), Fix128::ZERO);
3002 assert_eq!(
3003 Fix128::from_int(-1).shr_bits(200),
3004 Fix128 {
3005 hi: -1,
3006 lo: u64::MAX
3007 }
3008 );
3009 // half() と 1 bit shift は同値
3010 for x in [
3011 Fix128::PI,
3012 Fix128::from_ratio(-7, 3),
3013 Fix128::from_int(1234),
3014 ] {
3015 assert_eq!(x.shr_bits(1), x.half());
3016 }
3017 }
3018
3019 #[test]
3020 // 参照値として platform libm を使う (許容 1e-12、決定論の pin は *_golden_bit_patterns 側)
3021 #[allow(clippy::disallowed_methods)]
3022 fn atan_matches_f64_within_1e12() {
3023 for (v, expect) in [
3024 (Fix128::ZERO, 0.0),
3025 (Fix128::ONE, core::f64::consts::FRAC_PI_4),
3026 (Fix128::from_int(-1), -core::f64::consts::FRAC_PI_4),
3027 (Fix128::from_ratio(1, 2), 0.5f64.atan()),
3028 (Fix128::from_int(3), 3f64.atan()),
3029 (Fix128::from_ratio(-1, 8), (-0.125f64).atan()),
3030 (Fix128::from_int(1000), 1000f64.atan()),
3031 ] {
3032 let got = v.atan();
3033 assert!(
3034 close_f64(got, expect, 1e-12),
3035 "atan({}) = {} vs {expect}",
3036 v.to_f64(),
3037 got.to_f64()
3038 );
3039 }
3040 }
3041
3042 #[test]
3043 fn atan_golden_bit_patterns_1_1_1() {
3044 // 修正後の実装値を pin (cross-platform bit-exact の source of truth、意図的変更時のみ更新)
3045 let cases: [(Fix128, i64, u64); 7] = [
3046 (Fix128::ZERO, -1, 18446744073709455410),
3047 (Fix128::ONE, 0, 14488038916154342774),
3048 (Fix128::from_int(-1), -1, 3958705157555404150),
3049 (Fix128::from_ratio(1, 2), 0, 8552788783625182168),
3050 (Fix128::from_int(3), 1, 4594083626069865406),
3051 (Fix128::from_ratio(-1, 8), -1, 16152799315017801038),
3052 (Fix128::from_int(1000), 1, 10510887020674212946),
3053 ];
3054 for (v, hi, lo) in cases {
3055 assert_eq!(v.atan(), Fix128 { hi, lo }, "atan({})", v.to_f64());
3056 }
3057 }
3058
3059 #[test]
3060 // 参照値として platform libm を使う (許容 1e-12、決定論の pin は *_golden_bit_patterns 側)
3061 #[allow(clippy::disallowed_methods)]
3062 fn atan2_matches_f64_in_every_quadrant_and_on_axes() {
3063 let cases: [(i64, i64); 10] = [
3064 (1, 1),
3065 (1, -1),
3066 (-1, -1),
3067 (-1, 1),
3068 (1, 0),
3069 (-1, 0),
3070 (0, 1),
3071 (0, -1),
3072 (3, 4),
3073 (-2, 5),
3074 ];
3075 for (y, x) in cases {
3076 let got = Fix128::atan2(Fix128::from_int(y), Fix128::from_int(x));
3077 let expect = (y as f64).atan2(x as f64);
3078 assert!(
3079 close_f64(got, expect, 1e-12),
3080 "atan2({y}, {x}) = {} vs {expect}",
3081 got.to_f64()
3082 );
3083 }
3084 assert_eq!(Fix128::atan2(Fix128::ZERO, Fix128::ZERO), Fix128::ZERO);
3085 }
3086
3087 #[test]
3088 fn atan2_golden_bit_patterns_1_1_1() {
3089 let cases: [(i64, i64, i64, u64); 11] = [
3090 (1, 1, 0, 14488038916154342774),
3091 (1, -1, 2, 6570628601043732041),
3092 (-1, -1, -3, 11876115472666014883),
3093 (-1, 1, -1, 3958705157555404150),
3094 (1, 0, 1, 10529333758598939753),
3095 (-1, 0, -2, 7917410315110611863),
3096 (0, 1, -1, 18446744073709455410),
3097 (0, -1, 3, 2611923443488231685),
3098 (0, 0, 0, 0),
3099 (3, 4, 0, 11870500265058138062),
3100 (-2, 5, -1, 11427640316703283098),
3101 ];
3102 for (y, x, hi, lo) in cases {
3103 assert_eq!(
3104 Fix128::atan2(Fix128::from_int(y), Fix128::from_int(x)),
3105 Fix128 { hi, lo },
3106 "atan2({y}, {x})"
3107 );
3108 }
3109 }
3110
3111 #[test]
3112 // 参照値として platform libm を使う (許容 1e-12、決定論の pin は *_golden_bit_patterns 側)
3113 #[allow(clippy::disallowed_methods)]
3114 fn atan_identities_hold() {
3115 // odd: atan(-x) == -atan(x) (CORDIC は符号対称なので bit-exact ではなく 1e-12)
3116 for v in [
3117 Fix128::from_ratio(1, 3),
3118 Fix128::from_int(2),
3119 Fix128::from_ratio(17, 5),
3120 ] {
3121 let a = v.atan();
3122 let b = (-v).atan();
3123 assert!(close_f64(a + b, 0.0, 1e-12), "odd symmetry {}", v.to_f64());
3124 // atan(x) + atan(1/x) == π/2 (x > 0)
3125 let c = (Fix128::ONE / v).atan();
3126 assert!(
3127 close_f64(a + c, core::f64::consts::FRAC_PI_2, 1e-12),
3128 "complement {}",
3129 v.to_f64()
3130 );
3131 // atan2(y, x) == atan(y/x) for x > 0
3132 let d = Fix128::atan2(v, Fix128::from_int(2));
3133 assert!(close_f64(
3134 d,
3135 (v / Fix128::from_int(2)).atan().to_f64(),
3136 1e-12
3137 ));
3138 }
3139 // sin / cos は本修正で bit 単位で不変 (代表点 pin)
3140 let (s, c) = Fix128::HALF_PI.sin_cos();
3141 assert_eq!(s, Fix128 { hi: 1, lo: 1453 });
3142 assert_eq!(
3143 c,
3144 Fix128 {
3145 hi: -1,
3146 lo: 18446744073709456430
3147 }
3148 );
3149 }
3150
3151 // ---- Mat3Fix (mutation-score tests, 2026-09-15) ---------------------
3152
3153 fn fi(n: i64) -> Fix128 {
3154 Fix128::from_int(n)
3155 }
3156
3157 fn v3i(x: i64, y: i64, z: i64) -> Vec3Fix {
3158 Vec3Fix::from_int(x, y, z)
3159 }
3160
3161 /// 列ベクトル指定の整数行列 (M = [c0 c1 c2])
3162 fn mat(c0: (i64, i64, i64), c1: (i64, i64, i64), c2: (i64, i64, i64)) -> Mat3Fix {
3163 Mat3Fix::from_cols(
3164 v3i(c0.0, c0.1, c0.2),
3165 v3i(c1.0, c1.1, c1.2),
3166 v3i(c2.0, c2.1, c2.2),
3167 )
3168 }
3169
3170 #[test]
3171 fn mat3_mul_vec_uses_every_entry_with_the_right_index() {
3172 // 9 entry 全て異なる素数 → どの entry の取り違えでも値が変わる
3173 let m = mat((2, 3, 5), (7, 11, 13), (17, 19, 23));
3174 let v = v3i(1, 10, 100);
3175 // row i = c0[i]*1 + c1[i]*10 + c2[i]*100
3176 assert_eq!(
3177 m.mul_vec(v),
3178 v3i(2 + 70 + 1700, 3 + 110 + 1900, 5 + 130 + 2300)
3179 );
3180 assert_eq!(Mat3Fix::IDENTITY.mul_vec(v), v);
3181 assert_eq!(m.mul_vec(Vec3Fix::UNIT_X), v3i(2, 3, 5));
3182 assert_eq!(m.mul_vec(Vec3Fix::UNIT_Y), v3i(7, 11, 13));
3183 assert_eq!(m.mul_vec(Vec3Fix::UNIT_Z), v3i(17, 19, 23));
3184 }
3185
3186 #[test]
3187 fn mat3_determinant_closed_form() {
3188 // det [[2,7,17],[3,11,19],[5,13,23]] (列指定) = -78
3189 let m = mat((2, 3, 5), (7, 11, 13), (17, 19, 23));
3190 assert_eq!(m.determinant(), fi(-78));
3191 assert_eq!(Mat3Fix::IDENTITY.determinant(), Fix128::ONE);
3192 assert_eq!(
3193 Mat3Fix::diagonal(fi(2), fi(3), fi(-4)).determinant(),
3194 fi(-24)
3195 );
3196 // 列が線形従属 → 0
3197 assert_eq!(
3198 mat((1, 2, 3), (2, 4, 6), (0, 1, 0)).determinant(),
3199 Fix128::ZERO
3200 );
3201 // 転置不変
3202 assert_eq!(m.transpose().determinant(), fi(-78));
3203 // 各 entry を単独で変えると det が変わる (どの項も効いている)
3204 for r in 0..3 {
3205 for c in 0..3 {
3206 let mut m2 = m;
3207 let col = match c {
3208 0 => &mut m2.col0,
3209 1 => &mut m2.col1,
3210 _ => &mut m2.col2,
3211 };
3212 match r {
3213 0 => col.x = col.x + Fix128::ONE,
3214 1 => col.y = col.y + Fix128::ONE,
3215 _ => col.z = col.z + Fix128::ONE,
3216 }
3217 assert_ne!(m2.determinant(), fi(-78), "entry ({r},{c}) は det に効く");
3218 }
3219 }
3220 }
3221
3222 #[test]
3223 fn mat3_inverse_exact_for_unimodular_matrix() {
3224 // det = 1 の整数行列 → 逆行列も整数で exact
3225 // M = [[1,2,3],[0,1,4],[5,6,0]] (行表記) → det 1、M⁻¹ = [[-24,18,5],[20,-15,-4],[-5,4,1]]
3226 let m = mat((1, 0, 5), (2, 1, 6), (3, 4, 0));
3227 assert_eq!(m.determinant(), Fix128::ONE);
3228 let inv = m.inverse().expect("det 1");
3229 assert_eq!(inv, mat((-24, 20, -5), (18, -15, 4), (5, -4, 1)));
3230 assert_eq!(m.mul_mat(inv), Mat3Fix::IDENTITY);
3231 assert_eq!(inv.mul_mat(m), Mat3Fix::IDENTITY);
3232 assert_eq!(m * inv, Mat3Fix::IDENTITY);
3233 // 各 cofactor が正しい位置にある: inv の全 9 entry が異なる値
3234 let e = [
3235 inv.col0.x, inv.col0.y, inv.col0.z, inv.col1.x, inv.col1.y, inv.col1.z, inv.col2.x,
3236 inv.col2.y, inv.col2.z,
3237 ];
3238 for i in 0..9 {
3239 for j in (i + 1)..9 {
3240 assert_ne!(e[i], e[j], "entries {i} and {j}");
3241 }
3242 }
3243 }
3244
3245 #[test]
3246 fn mat3_inverse_scales_by_reciprocal_determinant() {
3247 // det = -8 (2 の冪) → 1/det exact
3248 let m = Mat3Fix::diagonal(fi(2), fi(-2), fi(2));
3249 let inv = m.inverse().expect("det -8");
3250 assert_eq!(
3251 inv,
3252 Mat3Fix::diagonal(
3253 Fix128::from_ratio(1, 2),
3254 Fix128::from_ratio(-1, 2),
3255 Fix128::from_ratio(1, 2)
3256 )
3257 );
3258 // 一般行列 det = -78 の逆行列 × 元 = I (数 ulp 許容)
3259 let g = mat((2, 3, 5), (7, 11, 13), (17, 19, 23));
3260 let gi = g.inverse().expect("det -78");
3261 let p = g.mul_mat(gi);
3262 let close = |a: Fix128, b: Fix128| (a - b).abs() < Fix128 { hi: 0, lo: 1 << 20 };
3263 for (col, unit) in [
3264 (p.col0, Vec3Fix::UNIT_X),
3265 (p.col1, Vec3Fix::UNIT_Y),
3266 (p.col2, Vec3Fix::UNIT_Z),
3267 ] {
3268 assert!(
3269 close(col.x, unit.x) && close(col.y, unit.y) && close(col.z, unit.z),
3270 "{col:?} vs {unit:?}"
3271 );
3272 }
3273 assert!(Mat3Fix::ZERO.inverse().is_none());
3274 assert!(mat((1, 2, 3), (2, 4, 6), (0, 1, 0)).inverse().is_none());
3275 assert_eq!(Mat3Fix::IDENTITY.inverse(), Some(Mat3Fix::IDENTITY));
3276 }
3277
3278 #[test]
3279 fn mat3_scale_mul_mat_and_operator_agree() {
3280 let m = mat((2, 3, 5), (7, 11, 13), (17, 19, 23));
3281 assert_eq!(m.scale(fi(3)), mat((6, 9, 15), (21, 33, 39), (51, 57, 69)));
3282 assert_eq!(m.scale(Fix128::ZERO), Mat3Fix::ZERO);
3283 let n = mat((1, 0, 0), (0, 2, 0), (1, 1, 1));
3284 // (M N) col_j = M · (N col_j)
3285 let mn = m.mul_mat(n);
3286 assert_eq!(mn.col0, m.mul_vec(v3i(1, 0, 0)));
3287 assert_eq!(mn.col1, m.mul_vec(v3i(0, 2, 0)));
3288 assert_eq!(mn.col2, m.mul_vec(v3i(1, 1, 1)));
3289 assert_eq!(m * n, mn);
3290 assert_ne!(n.mul_mat(m), mn, "非可換");
3291 assert_eq!(m.mul_mat(Mat3Fix::IDENTITY), m);
3292 assert_eq!(Mat3Fix::IDENTITY * m, m);
3293 assert_eq!(m.transpose().transpose(), m);
3294 assert_eq!(m.transpose().col0, v3i(2, 7, 17));
3295 }
3296
3297 // ---- QuatFix -------------------------------------------------------
3298
3299 fn q(x: i64, y: i64, z: i64, w: i64) -> QuatFix {
3300 QuatFix::new(fi(x), fi(y), fi(z), fi(w))
3301 }
3302
3303 #[test]
3304 fn quat_mul_is_the_hamilton_product() {
3305 // 基底: i*j = k, j*k = i, k*i = j, i*i = -1
3306 let i = q(1, 0, 0, 0);
3307 let j = q(0, 1, 0, 0);
3308 let k = q(0, 0, 1, 0);
3309 let one = q(0, 0, 0, 1);
3310 assert_eq!(i.mul(j), k);
3311 assert_eq!(j.mul(k), i);
3312 assert_eq!(k.mul(i), j);
3313 assert_eq!(j.mul(i), q(0, 0, -1, 0));
3314 assert_eq!(i.mul(i), q(0, 0, 0, -1));
3315 assert_eq!(one.mul(i), i);
3316 assert_eq!(i.mul(one), i);
3317 // 一般: (1,2,3,4) * (5,6,7,8) = (24, 48, 48, -6)
3318 let a = q(1, 2, 3, 4);
3319 let b = q(5, 6, 7, 8);
3320 assert_eq!(a.mul(b), q(24, 48, 48, -6));
3321 assert_eq!(b.mul(a), q(32, 32, 56, -6));
3322 assert_ne!(a.mul(b), b.mul(a));
3323 }
3324
3325 #[test]
3326 fn quat_conjugate_length_and_normalize() {
3327 let a = q(1, -2, 3, -4);
3328 assert_eq!(a.conjugate(), q(-1, 2, -3, -4));
3329 assert_eq!(a.conjugate().conjugate(), a);
3330 assert_eq!(a.length_squared(), fi(30));
3331 assert_eq!(q(0, 0, 0, 2).length_squared(), fi(4));
3332 assert_eq!(q(3, 0, 0, 0).length_squared(), fi(9));
3333 // a * conj(a) = |a|² (実部のみ)
3334 assert_eq!(a.mul(a.conjugate()), q(0, 0, 0, 30));
3335 // normalize: (0,0,0,2) → (0,0,0,1)、(0,3,0,4) → (0,0.6,0,0.8) は 5 で割る
3336 assert_eq!(q(0, 0, 0, 2).normalize(), QuatFix::IDENTITY);
3337 assert_eq!(q(0, 0, -4, 0).normalize(), q(0, 0, -1, 0));
3338 let n = q(0, 3, 0, 4).normalize();
3339 assert_eq!(
3340 n,
3341 QuatFix::new(Fix128::ZERO, fi(3) / fi(5), Fix128::ZERO, fi(4) / fi(5))
3342 );
3343 assert_eq!(q(0, 0, 0, 0).normalize(), QuatFix::IDENTITY);
3344 }
3345
3346 #[test]
3347 fn quat_from_axis_angle_components() {
3348 // 軸は正規化され、(x,y,z) = axis·sin(θ/2)、w = cos(θ/2)
3349 let theta = Fix128::from_ratio(1, 2);
3350 let (s, c) = theta.half().sin_cos();
3351 let qq = QuatFix::from_axis_angle(v3i(0, 0, 5), theta);
3352 assert_eq!(qq, QuatFix::new(Fix128::ZERO, Fix128::ZERO, s, c));
3353 let qx = QuatFix::from_axis_angle(v3i(-2, 0, 0), theta);
3354 assert_eq!(qx, QuatFix::new(-s, Fix128::ZERO, Fix128::ZERO, c));
3355 // 角度 0 → identity 近傍 (CORDIC gain 込み)
3356 let q0 = QuatFix::from_axis_angle(v3i(1, 0, 0), Fix128::ZERO);
3357 assert!((q0.w - Fix128::ONE).abs() < Fix128 { hi: 0, lo: 1 << 20 });
3358 assert!(q0.x.abs() < Fix128 { hi: 0, lo: 1 << 20 });
3359 }
3360
3361 #[test]
3362 fn quat_rotate_vec_by_quarter_turns() {
3363 // z 軸 90° 回転: x → y、y → -x (数 ulp 許容)
3364 let rz = QuatFix::from_axis_angle(v3i(0, 0, 1), Fix128::HALF_PI);
3365 let close = |a: Vec3Fix, b: Vec3Fix| (a - b).length() < Fix128 { hi: 0, lo: 1 << 24 };
3366 assert!(close(rz.rotate_vec(Vec3Fix::UNIT_X), Vec3Fix::UNIT_Y));
3367 assert!(close(rz.rotate_vec(Vec3Fix::UNIT_Y), -Vec3Fix::UNIT_X));
3368 assert!(close(rz.rotate_vec(Vec3Fix::UNIT_Z), Vec3Fix::UNIT_Z));
3369 // 180°: (0,0,1,0) は exact
3370 let half = q(0, 0, 1, 0);
3371 assert_eq!(half.rotate_vec(v3i(1, 2, 3)), v3i(-1, -2, 3));
3372 assert_eq!(QuatFix::IDENTITY.rotate_vec(v3i(1, 2, 3)), v3i(1, 2, 3));
3373 }
3374
3375 // ---- Vec3Fix helpers / select / From / Display ----------------------
3376
3377 #[test]
3378 fn vec3_cross_try_normalize_and_batch_dot() {
3379 assert_eq!(v3i(1, 0, 0).cross(v3i(0, 1, 0)), v3i(0, 0, 1));
3380 assert_eq!(v3i(0, 1, 0).cross(v3i(1, 0, 0)), v3i(0, 0, -1));
3381 assert_eq!(v3i(1, 2, 3).cross(v3i(4, 5, 6)), v3i(-3, 6, -3));
3382 assert_eq!(v3i(1, 2, 3).cross(v3i(1, 2, 3)), Vec3Fix::ZERO);
3383 assert_eq!(v3i(0, 0, 4).try_normalize(), Some(v3i(0, 0, 1)));
3384 assert_eq!(v3i(0, -2, 0).try_normalize(), Some(v3i(0, -1, 0)));
3385 assert_eq!(Vec3Fix::ZERO.try_normalize(), None);
3386 assert_eq!(v3i(1, 2, 3).scale(fi(-2)), v3i(-2, -4, -6));
3387 }
3388
3389 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
3390 #[test]
3391 fn vec3_dot_batch_4_matches_scalar_dot() {
3392 let a = [v3i(1, 0, 0), v3i(1, 1, 1), v3i(2, 3, 4), v3i(-1, 0, 5)];
3393 let b = [v3i(7, 8, 9), v3i(1, 2, 3), v3i(5, 6, 7), v3i(4, 4, 4)];
3394 assert_eq!(Vec3Fix::dot_batch_4(a, b), [fi(7), fi(6), fi(56), fi(16)]);
3395 }
3396
3397 #[test]
3398 fn select_fix128_is_a_bit_exact_branchless_select() {
3399 // hi / lo ともに全 bit パターンが異なる 2 値: & / | / ^ の取り違えで必ず壊れる
3400 let a = Fix128 {
3401 hi: 0x5A5A_5A5A_5A5A_5A5A,
3402 lo: 0xA5A5_A5A5_A5A5_A5A5,
3403 };
3404 let b = Fix128 {
3405 hi: -0x0F0F_0F0F_0F0F_0F10,
3406 lo: 0x0F0F_0F0F_0F0F_0F0F,
3407 };
3408 assert_eq!(select_fix128(true, a, b), a);
3409 assert_eq!(select_fix128(false, a, b), b);
3410 assert_eq!(select_fix128(true, b, a), b);
3411 assert_eq!(select_fix128(false, b, a), a);
3412 assert_eq!(select_fix128(true, Fix128::ZERO, a), Fix128::ZERO);
3413 assert_eq!(select_fix128(false, a, Fix128::ZERO), Fix128::ZERO);
3414 }
3415
3416 #[test]
3417 fn from_impls_roundtrip_and_display_format() {
3418 assert_eq!(Fix128::from(-7i64), fi(-7));
3419 assert_eq!(Fix128::from(9i32), fi(9));
3420 let arr3: [Fix128; 3] = v3i(1, -2, 3).into();
3421 assert_eq!(arr3, [fi(1), fi(-2), fi(3)]);
3422 assert_eq!(Vec3Fix::from([fi(4), fi(5), fi(-6)]), v3i(4, 5, -6));
3423 let arr4: [Fix128; 4] = q(1, 2, 3, 4).into();
3424 assert_eq!(arr4, [fi(1), fi(2), fi(3), fi(4)]);
3425 assert_eq!(QuatFix::from([fi(-1), fi(0), fi(2), fi(3)]), q(-1, 0, 2, 3));
3426 assert_eq!(format!("{}", Fix128::from_ratio(-5, 4)), "-1.2500");
3427 assert_eq!(format!("{}", v3i(1, -2, 3)), "(1.0000, -2.0000, 3.0000)");
3428 assert_eq!(
3429 format!("{}", q(0, 0, 0, 1)),
3430 "(0.0000, 0.0000, 0.0000, 1.0000)"
3431 );
3432 }
3433
3434 // ---- sin / cos (CORDIC range reduction + golden pins) ---------------
3435
3436 #[test]
3437 fn sin_cos_golden_bit_patterns() {
3438 // 17 角度の (angle, sin, cos) bit pattern pin (1.1.1 の shr_bits 集約前後で不変を確認済)
3439 // 範囲: [-π/2, π/2] 内 / 第 2・3 象限 / ±π ちょうど / 複数周回 (3π, -7π/2) / 大きな値 (100, -1000.25)
3440 let table: [(&str, Fix128, Fix128, Fix128); 17] = [
3441 (
3442 "0",
3443 Fix128 { hi: 0, lo: 0 },
3444 Fix128 {
3445 hi: -1,
3446 lo: 18446744073709456430,
3447 },
3448 Fix128 { hi: 1, lo: 1453 },
3449 ),
3450 (
3451 "pi/6",
3452 Fix128 {
3453 hi: 0,
3454 lo: 9658692610769497123,
3455 },
3456 Fix128 {
3457 hi: 0,
3458 lo: 9223372036854815772,
3459 },
3460 Fix128 {
3461 hi: 0,
3462 lo: 15975348984942493703,
3463 },
3464 ),
3465 (
3466 "pi/4",
3467 Fix128 {
3468 hi: 0,
3469 lo: 14488038916154245684,
3470 },
3471 Fix128 {
3472 hi: 0,
3473 lo: 13043817825332851804,
3474 },
3475 Fix128 {
3476 hi: 0,
3477 lo: 13043817825332714670,
3478 },
3479 ),
3480 (
3481 "pi/3",
3482 Fix128 {
3483 hi: 1,
3484 lo: 870641147829442630,
3485 },
3486 Fix128 {
3487 hi: 0,
3488 lo: 15975348984942493703,
3489 },
3490 Fix128 {
3491 hi: 0,
3492 lo: 9223372036854815772,
3493 },
3494 ),
3495 (
3496 "pi/2",
3497 Fix128 {
3498 hi: 1,
3499 lo: 10529333758598939753,
3500 },
3501 Fix128 { hi: 1, lo: 1453 },
3502 Fix128 {
3503 hi: -1,
3504 lo: 18446744073709456430,
3505 },
3506 ),
3507 (
3508 "2pi/3",
3509 Fix128 {
3510 hi: 2,
3511 lo: 1741282295658885258,
3512 },
3513 Fix128 {
3514 hi: 0,
3515 lo: 15975348984942493703,
3516 },
3517 Fix128 {
3518 hi: -1,
3519 lo: 9223372036854735844,
3520 },
3521 ),
3522 (
3523 "pi",
3524 Fix128 {
3525 hi: 3,
3526 lo: 2611923443488327891,
3527 },
3528 Fix128 {
3529 hi: -1,
3530 lo: 18446744073709456430,
3531 },
3532 Fix128 {
3533 hi: -2,
3534 lo: 18446744073709550163,
3535 },
3536 ),
3537 (
3538 "-pi/6",
3539 Fix128 {
3540 hi: -1,
3541 lo: 8788051462940054493,
3542 },
3543 Fix128 {
3544 hi: -1,
3545 lo: 9223372036854735845,
3546 },
3547 Fix128 {
3548 hi: 0,
3549 lo: 15975348984942493709,
3550 },
3551 ),
3552 (
3553 "-pi/2",
3554 Fix128 {
3555 hi: -2,
3556 lo: 7917410315110611863,
3557 },
3558 Fix128 {
3559 hi: -2,
3560 lo: 18446744073709550164,
3561 },
3562 Fix128 {
3563 hi: -1,
3564 lo: 18446744073709456423,
3565 },
3566 ),
3567 (
3568 "-3pi/4",
3569 Fix128 {
3570 hi: -3,
3571 lo: 11876115472665917794,
3572 },
3573 Fix128 {
3574 hi: -1,
3575 lo: 5402926248376699811,
3576 },
3577 Fix128 {
3578 hi: -1,
3579 lo: 5402926248376836953,
3580 },
3581 ),
3582 (
3583 "-pi",
3584 Fix128 {
3585 hi: -4,
3586 lo: 15834820630221223725,
3587 },
3588 Fix128 {
3589 hi: -1,
3590 lo: 18446744073709456430,
3591 },
3592 Fix128 {
3593 hi: -2,
3594 lo: 18446744073709550163,
3595 },
3596 ),
3597 (
3598 "5pi/4",
3599 Fix128 {
3600 hi: 3,
3601 lo: 17099962359642573575,
3602 },
3603 Fix128 {
3604 hi: -1,
3605 lo: 5402926248376699811,
3606 },
3607 Fix128 {
3608 hi: -1,
3609 lo: 5402926248376836953,
3610 },
3611 ),
3612 (
3613 "3pi",
3614 Fix128 {
3615 hi: 9,
3616 lo: 7835770330464983673,
3617 },
3618 Fix128 {
3619 hi: -1,
3620 lo: 18446744073709456430,
3621 },
3622 Fix128 {
3623 hi: -2,
3624 lo: 18446744073709550163,
3625 },
3626 ),
3627 (
3628 "-7pi/2",
3629 Fix128 {
3630 hi: -11,
3631 lo: 81639984645628190,
3632 },
3633 Fix128 { hi: 1, lo: 1453 },
3634 Fix128 { hi: 0, lo: 95186 },
3635 ),
3636 (
3637 "1/2",
3638 Fix128 {
3639 hi: 0,
3640 lo: 9223372036854775808,
3641 },
3642 Fix128 {
3643 hi: 0,
3644 lo: 8843840213032159770,
3645 },
3646 Fix128 {
3647 hi: 0,
3648 lo: 16188540922742043083,
3649 },
3650 ),
3651 (
3652 "100",
3653 Fix128 { hi: 100, lo: 0 },
3654 Fix128 {
3655 hi: -1,
3656 lo: 9105946684437943138,
3657 },
3658 Fix128 {
3659 hi: 0,
3660 lo: 15906975547020722691,
3661 },
3662 ),
3663 (
3664 "-1000.25",
3665 Fix128 {
3666 hi: -1001,
3667 lo: 13835058055282163712,
3668 },
3669 Fix128 {
3670 hi: -1,
3671 lo: 1101110721943816547,
3672 },
3673 Fix128 {
3674 hi: 0,
3675 lo: 6277847604637333680,
3676 },
3677 ),
3678 ];
3679 for (name, angle, sin, cos) in table {
3680 let (s, c) = angle.sin_cos();
3681 assert_eq!(s, sin, "sin({name})");
3682 assert_eq!(c, cos, "cos({name})");
3683 assert_eq!(angle.sin(), sin, "sin() alias {name}");
3684 assert_eq!(angle.cos(), cos, "cos() alias {name}");
3685 }
3686 }
3687
3688 #[test]
3689 // 参照値として platform libm を使う (許容 1e-11、決定論の pin は sin_cos_golden_bit_patterns 側)
3690 #[allow(clippy::disallowed_methods)]
3691 fn sin_cos_large_and_negative_angles_reduce_correctly() {
3692 let two_pi = core::f64::consts::PI * 2.0;
3693 let cases: [(Fix128, f64); 9] = [
3694 (Fix128::PI * fi(3), 3.0 * core::f64::consts::PI),
3695 (
3696 (Fix128::PI * Fix128::from_ratio(7, 2)).neg(),
3697 -3.5 * core::f64::consts::PI,
3698 ),
3699 (fi(100), 100.0),
3700 (Fix128::from_ratio(-4001, 4), -1000.25),
3701 (Fix128::TWO_PI + Fix128::from_ratio(1, 2), two_pi + 0.5),
3702 (
3703 (Fix128::TWO_PI + Fix128::from_ratio(1, 2)).neg(),
3704 -(two_pi + 0.5),
3705 ),
3706 (
3707 Fix128::PI * fi(1000) + fi(1),
3708 1000.0 * core::f64::consts::PI + 1.0,
3709 ),
3710 (
3711 Fix128::PI + Fix128::from_ratio(1, 1000),
3712 core::f64::consts::PI + 0.001,
3713 ),
3714 (
3715 (Fix128::PI + Fix128::from_ratio(1, 1000)).neg(),
3716 -(core::f64::consts::PI + 0.001),
3717 ),
3718 ];
3719 for (angle, f) in cases {
3720 let (s, c) = angle.sin_cos();
3721 assert!(
3722 (s.to_f64() - f.sin()).abs() < 1e-11,
3723 "sin({f}) = {} vs {}",
3724 s.to_f64(),
3725 f.sin()
3726 );
3727 assert!(
3728 (c.to_f64() - f.cos()).abs() < 1e-11,
3729 "cos({f}) = {} vs {}",
3730 c.to_f64(),
3731 f.cos()
3732 );
3733 }
3734 }
3735
3736 #[test]
3737 fn sin_cos_boundaries_and_symmetries() {
3738 let eps = Fix128 { hi: 0, lo: 1 << 24 }; // 2^-40
3739 let close = |a: Fix128, b: Fix128| (a - b).abs() < eps;
3740 // ±π ちょうどは reduction を通らない (`>` / `<` は false)、π+ε は通る: どちらも sin ≈ 0、cos ≈ -1
3741 for a in [
3742 Fix128::PI,
3743 Fix128::PI.neg(),
3744 Fix128::PI + Fix128 { hi: 0, lo: 1 },
3745 Fix128::PI.neg() - Fix128 { hi: 0, lo: 1 },
3746 ] {
3747 let (s, c) = a.sin_cos();
3748 assert!(
3749 close(s, Fix128::ZERO) && close(c, fi(-1)),
3750 "{a:?}: {s:?} {c:?}"
3751 );
3752 }
3753 // ±π/2 ちょうど (quadrant 分岐は `>` false) と π/2+ε (分岐 true): sin ≈ ±1、cos ≈ 0
3754 for (a, sign) in [
3755 (Fix128::HALF_PI, 1),
3756 (Fix128::HALF_PI.neg(), -1),
3757 (Fix128::HALF_PI + Fix128 { hi: 0, lo: 1 }, 1),
3758 ] {
3759 let (s, c) = a.sin_cos();
3760 assert!(
3761 close(s, fi(sign)) && close(c, Fix128::ZERO),
3762 "{a:?}: {s:?} {c:?}"
3763 );
3764 }
3765 // 第 2 / 第 3 象限の符号: sin(2π/3) > 0, cos < 0 / sin(-2π/3) < 0, cos < 0
3766 let (s2, c2) = (Fix128::PI * Fix128::from_ratio(2, 3)).sin_cos();
3767 assert!(s2 > Fix128::ZERO && c2 < Fix128::ZERO);
3768 let (s3, c3) = (Fix128::PI * Fix128::from_ratio(-2, 3)).sin_cos();
3769 assert!(s3 < Fix128::ZERO && c3 < Fix128::ZERO);
3770 // 奇関数 / 偶関数、周期性 (2π 加算で reduction 経路が変わっても一致)
3771 for x in [
3772 Fix128::from_ratio(1, 3),
3773 Fix128::from_ratio(-7, 5),
3774 fi(2),
3775 Fix128::from_ratio(11, 4),
3776 ] {
3777 let (s, c) = x.sin_cos();
3778 let (sn, cn) = x.neg().sin_cos();
3779 assert!(
3780 close(s + sn, Fix128::ZERO) && close(c, cn),
3781 "odd/even {x:?}"
3782 );
3783 let (sp, cp) = (x + Fix128::TWO_PI).sin_cos();
3784 let (sm, cm) = (x - Fix128::TWO_PI * fi(3)).sin_cos();
3785 assert!(
3786 close(s, sp) && close(c, cp) && close(s, sm) && close(c, cm),
3787 "period {x:?}"
3788 );
3789 // sin² + cos² = 1
3790 assert!(close(s * s + c * c, Fix128::ONE), "pythagoras {x:?}");
3791 }
3792 }
3793
3794 // ---- sqrt: digit recurrence vs Newton reference vs exact floor oracle ----
3795
3796 /// The pre-1.2.0 implementation, kept as a bit-for-bit reference.
3797 fn sqrt_newton_reference(v: Fix128) -> Fix128 {
3798 if v.is_negative() || v.is_zero() {
3799 return Fix128::ZERO;
3800 }
3801 let sig_bits = if v.hi > 0 {
3802 128 - (v.hi as u64).leading_zeros() as i64
3803 } else if v.hi == 0 && v.lo > 0 {
3804 64 - v.lo.leading_zeros() as i64
3805 } else {
3806 1
3807 };
3808 let result_bit = ((sig_bits + 63) / 2) as u32;
3809 let mut x = if result_bit >= 64 {
3810 Fix128 {
3811 hi: 1i64 << (result_bit - 64).min(62),
3812 lo: 0,
3813 }
3814 } else {
3815 Fix128 {
3816 hi: 0,
3817 lo: 1u64 << result_bit,
3818 }
3819 };
3820 for _ in 0..64 {
3821 let div = v / x;
3822 x = (x + div).half();
3823 }
3824 x
3825 }
3826
3827 /// 256-bit square of a u128 as (hi, lo) limbs.
3828 fn square_wide(r: u128) -> (u128, u128) {
3829 let (a, b) = (r >> 64, r & 0xFFFF_FFFF_FFFF_FFFF);
3830 let bb = b * b;
3831 let ab = a * b; // < 2^128
3832 let aa = a * a;
3833 // r^2 = aa<<128 + 2ab<<64 + bb
3834 let (mid, c1) = bb.overflowing_add(ab << 65);
3835 let hi = aa + (ab >> 63) + c1 as u128;
3836 (hi, mid)
3837 }
3838
3839 /// Exact-floor property of I64F64 sqrt: with N = raw << 64 (192-bit),
3840 /// root^2 <= N < (root+1)^2.
3841 fn assert_sqrt_is_exact_floor(v: Fix128) {
3842 let r = v.sqrt();
3843 let root = ((r.hi as u128) << 64) | r.lo as u128;
3844 let n = ((v.hi as u128) << 64) | v.lo as u128; // N = (n_hi:n_lo) = n << 64 → (n>>64, n<<64)
3845 let n_hi = n >> 64;
3846 let n_lo = n << 64;
3847 let (lo_hi, lo_lo) = square_wide(root);
3848 assert!(
3849 (lo_hi, lo_lo) <= (n_hi, n_lo),
3850 "root^2 > N for {v:?} (root {root:#x})"
3851 );
3852 let (up_hi, up_lo) = square_wide(root + 1);
3853 assert!(
3854 (up_hi, up_lo) > (n_hi, n_lo),
3855 "(root+1)^2 <= N for {v:?} (root {root:#x})"
3856 );
3857 }
3858
3859 fn lcg_samples(n: usize) -> impl Iterator<Item = Fix128> {
3860 // Deterministic LCG (Knuth MMIX), mixes magnitudes from 2^-64 to 2^62.
3861 let mut state: u64 = 0x9E37_79B9_7F4A_7C15;
3862 (0..n).map(move |i| {
3863 state = state
3864 .wrapping_mul(6_364_136_223_846_793_005)
3865 .wrapping_add(1_442_695_040_888_963_407);
3866 let lo = state;
3867 state = state
3868 .wrapping_mul(6_364_136_223_846_793_005)
3869 .wrapping_add(1_442_695_040_888_963_407);
3870 // Vary the integer-part magnitude: 0 .. 2^62 by shifting.
3871 let shift = (i % 64) as u32;
3872 let hi = ((state >> 1) >> shift) as i64;
3873 Fix128 { hi, lo }
3874 })
3875 }
3876
3877 #[test]
3878 fn sqrt_matches_newton_reference() {
3879 let edges = [
3880 Fix128::ZERO,
3881 Fix128::from_raw(0, 1),
3882 Fix128::from_raw(0, u64::MAX),
3883 Fix128::ONE,
3884 Fix128::from_ratio(1, 4),
3885 Fix128::from_int(2),
3886 Fix128::from_int(4),
3887 Fix128::from_int(1_000_000),
3888 Fix128::from_raw(i64::MAX, u64::MAX),
3889 Fix128::from_int(-3),
3890 ];
3891 for v in edges.iter().copied().chain(lcg_samples(20_000)) {
3892 assert_eq!(v.sqrt(), sqrt_newton_reference(v), "sqrt drift for {v:?}");
3893 }
3894 }
3895
3896 #[test]
3897 fn sqrt_is_exact_floor_and_monotonic() {
3898 assert_eq!(Fix128::from_ratio(1, 4).sqrt(), Fix128::from_ratio(1, 2));
3899 assert_eq!(Fix128::from_int(4).sqrt(), Fix128::from_int(2));
3900 assert_eq!(Fix128::from_int(-3).sqrt(), Fix128::ZERO);
3901 let mut prev = Fix128::ZERO;
3902 let mut samples: Vec<Fix128> = lcg_samples(20_000).collect();
3903 samples.push(Fix128::from_raw(i64::MAX, u64::MAX));
3904 samples.push(Fix128::from_raw(0, 1));
3905 samples.sort();
3906 for v in samples {
3907 assert_sqrt_is_exact_floor(v);
3908 let r = v.sqrt();
3909 assert!(r >= prev, "sqrt not monotonic at {v:?}");
3910 prev = r;
3911 }
3912 }
3913
3914 // ---- transcendental oracle sweep (f64 libm reference) ----
3915 //
3916 // Golden bit patterns only detect *change*; a value that was wrong from the
3917 // start gets pinned as-is (that is how the 1.0.0 atan bug shipped). Every
3918 // Fix128 transcendental is therefore also checked against an independent
3919 // f64 reference over a dense sweep of the domain.
3920
3921 #[test]
3922 #[allow(clippy::disallowed_methods)] // the f64 libm values are the oracle on purpose
3923 fn transcendental_sweep_matches_f64_reference() {
3924 // CORDIC with 48 iterations: worst-case error ≈ 2^-46 ≈ 1.4e-14 plus
3925 // range-reduction rounding; f64 reference itself carries 1e-16.
3926 const TOL: f64 = 1e-11;
3927 let steps = 4_000;
3928 for i in 0..=steps {
3929 // x sweeps [-4π, 4π] (range reduction on both sides) for sin / cos
3930 let t = -4.0 * core::f64::consts::PI
3931 + 8.0 * core::f64::consts::PI * (i as f64) / (steps as f64);
3932 let x = Fix128::from_f64(t);
3933 let xf = x.to_f64();
3934 let (s, c) = x.sin_cos();
3935 assert!(
3936 (s.to_f64() - xf.sin()).abs() < TOL,
3937 "sin({xf}) = {} vs {}",
3938 s.to_f64(),
3939 xf.sin()
3940 );
3941 assert!(
3942 (c.to_f64() - xf.cos()).abs() < TOL,
3943 "cos({xf}) = {} vs {}",
3944 c.to_f64(),
3945 xf.cos()
3946 );
3947 // atan over [-40, 40] (both sides of |x| = 1 argument reduction)
3948 let a = Fix128::from_f64(t * 10.0 / core::f64::consts::PI);
3949 let af = a.to_f64();
3950 assert!(
3951 (a.atan().to_f64() - af.atan()).abs() < TOL,
3952 "atan({af}) = {} vs {}",
3953 a.atan().to_f64(),
3954 af.atan()
3955 );
3956 // sqrt over (0, 1e6]
3957 let q = Fix128::from_f64(1e6 * (i as f64 + 1.0) / (steps as f64 + 1.0));
3958 let qf = q.to_f64();
3959 assert!(
3960 (q.sqrt().to_f64() - qf.sqrt()).abs() < 1e-9 * qf.sqrt().max(1.0),
3961 "sqrt({qf}) = {} vs {}",
3962 q.sqrt().to_f64(),
3963 qf.sqrt()
3964 );
3965 }
3966 // atan2: all four quadrants + axes, radius varied
3967 for iy in -20i64..=20 {
3968 for ix in -20i64..=20 {
3969 if ix == 0 && iy == 0 {
3970 continue;
3971 }
3972 let y = Fix128::from_ratio(iy * 7, 13);
3973 let x = Fix128::from_ratio(ix * 5, 11);
3974 let (yf, xf) = (y.to_f64(), x.to_f64());
3975 let got = Fix128::atan2(y, x).to_f64();
3976 let want = yf.atan2(xf);
3977 // atan2 returns in (-π, π]; the reference may return -π on the
3978 // negative x axis with -0.0 — normalise both to [0, 2π).
3979 let norm =
3980 |v: f64| (v + 2.0 * core::f64::consts::PI) % (2.0 * core::f64::consts::PI);
3981 assert!(
3982 (norm(got) - norm(want)).abs() < TOL
3983 || (norm(got) - norm(want)).abs() > 2.0 * core::f64::consts::PI - TOL,
3984 "atan2({yf}, {xf}) = {got} vs {want}"
3985 );
3986 }
3987 }
3988 }
3989
3990 // -----------------------------------------------------------------------
3991 // sub_simd / cross_simd bit-exactness (x86_64 + `simd` feature only)
3992 // -----------------------------------------------------------------------
3993
3994 /// `Fix128::sub_simd` must be bit-identical to `-`, including borrow
3995 /// propagation from `lo` into `hi` and wrapping at the i64 boundary.
3996 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
3997 #[test]
3998 fn fix128_sub_simd_is_bit_exact_with_operator_sub() {
3999 let cases: [(Fix128, Fix128); 8] = [
4000 (fi(7), fi(3)),
4001 (fi(3), fi(7)),
4002 (fi(-5), fi(9)),
4003 // lo borrow: 0 - tiny must borrow one from hi
4004 (Fix128::from_raw(1, 0), Fix128::from_raw(0, 1)),
4005 // lo wraps fully around
4006 (Fix128::from_raw(0, 0), Fix128::from_raw(0, u64::MAX)),
4007 // arbitrary fractional bit patterns
4008 (
4009 Fix128::from_raw(3, 0xABCD_EF01_2345_6789),
4010 Fix128::from_raw(-1, 0x1111_2222_3333_4444),
4011 ),
4012 (
4013 Fix128::from_raw(-7, 0xFEDC_BA98_7654_3210),
4014 Fix128::from_raw(5, 0xAAAA_BBBB_CCCC_DDDD),
4015 ),
4016 // i64 wrap on hi (wrapping semantics of `-` must be mirrored)
4017 (Fix128::from_raw(i64::MIN, 0), Fix128::from_raw(1, 0)),
4018 ];
4019 for (a, b) in cases {
4020 let scalar = a - b;
4021 // SAFETY: `sub_simd` has no preconditions; SSE2 is part of the
4022 // x86_64 baseline so the `target_feature(enable = "sse2")`
4023 // requirement is always satisfied on this target.
4024 let simd = unsafe { a.sub_simd(b) };
4025 assert_eq!(
4026 simd.hi, scalar.hi,
4027 "sub_simd hi mismatch for {a:?} - {b:?}: {:#018x} vs {:#018x}",
4028 simd.hi, scalar.hi
4029 );
4030 assert_eq!(
4031 simd.lo, scalar.lo,
4032 "sub_simd lo mismatch for {a:?} - {b:?}: {:#018x} vs {:#018x}",
4033 simd.lo, scalar.lo
4034 );
4035 // and the closed-form: (a - b) + b == a
4036 assert_eq!(simd + b, a);
4037 }
4038 // Explicit borrow check: (1.0) - (2^-64) == 0.FFFF…F (hi 0, lo MAX)
4039 // SAFETY: as above.
4040 let borrowed = unsafe { Fix128::from_raw(1, 0).sub_simd(Fix128::from_raw(0, 1)) };
4041 assert_eq!(borrowed, Fix128::from_raw(0, u64::MAX));
4042 }
4043
4044 /// `Vec3Fix::cross_simd` must be bit-identical to `cross()` and satisfy
4045 /// the closed-form cross-product identities.
4046 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
4047 #[test]
4048 fn vec3_cross_simd_is_bit_exact_with_cross() {
4049 // Right-handed basis: x × y = z, y × z = x, z × x = y
4050 let x = v3i(1, 0, 0);
4051 let y = v3i(0, 1, 0);
4052 let z = v3i(0, 0, 1);
4053 assert_eq!(x.cross_simd(y), z);
4054 assert_eq!(y.cross_simd(z), x);
4055 assert_eq!(z.cross_simd(x), y);
4056 // Anti-commutativity and self-cross = 0
4057 assert_eq!(y.cross_simd(x), z.scale(fi(-1)));
4058 assert_eq!(x.cross_simd(x), Vec3Fix::ZERO);
4059
4060 // Textbook integer case: (1,2,3) × (4,5,6) = (-3, 6, -3)
4061 assert_eq!(v3i(1, 2, 3).cross_simd(v3i(4, 5, 6)), v3i(-3, 6, -3));
4062
4063 // Arbitrary fractional patterns: bit-exact with the scalar path,
4064 // and the result is orthogonal to both inputs (dot == 0 exactly is
4065 // not guaranteed after Fix128 rounding, so compare to the scalar
4066 // path's dot instead of to zero).
4067 let pairs = [
4068 (v3i(1, 2, 3), v3i(4, 5, 6)),
4069 (v3i(-7, 0, 11), v3i(2, -9, 5)),
4070 (
4071 Vec3Fix::new(
4072 Fix128::from_raw(3, 0xABCD_EF01_2345_6789),
4073 Fix128::from_raw(-1, 0x1111_2222_3333_4444),
4074 Fix128::from_raw(7, 0xFEDC_BA98_7654_3210),
4075 ),
4076 Vec3Fix::new(
4077 Fix128::from_raw(2, 0x9876_5432_10FE_DCBA),
4078 Fix128::from_raw(5, 0xAAAA_BBBB_CCCC_DDDD),
4079 Fix128::from_raw(-3, 0x0F0F_0F0F_0F0F_0F0F),
4080 ),
4081 ),
4082 ];
4083 for (a, b) in pairs {
4084 let scalar = a.cross(b);
4085 let simd = a.cross_simd(b);
4086 assert_eq!(simd.x.hi, scalar.x.hi, "{a:?} × {b:?}: x.hi");
4087 assert_eq!(simd.x.lo, scalar.x.lo, "{a:?} × {b:?}: x.lo");
4088 assert_eq!(simd.y.hi, scalar.y.hi, "{a:?} × {b:?}: y.hi");
4089 assert_eq!(simd.y.lo, scalar.y.lo, "{a:?} × {b:?}: y.lo");
4090 assert_eq!(simd.z.hi, scalar.z.hi, "{a:?} × {b:?}: z.hi");
4091 assert_eq!(simd.z.lo, scalar.z.lo, "{a:?} × {b:?}: z.lo");
4092 assert_eq!(simd.dot(a), scalar.dot(a));
4093 assert_eq!(simd.dot(b), scalar.dot(b));
4094 // a × b == -(b × a)
4095 assert_eq!(b.cross_simd(a), simd.scale(fi(-1)));
4096 }
4097 }
4098 /// `atan2` with |y| > |x|: the ratio `y / x` overflows the i64 integer
4099 /// part once |x| is a few ulp (1 / 2⁻⁶³ = 2⁶³), so before 1.2.0
4100 /// `atan2(1, 2 ulp)` returned −0.755. The complement form keeps every
4101 /// case within 1e-12 of the f64 reference, including the quadrant
4102 /// boundaries and the exact axes.
4103 #[test]
4104 #[allow(clippy::disallowed_methods)]
4105 fn atan2_is_accurate_when_x_is_a_few_ulp() {
4106 let ulp2 = Fix128::from_raw(0, 2);
4107 let cases = [
4108 (Fix128::ONE, ulp2),
4109 (Fix128::ONE, -ulp2),
4110 (-Fix128::ONE, ulp2),
4111 (-Fix128::ONE, -ulp2),
4112 (Fix128::ONE - Fix128::from_raw(0, 3), ulp2),
4113 (Fix128::from_ratio(7, 3), Fix128::from_ratio(-1, 5)),
4114 (Fix128::from_ratio(-7, 3), Fix128::from_ratio(1, 5)),
4115 (Fix128::from_ratio(1, 5), Fix128::from_ratio(-7, 3)),
4116 (Fix128::from_int(3), Fix128::from_int(4)),
4117 (Fix128::from_int(4), Fix128::from_int(3)),
4118 ];
4119 for (y, x) in cases {
4120 let got = Fix128::atan2(y, x).to_f64();
4121 let want = y.to_f64().atan2(x.to_f64());
4122 assert!(
4123 (got - want).abs() < 1e-12,
4124 "atan2({}, {}) = {got}, want {want}",
4125 y.to_f64(),
4126 x.to_f64()
4127 );
4128 }
4129 // exact axes are unchanged
4130 assert_eq!(Fix128::atan2(Fix128::ONE, Fix128::ZERO), Fix128::HALF_PI);
4131 assert_eq!(Fix128::atan2(-Fix128::ONE, Fix128::ZERO), -Fix128::HALF_PI);
4132 // atan(0) through CORDIC is ~5e-15, not exactly 0 (unchanged behaviour)
4133 assert!(Fix128::atan2(Fix128::ZERO, Fix128::ONE).abs() < Fix128::from_f64(1e-12));
4134 }
4135
4136 // ------------------------------------------------------------------
4137 // Mutation-score batch 10 (scoped run 34960080446, 54 missed).
4138 //
4139 // Killable misses are covered below. The remaining misses are equivalent
4140 // mutants and are documented here rather than hidden:
4141 // - `|` → `^` on disjoint bit ranges: `sqrt` (`(rem << 2) | bits`,
4142 // `(root << k) | 1`), `half` / `shr_bits` / `double` (carry bit into a
4143 // freshly shifted-in zero), `Div` (`(hi << 64) | lo`), `select_fix128`
4144 // (`mask` and `!mask` are complementary) — identical results.
4145 // - `from_f64`: `f < 0.0` → `<=`: for `f == 0.0` `lo` is 0 and both
4146 // branches yield the same value.
4147 // - `from_ratio`: `num < 0` → `<=` (0 negated is 0) and `denom < 0` →
4148 // `<=` (`denom == 0` already returned).
4149 // - `cordic_sin_cos`: `>` → `>=` / `<` → `<=` on the ±π boundaries and
4150 // `+ π` → `- π` in `k = floor((θ + π) / 2π)` reduce to the same angle
4151 // because the clamp that follows absorbs exactly one 2π; the clamp
4152 // branches themselves (`θ > π` / `θ < −π` after reduction) are only
4153 // reachable through rounding of `2π · k` and no sampled angle reaches
4154 // them; `d > 0` → `>=` in both CORDIC loops: `d` is ±1, never 0.
4155 // - `sqrt`: `i += 1` → `i *= 1` loops forever and can only time out.
4156 // ------------------------------------------------------------------
4157
4158 /// `ceil` rounds toward +∞: integers are fixed points, any fraction
4159 /// bumps the integer part by exactly one (also across zero for
4160 /// negative inputs, where the two's-complement `hi` is already the floor).
4161 #[test]
4162 fn ceil_rounds_toward_positive_infinity() {
4163 assert_eq!(Fix128::from_int(2).ceil(), Fix128::from_int(2));
4164 assert_eq!(Fix128::from_int(-2).ceil(), Fix128::from_int(-2));
4165 assert_eq!(Fix128::ZERO.ceil(), Fix128::ZERO);
4166 assert_eq!(Fix128::from_ratio(5, 2).ceil(), Fix128::from_int(3));
4167 assert_eq!(Fix128::from_ratio(-5, 2).ceil(), Fix128::from_int(-2));
4168 assert_eq!(Fix128::from_ratio(-1, 2).ceil(), Fix128::ZERO);
4169 assert_eq!(Fix128::from_raw(7, 1).ceil(), Fix128::from_int(8));
4170 // ceil(x) − x ∈ [0, 1) and ceil(x) == −floor(−x)
4171 for raw in [
4172 Fix128::from_ratio(1, 3),
4173 Fix128::from_ratio(-1, 3),
4174 Fix128::from_ratio(1_000_001, 7),
4175 Fix128::from_ratio(-1_000_001, 7),
4176 Fix128::from_raw(-3, u64::MAX),
4177 ] {
4178 let c = raw.ceil();
4179 assert!(c >= raw && c - raw < Fix128::ONE, "{raw}");
4180 assert_eq!(c, raw.neg().floor().neg(), "{raw}");
4181 }
4182 }
4183
4184 /// `from_ratio` takes the sign from `num` and `denom` independently:
4185 /// −3/5 == 3/−5 == −(3/5), and −3/−5 == 3/5. The negative value is
4186 /// pinned to its two's-complement pair: 3/5 = ⌊0.6 · 2⁶⁴⌋ =
4187 /// 11068046444225730969, so −3/5 = (−1, 2⁶⁴ − 11068046444225730969).
4188 #[test]
4189 fn from_ratio_sign_comes_from_both_operands() {
4190 let pos = Fix128::from_ratio(3, 5);
4191 assert_eq!(pos, Fix128::from_raw(0, 11_068_046_444_225_730_969));
4192 let neg = Fix128::from_raw(-1, 7_378_697_629_483_820_647);
4193 assert_eq!(Fix128::from_ratio(-3, 5), neg);
4194 assert_eq!(Fix128::from_ratio(3, -5), neg);
4195 assert_eq!(Fix128::from_ratio(-3, -5), pos);
4196 assert_eq!(neg, pos.neg());
4197 assert_eq!(Fix128::from_ratio(0, -5), Fix128::ZERO);
4198 assert_eq!(Fix128::from_ratio(-7, 1), Fix128::from_int(-7));
4199 assert_eq!(Fix128::from_ratio(7, -1), Fix128::from_int(-7));
4200 assert!(Fix128::from_ratio(1, -1_000_000).is_negative());
4201 }
4202
4203 /// `exp` cut-off boundary: e⁻⁴⁴ = 7.78 · 10⁻²⁰ is 1.435 · 2⁻⁶⁴ (oracle:
4204 /// 60-digit decimal evaluation of e⁻⁴⁴ · 2⁶⁴ = 1.4353…), so it is still
4205 /// representable, while e⁻⁴⁵ = 0.53 · 2⁻⁶⁴ truncates to zero. The
4206 /// `hi < -44` early return must therefore not include −44, and must fire
4207 /// for every smaller integer part. Between −44 and −43.67 the
4208 /// intermediate 2^(|x|·log₂e) exceeds 2⁶³ and wraps in `powf_pos`, which
4209 /// lands the reciprocal on 2 ulp instead of the truncated 1 ulp — within
4210 /// one ulp of the true value, so the assertion is a range, not a pin.
4211 #[test]
4212 fn exp_cutoff_boundary_is_minus_44() {
4213 let at_boundary = Fix128::from_int(-44).exp();
4214 assert!(!at_boundary.is_zero());
4215 assert!(at_boundary <= Fix128::from_raw(0, 2), "{at_boundary}");
4216 assert_eq!(Fix128::from_int(-45).exp(), Fix128::ZERO);
4217 assert_eq!(Fix128::from_int(-46).exp(), Fix128::ZERO);
4218 assert_eq!(Fix128::from_int(-100).exp(), Fix128::ZERO);
4219 assert_eq!(Fix128::from_raw(-45, 1).exp(), Fix128::ZERO);
4220 // just above the boundary is still non-zero and ordered
4221 let a = Fix128::from_raw(-44, 1).exp();
4222 let b = Fix128::from_int(-43).exp();
4223 assert!(!a.is_zero());
4224 assert!(a < b, "{a} < {b}");
4225 }
4226
4227 /// `QuatFix::normalize` divides every component by the length (as a
4228 /// reciprocal multiply); a mutant that multiplies `x` by the length
4229 /// instead of its reciprocal is only visible with a non-zero `x` and a
4230 /// length ≠ 1: (3, 0, 0, 4) / 5 → x = 0.6, not 15.
4231 #[test]
4232 fn quat_normalize_divides_x_by_length() {
4233 let n = q(3, 0, 0, 4).normalize();
4234 assert_eq!(
4235 n,
4236 QuatFix::new(fi(3) / fi(5), Fix128::ZERO, Fix128::ZERO, fi(4) / fi(5))
4237 );
4238 // reciprocal multiply differs from a direct division by a few ulp
4239 let x = q(-6, 0, 0, 8).normalize().x;
4240 assert!((x - fi(-6) / fi(10)).abs() < Fix128::from_raw(0, 16), "{x}");
4241 assert_eq!(q(1, 0, 0, 0).normalize(), q(1, 0, 0, 0));
4242 }
4243
4244 /// `simd_width()` is the compile-time lane count and never 0; without
4245 /// the `simd` feature it is exactly 1 (the scalar path), with it on
4246 /// x86_64 it is 4 (SSE2) or 8 (AVX2) and on aarch64 4 (NEON).
4247 #[test]
4248 fn simd_width_matches_the_build_target() {
4249 assert!(simd_width() >= 1);
4250 assert_eq!(SIMD_WIDTH, simd_width());
4251 #[cfg(not(feature = "simd"))]
4252 assert_eq!(simd_width(), 1);
4253 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
4254 assert!(matches!(simd_width(), 4 | 8));
4255 #[cfg(all(feature = "simd", target_arch = "aarch64"))]
4256 assert_eq!(simd_width(), 4);
4257 #[cfg(all(
4258 feature = "simd",
4259 not(any(target_arch = "x86_64", target_arch = "aarch64"))
4260 ))]
4261 assert_eq!(simd_width(), 1);
4262 }
4263
4264 fn p2(e: u32) -> Fix128 {
4265 Fix128::from_int(1_i64 << e)
4266 }
4267
4268 /// oracle: `checked_normalize` is `None` for the zero vector and when the
4269 /// squared length leaves the range (`|v| = 2⁴⁰`), and `v/|v|` otherwise
4270 /// (`(3, 4, 0)/5`).
4271 #[test]
4272 fn checked_normalize_closed_form() {
4273 assert_eq!(Vec3Fix::ZERO.checked_normalize(), None);
4274 assert_eq!(
4275 Vec3Fix::new(p2(40), Fix128::ZERO, Fix128::ZERO).checked_normalize(),
4276 None
4277 );
4278 let n = Vec3Fix::from_int(3, 4, 0)
4279 .checked_normalize()
4280 .expect("in range");
4281 assert!((n.x.to_f64() - 0.6).abs() < 1e-15 && (n.y.to_f64() - 0.8).abs() < 1e-15);
4282 assert!(n.z.is_zero());
4283 }
4284
4285 /// oracle: past the squared range the length of `(2⁴⁰, 0, 0)` is exactly
4286 /// `2⁴⁰`, of `(2⁴⁰, 2⁴⁰, 0)` is `√2·2⁴⁰` and of `(2⁶², 2⁶², 2⁶²)` is
4287 /// `√3·2⁶² < 2⁶³` (to 1e-15 relative); a length of `√3·3·2⁶¹ ≥ 2⁶³` does
4288 /// not fit (`None`).
4289 #[test]
4290 fn checked_length_scaled_closed_form() {
4291 assert_eq!(
4292 Vec3Fix::new(p2(40), Fix128::ZERO, Fix128::ZERO).checked_length_scaled(),
4293 Some(p2(40))
4294 );
4295 let l = Vec3Fix::new(p2(40), p2(40), Fix128::ZERO)
4296 .checked_length_scaled()
4297 .expect("fits");
4298 assert!(
4299 (l.to_f64() / 1_099_511_627_776.0 - 2f64.sqrt()).abs() < 1e-15,
4300 "{l:?}"
4301 );
4302 let l = Vec3Fix::new(p2(62), p2(62), p2(62))
4303 .checked_length_scaled()
4304 .expect("√3·2⁶² < 2⁶³ fits");
4305 assert!(
4306 (l.to_f64() / 4_611_686_018_427_387_904.0 - 3f64.sqrt()).abs() < 1e-15,
4307 "{l:?}"
4308 );
4309 let c = Fix128::from_int(3_i64 << 61);
4310 assert_eq!(Vec3Fix::new(c, c, c).checked_length_scaled(), None);
4311 }
4312
4313 /// oracle: `try_normalize_scaled` gives the unit vector `(1, 0, 0)` for
4314 /// `(2⁴⁰, 0, 0)` (squared length out of range) and for `(2⁻⁴⁰, 0, 0)`
4315 /// (squared length below the resolution), `(0, −1, 0)` for `(0, −2⁴⁰, 0)`,
4316 /// and `None` for the zero vector.
4317 #[test]
4318 fn try_normalize_scaled_closed_form() {
4319 let unit_x = Vec3Fix::new(Fix128::ONE, Fix128::ZERO, Fix128::ZERO);
4320 assert_eq!(
4321 Vec3Fix::new(p2(40), Fix128::ZERO, Fix128::ZERO).try_normalize_scaled(),
4322 Some(unit_x)
4323 );
4324 let tiny = Fix128::ONE / p2(40);
4325 assert_eq!(
4326 Vec3Fix::new(tiny, Fix128::ZERO, Fix128::ZERO).try_normalize_scaled(),
4327 Some(unit_x)
4328 );
4329 assert_eq!(
4330 Vec3Fix::new(Fix128::ZERO, -p2(40), Fix128::ZERO).try_normalize_scaled(),
4331 Some(Vec3Fix::new(Fix128::ZERO, -Fix128::ONE, Fix128::ZERO))
4332 );
4333 assert_eq!(Vec3Fix::ZERO.try_normalize_scaled(), None);
4334 assert_eq!(Vec3Fix::ZERO.split_pow2(), (Vec3Fix::ZERO, 0));
4335 }
4336}