use crate::{ForceVector, MotionVector, SpatialRepresentation, SpatialScalar, SpatialTransform};
#[cfg(feature = "builtin")]
use crate::Builtin;
macro_rules! define_motion_subspace {
($($generics:tt)*) => {
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct MotionSubspace<$($generics)*> {
columns: [MotionVector<T, R>; N],
}
};
}
#[cfg(feature = "builtin")]
define_motion_subspace!(const N: usize, T: SpatialScalar = f64, R: SpatialRepresentation<T> = Builtin);
#[cfg(not(feature = "builtin"))]
define_motion_subspace!(const N: usize, T: SpatialScalar, R: SpatialRepresentation<T>);
impl<const N: usize, T, R> MotionSubspace<N, T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
pub fn from_columns(columns: [MotionVector<T, R>; N]) -> Self {
Self { columns }
}
pub fn columns(&self) -> &[MotionVector<T, R>; N] {
&self.columns
}
pub fn is_finite(&self) -> bool {
self.columns.iter().all(MotionVector::is_finite)
}
pub fn apply(&self, coefficients: &[T; N]) -> MotionVector<T, R> {
self.columns
.iter()
.zip(coefficients)
.fold(MotionVector::zeros(), |sum, (column, coefficient)| {
sum + *column * *coefficient
})
}
pub fn generalized_force(&self, force: &ForceVector<T, R>) -> [T; N] {
std::array::from_fn(|index| self.columns[index].dot(force))
}
pub fn generalized_forces<const M: usize>(
&self,
forces: &[ForceVector<T, R>; M],
) -> [[T; M]; N] {
std::array::from_fn(|row| {
std::array::from_fn(|column| self.columns[row].dot(&forces[column]))
})
}
pub fn transformed(&self, transform: &SpatialTransform<T, R>) -> Self {
Self::from_columns(std::array::from_fn(|index| {
transform.transform_motion(&self.columns[index])
}))
}
}
impl<T, R> From<MotionVector<T, R>> for MotionSubspace<1, T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
fn from(column: MotionVector<T, R>) -> Self {
Self::from_columns([column])
}
}
#[cfg(feature = "serde")]
impl<const N: usize, T, R> serde::Serialize for MotionSubspace<N, T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
MotionVector<T, R>: serde::Serialize,
{
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: serde::Serializer,
{
use serde::ser::SerializeTuple;
let mut tuple = serializer.serialize_tuple(N)?;
for column in &self.columns {
tuple.serialize_element(column)?;
}
tuple.end()
}
}
#[cfg(feature = "serde")]
impl<'de, const N: usize, T, R> serde::Deserialize<'de> for MotionSubspace<N, T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
MotionVector<T, R>: serde::Deserialize<'de>,
{
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where
D: serde::Deserializer<'de>,
{
struct ColumnsVisitor<const N: usize, T, R>(std::marker::PhantomData<(T, R)>);
impl<'de, const N: usize, T, R> serde::de::Visitor<'de> for ColumnsVisitor<N, T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
MotionVector<T, R>: serde::Deserialize<'de>,
{
type Value = MotionSubspace<N, T, R>;
fn expecting(&self, formatter: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
write!(formatter, "exactly {N} motion-subspace columns")
}
fn visit_seq<A>(self, mut sequence: A) -> Result<Self::Value, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
let mut columns: [Option<MotionVector<T, R>>; N] = std::array::from_fn(|_| None);
for (index, column) in columns.iter_mut().enumerate() {
*column = Some(
sequence
.next_element()?
.ok_or_else(|| serde::de::Error::invalid_length(index, &self))?,
);
}
if sequence.next_element::<serde::de::IgnoredAny>()?.is_some() {
return Err(serde::de::Error::invalid_length(N + 1, &self));
}
Ok(MotionSubspace::from_columns(std::array::from_fn(|index| {
columns[index]
.take()
.expect("every column was filled above")
})))
}
}
deserializer.deserialize_tuple(N, ColumnsVisitor(std::marker::PhantomData))
}
}