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 super::EdgeAndCenterType;
use super::tapered_end::ConeProperties;
use crate::meshanalyzer::SearchResult;
use crate::prelude::MaximumTracker;
use vector_traits::approx::ulps_eq;
use vector_traits::num_traits::Float;
use vector_traits::prelude::{GenericScalar, GenericVector2, GenericVector3, HasXY};

pub(crate) fn tapered_end_to_edge_collision<T: GenericVector3>(
    mut edge: EdgeAndCenterType<T>,
    cone: ConeProperties<T>,
    site_index: u32,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    if edge.distance_sq > cone.r_sq {
        // finite line was outside the radius range
        return;
    }

    debug_assert!(edge.p0.z() <= edge.p1.z());

    if !edge.rotate_translate_xz() {
        // No transform could be calculated, the line has no XY length, so just return the
        // collision with the high point.
        cone_z_value_from_single_point_line_in_xz_plane(
            edge.p1.z(),
            cone.r_sq,
            cone.height,
            (edge.center - edge.p0.to_2d()).magnitude_sq(),
            site_index,
            mt,
        );
        return;
    }

    // perform the test in the rotated-translated coordinate system
    solve_collision_line_in_xz_plane(edge, cone, site_index, mt)
}

#[inline(always)]
fn solve_collision_line_in_xz_plane<T: GenericVector3>(
    edge: EdgeAndCenterType<T>,
    cone: ConeProperties<T>,
    site_index: u32,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    // This simplification will make the math easier
    debug_assert!(edge.center.y() >= T::Scalar::ZERO);

    // Early rejection tests - if cone center is too far from line segment
    // Todo: this should already have been done before rotate-translate
    if edge.center.y() > cone.radius {
        return;
    }

    let line_dir = edge.p1 - edge.p0;
    let line_length_sq = line_dir.x().powi(2); // Only consider XY plane

    if line_length_sq > T::Scalar::ZERO {
        // Non-zero length line
        // TODO: Move this test up before rotation
        // Calculate shortest distance from cone center to infinite line segment
        // TODO: t is already in edge.t
        let t = -edge.p0.x() / line_dir.x();
        debug_assert!(ulps_eq!(t, edge.t), "{} != {}", t, edge.t);
        let closest_point = edge.p0.to_2d() + line_dir.to_2d() * t;
        let dist_sq = (edge.center - closest_point).magnitude_sq();

        // If the closest point on line to cone center is farther than radius, no intersection
        if dist_sq > cone.r_sq {
            return;
        }
    }

    // Compute line slope in XZ-plane
    let dz = edge.p1.z() - edge.p0.z(); // dz is always >=0 since the endpoints are ordered that way
    debug_assert!(ulps_eq!(dz, line_dir.z()));
    let dx = edge.p1.x() - edge.p0.x();
    debug_assert!(ulps_eq!(dx, line_dir.x()));

    let line_slope = dz / dx; // the vertical line case is handled up-stack

    // Determine which case we're in and delegate to the appropriate helper function

    // General case, the line is steeper than the cone sides
    if line_slope > cone.slope {
        return solve_steep_line_case_line_in_xz_plane(edge, cone, site_index, mt);
    }

    if edge.center.y() < T::Scalar::EPSILON {
        // cone vertex is practically directly over the line.
        solve_steep_cone_y_zero_case_line_in_xz_plane(edge, cone, line_slope, site_index, mt);
    } else {
        // General case, the cone sides are steeper than the line
        solve_steep_cone_case_line_in_xz_plane(edge, cone, line_slope, site_index, mt);
    }
}

