del-geo-core 0.1.38

//! vtx index is the same as the VTK

pub const EDGE2NODE: [[usize; 2]; 8] = [
    [0, 1], // edge 0
    [1, 2], // edge 1
    [2, 3], // edge 2
    [3, 0], // edge 3
    [0, 4], // edge 4
    [1, 4], // edge 5
    [2, 4], // edge 6
    [3, 4], // edge 7
];

pub const FACE2IDX: [usize; 6] = [0, 4, 7, 10, 13, 16];
pub const IDX2NODE: [usize; 16] = [0, 3, 2, 1, 0, 1, 4, 1, 2, 4, 2, 3, 4, 3, 0, 4];

pub fn shapefunc<Real>(pco: [Real; 3]) -> [Real; 5]
where
    Real: num_traits::Float,
{
    let one = Real::one();
    let r = pco[0];
    let s = pco[1];
    let t = pco[2];

    let rm = one - r;
    let sm = one - s;
    let tm = one - t;

    [
        rm * sm * tm, // sf[0]
        r * sm * tm,  // sf[1]
        r * s * tm,   // sf[2]
        rm * s * tm,  // sf[3]
        t,            // sf[4]
    ]
}

/// Returns parametric coordinates of the origin inside the pyramid, or None if outside.
pub fn parametric_coord_for_origin<Real>(
    p0: &[Real; 3],
    p1: &[Real; 3],
    p2: &[Real; 3],
    p3: &[Real; 3],
    p4: &[Real; 3],
) -> Option<[Real; 3]>
where
    Real: num_traits::Float,
{
    use slice_of_array::SliceFlatExt;
    let zero = Real::zero();
    let one = Real::one();
    let half = one / (one + one);
    let mut pco = [half; 3];
    for _ in 0..20 {
        let sf = shapefunc(pco);
        let pos = [
            sf[0] * p0[0] + sf[1] * p1[0] + sf[2] * p2[0] + sf[3] * p3[0] + sf[4] * p4[0],
            sf[0] * p0[1] + sf[1] * p1[1] + sf[2] * p2[1] + sf[3] * p3[1] + sf[4] * p4[1],
            sf[0] * p0[2] + sf[1] * p1[2] + sf[2] * p2[2] + sf[3] * p3[2] + sf[4] * p4[2],
        ];
        let jac = dxdr(p0, p1, p2, p3, p4, &pco);
        let jac_col: &[Real; 9] = jac.flat().try_into().unwrap();
        let jac_col = crate::mat3_col_major::transpose(jac_col);
        let j_inv = crate::mat3_col_major::try_inverse(&jac_col)?;
        let dpco = crate::mat3_col_major::mult_vec(&j_inv, &pos);
        for i in 0..3 {
            pco[i] = pco[i] - dpco[i];
        }
    }
    if pco[0] >= zero
        && pco[0] <= one
        && pco[1] >= zero
        && pco[1] <= one
        && pco[2] >= zero
        && pco[2] <= one
    {
        Some(pco)
    } else {
        None
    }
}

