dynamics_spatial/
vector3d.rs

1//! Defines **3D vectors** and related operations.
2
3use nalgebra::{Matrix3, 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        // FIXME: replace this by a trait implementation
80        let array = array.as_array();
81        if array.ndim() != 1 || array.len() != 3 {
82            return Err(PyErr::new::<pyo3::exceptions::PyValueError, _>(
83                "Input array must be one-dimensional with length 3.",
84            ));
85        }
86        Ok(Vector3D(Vector3::new(array[0], array[1], array[2])))
87    }
88
89    pub fn dot(&self, other: &Vector3D) -> f64 {
90        self.0.dot(&other.0)
91    }
92
93    pub fn skew(&self) -> Matrix3<f64> {
94        let x = self.0[0];
95        let y = self.0[1];
96        let z = self.0[2];
97        Matrix3::new(0.0, -z, y, z, 0.0, -x, -y, x, 0.0)
98    }
99}
100
101impl From<&[f64; 3]> for Vector3D {
102    fn from(array: &[f64; 3]) -> Self {
103        Vector3D(Vector3::new(array[0], array[1], array[2]))
104    }
105}
106
107impl Add for Vector3D {
108    type Output = Vector3D;
109
110    fn add(self, rhs: Self) -> Self::Output {
111        Vector3D(self.0 + rhs.0)
112    }
113}
114
115impl Sub for Vector3D {
116    type Output = Vector3D;
117
118    fn sub(self, rhs: Self) -> Self::Output {
119        Vector3D(self.0 - rhs.0)
120    }
121}
122
123impl Mul for Vector3D {
124    type Output = Vector3D;
125
126    fn mul(self, rhs: Self) -> Self::Output {
127        Vector3D(self.0.component_mul(&rhs.0))
128    }
129}
130
131impl Mul<f64> for Vector3D {
132    type Output = Vector3D;
133
134    fn mul(self, rhs: f64) -> Self::Output {
135        Vector3D(self.0 * rhs)
136    }
137}
138
139impl Mul<f64> for &Vector3D {
140    type Output = Vector3D;
141
142    fn mul(self, rhs: f64) -> Self::Output {
143        Vector3D(self.0 * rhs)
144    }
145}
146
147impl Mul<&Vector3D> for f64 {
148    type Output = Vector3D;
149
150    fn mul(self, rhs: &Vector3D) -> Self::Output {
151        Vector3D(rhs.0 * self)
152    }
153}
154
155impl Mul<Vector3D> for f64 {
156    type Output = Vector3D;
157
158    fn mul(self, rhs: Vector3D) -> Self::Output {
159        Vector3D(rhs.0 * self)
160    }
161}
162
163impl Neg for Vector3D {
164    type Output = Vector3D;
165
166    fn neg(self) -> Self::Output {
167        Vector3D(-self.0)
168    }
169}
dynamics_spatial/vector3d.rs

dynamics_spatial/
vector3d.rs
