diff --git a/src/query/time_of_impact/time_of_impact_heightfield_shape.rs b/src/query/time_of_impact/time_of_impact_heightfield_shape.rs
new file mode 100644
index 00000000..a380919e
--- /dev/null
+++ b/src/query/time_of_impact/time_of_impact_heightfield_shape.rs
@@ -0,0 +1,310 @@
+use crate::math::{Isometry, Real, Vector};
+use crate::query::{QueryDispatcher, Ray, Unsupported, TOI};
+use crate::shape::{GenericHeightField, HeightFieldCellStatus, HeightFieldStorage, Shape};
+#[cfg(feature = "dim3")]
+use crate::{bounding_volume::AABB, query::RayCast};
+
+/// Time Of Impact between a moving shape and a heightfield.
+#[cfg(feature = "dim2")]
+pub fn time_of_impact_heightfield_shape<Heights, Status, D: ?Sized>(
+    dispatcher: &D,
+    pos12: &Isometry<Real>,
+    vel12: &Vector<Real>,
+    heightfield1: &GenericHeightField<Heights, Status>,
+    g2: &dyn Shape,
+    max_toi: Real,
+    stop_at_penetration: bool,
+) -> Result<Option<TOI>, Unsupported>
+where
+    Heights: HeightFieldStorage<Item = Real>,
+    Status: HeightFieldStorage<Item = HeightFieldCellStatus>,
+    D: QueryDispatcher,
+{
+    let aabb2_1 = g2.compute_aabb(pos12);
+    let ray = Ray::new(aabb2_1.center(), *vel12);
+
+    let mut curr_range = heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1);
+    // Enlarge the range by 1 to account for movement within a cell.
+    let right = ray.dir.x > 0.0;
+
+    if right {
+        curr_range.end += 1;
+    } else {
+        curr_range.start -= 1;
+    }
+
+    let mut best_hit = None::<TOI>;
+
+    /*
+     * Test the segment under the ray.
+     */
+    let clamped_curr_range = curr_range.start.clamp(0, heightfield1.num_cells() as isize) as usize
+        ..curr_range.end.clamp(0, heightfield1.num_cells() as isize) as usize;
+    for curr in clamped_curr_range {
+        if let Some(seg) = heightfield1.segment_at(curr) {
+            // TODO: pre-check using a ray-cast on the AABBs first?
+            if let Some(hit) =
+                dispatcher.time_of_impact(pos12, vel12, &seg, g2, max_toi, stop_at_penetration)?
+            {
+                if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) {
+                    best_hit = Some(hit);
+                }
+            }
+        }
+    }
+
+    /*
+     * Test other segments in the path of the ray.
+     */
+    if ray.dir.x == 0.0 {
+        return Ok(best_hit);
+    }
+
+    let cell_width = heightfield1.cell_width();
+    let start_x = heightfield1.start_x();
+
+    let mut curr_elt = if right {
+        (curr_range.end - 1).max(0)
+    } else {
+        curr_range.start.min(heightfield1.num_cells() as isize - 1)
+    };
+
+    while (right && curr_elt < heightfield1.num_cells() as isize - 1) || (!right && curr_elt > 0) {
+        let curr_param;
+
+        if right {
+            curr_elt += 1;
+            curr_param = (cell_width * na::convert::<f64, Real>(curr_elt as f64) + start_x
+                - ray.origin.x)
+                / ray.dir.x;
+        } else {
+            curr_param =
+                (ray.origin.x - cell_width * na::convert::<f64, Real>(curr_elt as f64) - start_x)
+                    / ray.dir.x;
+            curr_elt -= 1;
+        }
+
+        if curr_param >= max_toi {
+            break;
+        }
+
+        if let Some(seg) = heightfield1.segment_at(curr_elt as usize) {
+            // TODO: pre-check using a ray-cast on the AABBs first?
+            if let Some(hit) =
+                dispatcher.time_of_impact(pos12, vel12, &seg, g2, max_toi, stop_at_penetration)?
+            {
+                if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) {
+                    best_hit = Some(hit);
+                }
+            }
+        }
+    }
+
+    Ok(best_hit)
+}
+
+/// Time Of Impact between a moving shape and a heightfield.
+#[cfg(feature = "dim3")]
+pub fn time_of_impact_heightfield_shape<Heights, Status, D: ?Sized>(
+    dispatcher: &D,
+    pos12: &Isometry<Real>,
+    vel12: &Vector<Real>,
+    heightfield1: &GenericHeightField<Heights, Status>,
+    g2: &dyn Shape,
+    max_toi: Real,
+    stop_at_penetration: bool,
+) -> Result<Option<TOI>, Unsupported>
+where
+    Heights: HeightFieldStorage<Item = Real>,
+    Status: HeightFieldStorage<Item = HeightFieldCellStatus>,
+    D: QueryDispatcher,
+{
+    let aabb1 = heightfield1.local_aabb();
+    let mut aabb2_1 = g2.compute_aabb(pos12);
+    let ray = Ray::new(aabb2_1.center(), *vel12);
+
+    // Find the first hit between the aabbs.
+    let hext2_1 = aabb2_1.half_extents();
+    let msum = AABB::new(aabb1.mins - hext2_1, aabb1.maxs + hext2_1);
+    if let Some(toi) = msum.cast_local_ray(&ray, max_toi, true) {
+        // Advance the aabb2 to the hit point.
+        aabb2_1.mins += ray.dir * toi;
+        aabb2_1.maxs += ray.dir * toi;
+    } else {
+        return Ok(None);
+    }
+
+    let (mut curr_range_i, mut curr_range_j) =
+        heightfield1.unclamped_elements_range_in_local_aabb(&aabb2_1);
+    let (ncells_i, ncells_j) = heightfield1.num_cells_ij();
+    let mut best_hit = None::<TOI>;
+
+    /*
+     * Enlarge the ranges by 1 to account for any movement within one cell.
+     */
+    if ray.dir.z > 0.0 {
+        curr_range_i.end += 1;
+    } else if ray.dir.z < 0.0 {
+        curr_range_i.start -= 1;
+    }
+
+    if ray.dir.x > 0.0 {
+        curr_range_j.end += 1;
+    } else if ray.dir.x < 0.0 {
+        curr_range_j.start -= 1;
+    }
+
+    /*
+     * Test the segment under the ray.
+     */
+    let clamped_curr_range_i = curr_range_i.start.clamp(0, ncells_i as isize)
+        ..curr_range_i.end.clamp(0, ncells_i as isize);
+    let clamped_curr_range_j = curr_range_j.start.clamp(0, ncells_j as isize)
+        ..curr_range_j.end.clamp(0, ncells_j as isize);
+
+    let mut hit_triangles = |i, j| {
+        if i >= 0 && j >= 0 {
+            let (tri_a, tri_b) = heightfield1.triangles_at(i as usize, j as usize);
+            for tri in [tri_a, tri_b] {
+                if let Some(tri) = tri {
+                    // TODO: pre-check using a ray-cast on the AABBs first?
+                    if let Some(hit) = dispatcher.time_of_impact(
+                        pos12,
+                        vel12,
+                        &tri,
+                        g2,
+                        max_toi,
+                        stop_at_penetration,
+                    )? {
+                        if hit.toi < best_hit.map(|toi| toi.toi).unwrap_or(Real::MAX) {
+                            best_hit = Some(hit);
+                        }
+                    }
+                }
+            }
+        }
+
+        Ok(())
+    };
+
+    for i in clamped_curr_range_i {
+        for j in clamped_curr_range_j.clone() {
+            hit_triangles(i, j)?;
+        }
+    }
+
+    if ray.dir.y == 0.0 {
+        return Ok(best_hit);
+    }
+
+    let mut cell = heightfield1.unclamped_cell_at_point(&aabb2_1.center());
+
+    loop {
+        let prev_cell = cell;
+
+        /*
+         * Find the next cell to cast the ray on.
+         */
+        let toi_x = if ray.dir.x > 0.0 {
+            let x = heightfield1.signed_x_at(cell.1 + 1);
+            (x - ray.origin.x) / ray.dir.x
+        } else if ray.dir.x < 0.0 {
+            let x = heightfield1.signed_x_at(cell.1 + 0);
+            (x - ray.origin.x) / ray.dir.x
+        } else {
+            Real::MAX
+        };
+
+        let toi_z = if ray.dir.z > 0.0 {
+            let z = heightfield1.signed_z_at(cell.0 + 1);
+            (z - ray.origin.z) / ray.dir.z
+        } else if ray.dir.z < 0.0 {
+            let z = heightfield1.signed_z_at(cell.0 + 0);
+            (z - ray.origin.z) / ray.dir.z
+        } else {
+            Real::MAX
+        };
+
+        if toi_x > max_toi && toi_z > max_toi {
+            break;
+        }
+
+        if toi_x >= 0.0 && toi_x <= toi_z {
+            cell.1 += ray.dir.x.signum() as isize;
+        }
+
+        if toi_z >= 0.0 && toi_z <= toi_x {
+            cell.0 += ray.dir.z.signum() as isize;
+        }
+
+        if cell == prev_cell {
+            break;
+        }
+
+        let cell_diff = (cell.0 - prev_cell.0, cell.1 - prev_cell.1);
+        curr_range_i.start += cell_diff.0;
+        curr_range_i.end += cell_diff.0;
+        curr_range_j.start += cell_diff.1;
+        curr_range_j.end += cell_diff.1;
+
+        let new_line_i = if cell_diff.0 > 0 {
+            curr_range_i.end
+        } else {
+            curr_range_i.start
+        };
+
+        let new_line_j = if cell_diff.1 > 0 {
+            curr_range_j.end
+        } else {
+            curr_range_j.start
+        };
+
+        let ignore_line_i = new_line_i < 0 || new_line_i >= ncells_i as isize;
+        let ignore_line_j = new_line_j < 0 || new_line_j >= ncells_j as isize;
+
+        if ignore_line_i && ignore_line_j {
+            break;
+        }
+
+        if !ignore_line_i && cell_diff.0 != 0 {
+            for j in curr_range_j.clone() {
+                hit_triangles(new_line_i, j)?;
+            }
+        }
+
+        if !ignore_line_j && cell_diff.1 != 0 {
+            for i in curr_range_i.clone() {
+                hit_triangles(i, new_line_j)?;
+            }
+        }
+    }
+
+    Ok(best_hit)
+}
+
+/// Time Of Impact between a moving shape and a heightfield.
+pub fn time_of_impact_shape_heightfield<Heights, Status, D: ?Sized>(
+    dispatcher: &D,
+    pos12: &Isometry<Real>,
+    vel12: &Vector<Real>,
+    g1: &dyn Shape,
+    heightfield2: &GenericHeightField<Heights, Status>,
+    max_toi: Real,
+    stop_at_penetration: bool,
+) -> Result<Option<TOI>, Unsupported>
+where
+    Heights: HeightFieldStorage<Item = Real>,
+    Status: HeightFieldStorage<Item = HeightFieldCellStatus>,
+    D: QueryDispatcher,
+{
+    Ok(time_of_impact_heightfield_shape(
+        dispatcher,
+        &pos12.inverse(),
+        &-pos12.inverse_transform_vector(&vel12),
+        heightfield2,
+        g1,
+        max_toi,
+        stop_at_penetration,
+    )?
+    .map(|toi| toi.swapped()))
+}
diff --git a/src/shape/heightfield2.rs b/src/shape/heightfield2.rs
index e176c2c9..a1e89a52 100644
--- a/src/shape/heightfield2.rs
+++ b/src/shape/heightfield2.rs
@@ -2,6 +2,7 @@
 use na::ComplexField;
 #[cfg(feature = "std")]
 use na::DVector;
