Skip to main content

vibe/
frame_morphism.rs

1//! Frame morphisms — Galilean now, Lorentz later (W5).
2//!
3//! A frame morphism transforms a pose from one reference frame to
4//! another. The simplest case is a Galilean transformation: a rigid
5//! translation + rotation between frames moving at constant velocity
6//! relative to each other. Lorentz transformations (special relativity)
7//! are deferred — they require the full 4D spacetime metric and
8//! time dilation.
9//!
10//! ## Why this matters
11//!
12//! Without frame morphisms, a pose in "camera space" and a pose in
13//! "world space" are just two Vec\<f64\> values with no relationship.
14//! The engine can't compose them, invert them, or chain them. Frame
15//! morphisms make the relationship explicit and computable.
16//!
17//! ## Galilean transformation
18//!
19//! A Galilean transformation between frame A and frame B is:
20//! - A translation (offset of B's origin in A's coordinates)
21//! - A rotation (orientation of B's axes relative to A's)
22//! - A velocity (for moving frames — B moves at constant velocity
23//!   relative to A)
24//!
25//! Position transforms as: p_B = R^(-1) * (p_A - offset - v * t)
26//! Time is absolute: t_B = t_A (no time dilation)
27//!
28//! Lorentz transformation (future) would add:
29//! - Time dilation: t_B = γ * (t_A - v·x_A / c²)
30//! - Length contraction: x_B = γ * (x_A - v * t_A)
31//! - Where γ = 1 / sqrt(1 - v²/c²)
32//!
33//! Reference: `docs/vibescript-full-impl-PLAN.md` §8.15 W5,
34//! excellence-first §4.
35
36use crate::value::{Pose, Value};
37use std::collections::BTreeMap;
38
39/// A Galilean frame morphism — translation + rotation + velocity (W5).
40#[derive(Debug, Clone, PartialEq)]
41pub struct GalileanMorphism {
42    /// Translation: offset of the target frame's origin in the
43    /// source frame's coordinates.
44    pub translation: Vec<f64>,
45    /// Rotation as a 3x3 matrix (row-major). Identity = no rotation.
46    pub rotation: [[f64; 3]; 3],
47    /// Velocity of the target frame relative to the source (m/s).
48    /// For stationary frames, this is [0, 0, 0].
49    pub velocity: Vec<f64>,
50}
51
52impl GalileanMorphism {
53    /// Identity morphism — no transformation.
54    pub fn identity() -> Self {
55        Self {
56            translation: vec![0.0, 0.0, 0.0],
57            rotation: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]],
58            velocity: vec![0.0, 0.0, 0.0],
59        }
60    }
61
62    /// Create a pure translation morphism.
63    pub fn translation(tx: f64, ty: f64, tz: f64) -> Self {
64        Self {
65            translation: vec![tx, ty, tz],
66            ..Self::identity()
67        }
68    }
69
70    /// Create a pure velocity morphism (moving frame, no rotation).
71    pub fn velocity(vx: f64, vy: f64, vz: f64) -> Self {
72        Self {
73            velocity: vec![vx, vy, vz],
74            ..Self::identity()
75        }
76    }
77
78    /// Transform a position from source frame to target frame.
79    /// p_target = R^T * (p_source - translation - velocity * t)
80    pub fn transform_position(&self, pos: &[f64], t: f64) -> Vec<f64> {
81        if pos.len() < 3 {
82            return pos.to_vec();
83        }
84        // Subtract translation and velocity * t
85        let x = pos[0] - self.translation[0] - self.velocity[0] * t;
86        let y = pos[1] - self.translation[1] - self.velocity[1] * t;
87        let z = pos[2] - self.translation[2] - self.velocity[2] * t;
88        // Apply inverse rotation (R^T * p)
89        let r = &self.rotation;
90        let nx = r[0][0] * x + r[1][0] * y + r[2][0] * z;
91        let ny = r[0][1] * x + r[1][1] * y + r[2][1] * z;
92        let nz = r[0][2] * x + r[1][2] * y + r[2][2] * z;
93        vec![nx, ny, nz]
94    }
95
96    /// Transform a pose from source frame to target frame.
97    /// Time is absolute in Galilean relativity (t_target = t_source).
98    pub fn transform_pose(&self, pose: &Pose, t: f64) -> Pose {
99        let new_position = self.transform_position(&pose.position, t);
100        // Transform orientation (rotate by inverse rotation)
101        let new_orientation = if pose.orientation.len() >= 4 {
102            // Quaternion rotation by inverse frame rotation
103            // For simplicity, keep orientation as-is for now
104            // (full quaternion composition is a future enhancement)
105            pose.orientation.clone()
106        } else {
107            pose.orientation.clone()
108        };
109        Pose {
110            position: new_position,
111            orientation: new_orientation,
112            frame: None, // Target frame
113        }
114    }
115
116    /// Inverse morphism — transforms from target back to source.
117    pub fn inverse(&self) -> Self {
118        // Inverse: swap translation sign, transpose rotation, negate velocity
119        let r = &self.rotation;
120        // Transpose rotation
121        let rt = [
122            [r[0][0], r[1][0], r[2][0]],
123            [r[0][1], r[1][1], r[2][1]],
124            [r[0][2], r[1][2], r[2][2]],
125        ];
126        // Inverse translation: -R^T * translation
127        let inv_tx = -(rt[0][0] * self.translation[0]
128            + rt[0][1] * self.translation[1]
129            + rt[0][2] * self.translation[2]);
130        let inv_ty = -(rt[1][0] * self.translation[0]
131            + rt[1][1] * self.translation[1]
132            + rt[1][2] * self.translation[2]);
133        let inv_tz = -(rt[2][0] * self.translation[0]
134            + rt[2][1] * self.translation[1]
135            + rt[2][2] * self.translation[2]);
136        // Inverse velocity: -R^T * velocity
137        let inv_vx = -(rt[0][0] * self.velocity[0]
138            + rt[0][1] * self.velocity[1]
139            + rt[0][2] * self.velocity[2]);
140        let inv_vy = -(rt[1][0] * self.velocity[0]
141            + rt[1][1] * self.velocity[1]
142            + rt[1][2] * self.velocity[2]);
143        let inv_vz = -(rt[2][0] * self.velocity[0]
144            + rt[2][1] * self.velocity[1]
145            + rt[2][2] * self.velocity[2]);
146        Self {
147            translation: vec![inv_tx, inv_ty, inv_tz],
148            rotation: rt,
149            velocity: vec![inv_vx, inv_vy, inv_vz],
150        }
151    }
152
153    /// Compose two morphisms: self ∘ other (apply other first, then self).
154    pub fn compose(&self, other: &Self) -> Self {
155        // For Galilean: combined translation = R_self * other.translation + self.translation
156        // combined rotation = R_self * R_other
157        // combined velocity = R_self * other.velocity + self.velocity
158        let r = &self.rotation;
159        let ro = &other.rotation;
160        // Combined rotation: R_self * R_other
161        let cr = [
162            [
163                r[0][0] * ro[0][0] + r[0][1] * ro[1][0] + r[0][2] * ro[2][0],
164                r[0][0] * ro[0][1] + r[0][1] * ro[1][1] + r[0][2] * ro[2][1],
165                r[0][0] * ro[0][2] + r[0][1] * ro[1][2] + r[0][2] * ro[2][2],
166            ],
167            [
168                r[1][0] * ro[0][0] + r[1][1] * ro[1][0] + r[1][2] * ro[2][0],
169                r[1][0] * ro[0][1] + r[1][1] * ro[1][1] + r[1][2] * ro[2][1],
170                r[1][0] * ro[0][2] + r[1][1] * ro[1][2] + r[1][2] * ro[2][2],
171            ],
172            [
173                r[2][0] * ro[0][0] + r[2][1] * ro[1][0] + r[2][2] * ro[2][0],
174                r[2][0] * ro[0][1] + r[2][1] * ro[1][1] + r[2][2] * ro[2][1],
175                r[2][0] * ro[0][2] + r[2][1] * ro[1][2] + r[2][2] * ro[2][2],
176            ],
177        ];
178        // Combined translation: R_self * other.translation + self.translation
179        let ot = &other.translation;
180        let ct = vec![
181            r[0][0] * ot[0] + r[0][1] * ot[1] + r[0][2] * ot[2] + self.translation[0],
182            r[1][0] * ot[0] + r[1][1] * ot[1] + r[1][2] * ot[2] + self.translation[1],
183            r[2][0] * ot[0] + r[2][1] * ot[1] + r[2][2] * ot[2] + self.translation[2],
184        ];
185        // Combined velocity: R_self * other.velocity + self.velocity
186        let ov = &other.velocity;
187        let cv = vec![
188            r[0][0] * ov[0] + r[0][1] * ov[1] + r[0][2] * ov[2] + self.velocity[0],
189            r[1][0] * ov[0] + r[1][1] * ov[1] + r[1][2] * ov[2] + self.velocity[1],
190            r[2][0] * ov[0] + r[2][1] * ov[1] + r[2][2] * ov[2] + self.velocity[2],
191        ];
192        Self {
193            translation: ct,
194            rotation: cr,
195            velocity: cv,
196        }
197    }
198
199    /// Convert to a Value::Record for inspection and graph storage.
200    pub fn to_value(&self) -> Value {
201        let mut rec = BTreeMap::new();
202        rec.insert("kind".into(), Value::String("galilean".into()));
203        rec.insert(
204            "translation".into(),
205            Value::List(self.translation.iter().map(|v| Value::F64(*v)).collect()),
206        );
207        rec.insert(
208            "rotation".into(),
209            Value::List(
210                self.rotation
211                    .iter()
212                    .flat_map(|row| row.iter().map(|v| Value::F64(*v)))
213                    .collect(),
214            ),
215        );
216        rec.insert(
217            "velocity".into(),
218            Value::List(self.velocity.iter().map(|v| Value::F64(*v)).collect()),
219        );
220        Value::Record(rec)
221    }
222}
223
224/// A Lorentz frame morphism — special relativistic 4D spacetime transformation.
225///
226/// Implements the exact 4-vector boost for arbitrary velocity vectors $\vec{v}$ with $|\vec{v}| < c$:
227/// $$ct' = \gamma (ct - \vec{\beta} \cdot \vec{r})$$
228/// $$\vec{r}' = \vec{r} + \left( \frac{\gamma - 1}{v^2} (\vec{v} \cdot \vec{r}) - \gamma t \right) \vec{v}$$
229/// Preserving the Minkowski spacetime invariant interval:
230/// $$s^2 = (c \Delta t)^2 - \Delta x^2 - \Delta y^2 - \Delta z^2$$
231#[derive(Debug, Clone, PartialEq)]
232pub struct LorentzMorphism {
233    /// Velocity of the target frame relative to the source (m/s).
234    pub velocity: Vec<f64>,
235    /// Speed of light (default: 299_792_458 m/s).
236    pub c: f64,
237}
238
239impl LorentzMorphism {
240    /// Standard speed of light in vacuum (m/s).
241    pub const C_DEFAULT: f64 = 299_792_458.0;
242
243    /// Create a new Lorentz boost morphism.
244    pub fn new(velocity: Vec<f64>, c: f64) -> Self {
245        Self { velocity, c }
246    }
247
248    /// Create an identity Lorentz morphism (zero boost).
249    pub fn identity() -> Self {
250        Self {
251            velocity: vec![0.0, 0.0, 0.0],
252            c: Self::C_DEFAULT,
253        }
254    }
255
256    /// Create a pure velocity boost along given vector.
257    pub fn boost(vx: f64, vy: f64, vz: f64) -> Self {
258        Self {
259            velocity: vec![vx, vy, vz],
260            c: Self::C_DEFAULT,
261        }
262    }
263
264    /// Lorentz factor γ = 1 / sqrt(1 - v²/c²)
265    pub fn gamma(&self) -> f64 {
266        let v2 = self.velocity.iter().map(|v| v * v).sum::<f64>();
267        let c2 = self.c * self.c;
268        if v2 >= c2 {
269            return f64::INFINITY;
270        }
271        1.0 / (1.0 - v2 / c2).sqrt()
272    }
273
274    /// Compute the 4x4 Lorentz boost transformation matrix $\Lambda^\mu_{\ \nu}$.
275    /// Basis order: [0]=ct, [1]=x, [2]=y, [3]=z.
276    pub fn boost_matrix_4x4(&self) -> [[f64; 4]; 4] {
277        let vx = self.velocity.first().copied().unwrap_or(0.0);
278        let vy = self.velocity.get(1).copied().unwrap_or(0.0);
279        let vz = self.velocity.get(2).copied().unwrap_or(0.0);
280        let v2 = vx * vx + vy * vy + vz * vz;
281        if v2 == 0.0 {
282            return [
283                [1.0, 0.0, 0.0, 0.0],
284                [0.0, 1.0, 0.0, 0.0],
285                [0.0, 0.0, 1.0, 0.0],
286                [0.0, 0.0, 0.0, 1.0],
287            ];
288        }
289
290        let gamma = self.gamma();
291        let bx = vx / self.c;
292        let by = vy / self.c;
293        let bz = vz / self.c;
294        let beta2 = v2 / (self.c * self.c);
295        let g_minus_1_over_b2 = (gamma - 1.0) / beta2;
296
297        [
298            [gamma, -gamma * bx, -gamma * by, -gamma * bz],
299            [
300                -gamma * bx,
301                1.0 + g_minus_1_over_b2 * bx * bx,
302                g_minus_1_over_b2 * bx * by,
303                g_minus_1_over_b2 * bx * bz,
304            ],
305            [
306                -gamma * by,
307                g_minus_1_over_b2 * by * bx,
308                1.0 + g_minus_1_over_b2 * by * by,
309                g_minus_1_over_b2 * by * bz,
310            ],
311            [
312                -gamma * bz,
313                g_minus_1_over_b2 * bz * bx,
314                g_minus_1_over_b2 * bz * by,
315                1.0 + g_minus_1_over_b2 * bz * bz,
316            ],
317        ]
318    }
319
320    /// Transform an event given as time t (seconds) and position [x, y, z] (meters).
321    /// Returns (t_prime, [x_prime, y_prime, z_prime]).
322    pub fn transform_event(&self, t: f64, pos: &[f64]) -> (f64, Vec<f64>) {
323        let ct = self.c * t;
324        let (ct_prime, r_prime) = self.boost_event(ct, pos);
325        (ct_prime / self.c, r_prime)
326    }
327
328    /// Transform a 4-vector event (ct, [x, y, z]).
329    pub fn boost_event(&self, ct: f64, r: &[f64]) -> (f64, Vec<f64>) {
330        let rx = r.first().copied().unwrap_or(0.0);
331        let ry = r.get(1).copied().unwrap_or(0.0);
332        let rz = r.get(2).copied().unwrap_or(0.0);
333
334        let vx = self.velocity.first().copied().unwrap_or(0.0);
335        let vy = self.velocity.get(1).copied().unwrap_or(0.0);
336        let vz = self.velocity.get(2).copied().unwrap_or(0.0);
337        let v2 = vx * vx + vy * vy + vz * vz;
338
339        if v2 == 0.0 {
340            return (ct, vec![rx, ry, rz]);
341        }
342
343        let gamma = self.gamma();
344        if gamma.is_infinite() {
345            return (
346                f64::INFINITY,
347                vec![f64::INFINITY, f64::INFINITY, f64::INFINITY],
348            );
349        }
350
351        let v_dot_r = vx * rx + vy * ry + vz * rz;
352        let beta_dot_r = v_dot_r / self.c;
353
354        // ct' = γ (ct - β · r)
355        let ct_prime = gamma * (ct - beta_dot_r);
356
357        // r' = r + ((γ - 1) * (v · r) / v² - γ * (ct / c)) * v
358        let t = ct / self.c;
359        let factor = ((gamma - 1.0) / v2) * v_dot_r - gamma * t;
360        let rx_prime = rx + factor * vx;
361        let ry_prime = ry + factor * vy;
362        let rz_prime = rz + factor * vz;
363
364        (ct_prime, vec![rx_prime, ry_prime, rz_prime])
365    }
366
367    /// Transform 3D position at time t (compatibility with Galilean interface).
368    pub fn transform_position(&self, pos: &[f64], t: f64) -> Vec<f64> {
369        self.transform_event(t, pos).1
370    }
371
372    /// Invariant spacetime interval: s² = (ct)² - (x² + y² + z²).
373    pub fn invariant_interval_squared(ct: f64, r: &[f64]) -> f64 {
374        let x = r.first().copied().unwrap_or(0.0);
375        let y = r.get(1).copied().unwrap_or(0.0);
376        let z = r.get(2).copied().unwrap_or(0.0);
377        ct * ct - (x * x + y * y + z * z)
378    }
379
380    /// Inverse boost morphism — transforms back by reversing velocity.
381    pub fn inverse(&self) -> Self {
382        Self {
383            velocity: self.velocity.iter().map(|v| -v).collect(),
384            c: self.c,
385        }
386    }
387
388    /// Convert to Vibe Value record for scripting.
389    pub fn to_value(&self) -> Value {
390        let mut rec = std::collections::BTreeMap::new();
391        rec.insert(
392            "velocity".into(),
393            Value::List(self.velocity.iter().map(|v| Value::F64(*v)).collect()),
394        );
395        rec.insert("c".into(), Value::F64(self.c));
396        rec.insert("gamma".into(), Value::F64(self.gamma()));
397        rec.insert("honesty".into(), Value::String(self.honesty_label().into()));
398        Value::Record(rec)
399    }
400
401    /// Honesty label — Lorentz boost is fully implemented.
402    pub fn honesty_label(&self) -> &'static str {
403        "real"
404    }
405}
406
407#[cfg(test)]
408mod tests {
409    use super::*;
410
411    // ── GalileanMorphism tests ────────────────────────────────────────
412
413    #[test]
414    fn w5_identity_transforms_unchanged() {
415        let m = GalileanMorphism::identity();
416        let pos = vec![1.0, 2.0, 3.0];
417        let result = m.transform_position(&pos, 0.0);
418        assert_eq!(result, vec![1.0, 2.0, 3.0]);
419    }
420
421    #[test]
422    fn w5_translation_transforms_position() {
423        let m = GalileanMorphism::translation(10.0, 20.0, 30.0);
424        let pos = vec![1.0, 2.0, 3.0];
425        let result = m.transform_position(&pos, 0.0);
426        assert_eq!(result, vec![-9.0, -18.0, -27.0]);
427    }
428
429    #[test]
430    fn w5_velocity_transforms_position_over_time() {
431        let m = GalileanMorphism::velocity(1.0, 0.0, 0.0);
432        let pos = vec![10.0, 0.0, 0.0];
433        // At t=5, the frame has moved 5m in x
434        let result = m.transform_position(&pos, 5.0);
435        assert_eq!(result, vec![5.0, 0.0, 0.0]);
436    }
437
438    #[test]
439    fn w5_inverse_round_trip() {
440        let m = GalileanMorphism::translation(10.0, 20.0, 30.0);
441        let pos = vec![1.0, 2.0, 3.0];
442        let transformed = m.transform_position(&pos, 0.0);
443        let inv = m.inverse();
444        let recovered = inv.transform_position(&transformed, 0.0);
445        for i in 0..3 {
446            assert!((recovered[i] - pos[i]).abs() < 1e-10);
447        }
448    }
449
450    #[test]
451    fn w5_compose_identity_with_any_is_any() {
452        let id = GalileanMorphism::identity();
453        let m = GalileanMorphism::translation(5.0, 0.0, 0.0);
454        let composed = id.compose(&m);
455        let pos = vec![1.0, 2.0, 3.0];
456        let result = composed.transform_position(&pos, 0.0);
457        assert_eq!(result, vec![-4.0, 2.0, 3.0]);
458    }
459
460    #[test]
461    fn w5_compose_two_translations() {
462        let m1 = GalileanMorphism::translation(5.0, 0.0, 0.0);
463        let m2 = GalileanMorphism::translation(0.0, 10.0, 0.0);
464        let composed = m1.compose(&m2);
465        let pos = vec![0.0, 0.0, 0.0];
466        let result = composed.transform_position(&pos, 0.0);
467        // Apply m2 first (translate by [0,10,0]), then m1 (translate by [5,0,0])
468        // Result: [0-0-5, 0-10-0, 0] = [-5, -10, 0]
469        assert_eq!(result, vec![-5.0, -10.0, 0.0]);
470    }
471
472    #[test]
473    fn w5_transform_pose() {
474        let m = GalileanMorphism::translation(10.0, 0.0, 0.0);
475        let pose = Pose {
476            position: vec![5.0, 0.0, 0.0],
477            orientation: vec![1.0, 0.0, 0.0, 0.0],
478            frame: Some("source".into()),
479        };
480        let result = m.transform_pose(&pose, 0.0);
481        assert_eq!(result.position, vec![-5.0, 0.0, 0.0]);
482    }
483
484    #[test]
485    fn w5_velocity_inverse_round_trip() {
486        let m = GalileanMorphism::velocity(3.0, 4.0, 0.0);
487        let pos = vec![10.0, 20.0, 30.0];
488        let t = 2.0;
489        let transformed = m.transform_position(&pos, t);
490        let inv = m.inverse();
491        let recovered = inv.transform_position(&transformed, t);
492        for i in 0..3 {
493            assert!((recovered[i] - pos[i]).abs() < 1e-10);
494        }
495    }
496
497    #[test]
498    fn w5_to_value() {
499        let m = GalileanMorphism::translation(1.0, 2.0, 3.0);
500        let v = m.to_value();
501        match v {
502            Value::Record(r) => {
503                assert_eq!(r.get("kind"), Some(&Value::String("galilean".into())));
504                assert!(r.contains_key("translation"));
505                assert!(r.contains_key("rotation"));
506                assert!(r.contains_key("velocity"));
507            }
508            _ => panic!("expected Record"),
509        }
510    }
511
512    // ── LorentzMorphism tests ─────────────────────────────────────────
513
514    #[test]
515    fn w5_lorentz_gamma_at_rest() {
516        let m = LorentzMorphism {
517            velocity: vec![0.0, 0.0, 0.0],
518            c: 299_792_458.0,
519        };
520        assert!((m.gamma() - 1.0).abs() < 1e-10);
521    }
522
523    #[test]
524    fn w5_lorentz_gamma_at_half_c() {
525        let c = 299_792_458.0;
526        let m = LorentzMorphism {
527            velocity: vec![c * 0.5, 0.0, 0.0],
528            c,
529        };
530        let expected = 1.0 / (1.0_f64 - 0.25).sqrt();
531        assert!((m.gamma() - expected).abs() < 1e-10);
532    }
533
534    #[test]
535    fn w5_lorentz_gamma_at_c_is_infinite() {
536        let c = 299_792_458.0;
537        let m = LorentzMorphism {
538            velocity: vec![c, 0.0, 0.0],
539            c,
540        };
541        assert!(m.gamma().is_infinite());
542    }
543
544    #[test]
545    fn w5_lorentz_honesty_is_real() {
546        let m = LorentzMorphism {
547            velocity: vec![1000.0, 0.0, 0.0],
548            c: 299_792_458.0,
549        };
550        assert_eq!(m.honesty_label(), "real");
551    }
552
553    #[test]
554    fn w5_lorentz_invariant_interval_preserved() {
555        let c = 299_792_458.0;
556        let m = LorentzMorphism::new(vec![0.6 * c, 0.0, 0.0], c);
557        let t = 2.0;
558        let pos = vec![300_000_000.0, 50.0, -100.0];
559        let ct = c * t;
560        let s2_orig = LorentzMorphism::invariant_interval_squared(ct, &pos);
561
562        let (ct_prime, r_prime) = m.boost_event(ct, &pos);
563        let s2_boosted = LorentzMorphism::invariant_interval_squared(ct_prime, &r_prime);
564
565        let diff = (s2_orig - s2_boosted).abs();
566        assert!(diff / s2_orig.abs() < 1e-10, "Minkowski metric s^2 must be invariant under Lorentz boost: orig={s2_orig}, boosted={s2_boosted}");
567    }
568
569    #[test]
570    fn w5_lorentz_inverse_recovers_original_event() {
571        let c = 299_792_458.0;
572        let m = LorentzMorphism::new(vec![0.3 * c, -0.2 * c, 0.1 * c], c);
573        let inv = m.inverse();
574
575        let t = 1.5;
576        let pos = vec![100.0, 200.0, 300.0];
577        let (t_prime, r_prime) = m.transform_event(t, &pos);
578        let (t_rec, r_rec) = inv.transform_event(t_prime, &r_prime);
579
580        assert!((t - t_rec).abs() < 1e-10, "Time must invert accurately");
581        assert!((pos[0] - r_rec[0]).abs() < 1e-6, "X must invert accurately");
582        assert!((pos[1] - r_rec[1]).abs() < 1e-6, "Y must invert accurately");
583        assert!((pos[2] - r_rec[2]).abs() < 1e-6, "Z must invert accurately");
584    }
585}