efd/
dim.rs

1use crate::{util::*, *};
2use alloc::{vec, vec::Vec};
3use core::{
4    f64::consts::{PI, TAU},
5    iter::zip,
6};
7#[cfg(not(feature = "std"))]
8#[allow(unused_imports)]
9use num_traits::*;
10
11/// EFD dimension marker.
12pub enum U<const D: usize> {}
13
14/// Coefficients type.
15pub type Coeffs<const D: usize> = Vec<Kernel<D>>;
16/// An owned matrix of specific coefficients. (Dx2)
17pub type Kernel<const D: usize> = na::SMatrix<f64, D, 2>;
18/// Rotation type of the EFD.
19pub type Rot<const D: usize> = <U<D> as EfdDim<D>>::Rot;
20
21trait Sealed {}
22impl<const D: usize> Sealed for U<D> {}
23
24/// Trait for the dimension [`U<D>`] of EFD.
25///
26/// Use `where U<D>: EfdDim<D>` bound to constraint the dimension `D` that
27/// implements this trait. Please see the [implementors section](#implementors)
28/// for the supported dimensions.
29///
30/// **This trait is sealed and cannot be implemented outside of this crate.**
31/// The API of this trait is not public and may change in the future.
32#[allow(private_bounds)]
33pub trait EfdDim<const D: usize>: Sync + Send + Sealed {
34    /// Rotation type of the dimension `D`.
35    ///
36    /// For the memory efficiency, the generic rotation matrix [`na::Rotation`]
37    /// is not used.
38    type Rot: RotHint<D>;
39
40    #[doc(hidden)]
41    fn get_rot(m: &[Kernel<D>]) -> Self::Rot;
42
43    #[doc(hidden)]
44    #[allow(clippy::type_complexity)]
45    fn get_coeff(
46        curve: &[[f64; D]],
47        is_open: bool,
48        harmonic: usize,
49        guide: Option<&[f64]>,
50    ) -> (Vec<f64>, Coeffs<D>, GeoVar<Self::Rot, D>) {
51        let is_closed = !is_open && curve.first() != curve.last();
52        // Differential of the components
53        let dxyz = diff(if is_closed {
54            to_mat(curve.closed_lin())
55        } else {
56            to_mat(curve)
57        });
58        // Length of curve between points
59        // from: (1) provided guide, or
60        //       (2) differential of the components
61        let dt = if let Some(guide) = guide {
62            debug_assert_eq!(guide.len(), dxyz.ncols());
63            na::Matrix1xX::from_row_slice(guide)
64        } else {
65            dxyz.map(pow2).row_sum().map(f64::sqrt)
66        };
67        // Length of curve from start to each point
68        let t = cumsum(dt.clone()).insert_column(0, 0.);
69        // Total length of curve
70        let zt = t[t.len() - 1];
71        // Length to angle
72        let phi = &t * TAU / zt * if is_open { 0.5 } else { 1. };
73        // Scalar for coefficients
74        let scalar = zt / pow2(PI) * if is_open { 2. } else { 0.5 };
75        // Coefficients (2dim * N)
76        // [x_cos, y_cos, z_cos, x_sin, y_sin, z_sin]'
77        let mut coeff = vec![Kernel::<D>::zeros(); harmonic];
78        for (n, c) in coeff.iter_mut().enumerate() {
79            let n = (n + 1) as f64;
80            let phi = &phi * n;
81            let scalar = scalar / pow2(n);
82            let cos_phi = diff(phi.map(f64::cos)).component_div(&dt);
83            zip(dxyz.row_iter(), &mut c.column_mut(0))
84                .for_each(|(d, c)| *c = scalar * d.component_mul(&cos_phi).sum());
85            if is_open {
86                continue;
87            }
88            let sin_phi = diff(phi.map(f64::sin)).component_div(&dt);
89            zip(dxyz.row_iter(), &mut c.column_mut(1))
90                .for_each(|(d, c)| *c = scalar * d.component_mul(&sin_phi).sum());
91        }
92        // Percentage of total stroke versus current stroke
93        let tdt = t.columns_range(1..).component_div(&dt);
94        // Scalar for the shape center
95        let scalar = 0.5 * diff(t.map(pow2)).component_div(&dt);
96        // Shape center
97        let mut center = curve[0];
98        for (dxyz, oxyz) in zip(dxyz.row_iter(), &mut center) {
99            let xi = cumsum(dxyz) - dxyz.component_mul(&tdt);
100            *oxyz += (dxyz.component_mul(&scalar) + xi.component_mul(&dt)).sum() / zt;
101        }
102        // Keep the t number the same as the input curve
103        let mut t = Vec::from(phi.data);
104        if is_closed {
105            t.pop();
106        }
107        (t, coeff, GeoVar::from_trans(center))
108    }
109
110    #[doc(hidden)]
111    fn norm_coeff(coeffs: &mut [Kernel<D>], mut t: Option<&mut [f64]>) -> GeoVar<Self::Rot, D> {
112        // Angle of starting point (theta)
113        // theta = atan2(2 * sum(m[:, 0] * m[:, 1]), sum(m[:, 0]^2) - sum(m[:, 1]^2))
114        // theta = 0 if is open curve
115        // m = m * theta
116        if coeffs[0].column(1).sum() != 0. {
117            let theta = {
118                let m1 = &coeffs[0];
119                let dy = 2. * m1.column_product().sum();
120                let dx = m1.map(pow2).row_sum();
121                0.5 * dy.atan2(dx[0] - dx[1])
122            };
123            for (i, m) in coeffs.iter_mut().enumerate() {
124                let theta = na::Rotation2::new((i + 1) as f64 * theta);
125                m.copy_from(&(*m * theta));
126            }
127            if let Some(t) = &mut t {
128                t.iter_mut().for_each(|v| *v -= theta);
129            }
130        }
131        // Normalize coefficients sign (zeta)
132        // - Check 1st and 2nd harmonics if two local coordinate systems are the closest
133        // - Plus PI to time parameters if zeta is -1
134        Self::norm_zeta(coeffs, t);
135        // Rotation angle (psi)
136        // m = psi' * m
137        let psi = Self::get_rot(coeffs);
138        let psi_mat = psi.clone().matrix();
139        for m in coeffs.iter_mut() {
140            m.tr_mul(&psi_mat).transpose_to(m);
141        }
142        // Scaling factor
143        // |u1| == a1 (after rotation normalized)
144        let scale = coeffs[0][0];
145        coeffs.iter_mut().for_each(|m| *m /= scale);
146        debug_assert!(scale.is_sign_positive());
147        GeoVar::new([0.; D], psi, scale)
148    }
149
150    #[doc(hidden)]
151    fn norm_zeta(coeffs: &mut [Kernel<D>], t: Option<&mut [f64]>) {
152        if coeffs.len() > 1 && {
153            let [u1, v1] = [coeffs[0].column(0), coeffs[0].column(1)];
154            let [u2, v2] = [coeffs[1].column(0), coeffs[1].column(1)];
155            (u2 - u1).norm() + (v2 - v1).norm() > (u2 + u1).norm() + (v2 + v1).norm()
156        } {
157            coeffs.iter_mut().step_by(2).for_each(|s| *s *= -1.);
158            if let Some(t) = t {
159                t.iter_mut().for_each(|v| *v += PI);
160            }
161        }
162    }
163
164    #[doc(hidden)]
165    fn reconstruct(
166        coeffs: &[Kernel<D>],
167        t_iter: impl ExactSizeIterator<Item = f64>,
168    ) -> Vec<[f64; D]> {
169        let t = na::Matrix1xX::from_iterator(t_iter.len(), t_iter);
170        coeffs
171            .iter()
172            .enumerate()
173            .map(|(n, m)| {
174                let t = (n + 1) as f64 * &t;
175                m * na::Matrix2xX::from_rows(&[t.map(f64::cos), t.map(f64::sin)])
176            })
177            .reduce(|a, b| a + b)
178            .unwrap_or_else(|| MatrixRxX::from_vec(Vec::new())) // empty coeffs
179            .column_iter()
180            .map(|row| row.into())
181            .collect()
182    }
183}
184
185impl EfdDim<1> for U<1> {
186    type Rot = na::Rotation<f64, 1>;
187
188    fn get_rot(m: &[Kernel<1>]) -> Self::Rot {
189        na::Rotation::from_matrix_unchecked(na::matrix![m[0][0].signum()])
190    }
191}
192
193impl EfdDim<2> for U<2> {
194    type Rot = na::UnitComplex<f64>;
195
196    fn get_rot(m: &[Kernel<2>]) -> Self::Rot {
197        na::UnitComplex::new(m[0][1].atan2(m[0][0]))
198    }
199}
200
201impl EfdDim<3> for U<3> {
202    type Rot = na::UnitQuaternion<f64>;
203
204    fn get_rot(m: &[Kernel<3>]) -> Self::Rot {
205        let m1 = &m[0];
206        let u = m1.column(0).normalize();
207        if let Some(v) = m1.column(1).try_normalize(f64::EPSILON) {
208            // Closed curve, use `u` and `v` plane as basis
209            let w = u.cross(&v);
210            na::UnitQuaternion::from_basis_unchecked(&[u, v, w])
211        } else if m.len() > 1 {
212            // Open curve, `v` is zero vector, use `u1` and `u2` plane as basis
213            let u2 = m[1].column(0);
214            // `w` is orthogonal to `u` and `u2`
215            let w = u.cross(&u2).normalize();
216            // A new `v` is orthogonal to `w` and `u`
217            let v = w.cross(&u);
218            na::UnitQuaternion::from_basis_unchecked(&[u, v, w])
219        } else {
220            // Open curve, one harmonic, just rotate `u` to x-axis
221            let [u, x] = [na::Unit::new_unchecked(u), na::Vector3::x_axis()];
222            na::UnitQuaternion::rotation_between_axis(&u, &x).unwrap_or_default()
223        }
224    }
225}
efd/dim.rs

efd/
dim.rs
