dynamics_spatial/
vector6d.rs

1//! Defines **spatial (6D) vectors** and related operations.
2
3use nalgebra::{Matrix6, Vector6};
4
5#[derive(Debug, Clone, Copy, PartialEq)]
6/// A 6D vector representing spatial motion (angular and linear components).
7///
8/// A spatial vector is represented as a 6-dimensional vector,
9/// which can be decomposed into $\begin{bmatrix} \omega & v \end{bmatrix}$,
10/// where $\omega$ is the angular component and $v$ is the linear component.
11pub struct Vector6D(pub(crate) Vector6<f64>);
12
13impl Vector6D {
14    #[must_use]
15    pub fn new(x: f64, y: f64, z: f64, w: f64, v: f64, u: f64) -> Self {
16        Self(Vector6::new(x, y, z, w, v, u))
17    }
18
19    #[must_use]
20    pub fn from_slice(data: &[f64; 6]) -> Self {
21        Self(Vector6::from_column_slice(data))
22    }
23
24    #[must_use]
25    pub fn zeros() -> Self {
26        Self(Vector6::zeros())
27    }
28
29    #[must_use]
30    pub fn as_slice(&self) -> &[f64; 6] {
31        self.0.as_slice().try_into().unwrap()
32    }
33
34    /// Returns the vector as a diagonal 6x6 matrix.
35    #[must_use]
36    pub fn as_diagonal(&self) -> Matrix6<f64> {
37        Matrix6::from_diagonal(&self.0)
38    }
39}
dynamics_spatial/vector6d.rs

dynamics_spatial/
vector6d.rs