/// This is a special case of line_cone_collision() where we know that the line is steeper than the cone,
/// and thus we know that the line only touches the cone on its circumference or that only the high point touches the
/// surface as the cone moves downward.
///
/// Analytically solves for the intersection between a line segment and a cone.
///
/// The cone is traveling in the negative Z direction (downward).
///
/// Optimized for line in XZ plane (Y=0).
fn solve_steep_line_case_line_in_xz_plane<T: GenericVector3>(
    edge: EdgeAndCenterType<T>,
    cone: ConeProperties<T>,
    site_index: u32,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    let cone_y = edge.center.y();

    let cr = cone.radius;
    let ch = cone.height;

    // Calculate vector from p0 to p1 (line direction)
    // todo: line_vec already computed up-stack
    let line_vec = edge.p1 - edge.p0;

    // Calculate distance from cone vertex to high endpoint in XY plane
    let high_xy_dist_sq = edge.p1.x().powi(2) + cone_y.powi(2);

    // Case 1: Line high point inside the cone projection
    if high_xy_dist_sq <= cone.r_sq {
        // The highest point is within the cone's base projection
        // Calculate cone vertex z-coordinate at first contact
        let z_offset = high_xy_dist_sq.sqrt() * cone.slope;
        let cz = edge.p1.z() - z_offset;

        // Insert result into the maximum tracker
        mt.insert(SearchResult::new(site_index, cz));

        return;
    }

    // Case 2: Find intersection of line with cone in XY plane
    // Solve the quadratic equation for intersection with cone base circle

    // Line is in XZ plane, so edge.p0.y and edge.p1.y are constant
    // Simplified quadratic coefficients
    let a = line_vec.x().powi(2);
    let b = T::Scalar::TWO * edge.p0.x() * line_vec.x();
    let c = edge.p0.x().powi(2) + cone_y.powi(2) - cr.powi(2);

    let discriminant = b.powi(2) - T::Scalar::FOUR * a * c;
    if discriminant < T::Scalar::ZERO {
        return; // No intersection
    }

    // Calculate intersection parameters
    let sqrt_discriminant = discriminant.sqrt();

    // TODO: This should have been covered before rotate-translate
    // Handle case where line is vertical in XZ plane
    if a.abs() < T::Scalar::EPSILON {
        if b.abs() < T::Scalar::EPSILON {
            // Line doesn't intersect cone base
            return;
        }
        let t = -c / b;
        if t < T::Scalar::ZERO || t > T::Scalar::ONE {
            return;
        }
        // TODO: call end_point_cone_collision_with_distance()
        let contact_point = edge.p0 + line_vec * t;
        let cz = contact_point.z() - ch;

        mt.insert(SearchResult::new(site_index, cz));
        return;
    }

    let t1 = (-b - sqrt_discriminant) / (T::Scalar::TWO * a);
    let t2 = (-b + sqrt_discriminant) / (T::Scalar::TWO * a);

    // Adjust to line segment bounds [0,1]
    let t_min = T::Scalar::ZERO.max(t1.min(t2));
    let t_max = T::Scalar::ONE.min(t1.max(t2));

    // If the entire valid range is outside [0,1], no valid intersection
    if t_max < T::Scalar::ZERO || t_min > T::Scalar::ONE {
        return;
    }

    // Find the highest Z point within the cone's XY projection
    let mut candidates = Vec::new();

    // Add t_min and t_max points if they're within the segment
    if t_min >= T::Scalar::ZERO && t_min <= T::Scalar::ONE {
        candidates.push(t_min);
    }
    if t_max >= T::Scalar::ZERO
        && t_max <= T::Scalar::ONE
        && (t_max - t_min).abs() > T::Scalar::EPSILON
    {
        candidates.push(t_max);
    }

    // Find the highest Z value among the candidates
    let mut highest_z = T::Scalar::NEG_INFINITY;
    let mut highest_t = None;

    for &t in &candidates {
        // Calculate the 3D point on the line
        let point = edge.p0 + line_vec * t;

        // This point is on the cone's base circle
        // Calculate the Z value of this contact point
        let point_z = point.z();

        if point_z > highest_z {
            highest_z = point_z;
            highest_t = Some(t);
        }
    }

    if let Some(t) = highest_t {
        // Calculate the contact point
        let contact_point = edge.p0 + line_vec * t;

        // Calculate cone vertex z-coordinate at first contact
        let cz = contact_point.z() - ch;

        // Insert result into the maximum tracker
        mt.insert(SearchResult::new(site_index, cz));
    }
}

