Complete the WQuadtree construction and ray-cast.

5 files changed, 386 insertions, 229 deletions

diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 5fcdf71..406727f 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -52,7 +52,8 @@ pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_norma
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 #[cfg(feature = "dim3")]
 pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
-pub(crate) use self::waabb::WAABB;
+pub(crate) use self::waabb::{WRay, WAABB};
+pub(crate) use self::wquadtree::WQuadtree;
 //pub(crate) use self::z_order::z_cmp_floats;
 
 mod ball;
@@ -79,4 +80,5 @@ pub(crate) mod sat;
 pub(crate) mod triangle;
 mod trimesh;
 mod waabb;
+mod wquadtree;
 //mod z_order;
diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs
index 702b5aa..0664a50 100644
--- a/src/geometry/waabb.rs
+++ b/src/geometry/waabb.rs
@@ -1,7 +1,10 @@
+use crate::geometry::Ray;
 #[cfg(feature = "serde-serialize")]
 use crate::math::DIM;
-use crate::math::{Point, SIMD_WIDTH};
+use crate::math::{Point, Vector, SIMD_WIDTH};
+use crate::utils;
 use ncollide::bounding_volume::AABB;
+use num::{One, Zero};
 #[cfg(feature = "simd-is-enabled")]
 use {
     crate::math::{SimdBool, SimdFloat},
@@ -10,6 +13,29 @@ use {
 
 #[derive(Debug, Copy, Clone)]
 #[cfg(feature = "simd-is-enabled")]
+pub(crate) struct WRay {
+    pub origin: Point<SimdFloat>,
+    pub dir: Vector<SimdFloat>,
+}
+
+impl WRay {
+    pub fn splat(ray: Ray) -> Self {
+        Self {
+            origin: Point::splat(ray.origin),
+            dir: Vector::splat(ray.dir),
+        }
+    }
+}
+
+#[derive(Debug, Copy, Clone)]
+#[cfg(not(feature = "simd-is-enabled"))]
+pub(crate) struct WRay {
+    pub origin: [Point<f32>; SIMD_WIDTH],
+    pub dir: [Vector<f32>; SIMD_WIDTH],
+}
+
+#[derive(Debug, Copy, Clone)]
+#[cfg(feature = "simd-is-enabled")]
 pub(crate) struct WAABB {
     pub mins: Point<SimdFloat>,
     pub maxs: Point<SimdFloat>,
@@ -124,6 +150,42 @@ impl WAABB {
         }
     }
 
+    pub fn intersects_ray(&self, ray: &WRay, max_toi: SimdFloat) -> SimdBool {
+        let _0 = SimdFloat::zero();
+        let _1 = SimdFloat::one();
+        let _infinity = SimdFloat::splat(f32::MAX);
+
+        let mut hit = SimdBool::splat(true);
+        let mut tmin = SimdFloat::zero();
+        let mut tmax = max_toi;
+
+        // TODO: could this be optimized more considering we really just need a boolean answer?
+        for i in 0usize..DIM {
+            let is_not_zero = ray.dir[i].simd_ne(_0);
+            let is_zero_test =
+                (ray.origin[i].simd_ge(self.mins[i]) & ray.origin[i].simd_le(self.maxs[i]));
+            let is_not_zero_test = {
+                let denom = _1 / ray.dir[i];
+                let mut inter_with_near_plane =
+                    ((self.mins[i] - ray.origin[i]) * denom).select(is_not_zero, -_infinity);
+                let mut inter_with_far_plane =
+                    ((self.maxs[i] - ray.origin[i]) * denom).select(is_not_zero, _infinity);
+
+                let gt = inter_with_near_plane.simd_gt(inter_with_far_plane);
+                utils::simd_swap(gt, &mut inter_with_near_plane, &mut inter_with_far_plane);
+
+                tmin = tmin.simd_max(inter_with_near_plane);
+                tmax = tmax.simd_min(inter_with_far_plane);
+
+                tmin.simd_le(tmax)
+            };
+
+            hit = hit & is_not_zero_test.select(is_not_zero, is_zero_test);
+        }
+
+        hit
+    }
+
     #[cfg(feature = "dim2")]
     pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool {
         self.mins.x.simd_le(other.maxs.x)
diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs
new file mode 100644
index 0000000..4e3bf54
--- /dev/null
+++ b/src/geometry/wquadtree.rs
@@ -0,0 +1,285 @@
+use crate::geometry::{ColliderHandle, ColliderSet, Ray, AABB};
+use crate::geometry::{WRay, WAABB};
+use crate::math::{Point, Vector};
+use crate::simd::{SimdFloat, SIMD_WIDTH};
+use ncollide::bounding_volume::BoundingVolume;
+use simba::simd::{SimdBool, SimdValue};
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+struct NodeIndex {
+    index: u32, // Index of the addressed node in the `nodes` array.
+    lane: u8,   // SIMD lane of the addressed node.
+}
+
+impl NodeIndex {
+    fn new(index: u32, lane: u8) -> Self {
+        Self { index, lane }
+    }
+
+    fn invalid() -> Self {
+        Self {
+            index: u32::MAX,
+            lane: 0,
+        }
+    }
+}
+
+#[derive(Copy, Clone, Debug)]
+struct WQuadtreeNodeChildren {
+    waabb: WAABB,
+    // Index of the nodes of the 4 nodes represented by self.
+    // If this is a leaf, it contains the proxy ids instead.
+    children: [u32; 4],
+    parent: NodeIndex,
+    leaf: bool, // TODO: pack this with the NodexIndex.lane?
+}
+
+#[derive(Copy, Clone, Debug, PartialEq, Eq)]
+struct ColliderNodeIndex {
+    node: NodeIndex,
+    handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here?
+}
+
+impl ColliderNodeIndex {
+    fn invalid() -> Self {
+        Self {
+            node: NodeIndex::invalid(),
+            handle: ColliderSet::invalid_handle(),
+        }
+    }
+}
+
+pub struct WQuadtree {
+    nodes: Vec<WQuadtreeNodeChildren>,
+    dirty: Vec<bool>, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism.
+    proxies: Vec<ColliderNodeIndex>,
+}
+
+impl WQuadtree {
+    pub fn new() -> Self {
+        WQuadtree {
+            nodes: Vec::new(),
+            dirty: Vec::new(),
+            proxies: Vec::new(),
+        }
+    }
+
+    pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet) {
+        self.nodes.clear();
+        self.dirty.clear();
+        self.proxies.clear();
+
+        // Create proxies.
+        let mut indices = Vec::with_capacity(colliders.len());
+        self.proxies = vec![ColliderNodeIndex::invalid(); colliders.len()];
+
+        for (handle, collider) in colliders.iter() {
+            let index = handle.into_raw_parts().0;
+            if self.proxies.len() < index {
+                self.proxies.resize(index + 1, ColliderNodeIndex::invalid());
+            }
+
+            self.proxies[index].handle = handle;
+            indices.push(index);
+        }
+
+        // Compute AABBs.
+        let mut aabbs = vec![AABB::new_invalid(); self.proxies.len()];
+        for (handle, collider) in colliders.iter() {
+            let index = handle.into_raw_parts().0;
+            let aabb = collider.compute_aabb();
+            aabbs[index] = aabb;
+        }
+
+        // Build the tree recursively.
+        let root_node = WQuadtreeNodeChildren {
+            waabb: WAABB::new_invalid(),
+            children: [1, u32::MAX, u32::MAX, u32::MAX],
+            parent: NodeIndex::invalid(),
+            leaf: false,
+        };
+
+        self.nodes.push(root_node);
+        let root_id = NodeIndex::new(0, 0);
+        let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id);
+        self.nodes[0].waabb = WAABB::from([
+            aabb,
+            AABB::new_invalid(),
+            AABB::new_invalid(),
+            AABB::new_invalid(),
+        ]);
+    }
+
+    fn do_recurse_build(
+        &mut self,
+        indices: &mut [usize],
+        aabbs: &[AABB],
+        parent: NodeIndex,
+    ) -> (u32, AABB) {
+        // Leaf case.
+        if indices.len() <= 4 {
+            let my_id = self.nodes.len();
+            let mut my_aabb = AABB::new_invalid();
+            let mut leaf_aabbs = [AABB::new_invalid(); 4];
+            let mut proxy_ids = [u32::MAX; 4];
+
+            for (k, id) in indices.iter().enumerate() {
+                my_aabb.merge(&aabbs[*id]);
+                leaf_aabbs[k] = aabbs[*id];
+                proxy_ids[k] = *id as u32;
+            }
+
+            let node = WQuadtreeNodeChildren {
+                waabb: WAABB::from(leaf_aabbs),
+                children: proxy_ids,
+                parent,
+                leaf: true,
+            };
+
+            self.nodes.push(node);
+            return (my_id as u32, my_aabb);
+        }
+
+        // Compute the center and variance along each dimension.
+        // In 3D we compute the variance to not-subdivide the dimension with lowest variance.
+        // Therefore variance computation is not needed in 2D because we only have 2 dimension
+        // to split in the first place.
+        let mut center = Point::origin();
+        #[cfg(feature = "dim3")]
+        let mut variance = Vector::zeros();
+
+        let denom = 1.0 / (indices.len() as f32);
+
+        for i in &*indices {
+            let coords = aabbs[*i].center().coords;
+            center += coords * denom;
+            #[cfg(feature = "dim3")]
+            {
+                variance += coords.component_mul(&coords) * denom;
+            }
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            variance = variance - center.coords.component_mul(&center.coords);
+        }
+
+        // Find the axis with minimum variance. This is the axis along
+        // which we are **not** subdividing our set.
+        let mut subdiv_dims = [0, 1];
+        #[cfg(feature = "dim3")]
+        {
+            let min = variance.imin();
+            subdiv_dims[0] = (min + 1) % 3;
+            subdiv_dims[1] = (min + 2) % 3;
+        }
+
+        // Split the set along the two subdiv_dims dimensions.
+        // TODO: should we split wrt. the median instead of the average?
+        // TODO: we should ensure each subslice contains at least 4 elements each (or less if
+        // indices has less than 16 elements in the first place.
+        let (left, right) = split_indices_wrt_dim(indices, &aabbs, &center, subdiv_dims[0]);
+
+        let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, &center, subdiv_dims[1]);
+        let (right_bottom, right_top) =
+            split_indices_wrt_dim(right, &aabbs, &center, subdiv_dims[1]);
+
+        // println!(
+        //     "Recursing on children: {}, {}, {}, {}",
+        //     left_bottom.len(),
+        //     left_top.len(),
+        //     right_bottom.len(),
+        //     right_top.len()
+        // );
+
+        let node = WQuadtreeNodeChildren {
+            waabb: WAABB::new_invalid(),
+            children: [0; 4], // Will be set after the recursive call
+            parent,
+            leaf: false,
+        };
+
+        let id = self.nodes.len() as u32;
+        self.nodes.push(node);
+
+        // Recurse!
+        let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0));
+        let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1));
+        let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2));
+        let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3));
+
+        // Now we know the indices of the grand-nodes.
+        self.nodes[id as usize].children = [a.0, b.0, c.0, d.0];
+        self.nodes[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]);
+
+        // TODO: will this chain of .merged be properly optimized?
+        let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&d.1);
+        (id, my_aabb)
+    }
+
+    pub fn cast_ray(&self, ray: &Ray, max_toi: f32) -> Vec<ColliderHandle> {
+        let mut res = Vec::new();
+
+        if self.nodes.is_empty() {
+            return res;
+        }
+
+        // Special case for the root.
+        let mut stack = vec![0u32];
+        let wray = WRay::splat(*ray);
+        let wmax_toi = SimdFloat::splat(max_toi);
+        while let Some(inode) = stack.pop() {
+            let node = self.nodes[inode as usize];
+            let hits = node.waabb.intersects_ray(&wray, wmax_toi);
+            let bitmask = hits.bitmask();
+
+            for ii in 0..SIMD_WIDTH {
+                if (bitmask & (1 << ii)) != 0 {
+                    if node.leaf {
+                        // We found a leaf!
+                        // Unfortunately, invalid AABBs return a hit as well.
+                        if let Some(proxy) = self.proxies.get(node.children[ii] as usize) {
+                            res.push(proxy.handle);
+                        }
+                    } else {
+                        // Internal node, visit the child.
+                        // Un fortunately, we have this check because invalid AABBs
+                        // return a hit as well.
+                        if node.children[ii] as usize <= self.nodes.len() {
+                            stack.push(node.children[ii]);
+                        }
+                    }
+                }
+            }
+        }
+
+        res
+    }
+}
+
+fn split_indices_wrt_dim<'a>(
+    indices: &'a mut [usize],
+    aabbs: &[AABB],
+    split_point: &Point<f32>,
+    dim: usize,
+) -> (&'a mut [usize], &'a mut [usize]) {
+    let mut icurr = 0;
+    let mut ilast = indices.len() - 1;
+
+    // The loop condition we can just do 0..indices.len()
+    // instead of the test icurr < ilast because we know
+    // we will iterate exactly once per index.
+    for _ in 0..indices.len() {
+        let i = indices[icurr];
+        let center = aabbs[i].center();
+
+        if center[dim] > split_point[dim] {
+            indices.swap(icurr, ilast);
+            ilast -= 1;
+        } else {
+            icurr += 1;
+        }
+    }
+
+    indices.split_at_mut(icurr)
+}
diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs
index 15caaef..070e996 100644
--- a/src/pipeline/query_pipeline.rs
+++ b/src/pipeline/query_pipeline.rs
@@ -1,231 +1,10 @@
 use crate::dynamics::RigidBodySet;
