del-msh-cpu 0.1.52

//! methods related to 3D polyline

use num_traits::{AsPrimitive, FloatConst};

/// the center of gravity
pub fn cog<T>(vtx2xyz: &[[T; 3]]) -> [T; 3]
where
    T: num_traits::Float,
{
    let one = T::one();
    let half = one / (one + one);
    let num_vtx = vtx2xyz.len();
    let mut cg = [T::zero(); 3];
    let mut w = T::zero();
    use del_geo_core::vec3::Vec3;
    for iseg in 0..num_vtx - 1 {
        let p0 = &vtx2xyz[iseg];
        let p1 = &vtx2xyz[iseg + 1];
        let len = p0.sub(p1).norm();
        cg = cg.add(&p0.add(p1).scale(half * len));
        w = w + len;
    }
    cg.scale(T::one() / w)
}

/// bi-normal vector on each vertex
pub fn vtx2framex<T>(vtx2xyz: &[[T; 3]]) -> Vec<[T; 3]>
where
    T: num_traits::Float,
{
    use del_geo_core::vec3::Vec3;
    let num_vtx = vtx2xyz.len();
    let mut vtx2bin = vec![[T::zero(); 3]; num_vtx];
    {
        // first segment
        let p1 = &vtx2xyz[1];
        let p0 = &vtx2xyz[0];
        let v01 = p1.sub(p0);
        let (x, _) = del_geo_core::vec3::basis_xy_from_basis_z(&v01);
        vtx2bin[0] = x;
    }
    for i_seg1 in 1..num_vtx - 1 {
        let iv0 = i_seg1 - 1;
        let iv1 = i_seg1;
        let iv2 = i_seg1 + 1;
        let i_seg0 = i_seg1 - 1;
        let p0 = &vtx2xyz[iv0];
        let p1 = &vtx2xyz[iv1];
        let p2 = &vtx2xyz[iv2];
        let v01 = p1.sub(p0);
        let v12 = p2.sub(p1);
        let rot = del_geo_core::mat3_col_major::minimum_rotation_matrix(&v01, &v12);
        let b12: [T; 3] = del_geo_core::mat3_col_major::mult_vec(&rot, &vtx2bin[i_seg0]);
        vtx2bin[i_seg1] = b12;
    }
    vtx2bin[num_vtx - 1] = vtx2bin[num_vtx - 2];
    vtx2bin
}

#[test]
fn test_vtx2framex() {
    let num_vtx = 10usize;
    let length = 3.0f64;
    let vtx2xyz: Vec<[f64; 3]> = (0..num_vtx)
        .map(|i_vtx| [0., i_vtx as f64 / num_vtx as f64 * length, 0.])
        .collect();
    let vtx2framex = vtx2framex(&vtx2xyz);
    vtx2framex.iter().for_each(|v| {
        let len = del_geo_core::vec3::norm(v);
        assert!((len - 1.0).abs() < 1.0e-8);
    });
}

pub fn framez<T>(vtx2xyz: &[[T; 3]], i_vtx: usize) -> [T; 3]
where
    T: num_traits::Float + Copy,
{
    let num_vtx = vtx2xyz.len();
    assert!(i_vtx < num_vtx);
    use del_geo_core::vec3::Vec3;
    if i_vtx == 0 {
        let p1 = &vtx2xyz[0];
        let p2 = &vtx2xyz[1];
        return p2.sub(p1).normalize();
    }
    if i_vtx == num_vtx - 1 {
        let p0 = &vtx2xyz[num_vtx - 2];
        let p1 = &vtx2xyz[num_vtx - 1];
        return p1.sub(p0).normalize();
    }
    let p0 = &vtx2xyz[i_vtx - 1];
    let p1 = &vtx2xyz[i_vtx];
    let p2 = &vtx2xyz[i_vtx + 1];
    let u01 = p1.sub(p0).normalize();
    let u12 = p2.sub(p1).normalize();
    u01.add(&u12).normalize()
}

pub fn vtx2framey<T>(vtx2xyz: &[[T; 3]], vtx2framex: &[[T; 3]]) -> Vec<[T; 3]>
where
    T: num_traits::Float,
{
    use del_geo_core::vec3::Vec3;
    let num_vtx = vtx2xyz.len();
    assert_eq!(vtx2framex.len(), num_vtx);
    let mut vtx2framey = vec![[T::zero(); 3]; num_vtx];
    for i_vtx in 0..num_vtx {
        let framez = framez(vtx2xyz, i_vtx);
        let framex = &vtx2framex[i_vtx];
        vtx2framey[i_vtx] = framez.cross(framex);
    }
    vtx2framey
}

pub fn normal_binormal<T>(vtx2xyz: &[[T; 3]]) -> (Vec<[T; 3]>, Vec<[T; 3]>)
where
    T: num_traits::Float + Copy,
{
    use del_geo_core::vec3::Vec3;
    let num_vtx = vtx2xyz.len();
    let mut vtx2bin = vec![[T::zero(); 3]; num_vtx];
    let mut vtx2nrm = vec![[T::zero(); 3]; num_vtx];
    for ivtx1 in 1..num_vtx - 1 {
        let ivtx0 = (ivtx1 + num_vtx - 1) % num_vtx;
        let ivtx2 = (ivtx1 + 1) % num_vtx;
        let v0 = &vtx2xyz[ivtx0];
        let v1 = &vtx2xyz[ivtx1];
        let v2 = &vtx2xyz[ivtx2];
        let v01 = v1.sub(v0);
        let v12 = v2.sub(v1);
        let binormal = v12.cross(&v01);
        vtx2bin[ivtx1] = binormal.normalize();
        let norm = v01.add(&v12).cross(&binormal);
        vtx2nrm[ivtx1] = norm.normalize();
    }
    vtx2nrm[0] = vtx2nrm[1];
    vtx2nrm[num_vtx - 1] = vtx2nrm[num_vtx - 2];
    vtx2bin[0] = vtx2bin[1];
    vtx2bin[num_vtx - 1] = vtx2bin[num_vtx - 2];
    (vtx2nrm, vtx2bin)
}

