sidereon_core/astro/math/
vec3.rs

1//! Small fixed-size 3D vector helpers.
2//!
3//! These helpers intentionally keep simple, explicit operation order. Callers
4//! that need a parity-specific order should use the named variants rather than
5//! copy-pasting a local helper.
6
7/// Error returned by checked 3D vector helpers.
8#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
9pub enum Vec3Error {
10    /// A vector input or output contained NaN or infinity.
11    #[error("invalid vec3 {field}: {reason}")]
12    InvalidInput {
13        field: &'static str,
14        reason: &'static str,
15    },
16}
17
18/// Add two finite 3D vectors.
19///
20/// This infallible primitive is intended for internal parity-sensitive math
21/// after public callers have validated inputs. Use [`checked_add3`] at public
22/// boundaries or fuzz entry points.
23#[inline]
24pub fn add3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
25    debug_assert!(finite3(&a));
26    debug_assert!(finite3(&b));
27    let out = [a[0] + b[0], a[1] + b[1], a[2] + b[2]];
28    debug_assert!(finite3(&out));
29    out
30}
31
32/// Checked addition for public/fuzz entry points.
33#[inline]
34pub fn checked_add3(a: [f64; 3], b: [f64; 3]) -> Result<[f64; 3], Vec3Error> {
35    validate_finite3(&a, "a")?;
36    validate_finite3(&b, "b")?;
37    let out = [a[0] + b[0], a[1] + b[1], a[2] + b[2]];
38    validate_finite3(&out, "sum")?;
39    Ok(out)
40}
41
42#[inline]
43pub fn sub3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
44    [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
45}
46
47#[inline]
48pub fn neg3(v: [f64; 3]) -> [f64; 3] {
49    [-v[0], -v[1], -v[2]]
50}
51
52#[inline]
53pub fn scale3(v: [f64; 3], s: f64) -> [f64; 3] {
54    [v[0] * s, v[1] * s, v[2] * s]
55}
56
57#[inline]
58pub fn dot3(a: [f64; 3], b: [f64; 3]) -> f64 {
59    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
60}
61
62#[inline]
63pub fn dot3_ref(a: &[f64; 3], b: &[f64; 3]) -> f64 {
64    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
65}
66
67#[inline]
68pub fn dot3_z_yx_ref(a: &[f64; 3], b: &[f64; 3]) -> f64 {
69    a[2] * b[2] + (a[1] * b[1] + a[0] * b[0])
70}
71
72#[inline]
73pub fn dot3_fused_z_yx_ref(a: &[f64; 3], b: &[f64; 3]) -> f64 {
74    a[2].mul_add(b[2], a[1].mul_add(b[1], a[0] * b[0]))
75}
76
77#[inline]
78pub fn norm3(v: [f64; 3]) -> f64 {
79    dot3(v, v).sqrt()
80}
81
82#[inline]
83pub fn norm3_ref(v: &[f64; 3]) -> f64 {
84    dot3_ref(v, v).sqrt()
85}
86
87#[inline]
88pub fn unit3(v: [f64; 3]) -> Option<[f64; 3]> {
89    match norm3(v) {
90        n if n > 0.0 => Some(scale3(v, 1.0 / n)),
91        _ => None,
92    }
93}
94
95#[inline]
96pub fn unit3_ref_unchecked(v: &[f64; 3]) -> [f64; 3] {
97    let n = norm3_ref(v);
98    [v[0] / n, v[1] / n, v[2] / n]
99}
100
101#[inline]
102pub fn cross3(a: [f64; 3], b: [f64; 3]) -> [f64; 3] {
103    [
104        a[1] * b[2] - a[2] * b[1],
105        a[2] * b[0] - a[0] * b[2],
106        a[0] * b[1] - a[1] * b[0],
107    ]
108}
109
110#[inline]
111pub fn cross3_ref(a: &[f64; 3], b: &[f64; 3]) -> [f64; 3] {
112    [
113        a[1] * b[2] - a[2] * b[1],
114        a[2] * b[0] - a[0] * b[2],
115        a[0] * b[1] - a[1] * b[0],
116    ]
117}
118
119#[inline]
120fn finite3(v: &[f64; 3]) -> bool {
121    v.iter().all(|value| value.is_finite())
122}
123
124#[inline]
125fn validate_finite3(v: &[f64; 3], field: &'static str) -> Result<(), Vec3Error> {
126    if finite3(v) {
127        Ok(())
128    } else {
129        Err(Vec3Error::InvalidInput {
130            field,
131            reason: "not finite",
132        })
133    }
134}
135
136#[cfg(test)]
137mod tests {
138    use super::*;
139
140    #[test]
141    fn named_dot_orders_match_their_explicit_recipes() {
142        let a = [1.25, -2.5, 3.75];
143        let b = [-4.0, 5.5, -6.25];
144
145        assert_eq!(
146            dot3(a, b).to_bits(),
147            (a[0] * b[0] + a[1] * b[1] + a[2] * b[2]).to_bits()
148        );
149        assert_eq!(
150            dot3_z_yx_ref(&a, &b).to_bits(),
151            (a[2] * b[2] + (a[1] * b[1] + a[0] * b[0])).to_bits()
152        );
153        assert_eq!(
154            dot3_fused_z_yx_ref(&a, &b).to_bits(),
155            a[2].mul_add(b[2], a[1].mul_add(b[1], a[0] * b[0]))
156                .to_bits()
157        );
158    }
159
160    #[test]
161    fn unit3_zero_vector_returns_none() {
162        assert_eq!(unit3([0.0, 0.0, 0.0]), None);
163    }
164
165    #[test]
166    fn checked_add3_rejects_non_finite_inputs_and_outputs() {
167        assert_eq!(
168            checked_add3([f64::NAN, 0.0, 0.0], [1.0, 2.0, 3.0]),
169            Err(Vec3Error::InvalidInput {
170                field: "a",
171                reason: "not finite"
172            })
173        );
174        assert_eq!(
175            checked_add3([1.0, 2.0, 3.0], [f64::INFINITY, 0.0, 0.0]),
176            Err(Vec3Error::InvalidInput {
177                field: "b",
178                reason: "not finite"
179            })
180        );
181        assert_eq!(
182            checked_add3([f64::MAX, 0.0, 0.0], [f64::MAX, 0.0, 0.0]),
183            Err(Vec3Error::InvalidInput {
184                field: "sum",
185                reason: "not finite"
186            })
187        );
188    }
189}
sidereon_core/astro/math/vec3.rs

sidereon_core/astro/math/
vec3.rs
