oximedia_virtual/math/
vector.rs

1//! Vector and point types for 3D geometry.
2
3use serde::{Deserialize, Serialize};
4use std::ops::{
5    Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign,
6};
7
8// ---------------------------------------------------------------------------
9// Point2
10// ---------------------------------------------------------------------------
11
12/// A 2D point.
13#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
14pub struct Point2<T> {
15    pub x: T,
16    pub y: T,
17}
18
19impl<T: Default> Point2<T> {
20    /// Origin (0, 0).
21    #[must_use]
22    pub fn origin() -> Self {
23        Self {
24            x: T::default(),
25            y: T::default(),
26        }
27    }
28}
29
30impl<T> Point2<T> {
31    /// Construct from components.
32    #[must_use]
33    pub fn new(x: T, y: T) -> Self {
34        Self { x, y }
35    }
36}
37
38// ---------------------------------------------------------------------------
39// Vector3
40// ---------------------------------------------------------------------------
41
42/// A 3-component vector.
43#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
44pub struct Vector3<T> {
45    pub x: T,
46    pub y: T,
47    pub z: T,
48}
49
50impl<T: Default> Vector3<T> {
51    /// Zero vector.
52    #[must_use]
53    pub fn zeros() -> Self {
54        Self {
55            x: T::default(),
56            y: T::default(),
57            z: T::default(),
58        }
59    }
60}
61
62impl<T> Vector3<T> {
63    /// Construct from components.
64    #[must_use]
65    pub fn new(x: T, y: T, z: T) -> Self {
66        Self { x, y, z }
67    }
68}
69
70impl Vector3<f64> {
71    /// Euclidean norm.
72    #[must_use]
73    pub fn norm(&self) -> f64 {
74        (self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
75    }
76
77    /// Normalise to unit length.  Returns zero vector if norm is near zero.
78    #[must_use]
79    pub fn normalize(&self) -> Self {
80        let n = self.norm();
81        if n < 1e-15 {
82            Self::zeros()
83        } else {
84            Self::new(self.x / n, self.y / n, self.z / n)
85        }
86    }
87
88    /// Dot product.
89    #[must_use]
90    pub fn dot(&self, rhs: &Self) -> f64 {
91        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
92    }
93
94    /// Cross product.
95    #[must_use]
96    pub fn cross(&self, rhs: &Self) -> Self {
97        Self::new(
98            self.y * rhs.z - self.z * rhs.y,
99            self.z * rhs.x - self.x * rhs.z,
100            self.x * rhs.y - self.y * rhs.x,
101        )
102    }
103
104    /// Outer product (self * rhs^T), yielding a 3x3 matrix.
105    #[must_use]
106    pub fn outer(&self, rhs: &Self) -> super::Matrix3<f64> {
107        let mut m = super::Matrix3::zeros();
108        m.data[0][0] = self.x * rhs.x;
109        m.data[0][1] = self.x * rhs.y;
110        m.data[0][2] = self.x * rhs.z;
111        m.data[1][0] = self.y * rhs.x;
112        m.data[1][1] = self.y * rhs.y;
113        m.data[1][2] = self.y * rhs.z;
114        m.data[2][0] = self.z * rhs.x;
115        m.data[2][1] = self.z * rhs.y;
116        m.data[2][2] = self.z * rhs.z;
117        m
118    }
119
120    /// Return the 1x3 transpose as a `TransposedVector3` for nalgebra-like
121    /// `centered * world_centered.transpose()` patterns producing a Matrix3.
122    #[must_use]
123    pub fn transpose(&self) -> TransposedVector3 {
124        TransposedVector3(*self)
125    }
126
127    /// Scale in place (matches nalgebra's `scale_mut` on vector views).
128    pub fn scale_mut(&mut self, s: f64) {
129        self.x *= s;
130        self.y *= s;
131        self.z *= s;
132    }
133}
134
135/// A transposed 3-vector (row vector) used for outer-product patterns.
136#[derive(Debug, Clone, Copy)]
137pub struct TransposedVector3(pub Vector3<f64>);
138
139/// `Vector3 * TransposedVector3 → Matrix3` (outer product).
140impl Mul<TransposedVector3> for Vector3<f64> {
141    type Output = super::Matrix3<f64>;
142    fn mul(self, rhs: TransposedVector3) -> Self::Output {
143        self.outer(&rhs.0)
144    }
145}
146
147// -- Arithmetic impls for Vector3<f64> --
148
149impl Add for Vector3<f64> {
150    type Output = Self;
151    fn add(self, rhs: Self) -> Self {
152        Self::new(self.x + rhs.x, self.y + rhs.y, self.z + rhs.z)
153    }
154}
155
156impl Sub for Vector3<f64> {
157    type Output = Self;
158    fn sub(self, rhs: Self) -> Self {
159        Self::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z)
160    }
161}
162
163impl Neg for Vector3<f64> {
164    type Output = Self;
165    fn neg(self) -> Self {
166        Self::new(-self.x, -self.y, -self.z)
167    }
168}
169
170impl Mul<f64> for Vector3<f64> {
171    type Output = Self;
172    fn mul(self, rhs: f64) -> Self {
173        Self::new(self.x * rhs, self.y * rhs, self.z * rhs)
174    }
175}
176
177impl Mul<Vector3<f64>> for f64 {
178    type Output = Vector3<f64>;
179    fn mul(self, rhs: Vector3<f64>) -> Vector3<f64> {
180        Vector3::new(self * rhs.x, self * rhs.y, self * rhs.z)
181    }
182}
183
184impl Div<f64> for Vector3<f64> {
185    type Output = Self;
186    fn div(self, rhs: f64) -> Self {
187        Self::new(self.x / rhs, self.y / rhs, self.z / rhs)
188    }
189}
190
191impl AddAssign for Vector3<f64> {
192    fn add_assign(&mut self, rhs: Self) {
193        self.x += rhs.x;
194        self.y += rhs.y;
195        self.z += rhs.z;
196    }
197}
198
199impl SubAssign for Vector3<f64> {
200    fn sub_assign(&mut self, rhs: Self) {
201        self.x -= rhs.x;
202        self.y -= rhs.y;
203        self.z -= rhs.z;
204    }
205}
206
207impl MulAssign<f64> for Vector3<f64> {
208    fn mul_assign(&mut self, rhs: f64) {
209        self.x *= rhs;
210        self.y *= rhs;
211        self.z *= rhs;
212    }
213}
214
215impl DivAssign<f64> for Vector3<f64> {
216    fn div_assign(&mut self, rhs: f64) {
217        self.x /= rhs;
218        self.y /= rhs;
219        self.z /= rhs;
220    }
221}
222
223// ---------------------------------------------------------------------------
224// Point3
225// ---------------------------------------------------------------------------
226
227/// A 3D point (distinct from Vector3 to model affine semantics).
228#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
229pub struct Point3<T> {
230    pub x: T,
231    pub y: T,
232    pub z: T,
233}
234
235impl<T: Default> Point3<T> {
236    /// Origin point.
237    #[must_use]
238    pub fn origin() -> Self {
239        Self {
240            x: T::default(),
241            y: T::default(),
242            z: T::default(),
243        }
244    }
245}
246
247impl<T> Point3<T> {
248    /// Construct from components.
249    #[must_use]
250    pub fn new(x: T, y: T, z: T) -> Self {
251        Self { x, y, z }
252    }
253}
254
255impl Point3<f64> {
256    /// Coords as a Vector3.
257    #[must_use]
258    pub fn coords(&self) -> Vector3<f64> {
259        Vector3::new(self.x, self.y, self.z)
260    }
261
262    /// Construct from a Vector3.
263    #[must_use]
264    pub fn from_vec(v: Vector3<f64>) -> Self {
265        Self::new(v.x, v.y, v.z)
266    }
267
268    /// Convert to homogeneous coordinates (4-element column vector).
269    #[must_use]
270    pub fn to_homogeneous(&self) -> [f64; 4] {
271        [self.x, self.y, self.z, 1.0]
272    }
273
274    /// Create from homogeneous coordinates.  Returns `None` if w is near zero.
275    #[must_use]
276    pub fn from_homogeneous(h: [f64; 4]) -> Option<Self> {
277        if h[3].abs() < 1e-15 {
278            None
279        } else {
280            Some(Self::new(h[0] / h[3], h[1] / h[3], h[2] / h[3]))
281        }
282    }
283}
284
285/// `Point3 - Point3 → Vector3`
286impl Sub for Point3<f64> {
287    type Output = Vector3<f64>;
288    fn sub(self, rhs: Self) -> Vector3<f64> {
289        Vector3::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z)
290    }
291}
292
293/// `&Point3 - &Point3 → Vector3`
294impl<'a, 'b> Sub<&'b Point3<f64>> for &'a Point3<f64> {
295    type Output = Vector3<f64>;
296    fn sub(self, rhs: &'b Point3<f64>) -> Vector3<f64> {
297        Vector3::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z)
298    }
299}
300
301/// `Point3 + Vector3 → Point3`
302impl Add<Vector3<f64>> for Point3<f64> {
303    type Output = Point3<f64>;
304    fn add(self, rhs: Vector3<f64>) -> Self {
305        Self::new(self.x + rhs.x, self.y + rhs.y, self.z + rhs.z)
306    }
307}
308
309/// `Point3 += Vector3`
310impl AddAssign<Vector3<f64>> for Point3<f64> {
311    fn add_assign(&mut self, rhs: Vector3<f64>) {
312        self.x += rhs.x;
313        self.y += rhs.y;
314        self.z += rhs.z;
315    }
316}
317
318/// `Point3 - Vector3 → Point3`
319impl Sub<Vector3<f64>> for Point3<f64> {
320    type Output = Point3<f64>;
321    fn sub(self, rhs: Vector3<f64>) -> Self {
322        Self::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z)
323    }
324}
325
326/// `Point3 * f64` — scale coordinates (matches nalgebra pattern).
327impl Mul<f64> for Point3<f64> {
328    type Output = Vector3<f64>;
329    fn mul(self, rhs: f64) -> Vector3<f64> {
330        Vector3::new(self.x * rhs, self.y * rhs, self.z * rhs)
331    }
332}
333
334/// Construct Point3 from Vector3 (via From trait).
335impl From<Vector3<f64>> for Point3<f64> {
336    fn from(v: Vector3<f64>) -> Self {
337        Self::new(v.x, v.y, v.z)
338    }
339}
340
341// ---------------------------------------------------------------------------
342// Vector6
343// ---------------------------------------------------------------------------
344
345/// A 6-component vector (used for Kalman state: [pos_x, pos_y, pos_z, vel_x, vel_y, vel_z]).
346#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
347pub struct Vector6<T> {
348    pub data: [T; 6],
349}
350
351impl<T: Default + Copy> Vector6<T> {
352    /// Zero vector.
353    #[must_use]
354    pub fn zeros() -> Self {
355        Self {
356            data: [T::default(); 6],
357        }
358    }
359}
360
361impl<T> Index<usize> for Vector6<T> {
362    type Output = T;
363    fn index(&self, i: usize) -> &T {
364        &self.data[i]
365    }
366}
367
368impl<T> IndexMut<usize> for Vector6<T> {
369    fn index_mut(&mut self, i: usize) -> &mut T {
370        &mut self.data[i]
371    }
372}
373
374impl Add for Vector6<f64> {
375    type Output = Self;
376    fn add(self, rhs: Self) -> Self {
377        let mut out = self;
378        for i in 0..6 {
379            out.data[i] += rhs.data[i];
380        }
381        out
382    }
383}
384
385impl Sub for Vector6<f64> {
386    type Output = Self;
387    fn sub(self, rhs: Self) -> Self {
388        let mut out = self;
389        for i in 0..6 {
390            out.data[i] -= rhs.data[i];
391        }
392        out
393    }
394}
395
396impl AddAssign for Vector6<f64> {
397    fn add_assign(&mut self, rhs: Self) {
398        for i in 0..6 {
399            self.data[i] += rhs.data[i];
400        }
401    }
402}
403
404// ---------------------------------------------------------------------------
405// Tests
406// ---------------------------------------------------------------------------
407
408#[cfg(test)]
409mod tests {
410    use super::*;
411
412    #[test]
413    fn test_vector3_norm() {
414        let v = Vector3::new(3.0, 4.0, 0.0);
415        assert!((v.norm() - 5.0).abs() < 1e-10);
416    }
417
418    #[test]
419    fn test_vector3_normalize() {
420        let v = Vector3::new(0.0, 0.0, 5.0);
421        let n = v.normalize();
422        assert!((n.z - 1.0).abs() < 1e-10);
423    }
424
425    #[test]
426    fn test_vector3_cross() {
427        let a = Vector3::new(1.0, 0.0, 0.0);
428        let b = Vector3::new(0.0, 1.0, 0.0);
429        let c = a.cross(&b);
430        assert!((c.z - 1.0).abs() < 1e-10);
431    }
432
433    #[test]
434    fn test_vector3_dot() {
435        let a = Vector3::new(1.0, 2.0, 3.0);
436        let b = Vector3::new(4.0, 5.0, 6.0);
437        assert!((a.dot(&b) - 32.0).abs() < 1e-10);
438    }
439
440    #[test]
441    fn test_point3_sub() {
442        let a = Point3::new(3.0, 4.0, 5.0);
443        let b = Point3::new(1.0, 1.0, 1.0);
444        let v = a - b;
445        assert!((v.x - 2.0).abs() < 1e-10);
446    }
447
448    #[test]
449    fn test_point3_add_vector() {
450        let p = Point3::new(1.0, 2.0, 3.0);
451        let v = Vector3::new(10.0, 20.0, 30.0);
452        let q = p + v;
453        assert!((q.x - 11.0).abs() < 1e-10);
454    }
455
456    #[test]
457    fn test_point3_homogeneous_roundtrip() {
458        let p = Point3::new(1.0, 2.0, 3.0);
459        let h = p.to_homogeneous();
460        let p2 = Point3::from_homogeneous(h).expect("should succeed in test");
461        assert!((p.x - p2.x).abs() < 1e-10);
462    }
463
464    #[test]
465    fn test_vector6_indexing() {
466        let mut v: Vector6<f64> = Vector6::zeros();
467        v[3] = 42.0;
468        assert!((v[3] - 42.0).abs() < 1e-10);
469    }
470}
oximedia_virtual/math/vector.rs

oximedia_virtual/math/
vector.rs