pub fn set_vtx2xyz_for_generalized_cylinder_open_end<Index, T>(
    vtx2xyz: &mut [[T; 3]],
    vtxl2xyz: &[[T; 3]],
    rad: T,
) where
    T: num_traits::Float + num_traits::FloatConst + 'static,
    usize: num_traits::AsPrimitive<T>,
{
    use del_geo_core::vec3::Vec3;
    use num_traits::AsPrimitive;
    let one = T::one();
    let two = one + one;
    let pi = T::PI();
    let num_vtxl = vtxl2xyz.len();
    let num_vtx = vtx2xyz.len();
    let ndiv_circum = num_vtx / num_vtxl;
    let vtxl2framex = vtx2framex(vtxl2xyz);
    let vtxl2framey = vtx2framey(vtxl2xyz, &vtxl2framex);
    for i_vtxl in 0..num_vtxl {
        let p0 = &vtxl2xyz[i_vtxl];
        let ex = &vtxl2framex[i_vtxl];
        let ey = &vtxl2framey[i_vtxl];
        for ic in 0..ndiv_circum {
            let theta = two * pi * ic.as_() / ndiv_circum.as_();
            let ay = ey.scale(rad * num_traits::Float::cos(theta));
            let ax = ex.scale(rad * num_traits::Float::sin(theta));
            let q = p0.add(&ay).add(&ax);
            vtx2xyz[i_vtxl * ndiv_circum + ic] = q;
        }
    }
}

pub fn to_trimesh3_capsule<T>(
    vtxl2xyz: &[[T; 3]],
    ndiv_circum: usize,
    ndiv_longtitude: usize,
    r: T,
) -> (Vec<[usize; 3]>, Vec<[T; 3]>)
where
    T: num_traits::Float + Copy + num_traits::FloatConst + 'static,
    usize: num_traits::AsPrimitive<T>,
{
    use del_geo_core::vec3::Vec3;
    use num_traits::AsPrimitive;
    assert!(ndiv_circum > 2);
    let num_vtxl = vtxl2xyz.len();
    let vtxl2framex = vtx2framex(vtxl2xyz);
    let vtxl2framey = vtx2framey(vtxl2xyz, &vtxl2framex);
    //
    let ndiv_length = num_vtxl - 1;
    let (tri2vtx, vtx2xyz) = crate::trimesh3_primitive::cylinder_closed_end_yup::<usize, T>(
        T::one(),
        T::one(),
        ndiv_circum,
        2 * ndiv_longtitude + ndiv_length - 2,
        true,
    );
    let tri2vtx = Vec::<[usize; 3]>::from(tri2vtx.as_slice());
    let mut vtx2xyz = Vec::<[T; 3]>::from(vtx2xyz.as_slice());
    assert_eq!(
        vtx2xyz.len(),
        (2 * ndiv_longtitude + ndiv_length - 1) * ndiv_circum + 2
    );
    let pi: T = T::PI();
    let one = T::one();
    let half: T = one / (one + one);
    {
        // south pole
        let p0 = vtxl2xyz[0];
        let ez = framez(vtxl2xyz, 0);
        let q = p0.sub(&ez.scale(r));
        vtx2xyz[0][0] = q[0];
        vtx2xyz[0][1] = q[1];
        vtx2xyz[0][2] = q[2];
    }
    for ir in 0..ndiv_longtitude {
        let p0 = vtxl2xyz[0];
        let ex = &vtxl2framex[0];
        let ey = &vtxl2framey[0];
        let ez = framez(vtxl2xyz, 0);
        let t0 = pi * half * (ndiv_longtitude - 1 - ir).as_() / ndiv_longtitude.as_();
        let c0 = r * num_traits::Float::cos(t0);
        for ic in 0..ndiv_circum {
            let theta = 2.as_() * pi * ic.as_() / ndiv_circum.as_();
            let az = ez.scale(-r * num_traits::Float::sin(t0));
            let ay = ey.scale(c0 * num_traits::Float::cos(theta));
            let ax = ex.scale(c0 * num_traits::Float::sin(theta));
            let q = p0.add(&az).add(&ay).add(&ax);
            vtx2xyz[1 + ir * ndiv_circum + ic][0] = q[0];
            vtx2xyz[1 + ir * ndiv_circum + ic][1] = q[1];
            vtx2xyz[1 + ir * ndiv_circum + ic][2] = q[2];
        }
    }
    for il in 0..ndiv_length - 1 {
        let p0 = vtxl2xyz[il + 1];
        let ex = &vtxl2framex[il + 1];
        let ey = &vtxl2framey[il + 1];
        for ic in 0..ndiv_circum {
            let theta = 2.as_() * pi * ic.as_() / ndiv_circum.as_();
            let ay = ey.scale(r * num_traits::Float::cos(theta));
            let ax = ex.scale(r * num_traits::Float::sin(theta));
            let q = p0.add(&ay).add(&ax);
            vtx2xyz[1 + (il + ndiv_longtitude) * ndiv_circum + ic][0] = q[0];
            vtx2xyz[1 + (il + ndiv_longtitude) * ndiv_circum + ic][1] = q[1];
            vtx2xyz[1 + (il + ndiv_longtitude) * ndiv_circum + ic][2] = q[2];
        }
    }
    for ir in 0..ndiv_longtitude {
        let p0 = vtxl2xyz[num_vtxl - 1];
        let ex = &vtxl2framex[num_vtxl - 1];
        let ey = &vtxl2framey[num_vtxl - 1];
        let ez = framez(vtxl2xyz, num_vtxl - 1);
        let t0 = pi * half * ir.as_() / ndiv_longtitude.as_();
        let c0 = r * num_traits::Float::cos(t0);
        for ic in 0..ndiv_circum {
            let theta = 2.as_() * pi * ic.as_() / ndiv_circum.as_();
            let az = ez.scale(r * num_traits::Float::sin(t0));
            let ay = ey.scale(c0 * num_traits::Float::cos(theta));
            let ax = ex.scale(c0 * num_traits::Float::sin(theta));
            let q = p0.add(&az).add(&ax).add(&ay);
            vtx2xyz[1 + (ir + ndiv_length + ndiv_longtitude - 1) * ndiv_circum + ic][0] = q[0];
            vtx2xyz[1 + (ir + ndiv_length + ndiv_longtitude - 1) * ndiv_circum + ic][1] = q[1];
            vtx2xyz[1 + (ir + ndiv_length + ndiv_longtitude - 1) * ndiv_circum + ic][2] = q[2];
        }
    }
    {
        // North Pole
        let p0 = vtxl2xyz[num_vtxl - 1];
        let ez = framez(vtxl2xyz, num_vtxl - 1);
        let q = p0.add(&ez.scale(r));
        let np = vtx2xyz.len();
        vtx2xyz[np - 1][0] = q[0];
        vtx2xyz[np - 1][1] = q[1];
        vtx2xyz[np - 1][2] = q[2];
    }
    (tri2vtx, vtx2xyz)
}

