use super::curve::Curve;
use crate::math::Vector3;
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
pub enum CurveType {
#[default]
Centripetal,
Chordal,
CatmullRom,
}
#[derive(Clone, Copy, Debug, Default)]
struct CubicPoly {
c0: f64,
c1: f64,
c2: f64,
c3: f64,
}
impl CubicPoly {
fn init(&mut self, x0: f64, x1: f64, t0: f64, t1: f64) {
self.c0 = x0;
self.c1 = t0;
self.c2 = -3.0 * x0 + 3.0 * x1 - 2.0 * t0 - t1;
self.c3 = 2.0 * x0 - 2.0 * x1 + t0 + t1;
}
fn init_catmull_rom(&mut self, x0: f64, x1: f64, x2: f64, x3: f64, tension: f64) {
self.init(x1, x2, tension * (x2 - x0), tension * (x3 - x1));
}
#[allow(clippy::too_many_arguments)]
fn init_non_uniform_catmull_rom(
&mut self,
x0: f64,
x1: f64,
x2: f64,
x3: f64,
dt0: f64,
dt1: f64,
dt2: f64,
) {
let mut t1 = (x1 - x0) / dt0 - (x2 - x0) / (dt0 + dt1) + (x2 - x1) / dt1;
let mut t2 = (x2 - x1) / dt1 - (x3 - x1) / (dt1 + dt2) + (x3 - x2) / dt2;
t1 *= dt1;
t2 *= dt1;
self.init(x1, x2, t1, t2);
}
fn calc(&self, t: f64) -> f64 {
let t2 = t * t;
let t3 = t2 * t;
self.c0 + self.c1 * t + self.c2 * t2 + self.c3 * t3
}
}
#[derive(Clone, Debug)]
pub struct CatmullRomCurve3 {
pub points: Vec<Vector3>,
pub closed: bool,
pub curve_type: CurveType,
pub tension: f64,
}
impl CatmullRomCurve3 {
pub fn new(points: Vec<Vector3>) -> Self {
Self {
points,
closed: false,
curve_type: CurveType::Centripetal,
tension: 0.5,
}
}
}
impl Curve for CatmullRomCurve3 {
type Point = Vector3;
fn type_name(&self) -> &'static str {
"CatmullRomCurve3"
}
fn is_closed_catmull_rom(&self) -> bool {
self.closed
}
fn get_point(&self, t: f64) -> Vector3 {
let mut point = Vector3::default();
let points = &self.points;
let l = points.len();
let p = (l as f64 - if self.closed { 0.0 } else { 1.0 }) * t;
let mut int_point = p.floor();
let mut weight = p - int_point;
if self.closed {
int_point += if int_point > 0.0 {
0.0
} else {
((int_point.abs() / l as f64).floor() + 1.0) * l as f64
};
} else if weight == 0.0 && int_point == l as f64 - 1.0 {
int_point = l as f64 - 2.0;
weight = 1.0;
}
let int_point = int_point as isize;
let li = l as isize;
let tmp2;
let p0 = if self.closed || int_point > 0 {
&points[((int_point - 1) % li) as usize]
} else {
let mut v = Vector3::default();
v.sub_vectors(&points[0], &points[1]).add(&points[0]);
tmp2 = v;
&tmp2
};
let p1 = &points[(int_point % li) as usize];
let p2 = &points[((int_point + 1) % li) as usize];
let tmp;
let p3 = if self.closed || int_point + 2 < li {
&points[((int_point + 2) % li) as usize]
} else {
let mut v = Vector3::default();
v.sub_vectors(&points[l - 1], &points[l - 2])
.add(&points[l - 1]);
tmp = v;
&tmp
};
let mut px = CubicPoly::default();
let mut py = CubicPoly::default();
let mut pz = CubicPoly::default();
match self.curve_type {
CurveType::Centripetal | CurveType::Chordal => {
let pow = if self.curve_type == CurveType::Chordal {
0.5
} else {
0.25
};
let mut dt0 = p0.distance_to_squared(p1).powf(pow);
let mut dt1 = p1.distance_to_squared(p2).powf(pow);
let mut dt2 = p2.distance_to_squared(p3).powf(pow);
if dt1 < 1e-4 {
dt1 = 1.0;
}
if dt0 < 1e-4 {
dt0 = dt1;
}
if dt2 < 1e-4 {
dt2 = dt1;
}
px.init_non_uniform_catmull_rom(p0.x, p1.x, p2.x, p3.x, dt0, dt1, dt2);
py.init_non_uniform_catmull_rom(p0.y, p1.y, p2.y, p3.y, dt0, dt1, dt2);
pz.init_non_uniform_catmull_rom(p0.z, p1.z, p2.z, p3.z, dt0, dt1, dt2);
}
CurveType::CatmullRom => {
px.init_catmull_rom(p0.x, p1.x, p2.x, p3.x, self.tension);
py.init_catmull_rom(p0.y, p1.y, p2.y, p3.y, self.tension);
pz.init_catmull_rom(p0.z, p1.z, p2.z, p3.z, self.tension);
}
}
point.set(px.calc(weight), py.calc(weight), pz.calc(weight));
point
}
}