use num_traits::{AsPrimitive, FloatConst};
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)
}
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];
{
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);
{
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];
}
}
{
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_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 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; }
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);
}
}
pub fn reduce(vtx2xyz: &[[f32; 3]], threshold: f32) -> Vec<[f32; 3]> {
let num_vtx = vtx2xyz.len();
let mut vtx2flg = vec![0; num_vtx]; 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 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(
"../target/reduce_polyline.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();
}
#[cfg(test)]
mod test_shorten {
fn make_polyline(num_vtx: usize) -> Vec<[f32; 3]> {
use rand::RngExt;
use rand::SeedableRng;
let mut reng = rand_chacha::ChaChaRng::seed_from_u64(0);
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) -> 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, 0.0);
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
}
#[test]
fn test0() {
let path_dir = std::path::Path::new("../target/out_del_msh_cpu");
std::fs::create_dir_all(path_dir).unwrap();
let num_vtx = 10;
let mut vtx2xyz = make_polyline(num_vtx);
let vtx2vtx = crate::polyline::vtx2vtx_rods(&[0, num_vtx]);
let mut hess = crate::sparse_solver::sparse_square::MatrixOwned::<[f32; 9]>::from_vtx2vtx(
&vtx2vtx.0, &vtx2vtx.1,
);
let mut grad = vec![[0.; 3]; num_vtx];
let mut vtx2sol = 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
};
let offset = 1.0e-2;
let dhat = 1.0e-3;
let park_clearance = 1.0e-6;
let barrier_stiffness = 1.0e+6;
for iter in 0..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, 0.0);
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 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 = del_geo_core::barrier::w_energy(dist, dhat, offset);
system_energy0 += barrier_stiffness * 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 vtx2sol,
&mut vtx2ap,
&mut vtx2p,
1.0e-5,
100,
hess.as_ref(),
);
let max_safe_time_total = {
let mut t_max = 1f32;
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 mut x0 = [
vtx2xyz[i0_vtx],
vtx2xyz[i1_vtx],
vtx2xyz[j0_vtx],
vtx2xyz[j1_vtx],
];
let mut dx = [
vtx2sol[i0_vtx],
vtx2sol[i1_vtx],
vtx2sol[j0_vtx],
vtx2sol[j1_vtx],
];
del_geo_core::mat3x4_array_of_cols::centerize(&mut x0);
del_geo_core::mat3x4_array_of_cols::centerize(&mut dx);
let scale = del_geo_core::ccd3::normalize_centerized_configuration(
&mut x0, &mut dx, t_max,
)
.unwrap();
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
};
dbg!(max_safe_time_total);
let mut step_time = max_safe_time_total;
for _jter in 0..10 {
let mut vtx2xyz1 = vtx2xyz.clone();
crate::vtx2xn::add_scaled_vector(&mut vtx2xyz1, step_time, &vtx2sol);
let system_energy1 = system_energy(&vtx2xyz1, dhat, offset, barrier_stiffness);
dbg!(system_energy1, step_time);
if system_energy1 > system_energy0 {
step_time *= 0.5;
} else {
break;
}
}
crate::vtx2xn::add_scaled_vector(&mut vtx2xyz, step_time, &vtx2sol);
crate::io_wavefront_obj::save_vtx2xyz_as_polyline(
path_dir.join(format!("ccd3_{iter}.obj")),
&vtx2xyz,
)
.unwrap();
}
}
}