pub fn to_trimesh3_cylinder_and_sphere<T>(
    vtx2xyz: &[[T; 3]],
    rad: T,
    ndiv: usize,
) -> (Vec<[usize; 3]>, Vec<[T; 3]>)
where
    T: num_traits::Float + num_traits::FloatConst + Copy + 'static,
    usize: num_traits::AsPrimitive<T>,
    f32: num_traits::AsPrimitive<T>,
{
    let mut tri2vtx: Vec<[usize; 3]> = Vec::new();
    let mut all_vtx2xyz: Vec<[T; 3]> = Vec::new();
    let num_vtx = vtx2xyz.len();

    // open cylinder for each segment
    for i_vtx in 0..num_vtx - 1 {
        let vtx_offset = all_vtx2xyz.len();
        let (tris, verts) = crate::trimesh3_primitive::cylinder_open_connecting_two_points(
            ndiv,
            rad,
            &vtx2xyz[i_vtx],
            &vtx2xyz[i_vtx + 1],
        );
        tri2vtx.extend(
            tris.iter()
                .map(|t| [t[0] + vtx_offset, t[1] + vtx_offset, t[2] + vtx_offset]),
        );
        all_vtx2xyz.extend_from_slice(&verts);
    }

    // sphere at each vertex
    for p in vtx2xyz.iter() {
        let vtx_offset = all_vtx2xyz.len();
        let sphere = crate::trimesh3_primitive::sphere_yup::<usize, T>(rad, ndiv, ndiv);
        tri2vtx.extend(
            sphere
                .tri2vtx
                .iter()
                .map(|t| [t[0] + vtx_offset, t[1] + vtx_offset, t[2] + vtx_offset]),
        );
        all_vtx2xyz.extend(
            sphere
                .vtx2xyz
                .iter()
                .map(|v| [v[0] + p[0], v[1] + p[1], v[2] + p[2]]),
        );
    }

    (tri2vtx, all_vtx2xyz)
}

pub fn to_trimesh3_ribbon<T>(
    vtxl2xyz: &[[T; 3]],
    vtxl2framex: &[[T; 3]],
    width: T,
) -> (Vec<[usize; 3]>, Vec<[T; 3]>)
where
    T: num_traits::Float + Copy + num_traits::FloatConst + 'static,
    usize: num_traits::AsPrimitive<T>,
{
    use del_geo_core::vec3::Vec3;
    let num_vtxl = vtxl2xyz.len();
    let mut vtx2xyz = vec![[T::zero(); 3]; num_vtxl * 2];
    for i_vtxl in 0..num_vtxl {
        vtx2xyz[i_vtxl * 2] = vtxl2xyz[i_vtxl];
        vtx2xyz[i_vtxl * 2 + 1] = vtxl2xyz[i_vtxl].add(&vtxl2framex[i_vtxl].scale(width));
    }
    let num_tri = (num_vtxl - 1) * 2;
    let mut tri2vtx = vec![[0usize; 3]; num_tri];
    for i_vtxl in 0..num_vtxl - 1 {
        tri2vtx[i_vtxl * 2] = [i_vtxl * 2, i_vtxl * 2 + 1, i_vtxl * 2 + 2];
        tri2vtx[i_vtxl * 2 + 1] = [i_vtxl * 2 + 1, i_vtxl * 2 + 3, i_vtxl * 2 + 2];
    }
    (tri2vtx, vtx2xyz)
}