/// Computes the contact position when the cone is centered in y (cone_y = 0).
///     In this case the cone's XY cross‑section is a circle centered at (0,0) with radius cr.
///     The cone surface in the XZ plane is given by:
///         z = (ch/cr) * |x|
///     while the line is:
///         z = line_slope * x + b,
///     with b computed from the line's start point.
///
/// The collision (first contact) occurs where the cone's surface and the line intersect,
/// and we select the contact point that gives the highest cone vertex z value (i.e. earliest contact).
pub(super) fn solve_steep_cone_y_zero_case_line_in_xz_plane<T: GenericVector3>(
    edge: EdgeAndCenterType<T>,
    cone: ConeProperties<T>,
    line_slope: T::Scalar,
    site_index: u32,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    // The line equation in the XZ plane is: z = line_slope * x + b.
    // Compute b from one endpoint (edge.p0).
    let b = edge.p0.z() - line_slope * edge.p0.x();

    // Determine the x-range of the line segment.
    let x0 = edge.p0.x();
    let x1 = edge.p1.x();
    let min_x = x0.min(x1);
    let max_x = x0.max(x1);

    // For the XY circle (centered at (0,0) with radius cr) and the segment lying on y=0,
    // if the segment's x-range does not intersect the interval [-cr, cr], there's no collision.
    if max_x < -cone.radius || min_x > cone.radius {
        return;
    }

    // In the ideal case the contact happens at x=0,
    // since the cone's surface is lowest there (z = (ch/cr)*|x| with x=0).
    if min_x <= T::Scalar::ZERO && T::Scalar::ZERO <= max_x {
        let _touch_x = T::Scalar::ZERO;
        let touch_line_z = b; // because line_slope * 0 + b = b
        let cone_z = touch_line_z; // no cone height offset at x=0

        // Insert result into the maximum tracker
        mt.insert(SearchResult::new(site_index, cone_z));

        return;
    }

    // Otherwise, the segment lies entirely to one side of zero.
    // Choose the endpoint with x closest to zero for the earliest contact.
    let touch_x = if min_x.abs() < max_x.abs() {
        min_x
    } else {
        max_x
    };
    let touch_line_z = line_slope * touch_x + b;

    // At that x, the cone's surface is at z = (ch/cr)*|touch_x|.
    let cone_z = touch_line_z - cone.slope * touch_x.abs();

    // Insert result into the maximum tracker
    mt.insert(SearchResult::new(site_index, cone_z));
}

