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 vector_traits::approx::ulps_eq;
use vector_traits::num_traits::Float;
use vector_traits::{
    approx,
    prelude::{GenericScalar, GenericVector2, GenericVector3},
};

/// Calculates the Z-coordinate where a 2d circle (radius in X&Y) intersects with an edge. The circle can be considered to
/// fall from infinite Z, so the effect is the same as for a cylinder or square end mill.
///
/// This function determines the Z-coordinate at which a circle, defined by its `center` and `probe_radius`,
/// intersects with a line segment defined by points `p0` and `p1`.
///
/// # Arguments
///
/// * `center`: The XY-coordinates of the center of the circle.
/// * `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 = square_end_to_edge_collision(center, probe_radius, p0, p1, SkipEndpoint::NoSkip);
/// ```
pub(crate) fn square_end_to_edge_collision<T: GenericVector3>(
    edge: EdgeAndCenterType<T>,
    probe_radius: T::Scalar,
    site_index: u32,
    mt: &mut MaximumTracker<SearchResult<T>>,
) where
    T::Scalar: approx::UlpsEq,
{
    let p0 = edge.p0;
    let p0_xy = p0.to_2d();
    let p1 = edge.p1;
    let r_sq = probe_radius * probe_radius;

    // Calculate vector from p0 to p1
    let dp = p1 - p0;
    if ulps_eq!(dp.x(), T::Scalar::ZERO) && ulps_eq!(dp.y(), T::Scalar::ZERO) {
        // when the line is almost vertical we only use p1
        if (edge.center - p1.to_2d()).magnitude_sq() <= r_sq {
            mt.insert(SearchResult::<T>::new(site_index, p1.z()));
        }
        return;
    }

    let dp_xy = dp.to_2d();

    let t: T::Scalar = (edge.center - p0_xy).dot(dp_xy) / dp_xy.magnitude_sq();
    let clamped_t = t.clamp(T::Scalar::ZERO, T::Scalar::ONE);

    // closest_point_xy is a point on the infinite line
    let clamped_closest_point_xy = p0_xy + dp_xy * clamped_t;
    let clamped_dist_sq_closest_point_xy = (edge.center - clamped_closest_point_xy).magnitude_sq();
    if clamped_dist_sq_closest_point_xy > r_sq {
        return;
    }

    // closest_point_xy is a point on the infinite line
    let un_clamped_closest_point = p0 + dp * t;

    let delta_d_sq = r_sq - (edge.center - un_clamped_closest_point.to_2d()).magnitude_sq();
    if delta_d_sq < T::Scalar::ZERO {
        return;
    }
    // we know that dp.z >= 0 so we are skipping the .abs() for dp.z
    let m_sq = dp.z().powi(2) / (dp.x().powi(2) + dp.y().powi(2));

    let cz = un_clamped_closest_point.z() + (m_sq * delta_d_sq).sqrt();

    mt.insert(SearchResult::new(site_index, cz.clamp(p0.z(), p1.z())));
}

/*
#[allow(dead_code)]
pub(crate) fn square_end_to_edge_collision_xz<T: GenericVector3>(
    edge: EdgeAndCenterType<T>,
    probe_radius: T::Scalar,
    site_index: usize,
    mt: &mut MaximumTracker<SearchResult<T>>,
) where
    T::Scalar: approx::UlpsEq,
{
    let r_sq = probe_radius * probe_radius;

    /*if edge.center.is_abs_diff_eq(
        T::Vector2::new_2d((2.04).into(), (0.12).into()),
        0.01.into(),
    ) && edge.p1.x().abs_diff_eq(&(2.023047).into(), 0.01.into())
    {
        // used to set breakpoints for debugging
        println!(
            "center:{:?} site:{site_index} p0:{:?} p1:{:?}",
            edge.center, edge.p0, edge.p1
        );
    }*/

    // use translation and rotation to place center at origin and align p0-p1 with the x-axis
    let transform = match RigidTransform2D::translate_rotate_align_x(
        edge.center,
        edge.p0.to_2d(),
        edge.p1.to_2d(),
    ) {
        Some(t) => t,
        None => {
            if edge.p1.to_2d().magnitude_sq() <= r_sq {
                mt.insert(SearchResult::<T>::new(site_index, edge.p1.z()));
            }
            return;
        }
    };
    let p0 = transform.transform_point(edge.p0);
    let p1 = transform.transform_point(edge.p1);

    // Calculate vector from p0 to p1
    let dp = p1 - p0;
    if ulps_eq!(dp.x(), T::Scalar::ZERO) {
        // when the line is almost vertical we only use p1
        if p1.to_2d().magnitude_sq() <= r_sq {
            mt.insert(SearchResult::<T>::new(site_index, p1.z()));
        }
        return;
    }

    let dp_xy = dp.to_2d();

    let t: T::Scalar = -(p0.to_2d().dot(dp_xy) / dp_xy.magnitude_sq());
    let clamped_t = t.clamp(T::Scalar::ZERO, T::Scalar::ONE);

    if (p0.to_2d() + dp_xy * clamped_t).magnitude_sq() > r_sq {
        return;
    }

    // unclamped_closest_point is a point on the infinite line
    let un_clamped_closest_point = p0 + dp * t;

    let delta_d_sq = r_sq - un_clamped_closest_point.to_2d().magnitude_sq();
    if delta_d_sq < T::Scalar::ZERO {
        return;
    }

    // we know that dp.z >= 0 so we are skipping the .abs() for dp.z
    let m_sq = (dp.z() / dp.x()).powi(2);
    let cz = un_clamped_closest_point.z() + (m_sq * delta_d_sq).sqrt();
    mt.insert(SearchResult::new(site_index, cz.clamp(p0.z(), p1.z())));
}
*/