dynamics_spatial/
vector6d.rs

1//! Defines **spatial (6D) vectors** and related operations.
2
3use std::{fmt::Display, ops::Mul};
4
5use nalgebra::{Matrix6, Vector6};
6
7#[derive(Debug, Clone, Copy, PartialEq)]
8/// A 6D vector representing spatial motion (angular and linear components).
9///
10/// A spatial vector is represented as a 6-dimensional vector,
11/// which can be decomposed into $\begin{bmatrix} \omega & v \end{bmatrix}$,
12/// where $\omega$ is the angular component and $v$ is the linear component.
13pub struct Vector6D(pub Vector6<f64>);
14
15impl Vector6D {
16    #[must_use]
17    pub fn new(x: f64, y: f64, z: f64, w: f64, v: f64, u: f64) -> Self {
18        Self(Vector6::new(x, y, z, w, v, u))
19    }
20
21    #[must_use]
22    pub fn from_slice(data: &[f64; 6]) -> Self {
23        Self(Vector6::from_column_slice(data))
24    }
25
26    #[must_use]
27    pub fn zeros() -> Self {
28        Self(Vector6::zeros())
29    }
30
31    #[must_use]
32    pub fn as_slice(&self) -> &[f64; 6] {
33        self.0.as_slice().try_into().unwrap()
34    }
35
36    /// Returns the vector as a diagonal 6x6 matrix.
37    #[must_use]
38    pub fn as_diagonal(&self) -> Matrix6<f64> {
39        Matrix6::from_diagonal(&self.0)
40    }
41}
42
43impl Mul<f64> for &Vector6D {
44    type Output = Vector6D;
45
46    fn mul(self, rhs: f64) -> Self::Output {
47        Vector6D(self.0 * rhs)
48    }
49}
50
51impl Mul<&Vector6D> for f64 {
52    type Output = Vector6D;
53
54    fn mul(self, rhs: &Vector6D) -> Self::Output {
55        Vector6D(self * rhs.0)
56    }
57}
58
59impl Display for Vector6D {
60    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
61        write!(f, "Vector6D({:?})", self.0.as_slice())
62    }
63}
dynamics_spatial/vector6d.rs

dynamics_spatial/
vector6d.rs