/// Handle the general case where the cone is offset from the line segment (Cy!=0) and the cone's slope
/// is greater than the line segment's slope:
/// The equation of the positive parabola formed by slicing a cone with its vertex at (0,Cy,0) in the XZ plane is:
///    z = (Ch/Cr)*sqrt(x²+Cy²)
///  And a line has the equation:
///    z = mx+b
fn solve_steep_cone_case_line_in_xz_plane<T: GenericVector3>(
    edge: EdgeAndCenterType<T>,
    cone: ConeProperties<T>,
    line_slope: T::Scalar,
    site_index: u32,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    let cone_y = edge.center.y().abs();
    let cr = cone.radius;
    let cone_slope = cone.slope;

    // Extract cone coordinates (cx is implicitly 0)
    let cy = cone_y;

    // Calculate line parameters
    let dx = edge.p1.x() - edge.p0.x();

    // Step 1: Check if either endpoint is inside the cone using helper function
    for endpoint in [edge.p0, edge.p1].iter() {
        let dist_to_endpoint_sq = point_xy_distance_to_cone_vertex_sq(endpoint.x(), cy);
        if dist_to_endpoint_sq <= cone.r_sq {
            let cone_z = end_point_cone_collision_with_distance(
                endpoint.z(),
                cone_slope,
                dist_to_endpoint_sq.sqrt(),
            );
            mt.insert(SearchResult::new(site_index, cone_z));
        }
    }

    // Step 2: Check the analytical touch point solution

    // cone_y always positive
    debug_assert!(cone_y >= T::Scalar::ZERO);
    // we know that distance to infinite line is <= cr
    debug_assert!(cy.abs() <= cr);
    // we know that the line is not vertical
    debug_assert!(
        dx.abs() > T::Scalar::EPSILON,
        "dx.abs():{} <=  T::Scalar::EPSILON",
        dx.abs()
    );

    let b = edge.p0.z() - line_slope * edge.p0.x();

    // we know m > 0
    debug_assert!(
        line_slope >= T::Scalar::ZERO,
        "Line slope ({cone_slope}) should be >= 0: ({line_slope})",
    );
    // we know cone slope is higher than m
    debug_assert!(
        cone_slope > line_slope,
        "Cone slope ({cone_slope}) is <= m ({line_slope})",
    );

    if line_slope < T::Scalar::EPSILON {
        let (x0, x1, z0, z1) = if edge.p0.x() < edge.p1.x() {
            (edge.p0.x(), edge.p1.x(), edge.p0.z(), edge.p1.z())
        } else {
            (edge.p1.x(), edge.p0.x(), edge.p1.z(), edge.p0.z())
        };
        // Compute z at x=0 using linear interpolation
        let t = -x0 / (x1 - x0);
        let z_at_zero = z0 + t * (z1 - z0);

        // Check if cone base intersects the line's x range
        if x0 <= T::Scalar::ZERO && T::Scalar::ZERO <= x1 {
            // Simplified distance calculation
            let dist_to_axis = cone_y;

            if dist_to_axis <= cr {
                let cone_z = z_at_zero - cone_slope * dist_to_axis;
                mt.insert(SearchResult::new(site_index, cone_z));
            }
            return;
        } else if T::Scalar::ZERO < x0 {
            // Check against line's low point
            let dist_to_low_point_sq = point_xy_distance_to_cone_vertex_sq(x0, cy);
            if dist_to_low_point_sq <= cone.r_sq {
                let cone_z = end_point_cone_collision_with_distance(
                    z0,
                    cone_slope,
                    dist_to_low_point_sq.sqrt(),
                );
                mt.insert(SearchResult::new(site_index, cone_z));
                return;
            }
        } else if T::Scalar::ZERO > x1 {
            // Check against line's high point
            let dist_to_high_point_sq = point_xy_distance_to_cone_vertex_sq(x1, cy);
            if dist_to_high_point_sq <= cone.r_sq {
                let cone_z = end_point_cone_collision_with_distance(
                    z1,
                    cone_slope,
                    dist_to_high_point_sq.sqrt(),
                );
                mt.insert(SearchResult::new(site_index, cone_z));
                return;
            }
        }
    } else {
        // m >= T::Scalar::EPSILON
        let denominator = (cone_slope.powi(2) - line_slope.powi(2)).sqrt();

        if denominator > T::Scalar::EPSILON {
            let touch_x = line_slope * cy / denominator;
            let min_x = edge.p0.x().min(edge.p1.x());
            let max_x = edge.p0.x().max(edge.p1.x());

            if (min_x..=max_x).contains(&touch_x) {
                let touch_line_z = line_slope * touch_x + b;
                let dist = point_xy_distance_to_cone_vertex_sq(touch_x, cy).sqrt();
                let touch_cone_z = cone_slope * dist;
                let cone_z = touch_line_z - touch_cone_z;

                if dist <= cr {
                    mt.insert(SearchResult::new(site_index, cone_z));
                    // cylinder can't possibly give higher Z results
                    return;
                }
            }
        }
    }

    // Step 3: Check line segment intersection with cone's bounding cylinder

    let direction = edge.p1 - edge.p0;

    let a = direction.x().powi(2) + direction.y().powi(2);
    let b = T::Scalar::TWO * (edge.p0.x() * direction.x() + (edge.p0.y() - cy) * direction.y());
    let c = edge.p0.x().powi(2) + (edge.p0.y() - cy).powi(2) - cone.r_sq;

    let discriminant = b.powi(2) - T::Scalar::FOUR * a * c;

    if discriminant >= T::Scalar::ZERO && a.abs() > T::Scalar::EPSILON {
        let sqrt_discriminant = discriminant.sqrt();
        let t1 = (-b + sqrt_discriminant) / (T::Scalar::TWO * a);
        let t2 = (-b - sqrt_discriminant) / (T::Scalar::TWO * a);

        for t in [t1, t2].iter() {
            if *t >= T::Scalar::ZERO && *t <= T::Scalar::ONE {
                let intersection = edge.p0 + direction * (*t);
                let point_xy_dist =
                    point_xy_distance_to_cone_vertex_sq(intersection.x(), cy).sqrt();
                let cone_height_at_point = cone_slope * point_xy_dist;
                let cone_z = intersection.z() - cone_height_at_point;
                mt.insert(SearchResult::new(site_index, cone_z));
            }
        }
    }
}

