1use std::fmt::Display;
2
3use approx::{AbsDiffEq, RelativeEq};
4use auto_ops::{impl_op_ex, impl_op_ex_commutative};
5use nalgebra::{Vector3, Vector4};
6
7use serde::{Deserialize, Serialize};
8
9#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
19pub struct Vec3 {
20 pub x: f64,
22 pub y: f64,
24 pub z: f64,
26}
27
28impl Display for Vec3 {
29 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
30 write!(f, "[{:6.3}, {:6.3}, {:6.3}]", self.x, self.y, self.z)
31 }
32}
33
34impl AbsDiffEq for Vec3 {
35 type Epsilon = <f64 as approx::AbsDiffEq>::Epsilon;
36
37 fn default_epsilon() -> Self::Epsilon {
38 f64::default_epsilon()
39 }
40
41 fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
42 f64::abs_diff_eq(&self.x, &other.x, epsilon)
43 && f64::abs_diff_eq(&self.y, &other.y, epsilon)
44 && f64::abs_diff_eq(&self.z, &other.z, epsilon)
45 }
46}
47impl RelativeEq for Vec3 {
48 fn default_max_relative() -> Self::Epsilon {
49 f64::default_max_relative()
50 }
51
52 fn relative_eq(
53 &self,
54 other: &Self,
55 epsilon: Self::Epsilon,
56 max_relative: Self::Epsilon,
57 ) -> bool {
58 f64::relative_eq(&self.x, &other.x, epsilon, max_relative)
59 && f64::relative_eq(&self.y, &other.y, epsilon, max_relative)
60 && f64::relative_eq(&self.z, &other.z, epsilon, max_relative)
61 }
62}
63
64impl From<Vec3> for Vector3<f64> {
65 fn from(value: Vec3) -> Self {
66 Vector3::new(value.x, value.y, value.z)
67 }
68}
69
70impl From<Vector3<f64>> for Vec3 {
71 fn from(value: Vector3<f64>) -> Self {
72 Vec3::new(value.x, value.y, value.z)
73 }
74}
75
76impl From<Vec<f64>> for Vec3 {
77 fn from(value: Vec<f64>) -> Self {
78 Self {
79 x: value[0],
80 y: value[1],
81 z: value[2],
82 }
83 }
84}
85
86impl From<Vec3> for Vec<f64> {
87 fn from(value: Vec3) -> Self {
88 vec![value.x, value.y, value.z]
89 }
90}
91
92impl From<[f64; 3]> for Vec3 {
93 fn from(value: [f64; 3]) -> Self {
94 Self {
95 x: value[0],
96 y: value[1],
97 z: value[2],
98 }
99 }
100}
101
102impl From<Vec3> for [f64; 3] {
103 fn from(value: Vec3) -> Self {
104 [value.x, value.y, value.z]
105 }
106}
107
108impl Default for Vec3 {
109 fn default() -> Self {
110 Vec3::zero()
111 }
112}
113
114impl Vec3 {
115 pub fn new(x: f64, y: f64, z: f64) -> Self {
117 Vec3 { x, y, z }
118 }
119
120 pub const fn zero() -> Self {
122 Vec3 {
123 x: 0.0,
124 y: 0.0,
125 z: 0.0,
126 }
127 }
128
129 pub const fn x() -> Self {
131 Vec3 {
132 x: 1.0,
133 y: 0.0,
134 z: 0.0,
135 }
136 }
137
138 pub const fn y() -> Self {
140 Vec3 {
141 x: 0.0,
142 y: 1.0,
143 z: 0.0,
144 }
145 }
146
147 pub const fn z() -> Self {
149 Vec3 {
150 x: 0.0,
151 y: 0.0,
152 z: 1.0,
153 }
154 }
155
156 pub fn px(&self) -> f64 {
158 self.x
159 }
160
161 pub fn py(&self) -> f64 {
163 self.y
164 }
165
166 pub fn pz(&self) -> f64 {
168 self.z
169 }
170
171 pub fn with_mass(&self, mass: f64) -> Vec4 {
173 let e = f64::sqrt(mass.powi(2) + self.mag2());
174 Vec4::new(self.px(), self.py(), self.pz(), e)
175 }
176
177 pub fn with_energy(&self, energy: f64) -> Vec4 {
179 Vec4::new(self.px(), self.py(), self.pz(), energy)
180 }
181
182 pub fn dot(&self, other: &Vec3) -> f64 {
184 self.x * other.x + self.y * other.y + self.z * other.z
185 }
186
187 pub fn cross(&self, other: &Vec3) -> Vec3 {
189 Vec3::new(
190 self.y * other.z - other.y * self.z,
191 self.z * other.x - other.z * self.x,
192 self.x * other.y - other.x * self.y,
193 )
194 }
195
196 pub fn mag(&self) -> f64 {
198 f64::sqrt(self.mag2())
199 }
200
201 pub fn mag2(&self) -> f64 {
203 self.dot(self)
204 }
205
206 pub fn costheta(&self) -> f64 {
208 self.z / self.mag()
209 }
210
211 pub fn theta(&self) -> f64 {
213 f64::acos(self.costheta())
214 }
215
216 pub fn phi(&self) -> f64 {
218 f64::atan2(self.y, self.x)
219 }
220
221 pub fn unit(&self) -> Vec3 {
223 let mag = self.mag();
224 Vec3::new(self.x / mag, self.y / mag, self.z / mag)
225 }
226}
227
228impl<'a> std::iter::Sum<&'a Vec3> for Vec3 {
229 fn sum<I: Iterator<Item = &'a Self>>(iter: I) -> Self {
230 iter.fold(Self::zero(), |a, b| a + b)
231 }
232}
233impl std::iter::Sum<Vec3> for Vec3 {
234 fn sum<I: Iterator<Item = Self>>(iter: I) -> Self {
235 iter.fold(Self::zero(), |a, b| a + b)
236 }
237}
238
239impl_op_ex!(+ |a: &Vec3, b: &Vec3| -> Vec3 { Vec3::new(a.x + b.x, a.y + b.y, a.z + b.z) });
240impl_op_ex!(-|a: &Vec3, b: &Vec3| -> Vec3 { Vec3::new(a.x - b.x, a.y - b.y, a.z - b.z) });
241impl_op_ex!(-|a: &Vec3| -> Vec3 { Vec3::new(-a.x, -a.y, -a.z) });
242impl_op_ex_commutative!(+ |a: &Vec3, b: &f64| -> Vec3 { Vec3::new(a.x + b, a.y + b, a.z + b) });
243impl_op_ex_commutative!(-|a: &Vec3, b: &f64| -> Vec3 { Vec3::new(a.x - b, a.y - b, a.z - b) });
244impl_op_ex_commutative!(*|a: &Vec3, b: &f64| -> Vec3 { Vec3::new(a.x * b, a.y * b, a.z * b) });
245impl_op_ex!(/ |a: &Vec3, b: &f64| -> Vec3 { Vec3::new(a.x / b, a.y / b, a.z / b) });
246
247#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
258pub struct Vec4 {
259 pub x: f64,
261 pub y: f64,
263 pub z: f64,
265 pub t: f64,
267}
268
269impl Display for Vec4 {
270 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
271 write!(
272 f,
273 "[{:6.3}, {:6.3}, {:6.3}; {:6.3}]",
274 self.x, self.y, self.z, self.t
275 )
276 }
277}
278
279impl AbsDiffEq for Vec4 {
280 type Epsilon = <f64 as approx::AbsDiffEq>::Epsilon;
281
282 fn default_epsilon() -> Self::Epsilon {
283 f64::default_epsilon()
284 }
285
286 fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
287 f64::abs_diff_eq(&self.x, &other.x, epsilon)
288 && f64::abs_diff_eq(&self.y, &other.y, epsilon)
289 && f64::abs_diff_eq(&self.z, &other.z, epsilon)
290 && f64::abs_diff_eq(&self.t, &other.t, epsilon)
291 }
292}
293impl RelativeEq for Vec4 {
294 fn default_max_relative() -> Self::Epsilon {
295 f64::default_max_relative()
296 }
297
298 fn relative_eq(
299 &self,
300 other: &Self,
301 epsilon: Self::Epsilon,
302 max_relative: Self::Epsilon,
303 ) -> bool {
304 f64::relative_eq(&self.x, &other.x, epsilon, max_relative)
305 && f64::relative_eq(&self.y, &other.y, epsilon, max_relative)
306 && f64::relative_eq(&self.z, &other.z, epsilon, max_relative)
307 && f64::relative_eq(&self.t, &other.t, epsilon, max_relative)
308 }
309}
310
311impl From<Vec4> for Vector4<f64> {
312 fn from(value: Vec4) -> Self {
313 Vector4::new(value.x, value.y, value.z, value.t)
314 }
315}
316
317impl From<Vector4<f64>> for Vec4 {
318 fn from(value: Vector4<f64>) -> Self {
319 Vec4::new(value.x, value.y, value.z, value.w)
320 }
321}
322
323impl From<Vec<f64>> for Vec4 {
324 fn from(value: Vec<f64>) -> Self {
325 Self {
326 x: value[0],
327 y: value[1],
328 z: value[2],
329 t: value[3],
330 }
331 }
332}
333
334impl From<Vec4> for Vec<f64> {
335 fn from(value: Vec4) -> Self {
336 vec![value.x, value.y, value.z, value.t]
337 }
338}
339
340impl From<[f64; 4]> for Vec4 {
341 fn from(value: [f64; 4]) -> Self {
342 Self {
343 x: value[0],
344 y: value[1],
345 z: value[2],
346 t: value[3],
347 }
348 }
349}
350
351impl From<Vec4> for [f64; 4] {
352 fn from(value: Vec4) -> Self {
353 [value.x, value.y, value.z, value.t]
354 }
355}
356
357impl Vec4 {
358 pub fn new(x: f64, y: f64, z: f64, t: f64) -> Self {
360 Vec4 { x, y, z, t }
361 }
362
363 pub fn px(&self) -> f64 {
365 self.x
366 }
367
368 pub fn py(&self) -> f64 {
370 self.y
371 }
372
373 pub fn pz(&self) -> f64 {
375 self.z
376 }
377
378 pub fn e(&self) -> f64 {
380 self.t
381 }
382
383 pub fn momentum(&self) -> Vec3 {
385 self.vec3()
386 }
387
388 pub fn gamma(&self) -> f64 {
390 let beta = self.beta();
391 let b2 = beta.dot(&beta);
392 1.0 / f64::sqrt(1.0 - b2)
393 }
394
395 pub fn beta(&self) -> Vec3 {
397 self.momentum() / self.e()
398 }
399
400 pub fn m(&self) -> f64 {
402 self.mag()
403 }
404
405 pub fn m2(&self) -> f64 {
407 self.mag2()
408 }
409
410 pub fn to_p4_string(&self) -> String {
412 format!(
413 "[e = {:.5}; p = ({:.5}, {:.5}, {:.5}); m = {:.5}]",
414 self.e(),
415 self.px(),
416 self.py(),
417 self.pz(),
418 self.m()
419 )
420 }
421
422 pub fn mag(&self) -> f64 {
424 f64::sqrt(self.mag2())
425 }
426
427 pub fn mag2(&self) -> f64 {
429 self.t * self.t - (self.x * self.x + self.y * self.y + self.z * self.z)
430 }
431
432 pub fn boost(&self, beta: &Vec3) -> Self {
434 let b2 = beta.dot(beta);
435 if b2 == 0.0 {
436 return *self;
437 }
438 let gamma = 1.0 / f64::sqrt(1.0 - b2);
439 let p3 = self.vec3() + beta * ((gamma - 1.0) * self.vec3().dot(beta) / b2 + gamma * self.t);
440 Vec4::new(p3.x, p3.y, p3.z, gamma * (self.t + beta.dot(&self.vec3())))
441 }
442
443 pub fn vec3(&self) -> Vec3 {
445 Vec3 {
446 x: self.x,
447 y: self.y,
448 z: self.z,
449 }
450 }
451}
452
453impl_op_ex!(+ |a: &Vec4, b: &Vec4| -> Vec4 { Vec4::new(a.x + b.x, a.y + b.y, a.z + b.z, a.t + b.t) });
454impl_op_ex!(-|a: &Vec4, b: &Vec4| -> Vec4 {
455 Vec4::new(a.x - b.x, a.y - b.y, a.z - b.z, a.t - b.t)
456});
457impl_op_ex!(-|a: &Vec4| -> Vec4 { Vec4::new(-a.x, -a.y, -a.z, a.t) });
458impl_op_ex_commutative!(+ |a: &Vec4, b: &f64| -> Vec4 { Vec4::new(a.x + b, a.y + b, a.z + b, a.t) });
459impl_op_ex_commutative!(-|a: &Vec4, b: &f64| -> Vec4 { Vec4::new(a.x - b, a.y - b, a.z - b, a.t) });
460impl_op_ex_commutative!(*|a: &Vec4, b: &f64| -> Vec4 { Vec4::new(a.x * b, a.y * b, a.z * b, a.t) });
461impl_op_ex!(/ |a: &Vec4, b: &f64| -> Vec4 { Vec4::new(a.x / b, a.y / b, a.z / b, a.t) });
462
463impl<'a> std::iter::Sum<&'a Vec4> for Vec4 {
464 fn sum<I: Iterator<Item = &'a Self>>(iter: I) -> Self {
465 iter.fold(Self::new(0.0, 0.0, 0.0, 0.0), |a, b| a + b)
466 }
467}
468
469impl std::iter::Sum<Vec4> for Vec4 {
470 fn sum<I: Iterator<Item = Self>>(iter: I) -> Self {
471 iter.fold(Self::new(0.0, 0.0, 0.0, 0.0), |a, b| a + b)
472 }
473}
474
475#[cfg(test)]
476mod tests {
477 use approx::{assert_abs_diff_eq, assert_relative_eq};
478 use nalgebra::{Vector3, Vector4};
479
480 use super::*;
481
482 #[test]
483 fn test_display() {
484 let v3 = Vec3::new(1.2341, -2.3452, 3.4563);
485 assert_eq!(format!("{}", v3), "[ 1.234, -2.345, 3.456]");
486 let v4 = Vec4::new(1.2341, -2.3452, 3.4563, 4.5674);
487 assert_eq!(format!("{}", v4), "[ 1.234, -2.345, 3.456; 4.567]");
488 }
489
490 #[test]
491 fn test_vec_vector_conversion() {
492 let v = Vec3::new(1.0, 2.0, 3.0);
493 let vector3: Vec<f64> = v.into();
494 assert_eq!(vector3[0], 1.0);
495 assert_eq!(vector3[1], 2.0);
496 assert_eq!(vector3[2], 3.0);
497
498 let v_from_vec: Vec3 = vector3.into();
499 assert_eq!(v_from_vec, v);
500
501 let v = Vec4::new(1.0, 2.0, 3.0, 4.0);
502 let vector4: Vec<f64> = v.into();
503 assert_eq!(vector4[0], 1.0);
504 assert_eq!(vector4[1], 2.0);
505 assert_eq!(vector4[2], 3.0);
506 assert_eq!(vector4[3], 4.0);
507
508 let v_from_vec: Vec4 = vector4.into();
509 assert_eq!(v_from_vec, v);
510 }
511
512 #[test]
513 fn test_vec_array_conversion() {
514 let arr = [1.0, 2.0, 3.0];
515 let v: Vec3 = arr.into();
516 assert_eq!(v, Vec3::new(1.0, 2.0, 3.0));
517
518 let back_to_array: [f64; 3] = v.into();
519 assert_eq!(back_to_array, arr);
520
521 let arr = [1.0, 2.0, 3.0, 4.0];
522 let v: Vec4 = arr.into();
523 assert_eq!(v, Vec4::new(1.0, 2.0, 3.0, 4.0));
524
525 let back_to_array: [f64; 4] = v.into();
526 assert_eq!(back_to_array, arr);
527 }
528
529 #[test]
530 fn test_vec_nalgebra_conversion() {
531 let v = Vec3::new(1.0, 2.0, 3.0);
532 let vector3: Vector3<f64> = v.into();
533 assert_eq!(vector3.x, 1.0);
534 assert_eq!(vector3.y, 2.0);
535 assert_eq!(vector3.z, 3.0);
536
537 let v_from_vec: Vec3 = vector3.into();
538 assert_eq!(v_from_vec, v);
539
540 let v = Vec4::new(1.0, 2.0, 3.0, 4.0);
541 let vector4: Vector4<f64> = v.into();
542 assert_eq!(vector4.x, 1.0);
543 assert_eq!(vector4.y, 2.0);
544 assert_eq!(vector4.z, 3.0);
545 assert_eq!(vector4.w, 4.0);
546
547 let v_from_vec: Vec4 = vector4.into();
548 assert_eq!(v_from_vec, v);
549 }
550
551 #[test]
552 fn test_vec_sums() {
553 let vectors = [Vec3::new(1.0, 2.0, 3.0), Vec3::new(4.0, 5.0, 6.0)];
554 let sum: Vec3 = vectors.iter().sum();
555 assert_eq!(sum, Vec3::new(5.0, 7.0, 9.0));
556 let sum: Vec3 = vectors.into_iter().sum();
557 assert_eq!(sum, Vec3::new(5.0, 7.0, 9.0));
558
559 let vectors = [Vec4::new(1.0, 2.0, 3.0, 4.0), Vec4::new(4.0, 5.0, 6.0, 7.0)];
560 let sum: Vec4 = vectors.iter().sum();
561 assert_eq!(sum, Vec4::new(5.0, 7.0, 9.0, 11.0));
562 let sum: Vec4 = vectors.into_iter().sum();
563 assert_eq!(sum, Vec4::new(5.0, 7.0, 9.0, 11.0));
564 }
565
566 #[test]
567 fn test_three_to_four_momentum_conversion() {
568 let p3 = Vec3::new(1.0, 2.0, 3.0);
569 let target_p4 = Vec4::new(1.0, 2.0, 3.0, 10.0);
570 let p4_from_mass = p3.with_mass(target_p4.m());
571 assert_eq!(target_p4.e(), p4_from_mass.e());
572 assert_eq!(target_p4.px(), p4_from_mass.px());
573 assert_eq!(target_p4.py(), p4_from_mass.py());
574 assert_eq!(target_p4.pz(), p4_from_mass.pz());
575 let p4_from_energy = p3.with_energy(target_p4.e());
576 assert_eq!(target_p4.e(), p4_from_energy.e());
577 assert_eq!(target_p4.px(), p4_from_energy.px());
578 assert_eq!(target_p4.py(), p4_from_energy.py());
579 assert_eq!(target_p4.pz(), p4_from_energy.pz());
580 }
581
582 #[test]
583 fn test_four_momentum_basics() {
584 let p = Vec4::new(3.0, 4.0, 5.0, 10.0);
585 assert_eq!(p.e(), 10.0);
586 assert_eq!(p.px(), 3.0);
587 assert_eq!(p.py(), 4.0);
588 assert_eq!(p.pz(), 5.0);
589 assert_eq!(p.momentum().px(), 3.0);
590 assert_eq!(p.momentum().py(), 4.0);
591 assert_eq!(p.momentum().pz(), 5.0);
592 assert_relative_eq!(p.beta().x, 0.3);
593 assert_relative_eq!(p.beta().y, 0.4);
594 assert_relative_eq!(p.beta().z, 0.5);
595 assert_relative_eq!(p.m2(), 50.0);
596 assert_relative_eq!(p.m(), f64::sqrt(50.0));
597 assert_eq!(
598 format!("{}", p.to_p4_string()),
599 "[e = 10.00000; p = (3.00000, 4.00000, 5.00000); m = 7.07107]"
600 );
601 assert_relative_eq!(Vec3::x().x, 1.0);
602 assert_relative_eq!(Vec3::x().y, 0.0);
603 assert_relative_eq!(Vec3::x().z, 0.0);
604 assert_relative_eq!(Vec3::y().x, 0.0);
605 assert_relative_eq!(Vec3::y().y, 1.0);
606 assert_relative_eq!(Vec3::y().z, 0.0);
607 assert_relative_eq!(Vec3::z().x, 0.0);
608 assert_relative_eq!(Vec3::z().y, 0.0);
609 assert_relative_eq!(Vec3::z().z, 1.0);
610 assert_relative_eq!(Vec3::default().x, 0.0);
611 assert_relative_eq!(Vec3::default().y, 0.0);
612 assert_relative_eq!(Vec3::default().z, 0.0);
613 }
614
615 #[test]
616 fn test_three_momentum_basics() {
617 let p = Vec4::new(3.0, 4.0, 5.0, 10.0);
618 let q = Vec4::new(1.2, -3.4, 7.6, 0.0);
619 let p3_view = p.momentum();
620 let q3_view = q.momentum();
621 assert_eq!(p3_view.px(), 3.0);
622 assert_eq!(p3_view.py(), 4.0);
623 assert_eq!(p3_view.pz(), 5.0);
624 assert_relative_eq!(p3_view.mag2(), 50.0);
625 assert_relative_eq!(p3_view.mag(), f64::sqrt(50.0));
626 assert_relative_eq!(p3_view.costheta(), 5.0 / f64::sqrt(50.0));
627 assert_relative_eq!(p3_view.theta(), f64::acos(5.0 / f64::sqrt(50.0)));
628 assert_relative_eq!(p3_view.phi(), f64::atan2(4.0, 3.0));
629 assert_relative_eq!(
630 p3_view.unit(),
631 Vec3::new(
632 3.0 / f64::sqrt(50.0),
633 4.0 / f64::sqrt(50.0),
634 5.0 / f64::sqrt(50.0)
635 )
636 );
637 assert_relative_eq!(p3_view.cross(&q3_view), Vec3::new(47.4, -16.8, -15.0));
638 }
639
640 #[test]
641 fn test_vec_equality() {
642 let p = Vec3::new(1.1, 2.2, 3.3);
643 let p2 = Vec3::new(1.1 * 2.0, 2.2 * 2.0, 3.3 * 2.0);
644 assert_abs_diff_eq!(p * 2.0, p2);
645 assert_relative_eq!(p * 2.0, p2);
646 let p = Vec4::new(1.1, 2.2, 3.3, 10.0);
647 let p2 = Vec4::new(1.1 * 2.0, 2.2 * 2.0, 3.3 * 2.0, 10.0);
648 assert_abs_diff_eq!(p * 2.0, p2);
649 assert_relative_eq!(p * 2.0, p2);
650 }
651
652 #[test]
653 fn test_boost_com() {
654 let p = Vec4::new(3.0, 4.0, 5.0, 10.0);
655 let zero = p.boost(&-p.beta()).momentum();
656 assert_relative_eq!(zero, Vec3::zero());
657 }
658
659 #[test]
660 fn test_boost() {
661 let p0 = Vec4::new(0.0, 0.0, 0.0, 1.0);
662 assert_relative_eq!(p0.gamma(), 1.0);
663 let p0 = Vec4::new(f64::sqrt(3.0) / 2.0, 0.0, 0.0, 1.0);
664 assert_relative_eq!(p0.gamma(), 2.0);
665 let p1 = Vec4::new(3.0, 4.0, 5.0, 10.0);
666 let p2 = Vec4::new(3.4, 2.3, 1.2, 9.0);
667 let p1_boosted = p1.boost(&-p2.beta());
668 assert_relative_eq!(p1_boosted.e(), 8.157632144622882);
669 assert_relative_eq!(p1_boosted.px(), -0.6489200627053444);
670 assert_relative_eq!(p1_boosted.py(), 1.5316128987581492);
671 assert_relative_eq!(p1_boosted.pz(), 3.712145860221643);
672 }
673}