-use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, AABB, WAABB};
+use crate::geometry::{
+    Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree, AABB, WAABB,
+};
 use crate::math::{Point, Vector};
 use ncollide::bounding_volume::BoundingVolume;
 
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
-struct NodeIndex {
-    index: u32, // Index of the addressed node in the `children` array.
-    lane: u8,   // SIMD lane of the addressed node.
-}
-
-impl NodeIndex {
-    fn new(index: u32, lane: u8) -> Self {
-        Self { index, lane }
-    }
-
-    fn invalid() -> Self {
-        Self {
-            index: u32::MAX,
-            lane: 0,
-        }
-    }
-}
-
-#[derive(Copy, Clone, Debug)]
-struct WAABBHierarchyNodeChildren {
-    waabb: WAABB,
-    // Index of the children of the 4 nodes represented by self.
-    // If this is a leaf, it contains the proxy ids instead.
-    grand_children: [u32; 4],
-    parent: NodeIndex,
-    leaf: bool, // TODO: pack this with the NodexIndex.lane?
-}
-
-#[derive(Copy, Clone, Debug, PartialEq, Eq)]
-struct ColliderNodeIndex {
-    node: NodeIndex,
-    handle: ColliderHandle, // The collider handle. TODO: only set the collider generation here?
-}
-
-impl ColliderNodeIndex {
-    fn invalid() -> Self {
-        Self {
-            node: NodeIndex::invalid(),
-            handle: ColliderSet::invalid_handle(),
-        }
-    }
-}
-
-struct WAABBHierarchy {
-    children: Vec<WAABBHierarchyNodeChildren>,
-    dirty: Vec<bool>, // TODO: use a bitvec/Vob and check it does not break cross-platform determinism.
-    proxies: Vec<ColliderNodeIndex>,
-}
-
-impl WAABBHierarchy {
-    pub fn new() -> Self {
-        WAABBHierarchy {
-            children: Vec::new(),
-            dirty: Vec::new(),
-            proxies: Vec::new(),
-        }
-    }
-
-    pub fn clear_and_rebuild(&mut self, colliders: &ColliderSet) {
-        self.children.clear();
-        self.dirty.clear();
-        self.proxies.clear();
-
-        // Create proxies.
-        let mut indices = Vec::with_capacity(colliders.len());
-        let mut proxies = vec![ColliderNodeIndex::invalid(); colliders.len()];
-        for (handle, collider) in colliders.iter() {
-            let index = handle.into_raw_parts().0;
-            if proxies.len() < handle.into_raw_parts().0 {
-                proxies.resize(index + 1, ColliderNodeIndex::invalid());
-            }
-
-            proxies[index].handle = handle;
-            indices.push(index);
-        }
-
-        // Compute AABBs.
-        let mut aabbs = vec![AABB::new_invalid(); proxies.len()];
-        for (handle, collider) in colliders.iter() {
-            let index = handle.into_raw_parts().0;
-            let aabb = collider.compute_aabb();
-            aabbs[index] = aabb;
-        }
-
-        // Build the tree recursively.
-        let root_node = WAABBHierarchyNodeChildren {
-            waabb: WAABB::new_invalid(),
-            grand_children: [1; 4],
-            parent: NodeIndex::invalid(),
-            leaf: false,
-        };
-
-        self.children.push(root_node);
-        let root_id = NodeIndex::new(0, 0);
-        let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id);
-        self.children[0].waabb = WAABB::splat(aabb);
-    }
-
-    fn do_recurse_build(
-        &mut self,
-        indices: &mut [usize],
-        aabbs: &[AABB],
-        parent: NodeIndex,
-    ) -> (u32, AABB) {
-        // Leaf case.
-        if indices.len() <= 4 {
-            let my_id = self.children.len();
-            let mut my_aabb = AABB::new_invalid();
-            let mut leaf_aabbs = [AABB::new_invalid(); 4];
-            let mut proxy_ids = [u32::MAX; 4];
-
-            for (k, id) in indices.iter().enumerate() {
-                my_aabb.merge(&aabbs[*id]);
-                leaf_aabbs[k] = aabbs[*id];
-                proxy_ids[k] = *id as u32;
-            }
-
-            let node = WAABBHierarchyNodeChildren {
-                waabb: WAABB::from(leaf_aabbs),
-                grand_children: proxy_ids,
-                parent,
-                leaf: true,
-            };
-
-            self.children.push(node);
-            return (my_id as u32, my_aabb);
-        }
-
-        // Compute the center and variance along each dimension.
-        // In 3D we compute the variance to not-subdivide the dimension with lowest variance.
-        // Therefore variance computation is not needed in 2D because we only have 2 dimension
-        // to split in the first place.
-        let mut center = Point::origin();
-        #[cfg(feature = "dim3")]
-        let mut variance = Vector::zeros();
-
-        let denom = 1.0 / (indices.len() as f32);
-
-        for i in &*indices {
-            let coords = aabbs[*i].center().coords;
-            center += coords * denom;
-            #[cfg(feature = "dim3")]
-            {
-                variance += coords.component_mul(&coords) * denom;
-            }
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            variance = variance - center.coords.component_mul(&center.coords);
-        }
-
-        // Find the axis with minimum variance. This is the axis along
-        // which we are **not** subdividing our set.
-        let mut subdiv_dims = [0, 1];
-        #[cfg(feature = "dim3")]
-        {
-            let min = variance.imin();
-            subdiv_dims[0] = (min + 1) % 3;
-            subdiv_dims[1] = (min + 2) % 3;
-        }
-
-        // Split the set along the two subdiv_dims dimensions.
-        // TODO: should we split wrt. the median instead of the average?
-        // TODO: we should ensure each subslice contains at least 4 elements each (or less if
-        // indices has less than 16 elements in the first place.
-        let (left, right) = split_indices_wrt_dim(indices, &aabbs, subdiv_dims[0]);
-        let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, subdiv_dims[1]);
-        let (right_bottom, right_top) = split_indices_wrt_dim(right, &aabbs, subdiv_dims[1]);
-
-        let node = WAABBHierarchyNodeChildren {
-            waabb: WAABB::new_invalid(),
-            grand_children: [0; 4], // Will be set after the recursive call
-            parent,
-            leaf: false,
-        };
-
-        let id = self.children.len() as u32;
-        self.children.push(node);
-
-        // Recurse!
-        let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0));
-        let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1));
-        let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2));
-        let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3));
-
-        // Now we know the indices of the grand-children.
-        self.children[id as usize].grand_children = [a.0, b.0, c.0, d.0];
-        self.children[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]);
-
-        // TODO: will this chain of .merged be properly optimized?
-        let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&c.1);
-        (id, my_aabb)
-    }
-}
-
-fn split_indices_wrt_dim<'a>(
-    indices: &'a mut [usize],
-    aabbs: &[AABB],
-    dim: usize,
-) -> (&'a mut [usize], &'a mut [usize]) {
-    let mut icurr = 0;
-    let mut ilast = indices.len() - 1;
-
-    // The loop condition we can just do 0..indices.len()
-    // instead of the test icurr < ilast because we know
-    // we will iterate exactly once per index.
-    for _ in 0..indices.len() {
-        let i = indices[icurr];
-        let center = aabbs[i].center();
-
-        if center[dim] > center[dim] {
-            indices.swap(icurr, ilast);
-            ilast -= 1;
-        } else {
-            icurr += 1;
-        }
-    }
-
-    indices.split_at_mut(icurr)
-}
-
 /// A pipeline for performing queries on all the colliders of a scene.
 pub struct QueryPipeline {
     // hierarchy: WAABBHierarchy,
@@ -261,11 +40,24 @@ impl QueryPipeline {
         ray: &Ray,
         max_toi: f32,
     ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
+        let t0 = instant::now();
+        let mut tree = WQuadtree::new();
+        tree.clear_and_rebuild(colliders);
+        println!("Built quadtree in time: {}", instant::now() - t0);
+        let t0 = instant::now();
+        let inter = tree.cast_ray(ray, max_toi);
+        println!(
+            "Found {} interefrences in time {}.",
+            inter.len(),
+            instant::now() - t0
+        );
+
+        let t0 = instant::now();
         let mut best = f32::MAX;
         let mut result = None;
 
-        // FIXME: this is a brute-force approach.
-        for (handle, collider) in colliders.iter() {
+        for handle in inter {
+            let collider = &colliders[handle];
             if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
                 if inter.toi < best {
                     best = inter.toi;
@@ -273,6 +65,7 @@ impl QueryPipeline {
                 }
             }
         }
+        println!("Cast time: {}", instant::now() - t0);
 
         result
     }
diff --git a/src/utils.rs b/src/utils.rs
index 91ce41c..c7cb908 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -11,7 +11,11 @@ use simba::simd::SimdValue;
 use std::collections::HashMap;
 use std::ops::{Add, Mul};
 #[cfg(feature = "simd-is-enabled")]
-use {crate::simd::SimdFloat, na::SimdPartialOrd, num::One};
+use {
+    crate::simd::{SimdBool, SimdFloat},
+    na::SimdPartialOrd,
+    num::One,
+};
 
 // pub(crate) const SIN_10_DEGREES: f32 = 0.17364817766;
 // pub(crate) const COS_10_DEGREES: f32 = 0.98480775301;
@@ -31,6 +35,17 @@ pub(crate) fn inv(val: f32) -> f32 {
     }
 }
 
+/// Conditionally swaps each lanes of `a` with those of `b`.
+///
+/// For each `i in [0..SIMD_WIDTH[`, if `do_swap.extract(i)` is `true` then
+/// `a.extract(i)` is swapped with `b.extract(i)`.
+#[cfg(feature = "simd-is-enabled")]
+pub fn simd_swap(do_swap: SimdBool, a: &mut SimdFloat, b: &mut SimdFloat) {
+    let _a = *a;
+    *a = b.select(do_swap, *a);
+    *b = _a.select(do_swap, *b);
+}
+
 /// Trait to copy the sign of each component of one scalar/vector/matrix to another.
 pub trait WSign<Rhs>: Sized {
     // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652