1use crate::value::{Pose, Value};
37use std::collections::BTreeMap;
38
39#[derive(Debug, Clone, PartialEq)]
41pub struct GalileanMorphism {
42 pub translation: Vec<f64>,
45 pub rotation: [[f64; 3]; 3],
47 pub velocity: Vec<f64>,
50}
51
52impl GalileanMorphism {
53 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 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 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 pub fn transform_position(&self, pos: &[f64], t: f64) -> Vec<f64> {
81 if pos.len() < 3 {
82 return pos.to_vec();
83 }
84 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 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 pub fn transform_pose(&self, pose: &Pose, t: f64) -> Pose {
99 let new_position = self.transform_position(&pose.position, t);
100 let new_orientation = if pose.orientation.len() >= 4 {
102 pose.orientation.clone()
106 } else {
107 pose.orientation.clone()
108 };
109 Pose {
110 position: new_position,
111 orientation: new_orientation,
112 frame: None, }
114 }
115
116 pub fn inverse(&self) -> Self {
118 let r = &self.rotation;
120 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 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 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 pub fn compose(&self, other: &Self) -> Self {
155 let r = &self.rotation;
159 let ro = &other.rotation;
160 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 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 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 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#[derive(Debug, Clone, PartialEq)]
232pub struct LorentzMorphism {
233 pub velocity: Vec<f64>,
235 pub c: f64,
237}
238
239impl LorentzMorphism {
240 pub const C_DEFAULT: f64 = 299_792_458.0;
242
243 pub fn new(velocity: Vec<f64>, c: f64) -> Self {
245 Self { velocity, c }
246 }
247
248 pub fn identity() -> Self {
250 Self {
251 velocity: vec![0.0, 0.0, 0.0],
252 c: Self::C_DEFAULT,
253 }
254 }
255
256 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 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 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 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 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 let ct_prime = gamma * (ct - beta_dot_r);
356
357 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 pub fn transform_position(&self, pos: &[f64], t: f64) -> Vec<f64> {
369 self.transform_event(t, pos).1
370 }
371
372 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 pub fn inverse(&self) -> Self {
382 Self {
383 velocity: self.velocity.iter().map(|v| -v).collect(),
384 c: self.c,
385 }
386 }
387
388 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 pub fn honesty_label(&self) -> &'static str {
403 "real"
404 }
405}
406
407#[cfg(test)]
408mod tests {
409 use super::*;
410
411 #[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 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 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 #[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}