hronn 0.7.0

// SPDX-License-Identifier: AGPL-3.0-or-later
// Copyright (c) 2023 lacklustr@protonmail.com https://github.com/eadf
// This file is part of the hronn crate.
pub(crate) mod ball_end;
pub(crate) mod ball_end_to_edge;
pub(crate) mod square_end;
pub(crate) mod square_end_to_edge;
pub(crate) mod tapered_end;
pub(crate) mod tapered_end_to_edge;
#[cfg(test)]
pub mod tests;

use super::meshanalyzer::MeshAnalyzer;
use crate::geo::RigidTransform2D;
use crate::{
    HronnError,
    geo::{Area2D, ConvertTo, PlaneFromTriangle},
    m_factor_from_plane_unit_normal, m_from_plane_unit_normal,
    meshanalyzer::SearchResult,
    triangle_normal,
    util::MaximumTracker,
};
use std::{fmt::Debug, marker::PhantomData};
use vector_traits::{
    num_traits::{AsPrimitive, Float},
    prelude::{GenericScalar, GenericVector2, GenericVector3, HasXY, HasXYZ},
};

/// this struct contains pre-computed data needed to probe a triangle.
/// If the triangle has plane not perpendicular to the Z axis, the plane data will be populated.
pub(crate) struct TriangleMetadata<T: GenericVector3, MESH: HasXYZ> {
    pub(crate) scalar: T::Scalar,
    pub(crate) plane: Option<PlaneMetadata<T::Vector2>>,
    #[doc(hidden)]
    _pdm: PhantomData<fn(MESH) -> MESH>,
}