/// Calculates the Euclidean distance between a point (x, 0)
/// and the cone's vertex (0, cy) in the XY plane
#[inline(always)]
fn point_xy_distance_to_cone_vertex_sq<T: GenericScalar>(x: T, cy: T) -> T {
    Float::powi(x, 2) + Float::powi(cy, 2)
}

// Helper function to compute cone z for endpoint collision
#[inline(always)]
fn end_point_cone_collision_with_distance<T: GenericScalar>(
    endpoint_z: T,
    slope: T,
    dist_to_endpoint: T,
) -> T {
    endpoint_z - slope * dist_to_endpoint
}

/*
#[allow(dead_code)]
fn cone_to_edge_collision_xz_plane<T: GenericVector3>(
    edge: EdgeAndCenterType<T>,
    cone: ConeProperties<T>,
    site_index: usize,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    // the edge is X-axis aligned to Y is the un clamped distance
    let un_clamped_distance = edge.p0.y().abs();
    if un_clamped_distance > cone.radius {
        // infinite edge was outside the range of the cone
        return;
    }

    // Calculate vector from p0 to p1
    let dp = edge.p1 - edge.p0;

    if dp.x().abs_diff_eq(&T::Scalar::ZERO, T::Scalar::EPSILON) {
        // when the line is almost vertical we only use p1
        cone_z_value_from_single_point_transformed(
            edge.p1,
            cone.r_sq,
            cone.height,
            (edge.center - edge.p1.to_2d()).magnitude_sq(),
            site_index,
            mt,
        );
        return;
    }
    let un_clamped_t = edge.t;
    // the 2d (xy) closest distance^2 between cone and edge
    let dist_sq_clamped_closest_point_xy = (edge.p0.to_2d()
        + dp.to_2d() * un_clamped_t.clamp(T::Scalar::ZERO, T::Scalar::ONE))
    .magnitude_sq();

    if dist_sq_clamped_closest_point_xy > cone.r_sq {
        return;
    }

    // ´m´ is always positive
    let m = dp.z() / dp.x();
    let m_sq = m * m;
    let m_tan_half_angle_sq = (m / cone.tan_half_angle).powi(2);

    let t_offset = {
        let dp_mag_sq = dp.magnitude_sq();
        let d_rq_sq = cone.r_sq * (m_sq.powi(2) + m_sq) / (m_sq + T::Scalar::ONE);
        let dist_sq_closest_point_xy = edge.p0.y().powi(2);

        // radius_factor_sq is a factor that diminishes by the radial distance from the edge.
        // at probe_radius the factor is 1.0 and 0.0 directly on top of the edge.
        let radius_factor_sq = dist_sq_closest_point_xy / cone.r_sq;

        -(radius_factor_sq * d_rq_sq / dp_mag_sq).sqrt()
    };

    if m_tan_half_angle_sq < T::Scalar::ONE {
        // the edge is not steeper than the cone ->
        // cone will collide somewhere along the cone surface (or apex)

        if un_clamped_t < T::Scalar::ZERO + t_offset {
            // test against p0
            let d_xy_p0_sq = edge.p0.to_2d().magnitude_sq();
            mt.insert(SearchResult::new(
                site_index,
                edge.p0.z() - (cone.radius / cone.tan_half_angle) * (d_xy_p0_sq / cone.r_sq).sqrt(),
            ));
            return;
        }

        if un_clamped_t > T::Scalar::ONE + t_offset {
            // test against p1
            let d_xy_p1_sq = edge.p1.to_2d().magnitude_sq();
            mt.insert(SearchResult::new(
                site_index,
                edge.p1.z() - (cone.radius / cone.tan_half_angle) * (d_xy_p1_sq / cone.r_sq).sqrt(),
            ));
            return;
        }

        let c0 = edge.p0.z() - m * edge.p0.x();
        let d =
            cone.height * (T::Scalar::ONE - m.powi(2)).sqrt() * (un_clamped_distance / cone.radius);
        mt.insert(SearchResult::new(site_index, c0 - d));
    } else {
        // the edge is steeper than the cone -> the cone acts like a cylinder when compared to the edge

        let dp_xy = dp.to_2d();
        if edge.center.is_abs_diff_eq(
            T::Vector2::new_2d((0.3475).into(), (-0.92).into()),
            0.01.into(),
        ) {
            println!("center:{:?}", edge.center);
        }
        if un_clamped_t > T::Scalar::ONE + t_offset {
            // test against p1
            let d_xy_p1_sq = edge.p1.to_2d().magnitude_sq();
            mt.insert(SearchResult::new(
                site_index,
                edge.p1.z() - (cone.radius / cone.tan_half_angle) * (d_xy_p1_sq / cone.r_sq).sqrt(),
            ));
            return;
        }
        if un_clamped_t < T::Scalar::ZERO + t_offset {
            // test against p0
            let d_xy_p0_sq = edge.p0.to_2d().magnitude_sq();
            mt.insert(SearchResult::new(
                site_index,
                edge.p0.z() - (cone.radius / cone.tan_half_angle) * (d_xy_p0_sq / cone.r_sq).sqrt(),
            ));
            return;
        }

        let clamped_t = un_clamped_t.clamp(T::Scalar::ZERO, T::Scalar::ONE);
        let clamped_closest_point_xy = edge.p0.to_2d() + dp_xy * clamped_t;
        let clamped_dist_sq_closest_point_xy = clamped_closest_point_xy.magnitude_sq();
        if clamped_dist_sq_closest_point_xy > cone.r_sq {
            return;
        }

        let unclamped_closest_point_xy = edge.p0.to_2d() + dp_xy * un_clamped_t;
        let delta_d_sq = cone.r_sq - unclamped_closest_point_xy.magnitude_sq();
        if delta_d_sq < T::Scalar::ZERO {
            return;
        }
        let unclamped_closest_point = edge.p0 + dp * un_clamped_t;
        let cz = unclamped_closest_point.z() + (m * m * delta_d_sq).sqrt()
            - cone.radius / cone.tan_half_angle;
        mt.insert(SearchResult::new(site_index, cz.min(edge.p1.z())));
    }
}
*/

