use crate::math::math_utils::clamp;
use crate::math::{Matrix4, Vector2, Vector3};
#[derive(Clone, Debug, Default, PartialEq)]
pub struct FrenetFrames {
pub tangents: Vec<Vector3>,
pub normals: Vec<Vector3>,
pub binormals: Vec<Vector3>,
}
mod sealed {
pub trait Sealed {}
impl Sealed for crate::math::Vector2 {}
impl Sealed for crate::math::Vector3 {}
}
pub trait CurveVector:
sealed::Sealed + Copy + Default + PartialEq + std::fmt::Debug + 'static
{
fn distance_to(&self, v: &Self) -> f64;
fn equals(&self, v: &Self) -> bool;
fn tangent(pt1: &Self, pt2: &Self) -> Self;
fn to_vector3(&self) -> Vector3;
}
impl CurveVector for Vector2 {
fn distance_to(&self, v: &Self) -> f64 {
Vector2::distance_to(self, v)
}
fn equals(&self, v: &Self) -> bool {
Vector2::equals(self, v)
}
fn tangent(pt1: &Self, pt2: &Self) -> Self {
let mut tangent = Vector2::default();
tangent.copy(pt2).sub(pt1).normalize();
tangent
}
fn to_vector3(&self) -> Vector3 {
Vector3::new(self.x, self.y, 0.0)
}
}
impl CurveVector for Vector3 {
fn distance_to(&self, v: &Self) -> f64 {
Vector3::distance_to(self, v)
}
fn equals(&self, v: &Self) -> bool {
Vector3::equals(self, v)
}
fn tangent(pt1: &Self, pt2: &Self) -> Self {
let mut tangent = Vector3::default();
tangent.copy(pt2).sub(pt1).normalize();
tangent
}
fn to_vector3(&self) -> Vector3 {
*self
}
}
pub(crate) fn compute_lengths<C: Curve + ?Sized>(curve: &C, divisions: usize) -> Vec<f64> {
let mut cache = Vec::new();
let mut last = curve.get_point(0.0);
let mut sum = 0.0;
cache.push(0.0);
for p in 1..=divisions {
let current = curve.get_point(p as f64 / divisions as f64);
sum += current.distance_to(&last);
cache.push(sum);
last = current;
}
cache
}
pub trait Curve {
type Point: CurveVector;
fn get_point(&self, t: f64) -> Self::Point;
fn type_name(&self) -> &'static str;
fn curve_path_resolution(&self, divisions: usize) -> usize {
divisions
}
fn is_closed_catmull_rom(&self) -> bool {
false
}
fn arc_length_divisions(&self) -> usize {
200
}
fn get_point_at(&self, u: f64) -> Self::Point {
let t = self.get_u_to_t_mapping(u, None);
self.get_point(t)
}
fn get_points(&self, divisions: usize) -> Vec<Self::Point> {
let mut points = Vec::new();
for d in 0..=divisions {
points.push(self.get_point(d as f64 / divisions as f64));
}
points
}
fn get_spaced_points(&self, divisions: usize) -> Vec<Self::Point> {
let mut points = Vec::new();
for d in 0..=divisions {
points.push(self.get_point_at(d as f64 / divisions as f64));
}
points
}
fn get_length(&self) -> f64 {
let lengths = self.get_lengths(self.arc_length_divisions());
lengths[lengths.len() - 1]
}
fn get_lengths(&self, divisions: usize) -> Vec<f64> {
compute_lengths(self, divisions)
}
fn get_u_to_t_mapping(&self, u: f64, distance: Option<f64>) -> f64 {
let arc_lengths = self.get_lengths(self.arc_length_divisions());
let il = arc_lengths.len();
let target_arc_length = match distance {
Some(distance) if distance != 0.0 => distance,
_ => u * arc_lengths[il - 1],
};
let (mut low, mut high): (isize, isize) = (0, il as isize - 1);
while low <= high {
let i = low + (high - low) / 2;
let comparison = arc_lengths[i as usize] - target_arc_length;
if comparison < 0.0 {
low = i + 1;
} else if comparison > 0.0 {
high = i - 1;
} else {
high = i;
break;
}
}
let i = high;
assert!(
i >= 0,
"three-rs: Curve::get_u_to_t_mapping: target arc length {target_arc_length} lies before the start of the curve"
);
let i = i as usize;
if arc_lengths[i] == target_arc_length {
return i as f64 / (il - 1) as f64;
}
assert!(
i + 1 < il,
"three-rs: Curve::get_u_to_t_mapping: target arc length {target_arc_length} lies past the end of the curve"
);
let length_before = arc_lengths[i];
let length_after = arc_lengths[i + 1];
let segment_length = length_after - length_before;
let segment_fraction = (target_arc_length - length_before) / segment_length;
(i as f64 + segment_fraction) / (il - 1) as f64
}
fn get_tangent(&self, t: f64) -> Self::Point {
let delta = 0.0001;
let mut t1 = t - delta;
let mut t2 = t + delta;
if t1 < 0.0 {
t1 = 0.0;
}
if t2 > 1.0 {
t2 = 1.0;
}
let pt1 = self.get_point(t1);
let pt2 = self.get_point(t2);
Self::Point::tangent(&pt1, &pt2)
}
fn get_tangent_at(&self, u: f64) -> Self::Point {
let t = self.get_u_to_t_mapping(u, None);
self.get_tangent(t)
}
fn compute_frenet_frames(&self, segments: usize, closed: bool) -> FrenetFrames {
let mut normal = Vector3::default();
let mut tangents: Vec<Vector3> = Vec::new();
let mut normals: Vec<Vector3> = Vec::new();
let mut binormals: Vec<Vector3> = Vec::new();
let mut vec = Vector3::default();
let mut mat = Matrix4::default();
for i in 0..=segments {
let u = i as f64 / segments as f64;
tangents.push(self.get_tangent_at(u).to_vector3());
}
normals.push(Vector3::default());
binormals.push(Vector3::default());
let mut min = f64::MAX;
let tx = tangents[0].x.abs();
let ty = tangents[0].y.abs();
let tz = tangents[0].z.abs();
if tx <= min {
min = tx;
normal.set(1.0, 0.0, 0.0);
}
if ty <= min {
min = ty;
normal.set(0.0, 1.0, 0.0);
}
if tz <= min {
normal.set(0.0, 0.0, 1.0);
}
vec.cross_vectors(&tangents[0], &normal).normalize();
let n0 = *Vector3::default().cross_vectors(&tangents[0], &vec);
normals[0] = n0;
binormals[0] = *Vector3::default().cross_vectors(&tangents[0], &normals[0]);
for i in 1..=segments {
normals.push(normals[i - 1]);
binormals.push(binormals[i - 1]);
vec.cross_vectors(&tangents[i - 1], &tangents[i]);
if vec.length() > f64::EPSILON {
vec.normalize();
let theta = clamp(tangents[i - 1].dot(&tangents[i]), -1.0, 1.0).acos();
normals[i].apply_matrix4(mat.make_rotation_axis(&vec, theta));
}
binormals[i] = *Vector3::default().cross_vectors(&tangents[i], &normals[i]);
}
if closed {
let mut theta = clamp(normals[0].dot(&normals[segments]), -1.0, 1.0).acos();
theta /= segments as f64;
if tangents[0].dot(vec.cross_vectors(&normals[0], &normals[segments])) > 0.0 {
theta = -theta;
}
for i in 1..=segments {
normals[i].apply_matrix4(mat.make_rotation_axis(&tangents[i], theta * i as f64));
binormals[i] = *Vector3::default().cross_vectors(&tangents[i], &normals[i]);
}
}
FrenetFrames {
tangents,
normals,
binormals,
}
}
}