impl<T: GenericVector3, MESH: HasXYZ> TriangleMetadata<T, MESH>
where
    MESH: ConvertTo<T>,
{
    pub fn new_for_ball_nose(
        probe_radius: T::Scalar,
        p0: MESH,
        p1: MESH,
        p2: MESH,
    ) -> Result<Self, HronnError> {
        let p0: T = p0.to();
        let p1: T = p1.to();
        let p2: T = p2.to();

        let u_normal = triangle_normal(p0, p1, p2)
            .try_normalize(T::Scalar::EPSILON)
            .ok_or_else(|| {
                HronnError::InvalidData(
                    "Could not normalize normal, is the mesh triangulated?".to_string(),
                )
            })?;
        let m_factor = m_factor_from_plane_unit_normal::<T>(u_normal);

        let (p0_prim_xy, p1_prim_xy, p2_prim_xy) = {
            let xy_offset = (u_normal * probe_radius).to_2d();
            (
                p0.to_2d() + xy_offset,
                p1.to_2d() + xy_offset,
                p2.to_2d() + xy_offset,
            )
        };

        let area = Area2D::new(p0_prim_xy, p1_prim_xy, p2_prim_xy);
        let pft = PlaneFromTriangle::new_from_normal(u_normal, p0);
        if area.value().abs() > T::Scalar::EPSILON {
            Ok(Self {
                scalar: m_factor * probe_radius - probe_radius,
                plane: Some(PlaneMetadata::<T::Vector2> {
                    pft,
                    translated_triangle: [p0_prim_xy, p1_prim_xy, p2_prim_xy],
                }),
                _pdm: PhantomData,
            })
        } else {
            Ok(Self {
                scalar: m_factor * probe_radius - probe_radius,
                plane: None,
                _pdm: PhantomData,
            })
        }
    }

    pub fn new_for_square_end(
        probe_radius: T::Scalar,
        p0: T,
        p1: T,
        p2: T,
    ) -> Result<Self, HronnError> {
        let u_normal_3d = triangle_normal(p0, p1, p2)
            .try_normalize(T::Scalar::EPSILON)
            .ok_or_else(|| {
                HronnError::InvalidData(
                    "Could not normalize normal, is the mesh triangulated?".to_string(),
                )
            })?;
        let m = m_from_plane_unit_normal::<T>(u_normal_3d);
        let p0_2d = p0.to_2d();
        let p1_2d = p1.to_2d();
        let p2_2d = p2.to_2d();

        if let Some(u_normal_2d) = u_normal_3d.to_2d().try_normalize(T::Scalar::EPSILON) {
            // the area of a translated triangle remains the same
            let area = Area2D::new(p0_2d, p1_2d, p2_2d);
            if area.value().abs() > T::Scalar::EPSILON {
                let xy_offset = u_normal_2d * probe_radius;
                let pft = {
                    let zoffset_p0 = T::new_3d(p0.x(), p0.y(), p0.z() + m * probe_radius);
                    PlaneFromTriangle::new_from_normal(u_normal_3d, zoffset_p0)
                };

                return Ok(Self {
                    scalar: m * probe_radius,
                    plane: Some(PlaneMetadata {
                        pft,
                        translated_triangle: [
                            p0_2d + xy_offset,
                            p1_2d + xy_offset,
                            p2_2d + xy_offset,
                        ],
                    }),
                    _pdm: PhantomData,
                });
            }
        } else {
            //println!("The triangle {p0} {p1} {p2} had no suitable plane. norm:{}", normal.xy());
            let area = Area2D::new(p0_2d, p1_2d, p2_2d);
            if area.value().abs() > T::Scalar::EPSILON {
                return Ok(Self {
                    scalar: m * probe_radius,
                    plane: Some(PlaneMetadata {
                        pft: PlaneFromTriangle::new_from_z_coord(p0.z()),
                        translated_triangle: [p0_2d, p1_2d, p2_2d],
                    }),
                    _pdm: PhantomData,
                });
            }
        }
        Ok(Self {
            scalar: m * probe_radius,
            plane: None,
            _pdm: PhantomData,
        })
    }

    /// Creates a new tapered nose object for collision detection with a triangular mesh.
    ///
    /// # Parameters
    /// * `cone_radius` - The radius of the cone used for collision detection. This defines both
    ///   the maximum width of the conical nose and the width of any cylindrical portion.
    /// * `cone_slope` - The slope of the tapered nose cone, defined as height/radius (cotangent of the semi-apex angle).
    ///   Higher values create a steeper, more needle-like cone, while lower values create a flatter cone.
    /// * `p0` - First vertex of the triangle.
    /// * `p1` - Second vertex of the triangle.
    /// * `p2` - Third vertex of the triangle.
    ///
    /// # Returns
    /// * `Result<Self, HronnError>` - The constructed object if successful, or an error if the triangle is invalid.
    ///
    /// # Errors
    /// * Returns `HronnError::InvalidData` if the triangle normal cannot be computed or normalized.
    pub fn new_for_tapered_nose(
        cone_radius: T::Scalar,
        cone_slope: T::Scalar,
        p0: T,
        p1: T,
        p2: T,
    ) -> Result<Self, HronnError> {
        let u_normal_3d = triangle_normal(p0, p1, p2)
            .try_normalize(T::Scalar::EPSILON)
            .ok_or_else(|| {
                HronnError::InvalidData(
                    "Could not normalize normal, is the mesh triangulated?".to_string(),
                )
            })?;

        let area = Area2D::new(p0.to_2d(), p1.to_2d(), p2.to_2d());
        let m = m_from_plane_unit_normal::<T>(u_normal_3d);
        let p0_2d = p0.to_2d();
        let p1_2d = p1.to_2d();
        let p2_2d = p2.to_2d();
        /*println!("p0:{:?}, p1:{:?}, p2:{:?}, m:{m}", p0, p1, p2);
                println!(
                    "u_normal:{:?}, u_normal.magnitude():{}, m:{m}",
                    u_normal,
                    u_normal.magnitude()
                );
                println!("probe_radius:{_probe_radius}");
        */
        if area.value().abs() > T::Scalar::EPSILON && m > T::Scalar::EPSILON {
            if m > cone_slope {
                // m > cone_slope meaning the triangle plane cone height is defined by the height
                // of the cone circumference touching the plane
                if let Some(u_normal_2d) = u_normal_3d.to_2d().try_normalize(T::Scalar::EPSILON) {
                    let xy_offset = u_normal_2d * cone_radius;
                    let z_offset_p0 = T::new_3d(
                        p0.x(),
                        p0.y(),
                        p0.z() + m * cone_radius - cone_radius * cone_slope,
                    );

                    Ok(Self {
                        scalar: m,
                        plane: Some(PlaneMetadata {
                            pft: PlaneFromTriangle::new_from_normal(u_normal_3d, z_offset_p0),
                            translated_triangle: [
                                p0_2d + xy_offset,
                                p1_2d + xy_offset,
                                p2_2d + xy_offset,
                            ],
                        }),
                        _pdm: PhantomData,
                    })
                } else {
                    // todo: is this correct?
                    Ok(Self {
                        scalar: m,
                        plane: Some(PlaneMetadata::<T::Vector2> {
                            pft: PlaneFromTriangle::new_from_z_coord(p0.z()),
                            translated_triangle: [p0.to_2d(), p1.to_2d(), p2.to_2d()],
                        }),
                        _pdm: PhantomData,
                    })
                }
            } else {
                // m < cone_slope meaning the triangle plane cone height is defined by the height
                // of the cone vertex touching the plane (cone sides never touches the plane)
                Ok(Self {
                    scalar: m,
                    plane: Some(PlaneMetadata::<T::Vector2> {
                        pft: PlaneFromTriangle::new_from_normal(u_normal_3d, p0),
                        translated_triangle: [p0.to_2d(), p1.to_2d(), p2.to_2d()],
                    }),
                    _pdm: PhantomData,
                })
            }
        } else {
            Ok(Self {
                scalar: m,
                plane: None,
                _pdm: PhantomData,
            })
        }
    }
}

