use nalgebra::{IsometryMatrix3, Point3, Rotation3, Translation3, Vector3};
use simba::scalar::SupersetOf;
use crate::misc::FloatingPoint;
#[derive(Debug, Clone)]
pub struct FrenetFrame<T: FloatingPoint> {
position: Point3<T>,
tangent: Vector3<T>,
normal: Vector3<T>,
binormal: Vector3<T>,
}
impl<T: FloatingPoint> FrenetFrame<T> {
pub fn new(
position: Point3<T>,
tangent: Vector3<T>,
normal: Vector3<T>,
binormal: Vector3<T>,
) -> Self {
Self {
position,
tangent,
normal,
binormal,
}
}
pub fn position(&self) -> &Point3<T> {
&self.position
}
pub fn tangent(&self) -> &Vector3<T> {
&self.tangent
}
pub fn normal(&self) -> &Vector3<T> {
&self.normal
}
pub fn binormal(&self) -> &Vector3<T> {
&self.binormal
}
pub fn rotation(&self) -> Rotation3<T> {
Rotation3::face_towards(&self.tangent, &self.normal)
}
pub fn matrix(&self) -> IsometryMatrix3<T> {
let trans = Translation3::from(self.position);
let rot = self.rotation();
trans * rot
}
pub fn cast<F: FloatingPoint + SupersetOf<T>>(&self) -> FrenetFrame<F> {
FrenetFrame {
position: self.position.cast(),
tangent: self.tangent.cast(),
normal: self.normal.cast(),
binormal: self.binormal.cast(),
}
}
}