/// Computes the Z coordinate of a cone's apex, given an endpoint of an edge in a transformed coordinate space.
///
/// This function is intended for use in a specific coordinate system where the apex of the cone is at the origin
/// (XY) and the cone's axis is aligned with the Z-axis. Given an endpoint `edge_endpoint` of an edge in this transformed space,
/// the function calculates the Z value of the cone's apex when the cone is lowered to just touch the edge. It takes into
/// account the XY distance from point `edge_endpoint` to the origin and the squared radius `r_sq` of the cone's base to compute
/// the Z offset of the apex.
///
/// # Arguments
///
/// * `edge_endpoint` - The 3D point representing an endpoint of the edge in the transformed coordinate space.
/// * `r_sq` - The squared radius of the cone's base.
/// * `height` - The height of the cone from its base to the apex.
/// * `site_index` - An index used to identify the specific cone or interaction in a collection or sequence.
/// * `mt` - A reference to a MaximumTracker used to store and track the potential Z value.
///
/// # Notes
///
/// The function assumes that the coordinate transformation aligns the cone's apex at the origin and orients its axis
/// along the Z-axis. The endpoint `p` represents a position on the edge in this space. The function is intended to be used
/// where the coordinates have been transformed appropriately for accurate calculations.
///
#[inline(always)]
fn cone_z_value_from_single_point_line_in_xz_plane<T: GenericVector3>(
    endpoint_z: T::Scalar,
    r_sq: T::Scalar,
    height: T::Scalar,
    dist_sq: T::Scalar,
    site_index: u32,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    if dist_sq < r_sq {
        mt.insert(SearchResult::new(
            site_index,
            endpoint_z - height * (dist_sq / r_sq).sqrt(),
        ));
    }
}

/*
#[allow(dead_code)]
#[inline(always)]
fn z_cone_vs_single_point<T: GenericVector3>(
    site_index: usize,
    p: T,
    probe_radius: T::Scalar,
    distance: T::Scalar,
    height: T::Scalar,
    mt: &mut MaximumTracker<SearchResult<T>>,
) {
    if distance <= probe_radius {
        mt.insert(SearchResult::new(
            site_index,
            p.z() - height * distance / probe_radius,
        ));
    }
}
*/