/// This structure contains the pre-computed data needed to process a sphere collision vs triangle
pub struct PlaneMetadata<T: GenericVector2> {
    pub pft: PlaneFromTriangle<T>,
    pub translated_triangle: [T; 3],
}

/// Parameters needed to query the kd-tree
pub struct QueryParameters<'a, T: GenericVector3, MESH: HasXYZ> {
    pub(crate) vertices: &'a [MESH],
    pub(crate) indices: &'a [u32],
    pub(crate) meta_data: &'a [TriangleMetadata<T, MESH>],
    pub(crate) probe_radius: T::Scalar,
    /// Currently only used for the tapered probe`
    pub(crate) probe_height: T::Scalar,
    /// Currently only used for the tapered probe, in this case it is `height/radius`
    //pub(crate) probe_a: T::Scalar,
    pub(crate) search_radius: T::Scalar,
}

pub trait Probe<T: GenericVector3, MESH: HasXYZ + ConvertTo<T>> {
    /// The radius of the probe
    fn probe_radius(&self) -> T::Scalar;

    /// Probe parameter A
    /// Currently only used for the tapered probe, in this case it is `tan(probe angle/2)`
    fn probe_slope(&self) -> T::Scalar;

    /// Probe height parameter
    /// Currently only used for the tapered probe
    fn probe_height(&self) -> T::Scalar;

    /// Internal use only.
    #[doc(hidden)]
    fn _mesh_analyzer(&self) -> &MeshAnalyzer<'_, T, MESH>;

    /// Internal use only.
    #[doc(hidden)]
    #[allow(private_interfaces)]
    fn _meta_data(&self) -> &[TriangleMetadata<T, MESH>];

    /// Internal use only.
    #[doc(hidden)]
    #[allow(clippy::type_complexity)]
    fn _collision_fn(
        &self,
    ) -> fn(
        query_parameters: &QueryParameters<'_, T, MESH>,
        site_index: u32,
        center: T::Vector2,
        mt: &mut MaximumTracker<SearchResult<T>>,
    );
}

pub struct SquareEndProbe<'b, T: GenericVector3, MESH: HasXYZ>
where
    MESH: ConvertTo<T>,
{
    probe_radius: T::Scalar,
    meta_data: Vec<TriangleMetadata<T, MESH>>,
    bound_mesh_analyzer: &'b MeshAnalyzer<'b, T, MESH>,
}

impl<'b, T: GenericVector3, MESH: HasXYZ> SquareEndProbe<'b, T, MESH>
where
    MESH: ConvertTo<T>,
{
    pub fn new(
        mesh_analyzer: &'b MeshAnalyzer<'_, T, MESH>,
        probe_radius: T::Scalar,
    ) -> Result<Self, HronnError> {
        Ok(SquareEndProbe {
            probe_radius,
            meta_data: square_end::shared_square_end_precompute_logic::<T, MESH>(
                mesh_analyzer.vertices.as_ref(),
                mesh_analyzer.indices.as_ref(),
                probe_radius,
            )?,
            bound_mesh_analyzer: mesh_analyzer,
        })
    }
}

