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.

use crate::meshanalyzer::SearchResult;
use crate::prelude::MaximumTracker;
use crate::probe::EdgeAndCenterType;
use crate::probe::ball_end::BallEndProperties;
use vector_traits::num_traits::real::Real;
use vector_traits::prelude::{GenericScalar, GenericVector2, GenericVector3};

/// Calculates the Z-coordinate where a sphere intersects with an edge.The sphere can be considered to
/// fall from infinite Z, so the effect is the same as for a Ball Nose end mill.
///
/// This function determines the Z-coordinate at which a sphere, defined by its `center` and `probe_radius`,
/// intersects with a line segment defined by points `p0` and `p1`. Unlike other variants, this function
/// does not rely on transforming the sphere's location and the edge into a canonical form but rather works
/// directly in the provided coordinate space.
///
/// # Arguments
///
/// * `center`: The XY-coordinates of the center of the sphere.
/// * `probe_radius`: The radius of the sphere.
/// * `p0`: One end of the line segment. The function internally re-arranges `p0` and `p1` based on their Z-coordinates,
///   so their order in the argument list is not significant.
/// * `p1`: The other end of the line segment.
/// * `skip`: An enum value indicating whether to skip calculations related to one of the endpoints of the line segment.
///
/// # Returns
///
/// * An `Option<f64>` representing the highest Z-coordinate of intersection. If there is no intersection, it returns `None`.
///
/// # Notes
///
/// * The function may internally swap `p0` and `p1` to ensure `p0` always has a Z-coordinate less than or equal to `p1`.
///   This means callers do not need to pre-sort these points before calling the function.
/// * The `skip` argument allows for efficient handling of cases where one of the endpoints' calculations can be avoided.
///
/// # Examples
///
/// ```rust,ignore
/// let center = Vector2::new(1.0, 2.0);
/// let probe_radius = 1.5;
/// let p0 = Vector3::new(0.0, 0.0, 0.0);
/// let p1 = Vector3::new(2.0, 2.0, 2.0);
/// let z_value = ball_nose_to_edge_collision(center, probe_radius, p0, p1, SkipEndpoint::NoSkip);
/// ```
pub(crate) fn ball_end_to_edge_collision<T: GenericVector3>(
    edge: EdgeAndCenterType<T>,
    ball_end: BallEndProperties<T>,
    site_index: u32,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    let p0 = edge.p0;
    let p1 = edge.p1;

    // Calculate vector from p0 to p1
    let dp = p1 - p0;
    if dp.x().abs() < T::Scalar::EPSILON && dp.y().abs() < T::Scalar::EPSILON {
        // when the line is almost vertical we only use p1
        z_sphere_distance_to_single_point::<T>(
            site_index,
            p1,
            ball_end.r_sq,
            (edge.center - p1.to_2d()).magnitude_sq(),
            ball_end.r,
            mt,
        );
        return;
    }

    let dp_mag_sq = dp.magnitude_sq();
    let dp_xy = dp.to_2d();
    let dep_xy_mag_sq = dp_xy.magnitude_sq();

    let t = (edge.center - p0.to_2d()).dot(dp_xy) / dep_xy_mag_sq;

    // closest_point_xy is a point on the infinite line
    let closest_point_xy = p0.to_2d() + dp_xy * t;
    let dist_sq_closest_point_xy = (edge.center - closest_point_xy).magnitude_sq();

    // radius_factor_sq is a factor that diminishes by the radial distance from the edge.
    // at probe_radius the factor is 0.0 and 1.0 directly on top of the edge.
    let radius_factor_sq = T::Scalar::ONE - dist_sq_closest_point_xy / ball_end.r_sq;
    if radius_factor_sq.is_sign_negative() {
        // closest point was out of reach of the radius
        return;
    }

    // we know that dp.z >= 0 so we are skipping the .abs() for dp.z
    let (m_factor_sq, t_offset) = if dp.z() > T::Scalar::EPSILON {
        let m_sq: T::Scalar = dp.z().powi(2) / (dp.x().powi(2) + dp.y().powi(2));
        let m_factor_sq = T::Scalar::ONE + m_sq;
        // if point r is directly under the sphere, and point q is the
        // point that is actually touching the edge, d_rq_sq is the
        // distance between them squared.
        let d_rq_sq = ball_end.r_sq * (m_sq.powi(2) + m_sq) / (m_sq + T::Scalar::ONE);
        (
            m_factor_sq,
            -(radius_factor_sq * d_rq_sq / dp_mag_sq).sqrt(),
        )
    } else {
        (T::Scalar::ONE, T::Scalar::ZERO)
    };

    if t > T::Scalar::ONE + t_offset {
        z_sphere_distance_to_single_point::<T>(
            site_index,
            p1,
            ball_end.r_sq,
            (edge.center - p1.to_2d()).magnitude_sq(),
            ball_end.r,
            mt,
        );
        return;
    }
    if t < t_offset {
        z_sphere_distance_to_single_point::<T>(
            site_index,
            p0,
            ball_end.r_sq,
            (edge.center - p0.to_2d()).magnitude_sq(),
            ball_end.r,
            mt,
        );
        return;
    }
    mt.insert(SearchResult::new(
        site_index,
        p0.z() + t * dp.z() + ball_end.r * (m_factor_sq * radius_factor_sq).sqrt() - ball_end.r,
    ))
}

#[inline(always)]
fn z_sphere_distance_to_single_point<T: GenericVector3>(
    site_index: u32,
    p: T,
    r_sq: T::Scalar,
    dist_sq: T::Scalar,
    probe_radius: T::Scalar,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    let dist_sq_diff = r_sq - dist_sq;
    if dist_sq_diff.is_sign_positive() {
        mt.insert(SearchResult::new(
            site_index,
            p.z() + dist_sq_diff.sqrt() - probe_radius,
        ));
    }
}