/// Returns the nearest point to the origin on the pyramid and its parametric coordinates (r, s, t).
/// Valid domain: r,s,t in \[0,1\].
/// Vertices: p0,p1,p2,p3 are the base quad (VTK order), p4 is the apex.
pub fn nearest_to_origin<Real>(
    p0: &[Real; 3],
    p1: &[Real; 3],
    p2: &[Real; 3],
    p3: &[Real; 3],
    p4: &[Real; 3],
) -> ([Real; 3], [Real; 3])
where
    Real: num_traits::Float + std::fmt::Debug,
{
    let zero = Real::zero();
    let one = Real::one();

    if let Some(pco) = parametric_coord_for_origin(p0, p1, p2, p3, p4) {
        return ([zero; 3], pco);
    }

    let mut p_min = *p0;
    let mut pco_min = [zero; 3]; // p0 is at (r=0, s=0, t=0)
    let mut d_min = crate::vec3::norm(p0);

    let mut update = |p: [Real; 3], pco: [Real; 3]| {
        let d = crate::vec3::norm(&p);
        if d < d_min {
            d_min = d;
            p_min = p;
            pco_min = pco;
        }
    };

    // base quad face (t=0): quad s0→r, s1→s
    {
        let (p, s0, s1) = crate::quad3::nearest_to_origin(p0, p1, p2, p3);
        update(p, [s0, s1, zero]);
    }

    // lateral tri faces: recover the free parameter from (1 - a_apex)
    // face p0,p1,p4 (s=0): a0=(1-r)(1-t), a1=r(1-t), a4=t → r = a1/(a0+a1)
    {
        let (p, a0, a1, a4) = crate::tri3::nearest_to_origin3(p0, p1, p4);
        let denom = a0 + a1;
        let r = if denom > zero { a1 / denom } else { zero };
        update(p, [r, zero, a4]);
    }
    // face p1,p2,p4 (r=1): a1=(1-s)(1-t), a2=s(1-t), a4=t → s = a2/(a1+a2)
    {
        let (p, a1, a2, a4) = crate::tri3::nearest_to_origin3(p1, p2, p4);
        let denom = a1 + a2;
        let s = if denom > zero { a2 / denom } else { zero };
        update(p, [one, s, a4]);
    }
    // face p2,p3,p4 (s=1): a2=r(1-t), a3=(1-r)(1-t), a4=t → r = a2/(a2+a3)
    {
        let (p, a2, a3, a4) = crate::tri3::nearest_to_origin3(p2, p3, p4);
        let denom = a2 + a3;
        let r = if denom > zero { a2 / denom } else { zero };
        update(p, [r, one, a4]);
    }
    // face p3,p0,p4 (r=0): a3=s(1-t), a0=(1-s)(1-t), a4=t → s = a3/(a3+a0)
    {
        let (p, a3, a0, a4) = crate::tri3::nearest_to_origin3(p3, p0, p4);
        let denom = a3 + a0;
        let s = if denom > zero { a3 / denom } else { zero };
        update(p, [zero, s, a4]);
    }

    // base edges (t=0): edge3 returns (p, s0=1-t_edge, s1=t_edge)
    {
        let (p, _s0, s1) = crate::edge3::nearest_to_origin3(p0, p1);
        update(p, [s1, zero, zero]);
    } // r=s1, s=0
    {
        let (p, _s0, s1) = crate::edge3::nearest_to_origin3(p1, p2);
        update(p, [one, s1, zero]);
    } // r=1, s=s1
    {
        let (p, s0, _s1) = crate::edge3::nearest_to_origin3(p2, p3);
        update(p, [s0, one, zero]);
    } // r=s0, s=1
    {
        let (p, s0, _s1) = crate::edge3::nearest_to_origin3(p3, p0);
        update(p, [zero, s0, zero]);
    } // r=0, s=s0

    // lateral edges (to apex): t=s1, base corner fixes r,s
    {
        let (p, _s0, s1) = crate::edge3::nearest_to_origin3(p0, p4);
        update(p, [zero, zero, s1]);
    }
    {
        let (p, _s0, s1) = crate::edge3::nearest_to_origin3(p1, p4);
        update(p, [one, zero, s1]);
    }
    {
        let (p, _s0, s1) = crate::edge3::nearest_to_origin3(p2, p4);
        update(p, [one, one, s1]);
    }
    {
        let (p, _s0, s1) = crate::edge3::nearest_to_origin3(p3, p4);
        update(p, [zero, one, s1]);
    }

    (p_min, pco_min)
}

/// pco: parametric coordinates
pub fn dndr<Real>(pco: &[Real; 3]) -> [[Real; 3]; 5]
where
    Real: num_traits::Float,
{
    let zero = Real::zero();
    let one = Real::one();
    let r = pco[0];
    let s = pco[1];
    let t = pco[2];

    let rm = one - r;
    let sm = one - s;
    let tm = one - t;

    let mut dndr = [[zero; 3]; 5];

    // r-derivatives
    dndr[0][0] = -sm * tm;
    dndr[1][0] = sm * tm;
    dndr[2][0] = s * tm;
    dndr[3][0] = -s * tm;
    dndr[4][0] = zero;

    // s-derivatives
    dndr[0][1] = -rm * tm;
    dndr[1][1] = -r * tm;
    dndr[2][1] = r * tm;
    dndr[3][1] = rm * tm;
    dndr[4][1] = zero;

    // t-derivatives
    dndr[0][2] = -rm * sm;
    dndr[1][2] = -r * sm;
    dndr[2][2] = -r * s;
    dndr[3][2] = -rm * s;
    dndr[4][2] = one;

    dndr
}