/*
pub fn nearest_to_polyline3<T>(
    poly_a: &Vec::<nalgebra::Vector3::<T>>,
    poly_b: &Vec::<nalgebra::Vector3::<T>>) -> (T,T,T)
    where T: nalgebra::RealField + Copy,
          f64: AsPrimitive<T>,
          usize: AsPrimitive<T>
{
    let mut res: (T,T,T) = (T::max_value().unwrap(), T::zero(), T::zero());
    for ia in 0..poly_a.len() -1 {
        let a0 = &poly_a[ia];
        let a1 = &poly_a[ia+1];
        for ib in 0..poly_b.len() -1 {
            let b0 = &poly_b[ib];
            let b1 = &poly_b[ib+1];
            let dis = del_geo::edge3::nearest_to_edge3(a0,a1,b0,b1);
            if dis.0 < res.0 {
                res.0 = dis.0;
                res.1 = ia.as_() + dis.1;
                res.2 = ib.as_() + dis.2;
            }
        }
    }
    res
}
*/

pub fn contacting_pair(
    poly2vtx: &[usize],
    vtx2xyz: &[[f32; 3]],
    dist0: f32,
) -> (Vec<usize>, Vec<f32>) {
    let num_poly = poly2vtx.len() - 1;
    let mut pair_idx = Vec::<usize>::new();
    let mut pair_prm = Vec::<f32>::new();
    for i_poly in 0..num_poly {
        for j_poly in i_poly + 1..num_poly {
            for i_seg in poly2vtx[i_poly]..poly2vtx[i_poly + 1] - 1 {
                let pi = &vtx2xyz[i_seg];
                let qi = &vtx2xyz[i_seg + 1];
                for j_seg in poly2vtx[j_poly]..poly2vtx[j_poly + 1] - 1 {
                    let pj = &vtx2xyz[j_seg];
                    let qj = &vtx2xyz[j_seg + 1];
                    let (dist, ri, rj) = del_geo_core::edge3::nearest_to_edge3(pi, qi, pj, qj);
                    if dist > dist0 {
                        continue;
                    }
                    pair_idx.extend([i_poly, j_poly]);
                    pair_prm.push((i_seg - poly2vtx[i_poly]) as f32 + ri);
                    pair_prm.push((j_seg - poly2vtx[j_poly]) as f32 + rj);
                }
            }
        }
    }
    (pair_idx, pair_prm)
}

pub fn position_from_barycentric_coordinate<T>(vtx2xyz: &[[T; 3]], r: T) -> [T; 3]
where
    T: num_traits::Float + num_traits::AsPrimitive<usize>,
    usize: num_traits::AsPrimitive<T>,
{
    use num_traits::AsPrimitive;
    let ied: usize = r.as_();
    let ned = vtx2xyz.len() - 1;
    assert!(ied < ned);
    let p0 = &vtx2xyz[ied];
    let p1 = &vtx2xyz[ied + 1];
    let r0 = r - ied.as_();
    use del_geo_core::vec3::Vec3;
    p0.add(&p1.sub(p0).scale(r0))
}

pub fn smooth<T>(vtx2xyz: &[[T; 3]], r: T, num_iter: usize) -> Vec<[T; 3]>
where
    T: num_traits::Float + Copy + 'static,
    f64: num_traits::AsPrimitive<T>,
{
    use del_geo_core::vec3::Vec3;
    let one = T::one();
    let half = one / (one + one);
    let num_vtx = vtx2xyz.len();
    let mut vtx2xyz1 = Vec::from(vtx2xyz);
    for _iter in 0..num_iter {
        for ip1 in 1..num_vtx - 1 {
            let ip0 = (ip1 + num_vtx - 1) % num_vtx;
            let ip2 = (ip1 + 1) % num_vtx;
            let p0 = &vtx2xyz1[ip0];
            let p1 = &vtx2xyz1[ip1];
            let p2 = &vtx2xyz1[ip2];
            let pm = p0.add(p2).scale(half);
            let p1n = pm.scale(r).add(&p1.scale(one - r));
            vtx2xyz1[ip1] = p1n;
        }
    }
    vtx2xyz1
}

pub fn length<T>(vtx2xyz: &[[T; 3]]) -> T
where
    T: num_traits::Float + std::ops::AddAssign + std::fmt::Debug,
{
    let num_vtx = vtx2xyz.len();
    let mut len = T::zero();
    for i_vtx in 0..num_vtx - 1 {
        let j_vtx = i_vtx + 1;
        let elen = del_geo_core::edge3::length(&vtx2xyz[i_vtx], &vtx2xyz[j_vtx]);
        len += elen;
    }
    len
}

fn reduce_recursive(
    vtx2flg: &mut [i32],
    i_vtx0: usize,
    i_vtx1: usize,
    vtx2xyz: &[[f32; 3]],
    threshold: f32,
) {
    use del_geo_core::vec3::Vec3;
    assert!(i_vtx1 as i64 - i_vtx0 as i64 > 1);
    let p0 = &vtx2xyz[i_vtx0];
    let p1 = &vtx2xyz[i_vtx1];
    let vtx_farest = {
        let mut vtx_nearest = (usize::MAX, 0.);
        #[allow(clippy::needless_range_loop)]
        for i_vtxm in i_vtx0 + 1..i_vtx1 {
            let pm = &vtx2xyz[i_vtxm];
            let (_dist, rn) = del_geo_core::edge3::nearest_to_point3(p0, p1, pm);
            let pn = del_geo_core::vec3::axpy(rn, &p1.sub(p0), p0);
            let dist = del_geo_core::edge3::length(pm, &pn);
            if dist >= vtx_nearest.1 {
                vtx_nearest = (i_vtxm, dist);
            }
        }
        vtx_nearest
    };
    assert_ne!(vtx_farest.0, usize::MAX);
    assert!(i_vtx0 < vtx_farest.0);
    assert!(vtx_farest.0 < i_vtx1);
    if vtx_farest.1 > threshold {
        vtx2flg[vtx_farest.0] = 1; // this is fixed
    }
    if vtx_farest.0 - i_vtx0 > 1 {
        reduce_recursive(vtx2flg, i_vtx0, vtx_farest.0, vtx2xyz, threshold);
    }
    if i_vtx1 - vtx_farest.0 > 1 {
        reduce_recursive(vtx2flg, vtx_farest.0, i_vtx1, vtx2xyz, threshold);
    }
}