impl<T: GenericVector3, MESH: HasXYZ> Probe<T, MESH> for SquareEndProbe<'_, T, MESH>
where
    MESH: ConvertTo<T>,
{
    #[inline(always)]
    fn probe_radius(&self) -> T::Scalar {
        self.probe_radius
    }

    #[inline(always)]
    fn probe_slope(&self) -> T::Scalar {
        T::Scalar::NEG_INFINITY
    }

    #[inline(always)]
    fn probe_height(&self) -> T::Scalar {
        T::Scalar::NEG_INFINITY
    }

    #[inline(always)]
    fn _mesh_analyzer(&self) -> &MeshAnalyzer<'_, T, MESH> {
        self.bound_mesh_analyzer
    }

    #[inline(always)]
    #[allow(private_interfaces)]
    fn _meta_data(&self) -> &[TriangleMetadata<T, MESH>] {
        &self.meta_data
    }

    #[inline(always)]
    fn _collision_fn(
        &self,
    ) -> fn(
        query_parameters: &QueryParameters<'_, T, MESH>,
        site_index: u32,
        center: T::Vector2,
        mt: &mut MaximumTracker<SearchResult<T>>,
    ) {
        square_end::square_end_compute_collision
    }
}

pub struct BallNoseProbe<'b, T: GenericVector3, MESH: HasXYZ>
where
    MESH: ConvertTo<T>,
{
    probe_radius: T::Scalar,
    meta_data: Vec<TriangleMetadata<T, MESH>>,
    bound_mesh_analyzer: &'b MeshAnalyzer<'b, T, MESH>,
}

impl<'b, T: GenericVector3, MESH: HasXYZ> BallNoseProbe<'b, T, MESH>
where
    MESH: ConvertTo<T>,
{
    pub fn new(
        mesh_analyzer: &'b MeshAnalyzer<'_, T, MESH>,
        probe_radius: T::Scalar,
    ) -> Result<Self, HronnError> {
        Ok(BallNoseProbe {
            probe_radius,
            meta_data: ball_end::shared_ball_nose_precompute_logic::<T, MESH>(
                mesh_analyzer.vertices.as_ref(),
                mesh_analyzer.indices.as_ref(),
                probe_radius,
            )?,
            bound_mesh_analyzer: mesh_analyzer,
        })
    }
}

impl<T: GenericVector3, MESH: HasXYZ> Probe<T, MESH> for BallNoseProbe<'_, T, MESH>
where
    MESH: ConvertTo<T>,
    T: ConvertTo<MESH>,
{
    #[inline(always)]
    fn probe_radius(&self) -> T::Scalar {
        self.probe_radius
    }

    #[inline(always)]
    fn probe_slope(&self) -> T::Scalar {
        T::Scalar::NEG_INFINITY
    }

    #[inline(always)]
    fn probe_height(&self) -> T::Scalar {
        T::Scalar::NEG_INFINITY
    }

    #[inline(always)]
    fn _mesh_analyzer(&self) -> &MeshAnalyzer<'_, T, MESH> {
        self.bound_mesh_analyzer
    }

    #[inline(always)]
    #[allow(private_interfaces)]
    fn _meta_data(&self) -> &[TriangleMetadata<T, MESH>] {
        &self.meta_data
    }

    #[inline(always)]
    fn _collision_fn(
        &self,
    ) -> fn(
        query_parameters: &QueryParameters<'_, T, MESH>,
        site_index: u32,
        center: T::Vector2,
        mt: &mut MaximumTracker<SearchResult<T>>,
    ) {
        ball_end::ball_nose_compute_collision
    }
}

pub struct TaperedProbe<'b, T: GenericVector3, MESH: HasXYZ>
where
    MESH: ConvertTo<T>,
{
    // The largest radius of the probe
    probe_radius: T::Scalar,
    probe_height: T::Scalar,
    // The cot(α/2) == height/radius value
    slope: T::Scalar,
    meta_data: Vec<TriangleMetadata<T, MESH>>,
    bound_mesh_analyzer: &'b MeshAnalyzer<'b, T, MESH>,
}

