use crate::{Bezier, BezierHandles};
use glam::DVec2;
impl std::ops::Index<usize> for Bezier {
type Output = DVec2;
fn index(&self, index: usize) -> &Self::Output {
match &self.handles {
BezierHandles::Linear => [&self.start, &self.end][index],
BezierHandles::Quadratic { handle } => [&self.start, handle, &self.end][index],
BezierHandles::Cubic { handle_start, handle_end } => [&self.start, handle_start, handle_end, &self.end][index],
}
}
}
#[derive(Debug, Clone)]
pub(crate) struct SymmetricalBasis(pub Vec<DVec2>);
impl SymmetricalBasis {
#[must_use]
fn derivative(&self) -> SymmetricalBasis {
let mut c = SymmetricalBasis(vec![DVec2::ZERO; self.len()]);
if self.iter().all(|x| x.abs_diff_eq(DVec2::ZERO, 1e-5)) {
return c;
}
for k in 0..(self.len() - 1) {
let d = (2. * k as f64 + 1.) * (self[k][1] - self[k][0]);
c[k][0] = d + (k as f64 + 1.) * self[k + 1][0];
c[k][1] = d - (k as f64 + 1.) * self[k + 1][1];
}
let k = self.len() - 1;
let d = (2. * k as f64 + 1.) * (self[k][1] - self[k][0]);
if d == 0. && k > 0 {
c.pop();
} else {
c[k][0] = d;
c[k][1] = d;
}
c
}
#[must_use]
pub fn to_bezier1d(&self) -> Bezier1d {
let sb = self;
assert!(!sb.is_empty());
let n;
let even;
let mut q = sb.len();
if sb[q - 1][0] == sb[q - 1][1] {
even = true;
q -= 1;
n = 2 * q;
} else {
even = false;
n = 2 * q - 1;
}
let mut bz = Bezier1d(vec![0.; n + 1]);
for k in 0..q {
let mut tjk = 1.;
for j in k..(n - k) {
bz[j] += tjk * sb[k][0];
bz[n - j] += tjk * sb[k][1]; tjk = binomial_increment_k(tjk, n - 2 * k - 1, j - k);
}
}
if even {
bz[q] += sb[q][0];
}
let mut bcj = n as f64;
for j in 1..n {
bz[j] /= bcj;
bcj = binomial_increment_k(bcj, n, j);
}
bz[0] = sb[0][0];
bz[n] = sb[0][1];
bz
}
fn normalize(&mut self) {
while self.len() > 1 && self.last().is_some_and(|x| x.abs_diff_eq(DVec2::ZERO, 1e-5)) {
self.pop();
}
}
#[must_use]
pub(crate) fn roots(&self) -> Vec<f64> {
match self.len() {
0 => Vec::new(),
1 => {
let mut res = Vec::new();
let d = self[0].x - self[0].y;
if d != 0. {
let r = self[0].x / d;
if (0. ..=1.).contains(&r) {
res.push(r);
}
}
res
}
_ => {
let mut bz = self.to_bezier1d();
let mut solutions = Vec::new();
if bz.len() == 0 || bz.iter().all(|&x| (x - bz[0]).abs() < 1e-5) {
return solutions;
}
while bz[0] == 0. {
bz = bz.deflate();
solutions.push(0.);
}
if bz.len() - 1 == 1 {
if bz[0].signum() != bz[1].signum() {
let d = bz[0] - bz[1];
if d != 0. {
let r = bz[0] / d;
if (0. ..=1.).contains(&r) {
solutions.push(r);
}
}
}
return solutions;
}
bz.find_bernstein_roots(&mut solutions, 0, 0., 1.);
solutions.sort_by(f64::total_cmp);
solutions
}
}
}
}
impl<'a> std::ops::Mul for &'a SymmetricalBasis {
type Output = SymmetricalBasis;
fn mul(self, b: Self) -> Self::Output {
let a = self;
if a.iter().all(|x| x.abs_diff_eq(DVec2::ZERO, 1e-5)) || b.iter().all(|x| x.abs_diff_eq(DVec2::ZERO, 1e-5)) {
return SymmetricalBasis(vec![DVec2::ZERO]);
}
let mut c = SymmetricalBasis(vec![DVec2::ZERO; a.len() + b.len()]);
for j in 0..b.len() {
for i in j..(a.len() + j) {
let tri = (b[j][1] - b[j][0]) * (a[i - j][1] - a[i - j][0]);
c[i + 1] += DVec2::splat(-tri);
}
}
for j in 0..b.len() {
for i in j..(a.len() + j) {
for dim in 0..2 {
c[i][dim] += b[j][dim] * a[i - j][dim];
}
}
}
c.normalize();
c
}
}
impl std::ops::Add for SymmetricalBasis {
type Output = SymmetricalBasis;
fn add(self, b: Self) -> Self::Output {
let a = self;
let out_size = a.len().max(b.len());
let min_size = a.len().min(b.len());
let mut result = SymmetricalBasis(vec![DVec2::ZERO; out_size]);
for i in 0..min_size {
result[i] = a[i] + b[i];
}
for i in min_size..a.len() {
result[i] = a[i];
}
for i in min_size..b.len() {
result[i] = b[i];
}
result
}
}
impl std::ops::Sub for SymmetricalBasis {
type Output = SymmetricalBasis;
fn sub(self, b: Self) -> Self::Output {
let a = self;
let out_size = a.len().max(b.len());
let min_size = a.len().min(b.len());
let mut result = SymmetricalBasis(vec![DVec2::ZERO; out_size]);
for i in 0..min_size {
result[i] = a[i] - b[i];
}
for i in min_size..a.len() {
result[i] = a[i];
}
for i in min_size..b.len() {
result[i] = -b[i];
}
result
}
}
impl std::ops::Deref for SymmetricalBasis {
type Target = Vec<DVec2>;
fn deref(&self) -> &Self::Target {
&self.0
}
}
impl std::ops::DerefMut for SymmetricalBasis {
fn deref_mut(&mut self) -> &mut Self::Target {
&mut self.0
}
}
#[derive(Debug, Clone)]
pub(crate) struct SymmetricalBasisPair {
pub x: SymmetricalBasis,
pub y: SymmetricalBasis,
}
impl SymmetricalBasisPair {
#[must_use]
pub fn derivative(&self) -> Self {
Self {
x: self.x.derivative(),
y: self.y.derivative(),
}
}
#[must_use]
pub fn dot(&self, other: &Self) -> SymmetricalBasis {
(&self.x * &other.x) + (&self.y * &other.y)
}
#[must_use]
pub fn cross(&self, rhs: &Self) -> SymmetricalBasis {
(&self.x * &rhs.y) - (&self.y * &rhs.x)
}
}
impl std::ops::Sub<DVec2> for SymmetricalBasisPair {
type Output = SymmetricalBasisPair;
fn sub(self, rhs: DVec2) -> Self::Output {
let sub = |a: &SymmetricalBasis, b: f64| {
if a.iter().all(|x| x.abs_diff_eq(DVec2::ZERO, 1e-5)) {
return SymmetricalBasis(vec![DVec2::splat(-b)]);
}
let mut result = a.clone();
result[0] -= DVec2::splat(b);
result
};
Self {
x: sub(&self.x, rhs.x),
y: sub(&self.y, rhs.y),
}
}
}
#[derive(Debug, Clone)]
pub struct Bezier1d(pub Vec<f64>);
impl std::ops::Deref for Bezier1d {
type Target = Vec<f64>;
fn deref(&self) -> &Self::Target {
&self.0
}
}
impl std::ops::DerefMut for Bezier1d {
fn deref_mut(&mut self) -> &mut Self::Target {
&mut self.0
}
}
impl Bezier1d {
const MAX_DEPTH: u32 = 53;
#[must_use]
fn deflate(&self) -> Self {
let bz = self;
if bz.is_empty() {
return Bezier1d(Vec::new());
}
let n = bz.len() - 1;
let mut b = Bezier1d(vec![0.; n]);
for i in 0..n {
b[i] = (n as f64 * bz[i + 1]) / (i as f64 + 1.)
}
b
}
#[must_use]
fn value_at(&self, t: f64) -> f64 {
let bz = self;
let order = bz.len() - 1;
let u = 1.0 - t;
let mut bc = 1.;
let mut tn = 1.;
let mut tmp = bz[0] * u;
for i in 1..order {
tn *= t;
bc = bc * (order as f64 - i as f64 + 1.) / i as f64;
tmp = (tmp + tn * bc * bz[i]) * u;
}
tmp + tn * t * bz[bz.len() - 1]
}
#[must_use]
fn secant(&self) -> f64 {
let bz = self;
let mut s = 0.;
let mut t = 1.;
let e = 1e-14;
let mut side = 0;
let mut r = 0.;
let mut fs = bz[0];
let mut ft = bz[bz.len() - 1];
for _n in 0..100 {
r = (fs * t - ft * s) / (fs - ft);
if (t - s).abs() < e * (t + s).abs() {
return r;
}
let fr = self.value_at(r);
if fr * ft > 0. {
t = r;
ft = fr;
if side == -1 {
fs /= 2.;
}
side = -1;
} else if fs * fr > 0. {
s = r;
fs = fr;
if side == 1 {
ft /= 2.;
}
side = 1;
} else {
break;
}
}
r
}
fn casteljau_subdivision(&self, t: f64) -> [Self; 2] {
let v = self;
let order = v.len() - 1;
let mut left = v.clone();
let mut right = v.clone();
let val = v.value_at(t);
for i in (1..=order).rev() {
left[i - 1] = right[0];
for j in i..v.len() {
right[j - 1] = right[j - 1] + ((right[j] - right[j - 1]) * t);
}
}
right[0] = val;
left[order] = right[0];
[left, right]
}
fn derivative(&self) -> Self {
let bz = self;
if bz.len() - 1 == 1 {
return Bezier1d(vec![bz[1] - bz[0]]);
}
let mut der = Bezier1d(vec![0.; bz.len() - 1]);
for i in 0..(bz.len() - 1) {
der[i] = (bz.len() - 1) as f64 * (bz[i + 1] - bz[i]);
}
der
}
fn find_bernstein_roots(&self, solutions: &mut Vec<f64>, depth: u32, left_t: f64, right_t: f64) {
let bz = self;
let mut n_crossings = 0;
let mut old_sign = bz[0].signum();
for i in 1..bz.len() {
let sign = bz[i].signum();
if sign != 0. {
if sign != old_sign && old_sign != 0. {
n_crossings += 1;
}
old_sign = sign;
}
}
if bz[bz.len() - 1].signum() == 0. {
n_crossings += 1;
}
if n_crossings == 0 {
return;
}
if n_crossings == 1 {
if depth > Self::MAX_DEPTH {
let ax = right_t - left_t;
let ay = bz.last().unwrap() - bz[0];
solutions.push(left_t - ax * bz[0] / ay);
return;
}
let r = bz.secant();
solutions.push(r * right_t + (1. - r) * left_t);
return;
}
let o = bz.len() - 1;
let mut left = Bezier1d(vec![0.; o + 1]);
let mut right = bz.clone();
let mut split_t = (left_t + right_t) * 0.5;
if depth > 2 {
let dbz = bz.derivative();
let mut d_solutions = Vec::new();
dbz.find_bernstein_roots(&mut d_solutions, 0, left_t, right_t);
d_solutions.sort_by(f64::total_cmp);
let mut d_split_t = 0.5;
if !d_solutions.is_empty() {
d_split_t = d_solutions[0];
split_t = left_t + (right_t - left_t) * d_split_t;
}
[left, right] = bz.casteljau_subdivision(d_split_t);
} else {
left[0] = right[0];
for i in 1..bz.len() {
for j in 0..(bz.len() - i) {
right[j] = (right[j] + right[j + 1]) * 0.5;
}
left[i] = right[0];
}
}
left.reverse();
while right.len() - 1 > 0 && (right[0]).abs() <= 1e-10 {
right = right.deflate();
left = left.deflate();
solutions.push(split_t);
}
left.reverse();
if right.len() - 1 > 0 {
left.find_bernstein_roots(solutions, depth + 1, left_t, split_t);
right.find_bernstein_roots(solutions, depth + 1, split_t, right_t);
}
}
}
#[must_use]
fn binomial_increment_k(b: f64, n: usize, k: usize) -> f64 {
b * (n as f64 - k as f64) / (k + 1) as f64
}
#[must_use]
fn binomial_decrement_n(b: f64, n: usize, k: usize) -> f64 {
b * (n as f64 - k as f64) / n as f64
}
#[must_use]
pub(crate) fn to_symmetrical_basis_pair(bezier: Bezier) -> SymmetricalBasisPair {
let n = match bezier.handles {
BezierHandles::Linear => 1,
BezierHandles::Quadratic { .. } => 2,
BezierHandles::Cubic { .. } => 3,
};
let q = (n + 1) / 2;
let even = n % 2 == 0;
let mut sb = SymmetricalBasisPair {
x: SymmetricalBasis(vec![DVec2::ZERO; q + even as usize]),
y: SymmetricalBasis(vec![DVec2::ZERO; q + even as usize]),
};
let mut nck = 1.;
for k in 0..q {
let mut tjk = nck;
for j in k..q {
sb.x[j][0] += tjk * bezier[k].x;
sb.x[j][1] += tjk * bezier[n - k].x;
sb.y[j][0] += tjk * bezier[k].y;
sb.y[j][1] += tjk * bezier[n - k].y;
tjk = binomial_increment_k(tjk, n - j - k, j - k);
tjk = binomial_decrement_n(tjk, n - j - k, j - k + 1);
tjk = -tjk;
}
tjk = -nck;
for j in (k + 1)..q {
sb.x[j][0] += tjk * bezier[n - k].x;
sb.x[j][1] += tjk * bezier[k].x;
sb.y[j][0] += tjk * bezier[n - k].y;
sb.y[j][1] += tjk * bezier[k].y;
tjk = binomial_increment_k(tjk, n - j - k - 1, j - k - 1);
tjk = binomial_decrement_n(tjk, n - j - k - 1, j - k);
tjk = -tjk;
}
nck = binomial_increment_k(nck, n, k);
}
if even {
let mut tjk = if q % 2 == 1 { -1. } else { 1. };
for k in 0..q {
sb.x[q][0] += tjk * (bezier[k].x + bezier[n - k].x);
sb.y[q][0] += tjk * (bezier[k].y + bezier[n - k].y);
tjk = binomial_increment_k(tjk, n, k);
tjk = -tjk;
}
sb.x[q][0] += tjk * bezier[q].x;
sb.x[q][1] = sb.x[q][0];
sb.y[q][0] += tjk * bezier[q].y;
sb.y[q][1] = sb.y[q][0];
}
sb.x[0][0] = bezier[0].x;
sb.x[0][1] = bezier[n].x;
sb.y[0][0] = bezier[0].y;
sb.y[0][1] = bezier[n].y;
sb
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn find_bernstein_roots() {
let bz = Bezier1d(vec![50.0, -100.0, 170.0]);
let mut solutions = Vec::new();
bz.find_bernstein_roots(&mut solutions, 0, 0., 1.);
solutions.sort_by(f64::total_cmp);
for &t in &solutions {
assert!(bz.value_at(t,).abs() < 1e-5, "roots should be roots {} {}", t, bz.value_at(t,));
}
}
}