/// <https://en.wikipedia.org/wiki/Ramer%E2%80%93Douglas%E2%80%93Peucker_algorithm>
pub fn reduce(vtx2xyz: &[[f32; 3]], threshold: f32) -> Vec<[f32; 3]> {
    let num_vtx = vtx2xyz.len();
    let mut vtx2flg = vec![0; num_vtx]; // 0: free, 1: fix
    vtx2flg[0] = 1;
    vtx2flg[num_vtx - 1] = 1;
    reduce_recursive(&mut vtx2flg, 0, num_vtx - 1, vtx2xyz, threshold);
    vtx2xyz
        .iter()
        .enumerate()
        .filter(|&(iv, _xyz)| vtx2flg[iv] == 1)
        .map(|(_iv, &xyz)| xyz)
        .collect()
}

#[test]
fn test_reduce() -> anyhow::Result<()> {
    let path_dir_out = std::path::Path::new("../target/out_del_msh_cpu");
    std::fs::create_dir_all(path_dir_out)?;
    //
    let vtx2xy = crate::polyloop2::from_circle(1.0, 100);
    let vtx2xyz: Vec<[_; 3]> = vtx2xy.iter().map(|v| [v[0], v[1], 0.0]).collect();
    let vtx2xyz_reduced = reduce(&vtx2xyz, 0.01);
    crate::io_wavefront_obj::save_vtx2xyz_as_polyloop(
        path_dir_out.join("polyline3_reduce.obj"),
        &vtx2xyz_reduced,
    )?;
    Ok(())
}

pub fn helix_from_delta_angle<T>(num_points: usize, dangle: T, rad0: T, pitch: T) -> Vec<[T; 3]>
where
    T: num_traits::Float + FloatConst + 'static,
    usize: AsPrimitive<T>,
{
    let two = T::one() + T::one();
    let mut vtx2xyz = Vec::with_capacity(num_points);
    for ip in 0..num_points {
        let angle = ip.as_() * dangle;
        let pos = [
            pitch * angle / (two * T::PI()),
            rad0 * angle.cos(),
            rad0 * angle.sin(),
        ];
        vtx2xyz.push(pos);
    }
    vtx2xyz
}

pub fn helix<T>(num_points: usize, elen: T, rad0: T, pitch: T) -> Vec<[T; 3]>
where
    T: num_traits::Float + num_traits::FloatConst + 'static,
    usize: num_traits::AsPrimitive<T>,
{
    let one = T::one();
    let two = one + one;
    let dangle = {
        let tmp = pitch / (two * T::PI());
        let tmp2 = tmp * tmp;
        let mut dangle = elen / (rad0 * rad0 + tmp2).sqrt();
        for _itr in 0..10 {
            let elen0 = (two * rad0 * rad0 * (one - dangle.cos()) + tmp2 * dangle * dangle).sqrt();
            let df = (rad0 * rad0 * dangle.sin() + dangle * tmp2) / elen0;
            let f = elen0 - elen;
            dangle = dangle - f / df;
        }
        dangle
    };
    helix_from_delta_angle(num_points, dangle, rad0, pitch)
}

#[test]
fn test_generate_trimesh() {
    let path_dir = std::path::Path::new("../target/out_del_msh_cpu");
    std::fs::create_dir_all(path_dir).unwrap();
    //
    let vtx2xyz_polyline = helix(100, 0.05, 0.7, 0.4);
    let (tri2vtx, vtx2xyz) = to_trimesh3_capsule(&vtx2xyz_polyline, 32, 32, 0.05);
    crate::io_wavefront_obj::save_tri2vtx_vtx2xyz(
        path_dir.join("polyline3_helix.obj"),
        &tri2vtx,
        &vtx2xyz,
    )
    .unwrap();
    //
    let vtx2xyz_polyline = del_geo_core::bezier_cubic::sample_uniform_param(
        100,
        &[0.9, 0.0, -0.1],
        &[-3.0, 0.9, -0.1],
        &[0.9, -3.0, 0.1],
        &[0.0, 0.9, 0.1],
        true,
        true,
    );
    let (tri2vtx, vtx2xyz) = to_trimesh3_capsule(&vtx2xyz_polyline, 32, 32, 0.05);
    crate::io_wavefront_obj::save_tri2vtx_vtx2xyz(
        path_dir.join("polyline3_bezier.obj"),
        &tri2vtx,
        &vtx2xyz,
    )
    .unwrap();
}