impl<'b, T: GenericVector3, MESH: HasXYZ> TaperedProbe<'b, T, MESH>
where
    MESH: ConvertTo<T>,
    T: ConvertTo<MESH>,
    f64: AsPrimitive<T::Scalar>,
{
    pub fn new(
        mesh_analyzer: &'b MeshAnalyzer<'_, T, MESH>,
        probe_radius: T::Scalar,
        probe_angle: T::Scalar,
    ) -> Result<Self, HronnError> {
        let slope: T::Scalar = T::Scalar::ONE / (probe_angle / T::Scalar::TWO).tan();
        let probe_height = probe_radius * slope;

        let rv = TaperedProbe {
            probe_radius,
            probe_height,
            slope,
            meta_data: tapered_end::shared_tapered_precompute_logic::<T, MESH>(
                mesh_analyzer.vertices.as_ref(),
                mesh_analyzer.indices.as_ref(),
                probe_radius,
                slope,
            )?,
            bound_mesh_analyzer: mesh_analyzer,
        };
        println!(
            "Tapered probe got radius:{} angle:{} tan(angle/2):{} height:{} ",
            rv.probe_radius(),
            probe_angle * (180.0 / std::f64::consts::PI).as_(),
            rv.probe_slope(),
            rv.probe_height()
        );
        Ok(rv)
    }
}

impl<T: GenericVector3, MESH: HasXYZ> Probe<T, MESH> for TaperedProbe<'_, T, MESH>
where
    MESH: ConvertTo<T>,
    T: ConvertTo<MESH>,
{
    #[inline(always)]
    fn probe_radius(&self) -> T::Scalar {
        self.probe_radius
    }
    #[inline(always)]
    fn probe_slope(&self) -> T::Scalar {
        self.slope
    }
    #[inline(always)]
    fn probe_height(&self) -> T::Scalar {
        self.probe_height
    }
    #[inline(always)]
    fn _mesh_analyzer(&self) -> &MeshAnalyzer<'_, T, MESH> {
        self.bound_mesh_analyzer
    }
    #[inline(always)]
    #[allow(private_interfaces)]
    fn _meta_data(&self) -> &[TriangleMetadata<T, MESH>] {
        &self.meta_data
    }
    #[inline(always)]
    fn _collision_fn(
        &self,
    ) -> fn(
        query_parameters: &QueryParameters<'_, T, MESH>,
        site_index: u32,
        center: T::Vector2,
        mt: &mut MaximumTracker<SearchResult<T>>,
    ) {
        tapered_end::tapered_compute_collision
    }
}

#[derive(Debug, Clone, Copy, PartialEq)]
#[repr(i8)]
#[allow(dead_code)]
pub enum SkipEndpoint {
    SkipP0 = -1,
    NoSkip = 0,
    SkipP1 = 1,
}

impl SkipEndpoint {
    #[allow(dead_code)]
    pub(crate) fn flip(self) -> Self {
        // Multiply by -1 to flip the sign
        unsafe { std::mem::transmute::<i8, Self>(-(self as i8)) }
    }
}

/// Represents the type of the closest point on a line segment to a given point.
/// - `T`: The generic type that must implement `GenericVector3`, representing a 3D vector.
pub struct EdgeAndCenterType<T: GenericVector3> {
    center: T::Vector2,
    distance_sq: T::Scalar,
    t: T::Scalar,
    p0: T,
    p1: T,
}

impl<T: GenericVector3> PartialEq for EdgeAndCenterType<T> {
    #[inline(always)]
    fn eq(&self, other: &Self) -> bool {
        self.get_distance_sq() == other.get_distance_sq()
    }
}

impl<T: GenericVector3> PartialOrd for EdgeAndCenterType<T> {
    #[inline(always)]
    fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
        match self.distance_sq.partial_cmp(&other.distance_sq) {
            Some(std::cmp::Ordering::Equal) => other
                .p0
                .z()
                .max(other.p1.z())
                .partial_cmp(&self.p0.z().max(self.p1.z())),
            other_ordering => other_ordering,
        }
    }
}

impl<T: GenericVector3> EdgeAndCenterType<T> {
    /// rotates and translates the vertices so that the line lies in the XZ plane (y=0)
    /// It also makes sure that self.center.y >= 0
    pub fn rotate_translate_xz(&mut self) -> bool {
        // use translation and rotation to place the line at the XZ plane
        let transform = match RigidTransform2D::translate_rotate_align_x(
            self.center,
            self.p0.to_2d(),
            self.p1.to_2d(),
        ) {
            Some(t) => t,
            None => return false,
        };

        self.center = transform.transform_2d_point(self.center);
        self.p0 = transform.transform_point(self.p0);
        self.p1 = transform.transform_point(self.p1);
        // offset in y, moving the line to y=0
        let offset = T::new_3d(self.center.x(), self.p0.y(), T::Scalar::ZERO);
        self.center -= offset.to_2d();
        *self.center.y_mut() = self.center.y().abs();
        self.p0 -= offset;
        self.p1 -= offset;

        true
    }

