<commit_before>use bounding_volume::AABB;
use math::Isometry;
use na::{Point2, Real, Vector3};
use partitioning::{BestFirstBVVisitStatus, BestFirstDataVisitStatus, BestFirstVisitor};
use query::{closest_points_internal, Ray, RayCast, RayIntersection};
use shape::{CompositeShape, HeightField, FeatureId};

#[cfg(feature = "dim2")]
impl<N: Real> RayCast<N> for HeightField<N> {
    #[inline]
    fn toi_and_normal_with_ray(
        &self,
        m: &Isometry<N>,
        ray: &Ray<N>,
        _: bool,
    ) -> Option<RayIntersection<N>>
    {
        let aabb = self.aabb();
        let ls_ray = ray.inverse_transform_by(m);
        let (min_t, max_t) = aabb.clip_ray_parameters(&ls_ray)?;
        let (clip_ray_a, clip_ray_b) = (ls_ray.point_at(min_t), ls_ray.point_at(max_t));
        let mut curr = match self.cell_at_point(&clip_ray_a) {
            Some(cell) => cell,
            // None may happen due to slight numerical errors.
            None => if ls_ray.origin.x > N::zero() { self.num_cells() - 1 } else {
                0
            }
        };

        /*
         * Test the segment under the ray.
         */
        if let Some(seg) = self.segment_at(curr) {
            let (s, t) = closest_points_internal::line_against_line_parameters(&ray.origin, &ray.dir, seg.a(), &seg.scaled_direction());
            if s >= N::zero() && t >= N::zero() && t <= N::one() {
                // Cast succeeded on the first element!
                // FIXME: return the correct feature and normal.
                return Some(RayIntersection::new(s, m * seg.normal().unwrap().unwrap(), FeatureId::Face(curr)))
            }
        }

        /*
         * Test other segments in the path of the ray.
         */
        if ls_ray.dir.x == N::zero() {
            return None;
        }

        let right = ls_ray.dir.x > N::zero();
        let cell_width = self.cell_width();
        let start_x = self.start_x();

        while (right && curr < self.num_cells()) || (!right && curr > 0) {
            let curr_param;

            if right {
                curr += 1;
                curr_param = (cell_width * na::convert(curr as f64) + start_x - ls_ray.origin.x) / ls_ray.dir.x;
            } else {
                curr_param = (ls_ray.origin.x - cell_width * na::convert(curr as f64) - start_x) / ls_ray.dir.x;
                curr -= 1;
            }

            if curr_param >= max_t {
                // The part of the ray after max_t is outside of the heightfield AABB.
                return None;
            }

            if let Some(seg) = self.segment_at(curr) {
                // TODO: test the y-coordinates (equivalent to an AABB test) before actually computing the intersection.
                let (s, t) = closest_points_internal::line_against_line_parameters(&ray.origin, &ray.dir, seg.a(), &seg.scaled_direction());
                if t >= N::zero() && t <= N::one() {
                    // FIXME: return the correct feature and normal.
                    return Some(RayIntersection::new(s, m * seg.normal().unwrap().unwrap(), FeatureId::Face(curr)))
                }
            }
        }

        None
    }
}


