use enum_dispatch::enum_dispatch;
use nalgebra::Vector3;
use crate::{
base::{DynClone, Float, Source, Transform},
crate_utils::need_std,
};
#[derive(Debug, Clone, PartialEq)]
pub enum SensorOutput<T: Float = f64> {
Scalar(T),
Vector(Vector3<T>),
Digital(i64),
}
#[enum_dispatch]
pub trait Observer<T: Float>: Transform<T> + Send + Sync + DynClone {
fn read(&self, source: &dyn Source<T>) -> SensorOutput<T>;
fn format(&self, f: &mut core::fmt::Formatter<'_>, _: &str) -> core::fmt::Result {
write!(f, "Observer at {}", self.pose())
}
}
#[cfg(feature = "std")]
impl<T: Float> core::fmt::Display for dyn Observer<T> {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
self.format(f, "")
}
}
need_std!(
use core::fmt::Display;
use delegate::delegate;
use dyn_clone::clone_trait_object;
use crate::base::Pose;
impl<T: Float> Transform<T> for Box<dyn Observer<T>> {
delegate!(
to (**self) {
fn pose(&self) -> &Pose<T>;
fn pose_mut(&mut self) -> &mut Pose<T>;
fn set_pose(&mut self, pose: Pose<T>);
}
);
}
clone_trait_object!(<T> Observer<T> where T: Float);
impl<T: Float> core::fmt::Debug for Box<dyn Observer<T>> {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
(**self).fmt(f)
}
}
impl<T: Float> Observer<T> for Box<dyn Observer<T>> {
delegate!(
to (**self) {
fn read(&self, source: &dyn Source<T>) -> SensorOutput<T>;
}
);
}
);