    /// Helper method to extract the distance scalar from the enum variant.
    #[inline(always)]
    fn get_distance_sq(&self) -> T::Scalar {
        self.distance_sq
    }

    /// Returns the `ClosestEdgeType` with the smaller distance.
    #[inline(always)]
    pub fn min(self, other: Self) -> Self {
        match self.partial_cmp(&other) {
            Some(std::cmp::Ordering::Less) | Some(std::cmp::Ordering::Equal) => self,
            Some(std::cmp::Ordering::Greater) | None => other,
        }
    }

    /// Returns a tuple of the two `ClosestEdgeType` instances with the smaller distances.
    pub fn min_two(self, other1: Self, other2: Self) -> (Self, Self) {
        // Find the smallest among `self`, `other1`, and `other2`
        if self <= other1 {
            if self <= other2 {
                (self, other1.min(other2))
            } else {
                (other2, self)
            }
        } else if other1 <= other2 {
            (other1, self.min(other2))
        } else {
            (other2, other1)
        }
    }
}

/// Calculates the squared distance from a point to the closest point on a line segment in XY.
/// It also determines the type of the closest point (either an endpoint or a point along the edge).
///
/// This function uses a vector formulation to calculate the perpendicular distance from
/// a point to a line segment (if applicable) or the distance to the nearest endpoint.
/// The calculation follows the formula: |(a-p)×(a-b)|/|a-b|.
/// Reference: <https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Another_vector_formulation>
///
/// # Arguments
/// - `p0`: The start point of the line segment.
/// - `p1`: The end point of the line segment.
/// - `p`: The 2d point from which the distance is to be calculated.
///
/// # Returns
///
/// Returns a `ClosestPointType<T>`, which contains the squared distance to the closest
/// point on the line segment and indicates whether this point is an endpoint or along the edge.
///
/// # Panics
///
/// Panics if `p0` and `p1` are the same point (i.e., if the line segment's length is zero),
/// as this would lead to a division by zero in the distance calculation.
///
/// # Type Parameters
///
/// - `T`: The generic type that must implement `GenericVector3`, representing a 3D vector.
///   Note that the distance calculations will be performed in 2D.
///   Note: The parameter `t` is intentionally left un-clamped to allow calculation of virtual
///   closest points outside the segment, enabling radius-based proximity checks.
#[inline(always)]
pub(crate) fn xy_distance_to_line_squared<T: GenericVector3>(
    p: T::Vector2,
    mut p0: T,
    mut p1: T,
) -> EdgeAndCenterType<T> {
    if p0.z() > p1.z() {
        // p1.z is now always higher or equal to p0.z
        std::mem::swap(&mut p0, &mut p1);
    }

    let l0 = p0.to_2d();
    let l1 = p1.to_2d();
    let l0_sub_l1 = l0 - l1;
    let l0_sub_l1_sq = l0_sub_l1.magnitude_sq();
    let l0_sub_p = l0 - p;

    // dot product normalized by the magnitude squared of l0_sub_l1
    let dot = l0_sub_p.dot(l0_sub_l1) / l0_sub_l1_sq;

    if dot < T::Scalar::ZERO {
        EdgeAndCenterType {
            center: p,
            distance_sq: l0_sub_p.magnitude_sq(),
            t: dot,
            p0,
            p1,
        }
    } else if dot > T::Scalar::ONE {
        EdgeAndCenterType {
            center: p,
            distance_sq: (l1 - p).magnitude_sq(),
            t: dot,
            p0,
            p1,
        }
    } else {
        let a_sub_p_cross_a_sub_b = l0_sub_p.perp_dot(l0_sub_l1);
        EdgeAndCenterType {
            center: p,
            distance_sq: a_sub_p_cross_a_sub_b * a_sub_p_cross_a_sub_b / l0_sub_l1_sq,
            t: dot,
            p0,
            p1,
        }
    }
}