use crate::common::points::mid_point;
use crate::common::surface_point::SurfacePoint;
use parry3d_f64::na::{Point, SVector, Unit};
use serde::{Deserialize, Serialize};
pub trait Measurement {
fn value(&self) -> f64;
}
#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct Distance<const D: usize> {
pub a: Point<f64, D>,
pub b: Point<f64, D>,
pub direction: Unit<SVector<f64, D>>,
}
impl<const D: usize> Distance<D> {
pub fn new(
a: Point<f64, D>,
b: Point<f64, D>,
direction: Option<Unit<SVector<f64, D>>>,
) -> Self {
let direction = direction.unwrap_or(Unit::new_normalize(b - a));
Self { a, b, direction }
}
pub fn reversed(&self) -> Self {
Self {
a: self.b,
b: self.a,
direction: -self.direction,
}
}
pub fn center(&self) -> SurfacePoint<D> {
SurfacePoint::new(mid_point(&self.a, &self.b), self.direction)
}
}
impl<const D: usize> Default for Distance<D> {
fn default() -> Self {
let a = Point::origin();
let b = Point::origin();
let mut d = SVector::<f64, D>::zeros();
d[0] = 1.0;
let d = Unit::new_normalize(d);
Self::new(a, b, Some(d))
}
}
impl<const D: usize> Measurement for Distance<D> {
fn value(&self) -> f64 {
self.direction.dot(&(self.b - self.a))
}
}