use crate::math::{Isometry, Point, Real, Translation, Vector};
pub trait RigidMotion {
fn position_at_time(&self, t: Real) -> Isometry<Real>;
}
impl RigidMotion for Isometry<Real> {
fn position_at_time(&self, _: Real) -> Isometry<Real> {
*self
}
}
pub struct InterpolatedRigidMotion {
pub start: Isometry<Real>,
pub end: Isometry<Real>,
}
impl InterpolatedRigidMotion {
pub fn new(start: Isometry<Real>, end: Isometry<Real>) -> Self {
InterpolatedRigidMotion { start, end }
}
}
impl RigidMotion for InterpolatedRigidMotion {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
self.start.lerp_slerp(&self.end, t)
}
}
pub struct ConstantLinearVelocityRigidMotion {
pub t0: Real,
pub start: Isometry<Real>,
pub velocity: Vector<Real>,
}
impl ConstantLinearVelocityRigidMotion {
pub fn new(t0: Real, start: Isometry<Real>, velocity: Vector<Real>) -> Self {
ConstantLinearVelocityRigidMotion {
t0,
start,
velocity,
}
}
}
impl RigidMotion for ConstantLinearVelocityRigidMotion {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
Isometry::from_parts(
(self.start.translation.vector + self.velocity * (t - self.t0)).into(),
self.start.rotation,
)
}
}
#[derive(Debug)]
pub struct ConstantVelocityRigidMotion {
pub t0: Real,
pub start: Isometry<Real>,
pub local_center: Point<Real>,
pub linvel: Vector<Real>,
#[cfg(feature = "dim2")]
pub angvel: Real,
#[cfg(feature = "dim3")]
pub angvel: Vector<Real>,
}
impl ConstantVelocityRigidMotion {
#[cfg(feature = "dim2")]
pub fn new(
t0: Real,
start: Isometry<Real>,
local_center: Point<Real>,
linvel: Vector<Real>,
angvel: Real,
) -> Self {
ConstantVelocityRigidMotion {
t0,
start,
local_center,
linvel,
angvel,
}
}
#[cfg(feature = "dim3")]
pub fn new(
t0: Real,
start: Isometry<Real>,
local_center: Point<Real>,
linvel: Vector<Real>,
angvel: Vector<Real>,
) -> Self {
ConstantVelocityRigidMotion {
t0,
start,
local_center,
linvel,
angvel,
}
}
}
impl RigidMotion for ConstantVelocityRigidMotion {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
let scaled_linvel = self.linvel * (t - self.t0);
let scaled_angvel = self.angvel * (t - self.t0);
let center = self.start.rotation * self.local_center.coords;
let lhs = self.start.translation * Translation::from(center);
let rhs = Translation::from(-center) * self.start.rotation;
lhs * Isometry::new(scaled_linvel, scaled_angvel) * rhs
}
}
pub trait RigidMotionComposition: RigidMotion {
fn inverse(&self) -> Inverse<Self> {
Inverse { motion: self }
}
fn inv_mul<'a>(&'a self, rhs: &'a dyn RigidMotion) -> InvMul<'a, Self, dyn RigidMotion + 'a> {
InvMul {
motion1: self,
motion2: rhs,
}
}
fn prepend_translation(&self, translation: Vector<Real>) -> PrependTranslation<Self> {
PrependTranslation {
motion: self,
translation,
}
}
fn append_translation(&self, translation: Vector<Real>) -> AppendTranslation<Self> {
AppendTranslation {
motion: self,
translation,
}
}
fn prepend_transformation(
&self,
transformation: Isometry<Real>,
) -> PrependTransformation<Self> {
PrependTransformation {
motion: self,
transformation,
}
}
fn append_transformation(&self, transformation: Isometry<Real>) -> AppendTransformation<Self> {
AppendTransformation {
motion: self,
transformation,
}
}
}
impl<M: ?Sized + RigidMotion> RigidMotionComposition for M {}
pub struct Inverse<'a, M: ?Sized> {
motion: &'a M,
}
impl<'a, M: ?Sized + RigidMotion> RigidMotion for Inverse<'a, M> {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
self.motion.position_at_time(t).inverse()
}
}
pub struct InvMul<'a, M1: ?Sized, M2: ?Sized> {
motion1: &'a M1,
motion2: &'a M2,
}
impl<'a, M1: ?Sized + RigidMotion, M2: ?Sized + RigidMotion> RigidMotion for InvMul<'a, M1, M2> {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
let m1 = self.motion1.position_at_time(t);
let m2 = self.motion2.position_at_time(t);
m1.inv_mul(&m2)
}
}
pub struct PrependTranslation<'a, M: ?Sized> {
motion: &'a M,
translation: Vector<Real>,
}
impl<'a, M: ?Sized + RigidMotion> RigidMotion for PrependTranslation<'a, M> {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
let m = self.motion.position_at_time(t);
m * Translation::from(self.translation)
}
}
pub struct AppendTranslation<'a, M: ?Sized> {
motion: &'a M,
translation: Vector<Real>,
}
impl<'a, M: ?Sized + RigidMotion> RigidMotion for AppendTranslation<'a, M> {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
let m = self.motion.position_at_time(t);
Translation::from(self.translation) * m
}
}
pub struct PrependTransformation<'a, M: ?Sized> {
motion: &'a M,
transformation: Isometry<Real>,
}
impl<'a, M: ?Sized + RigidMotion> RigidMotion for PrependTransformation<'a, M> {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
let m = self.motion.position_at_time(t);
m * self.transformation
}
}
pub struct AppendTransformation<'a, M: ?Sized> {
motion: &'a M,
transformation: Isometry<Real>,
}
impl<'a, M: ?Sized + RigidMotion> RigidMotion for AppendTransformation<'a, M> {
fn position_at_time(&self, t: Real) -> Isometry<Real> {
let m = self.motion.position_at_time(t);
self.transformation * m
}
}