/// Returns the maximum step size `t` in `[0, 1]` such that moving all vertices of a polyline
/// by `t * vtx2uvw` keeps every non-adjacent segment pair at distance ≥ `offset`.
/// Pairs already closer than `offset + park_clearance` are skipped (handled externally).
pub fn max_safe_step_polyline(
    vtx2xyz: &[[f32; 3]],
    vtx2uvw: &[[f32; 3]],
    offset: f32,
    park_clearance: f32,
) -> f32 {
    let num_vtx = vtx2xyz.len();
    let mut t_max = 1f32;
    let park = offset + park_clearance;
    for i_seg in 0..num_vtx - 1 {
        for j_seg in i_seg + 2..num_vtx - 1 {
            let [sqdist, _s, _t] = del_geo_core::edge3::nearest_to_edge3_accurate(
                &vtx2xyz[i_seg],
                &vtx2xyz[i_seg + 1],
                &vtx2xyz[j_seg],
                &vtx2xyz[j_seg + 1],
            );
            if sqdist <= park * park {
                continue;
            }
            let mut x0 = [
                vtx2xyz[i_seg],
                vtx2xyz[i_seg + 1],
                vtx2xyz[j_seg],
                vtx2xyz[j_seg + 1],
            ];
            let mut dx = [
                vtx2uvw[i_seg],
                vtx2uvw[i_seg + 1],
                vtx2uvw[j_seg],
                vtx2uvw[j_seg + 1],
            ];
            del_geo_core::mat3x4_array_of_cols::centerize(&mut x0);
            del_geo_core::mat3x4_array_of_cols::centerize(&mut dx);
            let Some(scale) =
                del_geo_core::ccd3::normalize_centerized_configuration(&mut x0, &mut dx, t_max)
            else {
                continue;
            };
            let max_relative_velocity = del_geo_core::ccd3::max_relative_norm::<_, 2>(&dx);
            let t_max_new = del_geo_core::ccd3::maximum_safe_time_accurate::<
                del_geo_core::ccd3::EdgeEdgeSquaredDist,
                2,
            >(
                &x0,
                &dx,
                max_relative_velocity,
                &del_geo_core::ccd3::EdgeEdgeSquaredDist,
                offset * scale,
                park_clearance * scale,
                t_max,
            );
            t_max = t_max.min(t_max_new);
        }
    }
    t_max
}

#[cfg(test)]
mod test_shorten {

    struct ContactPair {
        i_seg: usize,
        j_seg: usize,
        s: f32,
        t: f32,
    }

    fn find_contact_pairs(vtx2xyz: &[[f32; 3]], dhat: f32) -> Vec<ContactPair> {
        let num_vtx = vtx2xyz.len();
        let mut pairs = Vec::new();
        for i_seg in 0..num_vtx - 1 {
            for j_seg in i_seg + 2..num_vtx - 1 {
                let [sqdist, s, t] = del_geo_core::edge3::nearest_to_edge3_accurate(
                    &vtx2xyz[i_seg],
                    &vtx2xyz[i_seg + 1],
                    &vtx2xyz[j_seg],
                    &vtx2xyz[j_seg + 1],
                );
                let dist = sqdist.sqrt();
                if dist < dhat {
                    pairs.push(ContactPair { i_seg, j_seg, s, t });
                }
            }
        }
        pairs
    }

    fn make_polyline(num_vtx: usize, seed: u64) -> Vec<[f32; 3]> {
        use rand::RngExt;
        use rand::SeedableRng;
        let mut reng = rand_chacha::ChaChaRng::seed_from_u64(seed);
        let vtx2xyz: Vec<[f32; 3]> = {
            let mut vtx2xyz = (0..num_vtx)
                .map(|_| {
                    [
                        reng.random_range(-1.0..1.0),
                        reng.random_range(-1.0..1.0),
                        reng.random_range(-1.0..1.0),
                    ]
                })
                .collect::<Vec<_>>();
            vtx2xyz[0] = [-1., -1., -1.];
            vtx2xyz[num_vtx - 1] = [1., 1., 1.];
            vtx2xyz
        };
        vtx2xyz
    }

    fn system_energy(
        vtx2xyz: &[[f32; 3]],
        dhat: f32,
        offset: f32,
        barrier_stiffness: f32,
        edge_length_ini: f32,
    ) -> f32 {
        let num_vtx = vtx2xyz.len();
        let mut sum_w = 0.0;
        for i_seg in 0..vtx2xyz.len() - 1 {
            let i0_vtx = i_seg;
            let i1_vtx = i_seg + 1;
            let node2xyz = [vtx2xyz[i0_vtx], vtx2xyz[i1_vtx]];
            let (w, _dw, _ddw) = del_geo_core::edge3::wdwddw_squared_length_difference(
                &node2xyz,
                1.0,
                edge_length_ini,
                true,
            );
            sum_w += w;
        }
        for i_seg in 0..num_vtx - 1 {
            let i0_vtx = i_seg;
            let i1_vtx = i_seg + 1;
            for j_seg in i_seg + 2..num_vtx - 1 {
                let j0_vtx = j_seg;
                let j1_vtx = j_seg + 1;
                let [sqdist, _s, _t] = del_geo_core::edge3::nearest_to_edge3_accurate(
                    &vtx2xyz[i0_vtx],
                    &vtx2xyz[i1_vtx],
                    &vtx2xyz[j0_vtx],
                    &vtx2xyz[j1_vtx],
                );
                let dist = sqdist.sqrt();
                let w = barrier_stiffness * del_geo_core::barrier::w_energy(dist, dhat, offset);
                sum_w += w;
            }
        }
        sum_w
    }

