use core::borrow::Borrow;
use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
#[cfg(feature = "builtin")]
use crate::Builtin;
use crate::{SpatialRepresentation, SpatialScalar};
#[cfg(feature = "builtin")]
pub type SpatialCoordinates<T = f64, R = Builtin> = <R as SpatialRepresentation<T>>::Vector6;
#[cfg(not(feature = "builtin"))]
pub type SpatialCoordinates<T, R> = <R as SpatialRepresentation<T>>::Vector6;
#[cfg(feature = "builtin")]
pub type SpatialMatrix<T = f64, R = Builtin> = <R as SpatialRepresentation<T>>::Matrix6;
#[cfg(not(feature = "builtin"))]
pub type SpatialMatrix<T, R> = <R as SpatialRepresentation<T>>::Matrix6;
macro_rules! define_motion_vector {
($($generics:tt)*) => {
#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[cfg_attr(
feature = "serde",
serde(bound(
serialize = "R::Vector3: serde::Serialize",
deserialize = "R::Vector3: serde::Deserialize<'de>"
))
)]
pub struct MotionVector<$($generics)*> {
pub angular: R::Vector3,
pub linear: R::Vector3,
}
};
}
#[cfg(feature = "builtin")]
define_motion_vector!(T: SpatialScalar = f64, R: SpatialRepresentation<T> = Builtin);
#[cfg(not(feature = "builtin"))]
define_motion_vector!(T: SpatialScalar, R: SpatialRepresentation<T>);
impl<T, R> MotionVector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
pub fn new(angular: R::Vector3, linear: R::Vector3) -> Self {
Self { angular, linear }
}
pub fn zeros() -> Self {
Self::new(R::vector3_zero(), R::vector3_zero())
}
pub fn from_vector<V>(vector: V) -> Self
where
V: Borrow<R::Vector6>,
{
Self::from_array(R::vector6_to_array(vector.borrow()))
}
pub fn from_array(vector: [T; 6]) -> Self {
Self::new(
R::vector3_from_array([vector[0], vector[1], vector[2]]),
R::vector3_from_array([vector[3], vector[4], vector[5]]),
)
}
pub fn to_vector(&self) -> SpatialCoordinates<T, R> {
R::vector6_from_array(self.to_array())
}
pub fn to_array(&self) -> [T; 6] {
let angular = R::vector3_to_array(&self.angular);
let linear = R::vector3_to_array(&self.linear);
[
angular[0], angular[1], angular[2], linear[0], linear[1], linear[2],
]
}
pub fn is_finite(&self) -> bool {
R::vector3_is_finite(&self.angular) && R::vector3_is_finite(&self.linear)
}
pub fn dot(&self, force: &ForceVector<T, R>) -> T {
R::vector3_dot(&self.angular, &force.moment) + R::vector3_dot(&self.linear, &force.force)
}
pub fn cross_motion(&self, motion: &Self) -> Self {
Self::new(
R::vector3_cross(&self.angular, &motion.angular),
R::vector3_add(
&R::vector3_cross(&self.angular, &motion.linear),
&R::vector3_cross(&self.linear, &motion.angular),
),
)
}
pub fn cross_force(&self, force: &ForceVector<T, R>) -> ForceVector<T, R> {
ForceVector::new(
R::vector3_add(
&R::vector3_cross(&self.angular, &force.moment),
&R::vector3_cross(&self.linear, &force.force),
),
R::vector3_cross(&self.angular, &force.force),
)
}
pub fn cross_matrix(&self) -> SpatialMatrix<T, R> {
let angular = R::vector3_cross_matrix(&self.angular);
let linear = R::vector3_cross_matrix(&self.linear);
let zero = R::matrix3_zero();
R::matrix6_from_blocks(&angular, &zero, &linear, &angular)
}
pub fn cross_dual_matrix(&self) -> SpatialMatrix<T, R> {
R::matrix6_neg(&R::matrix6_transpose(&self.cross_matrix()))
}
}
macro_rules! define_force_vector {
($($generics:tt)*) => {
#[derive(Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[cfg_attr(
feature = "serde",
serde(bound(
serialize = "R::Vector3: serde::Serialize",
deserialize = "R::Vector3: serde::Deserialize<'de>"
))
)]
pub struct ForceVector<$($generics)*> {
pub moment: R::Vector3,
pub force: R::Vector3,
}
};
}
#[cfg(feature = "builtin")]
define_force_vector!(T: SpatialScalar = f64, R: SpatialRepresentation<T> = Builtin);
#[cfg(not(feature = "builtin"))]
define_force_vector!(T: SpatialScalar, R: SpatialRepresentation<T>);
impl<T, R> ForceVector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
pub fn new(moment: R::Vector3, force: R::Vector3) -> Self {
Self { moment, force }
}
pub fn zeros() -> Self {
Self::new(R::vector3_zero(), R::vector3_zero())
}
pub fn from_vector<V>(vector: V) -> Self
where
V: Borrow<R::Vector6>,
{
Self::from_array(R::vector6_to_array(vector.borrow()))
}
pub fn from_array(vector: [T; 6]) -> Self {
Self::new(
R::vector3_from_array([vector[0], vector[1], vector[2]]),
R::vector3_from_array([vector[3], vector[4], vector[5]]),
)
}
pub fn to_vector(&self) -> SpatialCoordinates<T, R> {
R::vector6_from_array(self.to_array())
}
pub fn to_array(&self) -> [T; 6] {
let moment = R::vector3_to_array(&self.moment);
let force = R::vector3_to_array(&self.force);
[
moment[0], moment[1], moment[2], force[0], force[1], force[2],
]
}
pub fn is_finite(&self) -> bool {
R::vector3_is_finite(&self.moment) && R::vector3_is_finite(&self.force)
}
pub fn dot(&self, motion: &MotionVector<T, R>) -> T {
motion.dot(self)
}
}
macro_rules! impl_vector_arithmetic {
($vector:ident, $first:ident, $second:ident) => {
impl<T, R> Add for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
type Output = Self;
fn add(self, rhs: Self) -> Self::Output {
Self::new(
R::vector3_add(&self.$first, &rhs.$first),
R::vector3_add(&self.$second, &rhs.$second),
)
}
}
impl<T, R> AddAssign for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
fn add_assign(&mut self, rhs: Self) {
*self = *self + rhs;
}
}
impl<T, R> Sub for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
type Output = Self;
fn sub(self, rhs: Self) -> Self::Output {
Self::new(
R::vector3_sub(&self.$first, &rhs.$first),
R::vector3_sub(&self.$second, &rhs.$second),
)
}
}
impl<T, R> SubAssign for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
fn sub_assign(&mut self, rhs: Self) {
*self = *self - rhs;
}
}
impl<T, R> Neg for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
type Output = Self;
fn neg(self) -> Self::Output {
Self::new(R::vector3_neg(&self.$first), R::vector3_neg(&self.$second))
}
}
impl<T, R> Mul<T> for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
type Output = Self;
fn mul(self, rhs: T) -> Self::Output {
Self::new(
R::vector3_scale(&self.$first, rhs),
R::vector3_scale(&self.$second, rhs),
)
}
}
impl<T, R> MulAssign<T> for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
fn mul_assign(&mut self, rhs: T) {
*self = *self * rhs;
}
}
impl<T, R> Div<T> for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
type Output = Self;
fn div(self, rhs: T) -> Self::Output {
self * (T::one() / rhs)
}
}
impl<T, R> DivAssign<T> for $vector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
fn div_assign(&mut self, rhs: T) {
*self = *self / rhs;
}
}
};
}
impl_vector_arithmetic!(MotionVector, angular, linear);
impl_vector_arithmetic!(ForceVector, moment, force);
impl<T, R> Mul<ForceVector<T, R>> for MotionVector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
type Output = T;
fn mul(self, rhs: ForceVector<T, R>) -> Self::Output {
self.dot(&rhs)
}
}
impl<T, R> Mul<MotionVector<T, R>> for ForceVector<T, R>
where
T: SpatialScalar,
R: SpatialRepresentation<T>,
{
type Output = T;
fn mul(self, rhs: MotionVector<T, R>) -> Self::Output {
self.dot(&rhs)
}
}
impl<R> Mul<MotionVector<f64, R>> for f64
where
R: SpatialRepresentation<f64>,
{
type Output = MotionVector<f64, R>;
fn mul(self, rhs: MotionVector<f64, R>) -> Self::Output {
rhs * self
}
}
impl<R> Mul<MotionVector<f32, R>> for f32
where
R: SpatialRepresentation<f32>,
{
type Output = MotionVector<f32, R>;
fn mul(self, rhs: MotionVector<f32, R>) -> Self::Output {
rhs * self
}
}
impl<R> Mul<ForceVector<f64, R>> for f64
where
R: SpatialRepresentation<f64>,
{
type Output = ForceVector<f64, R>;
fn mul(self, rhs: ForceVector<f64, R>) -> Self::Output {
rhs * self
}
}
impl<R> Mul<ForceVector<f32, R>> for f32
where
R: SpatialRepresentation<f32>,
{
type Output = ForceVector<f32, R>;
fn mul(self, rhs: ForceVector<f32, R>) -> Self::Output {
rhs * self
}
}