use core::marker::PhantomData;
use core::ops::Mul;
use spacelab_core::{Mat3, Vec3};
pub trait Frame: Copy + core::fmt::Debug + PartialEq + Eq + 'static {
const NAME: &'static str;
}
macro_rules! frame {
($(#[$m:meta])* $name:ident, $s:literal) => {
$(#[$m])*
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct $name;
impl Frame for $name {
const NAME: &'static str = $s;
}
};
}
frame!(
Gcrf, "GCRF"
);
frame!(
Cirs, "CIRS"
);
frame!(
Tirs, "TIRS"
);
frame!(
Pef, "PEF"
);
frame!(
Itrf, "ITRF"
);
frame!(
Teme, "TEME"
);
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Rotation<From: Frame, To: Frame> {
matrix: Mat3,
_frames: PhantomData<fn(From) -> To>,
}
impl<From: Frame, To: Frame> Rotation<From, To> {
#[must_use]
pub const fn from_matrix(matrix: Mat3) -> Self {
Self {
matrix,
_frames: PhantomData,
}
}
#[must_use]
pub const fn matrix(self) -> Mat3 {
self.matrix
}
#[must_use]
pub fn inverse(self) -> Rotation<To, From> {
Rotation::from_matrix(self.matrix.transpose())
}
}
impl<A: Frame, B: Frame, C: Frame> Mul<Rotation<A, B>> for Rotation<B, C> {
type Output = Rotation<A, C>;
fn mul(self, rhs: Rotation<A, B>) -> Rotation<A, C> {
Rotation::from_matrix(self.matrix * rhs.matrix)
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Position<F: Frame> {
pub vector: Vec3,
_frame: PhantomData<fn(F) -> F>,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Velocity<F: Frame> {
pub vector: Vec3,
_frame: PhantomData<fn(F) -> F>,
}
macro_rules! tagged_vector {
($name:ident) => {
impl<F: Frame> $name<F> {
#[must_use]
pub const fn new(vector: Vec3) -> Self {
Self {
vector,
_frame: PhantomData,
}
}
}
impl<A: Frame, B: Frame> Mul<$name<A>> for Rotation<A, B> {
type Output = $name<B>;
fn mul(self, v: $name<A>) -> $name<B> {
$name::new(self.matrix * v.vector)
}
}
};
}
tagged_vector!(Position);
tagged_vector!(Velocity);
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct StateVector<F: Frame> {
pub position: Position<F>,
pub velocity: Velocity<F>,
}
impl<F: Frame> StateVector<F> {
#[must_use]
pub const fn new(position: Vec3, velocity: Vec3) -> Self {
Self {
position: Position::new(position),
velocity: Velocity::new(velocity),
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Transform<From: Frame, To: Frame> {
pub rotation: Rotation<From, To>,
pub omega: Vec3,
}
impl<A: Frame, B: Frame> Transform<A, B> {
#[must_use]
pub const fn new(rotation: Rotation<A, B>, omega: Vec3) -> Self {
Self { rotation, omega }
}
#[must_use]
pub fn position(self, p: Position<A>) -> Position<B> {
self.rotation * p
}
#[must_use]
pub fn state(self, s: StateVector<A>) -> StateVector<B> {
let r = s.position.vector;
let v = s.velocity.vector;
StateVector::new(
self.rotation.matrix() * r,
self.rotation.matrix() * (v - self.omega.cross(r)),
)
}
#[must_use]
pub fn inverse(self) -> Transform<B, A> {
Transform::new(
self.rotation.inverse(),
self.rotation.matrix() * (-self.omega),
)
}
}