    pub fn resolve_collision(vtx2xyz: &mut [[f32; 3]], dist_trg: f32) {
        use del_geo_core::vec3::Vec3;
        for _ in 0..100 {
            let cps = find_contact_pairs(vtx2xyz, dist_trg);
            if cps.is_empty() {
                break;
            }
            for cp in &cps {
                let [sqdist, s, t] = del_geo_core::edge3::nearest_to_edge3_accurate(
                    &vtx2xyz[cp.i_seg],
                    &vtx2xyz[cp.i_seg + 1],
                    &vtx2xyz[cp.j_seg],
                    &vtx2xyz[cp.j_seg + 1],
                );
                let dist = sqdist.sqrt();
                if dist >= dist_trg {
                    continue;
                }
                // closest points on each segment
                let p = vtx2xyz[cp.i_seg]
                    .scale(1. - s)
                    .add(&vtx2xyz[cp.i_seg + 1].scale(s));
                let q = vtx2xyz[cp.j_seg]
                    .scale(1. - t)
                    .add(&vtx2xyz[cp.j_seg + 1].scale(t));
                // unit normal from q toward p; push each closest point out by half the gap
                let n = p.sub(&q).scale(1. / dist);
                let half_gap = (dist_trg - dist) * 0.5;
                // distribute to 4 vertices weighted by segment parameter (preserves linear momentum)
                let push = n.scale(half_gap);
                vtx2xyz[cp.i_seg] = vtx2xyz[cp.i_seg].add(&push.scale(1. - s));
                vtx2xyz[cp.i_seg + 1] = vtx2xyz[cp.i_seg + 1].add(&push.scale(s));
                vtx2xyz[cp.j_seg] = vtx2xyz[cp.j_seg].sub(&push.scale(1. - t));
                vtx2xyz[cp.j_seg + 1] = vtx2xyz[cp.j_seg + 1].sub(&push.scale(t));
            }
        }
    }

