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 /// Normalize to unit length.
1247 ///
1248 /// Returns `Self::ZERO` for zero-length vectors. Use [`Self::try_normalize`]
1249 /// when you need to distinguish a zero-length input from a valid unit vector.
1250 #[inline(always)]
1251 #[must_use]
1252 pub fn normalize(self) -> Self {
1253 let len = self.length();
1254 if len.is_zero() {
1255 Self::ZERO
1256 } else {
1257 self / len
1258 }
1259 }
1260
1261 /// Try to normalize, returning `None` for zero-length vectors.
1262 #[inline(always)]
1263 #[must_use]
1264 pub fn try_normalize(self) -> Option<Self> {
1265 let len = self.length();
1266 if len.is_zero() {
1267 None
1268 } else {
1269 Some(self / len)
1270 }
1271 }
1272
1273 /// Normalize and return both the unit vector and the original length.
1274 ///
1275 /// Avoids double sqrt when both normalized direction and distance are needed.
1276 /// Returns `(Self::ZERO, Fix128::ZERO)` if the vector is zero-length.
1277 #[inline(always)]
1278 #[must_use]
1279 pub fn normalize_with_length(self) -> (Self, Fix128) {
1280 let len = self.length();
1281 if len.is_zero() {
1282 (Self::ZERO, Fix128::ZERO)
1283 } else {
1284 let inv_len = Fix128::ONE / len;
1285 (self * inv_len, len)
1286 }
1287 }
1288
1289 // ------------------------------------------------------------------------
1290 // 範囲を検査する版
1291 //
1292 // `dot` / `length_squared` は各成分の積と和が wrap する (mod 2¹²⁸) ので、
1293 // |v| ≥ 2³¹·⁵ ≈ 3.04e9 で `length` は 0、`normalize` は ZERO を返す
1294 // (`tests/analytic_coordinate_range.rs`) 既存の関数は決定論の固定値と
1295 // bit 互換を保つため変えず、範囲の検査が要る経路だけが以下を使う
1296 // ------------------------------------------------------------------------
1297
1298 /// 成分ごとの [`Fix128::checked_add`] (`+` と同じ和、どれかの成分が
1299 /// `|·| ≥ 2⁶³` なら `None`)
1300 ///
1301 /// # Claims
1302 /// - `Some` の時は `self + rhs` と bit 一致する
1303 /// - 位置の加算のように wrap が反対側の端への移動になる経路だけが使う
1304 #[inline]
1305 #[must_use]
1306 pub fn checked_add(self, rhs: Self) -> Option<Self> {
1307 Some(Self {
1308 x: self.x.checked_add(rhs.x)?,
1309 y: self.y.checked_add(rhs.y)?,
1310 z: self.z.checked_add(rhs.z)?,
1311 })
1312 }
1313
1314 /// [`Self::dot`] と同じ値を計算し、**積か和のどれかが範囲外なら `None`** を返す
1315 ///
1316 /// # Claims
1317 /// - `Some` の時は `dot` と bit 一致する (同じ積を同じ順で足す)
1318 /// - 3 つの積のどれか、または途中の和が `|·| ≥ 2⁶³` なら `None`
1319 #[must_use]
1320 pub fn checked_dot(self, rhs: Self) -> Option<Fix128> {
1321 let x = self.x.checked_mul(rhs.x)?;
1322 let y = self.y.checked_mul(rhs.y)?;
1323 let z = self.z.checked_mul(rhs.z)?;
1324 x.checked_add(y)?.checked_add(z)
1325 }
1326
1327 /// [`Self::length_squared`] の範囲検査版 (`|v|² ≥ 2⁶³` で `None`)
1328 ///
1329 /// `Some` の時は `length_squared` と bit 一致する
1330 #[must_use]
1331 pub fn checked_length_squared(self) -> Option<Fix128> {
1332 self.checked_dot(self)
1333 }
1334
1335 /// [`Self::length`] の範囲検査版 (`|v| ≥ 2³¹·⁵` で `None`)
1336 ///
1337 /// `Some` の時は `length` と bit 一致する 範囲外でも長さが要るなら
1338 /// [`Self::checked_length_scaled`] を使う
1339 #[must_use]
1340 pub fn checked_length(self) -> Option<Fix128> {
1341 Some(self.checked_length_squared()?.sqrt())
1342 }
1343
1344 /// [`Self::try_normalize`] の範囲検査版
1345 ///
1346 /// 長さ 0 と `|v| ≥ 2³¹·⁵` (長さの 2 乗が範囲外) のどちらも `None`
1347 /// `Some` の時は `try_normalize` と bit 一致する 範囲外でも向きが要るなら
1348 /// [`Self::try_normalize_scaled`] を使う
1349 #[must_use]
1350 pub fn checked_normalize(self) -> Option<Self> {
1351 let len = self.checked_length()?;
1352 if len.is_zero() {
1353 None
1354 } else {
1355 Some(self / len)
1356 }
1357 }
1358
1359 /// 最大成分を `[2²⁹, 2³⁰)` に寄せた同じ向きのベクトルと、その倍率の指数 `k`
1360 /// (`self ≈ scaled · 2ᵏ`) 零ベクトルは `(ZERO, 0)`
1361 ///
1362 /// 寄せた後は 2 乗の和が `3·2⁶⁰` 未満なので `length_squared` が wrap しない
1363 /// 縮める時 (`k > 0`) は各成分の絶対値の下位 `k` bit を切り捨てる (符号は
1364 /// 保つので向きは原点対称)、広げる時 (`k < 0`) は厳密
1365 fn split_pow2(self) -> (Self, i32) {
1366 let raw = |f: Fix128| ((f.hi as i128) << 64) | (f.lo as i128);
1367 let (x, y, z) = (raw(self.x), raw(self.y), raw(self.z));
1368 let m = x.unsigned_abs().max(y.unsigned_abs()).max(z.unsigned_abs());
1369 if m == 0 {
1370 return (Self::ZERO, 0);
1371 }
1372 // bit 64 が 1.0、bit 93 が 2²⁹
1373 let k = (127 - m.leading_zeros() as i32) - 93;
1374 let scale = |r: i128| -> Fix128 {
1375 let a = r.unsigned_abs();
1376 let s = if k >= 0 { a >> k } else { a << (-k) };
1377 // s < 2⁹⁴ なので i128 に収まる
1378 let v = if r < 0 { -(s as i128) } else { s as i128 };
1379 Fix128::from_raw((v >> 64) as i64, v as u64)
1380 };
1381 (Self::new(scale(x), scale(y), scale(z)), k)
1382 }
1383
1384 /// 2 乗が範囲外になる大きさでも正しい長さ 結果自体が `≥ 2⁶³` (表せない)
1385 /// 時だけ `None`
1386 ///
1387 /// # Claims
1388 /// - `|v|² < 2⁶³` (`length_squared` が wrap しない範囲) では **`length` と
1389 /// bit 一致する** (同じ式をそのまま使う)
1390 /// - それより大きい時は、最大成分を 2 の冪で `[2²⁹, 2³⁰)` に寄せてから
1391 /// 2 乗するので wrap せず、相対誤差は約 `2⁻⁹²` (成分の切り捨てと
1392 /// `sqrt` の floor)
1393 /// - 結果が `≥ 2⁶³` (成分が `2⁶²` 級) なら `None`
1394 #[must_use]
1395 pub fn checked_length_scaled(self) -> Option<Fix128> {
1396 if let Some(len) = self.checked_length() {
1397 return Some(len);
1398 }
1399 let (s, k) = self.split_pow2();
1400 // ここに来るのは |v| ≥ 2³¹·⁵ の時だけなので k ≥ 1
1401 let ls = s.length();
1402 let raw = ((ls.hi as i128) << 64) | (ls.lo as i128);
1403 // ls < 2³¹ なので raw ≥ 0、raw << k < 2¹²⁷ が要る
1404 if k < 0 || raw.leading_zeros() <= k as u32 {
1405 return None;
1406 }
1407 let r = raw << k;
1408 Some(Fix128::from_raw((r >> 64) as i64, r as u64))
1409 }
1410
1411 /// 2 乗が範囲外になる大きさでも向きを返す正規化 零ベクトルだけ `None`
1412 ///
1413 /// # Claims
1414 /// - `try_normalize` が `Some` を返す入力では **`try_normalize` と bit 一致**
1415 /// - `try_normalize` が範囲外 (`|v| ≥ 2³¹·⁵`) や長さの 2 乗の下位桁あふれ
1416 /// (`|v| < 約 2⁻³²`) で `None` を返す非零ベクトルには、最大成分を
1417 /// `[2²⁹, 2³⁰)` に寄せたベクトルを正規化して返す
1418 #[must_use]
1419 pub fn try_normalize_scaled(self) -> Option<Self> {
1420 if let Some(n) = self.checked_normalize() {
1421 return Some(n);
1422 }
1423 let (s, _) = self.split_pow2();
1424 if s == Self::ZERO {
1425 return None;
1426 }
1427 // 最大成分 ∈ [2²⁹, 2³⁰) なので長さは 0 にならず、2 乗も wrap しない
1428 Some(s / s.length())
1429 }
1430
1431 /// Scale by scalar
1432 #[inline]
1433 #[must_use]
1434 pub fn scale(self, s: Fix128) -> Self {
1435 Self {
1436 x: self.x * s,
1437 y: self.y * s,
1438 z: self.z * s,
1439 }
1440 }
1441
1442 // ========================================================================
1443 // SIMD-Accelerated Operations
1444 // ========================================================================
1445
1446 /// SIMD-accelerated dot product — SSE2 inner implementation.
1447 ///
1448 /// # Determinism guarantee
1449 ///
1450 /// Dot product on the `simd` feature's x86_64 path — **scalar-equivalent**.
1451 ///
1452 /// Fix128 multiplication is 128-bit integer arithmetic (`u128`/`i128`); no
1453 /// SSE2/AVX2 instruction performs it, and the two Fix128 additions cannot
1454 /// use `_mm_add_epi64` because it does not carry from `lo` into `hi` (see
1455 /// the comment in the body). The result is bit-exact to `dot()` because it
1456 /// *is* `dot()`. Kept as the dispatch target of `dot_simd` so a future
1457 /// AVX-512 / NEON batch path can land without an API change.
1458 ///
1459 /// # Safety
1460 ///
1461 /// No preconditions; `#[target_feature(enable = "sse2")]` is always
1462 /// satisfied on x86_64.
1463 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
1464 #[target_feature(enable = "sse2")]
1465 unsafe fn dot_simd_sse2(self, rhs: Self) -> Fix128 {
1466 // --- Step 1: scalar Fix128 multiplications (128-bit, bit-exact) ------
1467 // SSE2 has no 128-bit multiply, so the multiply stays scalar.
1468 let px = self.x * rhs.x;
1469 let py = self.y * rhs.y;
1470 let pz = self.z * rhs.z;
1471
1472 // --- Step 2: Scalar Fix128 addition chain (px + py + pz) -------------
1473 //
1474 // SSE2's _mm_add_epi64 performs lane-independent 64-bit adds but cannot
1475 // propagate the carry from lo to hi that Fix128 addition requires.
1476 // A correct SIMD path would need to detect per-lane overflow, extract it,
1477 // and feed it into the hi lane — more instructions than the scalar path.
1478 //
1479 // Therefore we use the direct scalar carry chain which is already well
1480 // optimized by LLVM into ADC (add-with-carry) on x86_64.
1481 let ab = px + py;
1482 ab + pz
1483 }
1484
1485 /// SIMD-accelerated dot product — safe public entry point.
1486 ///
1487 /// On `x86_64` with the `simd` feature enabled this dispatches to the SSE2
1488 /// path (`dot_simd_sse2`). On every other platform it falls back to the
1489 /// scalar `dot()`. The result is **bit-exact identical** to `dot()` on every
1490 /// platform.
1491 #[inline]
1492 #[must_use]
1493 pub fn dot_simd(self, rhs: Self) -> Fix128 {
1494 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
1495 {
1496 // SAFETY: SSE2 is part of the x86_64 baseline ABI; all x86_64 CPUs
1497 // support it unconditionally.
1498 unsafe { self.dot_simd_sse2(rhs) }
1499 }
1500 #[cfg(not(all(feature = "simd", target_arch = "x86_64")))]
1501 {
1502 self.dot(rhs)
1503 }
1504 }
1505
1506 /// Squared length using SIMD-accelerated dot product.
1507 ///
1508 /// Equivalent to `length_squared()` but dispatches through `dot_simd`.
1509 /// Result is **bit-exact identical** to `length_squared()`.
1510 #[inline]
1511 #[must_use]
1512 pub fn length_squared_simd(self) -> Fix128 {
1513 self.dot_simd(self)
1514 }
1515
1516 /// Cross product through the `simd` feature's API surface —
1517 /// **scalar-equivalent**, bit-identical to `cross()` (see the module docs).
1518 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
1519 #[inline]
1520 pub fn cross_simd(self, rhs: Self) -> Self {
1521 // 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)
1522 // Compute all 6 products first, then subtract
1523 let yz = self.y * rhs.z;
1524 let zy = self.z * rhs.y;
1525 let zx = self.z * rhs.x;
1526 let xz = self.x * rhs.z;
1527 let xy = self.x * rhs.y;
1528 let yx = self.y * rhs.x;
1529
1530 Self {
1531 x: yz - zy,
1532 y: zx - xz,
1533 z: xy - yx,
1534 }
1535 }
1536
1537 /// Batch dot product for 4 vector pairs — **scalar-equivalent** (four
1538 /// `dot()` calls, bit-identical). The batch shape is the API a future
1539 /// AVX-512 / NEON path would fill in.
1540 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
1541 pub fn dot_batch_4(a: [Self; 4], b: [Self; 4]) -> [Fix128; 4] {
1542 // Process all 4 pairs
1543 [
1544 a[0].dot(b[0]),
1545 a[1].dot(b[1]),
1546 a[2].dot(b[2]),
1547 a[3].dot(b[3]),
1548 ]
1549 }
1550}
1551
1552impl Add for Vec3Fix {
1553 type Output = Self;
1554
1555 #[inline]
1556 fn add(self, rhs: Self) -> Self {
1557 Self {
1558 x: self.x + rhs.x,
1559 y: self.y + rhs.y,
1560 z: self.z + rhs.z,
1561 }
1562 }
1563}
1564
1565impl Sub for Vec3Fix {
1566 type Output = Self;
1567
1568 #[inline]
1569 fn sub(self, rhs: Self) -> Self {
1570 Self {
1571 x: self.x - rhs.x,
1572 y: self.y - rhs.y,
1573 z: self.z - rhs.z,
1574 }
1575 }
1576}
1577
1578impl Mul<Fix128> for Vec3Fix {
1579 type Output = Self;
1580
1581 #[inline]
1582 fn mul(self, rhs: Fix128) -> Self {
1583 self.scale(rhs)
1584 }
1585}
1586
1587impl Div<Fix128> for Vec3Fix {
1588 type Output = Self;
1589
1590 #[inline]
1591 fn div(self, rhs: Fix128) -> Self {
1592 Self {
1593 x: self.x / rhs,
1594 y: self.y / rhs,
1595 z: self.z / rhs,
1596 }
1597 }
1598}
1599
1600impl Neg for Vec3Fix {
1601 type Output = Self;
1602
1603 #[inline]
1604 fn neg(self) -> Self {
1605 Self {
1606 x: -self.x,
1607 y: -self.y,
1608 z: -self.z,
1609 }
1610 }
1611}
1612
1613impl core::fmt::Display for Vec3Fix {
1614 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1615 write!(f, "({}, {}, {})", self.x, self.y, self.z)
1616 }
1617}
1618
1619impl From<[Fix128; 3]> for Vec3Fix {
1620 #[inline]
1621 fn from(arr: [Fix128; 3]) -> Self {
1622 Self::new(arr[0], arr[1], arr[2])
1623 }
1624}
1625
1626impl From<Vec3Fix> for [Fix128; 3] {
1627 #[inline]
1628 fn from(v: Vec3Fix) -> Self {
1629 [v.x, v.y, v.z]
1630 }
1631}
1632
1633// ============================================================================
1634// QuatFix - Quaternion with Fixed-Point Components
1635// ============================================================================
1636
1637/// Quaternion using Fix128 components (for rotations)
1638///
1639/// Stored as (x, y, z, w) where w is the scalar part
1640#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
1641#[repr(C)]
1642pub struct QuatFix {
1643 /// X component (imaginary i)
1644 pub x: Fix128,
1645 /// Y component (imaginary j)
1646 pub y: Fix128,
1647 /// Z component (imaginary k)
1648 pub z: Fix128,
1649 /// W component (scalar/real part)
1650 pub w: Fix128,
1651}
1652
1653impl QuatFix {
1654 /// Identity quaternion (no rotation)
1655 pub const IDENTITY: Self = Self {
1656 x: Fix128::ZERO,
1657 y: Fix128::ZERO,
1658 z: Fix128::ZERO,
1659 w: Fix128::ONE,
1660 };
1661
1662 /// Create new quaternion
1663 #[inline]
1664 #[must_use]
1665 pub const fn new(x: Fix128, y: Fix128, z: Fix128, w: Fix128) -> Self {
1666 Self { x, y, z, w }
1667 }
1668
1669 /// Create from axis-angle representation
1670 #[must_use]
1671 pub fn from_axis_angle(axis: Vec3Fix, angle: Fix128) -> Self {
1672 let half_angle = angle.half();
1673 let (sin_ha, cos_ha) = half_angle.sin_cos();
1674 let axis_norm = axis.normalize();
1675 // no axis, no rotation: (0, 0, 0, cos) is not a unit quaternion and
1676 // would scale whatever it rotates by cos^2 (AUD-A-S1W5-023)
1677 if axis_norm == Vec3Fix::ZERO {
1678 return Self::IDENTITY;
1679 }
1680
1681 Self {
1682 x: axis_norm.x * sin_ha,
1683 y: axis_norm.y * sin_ha,
1684 z: axis_norm.z * sin_ha,
1685 w: cos_ha,
1686 }
1687 }
1688
1689 /// Quaternion multiplication (composition of rotations)
1690 #[allow(clippy::should_implement_trait)]
1691 #[must_use]
1692 pub fn mul(self, rhs: Self) -> Self {
1693 Self {
1694 x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
1695 y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
1696 z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w,
1697 w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
1698 }
1699 }
1700
1701 /// Conjugate (inverse for unit quaternions)
1702 #[inline]
1703 #[must_use]
1704 pub fn conjugate(self) -> Self {
1705 Self {
1706 x: -self.x,
1707 y: -self.y,
1708 z: -self.z,
1709 w: self.w,
1710 }
1711 }
1712
1713 /// Squared magnitude
1714 #[inline]
1715 #[must_use]
1716 pub fn length_squared(self) -> Fix128 {
1717 self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w
1718 }
1719
1720 /// Magnitude
1721 #[inline(always)]
1722 #[must_use]
1723 pub fn length(self) -> Fix128 {
1724 self.length_squared().sqrt()
1725 }
1726
1727 /// This rotation as a unit quaternion, for code that applies a stored
1728 /// rotation: a quaternion whose squared length is within `2^-32` of one
1729 /// is returned unchanged (bit for bit), any other is normalized, and the
1730 /// zero quaternion becomes the identity.
1731 ///
1732 /// `q v q*` scales by `|q|^2`, so applying a non-unit quaternion resizes
1733 /// what it rotates (AUD-A-S4W1-004, AUD-A-S5W3-004, AUD-A-S5W3-014).
1734 #[must_use]
1735 pub(crate) fn unit_rotation(self) -> Self {
1736 let tol = Fix128::from_raw(0, 1 << 32);
1737 if (self.length_squared() - Fix128::ONE).abs() <= tol {
1738 self
1739 } else {
1740 self.normalize()
1741 }
1742 }
1743
1744 /// Normalize to unit quaternion (reciprocal: 1 division + 4 multiplications)
1745 #[inline(always)]
1746 #[must_use]
1747 pub fn normalize(self) -> Self {
1748 let len = self.length();
1749 if len.is_zero() {
1750 Self::IDENTITY
1751 } else {
1752 let inv_len = Fix128::ONE / len;
1753 Self {
1754 x: self.x * inv_len,
1755 y: self.y * inv_len,
1756 z: self.z * inv_len,
1757 w: self.w * inv_len,
1758 }
1759 }
1760 }
1761
1762 /// Rotate a vector by this quaternion
1763 #[must_use]
1764 pub fn rotate_vec(self, v: Vec3Fix) -> Vec3Fix {
1765 // q * v * q^-1
1766 let qv = Self::new(v.x, v.y, v.z, Fix128::ZERO);
1767 let result = self.mul(qv).mul(self.conjugate());
1768 Vec3Fix::new(result.x, result.y, result.z)
1769 }
1770}
1771
1772impl core::fmt::Display for QuatFix {
1773 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1774 write!(f, "({}, {}, {}, {})", self.x, self.y, self.z, self.w)
1775 }
1776}
1777
1778impl From<[Fix128; 4]> for QuatFix {
1779 #[inline]
1780 fn from(arr: [Fix128; 4]) -> Self {
1781 Self::new(arr[0], arr[1], arr[2], arr[3])
1782 }
1783}
1784
1785impl From<QuatFix> for [Fix128; 4] {
1786 #[inline]
1787 fn from(q: QuatFix) -> Self {
1788 [q.x, q.y, q.z, q.w]
1789 }
1790}
1791
1792// ============================================================================
1793// 3x3 Matrix (Inertia Tensor)
1794// ============================================================================
1795
1796/// 3x3 Matrix for inertia tensors and rotations
1797#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
1798#[repr(C)]
1799pub struct Mat3Fix {
1800 /// Column 0
1801 pub col0: Vec3Fix,
1802 /// Column 1
1803 pub col1: Vec3Fix,
1804 /// Column 2
1805 pub col2: Vec3Fix,
1806}
1807
1808/// Why [`Mat3Fix::polar_rotation`] could not produce a rotation.
1809///
1810/// The three refusals and the two iteration outcomes are separate variants on
1811/// purpose: **the caller's next action is different for each**, and collapsing
1812/// them into one `None` was measured to mislead. The same crate already learned
1813/// this for the conjugate gradient, where `pᵀKp ≤ 0` was reported uniformly as
1814/// "under-constrained" until it was split into a null space (add constraints) and
1815/// a rounding floor (revisit the preconditioner or the tolerance). During this type's own
1816/// development a budget that was merely too small was read as a degeneracy and
1817/// cost a round of diagnosis, which is the same failure one layer up.
1818#[derive(Debug, Clone, Copy, PartialEq, Eq)]
1819#[non_exhaustive]
1820pub enum PolarError {
1821 /// `det F ≤ 0`: the deformation has turned the element inside out.
1822 ///
1823 /// There is no rotation factor — the nearest orthogonal matrix is a reflection
1824 /// with `det = −1`. **Fix the mesh or the deformation**; a larger budget or a
1825 /// lower floor will not help. Returned whatever `det_floor` was given, so a
1826 /// caller passing a negative floor still cannot receive a reflection.
1827 Inverted,
1828 /// `det F ≤ det_floor`: too close to singular for the caller's policy.
1829 ///
1830 /// **Revisit the floor, or the element that produced this gradient.**
1831 Degenerate,
1832 /// An intermediate iterate was exactly singular, so it could not be inverted.
1833 ///
1834 /// Distinct from [`Self::Degenerate`] because the *input* passed the floor:
1835 /// what failed is the path, not the data. **Lower the floor's tolerance for
1836 /// ill-conditioning, or inspect the gradient.**
1837 SingularStep {
1838 /// Steps completed before the singular iterate.
1839 steps: u32,
1840 },
1841 /// `max_iters` was exhausted while the change was still above the settling
1842 /// bound.
1843 ///
1844 /// ⚠️ **The input is fine — raise the budget.** Measured requirements run to 21
1845 /// steps for a gradient with one singular value four decades from one, so a
1846 /// budget of 16 is too small for the general case; see
1847 /// [`Mat3Fix::polar_rotation`] for the table.
1848 NotConverged {
1849 /// Steps performed, equal to the `max_iters` that was given.
1850 steps: u32,
1851 },
1852}
1853
1854/// A 256-bit two's-complement integer `(high, low)`, used by
1855/// [`Mat3Fix::inverse`] to sum products exactly.
1856type Wide = (i128, u128);
1857
1858/// `floor(a·b / 2⁶⁴)` in raw units, exact: the full product of the two Q64.64
1859/// values, of which [`Mul`] keeps the low 128 bits.
1860fn wide_mul_floor(a: Fix128, b: Fix128) -> Wide {
1861 const LOW: u128 = u64::MAX as u128;
1862 let ra = ((a.hi as i128) << 64) | a.lo as i128;
1863 let rb = ((b.hi as i128) << 64) | b.lo as i128;
1864 let (ua, ub) = (ra.unsigned_abs(), rb.unsigned_abs());
1865 let (a1, a0) = (ua >> 64, ua & LOW);
1866 let (b1, b0) = (ub >> 64, ub & LOW);
1867 // |a|, |b| ≤ 2¹²⁷, so the magnitude is below 2²⁵⁴ and `hi` cannot overflow
1868 let (mid, mid_carry) = (a0 * b1).overflowing_add(a1 * b0);
1869 let (lo, lo_carry) = (a0 * b0).overflowing_add(mid << 64);
1870 let hi = a1 * b1 + (mid >> 64) + ((mid_carry as u128) << 64) + lo_carry as u128;
1871 let p = if (ra < 0) != (rb < 0) {
1872 wide_neg((hi as i128, lo))
1873 } else {
1874 (hi as i128, lo)
1875 };
1876 // Arithmetic shift right by 64 bits: floor on the two's-complement value
1877 (p.0 >> 64, ((p.0 as u128) << 64) | (p.1 >> 64))
1878}
1879
1880fn wide_add(x: Wide, y: Wide) -> Wide {
1881 let (lo, carry) = x.1.overflowing_add(y.1);
1882 (x.0.wrapping_add(y.0).wrapping_add(carry as i128), lo)
1883}
1884
1885fn wide_neg(x: Wide) -> Wide {
1886 wide_add((!x.0, !x.1), (0, 1))
1887}
1888
1889/// The value as a [`Fix128`], or `None` when it is outside `[-2⁶³, 2⁶³)`.
1890fn wide_fit(x: Wide) -> Option<Fix128> {
1891 (x.0 == (x.1 as i128) >> 127).then(|| Fix128::from_raw((x.1 >> 64) as i64, x.1 as u64))
1892}
1893
1894/// `p·q − r·s` with the products summed exactly; `None` when the difference
1895/// does not fit. Bit-identical to `p * q - r * s` whenever it fits.
1896fn wide_cofactor(p: Fix128, q: Fix128, r: Fix128, s: Fix128) -> Option<Fix128> {
1897 wide_fit(wide_add(
1898 wide_mul_floor(p, q),
1899 wide_neg(wide_mul_floor(r, s)),
1900 ))
1901}
1902
1903impl Mat3Fix {
1904 /// Identity matrix
1905 pub const IDENTITY: Self = Self {
1906 col0: Vec3Fix::UNIT_X,
1907 col1: Vec3Fix::UNIT_Y,
1908 col2: Vec3Fix::UNIT_Z,
1909 };
1910
1911 /// Zero matrix
1912 pub const ZERO: Self = Self {
1913 col0: Vec3Fix::ZERO,
1914 col1: Vec3Fix::ZERO,
1915 col2: Vec3Fix::ZERO,
1916 };
1917
1918 /// Create from columns
1919 #[inline]
1920 #[must_use]
1921 pub const fn from_cols(col0: Vec3Fix, col1: Vec3Fix, col2: Vec3Fix) -> Self {
1922 Self { col0, col1, col2 }
1923 }
1924
1925 /// Create diagonal matrix
1926 #[inline]
1927 #[must_use]
1928 pub const fn diagonal(x: Fix128, y: Fix128, z: Fix128) -> Self {
1929 Self {
1930 col0: Vec3Fix::new(x, Fix128::ZERO, Fix128::ZERO),
1931 col1: Vec3Fix::new(Fix128::ZERO, y, Fix128::ZERO),
1932 col2: Vec3Fix::new(Fix128::ZERO, Fix128::ZERO, z),
1933 }
1934 }
1935
1936 /// Matrix-vector multiplication
1937 #[inline]
1938 #[must_use]
1939 pub fn mul_vec(self, v: Vec3Fix) -> Vec3Fix {
1940 Vec3Fix::new(
1941 self.col0.x * v.x + self.col1.x * v.y + self.col2.x * v.z,
1942 self.col0.y * v.x + self.col1.y * v.y + self.col2.y * v.z,
1943 self.col0.z * v.x + self.col1.z * v.y + self.col2.z * v.z,
1944 )
1945 }
1946
1947 /// Transpose
1948 #[inline]
1949 #[must_use]
1950 pub const fn transpose(self) -> Self {
1951 Self {
1952 col0: Vec3Fix::new(self.col0.x, self.col1.x, self.col2.x),
1953 col1: Vec3Fix::new(self.col0.y, self.col1.y, self.col2.y),
1954 col2: Vec3Fix::new(self.col0.z, self.col1.z, self.col2.z),
1955 }
1956 }
1957
1958 /// Scale all elements
1959 #[inline]
1960 #[must_use]
1961 pub fn scale(self, s: Fix128) -> Self {
1962 Self {
1963 col0: self.col0.scale(s),
1964 col1: self.col1.scale(s),
1965 col2: self.col2.scale(s),
1966 }
1967 }
1968
1969 /// Matrix-matrix multiplication (self * rhs)
1970 #[inline]
1971 #[must_use]
1972 pub fn mul_mat(self, rhs: Self) -> Self {
1973 Self {
1974 col0: self.mul_vec(rhs.col0),
1975 col1: self.mul_vec(rhs.col1),
1976 col2: self.mul_vec(rhs.col2),
1977 }
1978 }
1979
1980 /// Determinant
1981 #[inline]
1982 #[must_use]
1983 pub fn determinant(self) -> Fix128 {
1984 self.col0.x * (self.col1.y * self.col2.z - self.col1.z * self.col2.y)
1985 - self.col1.x * (self.col0.y * self.col2.z - self.col0.z * self.col2.y)
1986 + self.col2.x * (self.col0.y * self.col1.z - self.col0.z * self.col1.y)
1987 }
1988
1989 /// Inverse matrix.
1990 ///
1991 /// Returns `None` if the matrix is singular **or if the inverse cannot be
1992 /// computed inside the Q64.64 range**: a cofactor, the determinant, `1 / det`
1993 /// (`|det| ≤ 2⁻⁶³`) or one of the entries `cofactor / det` does not fit.
1994 /// Earlier versions wrapped in the out-of-range cases and returned `Some` with flipped
1995 /// signs (e.g. `diag(2²¹, 2²¹, 2²¹)`, whose determinant is `2⁶³`).
1996 ///
1997 /// The products inside each cofactor and inside the determinant are summed
1998 /// exactly (256-bit), so a single product beyond `2⁶³` that cancels out does
1999 /// not make the result `None`: in that case the wrapping arithmetic was
2000 /// already exact modulo `2¹²⁸`, and the result is bit-identical to the
2001 /// unchecked formula, as it is for every input whose intermediates fit.
2002 ///
2003 /// A matrix whose entries are all small (largest below `1/2`) and whose
2004 /// determinant is below `2⁻³²` is scaled up by a power of two first, so its
2005 /// determinant is not quantised away (`diag(1e-5, 1e-5, 1e-5)` had a relative
2006 /// error of 5e-5, and below `2⁻⁶³` the result was `None` although the inverse
2007 /// fits): `A⁻¹ = 2ᵏ (2ᵏ A)⁻¹`, both scalings exact. Every other matrix gets the
2008 /// unscaled result, bit for bit. `k` follows the largest entry, so the gain
2009 /// is for matrices whose entries are of similar size: one with entries many
2010 /// orders apart (`diag(3·2⁻⁶⁴, 2⁻³³, 3·2⁻⁶⁴)`) still has its determinant
2011 /// quantised and can be off by a large relative error (12.5 % there), where
2012 /// it used to be `None`.
2013 #[must_use]
2014 pub fn inverse(self) -> Option<Self> {
2015 let direct = self.inverse_and_det();
2016 if let Some((inv, det)) = direct {
2017 if det.abs() >= Fix128::from_raw(0, 1u64 << 32) {
2018 return Some(inv);
2019 }
2020 }
2021 let largest = [self.col0, self.col1, self.col2]
2022 .iter()
2023 .flat_map(|v| [v.x.abs(), v.y.abs(), v.z.abs()])
2024 .fold(Fix128::ZERO, |m, x| if x > m { x } else { m });
2025 let half = Fix128::from_raw(0, 1u64 << 63);
2026 if largest.is_zero() || largest >= half {
2027 return direct.map(|(inv, _)| inv);
2028 }
2029 // the shift that brings `largest` into [1/2, 1), at most 62
2030 let mut k = 0u32;
2031 let mut scaled = largest;
2032 while scaled < half && k < 62 {
2033 scaled = scaled + scaled;
2034 k += 1;
2035 }
2036 let up = Fix128::from_int(1i64 << k);
2037 let scale = |v: Vec3Fix| Vec3Fix::new(v.x * up, v.y * up, v.z * up);
2038 let (inv, _) = Self {
2039 col0: scale(self.col0),
2040 col1: scale(self.col1),
2041 col2: scale(self.col2),
2042 }
2043 .inverse_and_det()?;
2044 let back = |v: Vec3Fix| -> Option<Vec3Fix> {
2045 Some(Vec3Fix::new(
2046 v.x.checked_mul(up)?,
2047 v.y.checked_mul(up)?,
2048 v.z.checked_mul(up)?,
2049 ))
2050 };
2051 Some(Self {
2052 col0: back(inv.col0)?,
2053 col1: back(inv.col1)?,
2054 col2: back(inv.col2)?,
2055 })
2056 }
2057
2058 /// [`Self::inverse`] without the rescaling, with the determinant it used.
2059 fn inverse_and_det(self) -> Option<(Self, Fix128)> {
2060 let (a, b, c) = (self.col0, self.col1, self.col2);
2061
2062 // Cofactor matrix transposed (adjugate)
2063 let c00 = wide_cofactor(b.y, c.z, b.z, c.y)?;
2064 let c01 = wide_cofactor(a.z, c.y, a.y, c.z)?;
2065 let c02 = wide_cofactor(a.y, b.z, a.z, b.y)?;
2066
2067 let c10 = wide_cofactor(b.z, c.x, b.x, c.z)?;
2068 let c11 = wide_cofactor(a.x, c.z, a.z, c.x)?;
2069 let c12 = wide_cofactor(a.z, b.x, a.x, b.z)?;
2070
2071 let c20 = wide_cofactor(b.x, c.y, b.y, c.x)?;
2072 let c21 = wide_cofactor(a.y, c.x, a.x, c.y)?;
2073 let c22 = wide_cofactor(a.x, b.y, a.y, b.x)?;
2074
2075 // Same expansion as `determinant`, summed exactly
2076 let minor = wide_cofactor(a.y, c.z, a.z, c.y)?;
2077 let det = wide_fit(wide_add(
2078 wide_add(
2079 wide_mul_floor(a.x, c00),
2080 wide_neg(wide_mul_floor(b.x, minor)),
2081 ),
2082 wide_mul_floor(c.x, c02),
2083 ))?;
2084 if det.is_zero() {
2085 return None;
2086 }
2087 // `1 / det` is `2¹²⁸ / raw` in raw units, which reaches `2¹²⁷` for
2088 // `|raw| ≤ 2` (`|det| ≤ 2⁻⁶³`) and would wrap inside the division
2089 let det_raw = ((det.hi as i128) << 64) | det.lo as i128;
2090 if det_raw.unsigned_abs() <= 2 {
2091 return None;
2092 }
2093 let inv_det = Fix128::ONE / det;
2094 let e = |x: Fix128| wide_fit(wide_mul_floor(x, inv_det));
2095
2096 Some((
2097 Self {
2098 col0: Vec3Fix::new(e(c00)?, e(c01)?, e(c02)?),
2099 col1: Vec3Fix::new(e(c10)?, e(c11)?, e(c12)?),
2100 col2: Vec3Fix::new(e(c20)?, e(c21)?, e(c22)?),
2101 },
2102 det,
2103 ))
2104 }
2105
2106 /// Sum of two matrices, component by component.
2107 #[inline]
2108 fn add_mat(self, rhs: Self) -> Self {
2109 Self {
2110 col0: self.col0 + rhs.col0,
2111 col1: self.col1 + rhs.col1,
2112 col2: self.col2 + rhs.col2,
2113 }
2114 }
2115
2116 /// Smallest power of two at or above a **positive** `v`.
2117 ///
2118 /// Only used to pick an exact divisor for the polar iteration, which needs the
2119 /// divisor within a factor of two of `v` and nothing more, so when doubling
2120 /// the leading bit would leave the range the leading bit itself is returned
2121 /// instead of failing. `Fix128::ONE` for a non-positive `v`, which
2122 /// [`Self::polar_rotation`] has already rejected before it gets here.
2123 fn ceil_power_of_two(v: Fix128) -> Fix128 {
2124 if v <= Fix128::ZERO {
2125 return Fix128::ONE;
2126 }
2127 let raw = ((v.hi as u64 as u128) << 64) | v.lo as u128;
2128 let bits = 128 - raw.leading_zeros();
2129 let floor_pow = 1u128 << (bits - 1);
2130 let pow = if raw == floor_pow || bits >= 127 {
2131 floor_pow
2132 } else {
2133 floor_pow << 1
2134 };
2135 Fix128 {
2136 hi: (pow >> 64) as i64,
2137 lo: pow as u64,
2138 }
2139 }
2140
2141 /// Largest absolute component, as a scale for this matrix.
2142 ///
2143 /// Useful for turning an intrinsic, scale-free criterion into an absolute one
2144 /// at the call site: a determinant scales as the **cube** of the matrix, so
2145 /// `‖F‖³` is the quantity a determinant floor has to be expressed against.
2146 /// See [`Self::polar_rotation`].
2147 #[must_use]
2148 pub fn max_abs_component(self) -> Fix128 {
2149 let mut worst = Fix128::ZERO;
2150 for c in [self.col0, self.col1, self.col2] {
2151 for v in [c.x, c.y, c.z] {
2152 let a = v.abs();
2153 if a > worst {
2154 worst = a;
2155 }
2156 }
2157 }
2158 worst
2159 }
2160
2161 /// Rotation factor `R` of the polar decomposition `F = R·U`, where `U` is
2162 /// symmetric positive definite.
2163 ///
2164 /// This is the piece a co-rotational formulation needs: the rotation a
2165 /// deformation gradient contains, separated from the stretch, so an element's
2166 /// small-strain stiffness can be rotated into the deformed frame instead of
2167 /// reading a large rotation as a strain.
2168 ///
2169 /// # Method
2170 ///
2171 /// Higham's Newton iteration for the orthogonal polar factor,
2172 ///
2173 /// ```text
2174 /// R₀ = F, R_{k+1} = ½ (R_k + R_k⁻ᵀ)
2175 /// ```
2176 ///
2177 /// which needs only [`Self::inverse`], [`Self::transpose`], addition and a
2178 /// halving — no square root, no trigonometry, no eigensolver. Every operation
2179 /// is one [`Fix128`] performs exactly, so two runs on different targets
2180 /// produce bit-identical rotations and nothing here can reach a platform
2181 /// `libm`. Nothing allocates, so this is available in `no_std` builds.
2182 ///
2183 /// # Convergence, measured
2184 ///
2185 /// Quadratic near the answer — the number of correct bits doubles per step —
2186 /// and linear while a singular value is far from one, where the iteration
2187 /// halves the distance per step. Measured change per step for a general
2188 /// gradient:
2189 ///
2190 /// ```text
2191 /// 4.17e-1 → 8.01e-2 → 3.20e-3 → 5.12e-6 → 1.31e-11 → 0
2192 /// ```
2193 ///
2194 /// Measured step counts to the settling bound
2195 /// (`tests/analytic_polar_decomposition.rs` prints the first four; the rest were
2196 /// measured by raising `max_iters` until the call succeeded):
2197 ///
2198 /// | `F` | steps |
2199 /// |---|---|
2200 /// | simple shear `γ = 0.4` | 6 |
2201 /// | `diag(1.5, 0.8, 1.1)` | 7 |
2202 /// | rotation ∘ `diag(1.3, 0.9, 1.05)` | 7 |
2203 /// | a general full matrix | 7 |
2204 /// | `diag(1, 1, 0.1)` | 9 |
2205 /// | `diag(1e3, 1e3, 1e2)` | 9 |
2206 /// | `diag(4, 1, 0.25)` | 10 |
2207 /// | `diag(1e-3, 1e-3, 1e-4)` | 10 |
2208 /// | `diag(1, 1, 0.01)` | 13 |
2209 /// | `diag(1, 1, 1e-4)` | 19 |
2210 ///
2211 /// So **`max_iters = 32` covers everything measured with margin, and 16 is not
2212 /// enough** — the flattest gradient the intrinsic floor still accepts needs 19.
2213 ///
2214 /// The two uniformly scaled rows are the point of the normalisation below:
2215 /// `diag(1e3, 1e3, 1e2)` and `diag(1e-3, 1e-3, 1e-4)` are the same shape six
2216 /// decades apart and both finish in 9 or 10 steps, so **the budget a caller has
2217 /// to pass does not depend on the units its field is expressed in**. What the
2218 /// count does depend on is the *spread*: `diag(1, 1, 1e-4)` is slow because one
2219 /// singular value is four decades from the others, and no uniform scaling can
2220 /// fix that.
2221 ///
2222 /// An exactly representable rotation is returned *unchanged*: its largest
2223 /// component lies in `[1/√3, 1]` so the divisor is exactly one, and for
2224 /// orthogonal `F`, `F⁻ᵀ = F`, so the first step is `½(F + F)` with no
2225 /// rounding.
2226 ///
2227 /// # Why the iteration stops on a change of a few bits, not on no change
2228 ///
2229 /// ⚠️ **There is in general no matrix this iteration leaves unchanged.**
2230 /// [`Fix128`] multiplication **truncates** rather than rounding to nearest, so
2231 /// the rounded map carries a systematic bias of a fraction of a unit in the
2232 /// last place per step, and the iterate keeps creeping after it has converged
2233 /// to everything the representation can hold. Measured on a rotation about x
2234 /// built from `3/5` and `4/5` (whose determinant is `1 + 819·2⁻⁶⁴`, not
2235 /// exactly one), tracked for 24 steps:
2236 ///
2237 /// ```text
2238 /// step 1: col1.y lo=…730314 col1.z lo=…641784
2239 /// step 4: col1.y lo=…730312 col1.z lo=…641785
2240 /// step 9: col1.y lo=…730310 col1.z lo=…641786
2241 /// step 14: col1.y lo=…730309 col1.z lo=…641787 ← no fixed point, no cycle
2242 /// ```
2243 ///
2244 /// So `next == r` is not a reachable stopping condition, and neither is a
2245 /// period-two cycle: both were tried and both burned the whole budget, which
2246 /// the caller then received as a failure to decompose. The drift is **1 unit
2247 /// in the last place per one or two steps**; the iteration stops at a change of
2248 /// **4**, which is above the drift with margin and far below anything a caller
2249 /// of a rotation matrix can observe.
2250 ///
2251 /// Rounding to nearest would not remove the need for this rule — it would make
2252 /// the bias unbiased rather than systematic, and the map would still be a
2253 /// discretisation with no guaranteed fixed point.
2254 ///
2255 /// The bound is scale free because the iterate is normalised: `‖r‖ ≈ 1`, so
2256 /// "four units in the last place" is a relative quantity, not the absolute
2257 /// threshold that always breaks at some other problem size.
2258 ///
2259 /// # Why `det_floor` is a parameter and not a constant
2260 ///
2261 /// A matrix does not know the scale of the field it came from, so a floor
2262 /// written *inside* this function would be an absolute threshold — the shape
2263 /// that always breaks at some problem size. The policy belongs to the caller:
2264 /// a finite-element assembly derives it from the element volume, and a caller
2265 /// that only wants the intrinsic numerical criterion passes
2266 ///
2267 /// ```text
2268 /// det_floor = ε · ‖F‖³ (Self::max_abs_component gives ‖F‖)
2269 /// ```
2270 ///
2271 /// which is scale free, because both sides scale as the cube of the matrix.
2272 ///
2273 /// # Returns `None`
2274 ///
2275 /// - When `det F ≤ 0`, **whatever `det_floor` is given**. A non-positive
2276 /// determinant means the deformation has turned the element inside out, and
2277 /// the nearest orthogonal matrix is then a *reflection* with `det = −1`.
2278 /// Returning it would silently flip the sign of every stress component, so
2279 /// this rejection is not delegated to the caller's floor: a caller that
2280 /// passes a negative `det_floor` still cannot get a reflection back.
2281 /// - When `det F ≤ det_floor`.
2282 /// - When `max_iters` is exhausted, or an intermediate matrix is exactly
2283 /// singular.
2284 ///
2285 /// # Errors
2286 ///
2287 /// One of the four [`PolarError`] variants, which are kept separate because the
2288 /// caller's next action differs for each: [`PolarError::Inverted`] and
2289 /// [`PolarError::Degenerate`] are about the data, [`PolarError::NotConverged`]
2290 /// is about the budget, and [`PolarError::SingularStep`] is about the path.
2291 pub fn polar_rotation(self, det_floor: Fix128, max_iters: u32) -> Result<Self, PolarError> {
2292 self.polar_rotation_steps(det_floor, max_iters)
2293 .map(|(r, _)| r)
2294 }
2295
2296 /// [`Self::polar_rotation`], also reporting how many iterations it took.
2297 ///
2298 /// Crate-internal so the step count can be pinned by a test without becoming
2299 /// part of the published surface.
2300 pub(crate) fn polar_rotation_steps(
2301 self,
2302 det_floor: Fix128,
2303 max_iters: u32,
2304 ) -> Result<(Self, u32), PolarError> {
2305 let det = self.determinant();
2306 // Unconditional, and checked before the caller's floor: a reflection must
2307 // not be reachable through `det_floor`, however that floor was chosen.
2308 if det <= Fix128::ZERO {
2309 return Err(PolarError::Inverted);
2310 }
2311 if det <= det_floor {
2312 return Err(PolarError::Degenerate);
2313 }
2314
2315 // Normalise by a **power of two** at or above ‖F‖ before iterating.
2316 //
2317 // The polar factor is invariant under a positive scaling (`F/s = R·(U/s)`
2318 // and `U/s` is still symmetric positive definite), but the *iteration
2319 // count* is not: Higham's iteration is linear while the singular values
2320 // are far from one, halving the largest per step. Measured, `diag(1, 1, 0.1)`
2321 // needs 9 steps and the six-decades-smaller `diag(1e-3, 1e-3, 1e-4)` needs
2322 // 10 — without normalisation the second did not converge in 16 at all, so a
2323 // caller's budget would have depended on the units its field is expressed
2324 // in.
2325 //
2326 // Dividing by ‖F‖ itself would fix the step count and **break
2327 // idempotence**: the iteration settles on a fixed point of the rounded
2328 // map, which for `diag(1.5, 0.8, 1.1)` is `diag(1 − 2⁻⁶⁴, …)`, and
2329 // dividing *that* by its own largest component lands on the neighbouring
2330 // fixed point `I`. Rounding the divisor up to a power of two avoids it:
2331 // every column of an orthogonal matrix is a unit vector, so its largest
2332 // component is at least `1/√3 ≈ 0.577` and at most `1`, which rounds up to
2333 // exactly one — a rotation is therefore never rescaled, and feeding one
2334 // back in returns it bit for bit.
2335 // `det > 0` already rules out a zero matrix, so the scale is positive.
2336 let divisor = Self::ceil_power_of_two(self.max_abs_component());
2337 let mut r = if divisor == Fix128::ONE {
2338 self
2339 } else {
2340 Self {
2341 col0: self.col0 / divisor,
2342 col1: self.col1 / divisor,
2343 col2: self.col2 / divisor,
2344 }
2345 };
2346 /// Change at which the iteration is considered settled, in units of
2347 /// `2⁻⁶⁴`. The measured drift of the truncating map is one per one or two
2348 /// steps, so four is above it with margin; the iterate is normalised to
2349 /// `‖r‖ ≈ 1`, which is what makes a bit count a relative quantity.
2350 const SETTLED: Fix128 = Fix128 { hi: 0, lo: 4 };
2351
2352 let half = Fix128 { hi: 0, lo: 1 << 63 }; // exactly 0.5
2353 let mut steps = 0_u32;
2354 while steps < max_iters {
2355 let inverse = match r.inverse() {
2356 Some(i) => i,
2357 None => return Err(PolarError::SingularStep { steps }),
2358 };
2359 let next = r.add_mat(inverse.transpose()).scale(half);
2360 steps += 1;
2361 let change = next.add_mat(r.scale(Fix128::NEG_ONE)).max_abs_component();
2362 r = next;
2363 if change <= SETTLED {
2364 return Ok((r, steps));
2365 }
2366 }
2367 Err(PolarError::NotConverged { steps })
2368 }
2369}
2370
2371// ============================================================================
2372// Runtime SIMD Width Detection
2373// ============================================================================
2374
2375/// Returns the SIMD width for the current build target.
2376///
2377/// Dispatches at compile time based on enabled features and target architecture:
2378/// - AVX2 (`x86_64)`: 8 lanes (256-bit / 32-bit float)
2379/// - SSE2 / no-AVX2 (`x86_64` without avx2): 4 lanes (128-bit)
2380/// - NEON (aarch64): 4 lanes (128-bit)
2381/// - Other targets with `simd` (wasm32, ...): 1, no SIMD path, the scalar code runs
2382/// - Scalar fallback (no `simd` feature): 1
2383///
2384/// Use the [`SIMD_WIDTH`] constant for a zero-cost compile-time value.
2385#[inline(always)]
2386#[must_use]
2387pub const fn simd_width() -> usize {
2388 #[cfg(all(feature = "simd", target_arch = "x86_64", target_feature = "avx2"))]
2389 {
2390 8
2391 }
2392 #[cfg(all(feature = "simd", target_arch = "x86_64", not(target_feature = "avx2")))]
2393 {
2394 4
2395 }
2396 #[cfg(all(feature = "simd", target_arch = "aarch64"))]
2397 {
2398 4
2399 }
2400 // no SIMD path for other targets (wasm32, ...): the scalar code runs
2401 #[cfg(all(
2402 feature = "simd",
2403 not(any(target_arch = "x86_64", target_arch = "aarch64"))
2404 ))]
2405 {
2406 1
2407 }
2408 #[cfg(not(feature = "simd"))]
2409 {
2410 1
2411 }
2412}
2413
2414/// Compile-time SIMD lane width for the current build target.
2415///
2416/// Equals `simd_width()`. Use this for array sizes and loop-unroll factors.
2417pub const SIMD_WIDTH: usize = simd_width();
2418
2419// ============================================================================
2420// Branchless Helpers
2421// ============================================================================
2422
2423/// Branchless select: returns `a` if `condition` is true, `b` otherwise.
2424///
2425/// Uses a bitwise mask approach identical to CMOV — no branch, no pipeline flush.
2426/// The result is bit-exact identical to `if condition { a } else { b }`.
2427///
2428/// # Safety
2429///
2430/// This is always safe. The mask is constructed from `-(condition as i64)`,
2431/// which is `0xFFFFFFFF_FFFFFFFF` when true and `0x00000000_00000000` when false.
2432/// Bitwise AND/OR then selects the correct limbs without any conditional instruction.
2433#[inline(always)]
2434#[must_use]
2435pub(crate) const fn select_fix128(condition: bool, a: Fix128, b: Fix128) -> Fix128 {
2436 // mask = 0xFFFF...FFFF when condition is true, 0x0000...0000 when false
2437 let mask = -(condition as i64) as u64;
2438 let inv_mask = !mask;
2439
2440 // Select hi (signed i64): cast to u64 for bitwise ops, cast back
2441 let hi = ((a.hi as u64 & mask) | (b.hi as u64 & inv_mask)) as i64;
2442 // Select lo (unsigned u64): direct bitwise ops
2443 let lo = (a.lo & mask) | (b.lo & inv_mask);
2444
2445 Fix128 { hi, lo }
2446}
2447
2448/// Branchless select for `Vec3Fix`.
2449///
2450/// Returns `a` if `condition` is true, `b` otherwise.
2451/// Applies `select_fix128` component-wise.
2452#[inline(always)]
2453#[must_use]
2454pub(crate) const fn select_vec3(condition: bool, a: Vec3Fix, b: Vec3Fix) -> Vec3Fix {
2455 Vec3Fix {
2456 x: select_fix128(condition, a.x, b.x),
2457 y: select_fix128(condition, a.y, b.y),
2458 z: select_fix128(condition, a.z, b.z),
2459 }
2460}
2461
2462impl core::ops::Mul<Self> for Mat3Fix {
2463 type Output = Self;
2464
2465 fn mul(self, rhs: Self) -> Self {
2466 Self {
2467 col0: self.mul_vec(rhs.col0),
2468 col1: self.mul_vec(rhs.col1),
2469 col2: self.mul_vec(rhs.col2),
2470 }
2471 }
2472}
2473
2474#[cfg(test)]
2475mod tests {
2476 use super::*;
2477 #[cfg(not(feature = "std"))]
2478 use alloc::{format, vec::Vec};
2479
2480 /// `rescaled_direction`: untouched inside `[2^-24, 2^24]`, an exact
2481 /// power-of-two scaling to a largest component in `[1, 2)` outside it.
2482 #[test]
2483 fn rescaled_direction_keeps_the_window_and_scales_exactly_outside_it() {
2484 let v = |x: i64, y: i64, z: i64| {
2485 Vec3Fix::new(
2486 Fix128::from_int(x),
2487 Fix128::from_int(y),
2488 Fix128::from_int(z),
2489 )
2490 };
2491 // inside the window: bit for bit the same, at both ends and in between
2492 for d in [
2493 v(3, -4, 5),
2494 Vec3Fix::new(Fix128::from_raw(0, 1 << 40), Fix128::ZERO, Fix128::ZERO), // 2^-24
2495 Vec3Fix::new(Fix128::from_raw(1 << 24, 0), Fix128::ZERO, Fix128::ZERO), // 2^24
2496 Vec3Fix::new(Fix128::from_ratio(1, 1_000_000), Fix128::ZERO, Fix128::ZERO),
2497 ] {
2498 assert_eq!(d.rescaled_direction(), d);
2499 }
2500 assert_eq!(Vec3Fix::ZERO.rescaled_direction(), Vec3Fix::ZERO);
2501 // tiny: (3, -4, 1) * 2^-40 -> (3, -4, 1) / 4, exact (largest 1.0 in [1, 2))
2502 let tiny = Vec3Fix::new(
2503 Fix128::from_raw(0, 3 << 24),
2504 -Fix128::from_raw(0, 4 << 24),
2505 Fix128::from_raw(0, 1 << 24),
2506 );
2507 assert_eq!(
2508 tiny.rescaled_direction(),
2509 Vec3Fix::new(
2510 Fix128::from_ratio(3, 4),
2511 -Fix128::ONE,
2512 Fix128::from_ratio(1, 4)
2513 )
2514 );
2515 // huge: (2^40, 2^39, 0) -> (1, 1/2, 0), exact
2516 let huge = Vec3Fix::new(
2517 Fix128::from_raw(1 << 40, 0),
2518 Fix128::from_raw(1 << 39, 0),
2519 Fix128::ZERO,
2520 );
2521 assert_eq!(
2522 huge.rescaled_direction(),
2523 Vec3Fix::new(Fix128::ONE, Fix128::from_ratio(1, 2), Fix128::ZERO)
2524 );
2525 }
2526
2527 /// `unit_rotation`: a unit quaternion (as produced by `from_axis_angle`,
2528 /// not exactly of length one) is kept bit for bit, a non-unit one is
2529 /// normalized, the zero quaternion is the identity.
2530 #[test]
2531 fn unit_rotation_keeps_unit_inputs_and_normalizes_the_rest() {
2532 let q = QuatFix::from_axis_angle(
2533 Vec3Fix::new(Fix128::ONE, Fix128::from_int(2), Fix128::from_int(3)),
2534 Fix128::from_ratio(7, 10),
2535 );
2536 assert_eq!(q.unit_rotation(), q);
2537 assert_eq!(QuatFix::IDENTITY.unit_rotation(), QuatFix::IDENTITY);
2538 let two = QuatFix::new(
2539 Fix128::ZERO,
2540 Fix128::ZERO,
2541 Fix128::ZERO,
2542 Fix128::from_int(2),
2543 );
2544 assert_eq!(two.unit_rotation(), QuatFix::IDENTITY);
2545 let zero = QuatFix::new(Fix128::ZERO, Fix128::ZERO, Fix128::ZERO, Fix128::ZERO);
2546 assert_eq!(zero.unit_rotation(), QuatFix::IDENTITY);
2547 }
2548
2549 #[test]
2550 #[allow(clippy::disallowed_methods)] // f64::powf is the oracle
2551 fn powf_pos_matches_f64_reference() {
2552 let mut worst = 0.0f64;
2553 for bi in 0..40 {
2554 let b = 1e-3 * 1e6f64.powf(bi as f64 / 39.0); // 1e-3 .. 1e3
2555 for ei in 0..=32 {
2556 let e = 8.0 * ei as f64 / 32.0;
2557 let got = Fix128::from_f64(b).powf_pos(Fix128::from_f64(e)).to_f64();
2558 let want = b.powf(e);
2559 if want < 1e-9 || want > 1e15 {
2560 continue; // Fix128 has 2⁻⁶⁴ absolute resolution: below 1e-9 the
2561 // relative error is dominated by truncation
2562 }
2563 let rel = ((got - want) / want).abs();
2564 worst = worst.max(rel);
2565 assert!(rel < 1e-6, "{b}^{e} = {got} vs {want} (rel {rel:e})");
2566 }
2567 }
2568 assert!(worst > 0.0, "sweep ran");
2569 // exact cases
2570 assert_eq!(
2571 Fix128::from_int(2).powf_pos(Fix128::from_int(10)),
2572 Fix128::from_int(1024)
2573 );
2574 assert_eq!(
2575 Fix128::from_int(9).powf_pos(Fix128::from_ratio(1, 2)),
2576 Fix128::from_int(3)
2577 );
2578 assert_eq!(Fix128::from_int(5).powf_pos(Fix128::ZERO), Fix128::ONE);
2579 assert_eq!(Fix128::ZERO.powf_pos(Fix128::ONE), Fix128::ZERO);
2580 assert_eq!(Fix128::from_int(-2).powf_pos(Fix128::ONE), Fix128::ZERO);
2581 assert_eq!(
2582 Fix128::from_int(2).powf_pos(Fix128::from_int(-1)),
2583 Fix128::ZERO
2584 );
2585 }
2586
2587 #[test]
2588 #[allow(clippy::disallowed_methods)] // f64::exp is the oracle
2589 fn exp_matches_f64_reference() {
2590 for i in 0..=400 {
2591 let x = -40.0 + 80.0 * f64::from(i) / 400.0;
2592 let got = Fix128::from_f64(x).exp().to_f64();
2593 let want = x.exp();
2594 if want < 1e-9 {
2595 continue; // below the 2⁻⁶⁴ resolution floor in relative terms
2596 }
2597 let rel = ((got - want) / want).abs();
2598 assert!(rel < 2e-6, "exp({x}) = {got} vs {want} (rel {rel:e})");
2599 }
2600 assert_eq!(Fix128::ZERO.exp(), Fix128::ONE);
2601 assert!(Fix128::from_int(-50).exp().is_zero());
2602 assert_eq!(
2603 Fix128::from_int(50).exp(),
2604 Fix128::from_raw(i64::MAX, u64::MAX)
2605 );
2606 // exp(1) = e to 1e-6
2607 assert!((Fix128::ONE.exp().to_f64() - core::f64::consts::E).abs() < 3e-6);
2608 }
2609
2610 #[test]
2611 fn div_by_zero_is_zero_and_checked_div_is_none() {
2612 let x = Fix128::from_int(7);
2613 assert_eq!(x / Fix128::ZERO, Fix128::ZERO);
2614 assert_eq!(Fix128::ZERO / Fix128::ZERO, Fix128::ZERO);
2615 assert_eq!(x.checked_div(Fix128::ZERO), None);
2616 assert_eq!(
2617 x.checked_div(Fix128::from_int(2)),
2618 Some(Fix128::from_ratio(7, 2))
2619 );
2620 assert_eq!(
2621 Fix128::from_ratio(-1, 3).checked_div(Fix128::from_ratio(2, 7)),
2622 Some(Fix128::from_ratio(-1, 3) / Fix128::from_ratio(2, 7))
2623 );
2624 }
2625
2626 #[test]
2627 fn fix128_mul_wraps_on_integer_overflow() {
2628 // 2^62 * 4 = 2^64 → integer part wraps modulo 2^64 → 0.
2629 let a = Fix128::from_int(1 << 62);
2630 let b = Fix128::from_int(4);
2631 assert_eq!(a * b, Fix128::ZERO);
2632 // 2^62 * 3 = 3·2^62 = 2^63 + 2^62 → wraps to -2^63 + 2^62 = -2^62.
2633 assert_eq!(a * Fix128::from_int(3), Fix128::from_int(-(1 << 62)));
2634 }
2635
2636 #[test]
2637 fn fix128_mul_truncates_toward_negative_infinity() {
2638 // -2^-64 is the raw pattern {hi: -1, lo: u64::MAX}.
2639 let tiny_neg = Fix128::from_raw(-1, u64::MAX);
2640 let half = Fix128::from_ratio(1, 2);
2641 // Exact value -2^-65 is not representable; floor gives -2^-64.
2642 assert_eq!(tiny_neg * half, tiny_neg);
2643 // Positive counterpart truncates to 0.
2644 let tiny_pos = Fix128::from_raw(0, 1);
2645 assert_eq!(tiny_pos * half, Fix128::ZERO);
2646 }
2647
2648 #[test]
2649 fn test_fix128_basic_ops() {
2650 let a = Fix128::from_int(5);
2651 let b = Fix128::from_int(3);
2652
2653 let sum = a + b;
2654 assert_eq!(sum.hi, 8);
2655 assert_eq!(sum.lo, 0);
2656
2657 let diff = a - b;
2658 assert_eq!(diff.hi, 2);
2659 assert_eq!(diff.lo, 0);
2660 }
2661
2662 #[test]
2663 fn test_fix128_mul() {
2664 let a = Fix128::from_int(6);
2665 let b = Fix128::from_int(7);
2666 let product = a * b;
2667 assert_eq!(product.hi, 42);
2668 }
2669
2670 #[test]
2671 fn test_fix128_div() {
2672 let a = Fix128::from_int(42);
2673 let b = Fix128::from_int(6);
2674 let quot = a / b;
2675 assert_eq!(quot.hi, 7);
2676 }
2677
2678 #[test]
2679 fn test_fix128_neg() {
2680 let a = Fix128::from_int(5);
2681 let neg_a = -a;
2682 assert_eq!(neg_a.hi, -5);
2683
2684 let sum = a + neg_a;
2685 assert!(sum.is_zero());
2686 }
2687
2688 #[test]
2689 fn test_vec3_dot() {
2690 let a = Vec3Fix::from_int(1, 2, 3);
2691 let b = Vec3Fix::from_int(4, 5, 6);
2692 let dot = a.dot(b);
2693 // 1*4 + 2*5 + 3*6 = 4 + 10 + 18 = 32
2694 assert_eq!(dot.hi, 32);
2695 }
2696
2697 #[test]
2698 fn test_vec3_cross() {
2699 let a = Vec3Fix::UNIT_X;
2700 let b = Vec3Fix::UNIT_Y;
2701 let cross = a.cross(b);
2702 // X × Y = Z
2703 assert_eq!(cross.z.hi, 1);
2704 assert!(cross.x.is_zero());
2705 assert!(cross.y.is_zero());
2706 }
2707
2708 #[test]
2709 fn test_quat_identity() {
2710 let q = QuatFix::IDENTITY;
2711 let v = Vec3Fix::from_int(1, 2, 3);
2712 let rotated = q.rotate_vec(v);
2713
2714 assert_eq!(rotated.x.hi, 1);
2715 assert_eq!(rotated.y.hi, 2);
2716 assert_eq!(rotated.z.hi, 3);
2717 }
2718
2719 #[test]
2720 fn test_determinism() {
2721 // The same operations should always produce the same results
2722 let a = Fix128::from_raw(12345, 0xABCDEF0123456789);
2723 let b = Fix128::from_raw(67890, 0x9876543210FEDCBA);
2724
2725 let result1 = (a * b) + (a - b);
2726 let result2 = (a * b) + (a - b);
2727
2728 assert_eq!(result1.hi, result2.hi);
2729 assert_eq!(result1.lo, result2.lo);
2730 }
2731
2732 #[test]
2733 fn test_fix128_f32_roundtrip() {
2734 let a = Fix128::from_f32(core::f32::consts::PI);
2735 let back = a.to_f32();
2736 assert!((back - core::f32::consts::PI).abs() < 0.001);
2737
2738 let b = Fix128::from_f32(-7.5);
2739 let back_b = b.to_f32();
2740 assert!((back_b - (-7.5)).abs() < 0.001);
2741 }
2742
2743 #[test]
2744 fn test_vec3fix_f32_roundtrip() {
2745 let v = Vec3Fix::from_f32(1.0, -2.5, 3.75);
2746 let (x, y, z) = v.to_f32();
2747 assert!((x - 1.0).abs() < 0.001);
2748 assert!((y - (-2.5)).abs() < 0.001);
2749 assert!((z - 3.75).abs() < 0.001);
2750 }
2751
2752 // -----------------------------------------------------------------------
2753 // dot_simd / length_squared_simd bit-exactness tests
2754 // -----------------------------------------------------------------------
2755
2756 /// Verify dot_simd produces a bit-exact identical result to dot() for
2757 /// simple integer vectors.
2758 #[test]
2759 fn test_dot_simd_integer_vectors_bit_exact() {
2760 let a = Vec3Fix::from_int(1, 2, 3);
2761 let b = Vec3Fix::from_int(4, 5, 6);
2762 let scalar = a.dot(b);
2763 let simd = a.dot_simd(b);
2764 // 1*4 + 2*5 + 3*6 = 32
2765 assert_eq!(scalar.hi, 32, "scalar dot hi mismatch");
2766 assert_eq!(scalar.lo, 0, "scalar dot lo mismatch");
2767 assert_eq!(simd.hi, scalar.hi, "dot_simd hi != scalar hi");
2768 assert_eq!(simd.lo, scalar.lo, "dot_simd lo != scalar lo");
2769 }
2770
2771 /// Verify dot_simd with arbitrary fractional Fix128 values.
2772 #[test]
2773 fn test_dot_simd_fractional_bit_exact() {
2774 let ax = Fix128::from_raw(3, 0xABCD_EF01_2345_6789);
2775 let ay = Fix128::from_raw(-1, 0x1111_2222_3333_4444);
2776 let az = Fix128::from_raw(7, 0xFEDC_BA98_7654_3210);
2777
2778 let bx = Fix128::from_raw(2, 0x9876_5432_10FE_DCBA);
2779 let by = Fix128::from_raw(5, 0xAAAA_BBBB_CCCC_DDDD);
2780 let bz = Fix128::from_raw(-3, 0x0F0F_0F0F_0F0F_0F0F);
2781
2782 let a = Vec3Fix::new(ax, ay, az);
2783 let b = Vec3Fix::new(bx, by, bz);
2784
2785 let scalar = a.dot(b);
2786 let simd = a.dot_simd(b);
2787
2788 assert_eq!(
2789 simd.hi, scalar.hi,
2790 "dot_simd hi={:#018x} != scalar hi={:#018x}",
2791 simd.hi, scalar.hi
2792 );
2793 assert_eq!(
2794 simd.lo, scalar.lo,
2795 "dot_simd lo={:#018x} != scalar lo={:#018x}",
2796 simd.lo, scalar.lo
2797 );
2798 }
2799
2800 /// Verify dot_simd with negative components.
2801 #[test]
2802 fn test_dot_simd_negative_components_bit_exact() {
2803 let a = Vec3Fix::from_int(-3, 4, -5);
2804 let b = Vec3Fix::from_int(6, -7, 8);
2805 // -3*6 + 4*(-7) + (-5)*8 = -18 - 28 - 40 = -86
2806 let scalar = a.dot(b);
2807 let simd = a.dot_simd(b);
2808 assert_eq!(scalar.hi, -86, "scalar dot should be -86");
2809 assert_eq!(simd.hi, scalar.hi, "dot_simd hi mismatch on negatives");
2810 assert_eq!(simd.lo, scalar.lo, "dot_simd lo mismatch on negatives");
2811 }
2812
2813 /// Verify dot_simd of a zero vector is zero.
2814 #[test]
2815 fn test_dot_simd_zero_vector_bit_exact() {
2816 let a = Vec3Fix::ZERO;
2817 let b = Vec3Fix::from_int(100, 200, 300);
2818 let scalar = a.dot(b);
2819 let simd = a.dot_simd(b);
2820 assert!(scalar.is_zero(), "scalar dot with zero should be zero");
2821 assert_eq!(simd.hi, scalar.hi, "dot_simd hi mismatch (zero vector)");
2822 assert_eq!(simd.lo, scalar.lo, "dot_simd lo mismatch (zero vector)");
2823 }
2824
2825 /// Verify dot_simd self-dot (a . a) for unit vectors.
2826 #[test]
2827 fn test_dot_simd_unit_vectors_self_dot() {
2828 for unit in [Vec3Fix::UNIT_X, Vec3Fix::UNIT_Y, Vec3Fix::UNIT_Z] {
2829 let scalar = unit.dot(unit);
2830 let simd = unit.dot_simd(unit);
2831 assert_eq!(scalar.hi, 1, "unit self-dot should be 1");
2832 assert_eq!(scalar.lo, 0, "unit self-dot lo should be 0");
2833 assert_eq!(simd.hi, scalar.hi, "dot_simd hi mismatch on unit self-dot");
2834 assert_eq!(simd.lo, scalar.lo, "dot_simd lo mismatch on unit self-dot");
2835 }
2836 }
2837
2838 /// Verify length_squared_simd is bit-exact with length_squared.
2839 #[test]
2840 fn test_length_squared_simd_bit_exact() {
2841 let v = Vec3Fix::from_int(3, 4, 0);
2842 // 3^2 + 4^2 + 0^2 = 25
2843 let scalar = v.length_squared();
2844 let simd = v.length_squared_simd();
2845 assert_eq!(scalar.hi, 25, "length_squared should be 25");
2846 assert_eq!(simd.hi, scalar.hi, "length_squared_simd hi mismatch");
2847 assert_eq!(simd.lo, scalar.lo, "length_squared_simd lo mismatch");
2848 }
2849
2850 /// Verify length_squared_simd with fractional components.
2851 #[test]
2852 fn test_length_squared_simd_fractional_bit_exact() {
2853 let v = Vec3Fix::new(
2854 Fix128::from_raw(1, 0x8000_0000_0000_0000), // 1.5
2855 Fix128::from_raw(2, 0x0000_0000_0000_0000), // 2.0
2856 Fix128::from_raw(0, 0x8000_0000_0000_0000), // 0.5
2857 );
2858 let scalar = v.length_squared();
2859 let simd = v.length_squared_simd();
2860 // 1.5^2 + 2.0^2 + 0.5^2 = 2.25 + 4.0 + 0.25 = 6.5
2861 assert_eq!(
2862 simd.hi, scalar.hi,
2863 "length_squared_simd hi mismatch (fractional)"
2864 );
2865 assert_eq!(
2866 simd.lo, scalar.lo,
2867 "length_squared_simd lo mismatch (fractional)"
2868 );
2869 }
2870
2871 /// Exhaustive bit-exactness sweep: 16 pseudo-random raw Fix128 vectors.
2872 #[test]
2873 fn test_dot_simd_exhaustive_raw_sweep() {
2874 // Deterministic pseudo-random values (no RNG dependency).
2875 let raws: [(i64, u64); 8] = [
2876 (0, 0x0000_0000_0000_0001),
2877 (1, 0xFFFF_FFFF_FFFF_FFFF),
2878 (-1, 0x0000_0000_0000_0000),
2879 (42, 0x1234_5678_9ABC_DEF0),
2880 (-7, 0xDEAD_BEEF_CAFE_BABE),
2881 (100, 0xAAAA_AAAA_AAAA_AAAA),
2882 (-100, 0x5555_5555_5555_5555),
2883 (i64::MAX / 2, 0x8000_0000_0000_0000),
2884 ];
2885
2886 for &(ah, al) in &raws {
2887 for &(bh, bl) in &raws {
2888 let a = Vec3Fix::new(
2889 Fix128::from_raw(ah, al),
2890 Fix128::from_raw(bh, bl),
2891 Fix128::from_raw(ah.wrapping_add(bh), al ^ bl),
2892 );
2893 let b = Vec3Fix::new(
2894 Fix128::from_raw(bh, bl),
2895 Fix128::from_raw(ah, al),
2896 Fix128::from_raw(ah.wrapping_sub(bh), al.wrapping_add(bl)),
2897 );
2898 let scalar = a.dot(b);
2899 let simd = a.dot_simd(b);
2900 assert_eq!(
2901 simd.hi, scalar.hi,
2902 "sweep: dot_simd hi mismatch for a=({ah},{al:#x}) b=({bh},{bl:#x})"
2903 );
2904 assert_eq!(
2905 simd.lo, scalar.lo,
2906 "sweep: dot_simd lo mismatch for a=({ah},{al:#x}) b=({bh},{bl:#x})"
2907 );
2908 }
2909 }
2910 }
2911
2912 // ---- atan / atan2 (1.1.1: cordic_atan の shift 桁落ち修正) --------
2913
2914 fn close_f64(v: Fix128, expect: f64, tol: f64) -> bool {
2915 (v.to_f64() - expect).abs() <= tol
2916 }
2917
2918 #[test]
2919 fn shr_bits_is_exact_arithmetic_shift() {
2920 let one = Fix128::ONE;
2921 assert_eq!(one.shr_bits(0), one);
2922 assert_eq!(one.shr_bits(1), Fix128::from_ratio(1, 2)); // 1.1.1 以前の atan 版は 0 になっていた
2923 assert_eq!(one.shr_bits(3), Fix128::from_ratio(1, 8));
2924 assert_eq!(Fix128::from_int(-1).shr_bits(1), Fix128::from_ratio(-1, 2));
2925 assert_eq!(Fix128::from_int(-6).shr_bits(2), Fix128::from_ratio(-3, 2));
2926 let v = Fix128 {
2927 hi: 5,
2928 lo: 0x8000_0000_0000_0001,
2929 };
2930 assert_eq!(
2931 v.shr_bits(1),
2932 Fix128 {
2933 hi: 2,
2934 lo: 0xC000_0000_0000_0000
2935 }
2936 );
2937 assert_eq!(v.shr_bits(64), Fix128 { hi: 0, lo: 5 });
2938 assert_eq!(v.shr_bits(65), Fix128 { hi: 0, lo: 2 });
2939 assert_eq!(
2940 Fix128::from_int(-1).shr_bits(64),
2941 Fix128 {
2942 hi: -1,
2943 lo: u64::MAX
2944 }
2945 );
2946 assert_eq!(v.shr_bits(200), Fix128::ZERO);
2947 assert_eq!(
2948 Fix128::from_int(-1).shr_bits(200),
2949 Fix128 {
2950 hi: -1,
2951 lo: u64::MAX
2952 }
2953 );
2954 // half() と 1 bit shift は同値
2955 for x in [
2956 Fix128::PI,
2957 Fix128::from_ratio(-7, 3),
2958 Fix128::from_int(1234),
2959 ] {
2960 assert_eq!(x.shr_bits(1), x.half());
2961 }
2962 }
2963
2964 #[test]
2965 // 参照値として platform libm を使う (許容 1e-12、決定論の pin は *_golden_bit_patterns 側)
2966 #[allow(clippy::disallowed_methods)]
2967 fn atan_matches_f64_within_1e12() {
2968 for (v, expect) in [
2969 (Fix128::ZERO, 0.0),
2970 (Fix128::ONE, core::f64::consts::FRAC_PI_4),
2971 (Fix128::from_int(-1), -core::f64::consts::FRAC_PI_4),
2972 (Fix128::from_ratio(1, 2), 0.5f64.atan()),
2973 (Fix128::from_int(3), 3f64.atan()),
2974 (Fix128::from_ratio(-1, 8), (-0.125f64).atan()),
2975 (Fix128::from_int(1000), 1000f64.atan()),
2976 ] {
2977 let got = v.atan();
2978 assert!(
2979 close_f64(got, expect, 1e-12),
2980 "atan({}) = {} vs {expect}",
2981 v.to_f64(),
2982 got.to_f64()
2983 );
2984 }
2985 }
2986
2987 #[test]
2988 fn atan_golden_bit_patterns_1_1_1() {
2989 // 修正後の実装値を pin (cross-platform bit-exact の source of truth、意図的変更時のみ更新)
2990 let cases: [(Fix128, i64, u64); 7] = [
2991 (Fix128::ZERO, -1, 18446744073709455410),
2992 (Fix128::ONE, 0, 14488038916154342774),
2993 (Fix128::from_int(-1), -1, 3958705157555404150),
2994 (Fix128::from_ratio(1, 2), 0, 8552788783625182168),
2995 (Fix128::from_int(3), 1, 4594083626069865406),
2996 (Fix128::from_ratio(-1, 8), -1, 16152799315017801038),
2997 (Fix128::from_int(1000), 1, 10510887020674212946),
2998 ];
2999 for (v, hi, lo) in cases {
3000 assert_eq!(v.atan(), Fix128 { hi, lo }, "atan({})", v.to_f64());
3001 }
3002 }
3003
3004 #[test]
3005 // 参照値として platform libm を使う (許容 1e-12、決定論の pin は *_golden_bit_patterns 側)
3006 #[allow(clippy::disallowed_methods)]
3007 fn atan2_matches_f64_in_every_quadrant_and_on_axes() {
3008 let cases: [(i64, i64); 10] = [
3009 (1, 1),
3010 (1, -1),
3011 (-1, -1),
3012 (-1, 1),
3013 (1, 0),
3014 (-1, 0),
3015 (0, 1),
3016 (0, -1),
3017 (3, 4),
3018 (-2, 5),
3019 ];
3020 for (y, x) in cases {
3021 let got = Fix128::atan2(Fix128::from_int(y), Fix128::from_int(x));
3022 let expect = (y as f64).atan2(x as f64);
3023 assert!(
3024 close_f64(got, expect, 1e-12),
3025 "atan2({y}, {x}) = {} vs {expect}",
3026 got.to_f64()
3027 );
3028 }
3029 assert_eq!(Fix128::atan2(Fix128::ZERO, Fix128::ZERO), Fix128::ZERO);
3030 }
3031
3032 #[test]
3033 fn atan2_golden_bit_patterns_1_1_1() {
3034 let cases: [(i64, i64, i64, u64); 11] = [
3035 (1, 1, 0, 14488038916154342774),
3036 (1, -1, 2, 6570628601043732041),
3037 (-1, -1, -3, 11876115472666014883),
3038 (-1, 1, -1, 3958705157555404150),
3039 (1, 0, 1, 10529333758598939753),
3040 (-1, 0, -2, 7917410315110611863),
3041 (0, 1, -1, 18446744073709455410),
3042 (0, -1, 3, 2611923443488231685),
3043 (0, 0, 0, 0),
3044 (3, 4, 0, 11870500265058138062),
3045 (-2, 5, -1, 11427640316703283098),
3046 ];
3047 for (y, x, hi, lo) in cases {
3048 assert_eq!(
3049 Fix128::atan2(Fix128::from_int(y), Fix128::from_int(x)),
3050 Fix128 { hi, lo },
3051 "atan2({y}, {x})"
3052 );
3053 }
3054 }
3055
3056 #[test]
3057 // 参照値として platform libm を使う (許容 1e-12、決定論の pin は *_golden_bit_patterns 側)
3058 #[allow(clippy::disallowed_methods)]
3059 fn atan_identities_hold() {
3060 // odd: atan(-x) == -atan(x) (CORDIC は符号対称なので bit-exact ではなく 1e-12)
3061 for v in [
3062 Fix128::from_ratio(1, 3),
3063 Fix128::from_int(2),
3064 Fix128::from_ratio(17, 5),
3065 ] {
3066 let a = v.atan();
3067 let b = (-v).atan();
3068 assert!(close_f64(a + b, 0.0, 1e-12), "odd symmetry {}", v.to_f64());
3069 // atan(x) + atan(1/x) == π/2 (x > 0)
3070 let c = (Fix128::ONE / v).atan();
3071 assert!(
3072 close_f64(a + c, core::f64::consts::FRAC_PI_2, 1e-12),
3073 "complement {}",
3074 v.to_f64()
3075 );
3076 // atan2(y, x) == atan(y/x) for x > 0
3077 let d = Fix128::atan2(v, Fix128::from_int(2));
3078 assert!(close_f64(
3079 d,
3080 (v / Fix128::from_int(2)).atan().to_f64(),
3081 1e-12
3082 ));
3083 }
3084 // sin / cos は本修正で bit 単位で不変 (代表点 pin)
3085 let (s, c) = Fix128::HALF_PI.sin_cos();
3086 assert_eq!(s, Fix128 { hi: 1, lo: 1453 });
3087 assert_eq!(
3088 c,
3089 Fix128 {
3090 hi: -1,
3091 lo: 18446744073709456430
3092 }
3093 );
3094 }
3095
3096 // ---- Mat3Fix (mutation-score tests, 2026-09-15) ---------------------
3097
3098 fn fi(n: i64) -> Fix128 {
3099 Fix128::from_int(n)
3100 }
3101
3102 fn v3i(x: i64, y: i64, z: i64) -> Vec3Fix {
3103 Vec3Fix::from_int(x, y, z)
3104 }
3105
3106 /// 列ベクトル指定の整数行列 (M = [c0 c1 c2])
3107 fn mat(c0: (i64, i64, i64), c1: (i64, i64, i64), c2: (i64, i64, i64)) -> Mat3Fix {
3108 Mat3Fix::from_cols(
3109 v3i(c0.0, c0.1, c0.2),
3110 v3i(c1.0, c1.1, c1.2),
3111 v3i(c2.0, c2.1, c2.2),
3112 )
3113 }
3114
3115 #[test]
3116 fn mat3_mul_vec_uses_every_entry_with_the_right_index() {
3117 // 9 entry 全て異なる素数 → どの entry の取り違えでも値が変わる
3118 let m = mat((2, 3, 5), (7, 11, 13), (17, 19, 23));
3119 let v = v3i(1, 10, 100);
3120 // row i = c0[i]*1 + c1[i]*10 + c2[i]*100
3121 assert_eq!(
3122 m.mul_vec(v),
3123 v3i(2 + 70 + 1700, 3 + 110 + 1900, 5 + 130 + 2300)
3124 );
3125 assert_eq!(Mat3Fix::IDENTITY.mul_vec(v), v);
3126 assert_eq!(m.mul_vec(Vec3Fix::UNIT_X), v3i(2, 3, 5));
3127 assert_eq!(m.mul_vec(Vec3Fix::UNIT_Y), v3i(7, 11, 13));
3128 assert_eq!(m.mul_vec(Vec3Fix::UNIT_Z), v3i(17, 19, 23));
3129 }
3130
3131 #[test]
3132 fn mat3_determinant_closed_form() {
3133 // det [[2,7,17],[3,11,19],[5,13,23]] (列指定) = -78
3134 let m = mat((2, 3, 5), (7, 11, 13), (17, 19, 23));
3135 assert_eq!(m.determinant(), fi(-78));
3136 assert_eq!(Mat3Fix::IDENTITY.determinant(), Fix128::ONE);
3137 assert_eq!(
3138 Mat3Fix::diagonal(fi(2), fi(3), fi(-4)).determinant(),
3139 fi(-24)
3140 );
3141 // 列が線形従属 → 0
3142 assert_eq!(
3143 mat((1, 2, 3), (2, 4, 6), (0, 1, 0)).determinant(),
3144 Fix128::ZERO
3145 );
3146 // 転置不変
3147 assert_eq!(m.transpose().determinant(), fi(-78));
3148 // 各 entry を単独で変えると det が変わる (どの項も効いている)
3149 for r in 0..3 {
3150 for c in 0..3 {
3151 let mut m2 = m;
3152 let col = match c {
3153 0 => &mut m2.col0,
3154 1 => &mut m2.col1,
3155 _ => &mut m2.col2,
3156 };
3157 match r {
3158 0 => col.x = col.x + Fix128::ONE,
3159 1 => col.y = col.y + Fix128::ONE,
3160 _ => col.z = col.z + Fix128::ONE,
3161 }
3162 assert_ne!(m2.determinant(), fi(-78), "entry ({r},{c}) は det に効く");
3163 }
3164 }
3165 }
3166
3167 #[test]
3168 fn mat3_inverse_exact_for_unimodular_matrix() {
3169 // det = 1 の整数行列 → 逆行列も整数で exact
3170 // M = [[1,2,3],[0,1,4],[5,6,0]] (行表記) → det 1、M⁻¹ = [[-24,18,5],[20,-15,-4],[-5,4,1]]
3171 let m = mat((1, 0, 5), (2, 1, 6), (3, 4, 0));
3172 assert_eq!(m.determinant(), Fix128::ONE);
3173 let inv = m.inverse().expect("det 1");
3174 assert_eq!(inv, mat((-24, 20, -5), (18, -15, 4), (5, -4, 1)));
3175 assert_eq!(m.mul_mat(inv), Mat3Fix::IDENTITY);
3176 assert_eq!(inv.mul_mat(m), Mat3Fix::IDENTITY);
3177 assert_eq!(m * inv, Mat3Fix::IDENTITY);
3178 // 各 cofactor が正しい位置にある: inv の全 9 entry が異なる値
3179 let e = [
3180 inv.col0.x, inv.col0.y, inv.col0.z, inv.col1.x, inv.col1.y, inv.col1.z, inv.col2.x,
3181 inv.col2.y, inv.col2.z,
3182 ];
3183 for i in 0..9 {
3184 for j in (i + 1)..9 {
3185 assert_ne!(e[i], e[j], "entries {i} and {j}");
3186 }
3187 }
3188 }
3189
3190 #[test]
3191 fn mat3_inverse_scales_by_reciprocal_determinant() {
3192 // det = -8 (2 の冪) → 1/det exact
3193 let m = Mat3Fix::diagonal(fi(2), fi(-2), fi(2));
3194 let inv = m.inverse().expect("det -8");
3195 assert_eq!(
3196 inv,
3197 Mat3Fix::diagonal(
3198 Fix128::from_ratio(1, 2),
3199 Fix128::from_ratio(-1, 2),
3200 Fix128::from_ratio(1, 2)
3201 )
3202 );
3203 // 一般行列 det = -78 の逆行列 × 元 = I (数 ulp 許容)
3204 let g = mat((2, 3, 5), (7, 11, 13), (17, 19, 23));
3205 let gi = g.inverse().expect("det -78");
3206 let p = g.mul_mat(gi);
3207 let close = |a: Fix128, b: Fix128| (a - b).abs() < Fix128 { hi: 0, lo: 1 << 20 };
3208 for (col, unit) in [
3209 (p.col0, Vec3Fix::UNIT_X),
3210 (p.col1, Vec3Fix::UNIT_Y),
3211 (p.col2, Vec3Fix::UNIT_Z),
3212 ] {
3213 assert!(
3214 close(col.x, unit.x) && close(col.y, unit.y) && close(col.z, unit.z),
3215 "{col:?} vs {unit:?}"
3216 );
3217 }
3218 assert!(Mat3Fix::ZERO.inverse().is_none());
3219 assert!(mat((1, 2, 3), (2, 4, 6), (0, 1, 0)).inverse().is_none());
3220 assert_eq!(Mat3Fix::IDENTITY.inverse(), Some(Mat3Fix::IDENTITY));
3221 }
3222
3223 #[test]
3224 fn mat3_scale_mul_mat_and_operator_agree() {
3225 let m = mat((2, 3, 5), (7, 11, 13), (17, 19, 23));
3226 assert_eq!(m.scale(fi(3)), mat((6, 9, 15), (21, 33, 39), (51, 57, 69)));
3227 assert_eq!(m.scale(Fix128::ZERO), Mat3Fix::ZERO);
3228 let n = mat((1, 0, 0), (0, 2, 0), (1, 1, 1));
3229 // (M N) col_j = M · (N col_j)
3230 let mn = m.mul_mat(n);
3231 assert_eq!(mn.col0, m.mul_vec(v3i(1, 0, 0)));
3232 assert_eq!(mn.col1, m.mul_vec(v3i(0, 2, 0)));
3233 assert_eq!(mn.col2, m.mul_vec(v3i(1, 1, 1)));
3234 assert_eq!(m * n, mn);
3235 assert_ne!(n.mul_mat(m), mn, "非可換");
3236 assert_eq!(m.mul_mat(Mat3Fix::IDENTITY), m);
3237 assert_eq!(Mat3Fix::IDENTITY * m, m);
3238 assert_eq!(m.transpose().transpose(), m);
3239 assert_eq!(m.transpose().col0, v3i(2, 7, 17));
3240 }
3241
3242 // ---- QuatFix -------------------------------------------------------
3243
3244 fn q(x: i64, y: i64, z: i64, w: i64) -> QuatFix {
3245 QuatFix::new(fi(x), fi(y), fi(z), fi(w))
3246 }
3247
3248 #[test]
3249 fn quat_mul_is_the_hamilton_product() {
3250 // 基底: i*j = k, j*k = i, k*i = j, i*i = -1
3251 let i = q(1, 0, 0, 0);
3252 let j = q(0, 1, 0, 0);
3253 let k = q(0, 0, 1, 0);
3254 let one = q(0, 0, 0, 1);
3255 assert_eq!(i.mul(j), k);
3256 assert_eq!(j.mul(k), i);
3257 assert_eq!(k.mul(i), j);
3258 assert_eq!(j.mul(i), q(0, 0, -1, 0));
3259 assert_eq!(i.mul(i), q(0, 0, 0, -1));
3260 assert_eq!(one.mul(i), i);
3261 assert_eq!(i.mul(one), i);
3262 // 一般: (1,2,3,4) * (5,6,7,8) = (24, 48, 48, -6)
3263 let a = q(1, 2, 3, 4);
3264 let b = q(5, 6, 7, 8);
3265 assert_eq!(a.mul(b), q(24, 48, 48, -6));
3266 assert_eq!(b.mul(a), q(32, 32, 56, -6));
3267 assert_ne!(a.mul(b), b.mul(a));
3268 }
3269
3270 #[test]
3271 fn quat_conjugate_length_and_normalize() {
3272 let a = q(1, -2, 3, -4);
3273 assert_eq!(a.conjugate(), q(-1, 2, -3, -4));
3274 assert_eq!(a.conjugate().conjugate(), a);
3275 assert_eq!(a.length_squared(), fi(30));
3276 assert_eq!(q(0, 0, 0, 2).length_squared(), fi(4));
3277 assert_eq!(q(3, 0, 0, 0).length_squared(), fi(9));
3278 // a * conj(a) = |a|² (実部のみ)
3279 assert_eq!(a.mul(a.conjugate()), q(0, 0, 0, 30));
3280 // normalize: (0,0,0,2) → (0,0,0,1)、(0,3,0,4) → (0,0.6,0,0.8) は 5 で割る
3281 assert_eq!(q(0, 0, 0, 2).normalize(), QuatFix::IDENTITY);
3282 assert_eq!(q(0, 0, -4, 0).normalize(), q(0, 0, -1, 0));
3283 let n = q(0, 3, 0, 4).normalize();
3284 assert_eq!(
3285 n,
3286 QuatFix::new(Fix128::ZERO, fi(3) / fi(5), Fix128::ZERO, fi(4) / fi(5))
3287 );
3288 assert_eq!(q(0, 0, 0, 0).normalize(), QuatFix::IDENTITY);
3289 }
3290
3291 #[test]
3292 fn quat_from_axis_angle_components() {
3293 // 軸は正規化され、(x,y,z) = axis·sin(θ/2)、w = cos(θ/2)
3294 let theta = Fix128::from_ratio(1, 2);
3295 let (s, c) = theta.half().sin_cos();
3296 let qq = QuatFix::from_axis_angle(v3i(0, 0, 5), theta);
3297 assert_eq!(qq, QuatFix::new(Fix128::ZERO, Fix128::ZERO, s, c));
3298 let qx = QuatFix::from_axis_angle(v3i(-2, 0, 0), theta);
3299 assert_eq!(qx, QuatFix::new(-s, Fix128::ZERO, Fix128::ZERO, c));
3300 // 角度 0 → identity 近傍 (CORDIC gain 込み)
3301 let q0 = QuatFix::from_axis_angle(v3i(1, 0, 0), Fix128::ZERO);
3302 assert!((q0.w - Fix128::ONE).abs() < Fix128 { hi: 0, lo: 1 << 20 });
3303 assert!(q0.x.abs() < Fix128 { hi: 0, lo: 1 << 20 });
3304 }
3305
3306 #[test]
3307 fn quat_rotate_vec_by_quarter_turns() {
3308 // z 軸 90° 回転: x → y、y → -x (数 ulp 許容)
3309 let rz = QuatFix::from_axis_angle(v3i(0, 0, 1), Fix128::HALF_PI);
3310 let close = |a: Vec3Fix, b: Vec3Fix| (a - b).length() < Fix128 { hi: 0, lo: 1 << 24 };
3311 assert!(close(rz.rotate_vec(Vec3Fix::UNIT_X), Vec3Fix::UNIT_Y));
3312 assert!(close(rz.rotate_vec(Vec3Fix::UNIT_Y), -Vec3Fix::UNIT_X));
3313 assert!(close(rz.rotate_vec(Vec3Fix::UNIT_Z), Vec3Fix::UNIT_Z));
3314 // 180°: (0,0,1,0) は exact
3315 let half = q(0, 0, 1, 0);
3316 assert_eq!(half.rotate_vec(v3i(1, 2, 3)), v3i(-1, -2, 3));
3317 assert_eq!(QuatFix::IDENTITY.rotate_vec(v3i(1, 2, 3)), v3i(1, 2, 3));
3318 }
3319
3320 // ---- Vec3Fix helpers / select / From / Display ----------------------
3321
3322 #[test]
3323 fn vec3_cross_try_normalize_and_batch_dot() {
3324 assert_eq!(v3i(1, 0, 0).cross(v3i(0, 1, 0)), v3i(0, 0, 1));
3325 assert_eq!(v3i(0, 1, 0).cross(v3i(1, 0, 0)), v3i(0, 0, -1));
3326 assert_eq!(v3i(1, 2, 3).cross(v3i(4, 5, 6)), v3i(-3, 6, -3));
3327 assert_eq!(v3i(1, 2, 3).cross(v3i(1, 2, 3)), Vec3Fix::ZERO);
3328 assert_eq!(v3i(0, 0, 4).try_normalize(), Some(v3i(0, 0, 1)));
3329 assert_eq!(v3i(0, -2, 0).try_normalize(), Some(v3i(0, -1, 0)));
3330 assert_eq!(Vec3Fix::ZERO.try_normalize(), None);
3331 assert_eq!(v3i(1, 2, 3).scale(fi(-2)), v3i(-2, -4, -6));
3332 }
3333
3334 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
3335 #[test]
3336 fn vec3_dot_batch_4_matches_scalar_dot() {
3337 let a = [v3i(1, 0, 0), v3i(1, 1, 1), v3i(2, 3, 4), v3i(-1, 0, 5)];
3338 let b = [v3i(7, 8, 9), v3i(1, 2, 3), v3i(5, 6, 7), v3i(4, 4, 4)];
3339 assert_eq!(Vec3Fix::dot_batch_4(a, b), [fi(7), fi(6), fi(56), fi(16)]);
3340 }
3341
3342 #[test]
3343 fn select_fix128_is_a_bit_exact_branchless_select() {
3344 // hi / lo ともに全 bit パターンが異なる 2 値: & / | / ^ の取り違えで必ず壊れる
3345 let a = Fix128 {
3346 hi: 0x5A5A_5A5A_5A5A_5A5A,
3347 lo: 0xA5A5_A5A5_A5A5_A5A5,
3348 };
3349 let b = Fix128 {
3350 hi: -0x0F0F_0F0F_0F0F_0F10,
3351 lo: 0x0F0F_0F0F_0F0F_0F0F,
3352 };
3353 assert_eq!(select_fix128(true, a, b), a);
3354 assert_eq!(select_fix128(false, a, b), b);
3355 assert_eq!(select_fix128(true, b, a), b);
3356 assert_eq!(select_fix128(false, b, a), a);
3357 assert_eq!(select_fix128(true, Fix128::ZERO, a), Fix128::ZERO);
3358 assert_eq!(select_fix128(false, a, Fix128::ZERO), Fix128::ZERO);
3359 }
3360
3361 #[test]
3362 fn from_impls_roundtrip_and_display_format() {
3363 assert_eq!(Fix128::from(-7i64), fi(-7));
3364 assert_eq!(Fix128::from(9i32), fi(9));
3365 let arr3: [Fix128; 3] = v3i(1, -2, 3).into();
3366 assert_eq!(arr3, [fi(1), fi(-2), fi(3)]);
3367 assert_eq!(Vec3Fix::from([fi(4), fi(5), fi(-6)]), v3i(4, 5, -6));
3368 let arr4: [Fix128; 4] = q(1, 2, 3, 4).into();
3369 assert_eq!(arr4, [fi(1), fi(2), fi(3), fi(4)]);
3370 assert_eq!(QuatFix::from([fi(-1), fi(0), fi(2), fi(3)]), q(-1, 0, 2, 3));
3371 assert_eq!(format!("{}", Fix128::from_ratio(-5, 4)), "-1.2500");
3372 assert_eq!(format!("{}", v3i(1, -2, 3)), "(1.0000, -2.0000, 3.0000)");
3373 assert_eq!(
3374 format!("{}", q(0, 0, 0, 1)),
3375 "(0.0000, 0.0000, 0.0000, 1.0000)"
3376 );
3377 }
3378
3379 // ---- sin / cos (CORDIC range reduction + golden pins) ---------------
3380
3381 #[test]
3382 fn sin_cos_golden_bit_patterns() {
3383 // 17 角度の (angle, sin, cos) bit pattern pin (1.1.1 の shr_bits 集約前後で不変を確認済)
3384 // 範囲: [-π/2, π/2] 内 / 第 2・3 象限 / ±π ちょうど / 複数周回 (3π, -7π/2) / 大きな値 (100, -1000.25)
3385 let table: [(&str, Fix128, Fix128, Fix128); 17] = [
3386 (
3387 "0",
3388 Fix128 { hi: 0, lo: 0 },
3389 Fix128 {
3390 hi: -1,
3391 lo: 18446744073709456430,
3392 },
3393 Fix128 { hi: 1, lo: 1453 },
3394 ),
3395 (
3396 "pi/6",
3397 Fix128 {
3398 hi: 0,
3399 lo: 9658692610769497123,
3400 },
3401 Fix128 {
3402 hi: 0,
3403 lo: 9223372036854815772,
3404 },
3405 Fix128 {
3406 hi: 0,
3407 lo: 15975348984942493703,
3408 },
3409 ),
3410 (
3411 "pi/4",
3412 Fix128 {
3413 hi: 0,
3414 lo: 14488038916154245684,
3415 },
3416 Fix128 {
3417 hi: 0,
3418 lo: 13043817825332851804,
3419 },
3420 Fix128 {
3421 hi: 0,
3422 lo: 13043817825332714670,
3423 },
3424 ),
3425 (
3426 "pi/3",
3427 Fix128 {
3428 hi: 1,
3429 lo: 870641147829442630,
3430 },
3431 Fix128 {
3432 hi: 0,
3433 lo: 15975348984942493703,
3434 },
3435 Fix128 {
3436 hi: 0,
3437 lo: 9223372036854815772,
3438 },
3439 ),
3440 (
3441 "pi/2",
3442 Fix128 {
3443 hi: 1,
3444 lo: 10529333758598939753,
3445 },
3446 Fix128 { hi: 1, lo: 1453 },
3447 Fix128 {
3448 hi: -1,
3449 lo: 18446744073709456430,
3450 },
3451 ),
3452 (
3453 "2pi/3",
3454 Fix128 {
3455 hi: 2,
3456 lo: 1741282295658885258,
3457 },
3458 Fix128 {
3459 hi: 0,
3460 lo: 15975348984942493703,
3461 },
3462 Fix128 {
3463 hi: -1,
3464 lo: 9223372036854735844,
3465 },
3466 ),
3467 (
3468 "pi",
3469 Fix128 {
3470 hi: 3,
3471 lo: 2611923443488327891,
3472 },
3473 Fix128 {
3474 hi: -1,
3475 lo: 18446744073709456430,
3476 },
3477 Fix128 {
3478 hi: -2,
3479 lo: 18446744073709550163,
3480 },
3481 ),
3482 (
3483 "-pi/6",
3484 Fix128 {
3485 hi: -1,
3486 lo: 8788051462940054493,
3487 },
3488 Fix128 {
3489 hi: -1,
3490 lo: 9223372036854735845,
3491 },
3492 Fix128 {
3493 hi: 0,
3494 lo: 15975348984942493709,
3495 },
3496 ),
3497 (
3498 "-pi/2",
3499 Fix128 {
3500 hi: -2,
3501 lo: 7917410315110611863,
3502 },
3503 Fix128 {
3504 hi: -2,
3505 lo: 18446744073709550164,
3506 },
3507 Fix128 {
3508 hi: -1,
3509 lo: 18446744073709456423,
3510 },
3511 ),
3512 (
3513 "-3pi/4",
3514 Fix128 {
3515 hi: -3,
3516 lo: 11876115472665917794,
3517 },
3518 Fix128 {
3519 hi: -1,
3520 lo: 5402926248376699811,
3521 },
3522 Fix128 {
3523 hi: -1,
3524 lo: 5402926248376836953,
3525 },
3526 ),
3527 (
3528 "-pi",
3529 Fix128 {
3530 hi: -4,
3531 lo: 15834820630221223725,
3532 },
3533 Fix128 {
3534 hi: -1,
3535 lo: 18446744073709456430,
3536 },
3537 Fix128 {
3538 hi: -2,
3539 lo: 18446744073709550163,
3540 },
3541 ),
3542 (
3543 "5pi/4",
3544 Fix128 {
3545 hi: 3,
3546 lo: 17099962359642573575,
3547 },
3548 Fix128 {
3549 hi: -1,
3550 lo: 5402926248376699811,
3551 },
3552 Fix128 {
3553 hi: -1,
3554 lo: 5402926248376836953,
3555 },
3556 ),
3557 (
3558 "3pi",
3559 Fix128 {
3560 hi: 9,
3561 lo: 7835770330464983673,
3562 },
3563 Fix128 {
3564 hi: -1,
3565 lo: 18446744073709456430,
3566 },
3567 Fix128 {
3568 hi: -2,
3569 lo: 18446744073709550163,
3570 },
3571 ),
3572 (
3573 "-7pi/2",
3574 Fix128 {
3575 hi: -11,
3576 lo: 81639984645628190,
3577 },
3578 Fix128 { hi: 1, lo: 1453 },
3579 Fix128 { hi: 0, lo: 95186 },
3580 ),
3581 (
3582 "1/2",
3583 Fix128 {
3584 hi: 0,
3585 lo: 9223372036854775808,
3586 },
3587 Fix128 {
3588 hi: 0,
3589 lo: 8843840213032159770,
3590 },
3591 Fix128 {
3592 hi: 0,
3593 lo: 16188540922742043083,
3594 },
3595 ),
3596 (
3597 "100",
3598 Fix128 { hi: 100, lo: 0 },
3599 Fix128 {
3600 hi: -1,
3601 lo: 9105946684437943138,
3602 },
3603 Fix128 {
3604 hi: 0,
3605 lo: 15906975547020722691,
3606 },
3607 ),
3608 (
3609 "-1000.25",
3610 Fix128 {
3611 hi: -1001,
3612 lo: 13835058055282163712,
3613 },
3614 Fix128 {
3615 hi: -1,
3616 lo: 1101110721943816547,
3617 },
3618 Fix128 {
3619 hi: 0,
3620 lo: 6277847604637333680,
3621 },
3622 ),
3623 ];
3624 for (name, angle, sin, cos) in table {
3625 let (s, c) = angle.sin_cos();
3626 assert_eq!(s, sin, "sin({name})");
3627 assert_eq!(c, cos, "cos({name})");
3628 assert_eq!(angle.sin(), sin, "sin() alias {name}");
3629 assert_eq!(angle.cos(), cos, "cos() alias {name}");
3630 }
3631 }
3632
3633 #[test]
3634 // 参照値として platform libm を使う (許容 1e-11、決定論の pin は sin_cos_golden_bit_patterns 側)
3635 #[allow(clippy::disallowed_methods)]
3636 fn sin_cos_large_and_negative_angles_reduce_correctly() {
3637 let two_pi = core::f64::consts::PI * 2.0;
3638 let cases: [(Fix128, f64); 9] = [
3639 (Fix128::PI * fi(3), 3.0 * core::f64::consts::PI),
3640 (
3641 (Fix128::PI * Fix128::from_ratio(7, 2)).neg(),
3642 -3.5 * core::f64::consts::PI,
3643 ),
3644 (fi(100), 100.0),
3645 (Fix128::from_ratio(-4001, 4), -1000.25),
3646 (Fix128::TWO_PI + Fix128::from_ratio(1, 2), two_pi + 0.5),
3647 (
3648 (Fix128::TWO_PI + Fix128::from_ratio(1, 2)).neg(),
3649 -(two_pi + 0.5),
3650 ),
3651 (
3652 Fix128::PI * fi(1000) + fi(1),
3653 1000.0 * core::f64::consts::PI + 1.0,
3654 ),
3655 (
3656 Fix128::PI + Fix128::from_ratio(1, 1000),
3657 core::f64::consts::PI + 0.001,
3658 ),
3659 (
3660 (Fix128::PI + Fix128::from_ratio(1, 1000)).neg(),
3661 -(core::f64::consts::PI + 0.001),
3662 ),
3663 ];
3664 for (angle, f) in cases {
3665 let (s, c) = angle.sin_cos();
3666 assert!(
3667 (s.to_f64() - f.sin()).abs() < 1e-11,
3668 "sin({f}) = {} vs {}",
3669 s.to_f64(),
3670 f.sin()
3671 );
3672 assert!(
3673 (c.to_f64() - f.cos()).abs() < 1e-11,
3674 "cos({f}) = {} vs {}",
3675 c.to_f64(),
3676 f.cos()
3677 );
3678 }
3679 }
3680
3681 #[test]
3682 fn sin_cos_boundaries_and_symmetries() {
3683 let eps = Fix128 { hi: 0, lo: 1 << 24 }; // 2^-40
3684 let close = |a: Fix128, b: Fix128| (a - b).abs() < eps;
3685 // ±π ちょうどは reduction を通らない (`>` / `<` は false)、π+ε は通る: どちらも sin ≈ 0、cos ≈ -1
3686 for a in [
3687 Fix128::PI,
3688 Fix128::PI.neg(),
3689 Fix128::PI + Fix128 { hi: 0, lo: 1 },
3690 Fix128::PI.neg() - Fix128 { hi: 0, lo: 1 },
3691 ] {
3692 let (s, c) = a.sin_cos();
3693 assert!(
3694 close(s, Fix128::ZERO) && close(c, fi(-1)),
3695 "{a:?}: {s:?} {c:?}"
3696 );
3697 }
3698 // ±π/2 ちょうど (quadrant 分岐は `>` false) と π/2+ε (分岐 true): sin ≈ ±1、cos ≈ 0
3699 for (a, sign) in [
3700 (Fix128::HALF_PI, 1),
3701 (Fix128::HALF_PI.neg(), -1),
3702 (Fix128::HALF_PI + Fix128 { hi: 0, lo: 1 }, 1),
3703 ] {
3704 let (s, c) = a.sin_cos();
3705 assert!(
3706 close(s, fi(sign)) && close(c, Fix128::ZERO),
3707 "{a:?}: {s:?} {c:?}"
3708 );
3709 }
3710 // 第 2 / 第 3 象限の符号: sin(2π/3) > 0, cos < 0 / sin(-2π/3) < 0, cos < 0
3711 let (s2, c2) = (Fix128::PI * Fix128::from_ratio(2, 3)).sin_cos();
3712 assert!(s2 > Fix128::ZERO && c2 < Fix128::ZERO);
3713 let (s3, c3) = (Fix128::PI * Fix128::from_ratio(-2, 3)).sin_cos();
3714 assert!(s3 < Fix128::ZERO && c3 < Fix128::ZERO);
3715 // 奇関数 / 偶関数、周期性 (2π 加算で reduction 経路が変わっても一致)
3716 for x in [
3717 Fix128::from_ratio(1, 3),
3718 Fix128::from_ratio(-7, 5),
3719 fi(2),
3720 Fix128::from_ratio(11, 4),
3721 ] {
3722 let (s, c) = x.sin_cos();
3723 let (sn, cn) = x.neg().sin_cos();
3724 assert!(
3725 close(s + sn, Fix128::ZERO) && close(c, cn),
3726 "odd/even {x:?}"
3727 );
3728 let (sp, cp) = (x + Fix128::TWO_PI).sin_cos();
3729 let (sm, cm) = (x - Fix128::TWO_PI * fi(3)).sin_cos();
3730 assert!(
3731 close(s, sp) && close(c, cp) && close(s, sm) && close(c, cm),
3732 "period {x:?}"
3733 );
3734 // sin² + cos² = 1
3735 assert!(close(s * s + c * c, Fix128::ONE), "pythagoras {x:?}");
3736 }
3737 }
3738
3739 // ---- sqrt: digit recurrence vs Newton reference vs exact floor oracle ----
3740
3741 /// The pre-1.2.0 implementation, kept as a bit-for-bit reference.
3742 fn sqrt_newton_reference(v: Fix128) -> Fix128 {
3743 if v.is_negative() || v.is_zero() {
3744 return Fix128::ZERO;
3745 }
3746 let sig_bits = if v.hi > 0 {
3747 128 - (v.hi as u64).leading_zeros() as i64
3748 } else if v.hi == 0 && v.lo > 0 {
3749 64 - v.lo.leading_zeros() as i64
3750 } else {
3751 1
3752 };
3753 let result_bit = ((sig_bits + 63) / 2) as u32;
3754 let mut x = if result_bit >= 64 {
3755 Fix128 {
3756 hi: 1i64 << (result_bit - 64).min(62),
3757 lo: 0,
3758 }
3759 } else {
3760 Fix128 {
3761 hi: 0,
3762 lo: 1u64 << result_bit,
3763 }
3764 };
3765 for _ in 0..64 {
3766 let div = v / x;
3767 x = (x + div).half();
3768 }
3769 x
3770 }
3771
3772 /// 256-bit square of a u128 as (hi, lo) limbs.
3773 fn square_wide(r: u128) -> (u128, u128) {
3774 let (a, b) = (r >> 64, r & 0xFFFF_FFFF_FFFF_FFFF);
3775 let bb = b * b;
3776 let ab = a * b; // < 2^128
3777 let aa = a * a;
3778 // r^2 = aa<<128 + 2ab<<64 + bb
3779 let (mid, c1) = bb.overflowing_add(ab << 65);
3780 let hi = aa + (ab >> 63) + c1 as u128;
3781 (hi, mid)
3782 }
3783
3784 /// Exact-floor property of I64F64 sqrt: with N = raw << 64 (192-bit),
3785 /// root^2 <= N < (root+1)^2.
3786 fn assert_sqrt_is_exact_floor(v: Fix128) {
3787 let r = v.sqrt();
3788 let root = ((r.hi as u128) << 64) | r.lo as u128;
3789 let n = ((v.hi as u128) << 64) | v.lo as u128; // N = (n_hi:n_lo) = n << 64 → (n>>64, n<<64)
3790 let n_hi = n >> 64;
3791 let n_lo = n << 64;
3792 let (lo_hi, lo_lo) = square_wide(root);
3793 assert!(
3794 (lo_hi, lo_lo) <= (n_hi, n_lo),
3795 "root^2 > N for {v:?} (root {root:#x})"
3796 );
3797 let (up_hi, up_lo) = square_wide(root + 1);
3798 assert!(
3799 (up_hi, up_lo) > (n_hi, n_lo),
3800 "(root+1)^2 <= N for {v:?} (root {root:#x})"
3801 );
3802 }
3803
3804 fn lcg_samples(n: usize) -> impl Iterator<Item = Fix128> {
3805 // Deterministic LCG (Knuth MMIX), mixes magnitudes from 2^-64 to 2^62.
3806 let mut state: u64 = 0x9E37_79B9_7F4A_7C15;
3807 (0..n).map(move |i| {
3808 state = state
3809 .wrapping_mul(6_364_136_223_846_793_005)
3810 .wrapping_add(1_442_695_040_888_963_407);
3811 let lo = state;
3812 state = state
3813 .wrapping_mul(6_364_136_223_846_793_005)
3814 .wrapping_add(1_442_695_040_888_963_407);
3815 // Vary the integer-part magnitude: 0 .. 2^62 by shifting.
3816 let shift = (i % 64) as u32;
3817 let hi = ((state >> 1) >> shift) as i64;
3818 Fix128 { hi, lo }
3819 })
3820 }
3821
3822 #[test]
3823 fn sqrt_matches_newton_reference() {
3824 let edges = [
3825 Fix128::ZERO,
3826 Fix128::from_raw(0, 1),
3827 Fix128::from_raw(0, u64::MAX),
3828 Fix128::ONE,
3829 Fix128::from_ratio(1, 4),
3830 Fix128::from_int(2),
3831 Fix128::from_int(4),
3832 Fix128::from_int(1_000_000),
3833 Fix128::from_raw(i64::MAX, u64::MAX),
3834 Fix128::from_int(-3),
3835 ];
3836 for v in edges.iter().copied().chain(lcg_samples(20_000)) {
3837 assert_eq!(v.sqrt(), sqrt_newton_reference(v), "sqrt drift for {v:?}");
3838 }
3839 }
3840
3841 #[test]
3842 fn sqrt_is_exact_floor_and_monotonic() {
3843 assert_eq!(Fix128::from_ratio(1, 4).sqrt(), Fix128::from_ratio(1, 2));
3844 assert_eq!(Fix128::from_int(4).sqrt(), Fix128::from_int(2));
3845 assert_eq!(Fix128::from_int(-3).sqrt(), Fix128::ZERO);
3846 let mut prev = Fix128::ZERO;
3847 let mut samples: Vec<Fix128> = lcg_samples(20_000).collect();
3848 samples.push(Fix128::from_raw(i64::MAX, u64::MAX));
3849 samples.push(Fix128::from_raw(0, 1));
3850 samples.sort();
3851 for v in samples {
3852 assert_sqrt_is_exact_floor(v);
3853 let r = v.sqrt();
3854 assert!(r >= prev, "sqrt not monotonic at {v:?}");
3855 prev = r;
3856 }
3857 }
3858
3859 // ---- transcendental oracle sweep (f64 libm reference) ----
3860 //
3861 // Golden bit patterns only detect *change*; a value that was wrong from the
3862 // start gets pinned as-is (that is how the 1.0.0 atan bug shipped). Every
3863 // Fix128 transcendental is therefore also checked against an independent
3864 // f64 reference over a dense sweep of the domain.
3865
3866 #[test]
3867 #[allow(clippy::disallowed_methods)] // the f64 libm values are the oracle on purpose
3868 fn transcendental_sweep_matches_f64_reference() {
3869 // CORDIC with 48 iterations: worst-case error ≈ 2^-46 ≈ 1.4e-14 plus
3870 // range-reduction rounding; f64 reference itself carries 1e-16.
3871 const TOL: f64 = 1e-11;
3872 let steps = 4_000;
3873 for i in 0..=steps {
3874 // x sweeps [-4π, 4π] (range reduction on both sides) for sin / cos
3875 let t = -4.0 * core::f64::consts::PI
3876 + 8.0 * core::f64::consts::PI * (i as f64) / (steps as f64);
3877 let x = Fix128::from_f64(t);
3878 let xf = x.to_f64();
3879 let (s, c) = x.sin_cos();
3880 assert!(
3881 (s.to_f64() - xf.sin()).abs() < TOL,
3882 "sin({xf}) = {} vs {}",
3883 s.to_f64(),
3884 xf.sin()
3885 );
3886 assert!(
3887 (c.to_f64() - xf.cos()).abs() < TOL,
3888 "cos({xf}) = {} vs {}",
3889 c.to_f64(),
3890 xf.cos()
3891 );
3892 // atan over [-40, 40] (both sides of |x| = 1 argument reduction)
3893 let a = Fix128::from_f64(t * 10.0 / core::f64::consts::PI);
3894 let af = a.to_f64();
3895 assert!(
3896 (a.atan().to_f64() - af.atan()).abs() < TOL,
3897 "atan({af}) = {} vs {}",
3898 a.atan().to_f64(),
3899 af.atan()
3900 );
3901 // sqrt over (0, 1e6]
3902 let q = Fix128::from_f64(1e6 * (i as f64 + 1.0) / (steps as f64 + 1.0));
3903 let qf = q.to_f64();
3904 assert!(
3905 (q.sqrt().to_f64() - qf.sqrt()).abs() < 1e-9 * qf.sqrt().max(1.0),
3906 "sqrt({qf}) = {} vs {}",
3907 q.sqrt().to_f64(),
3908 qf.sqrt()
3909 );
3910 }
3911 // atan2: all four quadrants + axes, radius varied
3912 for iy in -20i64..=20 {
3913 for ix in -20i64..=20 {
3914 if ix == 0 && iy == 0 {
3915 continue;
3916 }
3917 let y = Fix128::from_ratio(iy * 7, 13);
3918 let x = Fix128::from_ratio(ix * 5, 11);
3919 let (yf, xf) = (y.to_f64(), x.to_f64());
3920 let got = Fix128::atan2(y, x).to_f64();
3921 let want = yf.atan2(xf);
3922 // atan2 returns in (-π, π]; the reference may return -π on the
3923 // negative x axis with -0.0 — normalise both to [0, 2π).
3924 let norm =
3925 |v: f64| (v + 2.0 * core::f64::consts::PI) % (2.0 * core::f64::consts::PI);
3926 assert!(
3927 (norm(got) - norm(want)).abs() < TOL
3928 || (norm(got) - norm(want)).abs() > 2.0 * core::f64::consts::PI - TOL,
3929 "atan2({yf}, {xf}) = {got} vs {want}"
3930 );
3931 }
3932 }
3933 }
3934
3935 // -----------------------------------------------------------------------
3936 // sub_simd / cross_simd bit-exactness (x86_64 + `simd` feature only)
3937 // -----------------------------------------------------------------------
3938
3939 /// `Fix128::sub_simd` must be bit-identical to `-`, including borrow
3940 /// propagation from `lo` into `hi` and wrapping at the i64 boundary.
3941 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
3942 #[test]
3943 fn fix128_sub_simd_is_bit_exact_with_operator_sub() {
3944 let cases: [(Fix128, Fix128); 8] = [
3945 (fi(7), fi(3)),
3946 (fi(3), fi(7)),
3947 (fi(-5), fi(9)),
3948 // lo borrow: 0 - tiny must borrow one from hi
3949 (Fix128::from_raw(1, 0), Fix128::from_raw(0, 1)),
3950 // lo wraps fully around
3951 (Fix128::from_raw(0, 0), Fix128::from_raw(0, u64::MAX)),
3952 // arbitrary fractional bit patterns
3953 (
3954 Fix128::from_raw(3, 0xABCD_EF01_2345_6789),
3955 Fix128::from_raw(-1, 0x1111_2222_3333_4444),
3956 ),
3957 (
3958 Fix128::from_raw(-7, 0xFEDC_BA98_7654_3210),
3959 Fix128::from_raw(5, 0xAAAA_BBBB_CCCC_DDDD),
3960 ),
3961 // i64 wrap on hi (wrapping semantics of `-` must be mirrored)
3962 (Fix128::from_raw(i64::MIN, 0), Fix128::from_raw(1, 0)),
3963 ];
3964 for (a, b) in cases {
3965 let scalar = a - b;
3966 // SAFETY: `sub_simd` has no preconditions; SSE2 is part of the
3967 // x86_64 baseline so the `target_feature(enable = "sse2")`
3968 // requirement is always satisfied on this target.
3969 let simd = unsafe { a.sub_simd(b) };
3970 assert_eq!(
3971 simd.hi, scalar.hi,
3972 "sub_simd hi mismatch for {a:?} - {b:?}: {:#018x} vs {:#018x}",
3973 simd.hi, scalar.hi
3974 );
3975 assert_eq!(
3976 simd.lo, scalar.lo,
3977 "sub_simd lo mismatch for {a:?} - {b:?}: {:#018x} vs {:#018x}",
3978 simd.lo, scalar.lo
3979 );
3980 // and the closed-form: (a - b) + b == a
3981 assert_eq!(simd + b, a);
3982 }
3983 // Explicit borrow check: (1.0) - (2^-64) == 0.FFFF…F (hi 0, lo MAX)
3984 // SAFETY: as above.
3985 let borrowed = unsafe { Fix128::from_raw(1, 0).sub_simd(Fix128::from_raw(0, 1)) };
3986 assert_eq!(borrowed, Fix128::from_raw(0, u64::MAX));
3987 }
3988
3989 /// `Vec3Fix::cross_simd` must be bit-identical to `cross()` and satisfy
3990 /// the closed-form cross-product identities.
3991 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
3992 #[test]
3993 fn vec3_cross_simd_is_bit_exact_with_cross() {
3994 // Right-handed basis: x × y = z, y × z = x, z × x = y
3995 let x = v3i(1, 0, 0);
3996 let y = v3i(0, 1, 0);
3997 let z = v3i(0, 0, 1);
3998 assert_eq!(x.cross_simd(y), z);
3999 assert_eq!(y.cross_simd(z), x);
4000 assert_eq!(z.cross_simd(x), y);
4001 // Anti-commutativity and self-cross = 0
4002 assert_eq!(y.cross_simd(x), z.scale(fi(-1)));
4003 assert_eq!(x.cross_simd(x), Vec3Fix::ZERO);
4004
4005 // Textbook integer case: (1,2,3) × (4,5,6) = (-3, 6, -3)
4006 assert_eq!(v3i(1, 2, 3).cross_simd(v3i(4, 5, 6)), v3i(-3, 6, -3));
4007
4008 // Arbitrary fractional patterns: bit-exact with the scalar path,
4009 // and the result is orthogonal to both inputs (dot == 0 exactly is
4010 // not guaranteed after Fix128 rounding, so compare to the scalar
4011 // path's dot instead of to zero).
4012 let pairs = [
4013 (v3i(1, 2, 3), v3i(4, 5, 6)),
4014 (v3i(-7, 0, 11), v3i(2, -9, 5)),
4015 (
4016 Vec3Fix::new(
4017 Fix128::from_raw(3, 0xABCD_EF01_2345_6789),
4018 Fix128::from_raw(-1, 0x1111_2222_3333_4444),
4019 Fix128::from_raw(7, 0xFEDC_BA98_7654_3210),
4020 ),
4021 Vec3Fix::new(
4022 Fix128::from_raw(2, 0x9876_5432_10FE_DCBA),
4023 Fix128::from_raw(5, 0xAAAA_BBBB_CCCC_DDDD),
4024 Fix128::from_raw(-3, 0x0F0F_0F0F_0F0F_0F0F),
4025 ),
4026 ),
4027 ];
4028 for (a, b) in pairs {
4029 let scalar = a.cross(b);
4030 let simd = a.cross_simd(b);
4031 assert_eq!(simd.x.hi, scalar.x.hi, "{a:?} × {b:?}: x.hi");
4032 assert_eq!(simd.x.lo, scalar.x.lo, "{a:?} × {b:?}: x.lo");
4033 assert_eq!(simd.y.hi, scalar.y.hi, "{a:?} × {b:?}: y.hi");
4034 assert_eq!(simd.y.lo, scalar.y.lo, "{a:?} × {b:?}: y.lo");
4035 assert_eq!(simd.z.hi, scalar.z.hi, "{a:?} × {b:?}: z.hi");
4036 assert_eq!(simd.z.lo, scalar.z.lo, "{a:?} × {b:?}: z.lo");
4037 assert_eq!(simd.dot(a), scalar.dot(a));
4038 assert_eq!(simd.dot(b), scalar.dot(b));
4039 // a × b == -(b × a)
4040 assert_eq!(b.cross_simd(a), simd.scale(fi(-1)));
4041 }
4042 }
4043 /// `atan2` with |y| > |x|: the ratio `y / x` overflows the i64 integer
4044 /// part once |x| is a few ulp (1 / 2⁻⁶³ = 2⁶³), so before 1.2.0
4045 /// `atan2(1, 2 ulp)` returned −0.755. The complement form keeps every
4046 /// case within 1e-12 of the f64 reference, including the quadrant
4047 /// boundaries and the exact axes.
4048 #[test]
4049 #[allow(clippy::disallowed_methods)]
4050 fn atan2_is_accurate_when_x_is_a_few_ulp() {
4051 let ulp2 = Fix128::from_raw(0, 2);
4052 let cases = [
4053 (Fix128::ONE, ulp2),
4054 (Fix128::ONE, -ulp2),
4055 (-Fix128::ONE, ulp2),
4056 (-Fix128::ONE, -ulp2),
4057 (Fix128::ONE - Fix128::from_raw(0, 3), ulp2),
4058 (Fix128::from_ratio(7, 3), Fix128::from_ratio(-1, 5)),
4059 (Fix128::from_ratio(-7, 3), Fix128::from_ratio(1, 5)),
4060 (Fix128::from_ratio(1, 5), Fix128::from_ratio(-7, 3)),
4061 (Fix128::from_int(3), Fix128::from_int(4)),
4062 (Fix128::from_int(4), Fix128::from_int(3)),
4063 ];
4064 for (y, x) in cases {
4065 let got = Fix128::atan2(y, x).to_f64();
4066 let want = y.to_f64().atan2(x.to_f64());
4067 assert!(
4068 (got - want).abs() < 1e-12,
4069 "atan2({}, {}) = {got}, want {want}",
4070 y.to_f64(),
4071 x.to_f64()
4072 );
4073 }
4074 // exact axes are unchanged
4075 assert_eq!(Fix128::atan2(Fix128::ONE, Fix128::ZERO), Fix128::HALF_PI);
4076 assert_eq!(Fix128::atan2(-Fix128::ONE, Fix128::ZERO), -Fix128::HALF_PI);
4077 // atan(0) through CORDIC is ~5e-15, not exactly 0 (unchanged behaviour)
4078 assert!(Fix128::atan2(Fix128::ZERO, Fix128::ONE).abs() < Fix128::from_f64(1e-12));
4079 }
4080
4081 // ------------------------------------------------------------------
4082 // Mutation-score batch 10 (scoped run 34960080446, 54 missed).
4083 //
4084 // Killable misses are covered below. The remaining misses are equivalent
4085 // mutants and are documented here rather than hidden:
4086 // - `|` → `^` on disjoint bit ranges: `sqrt` (`(rem << 2) | bits`,
4087 // `(root << k) | 1`), `half` / `shr_bits` / `double` (carry bit into a
4088 // freshly shifted-in zero), `Div` (`(hi << 64) | lo`), `select_fix128`
4089 // (`mask` and `!mask` are complementary) — identical results.
4090 // - `from_f64`: `f < 0.0` → `<=`: for `f == 0.0` `lo` is 0 and both
4091 // branches yield the same value.
4092 // - `from_ratio`: `num < 0` → `<=` (0 negated is 0) and `denom < 0` →
4093 // `<=` (`denom == 0` already returned).
4094 // - `cordic_sin_cos`: `>` → `>=` / `<` → `<=` on the ±π boundaries and
4095 // `+ π` → `- π` in `k = floor((θ + π) / 2π)` reduce to the same angle
4096 // because the clamp that follows absorbs exactly one 2π; the clamp
4097 // branches themselves (`θ > π` / `θ < −π` after reduction) are only
4098 // reachable through rounding of `2π · k` and no sampled angle reaches
4099 // them; `d > 0` → `>=` in both CORDIC loops: `d` is ±1, never 0.
4100 // - `sqrt`: `i += 1` → `i *= 1` loops forever and can only time out.
4101 // ------------------------------------------------------------------
4102
4103 /// `ceil` rounds toward +∞: integers are fixed points, any fraction
4104 /// bumps the integer part by exactly one (also across zero for
4105 /// negative inputs, where the two's-complement `hi` is already the floor).
4106 #[test]
4107 fn ceil_rounds_toward_positive_infinity() {
4108 assert_eq!(Fix128::from_int(2).ceil(), Fix128::from_int(2));
4109 assert_eq!(Fix128::from_int(-2).ceil(), Fix128::from_int(-2));
4110 assert_eq!(Fix128::ZERO.ceil(), Fix128::ZERO);
4111 assert_eq!(Fix128::from_ratio(5, 2).ceil(), Fix128::from_int(3));
4112 assert_eq!(Fix128::from_ratio(-5, 2).ceil(), Fix128::from_int(-2));
4113 assert_eq!(Fix128::from_ratio(-1, 2).ceil(), Fix128::ZERO);
4114 assert_eq!(Fix128::from_raw(7, 1).ceil(), Fix128::from_int(8));
4115 // ceil(x) − x ∈ [0, 1) and ceil(x) == −floor(−x)
4116 for raw in [
4117 Fix128::from_ratio(1, 3),
4118 Fix128::from_ratio(-1, 3),
4119 Fix128::from_ratio(1_000_001, 7),
4120 Fix128::from_ratio(-1_000_001, 7),
4121 Fix128::from_raw(-3, u64::MAX),
4122 ] {
4123 let c = raw.ceil();
4124 assert!(c >= raw && c - raw < Fix128::ONE, "{raw}");
4125 assert_eq!(c, raw.neg().floor().neg(), "{raw}");
4126 }
4127 }
4128
4129 /// `from_ratio` takes the sign from `num` and `denom` independently:
4130 /// −3/5 == 3/−5 == −(3/5), and −3/−5 == 3/5. The negative value is
4131 /// pinned to its two's-complement pair: 3/5 = ⌊0.6 · 2⁶⁴⌋ =
4132 /// 11068046444225730969, so −3/5 = (−1, 2⁶⁴ − 11068046444225730969).
4133 #[test]
4134 fn from_ratio_sign_comes_from_both_operands() {
4135 let pos = Fix128::from_ratio(3, 5);
4136 assert_eq!(pos, Fix128::from_raw(0, 11_068_046_444_225_730_969));
4137 let neg = Fix128::from_raw(-1, 7_378_697_629_483_820_647);
4138 assert_eq!(Fix128::from_ratio(-3, 5), neg);
4139 assert_eq!(Fix128::from_ratio(3, -5), neg);
4140 assert_eq!(Fix128::from_ratio(-3, -5), pos);
4141 assert_eq!(neg, pos.neg());
4142 assert_eq!(Fix128::from_ratio(0, -5), Fix128::ZERO);
4143 assert_eq!(Fix128::from_ratio(-7, 1), Fix128::from_int(-7));
4144 assert_eq!(Fix128::from_ratio(7, -1), Fix128::from_int(-7));
4145 assert!(Fix128::from_ratio(1, -1_000_000).is_negative());
4146 }
4147
4148 /// `exp` cut-off boundary: e⁻⁴⁴ = 7.78 · 10⁻²⁰ is 1.435 · 2⁻⁶⁴ (oracle:
4149 /// 60-digit decimal evaluation of e⁻⁴⁴ · 2⁶⁴ = 1.4353…), so it is still
4150 /// representable, while e⁻⁴⁵ = 0.53 · 2⁻⁶⁴ truncates to zero. The
4151 /// `hi < -44` early return must therefore not include −44, and must fire
4152 /// for every smaller integer part. Between −44 and −43.67 the
4153 /// intermediate 2^(|x|·log₂e) exceeds 2⁶³ and wraps in `powf_pos`, which
4154 /// lands the reciprocal on 2 ulp instead of the truncated 1 ulp — within
4155 /// one ulp of the true value, so the assertion is a range, not a pin.
4156 #[test]
4157 fn exp_cutoff_boundary_is_minus_44() {
4158 let at_boundary = Fix128::from_int(-44).exp();
4159 assert!(!at_boundary.is_zero());
4160 assert!(at_boundary <= Fix128::from_raw(0, 2), "{at_boundary}");
4161 assert_eq!(Fix128::from_int(-45).exp(), Fix128::ZERO);
4162 assert_eq!(Fix128::from_int(-46).exp(), Fix128::ZERO);
4163 assert_eq!(Fix128::from_int(-100).exp(), Fix128::ZERO);
4164 assert_eq!(Fix128::from_raw(-45, 1).exp(), Fix128::ZERO);
4165 // just above the boundary is still non-zero and ordered
4166 let a = Fix128::from_raw(-44, 1).exp();
4167 let b = Fix128::from_int(-43).exp();
4168 assert!(!a.is_zero());
4169 assert!(a < b, "{a} < {b}");
4170 }
4171
4172 /// `QuatFix::normalize` divides every component by the length (as a
4173 /// reciprocal multiply); a mutant that multiplies `x` by the length
4174 /// instead of its reciprocal is only visible with a non-zero `x` and a
4175 /// length ≠ 1: (3, 0, 0, 4) / 5 → x = 0.6, not 15.
4176 #[test]
4177 fn quat_normalize_divides_x_by_length() {
4178 let n = q(3, 0, 0, 4).normalize();
4179 assert_eq!(
4180 n,
4181 QuatFix::new(fi(3) / fi(5), Fix128::ZERO, Fix128::ZERO, fi(4) / fi(5))
4182 );
4183 // reciprocal multiply differs from a direct division by a few ulp
4184 let x = q(-6, 0, 0, 8).normalize().x;
4185 assert!((x - fi(-6) / fi(10)).abs() < Fix128::from_raw(0, 16), "{x}");
4186 assert_eq!(q(1, 0, 0, 0).normalize(), q(1, 0, 0, 0));
4187 }
4188
4189 /// `simd_width()` is the compile-time lane count and never 0; without
4190 /// the `simd` feature it is exactly 1 (the scalar path), with it on
4191 /// x86_64 it is 4 (SSE2) or 8 (AVX2) and on aarch64 4 (NEON).
4192 #[test]
4193 fn simd_width_matches_the_build_target() {
4194 assert!(simd_width() >= 1);
4195 assert_eq!(SIMD_WIDTH, simd_width());
4196 #[cfg(not(feature = "simd"))]
4197 assert_eq!(simd_width(), 1);
4198 #[cfg(all(feature = "simd", target_arch = "x86_64"))]
4199 assert!(matches!(simd_width(), 4 | 8));
4200 #[cfg(all(feature = "simd", target_arch = "aarch64"))]
4201 assert_eq!(simd_width(), 4);
4202 #[cfg(all(
4203 feature = "simd",
4204 not(any(target_arch = "x86_64", target_arch = "aarch64"))
4205 ))]
4206 assert_eq!(simd_width(), 1);
4207 }
4208
4209 fn p2(e: u32) -> Fix128 {
4210 Fix128::from_int(1_i64 << e)
4211 }
4212
4213 /// oracle: `checked_normalize` is `None` for the zero vector and when the
4214 /// squared length leaves the range (`|v| = 2⁴⁰`), and `v/|v|` otherwise
4215 /// (`(3, 4, 0)/5`).
4216 #[test]
4217 fn checked_normalize_closed_form() {
4218 assert_eq!(Vec3Fix::ZERO.checked_normalize(), None);
4219 assert_eq!(
4220 Vec3Fix::new(p2(40), Fix128::ZERO, Fix128::ZERO).checked_normalize(),
4221 None
4222 );
4223 let n = Vec3Fix::from_int(3, 4, 0)
4224 .checked_normalize()
4225 .expect("in range");
4226 assert!((n.x.to_f64() - 0.6).abs() < 1e-15 && (n.y.to_f64() - 0.8).abs() < 1e-15);
4227 assert!(n.z.is_zero());
4228 }
4229
4230 /// oracle: past the squared range the length of `(2⁴⁰, 0, 0)` is exactly
4231 /// `2⁴⁰`, of `(2⁴⁰, 2⁴⁰, 0)` is `√2·2⁴⁰` and of `(2⁶², 2⁶², 2⁶²)` is
4232 /// `√3·2⁶² < 2⁶³` (to 1e-15 relative); a length of `√3·3·2⁶¹ ≥ 2⁶³` does
4233 /// not fit (`None`).
4234 #[test]
4235 fn checked_length_scaled_closed_form() {
4236 assert_eq!(
4237 Vec3Fix::new(p2(40), Fix128::ZERO, Fix128::ZERO).checked_length_scaled(),
4238 Some(p2(40))
4239 );
4240 let l = Vec3Fix::new(p2(40), p2(40), Fix128::ZERO)
4241 .checked_length_scaled()
4242 .expect("fits");
4243 assert!(
4244 (l.to_f64() / 1_099_511_627_776.0 - 2f64.sqrt()).abs() < 1e-15,
4245 "{l:?}"
4246 );
4247 let l = Vec3Fix::new(p2(62), p2(62), p2(62))
4248 .checked_length_scaled()
4249 .expect("√3·2⁶² < 2⁶³ fits");
4250 assert!(
4251 (l.to_f64() / 4_611_686_018_427_387_904.0 - 3f64.sqrt()).abs() < 1e-15,
4252 "{l:?}"
4253 );
4254 let c = Fix128::from_int(3_i64 << 61);
4255 assert_eq!(Vec3Fix::new(c, c, c).checked_length_scaled(), None);
4256 }
4257
4258 /// oracle: `try_normalize_scaled` gives the unit vector `(1, 0, 0)` for
4259 /// `(2⁴⁰, 0, 0)` (squared length out of range) and for `(2⁻⁴⁰, 0, 0)`
4260 /// (squared length below the resolution), `(0, −1, 0)` for `(0, −2⁴⁰, 0)`,
4261 /// and `None` for the zero vector.
4262 #[test]
4263 fn try_normalize_scaled_closed_form() {
4264 let unit_x = Vec3Fix::new(Fix128::ONE, Fix128::ZERO, Fix128::ZERO);
4265 assert_eq!(
4266 Vec3Fix::new(p2(40), Fix128::ZERO, Fix128::ZERO).try_normalize_scaled(),
4267 Some(unit_x)
4268 );
4269 let tiny = Fix128::ONE / p2(40);
4270 assert_eq!(
4271 Vec3Fix::new(tiny, Fix128::ZERO, Fix128::ZERO).try_normalize_scaled(),
4272 Some(unit_x)
4273 );
4274 assert_eq!(
4275 Vec3Fix::new(Fix128::ZERO, -p2(40), Fix128::ZERO).try_normalize_scaled(),
4276 Some(Vec3Fix::new(Fix128::ZERO, -Fix128::ONE, Fix128::ZERO))
4277 );
4278 assert_eq!(Vec3Fix::ZERO.try_normalize_scaled(), None);
4279 assert_eq!(Vec3Fix::ZERO.split_pow2(), (Vec3Fix::ZERO, 0));
4280 }
4281}