+use std::ops::Range;
 #[cfg(all(feature = "std", feature = "cuda"))]
 use {crate::utils::CudaArray1, cust::error::CudaResult};
 
@@ -173,41 +174,44 @@ where
 
     /// The width of a single cell of this heightfield, without taking the scale factor into account.
     pub fn unit_cell_width(&self) -> Real {
-        1.0 / na::convert::<f64, Real>(self.heights.len() as f64 - 1.0)
+        1.0 / (self.heights.len() as Real - 1.0)
     }
 
     /// The left-most x-coordinate of this heightfield.
     pub fn start_x(&self) -> Real {
-        self.scale.x * na::convert::<f64, Real>(-0.5)
+        self.scale.x * -0.5
+    }
+
+    fn quantize_floor_unclamped(&self, val: Real, seg_length: Real) -> isize {
+        ((val + 0.5) / seg_length).floor() as isize
+    }
+
+    fn quantize_ceil_unclamped(&self, val: Real, seg_length: Real) -> isize {
+        ((val + 0.5) / seg_length).ceil() as isize
     }
 
     fn quantize_floor(&self, val: Real, seg_length: Real) -> usize {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
-        let i = na::clamp(
-            ((val + _0_5) / seg_length).floor(),
+        na::clamp(
+            ((val + 0.5) / seg_length).floor(),
             0.0,
-            na::convert::<f64, Real>((self.num_cells() - 1) as f64),
-        );
-        na::convert_unchecked::<Real, f64>(i) as usize
+            (self.num_cells() - 1) as Real,
+        ) as usize
     }
 
     fn quantize_ceil(&self, val: Real, seg_length: Real) -> usize {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
-        let i = na::clamp(
-            ((val + _0_5) / seg_length).ceil(),
+        na::clamp(
+            ((val + 0.5) / seg_length).ceil(),
             0.0,
-            na::convert::<f64, Real>(self.num_cells() as f64),
-        );
-        na::convert_unchecked::<Real, f64>(i) as usize
+            self.num_cells() as Real,
+        ) as usize
     }
 
     /// Index of the cell a point is on after vertical projection.
     pub fn cell_at_point(&self, pt: &Point2<Real>) -> Option<usize> {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
         let scaled_pt = pt.coords.component_div(&self.scale);
         let seg_length = self.unit_cell_width();
 
-        if scaled_pt.x < -_0_5 || scaled_pt.x > _0_5 {
+        if scaled_pt.x < -0.5 || scaled_pt.x > 0.5 {
             // Outside of the heightfield bounds.
             None
         } else {
@@ -241,10 +245,9 @@ where
             return None;
         }
 
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
-        let seg_length = 1.0 / na::convert::<f64, Real>(self.heights.len() as f64 - 1.0);
+        let seg_length = 1.0 / (self.heights.len() as Real - 1.0);
 
-        let x0 = -_0_5 + seg_length * na::convert::<f64, Real>(i as f64);
+        let x0 = -0.5 + seg_length * (i as Real);
         let x1 = x0 + seg_length;
 
         let y0 = self.heights.get(i + 0);
@@ -270,14 +273,24 @@ where
         !self.status.get(i)
     }
 
+    /// The range of segment ids that may intersect the given local AABB.
+    pub fn unclamped_elements_range_in_local_aabb(&self, aabb: &AABB) -> Range<isize> {
+        let ref_mins = aabb.mins.coords.component_div(&self.scale);
+        let ref_maxs = aabb.maxs.coords.component_div(&self.scale);
+        let seg_length = 1.0 / (self.heights.len() as Real - 1.0);
+
+        let min_x = self.quantize_floor_unclamped(ref_mins.x, seg_length);
+        let max_x = self.quantize_ceil_unclamped(ref_maxs.x, seg_length);
+        min_x..max_x
+    }
+
     /// Applies `f` to each segment of this heightfield that intersects the given `aabb`.
     pub fn map_elements_in_local_aabb(&self, aabb: &AABB, f: &mut impl FnMut(u32, &Segment)) {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
         let ref_mins = aabb.mins.coords.component_div(&self.scale);
         let ref_maxs = aabb.maxs.coords.component_div(&self.scale);
-        let seg_length = 1.0 / na::convert::<f64, Real>(self.heights.len() as f64 - 1.0);
+        let seg_length = 1.0 / (self.heights.len() as Real - 1.0);
 
-        if ref_maxs.x < -_0_5 || ref_mins.x > _0_5 {
+        if ref_maxs.x < -0.5 || ref_mins.x > 0.5 {
             // Outside of the heightfield bounds.
             return;
         }
@@ -292,7 +305,7 @@ where
                 continue;
             }
 
-            let x0 = -_0_5 + seg_length * na::convert::<f64, Real>(i as f64);
+            let x0 = -0.5 + seg_length * (i as Real);
             let x1 = x0 + seg_length;
 
             let y0 = self.heights.get(i + 0);
diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs
index 7f317346..9123a664 100644
--- a/src/shape/heightfield3.rs
+++ b/src/shape/heightfield3.rs
@@ -1,5 +1,6 @@
 #[cfg(feature = "std")]
 use na::DMatrix;
+use std::ops::Range;
 #[cfg(all(feature = "std", feature = "cuda"))]
 use {crate::utils::CudaArray2, cust::error::CudaResult};
 
@@ -106,7 +107,7 @@ impl HeightField {
         );
         let max = heights.max();
         let min = heights.min();
-        let hscale = scale * na::convert::<_, Real>(0.5);
+        let hscale = scale * 0.5;
         let aabb = AABB::new(
             Point3::new(-hscale.x, min * scale.y, -hscale.z),
             Point3::new(hscale.x, max * scale.y, hscale.z),
@@ -187,29 +188,28 @@ where
         }
     }
 
+    fn quantize_floor_unclamped(&self, val: Real, cell_size: Real) -> isize {
+        ((val + 0.5) / cell_size).floor() as isize
+    }
+
+    fn quantize_ceil_unclamped(&self, val: Real, cell_size: Real) -> isize {
+        ((val + 0.5) / cell_size).ceil() as isize
+    }
+
     fn quantize_floor(&self, val: Real, cell_size: Real, num_cells: usize) -> usize {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
-        let i = na::clamp(
-            ((val + _0_5) / cell_size).floor(),
+        na::clamp(
+            ((val + 0.5) / cell_size).floor(),
             0.0,
-            na::convert::<f64, Real>((num_cells - 1) as f64),
-        );
-        na::convert_unchecked::<Real, f64>(i) as usize
+            (num_cells - 1) as Real,
+        ) as usize
     }
 
     fn quantize_ceil(&self, val: Real, cell_size: Real, num_cells: usize) -> usize {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
-        let i = na::clamp(
-            ((val + _0_5) / cell_size).ceil(),
-            0.0,
-            na::convert::<f64, Real>(num_cells as f64),
-        );
-        na::convert_unchecked::<Real, f64>(i) as usize
+        na::clamp(((val + 0.5) / cell_size).ceil(), 0.0, num_cells as Real) as usize
     }
 
     /// The pair of index of the cell containing the vertical projection of the given point.
     pub fn closest_cell_at_point(&self, pt: &Point3<Real>) -> (usize, usize) {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
         let scaled_pt = pt.coords.component_div(&self.scale);
         let cell_width = self.unit_cell_width();
         let cell_height = self.unit_cell_height();
@@ -223,14 +223,13 @@ where
 
     /// The pair of index of the cell containing the vertical projection of the given point.
     pub fn cell_at_point(&self, pt: &Point3<Real>) -> Option<(usize, usize)> {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
         let scaled_pt = pt.coords.component_div(&self.scale);
         let cell_width = self.unit_cell_width();
         let cell_height = self.unit_cell_height();
         let ncells_x = self.ncols();
         let ncells_z = self.nrows();
 
-        if scaled_pt.x < -_0_5 || scaled_pt.x > _0_5 || scaled_pt.z < -_0_5 || scaled_pt.z > _0_5 {
+        if scaled_pt.x < -0.5 || scaled_pt.x > 0.5 || scaled_pt.z < -0.5 || scaled_pt.z > 0.5 {
             // Outside of the heightfield bounds.
             None
         } else {
@@ -240,16 +239,35 @@ where
         }
     }
 
+    /// The pair of index of the cell containing the vertical projection of the given point.
+    pub fn unclamped_cell_at_point(&self, pt: &Point3<Real>) -> (isize, isize) {
+        let scaled_pt = pt.coords.component_div(&self.scale);
+        let cell_width = self.unit_cell_width();
+        let cell_height = self.unit_cell_height();
+
+        let j = self.quantize_floor_unclamped(scaled_pt.x, cell_width);
+        let i = self.quantize_floor_unclamped(scaled_pt.z, cell_height);
+        (i, j)
+    }
+
     /// The smallest x coordinate of the `j`-th column of this heightfield.
     pub fn x_at(&self, j: usize) -> Real {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
-        (-_0_5 + self.unit_cell_width() * na::convert::<f64, Real>(j as f64)) * self.scale.x
+        (-0.5 + self.unit_cell_width() * (j as Real)) * self.scale.x
     }
 
     /// The smallest z coordinate of the start of the `i`-th row of this heightfield.
     pub fn z_at(&self, i: usize) -> Real {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
-        (-_0_5 + self.unit_cell_height() * na::convert::<f64, Real>(i as f64)) * self.scale.z
+        (-0.5 + self.unit_cell_height() * (i as Real)) * self.scale.z
+    }
+
+    /// The smallest x coordinate of the `j`-th column of this heightfield.
+    pub fn signed_x_at(&self, j: isize) -> Real {
+        (-0.5 + self.unit_cell_width() * (j as Real)) * self.scale.x
+    }
+
+    /// The smallest z coordinate of the start of the `i`-th row of this heightfield.
+    pub fn signed_z_at(&self, i: isize) -> Real {
+        (-0.5 + self.unit_cell_height() * (i as Real)) * self.scale.z
     }
 
     /// An iterator through all the triangles of this heightfield.
@@ -281,6 +299,10 @@ where
     /// Returns `None` fore triangles that have been removed because of their user-defined status
     /// flags (described by the `HeightFieldCellStatus` bitfield).
     pub fn triangles_at(&self, i: usize, j: usize) -> (Option<Triangle>, Option<Triangle>) {
+        if i >= self.heights.nrows() - 1 || j >= self.heights.ncols() - 1 {
+            return (None, None);
+        }
+
         let status = self.status.get(i, j);
 
         if status.contains(
@@ -293,11 +315,10 @@ where
         let cell_width = self.unit_cell_width();
         let cell_height = self.unit_cell_height();
 
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
-        let z0 = -_0_5 + cell_height * na::convert::<f64, Real>(i as f64);
+        let z0 = -0.5 + cell_height * (i as Real);
         let z1 = z0 + cell_height;
 
-        let x0 = -_0_5 + cell_width * na::convert::<f64, Real>(j as f64);
+        let x0 = -0.5 + cell_width * (j as Real);
         let x1 = x0 + cell_width;
 
         let y00 = self.heights.get(i + 0, j + 0);
@@ -347,6 +368,11 @@ where
         }
     }
 
+    /// The number of cells of this heightfield along each dimension.
+    pub fn num_cells_ij(&self) -> (usize, usize) {
+        (self.nrows(), self.ncols())
+    }
+
     /// The status of the `(i, j)`-th cell.
     pub fn cell_status(&self, i: usize, j: usize) -> HeightFieldCellStatus {
         self.status.get(i, j)
@@ -403,12 +429,12 @@ where
 
     /// The width (extent along its local `x` axis) of each cell of this heightmap, excluding the scale factor.
     pub fn unit_cell_width(&self) -> Real {
-        1.0 / na::convert::<f64, Real>(self.heights.ncols() as f64 - 1.0)
+        1.0 / (self.heights.ncols() as Real - 1.0)
     }
 
     /// The height (extent along its local `z` axis) of each cell of this heightmap, excluding the scale factor.
     pub fn unit_cell_height(&self) -> Real {
-        1.0 / na::convert::<f64, Real>(self.heights.nrows() as f64 - 1.0)
+        1.0 / (self.heights.nrows() as Real - 1.0)
     }
 
     /// The AABB of this heightmap.
@@ -510,9 +536,26 @@ where
         }
     }
 
+    /// The range of segment ids that may intersect the given local AABB.
+    pub fn unclamped_elements_range_in_local_aabb(
+        &self,
+        aabb: &AABB,
+    ) -> (Range<isize>, Range<isize>) {
+        let ref_mins = aabb.mins.coords.component_div(&self.scale);
+        let ref_maxs = aabb.maxs.coords.component_div(&self.scale);
+        let cell_width = self.unit_cell_width();
+        let cell_height = self.unit_cell_height();
+
+        let min_x = self.quantize_floor_unclamped(ref_mins.x, cell_width);
+        let min_z = self.quantize_floor_unclamped(ref_mins.z, cell_height);
+
+        let max_x = self.quantize_ceil_unclamped(ref_maxs.x, cell_width);
+        let max_z = self.quantize_ceil_unclamped(ref_maxs.z, cell_height);
+        (min_z..max_z, min_x..max_x)
+    }
+
     /// Applies the function `f` to all the triangles of this heightfield intersecting the given AABB.
     pub fn map_elements_in_local_aabb(&self, aabb: &AABB, f: &mut impl FnMut(u32, &Triangle)) {
-        let _0_5: Real = na::convert::<f64, Real>(0.5);
         let ncells_x = self.ncols();
         let ncells_z = self.nrows();
 
@@ -521,7 +564,7 @@ where
         let cell_width = self.unit_cell_width();
         let cell_height = self.unit_cell_height();
 
-        if ref_maxs.x <= -_0_5 || ref_maxs.z <= -_0_5 || ref_mins.x >= _0_5 || ref_mins.z >= _0_5 {
+        if ref_maxs.x <= -0.5 || ref_maxs.z <= -0.5 || ref_mins.x >= 0.5 || ref_mins.z >= 0.5 {
             // Outside of the heightfield bounds.
             return;
         }
@@ -542,10 +585,10 @@ where
                     continue;
                 }
 
-                let z0 = -_0_5 + cell_height * na::convert::<f64, Real>(i as f64);
+                let z0 = -0.5 + cell_height * (i as Real);
                 let z1 = z0 + cell_height;
 
-                let x0 = -_0_5 + cell_width * na::convert::<f64, Real>(j as f64);
+                let x0 = -0.5 + cell_width * (j as Real);
                 let x1 = x0 + cell_width;
 
                 let y00 = self.heights.get(i + 0, j + 0);