    #[test]
    fn test0() {
        let path_dir = std::path::Path::new("../target/out_del_msh_cpu");
        std::fs::create_dir_all(path_dir).unwrap();
        //
        const NUM_FRAME: usize = 4500;
        const FRAME_INTERVAL_SAVE_FILE: usize = 50;
        const OFFSET: f32 = 1.0e-2; // twice the the radius of rod
        const DHAT: f32 = 1.0e-3; // the distance from the OFFSET where contact force become non-zero
        const PARK_CLEARANCE: f32 = 1.0e-6; // the distance from the OFFSET just before collision
        const BARRIER_STIFFNESS: f32 = 1.0e+6;
        //
        let num_vtx = 15;
        let mut vtx2xyz = make_polyline(num_vtx, 4);
        let edge_length_ini = 0.5 * 2.0 * 3f32.sqrt() / (num_vtx - 1) as f32; // half-length of diagonal

        let vtx2vtx_polyline = crate::polyline::vtx2vtx_rods(&[0, num_vtx]);
        let mut grad = vec![[0.; 3]; num_vtx];
        let mut vtx2uvw = grad.clone();
        let mut vtx2ap = grad.clone();
        let mut vtx2p = grad.clone();
        let mut tmp = vec![usize::MAX; num_vtx];
        let blk2isfix = {
            let mut blk2isfix = vec![[0i32; 3]; num_vtx];
            blk2isfix[0][0] = 1;
            blk2isfix[0][1] = 1;
            blk2isfix[0][2] = 1;
            blk2isfix[num_vtx - 1][0] = 1;
            blk2isfix[num_vtx - 1][1] = 1;
            blk2isfix[num_vtx - 1][2] = 1;
            blk2isfix
        };
        resolve_collision(&mut vtx2xyz, OFFSET * 1.5);
        for iter in 0..NUM_FRAME {
            let contact_paris = find_contact_pairs(&vtx2xyz, DHAT + OFFSET);
            println!("num_contact_paris: {}", contact_paris.len());
            let mut hess = {
                let cp2vtx: Vec<[usize; 4]> = contact_paris
                    .iter()
                    .map(|cp| [cp.i_seg, cp.i_seg + 1, cp.j_seg, cp.j_seg + 1])
                    .collect();
                let vtx2vtx_cp = crate::vtx2vtx::from_uniform_mesh(&cp2vtx, num_vtx, false);
                let vtx2vtx = crate::vtx2vtx::merge(
                    &vtx2vtx_polyline.0,
                    &vtx2vtx_polyline.1,
                    &vtx2vtx_cp.0,
                    &vtx2vtx_cp.1,
                    num_vtx,
                );
                crate::sparse_solver::sparse_square::MatrixOwned::<[f32; 9]>::from_vtx2vtx(
                    &vtx2vtx.0, &vtx2vtx.1,
                )
            };
            // -------
            hess.set_zero();
            crate::vtx2xn::set_zero(&mut grad);
            let mut system_energy0 = 0.0;
            for i_seg in 0..vtx2xyz.len() - 1 {
                let i0_vtx = i_seg;
                let i1_vtx = i_seg + 1;
                let node2xyz = [vtx2xyz[i0_vtx], vtx2xyz[i1_vtx]];
                let (w, dw, ddw) = del_geo_core::edge3::wdwddw_squared_length_difference(
                    &node2xyz,
                    1.0,
                    edge_length_ini,
                    true,
                );
                hess.merge(&ddw, &[i_seg, i_seg + 1], &mut tmp);
                use del_geo_core::vecn::VecN;
                grad[i0_vtx].add_in_place(&dw[0].scale(-1.));
                grad[i1_vtx].add_in_place(&dw[1].scale(-1.));
                system_energy0 += w;
            }
            for cp in &contact_paris {
                let (s, t) = (cp.s, cp.t);
                let dist = {
                    use del_geo_core::vec3::Vec3;
                    let p = vtx2xyz[cp.i_seg]
                        .scale(1. - s)
                        .add(&vtx2xyz[cp.i_seg + 1].scale(s));
                    let q = vtx2xyz[cp.j_seg]
                        .scale(1. - t)
                        .add(&vtx2xyz[cp.j_seg + 1].scale(t));
                    p.sub(&q).norm()
                };
                let w = BARRIER_STIFFNESS * del_geo_core::barrier::w_energy(dist, DHAT, OFFSET);
                let dwdd = BARRIER_STIFFNESS * del_geo_core::barrier::dw_energy(dist, DHAT, OFFSET);
                let ddwddd =
                    BARRIER_STIFFNESS * del_geo_core::barrier::ddw_energy(dist, DHAT, OFFSET);
                {
                    use del_geo_core::vec3::Vec3;
                    //use del_geo_core::vecn::VecN;
                    // direction from j closest point to i closest point
                    let p_i = vtx2xyz[cp.i_seg]
                        .scale(1. - s)
                        .add(&vtx2xyz[cp.i_seg + 1].scale(s));
                    let p_j = vtx2xyz[cp.j_seg]
                        .scale(1. - t)
                        .add(&vtx2xyz[cp.j_seg + 1].scale(t));
                    let unit_diff = p_i.sub(&p_j).scale(1. / dist);
                    // ∂dist/∂node = factor * unit_diff
                    let factors = [1. - s, s, -(1. - t), -t];
                    let g: [[f32; 3]; 4] = core::array::from_fn(|i| unit_diff.scale(factors[i]));
                    // gradient: -dE/dx accumulated into grad
                    let nodes = [cp.i_seg, cp.i_seg + 1, cp.j_seg, cp.j_seg + 1];
                    for (i_node, &i_vtx) in nodes.iter().enumerate() {
                        grad[i_vtx].add_in_place(&g[i_node].scale(-dwdd));
                    }
                    // Hessian (GG^T approximation, drops the ∂²dist/∂x² term)
                    let emat: [[[f32; 9]; 4]; 4] = core::array::from_fn(|i| {
                        core::array::from_fn(|j| {
                            // column-major outer product of g[i] and g[j], scaled by ddwddd
                            core::array::from_fn(|k| ddwddd * g[i][k % 3] * g[j][k / 3])
                        })
                    });
                    hess.merge(&emat, &nodes, &mut tmp);
                }
                system_energy0 += w;
            }
            println!("{iter} --> {system_energy0}");
            {
                let eps = 1.0e-5;
                hess.row2val.iter_mut().for_each(|val| {
                    val[0] += eps;
                    val[1] += eps;
                    val[2] += eps;
                });
            }
            crate::vtx2xn::set_fixed(&mut grad, &blk2isfix);
            hess.set_fixed(0., &blk2isfix);
            crate::sparse_solver::cg::conjugate_gradient(
                &mut grad,
                &mut vtx2uvw,
                &mut vtx2ap,
                &mut vtx2p,
                1.0e-5,
                100,
                hess.as_ref(),
            );
            {
                let sq_upd = crate::vtx2xn::dot(&vtx2uvw, &vtx2uvw);
                if sq_upd < 1.0e-10 {
                    println!("converged");
                    break;
                }
            }
            // compute time [0, 1] where the rod can move at least the distance with PARK_CLEARANCE.
            let max_safe_time_total = crate::polyline3::max_safe_step_polyline(
                &vtx2xyz,
                &vtx2uvw,
                OFFSET,
                PARK_CLEARANCE,
            );
            //dbg!(max_safe_time_total);
            let mut step_time = max_safe_time_total * 0.9999; // slightly reduce the time to avoid the clearance be exactly PARK_CLEARANCE
            let mut line_search_converged = false;
            for _jter in 0..12 {
                let mut vtx2xyz1 = vtx2xyz.clone();
                crate::vtx2xn::add_scaled_vector(&mut vtx2xyz1, step_time, &vtx2uvw);
                let system_energy1 =
                    system_energy(&vtx2xyz1, DHAT, OFFSET, BARRIER_STIFFNESS, edge_length_ini);
                println!(
                    "energy1: {} energy0:{} steptime:{}",
                    system_energy1, system_energy0, step_time
                );
                if system_energy1 > system_energy0 {
                    step_time *= 0.5;
                } else {
                    line_search_converged = true;
                    break;
                }
            }
            if !line_search_converged {
                println!("WARNING: line search reached maximum iterations without reducing energy");
                break;
            }
            crate::vtx2xn::add_scaled_vector(&mut vtx2xyz, step_time, &vtx2uvw);
            /*
            crate::io_wavefront_obj::save_vtx2xyz_as_polyline(
                path_dir.join(format!("ccd3_{iter:04}.obj")),
                &vtx2xyz,
            )
            .unwrap();
             */
            let (tri2vtx0, vtx02xyz0) =
                crate::polyline3::to_trimesh3_cylinder_and_sphere(&vtx2xyz, OFFSET * 0.5, 32);
            if iter % FRAME_INTERVAL_SAVE_FILE == 0 {
                let idx = iter / FRAME_INTERVAL_SAVE_FILE;
                crate::io_wavefront_obj::save_tri2vtx_vtx2xyz(
                    path_dir.join(format!("ccd3_{idx:04}.obj")),
                    &tri2vtx0,
                    &vtx02xyz0,
                )
                .unwrap()
            }
        }
    }
}