pub fn dxdr<Real>(
    p0: &[Real; 3],
    p1: &[Real; 3],
    p2: &[Real; 3],
    p3: &[Real; 3],
    p4: &[Real; 3],
    pco: &[Real; 3],
) -> [[Real; 3]; 3]
where
    Real: num_traits::Float,
{
    let dndr = dndr(pco);
    let mut dxdr = [[Real::zero(); 3]; 3];
    for idim in 0..3 {
        for ir in 0..3 {
            dxdr[idim][ir] = dxdr[idim][ir] + p0[idim] * dndr[0][ir];
            dxdr[idim][ir] = dxdr[idim][ir] + p1[idim] * dndr[1][ir];
            dxdr[idim][ir] = dxdr[idim][ir] + p2[idim] * dndr[2][ir];
            dxdr[idim][ir] = dxdr[idim][ir] + p3[idim] * dndr[3][ir];
            dxdr[idim][ir] = dxdr[idim][ir] + p4[idim] * dndr[4][ir];
        }
    }
    dxdr
}

pub fn volume<Real>(
    p0: &[Real; 3],
    p1: &[Real; 3],
    p2: &[Real; 3],
    p3: &[Real; 3],
    p4: &[Real; 3],
    i_gauss_degree: usize,
) -> Real
where
    Real: num_traits::Float + 'static,
    crate::quadrature_line::Quad<Real>: crate::quadrature_line::QuadratureLine<Real>,
{
    let one = Real::one();
    let half = one / (one + one);
    let one8th = half * half * half;
    use crate::quadrature_line::QuadratureLine;
    let quadrature: &[[Real; 2]] = crate::quadrature_line::Quad::<Real>::hoge(i_gauss_degree);
    let mut volume = Real::zero();
    let num_quadr = quadrature.len();
    for (ir1, ir2, ir3) in itertools::iproduct!(0..num_quadr, 0..num_quadr, 0..num_quadr) {
        let pco = [
            (quadrature[ir1][0] + one) * half,
            (quadrature[ir2][0] + one) * half,
            (quadrature[ir3][0] + one) * half,
        ];
        let w = quadrature[ir1][1] * quadrature[ir2][1] * quadrature[ir3][1];
        let dxdr = dxdr(p0, p1, p2, p3, p4, &pco);
        use slice_of_array::SliceFlatExt;
        let detjac = crate::mat3_col_major::determinant::<Real>(dxdr.flat().try_into().unwrap());
        volume = volume + detjac * w;
    }
    volume * one8th
}

