space_dust/
math.rs

1//! Mathematical operations for astrodynamics calculations.
2//!
3//! This module provides 3D vector and 3x3 matrix types optimized for
4//! coordinate transformations and orbital mechanics calculations.
5
6use std::ops::{Add, Div, Mul, Neg, Sub};
7
8// ============================================================================
9// Vector3 - 3D Vector
10// ============================================================================
11
12/// A 3D vector for position, velocity, and other spatial quantities.
13#[derive(Debug, Clone, Copy, PartialEq)]
14pub struct Vector3 {
15    /// X component
16    pub x: f64,
17    /// Y component
18    pub y: f64,
19    /// Z component
20    pub z: f64,
21}
22
23impl Vector3 {
24    /// Create a new Vector3.
25    #[inline]
26    pub fn new(x: f64, y: f64, z: f64) -> Self {
27        Self { x, y, z }
28    }
29
30    /// Create a zero vector.
31    #[inline]
32    pub fn zero() -> Self {
33        Self::new(0.0, 0.0, 0.0)
34    }
35
36    /// Create a unit vector along the X axis.
37    #[inline]
38    pub fn unit_x() -> Self {
39        Self::new(1.0, 0.0, 0.0)
40    }
41
42    /// Create a unit vector along the Y axis.
43    #[inline]
44    pub fn unit_y() -> Self {
45        Self::new(0.0, 1.0, 0.0)
46    }
47
48    /// Create a unit vector along the Z axis.
49    #[inline]
50    pub fn unit_z() -> Self {
51        Self::new(0.0, 0.0, 1.0)
52    }
53
54    /// Create a Vector3 from an array.
55    #[inline]
56    pub fn from_array(arr: [f64; 3]) -> Self {
57        Self::new(arr[0], arr[1], arr[2])
58    }
59
60    /// Create a Vector3 from a tuple.
61    #[inline]
62    pub fn from_tuple(t: (f64, f64, f64)) -> Self {
63        Self::new(t.0, t.1, t.2)
64    }
65
66    /// Convert to an array.
67    #[inline]
68    pub fn to_array(&self) -> [f64; 3] {
69        [self.x, self.y, self.z]
70    }
71
72    /// Convert to a tuple.
73    #[inline]
74    pub fn to_tuple(&self) -> (f64, f64, f64) {
75        (self.x, self.y, self.z)
76    }
77
78    /// Compute the magnitude (length) of the vector.
79    #[inline]
80    pub fn magnitude(&self) -> f64 {
81        (self.x * self.x + self.y * self.y + self.z * self.z).sqrt()
82    }
83
84    /// Compute the squared magnitude (avoids sqrt).
85    #[inline]
86    pub fn magnitude_squared(&self) -> f64 {
87        self.x * self.x + self.y * self.y + self.z * self.z
88    }
89
90    /// Normalize the vector to unit length.
91    /// Returns a zero vector if the magnitude is zero.
92    #[inline]
93    pub fn normalize(&self) -> Self {
94        let mag = self.magnitude();
95        if mag > 0.0 {
96            Self::new(self.x / mag, self.y / mag, self.z / mag)
97        } else {
98            Self::zero()
99        }
100    }
101
102    /// Compute the dot product with another vector.
103    #[inline]
104    pub fn dot(&self, other: &Vector3) -> f64 {
105        self.x * other.x + self.y * other.y + self.z * other.z
106    }
107
108    /// Compute the cross product with another vector.
109    #[inline]
110    pub fn cross(&self, other: &Vector3) -> Self {
111        Self::new(
112            self.y * other.z - self.z * other.y,
113            self.z * other.x - self.x * other.z,
114            self.x * other.y - self.y * other.x,
115        )
116    }
117
118    /// Compute the angle between two vectors in radians.
119    #[inline]
120    pub fn angle(&self, other: &Vector3) -> f64 {
121        let dot = self.dot(other);
122        let mag_product = self.magnitude() * other.magnitude();
123        if mag_product > 0.0 {
124            (dot / mag_product).clamp(-1.0, 1.0).acos()
125        } else {
126            0.0
127        }
128    }
129
130    /// Scale the vector by a scalar.
131    #[inline]
132    pub fn scale(&self, scalar: f64) -> Self {
133        Self::new(self.x * scalar, self.y * scalar, self.z * scalar)
134    }
135
136    /// Rotate the vector around an arbitrary axis by an angle (radians).
137    pub fn rotate_around_axis(&self, axis: &Vector3, angle: f64) -> Self {
138        let axis = axis.normalize();
139        let c = angle.cos();
140        let s = angle.sin();
141        let t = 1.0 - c;
142
143        let x = (t * axis.x * axis.x + c) * self.x
144            + (t * axis.x * axis.y - s * axis.z) * self.y
145            + (t * axis.x * axis.z + s * axis.y) * self.z;
146
147        let y = (t * axis.x * axis.y + s * axis.z) * self.x
148            + (t * axis.y * axis.y + c) * self.y
149            + (t * axis.y * axis.z - s * axis.x) * self.z;
150
151        let z = (t * axis.x * axis.z - s * axis.y) * self.x
152            + (t * axis.y * axis.z + s * axis.x) * self.y
153            + (t * axis.z * axis.z + c) * self.z;
154
155        Self::new(x, y, z)
156    }
157
158    /// Rotate the vector around the X axis.
159    #[inline]
160    pub fn rotate_x(&self, angle: f64) -> Self {
161        let c = angle.cos();
162        let s = angle.sin();
163        Self::new(self.x, c * self.y - s * self.z, s * self.y + c * self.z)
164    }
165
166    /// Rotate the vector around the Y axis.
167    #[inline]
168    pub fn rotate_y(&self, angle: f64) -> Self {
169        let c = angle.cos();
170        let s = angle.sin();
171        Self::new(c * self.x + s * self.z, self.y, -s * self.x + c * self.z)
172    }
173
174    /// Rotate the vector around the Z axis.
175    #[inline]
176    pub fn rotate_z(&self, angle: f64) -> Self {
177        let c = angle.cos();
178        let s = angle.sin();
179        Self::new(c * self.x - s * self.y, s * self.x + c * self.y, self.z)
180    }
181
182    /// Linear interpolation between two vectors.
183    #[inline]
184    pub fn lerp(&self, other: &Vector3, t: f64) -> Self {
185        Self::new(
186            self.x + t * (other.x - self.x),
187            self.y + t * (other.y - self.y),
188            self.z + t * (other.z - self.z),
189        )
190    }
191
192    /// Check if the vector is approximately zero.
193    #[inline]
194    pub fn is_zero(&self, epsilon: f64) -> bool {
195        self.magnitude_squared() < epsilon * epsilon
196    }
197
198    /// Check if two vectors are approximately equal.
199    #[inline]
200    pub fn approx_eq(&self, other: &Vector3, epsilon: f64) -> bool {
201        (self.x - other.x).abs() < epsilon
202            && (self.y - other.y).abs() < epsilon
203            && (self.z - other.z).abs() < epsilon
204    }
205}
206
207impl Default for Vector3 {
208    fn default() -> Self {
209        Self::zero()
210    }
211}
212
213impl Add for Vector3 {
214    type Output = Self;
215
216    #[inline]
217    fn add(self, other: Self) -> Self {
218        Self::new(self.x + other.x, self.y + other.y, self.z + other.z)
219    }
220}
221
222impl Sub for Vector3 {
223    type Output = Self;
224
225    #[inline]
226    fn sub(self, other: Self) -> Self {
227        Self::new(self.x - other.x, self.y - other.y, self.z - other.z)
228    }
229}
230
231impl Neg for Vector3 {
232    type Output = Self;
233
234    #[inline]
235    fn neg(self) -> Self {
236        Self::new(-self.x, -self.y, -self.z)
237    }
238}
239
240impl Mul<f64> for Vector3 {
241    type Output = Self;
242
243    #[inline]
244    fn mul(self, scalar: f64) -> Self {
245        Self::new(self.x * scalar, self.y * scalar, self.z * scalar)
246    }
247}
248
249impl Mul<Vector3> for f64 {
250    type Output = Vector3;
251
252    #[inline]
253    fn mul(self, vec: Vector3) -> Vector3 {
254        Vector3::new(self * vec.x, self * vec.y, self * vec.z)
255    }
256}
257
258impl Div<f64> for Vector3 {
259    type Output = Self;
260
261    #[inline]
262    fn div(self, scalar: f64) -> Self {
263        Self::new(self.x / scalar, self.y / scalar, self.z / scalar)
264    }
265}
266
267impl From<[f64; 3]> for Vector3 {
268    fn from(arr: [f64; 3]) -> Self {
269        Self::from_array(arr)
270    }
271}
272
273impl From<(f64, f64, f64)> for Vector3 {
274    fn from(t: (f64, f64, f64)) -> Self {
275        Self::from_tuple(t)
276    }
277}
278
279impl From<Vector3> for [f64; 3] {
280    fn from(v: Vector3) -> Self {
281        v.to_array()
282    }
283}
284
285impl From<Vector3> for (f64, f64, f64) {
286    fn from(v: Vector3) -> Self {
287        v.to_tuple()
288    }
289}
290
291// ============================================================================
292// Matrix3 - 3x3 Matrix
293// ============================================================================
294
295/// A 3x3 matrix for rotations and coordinate transformations.
296///
297/// The matrix is stored in row-major order:
298/// ```text
299/// | m11 m12 m13 |
300/// | m21 m22 m23 |
301/// | m31 m32 m33 |
302/// ```
303#[derive(Debug, Clone, Copy, PartialEq)]
304pub struct Matrix3 {
305    pub m11: f64,
306    pub m12: f64,
307    pub m13: f64,
308    pub m21: f64,
309    pub m22: f64,
310    pub m23: f64,
311    pub m31: f64,
312    pub m32: f64,
313    pub m33: f64,
314}
315
316impl Matrix3 {
317    /// Create a new Matrix3 from individual elements.
318    #[allow(clippy::too_many_arguments)]
319    pub fn new(
320        m11: f64,
321        m12: f64,
322        m13: f64,
323        m21: f64,
324        m22: f64,
325        m23: f64,
326        m31: f64,
327        m32: f64,
328        m33: f64,
329    ) -> Self {
330        Self {
331            m11,
332            m12,
333            m13,
334            m21,
335            m22,
336            m23,
337            m31,
338            m32,
339            m33,
340        }
341    }
342
343    /// Create an identity matrix.
344    pub fn identity() -> Self {
345        Self::new(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)
346    }
347
348    /// Create a zero matrix.
349    pub fn zero() -> Self {
350        Self::new(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
351    }
352
353    /// Create a matrix from row vectors.
354    pub fn from_rows(row1: Vector3, row2: Vector3, row3: Vector3) -> Self {
355        Self::new(
356            row1.x, row1.y, row1.z, row2.x, row2.y, row2.z, row3.x, row3.y, row3.z,
357        )
358    }
359
360    /// Create a matrix from column vectors.
361    pub fn from_cols(col1: Vector3, col2: Vector3, col3: Vector3) -> Self {
362        Self::new(
363            col1.x, col2.x, col3.x, col1.y, col2.y, col3.y, col1.z, col2.z, col3.z,
364        )
365    }
366
367    /// Create a rotation matrix about the X axis.
368    pub fn rotation_x(angle: f64) -> Self {
369        let c = angle.cos();
370        let s = angle.sin();
371        Self::new(1.0, 0.0, 0.0, 0.0, c, s, 0.0, -s, c)
372    }
373
374    /// Create a rotation matrix about the Y axis.
375    pub fn rotation_y(angle: f64) -> Self {
376        let c = angle.cos();
377        let s = angle.sin();
378        Self::new(c, 0.0, -s, 0.0, 1.0, 0.0, s, 0.0, c)
379    }
380
381    /// Create a rotation matrix about the Z axis.
382    /// This rotates vectors counter-clockwise when viewed from +Z axis.
383    pub fn rotation_z(angle: f64) -> Self {
384        let c = angle.cos();
385        let s = angle.sin();
386        Self::new(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0)
387    }
388
389    /// Get a row as a Vector3.
390    pub fn row(&self, index: usize) -> Vector3 {
391        match index {
392            0 => Vector3::new(self.m11, self.m12, self.m13),
393            1 => Vector3::new(self.m21, self.m22, self.m23),
394            2 => Vector3::new(self.m31, self.m32, self.m33),
395            _ => panic!("Row index out of bounds"),
396        }
397    }
398
399    /// Get a column as a Vector3.
400    pub fn col(&self, index: usize) -> Vector3 {
401        match index {
402            0 => Vector3::new(self.m11, self.m21, self.m31),
403            1 => Vector3::new(self.m12, self.m22, self.m32),
404            2 => Vector3::new(self.m13, self.m23, self.m33),
405            _ => panic!("Column index out of bounds"),
406        }
407    }
408
409    /// Transpose the matrix.
410    pub fn transpose(&self) -> Self {
411        Self::new(
412            self.m11, self.m21, self.m31, self.m12, self.m22, self.m32, self.m13, self.m23,
413            self.m33,
414        )
415    }
416
417    /// Compute the determinant.
418    pub fn determinant(&self) -> f64 {
419        self.m11 * (self.m22 * self.m33 - self.m23 * self.m32)
420            - self.m12 * (self.m21 * self.m33 - self.m23 * self.m31)
421            + self.m13 * (self.m21 * self.m32 - self.m22 * self.m31)
422    }
423
424    /// Compute the inverse of the matrix.
425    /// Returns None if the matrix is singular.
426    pub fn inverse(&self) -> Option<Self> {
427        let det = self.determinant();
428        if det.abs() < 1e-15 {
429            return None;
430        }
431
432        let inv_det = 1.0 / det;
433
434        Some(Self::new(
435            (self.m22 * self.m33 - self.m23 * self.m32) * inv_det,
436            (self.m13 * self.m32 - self.m12 * self.m33) * inv_det,
437            (self.m12 * self.m23 - self.m13 * self.m22) * inv_det,
438            (self.m23 * self.m31 - self.m21 * self.m33) * inv_det,
439            (self.m11 * self.m33 - self.m13 * self.m31) * inv_det,
440            (self.m13 * self.m21 - self.m11 * self.m23) * inv_det,
441            (self.m21 * self.m32 - self.m22 * self.m31) * inv_det,
442            (self.m12 * self.m31 - self.m11 * self.m32) * inv_det,
443            (self.m11 * self.m22 - self.m12 * self.m21) * inv_det,
444        ))
445    }
446
447    /// Multiply the matrix by a vector.
448    pub fn mul_vec(&self, v: &Vector3) -> Vector3 {
449        Vector3::new(
450            self.m11 * v.x + self.m12 * v.y + self.m13 * v.z,
451            self.m21 * v.x + self.m22 * v.y + self.m23 * v.z,
452            self.m31 * v.x + self.m32 * v.y + self.m33 * v.z,
453        )
454    }
455
456    /// Multiply two matrices.
457    pub fn mul_mat(&self, other: &Matrix3) -> Self {
458        Self::new(
459            self.m11 * other.m11 + self.m12 * other.m21 + self.m13 * other.m31,
460            self.m11 * other.m12 + self.m12 * other.m22 + self.m13 * other.m32,
461            self.m11 * other.m13 + self.m12 * other.m23 + self.m13 * other.m33,
462            self.m21 * other.m11 + self.m22 * other.m21 + self.m23 * other.m31,
463            self.m21 * other.m12 + self.m22 * other.m22 + self.m23 * other.m32,
464            self.m21 * other.m13 + self.m22 * other.m23 + self.m23 * other.m33,
465            self.m31 * other.m11 + self.m32 * other.m21 + self.m33 * other.m31,
466            self.m31 * other.m12 + self.m32 * other.m22 + self.m33 * other.m32,
467            self.m31 * other.m13 + self.m32 * other.m23 + self.m33 * other.m33,
468        )
469    }
470
471    /// Scale the matrix by a scalar.
472    pub fn scale(&self, scalar: f64) -> Self {
473        Self::new(
474            self.m11 * scalar,
475            self.m12 * scalar,
476            self.m13 * scalar,
477            self.m21 * scalar,
478            self.m22 * scalar,
479            self.m23 * scalar,
480            self.m31 * scalar,
481            self.m32 * scalar,
482            self.m33 * scalar,
483        )
484    }
485
486    /// Check if this is approximately an identity matrix.
487    pub fn is_identity(&self, epsilon: f64) -> bool {
488        (self.m11 - 1.0).abs() < epsilon
489            && (self.m22 - 1.0).abs() < epsilon
490            && (self.m33 - 1.0).abs() < epsilon
491            && self.m12.abs() < epsilon
492            && self.m13.abs() < epsilon
493            && self.m21.abs() < epsilon
494            && self.m23.abs() < epsilon
495            && self.m31.abs() < epsilon
496            && self.m32.abs() < epsilon
497    }
498
499    /// Check if this is approximately a rotation matrix (orthogonal with det = 1).
500    pub fn is_rotation(&self, epsilon: f64) -> bool {
501        // Check determinant is approximately 1
502        if (self.determinant() - 1.0).abs() > epsilon {
503            return false;
504        }
505
506        // Check orthogonality: M * M^T should be identity
507        let product = self.mul_mat(&self.transpose());
508        product.is_identity(epsilon)
509    }
510}
511
512impl Default for Matrix3 {
513    fn default() -> Self {
514        Self::identity()
515    }
516}
517
518impl Mul<Matrix3> for Matrix3 {
519    type Output = Self;
520
521    fn mul(self, other: Self) -> Self {
522        self.mul_mat(&other)
523    }
524}
525
526impl Mul<Vector3> for Matrix3 {
527    type Output = Vector3;
528
529    fn mul(self, v: Vector3) -> Vector3 {
530        self.mul_vec(&v)
531    }
532}
533
534impl Mul<&Vector3> for Matrix3 {
535    type Output = Vector3;
536
537    fn mul(self, v: &Vector3) -> Vector3 {
538        self.mul_vec(v)
539    }
540}
541
542impl Mul<f64> for Matrix3 {
543    type Output = Self;
544
545    fn mul(self, scalar: f64) -> Self {
546        self.scale(scalar)
547    }
548}
549
550// ============================================================================
551// Mathematical Functions
552// ============================================================================
553
554/// Evaluate a polynomial using Horner's method.
555/// Coefficients are ordered from highest degree to constant term.
556/// e.g., `[a, b, c, d]` represents `a*x³ + b*x² + c*x + d`
557pub fn poly_eval(coefficients: &[f64], x: f64) -> f64 {
558    coefficients.iter().fold(0.0, |acc, &coef| acc * x + coef)
559}
560
561/// Linear interpolation between two values.
562#[inline]
563pub fn lerp(a: f64, b: f64, t: f64) -> f64 {
564    a + t * (b - a)
565}
566
567/// Clamp a value to a range.
568#[inline]
569pub fn clamp(value: f64, min: f64, max: f64) -> f64 {
570    value.max(min).min(max)
571}
572
573/// Sign function: returns -1, 0, or 1 based on the sign of the input.
574#[inline]
575pub fn sign(value: f64) -> f64 {
576    if value > 0.0 {
577        1.0
578    } else if value < 0.0 {
579        -1.0
580    } else {
581        0.0
582    }
583}
584
585/// Safe arcsin that clamps input to [-1, 1].
586#[inline]
587pub fn safe_asin(x: f64) -> f64 {
588    x.clamp(-1.0, 1.0).asin()
589}
590
591/// Safe arccos that clamps input to [-1, 1].
592#[inline]
593pub fn safe_acos(x: f64) -> f64 {
594    x.clamp(-1.0, 1.0).acos()
595}
596
597#[cfg(test)]
598mod tests {
599    use super::*;
600    use std::f64::consts::PI;
601
602    const EPSILON: f64 = 1e-12;
603
604    #[test]
605    fn test_vector3_basic() {
606        let v = Vector3::new(1.0, 2.0, 3.0);
607        assert_eq!(v.x, 1.0);
608        assert_eq!(v.y, 2.0);
609        assert_eq!(v.z, 3.0);
610    }
611
612    #[test]
613    fn test_vector3_magnitude() {
614        let v = Vector3::new(3.0, 4.0, 0.0);
615        assert!((v.magnitude() - 5.0).abs() < EPSILON);
616    }
617
618    #[test]
619    fn test_vector3_normalize() {
620        let v = Vector3::new(3.0, 4.0, 0.0);
621        let n = v.normalize();
622        assert!((n.magnitude() - 1.0).abs() < EPSILON);
623        assert!((n.x - 0.6).abs() < EPSILON);
624        assert!((n.y - 0.8).abs() < EPSILON);
625    }
626
627    #[test]
628    fn test_vector3_dot() {
629        let v1 = Vector3::new(1.0, 2.0, 3.0);
630        let v2 = Vector3::new(4.0, 5.0, 6.0);
631        assert!((v1.dot(&v2) - 32.0).abs() < EPSILON);
632    }
633
634    #[test]
635    fn test_vector3_cross() {
636        let x = Vector3::unit_x();
637        let y = Vector3::unit_y();
638        let z = x.cross(&y);
639        assert!(z.approx_eq(&Vector3::unit_z(), EPSILON));
640    }
641
642    #[test]
643    fn test_vector3_rotate_z() {
644        let v = Vector3::unit_x();
645        let rotated = v.rotate_z(PI / 2.0);
646        assert!(rotated.approx_eq(&Vector3::unit_y(), EPSILON));
647    }
648
649    #[test]
650    fn test_matrix3_identity() {
651        let m = Matrix3::identity();
652        let v = Vector3::new(1.0, 2.0, 3.0);
653        let result = m.mul_vec(&v);
654        assert!(result.approx_eq(&v, EPSILON));
655    }
656
657    #[test]
658    fn test_matrix3_rotation_z() {
659        let m = Matrix3::rotation_z(PI / 2.0);
660        let v = Vector3::unit_x();
661        let result = m.mul_vec(&v);
662        assert!(result.approx_eq(&Vector3::unit_y(), EPSILON));
663    }
664
665    #[test]
666    fn test_matrix3_transpose() {
667        let m = Matrix3::new(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
668        let t = m.transpose();
669        assert!((t.m12 - 4.0).abs() < EPSILON);
670        assert!((t.m21 - 2.0).abs() < EPSILON);
671    }
672
673    #[test]
674    fn test_matrix3_determinant() {
675        let m = Matrix3::identity();
676        assert!((m.determinant() - 1.0).abs() < EPSILON);
677
678        let m2 = Matrix3::rotation_z(0.5);
679        assert!((m2.determinant() - 1.0).abs() < EPSILON);
680    }
681
682    #[test]
683    fn test_matrix3_inverse() {
684        let m = Matrix3::rotation_z(0.5);
685        let inv = m.inverse().unwrap();
686        let product = m.mul_mat(&inv);
687        assert!(product.is_identity(EPSILON));
688    }
689
690    #[test]
691    fn test_matrix3_is_rotation() {
692        let m = Matrix3::rotation_z(0.5);
693        assert!(m.is_rotation(EPSILON));
694
695        let m2 = Matrix3::identity().scale(2.0);
696        assert!(!m2.is_rotation(EPSILON));
697    }
698
699    #[test]
700    fn test_poly_eval() {
701        // 2x² + 3x + 1 at x = 2 should be 2*4 + 3*2 + 1 = 15
702        let coeffs = [2.0, 3.0, 1.0];
703        assert!((poly_eval(&coeffs, 2.0) - 15.0).abs() < EPSILON);
704    }
705
706    #[test]
707    fn test_lerp() {
708        assert!((lerp(0.0, 10.0, 0.5) - 5.0).abs() < EPSILON);
709        assert!((lerp(0.0, 10.0, 0.0) - 0.0).abs() < EPSILON);
710        assert!((lerp(0.0, 10.0, 1.0) - 10.0).abs() < EPSILON);
711    }
712}
space_dust/math.rs

space_dust/
math.rs
