dynamics_spatial/
vector3d.rs

1//! Defines **3D vectors** and related operations.
2
3use nalgebra::Vector3;
4use std::ops::{Add, Mul, Neg, Sub};
5
6#[cfg(feature = "python")]
7use numpy::{PyReadonlyArrayDyn, ToPyArray, ndarray::Array1};
8#[cfg(feature = "python")]
9use pyo3::prelude::*;
10
11#[derive(Debug, Clone, Copy, PartialEq, Default)]
12/// A 3D vector, commonly used for positions.
13pub struct Vector3D(pub(crate) Vector3<f64>);
14
15impl Vector3D {
16    /// Creates a new `Vector3D` with the given x, y, z components.
17    #[must_use]
18    pub fn new(x: f64, y: f64, z: f64) -> Self {
19        Self(Vector3::new(x, y, z))
20    }
21
22    /// Creates a zero vector.
23    #[must_use]
24    pub fn zeros() -> Self {
25        Self(Vector3::zeros())
26    }
27
28    #[must_use]
29    pub fn as_slice(&self) -> &[f64; 3] {
30        self.0.as_slice().try_into().unwrap()
31    }
32
33    /// Returns the L2 norm of the vector.
34    #[must_use]
35    pub fn norm(&self) -> f64 {
36        self.0.norm()
37    }
38
39    /// Returns the `x` unit vector, that is (1, 0, 0).
40    #[must_use]
41    pub fn x() -> Self {
42        Self(Vector3::x())
43    }
44
45    /// Returns the `y` unit vector, that is (0, 1, 0).
46    #[must_use]
47    pub fn y() -> Self {
48        Self(Vector3::y())
49    }
50
51    /// Returns the `z` unit vector, that is (0, 0, 1).
52    #[must_use]
53    pub fn z() -> Self {
54        Self(Vector3::z())
55    }
56
57    /// Computes the cross product of two 3D vectors.
58    #[must_use]
59    pub fn cross(&self, other: &Vector3D) -> Vector3D {
60        Vector3D(self.0.cross(&other.0))
61    }
62
63    #[must_use]
64    #[cfg(feature = "python")]
65    /// Converts the `Vector3D` to a one-dimensional NumPy array of length 3.
66    pub fn to_numpy(&self, py: Python) -> Py<PyAny> {
67        Array1::from_iter(self.0.iter().copied())
68            .to_pyarray(py)
69            .into_any()
70            .unbind()
71    }
72
73    #[cfg(feature = "python")]
74    /// Creates a `Vector3D` from a one-dimensional NumPy array of length 3.
75    ///
76    /// # Errors
77    /// Returns a `PyValueError` if the input array does not have the correct shape
78    pub fn from_pyarray(array: &PyReadonlyArrayDyn<f64>) -> Result<Self, PyErr> {
79        let array = array.as_array();
80        if array.ndim() != 1 || array.len() != 3 {
81            return Err(PyErr::new::<pyo3::exceptions::PyValueError, _>(
82                "Input array must be one-dimensional with length 3.",
83            ));
84        }
85        Ok(Vector3D(Vector3::new(array[0], array[1], array[2])))
86    }
87
88    pub fn dot(&self, other: &Vector3D) -> f64 {
89        self.0.dot(&other.0)
90    }
91}
92
93impl Add for Vector3D {
94    type Output = Vector3D;
95
96    fn add(self, rhs: Self) -> Self::Output {
97        Vector3D(self.0 + rhs.0)
98    }
99}
100
101impl Sub for Vector3D {
102    type Output = Vector3D;
103
104    fn sub(self, rhs: Self) -> Self::Output {
105        Vector3D(self.0 - rhs.0)
106    }
107}
108
109impl Mul for Vector3D {
110    type Output = Vector3D;
111
112    fn mul(self, rhs: Self) -> Self::Output {
113        Vector3D(self.0.component_mul(&rhs.0))
114    }
115}
116
117impl Mul<f64> for Vector3D {
118    type Output = Vector3D;
119
120    fn mul(self, rhs: f64) -> Self::Output {
121        Vector3D(self.0 * rhs)
122    }
123}
124
125impl Mul<f64> for &Vector3D {
126    type Output = Vector3D;
127
128    fn mul(self, rhs: f64) -> Self::Output {
129        Vector3D(self.0 * rhs)
130    }
131}
132
133impl Mul<&Vector3D> for f64 {
134    type Output = Vector3D;
135
136    fn mul(self, rhs: &Vector3D) -> Self::Output {
137        Vector3D(rhs.0 * self)
138    }
139}
140
141impl Mul<Vector3D> for f64 {
142    type Output = Vector3D;
143
144    fn mul(self, rhs: Vector3D) -> Self::Output {
145        Vector3D(rhs.0 * self)
146    }
147}
148
149impl Neg for Vector3D {
150    type Output = Vector3D;
151
152    fn neg(self) -> Self::Output {
153        Vector3D(-self.0)
154    }
155}
dynamics_spatial/vector3d.rs

dynamics_spatial/
vector3d.rs