#[test]
fn test_nearest_to_origin() {
    // origin inside: base quad at z=-1, apex at z=2
    {
        let p0 = [-1.0f64, -1.0, -1.0];
        let p1 = [1.0, -1.0, -1.0];
        let p2 = [1.0, 1.0, -1.0];
        let p3 = [-1.0, 1.0, -1.0];
        let p4 = [0.0, 0.0, 2.0];
        let (p, pco) = nearest_to_origin(&p0, &p1, &p2, &p3, &p4);
        assert!(
            crate::vec3::norm(&p) < 1.0e-10,
            "inside: nearest point should be origin, got {:?}",
            p
        );
        let (r, s, t) = (pco[0], pco[1], pco[2]);
        assert!(
            r > 0.0 && s > 0.0 && t > 0.0 && r < 1.0 && s < 1.0 && t < 1.0,
            "inside: pco should be strictly interior: {:?}",
            pco
        );
        let w = shapefunc(pco);
        let vs = [p0, p1, p2, p3, p4];
        let recon: [f64; 3] =
            std::array::from_fn(|i| w.iter().zip(vs.iter()).map(|(&wi, vi)| wi * vi[i]).sum());
        assert!(
            crate::vec3::norm(&crate::vec3::sub(&recon, &p)) < 1.0e-10,
            "reconstruction failed"
        );
    }

    // origin outside: pyramid shifted to x>=3, nearest point on the base face
    {
        let p0 = [3.0f64, -1.0, -1.0];
        let p1 = [5.0, -1.0, -1.0];
        let p2 = [5.0, 1.0, -1.0];
        let p3 = [3.0, 1.0, -1.0];
        let p4 = [4.0, 0.0, 2.0];
        let (p, pco) = nearest_to_origin(&p0, &p1, &p2, &p3, &p4);
        let (r, s, t) = (pco[0], pco[1], pco[2]);
        assert!(
            r >= -1.0e-10
                && s >= -1.0e-10
                && t >= -1.0e-10
                && r <= 1.0 + 1.0e-10
                && s <= 1.0 + 1.0e-10
                && t <= 1.0 + 1.0e-10,
            "pco out of domain: {:?}",
            pco
        );
        let w = shapefunc(pco);
        let vs = [p0, p1, p2, p3, p4];
        let recon: [f64; 3] =
            std::array::from_fn(|i| w.iter().zip(vs.iter()).map(|(&wi, vi)| wi * vi[i]).sum());
        assert!(
            crate::vec3::norm(&crate::vec3::sub(&recon, &p)) < 1.0e-10,
            "reconstruction failed"
        );
        let d = crate::vec3::norm(&p);
        for v in &vs {
            assert!(
                d <= crate::vec3::norm(v) + 1.0e-10,
                "vertex {:?} is closer than result",
                v
            );
        }
    }
}

pub fn jacobian_determinant_and_conditiona_number<Real>(
    p0: &[Real; 3],
    p1: &[Real; 3],
    p2: &[Real; 3],
    p3: &[Real; 3],
    p4: &[Real; 3],
    pco: &[Real; 3],
) -> Option<(Real, Real)>
where
    Real: num_traits::Float + num_traits::FloatConst,
{
    let dxdr = dxdr(p0, p1, p2, p3, p4, pco);
    use slice_of_array::SliceFlatExt;
    let detjac = crate::mat3_row_major::determinant(dxdr.flat().try_into().unwrap());
    let jtj = crate::mat3_sym::from_mat3_array_of_array_by_ata(&dxdr);
    let es = crate::mat3_sym::eigen_values_analytic(&jtj)?;
    let c = (es[2] / es[0]).sqrt();
    Some((detjac, c))
}

pub fn subdivide<INDEX>(
    corner: &[INDEX; 5],
    edge: &[INDEX; 8],
    quad: INDEX,
) -> ([[INDEX; 5]; 6], [[INDEX; 4]; 4])
where
    INDEX: num_traits::PrimInt,
{
    // corner:
    //   0,1,2,3 : base quad vertices
    //   4       : apex
    //
    // edge:
    //   0 : midpoint of (0,1)
    //   1 : midpoint of (1,2)
    //   2 : midpoint of (2,3)
    //   3 : midpoint of (3,0)
    //   4 : midpoint of (0,4)
    //   5 : midpoint of (1,4)
    //   6 : midpoint of (2,4)
    //   7 : midpoint of (3,4)
    //
    // quad:
    //   center of base quad (0,1,2,3)

    let pyramids = [
        [corner[0], edge[0], quad, edge[3], edge[4]],
        [edge[0], corner[1], edge[1], quad, edge[5]],
        [quad, edge[1], corner[2], edge[2], edge[6]],
        [edge[3], quad, edge[2], corner[3], edge[7]],
        [edge[4], edge[5], edge[6], edge[7], corner[4]],
        [edge[7], edge[6], edge[5], edge[4], quad],
    ];

    let tetrahedra = [
        [edge[0], edge[4], edge[5], quad],
        [edge[1], edge[5], edge[6], quad],
        [edge[2], edge[6], edge[7], quad],
        [edge[3], edge[7], edge[4], quad],
    ];

    (pyramids, tetrahedra)
}