#[cfg(feature = "dim3")]
impl<N: Real> RayCast<N> for HeightField<N> {
    #[inline]
    fn toi_and_normal_with_ray(
        &self,
        m: &Isometry<N>,
        ray: &Ray<N>,
        solid: bool,
    ) -> Option<RayIntersection<N>>
    {
        let aabb = self.aabb();
        let ls_ray = ray.inverse_transform_by(m);
        let (min_t, max_t) = aabb.clip_ray_parameters(&ls_ray)?;
        let (clip_ray_a, clip_ray_b) = (ls_ray.point_at(min_t), ls_ray.point_at(max_t));
        let mut cell = match self.cell_at_point(&clip_ray_a) {
            Some(cell) => cell,
            // None may happen due to slight numerical errors.
            None => (0, 0) /* XXX: if ls_ray.origin.x > N::zero() { self.num_cells() - 1 } else {
                0
            }*/
        };

        loop {
            let tris = self.triangles_at(cell.0, cell.1);
            let inter1 = tris.0.and_then(|tri| tri.toi_and_normal_with_ray(m, ray, solid));
            let inter2 = tris.1.and_then(|tri| tri.toi_and_normal_with_ray(m, ray, solid));

            match (inter1, inter2) {
                (Some(inter1), Some(inter2)) => {
                    // XXX: adjust the feature ID.
                    if inter1.toi < inter2.toi {
                        return Some(inter1);
                    } else {
                        return Some(inter2);
                    }
                }
                (Some(inter), None) => {<commit_msg>Fix ray-cast on 3D heightfield initialization.<commit_after>use bounding_volume::AABB;
use math::Isometry;
use na::{Point2, Real, Vector3};
use partitioning::{BestFirstBVVisitStatus, BestFirstDataVisitStatus, BestFirstVisitor};
use query::{closest_points_internal, Ray, RayCast, RayIntersection};
use shape::{CompositeShape, HeightField, FeatureId};

#[cfg(feature = "dim2")]
impl<N: Real> RayCast<N> for HeightField<N> {
    #[inline]
    fn toi_and_normal_with_ray(
        &self,
        m: &Isometry<N>,
        ray: &Ray<N>,
        _: bool,
    ) -> Option<RayIntersection<N>>
    {
        let aabb = self.aabb();
        let ls_ray = ray.inverse_transform_by(m);
        let (min_t, max_t) = aabb.clip_ray_parameters(&ls_ray)?;
        let (clip_ray_a, clip_ray_b) = (ls_ray.point_at(min_t), ls_ray.point_at(max_t));
        let mut curr = match self.cell_at_point(&clip_ray_a) {
            Some(cell) => cell,
            // None may happen due to slight numerical errors.
            None => if ls_ray.origin.x > N::zero() { self.num_cells() - 1 } else {
                0
            }
        };

        /*
         * Test the segment under the ray.
         */
        if let Some(seg) = self.segment_at(curr) {
            let (s, t) = closest_points_internal::line_against_line_parameters(&ray.origin, &ray.dir, seg.a(), &seg.scaled_direction());
            if s >= N::zero() && t >= N::zero() && t <= N::one() {
                // Cast succeeded on the first element!
                // FIXME: return the correct feature and normal.
                return Some(RayIntersection::new(s, m * seg.normal().unwrap().unwrap(), FeatureId::Face(curr)))
            }
        }

        /*
         * Test other segments in the path of the ray.
         */
        if ls_ray.dir.x == N::zero() {
            return None;
        }

        let right = ls_ray.dir.x > N::zero();
        let cell_width = self.cell_width();
        let start_x = self.start_x();

        while (right && curr < self.num_cells()) || (!right && curr > 0) {
            let curr_param;

            if right {
                curr += 1;
                curr_param = (cell_width * na::convert(curr as f64) + start_x - ls_ray.origin.x) / ls_ray.dir.x;
            } else {
                curr_param = (ls_ray.origin.x - cell_width * na::convert(curr as f64) - start_x) / ls_ray.dir.x;
                curr -= 1;
            }

            if curr_param >= max_t {
                // The part of the ray after max_t is outside of the heightfield AABB.
                return None;
            }

            if let Some(seg) = self.segment_at(curr) {
                // TODO: test the y-coordinates (equivalent to an AABB test) before actually computing the intersection.
                let (s, t) = closest_points_internal::line_against_line_parameters(&ray.origin, &ray.dir, seg.a(), &seg.scaled_direction());
                if t >= N::zero() && t <= N::one() {
                    // FIXME: return the correct feature and normal.
                    return Some(RayIntersection::new(s, m * seg.normal().unwrap().unwrap(), FeatureId::Face(curr)))
                }
            }
        }

        None
    }
}


#[cfg(feature = "dim3")]
impl<N: Real> RayCast<N> for HeightField<N> {
    #[inline]
    fn toi_and_normal_with_ray(
        &self,
        m: &Isometry<N>,
        ray: &Ray<N>,
        solid: bool,
    ) -> Option<RayIntersection<N>>
    {
        let aabb = self.aabb();
        let ls_ray = ray.inverse_transform_by(m);
        let (min_t, max_t) = aabb.clip_ray_parameters(&ls_ray)?;
        let (clip_ray_a, clip_ray_b) = (ls_ray.point_at(min_t), ls_ray.point_at(max_t));
        let mut cell = match self.cell_at_point(&clip_ray_a) {
            Some(cell) => cell,
            // None may happen due to slight numerical errors.
            None => {
                let i = if ls_ray.origin.z > N::zero() {
                    self.nrows()
                } else {
                    0
                };

                let j = if ls_ray.origin.x > N::zero() {
                    self.ncols()
                } else {
                    0
                };

                (i, j)
            }
        };

        loop {
            let tris = self.triangles_at(cell.0, cell.1);
            let inter1 = tris.0.and_then(|tri| tri.toi_and_normal_with_ray(m, ray, solid));
            let inter2 = tris.1.and_then(|tri| tri.toi_and_normal_with_ray(m, ray, solid));

            match (inter1, inter2) {
                (Some(inter1), Some(inter2)) => {
                    // XXX: adjust the feature ID.
                    if inter1.toi < inter2.toi {
                        return Some(inter1);
                    } else {
                        return Some(inter2);
                    }
                }
                (Some(inter), None) => {
{'fraction_non_alphanumeric': 0.13189600507292326, 'fraction_numerical': 0.0064468399915451275, 'mean_word_length': 1.6983176504134587, 'pattern_counts': {'":': 0, '<': 39, '<?xml version=': 0, '>': 64, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4999971', 'n_tokens_mistral': 3041, 'n_tokens_neox': 2926, 'n_words': 675}