From 422c463f51a1884fb3247447c74b66424b5ec539 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:26 +0200 Subject: [PATCH 01/12] feat: incremental and parallel BVH updates --- CHANGELOG.md | 8 + src/partitioning/bvh/bvh_binned_build.rs | 1 + src/partitioning/bvh/bvh_insert.rs | 184 ++++++++- src/partitioning/bvh/bvh_refit.rs | 430 +++++++++++++++++++++- src/partitioning/bvh/bvh_tests.rs | 68 ++++ src/partitioning/bvh/bvh_traverse_bvtt.rs | 242 +++++++++++- src/partitioning/bvh/bvh_tree.rs | 80 +++- src/partitioning/bvh/mod.rs | 1 + src/partitioning/mod.rs | 4 +- 9 files changed, 960 insertions(+), 58 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 95ccc406..123366f0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,11 @@ +## Unreleased + +### Added + +- `Bvh` gains incremental and parallel update APIs: `refit_partial`, flag-preserving + `refit_without_resolve` variants, `refit_parallel`, parallel BVTT traversal, and batched + parallel leaf updates. + ## 0.29.0 ### Breaking changes diff --git a/src/partitioning/bvh/bvh_binned_build.rs b/src/partitioning/bvh/bvh_binned_build.rs index e20c3eca..eb5812d5 100644 --- a/src/partitioning/bvh/bvh_binned_build.rs +++ b/src/partitioning/bvh/bvh_binned_build.rs @@ -27,6 +27,7 @@ impl Bvh { self.nodes.clear(); self.parents.clear(); + self.free_wide_nodes.clear(); self.nodes.push(BvhNodeWide::zeros()); self.parents.push(BvhNodeIndex::default()); diff --git a/src/partitioning/bvh/bvh_insert.rs b/src/partitioning/bvh/bvh_insert.rs index 97188643..3e8dc6b9 100644 --- a/src/partitioning/bvh/bvh_insert.rs +++ b/src/partitioning/bvh/bvh_insert.rs @@ -1,9 +1,26 @@ -use super::bvh_tree::{BvhNodeIndex, BvhNodeWide}; use super::BvhNode; +use super::bvh_tree::{BvhNodeIndex, BvhNodeWide}; use crate::bounding_volume::{Aabb, BoundingVolume}; use crate::math::{Real, Vector}; use crate::partitioning::Bvh; use alloc::vec; +#[cfg(feature = "parallel")] +use alloc::vec::Vec; + +/// Result of a leaf update through [`Bvh::insert_or_update_partially`] or +/// [`Bvh::insert_with_change_detection`]. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub enum BvhLeafUpdateStatus { + /// The leaf already existed and its stored (fattened) AABB still contains the new + /// AABB: the tree was left completely untouched. + Unchanged, + /// The leaf already existed and its stored AABB was rewritten in place (with + /// [`Bvh::insert_or_update_partially`], its ancestors might no longer enclose it + /// until the next refit). + UpdatedInPlace, + /// The leaf didn't exist yet and was inserted (the tree topology changed). + Inserted, +} impl Bvh { /// Inserts a new leaf into the BVH or updates an existing one. @@ -124,7 +141,7 @@ impl Bvh { /// [`refit`]: Self::refit /// [`optimize_incremental`]: Self::optimize_incremental pub fn insert(&mut self, aabb: Aabb, leaf_index: u32) { - self.insert_with_change_detection(aabb, leaf_index, 0.0) + let _ = self.insert_with_change_detection(aabb, leaf_index, 0.0); } /// Inserts a leaf into this BVH, or updates it if already exists. @@ -137,7 +154,7 @@ impl Bvh { aabb: Aabb, leaf_index: u32, change_detection_margin: Real, - ) { + ) -> BvhLeafUpdateStatus { if let Some(leaf) = self.leaf_node_indices.get(leaf_index as usize) { let node = &mut self.nodes[*leaf]; @@ -148,7 +165,7 @@ impl Bvh { node.data.set_change_pending(); } else { // No change detected, no propagation needed. - return; + return BvhLeafUpdateStatus::Unchanged; } } else { node.mins = aabb.mins; @@ -170,7 +187,7 @@ impl Bvh { let wide_node_id = leaf.decompose().0; if wide_node_id == 0 { // Already at the root, no propagation possible. - return; + return BvhLeafUpdateStatus::UpdatedInPlace; } let mut parent = self.parents[wide_node_id]; @@ -191,8 +208,11 @@ impl Bvh { parent = self.parents[wide_node_id]; } + + BvhLeafUpdateStatus::UpdatedInPlace } else { self.insert_new_unchecked(aabb, leaf_index); + BvhLeafUpdateStatus::Inserted } } @@ -211,7 +231,7 @@ impl Bvh { aabb: Aabb, leaf_index: u32, change_detection_margin: Real, - ) { + ) -> BvhLeafUpdateStatus { if let Some(leaf) = self.leaf_node_indices.get(leaf_index as usize) { let node = &mut self.nodes[*leaf]; @@ -220,13 +240,99 @@ impl Bvh { node.mins = aabb.mins - Vector::splat(change_detection_margin); node.maxs = aabb.maxs + Vector::splat(change_detection_margin); node.data.set_change_pending(); + BvhLeafUpdateStatus::UpdatedInPlace + } else { + // The new AABB is still inside the leaf's fat AABB: the tree + // is left untouched. + BvhLeafUpdateStatus::Unchanged } } else { node.mins = aabb.mins; node.maxs = aabb.maxs; + BvhLeafUpdateStatus::UpdatedInPlace } } else { self.insert_new_unchecked(aabb, leaf_index); + BvhLeafUpdateStatus::Inserted + } + } + + /// Batch, parallel version of [`Self::insert_or_update_partially`]. + /// + /// Applies every update whose leaf already exists in parallel (existing + /// leaves are updated in place: distinct leaf indices map to distinct + /// [`BvhNode`]s, so the writes are disjoint — two sibling leaves share a + /// wide node but occupy its two disjoint halves), then inserts the new + /// leaves sequentially. `statuses` is filled with one entry per update, in + /// order. + /// + /// Like [`Self::insert_or_update_partially`], the ascendants of updated + /// leaves are **not** updated: the tree must be refitted (e.g. + /// [`Bvh::refit`]) before the next query for its results to be exact. + /// + /// # Panics + /// + /// May panic (or leave arbitrary leaf AABBs, but no memory unsafety beyond + /// a torn AABB) if the same leaf index appears twice in `updates`. + #[cfg(feature = "parallel")] + pub fn insert_or_update_batch_partially_parallel( + &mut self, + updates: &[(Aabb, u32, Real)], + statuses: &mut Vec, + ) { + use rayon::prelude::*; + + statuses.clear(); + statuses.resize(updates.len(), BvhLeafUpdateStatus::Unchanged); + + // Phase 1 (parallel): in-place updates of the existing leaves. + struct NodesPtr(*mut BvhNodeWide); + unsafe impl Sync for NodesPtr {} + let nodes = &NodesPtr(self.nodes.0.as_mut_ptr()); + let leaf_node_indices = &self.leaf_node_indices; + + updates.par_iter().zip(statuses.par_iter_mut()).for_each( + move |((aabb, leaf_index, change_detection_margin), status)| { + let Some(leaf) = leaf_node_indices.get(*leaf_index as usize) else { + // Structural change: deferred to the sequential phase below. + *status = BvhLeafUpdateStatus::Inserted; + return; + }; + let (wide_id, is_right) = leaf.decompose(); + // SAFETY: distinct leaf indices map to distinct (wide, side) + // slots, i.e. disjoint `BvhNode`s; `leaf_node_indices` + // is only read during this phase. + let wide = unsafe { &mut *nodes.0.add(wide_id) }; + let node = if is_right { + &mut wide.right + } else { + &mut wide.left + }; + + if *change_detection_margin > 0.0 { + if !node.contains_aabb(aabb) { + node.mins = aabb.mins - Vector::splat(*change_detection_margin); + node.maxs = aabb.maxs + Vector::splat(*change_detection_margin); + node.data.set_change_pending(); + *status = BvhLeafUpdateStatus::UpdatedInPlace; + } else { + // The new AABB is still inside the leaf's fat AABB: the + // tree is left untouched. + *status = BvhLeafUpdateStatus::Unchanged; + } + } else { + node.mins = aabb.mins; + node.maxs = aabb.maxs; + *status = BvhLeafUpdateStatus::UpdatedInPlace; + } + }, + ); + + // Phase 2 (sequential): structural insertions. + for (update, status) in updates.iter().zip(statuses.iter()) { + if *status == BvhLeafUpdateStatus::Inserted { + self.insert_new_unchecked(update.0, update.1); + } } } @@ -302,13 +408,11 @@ impl Bvh { // We create a new wide leaf containing the current and new leaves and // attach it to `left`. if left.is_leaf() { - let new_leaf_id = self.nodes.len(); let wide_node = BvhNodeWide { left: *left, right: BvhNode::leaf(aabb, leaf_index), }; - self.nodes.push(wide_node); - self.parents.push(BvhNodeIndex::left(curr_id)); + let new_leaf_id = self.alloc_wide_node(wide_node, BvhNodeIndex::left(curr_id)); let left = &mut self.nodes[curr_id as usize].left; self.leaf_node_indices[left.children as usize] = @@ -333,13 +437,11 @@ impl Bvh { // We create a new wide leaf containing the current and new leaves and // attach it to `right`. if right.is_leaf() { - let new_leaf_id = self.nodes.len(); let new_node = BvhNodeWide { left: BvhNode::leaf(aabb, leaf_index), right: *right, }; - self.nodes.push(new_node); - self.parents.push(BvhNodeIndex::right(curr_id)); + let new_leaf_id = self.alloc_wide_node(new_node, BvhNodeIndex::right(curr_id)); let right = &mut self.nodes[curr_id as usize].right; self.leaf_node_indices[leaf_index as usize] = @@ -369,6 +471,64 @@ impl Bvh { } } + /// Allocates a slot for a new wide node, preferring slots orphaned by earlier + /// leaf removals over growing the node array. + fn alloc_wide_node(&mut self, node: BvhNodeWide, parent: BvhNodeIndex) -> usize { + if let Some(free) = self.free_wide_nodes.pop() { + self.nodes[free as usize] = node; + self.parents[free as usize] = parent; + free as usize + } else { + let id = self.nodes.len(); + self.nodes.push(node); + self.parents.push(parent); + id + } + } + + /// Updates the leaf AABB like [`Self::insert_with_change_detection`], but + /// relocates the leaf through a removal + SAH re-insertion whenever its fattened + /// AABB must actually change. + /// + /// Unlike the in-place update of [`Self::insert_with_change_detection`] (which + /// keeps the leaf's tree position and only enlarges its ancestors), the + /// re-insertion picks a fresh position by SAH descent — including rotations — + /// so a stream of such updates keeps the tree quality high on its own, without + /// requiring periodic [`Self::optimize_incremental`] passes. The freed wide-node + /// slot is recycled by the re-insertion itself, so the node array doesn't grow. + /// + /// This is the preferred update path when only a small fraction of the leaves + /// move (the per-leaf cost is an O(log n) descent instead of O(1)); for bulk + /// updates, prefer in-place updates followed by a refit and periodic + /// optimization. + /// + /// Compatible with [`Self::refit_partial`] under the same rules as insertions. + pub fn reinsert_or_update_with_change_detection( + &mut self, + aabb: Aabb, + leaf_index: u32, + change_detection_margin: Real, + ) -> BvhLeafUpdateStatus { + if let Some(leaf) = self.leaf_node_indices.get(leaf_index as usize) { + if self.nodes[*leaf].contains_aabb(&aabb) { + return BvhLeafUpdateStatus::Unchanged; + } + + self.remove(leaf_index); + let fat_aabb = Aabb { + mins: aabb.mins - Vector::splat(change_detection_margin), + maxs: aabb.maxs + Vector::splat(change_detection_margin), + }; + // The new leaf is created with a pending change flag, exactly like an + // in-place update that escaped its previous fattened AABB. + self.insert_new_unchecked(fat_aabb, leaf_index); + BvhLeafUpdateStatus::UpdatedInPlace + } else { + self.insert_new_unchecked(aabb, leaf_index); + BvhLeafUpdateStatus::Inserted + } + } + // Applies a tree rotation at the given `node` if this improves the SAH metric at that node. fn maybe_apply_rotation(&mut self, node_id: u32) { let node = self.nodes[node_id as usize]; diff --git a/src/partitioning/bvh/bvh_refit.rs b/src/partitioning/bvh/bvh_refit.rs index f1b302d5..1ec3d2af 100644 --- a/src/partitioning/bvh/bvh_refit.rs +++ b/src/partitioning/bvh/bvh_refit.rs @@ -3,6 +3,22 @@ use super::{Bvh, BvhNode, BvhWorkspace}; use crate::utils::VecMap; use alloc::vec::Vec; +/// Raw pointers to the refit buffers, shared across parallel refit tasks. +/// +/// Safety: tasks write disjoint index ranges (see `refit_recurse_parallel`). +#[cfg(feature = "parallel")] +#[derive(Copy, Clone)] +struct RefitPtrs { + target: *mut BvhNodeVec, + leaf_data: *mut VecMap, + parents: *mut Vec, +} + +#[cfg(feature = "parallel")] +unsafe impl Send for RefitPtrs {} +#[cfg(feature = "parallel")] +unsafe impl Sync for RefitPtrs {} + impl Bvh { /// Updates the BVH's internal node AABBs after leaf changes. /// @@ -168,7 +184,27 @@ impl Bvh { /// [`remove`]: Bvh::remove /// [`optimize_incremental`]: Bvh::optimize_incremental pub fn refit(&mut self, workspace: &mut BvhWorkspace) { - Self::refit_buffers( + Self::refit_buffers::( + &mut self.nodes, + &mut workspace.refit_tmp, + &mut self.leaf_node_indices, + &mut self.parents, + ); + + // Swap the old nodes with the refitted ones. + core::mem::swap(&mut self.nodes, &mut workspace.refit_tmp); + // The refit rebuilt the node array in depth-first order, dropping the + // orphaned slots the free list pointed to. + self.free_wide_nodes.clear(); + } + + /// Same as [`Self::refit`], but processes independent subtrees in parallel. + /// + /// The result is identical to [`Self::refit`] (same node layout, same flags); + /// only the work distribution differs. + #[cfg(feature = "parallel")] + pub fn refit_parallel(&mut self, workspace: &mut BvhWorkspace) { + Self::refit_buffers_parallel::( &mut self.nodes, &mut workspace.refit_tmp, &mut self.leaf_node_indices, @@ -177,9 +213,254 @@ impl Bvh { // Swap the old nodes with the refitted ones. core::mem::swap(&mut self.nodes, &mut workspace.refit_tmp); + // The refit rebuilt the node array in depth-first order, dropping the + // orphaned slots the free list pointed to. + self.free_wide_nodes.clear(); } - pub(super) fn refit_buffers( + #[cfg(feature = "parallel")] + fn refit_buffers_parallel( + source: &mut BvhNodeVec, + target: &mut BvhNodeVec, + leaf_data: &mut VecMap, + parents: &mut Vec, + ) { + // Subtrees below this leaf count are refitted sequentially. + const SEQ_LEAF_THRESHOLD: u32 = 2048; + // Bounds the number of spawned tasks to 2^MAX_SPLIT_DEPTH. + const MAX_SPLIT_DEPTH: u32 = 6; + + if source.is_empty() || source[0].leaf_count() <= SEQ_LEAF_THRESHOLD.max(2) { + return Self::refit_buffers::(source, target, leaf_data, parents); + } + + target.resize( + source.len(), + BvhNodeWide { + left: BvhNode::zeros(), + right: BvhNode::zeros(), + }, + ); + parents.resize(source.len(), BvhNodeIndex::default()); + + let ptrs = RefitPtrs { + target: target as *mut BvhNodeVec, + leaf_data: leaf_data as *mut VecMap, + parents: parents as *mut Vec, + }; + + // Mirror of `refit_buffers`' root special case, with the sequential target-id + // counter replaced by offsets computed from the subtree leaf counts: the + // depth-first layout of a subtree with `n` leaves spans exactly `n - 1` wide + // nodes. + let root = source[0]; + let left_size = if root.left.is_leaf() { + 0 + } else { + root.left.leaf_count() - 1 + }; + let right_size = if root.right.is_leaf() { + 0 + } else { + root.right.leaf_count() - 1 + }; + let final_len = (1 + left_size + right_size) as usize; + + let _ = rayon::join( + || { + // SAFETY: this task writes only to target slots [1, 1 + left_size), + // entry target[0].left, and the leaf data of leaves of the + // left subtree — all disjoint from the other task. + let target = unsafe { &mut *ptrs.target }; + if !root.left.is_leaf() { + Self::refit_recurse_parallel::( + source, + ptrs, + root.left.children, + 1, + BvhNodeIndex::left(0), + MAX_SPLIT_DEPTH, + SEQ_LEAF_THRESHOLD, + ); + } else { + target[0].left = root.left; + if RESOLVE { + target[0].left.data.resolve_pending_change(); + } + } + }, + || { + // SAFETY: see the other task; slots [1 + left_size, final_len) and + // entry target[0].right. + let target = unsafe { &mut *ptrs.target }; + if !root.right.is_leaf() { + Self::refit_recurse_parallel::( + source, + ptrs, + root.right.children, + 1 + left_size, + BvhNodeIndex::right(0), + MAX_SPLIT_DEPTH, + SEQ_LEAF_THRESHOLD, + ); + } else { + target[0].right = root.right; + if RESOLVE { + target[0].right.data.resolve_pending_change(); + } + } + }, + ); + + source.truncate(final_len); + target.truncate(final_len); + parents.truncate(final_len); + } + + /// Recursive parallel counterpart of `refit_recurse`. + /// + /// `target_id` is the depth-first slot this subtree's root occupies (its left + /// child subtree starts at `target_id + 1`, its right child subtree right after + /// the left one, whose extent is known from its leaf count). + #[cfg(feature = "parallel")] + fn refit_recurse_parallel( + source: &BvhNodeVec, + ptrs: RefitPtrs, + source_id: u32, + target_id: u32, + parent: BvhNodeIndex, + depth: u32, + seq_leaf_threshold: u32, + ) { + let node = source[source_id as usize]; + let leaf_count = node.left.leaf_count() + node.right.leaf_count(); + + if depth == 0 || leaf_count <= seq_leaf_threshold { + // + + // SAFETY: the sequential refit of this subtree only touches the target + // slots [target_id, target_id + leaf_count - 1), its parent entry, + // and its own leaves' data: all disjoint from the other tasks. + let target = unsafe { &mut *ptrs.target }; + let leaf_data = unsafe { &mut *ptrs.leaf_data }; + let parents = unsafe { &mut *ptrs.parents }; + let mut counter = target_id; + Self::refit_recurse::( + source, + target, + leaf_data, + parents, + source_id, + &mut counter, + parent, + ); + debug_assert_eq!(counter, target_id + leaf_count - 1); + return; + } + + let left_size = if node.left.is_leaf() { + 0 + } else { + node.left.leaf_count() - 1 + }; + + let _ = rayon::join( + || { + let target = unsafe { &mut *ptrs.target }; + let leaf_data = unsafe { &mut *ptrs.leaf_data }; + if !node.left.is_leaf() { + Self::refit_recurse_parallel::( + source, + ptrs, + node.left.children, + target_id + 1, + BvhNodeIndex::left(target_id), + depth - 1, + seq_leaf_threshold, + ); + } else { + target[target_id as usize].left = node.left; + if RESOLVE { + target[target_id as usize] + .left + .data + .resolve_pending_change(); + } + leaf_data[node.left.children as usize] = BvhNodeIndex::left(target_id); + } + }, + || { + let target = unsafe { &mut *ptrs.target }; + let leaf_data = unsafe { &mut *ptrs.leaf_data }; + if !node.right.is_leaf() { + Self::refit_recurse_parallel::( + source, + ptrs, + node.right.children, + target_id + 1 + left_size, + BvhNodeIndex::right(target_id), + depth - 1, + seq_leaf_threshold, + ); + } else { + target[target_id as usize].right = node.right; + if RESOLVE { + target[target_id as usize] + .right + .data + .resolve_pending_change(); + } + leaf_data[node.right.children as usize] = BvhNodeIndex::right(target_id); + } + }, + ); + + // Both children of this wide node are now written: compute the summary entry + // in the parent, like the tail of `refit_recurse`. + let target = unsafe { &mut *ptrs.target }; + let parents = unsafe { &mut *ptrs.parents }; + let merged = target[target_id as usize] + .left + .merged(&target[target_id as usize].right, target_id); + target[parent] = merged; + parents[target_id as usize] = parent; + } + + /// Same as [`Self::refit`], but leaves every change-detection flag untouched. + /// + /// Use this to make the tree valid for queries after a batch of + /// [`Self::insert_or_update_partially`] without consuming the pending change + /// flags: a later flag-resolving [`Self::refit`] (or + /// [`Self::refit_partial`]) will promote them as if this call never happened. + pub fn refit_without_resolve(&mut self, workspace: &mut BvhWorkspace) { + Self::refit_buffers::( + &mut self.nodes, + &mut workspace.refit_tmp, + &mut self.leaf_node_indices, + &mut self.parents, + ); + core::mem::swap(&mut self.nodes, &mut workspace.refit_tmp); + // The refit rebuilt the node array in depth-first order, dropping the + // orphaned slots the free list pointed to. + self.free_wide_nodes.clear(); + } + + /// Parallel version of [`Self::refit_without_resolve`]. + #[cfg(feature = "parallel")] + pub fn refit_without_resolve_parallel(&mut self, workspace: &mut BvhWorkspace) { + Self::refit_buffers_parallel::( + &mut self.nodes, + &mut workspace.refit_tmp, + &mut self.leaf_node_indices, + &mut self.parents, + ); + core::mem::swap(&mut self.nodes, &mut workspace.refit_tmp); + // The refit rebuilt the node array in depth-first order, dropping the + // orphaned slots the free list pointed to. + self.free_wide_nodes.clear(); + } + + pub(super) fn refit_buffers( source: &mut BvhNodeVec, target: &mut BvhNodeVec, leaf_data: &mut VecMap, @@ -193,9 +474,11 @@ impl Bvh { target.clear(); parents.clear(); target.push(source[0]); - target[0].left.data.resolve_pending_change(); - if target[0].right.leaf_count() > 0 { - target[0].right.data.resolve_pending_change(); + if RESOLVE { + target[0].left.data.resolve_pending_change(); + if target[0].right.leaf_count() > 0 { + target[0].right.data.resolve_pending_change(); + } } parents.push(BvhNodeIndex::default()); } else if !source.is_empty() && source[0].leaf_count() > 2 { @@ -214,7 +497,7 @@ impl Bvh { let right_child_id = source[0].right.children; if !source[0].left.is_leaf() { - Self::refit_recurse( + Self::refit_recurse::( source, target, leaf_data, @@ -225,7 +508,9 @@ impl Bvh { ); } else { target[0].left = source[0].left; - target[0].left.data.resolve_pending_change(); + if RESOLVE { + target[0].left.data.resolve_pending_change(); + } // NOTE: updating the leaf_data shouldn’t be needed here since the root // is always at 0. @@ -233,7 +518,7 @@ impl Bvh { } if !source[0].right.is_leaf() { - Self::refit_recurse( + Self::refit_recurse::( source, target, leaf_data, @@ -244,7 +529,9 @@ impl Bvh { ); } else { target[0].right = source[0].right; - target[0].right.data.resolve_pending_change(); + if RESOLVE { + target[0].right.data.resolve_pending_change(); + } // NOTE: updating the leaf_data shouldn’t be needed here since the root // is always at 0. // *self.leaf_data.get_mut_unknown_gen(right_child_id).unwrap() = BvhNodeIndex::right(0); @@ -256,7 +543,7 @@ impl Bvh { } } - fn refit_recurse( + fn refit_recurse( source: &BvhNodeVec, target: &mut BvhNodeVec, leaf_data: &mut VecMap, @@ -275,7 +562,7 @@ impl Bvh { let right_source_id = node.right.children; if !left_is_leaf { - Self::refit_recurse( + Self::refit_recurse::( source, target, leaf_data, @@ -287,15 +574,17 @@ impl Bvh { } else { let node = &source[source_id as usize]; target[target_id as usize].left = node.left; - target[target_id as usize] - .left - .data - .resolve_pending_change(); + if RESOLVE { + target[target_id as usize] + .left + .data + .resolve_pending_change(); + } leaf_data[node.left.children as usize] = BvhNodeIndex::left(target_id); } if !right_is_leaf { - Self::refit_recurse( + Self::refit_recurse::( source, target, leaf_data, @@ -307,10 +596,12 @@ impl Bvh { } else { let node = &source[source_id as usize]; target[target_id as usize].right = node.right; - target[target_id as usize] - .right - .data - .resolve_pending_change(); + if RESOLVE { + target[target_id as usize] + .right + .data + .resolve_pending_change(); + } leaf_data[node.right.children as usize] = BvhNodeIndex::right(target_id); } @@ -319,6 +610,105 @@ impl Bvh { parents[target_id as usize] = parent; } + /// Incrementally refits the tree after a small number of leaf updates or + /// insertions. + /// + /// This is a faster alternative to [`Self::refit`] valid only if, since the last + /// refit, the only tree modifications were calls to + /// [`Self::insert_or_update_partially`], [`Self::insert`], + /// [`Self::insert_with_change_detection`], or + /// [`Self::reinsert_or_update_with_change_detection`] (in-place updates, + /// insertions, or removal-based relocations of leaves). In particular, no leaf + /// was removed without being re-inserted in the same batch, and no optimization + /// ran. Otherwise, call [`Self::refit`] instead. + /// + /// Insertions are safe here because `insert_new_unchecked` keeps the tree + /// geometrically valid on its own (it enlarges the ancestor AABBs and increments + /// their leaf counts during its descent) and creates the new leaf with a pending + /// change flag. Any transient change-flag state it leaves behind (the pending + /// flag inherited by the wide node that used to hold the insertion sibling, the + /// raw-merged flags written by its SAH rotations) lies on the inserted leaf's + /// ancestor path, which the walk below rewrites all the way to the root — see + /// [`Self::refit_path`]. + /// + /// [`BvhLeafUpdateStatus::Inserted`]: super::BvhLeafUpdateStatus::Inserted + /// + /// `previously_changed` must contain (a superset of) the leaves whose change flag + /// was set by the previous refit: their change flag gets cleared. `newly_changed` + /// must contain (a superset of) the leaves updated in-place or inserted since the + /// last refit: their change flag gets set if their fat AABB actually changed + /// (inserted leaves always count as changed). + /// + /// Unlike [`Self::refit`], this runs in `O(changed * tree_height)` instead of + /// `O(node_count)`, but doesn't reorder nodes in memory. + pub fn refit_partial(&mut self, previously_changed: &[u32], newly_changed: &[u32]) { + // First resolve the change flags of every impacted leaf, and only then walk + // their ancestor paths. Walking while some leaves still hold an unresolved + // pending flag would propagate that transient state into internal nodes + // (a pending internal node reads as "unchanged" during traversals). + for leaf in previously_changed { + let Some(leaf_node_id) = self.leaf_node_indices.get(*leaf as usize).copied() else { + continue; + }; + let data = &mut self.nodes[leaf_node_id].data; + + // Clear the flag of leaves that were changed at the previous refit and + // didn't move since. Leaves that moved again (pending) are promoted by + // the next loop instead. + if !data.is_change_pending() && data.is_changed() { + data.resolve_pending_change(); + } + } + + for leaf in newly_changed { + let Some(leaf_node_id) = self.leaf_node_indices.get(*leaf as usize).copied() else { + continue; + }; + let data = &mut self.nodes[leaf_node_id].data; + + // Promote pending leaves to CHANGED. Leaves whose update stayed within + // the change-detection margin have no flag to update. + if data.is_change_pending() { + data.resolve_pending_change(); + } + } + + for leaf in previously_changed.iter().chain(newly_changed) { + let Some(leaf_node_id) = self.leaf_node_indices.get(*leaf as usize).copied() else { + continue; + }; + self.refit_path(leaf_node_id); + } + } + + /// Propagates AABB and change-flag updates from the given node up to the root. + /// + /// The walk deliberately does NOT stop early when an ancestor's recomputed value + /// matches its stored one: leaf insertions can leave transient change-flag state + /// (pending flags inherited by the wide node that used to hold the insertion + /// sibling, raw-merged flags written by the insertion's SAH rotations) anywhere + /// on the inserted leaf's ancestor path. Walking the whole path rewrites every + /// ancestor with an exact, normalized recomputation, which is what keeps the + /// internal change flags exactly equal to the OR of their descendant leaves' + /// flags. An early-out could strand a stale pending flag above the break point, + /// and a pending internal node reads as "unchanged" during change-detection + /// traversals — hiding every changed leaf underneath. The full walk costs + /// O(tree height) per changed leaf, which is fine in the small-change regime + /// partial refits are meant for. + fn refit_path(&mut self, node: BvhNodeIndex) { + let (mut wide_id, _) = node.decompose(); + + while wide_id != 0 { + let parent = self.parents[wide_id]; + let wide = &self.nodes[wide_id]; + let mut recomputed = wide.left.merged(&wide.right, wide_id as u32); + recomputed.data.normalize_change_flag(); + + self.nodes[parent] = recomputed; + (wide_id, _) = parent.decompose(); + } + } + /// Similar to [`Self::refit`] but without any optimization of the internal node storage layout. /// /// This can be faster than [`Self::refit`] but doesn’t reorder node to be more cache-efficient diff --git a/src/partitioning/bvh/bvh_tests.rs b/src/partitioning/bvh/bvh_tests.rs index 404990bd..1b7c233e 100644 --- a/src/partitioning/bvh/bvh_tests.rs +++ b/src/partitioning/bvh/bvh_tests.rs @@ -279,3 +279,71 @@ fn bvh_remove_to_partial_root_then_optimize() { assert_eq!(bvh.parents.len(), 1); bvh.assert_well_formed(); } + +#[cfg(feature = "parallel")] +#[cfg(all(feature = "dim3", feature = "f32"))] +mod parallel_batch_update { + use crate::bounding_volume::Aabb; + use crate::math::Vector; + use crate::partitioning::{Bvh, BvhLeafUpdateStatus, BvhWorkspace}; + use alloc::vec::Vec; + + /// The parallel batch leaf update must behave exactly like the sequential + /// per-leaf `insert_or_update_partially` calls (statuses and final tree). + #[test] + fn batch_partial_update_matches_sequential() { + let aabb = |i: usize, offset: f32| { + Aabb::new( + Vector::new(i as f32 + offset, 0.0, 0.0).into(), + Vector::new(i as f32 + offset + 1.0, 1.0, 1.0).into(), + ) + }; + + let mut seq = Bvh::new(); + let mut par = Bvh::new(); + for i in 0..1000 { + seq.insert(aabb(i, 0.0), i as u32); + par.insert(aabb(i, 0.0), i as u32); + } + // Fatten every leaf so the batch below exercises the within-margin + // (`Unchanged`) path too. + for i in 0..1000 { + let _ = seq.insert_or_update_partially(aabb(i, 2.5), i as u32, 0.5); + let _ = par.insert_or_update_partially(aabb(i, 2.5), i as u32, 0.5); + } + + // A mix of: moved-beyond-margin leaves, moved-within-margin leaves, and + // brand new leaves. + let updates: Vec<(Aabb, u32, f32)> = (0..1500) + .map(|i| { + let offset = if i % 3 == 0 { 2.51 } else { 5.0 }; + (aabb(i, offset), i as u32, 0.1) + }) + .collect(); + + let seq_statuses: Vec = updates + .iter() + .map(|(aabb, leaf, margin)| seq.insert_or_update_partially(*aabb, *leaf, *margin)) + .collect(); + + let mut par_statuses = Vec::new(); + par.insert_or_update_batch_partially_parallel(&updates, &mut par_statuses); + + assert_eq!(seq_statuses, par_statuses); + assert!(seq_statuses.contains(&BvhLeafUpdateStatus::Unchanged)); + assert!(seq_statuses.contains(&BvhLeafUpdateStatus::UpdatedInPlace)); + assert!(seq_statuses.contains(&BvhLeafUpdateStatus::Inserted)); + + let mut w1 = BvhWorkspace::default(); + let mut w2 = BvhWorkspace::default(); + seq.refit(&mut w1); + par.refit(&mut w2); + for i in 0..1500u32 { + assert_eq!( + seq.leaf_node(i).map(|n| n.aabb()), + par.leaf_node(i).map(|n| n.aabb()), + "leaf {i} differs between sequential and parallel updates" + ); + } + } +} diff --git a/src/partitioning/bvh/bvh_traverse_bvtt.rs b/src/partitioning/bvh/bvh_traverse_bvtt.rs index 59cda19a..b43ec335 100644 --- a/src/partitioning/bvh/bvh_traverse_bvtt.rs +++ b/src/partitioning/bvh/bvh_traverse_bvtt.rs @@ -1,4 +1,5 @@ use super::{Bvh, BvhNode, BvhWorkspace}; +use alloc::vec::Vec; use smallvec::SmallVec; const TRAVERSAL_STACK_SIZE: usize = 32; @@ -27,7 +28,7 @@ impl Bvh { } workspace.traversal_stack.clear(); - self.self_intersect_node::(workspace, 0, f) + self.self_intersect_node::(&mut workspace.traversal_stack, 0, f) } // Traverses overlaps of a single node with itself. @@ -37,7 +38,7 @@ impl Bvh { // TODO: take change detection into account. fn self_intersect_node( &self, - workspace: &mut BvhWorkspace, + stack: &mut Vec, id: u32, f: &mut impl FnMut(u32, u32), ) { @@ -54,30 +55,30 @@ impl Bvh { let right_is_leaf = node.right.is_leaf(); if (!CHANGE_DETECTION || node.left.is_changed()) && !left_is_leaf { - self.self_intersect_node::(workspace, left_child, f); + self.self_intersect_node::(stack, left_child, f); } if (!CHANGE_DETECTION || node.right.is_changed()) && !right_is_leaf { - self.self_intersect_node::(workspace, right_child, f); + self.self_intersect_node::(stack, right_child, f); } if left_right_intersect { match (left_is_leaf, right_is_leaf) { (true, true) => f(left_child, right_child), (true, false) => self.traverse_single_subtree::( - workspace, + stack, &node.left, right_child, f, ), (false, true) => self.traverse_single_subtree::( - workspace, + stack, &node.right, left_child, f, ), (false, false) => self.traverse_two_branches::( - workspace, + stack, left_child, right_child, f, @@ -88,7 +89,7 @@ impl Bvh { fn traverse_two_branches( &self, - workspace: &mut BvhWorkspace, + stack: &mut Vec, a: u32, b: u32, f: &mut impl FnMut(u32, u32), @@ -116,16 +117,16 @@ impl Bvh { match ($child_a.is_leaf(), $child_b.is_leaf()) { (true, true) => f($child_a.children, $child_b.children), (true, false) => { - self.traverse_single_subtree::(workspace, $child_a, $child_b.children, f) + self.traverse_single_subtree::(stack, $child_a, $child_b.children, f) } (false, true) => self.traverse_single_subtree::( - workspace, + stack, $child_b, $child_a.children, f, ), (false, false) => self.traverse_two_branches::( - workspace, + stack, $child_a.children, $child_b.children, f, @@ -144,12 +145,12 @@ impl Bvh { // Checks overlap between a single node and a subtree. fn traverse_single_subtree( &self, - workspace: &mut BvhWorkspace, + stack: &mut Vec, node: &BvhNode, subtree: u32, f: &mut impl FnMut(u32, u32), ) { - debug_assert!(workspace.traversal_stack.is_empty()); + debug_assert!(stack.is_empty()); // Since this is traversing against a single node it is more efficient to keep the leaf reference // around and traverse the branch using a manual stack. Left branches are traversed by the main @@ -187,13 +188,13 @@ impl Bvh { } else { // We already advanced in curr_id once, push the other // branch to the stack. - workspace.traversal_stack.push(right.children); + stack.push(right.children); } } if !found_next { // Pop the stack to find the next candidate. - if let Some(next_id) = workspace.traversal_stack.pop() { + if let Some(next_id) = stack.pop() { curr_id = next_id; } else { // Traversal is finished. @@ -203,6 +204,217 @@ impl Bvh { } } + /// Parallel version of [`Self::traverse_bvtt_single_tree`], returning the reached + /// leaf pairs instead of invoking a closure. + /// + /// The returned pairs are in the exact same order as the calls the sequential + /// traversal would have made, so both versions are interchangeable, including for + /// determinism-sensitive callers. + #[cfg(feature = "parallel")] + pub fn traverse_bvtt_single_tree_parallel( + &self, + ) -> Vec<(u32, u32)> { + if self.nodes.is_empty() || self.nodes[0].right.leaf_count() == 0 { + // Not enough nodes for any overlap. + return Vec::new(); + } + + const MAX_SPLIT_DEPTH: u32 = 6; + self.self_intersect_node_parallel::(0, MAX_SPLIT_DEPTH) + } + + /// Task-parallel mirror of `self_intersect_node`: the recursions become tasks and + /// their results are concatenated in the same order as the sequential recursion. + #[cfg(feature = "parallel")] + fn self_intersect_node_parallel( + &self, + id: u32, + depth: u32, + ) -> Vec<(u32, u32)> { + // Subtrees below this leaf count are traversed sequentially. + const SEQ_LEAF_THRESHOLD: u32 = 512; + + let node = &self.nodes[id as usize]; + + if CHANGE_DETECTION && !node.right.is_changed() && !node.left.is_changed() { + return Vec::new(); + } + + if depth == 0 || node.left.leaf_count() + node.right.leaf_count() <= SEQ_LEAF_THRESHOLD { + let mut out = Vec::new(); + let mut stack = Vec::new(); + self.self_intersect_node::(&mut stack, id, &mut |a, b| { + out.push((a, b)) + }); + return out; + } + + let left_right_intersect = node.left.intersects(&node.right); + let left_child = node.left.children; + let right_child = node.right.children; + let left_is_leaf = node.left.is_leaf(); + let right_is_leaf = node.right.is_leaf(); + + let (mut left_pairs, (mut right_pairs, mut cross_pairs)) = rayon::join( + || { + if (!CHANGE_DETECTION || node.left.is_changed()) && !left_is_leaf { + self.self_intersect_node_parallel::(left_child, depth - 1) + } else { + Vec::new() + } + }, + || { + rayon::join( + || { + if (!CHANGE_DETECTION || node.right.is_changed()) && !right_is_leaf { + self.self_intersect_node_parallel::( + right_child, + depth - 1, + ) + } else { + Vec::new() + } + }, + || { + let mut out = Vec::new(); + if left_right_intersect { + match (left_is_leaf, right_is_leaf) { + (true, true) => out.push((left_child, right_child)), + (true, false) => { + let mut stack = Vec::new(); + self.traverse_single_subtree::( + &mut stack, + &node.left, + right_child, + &mut |a, b| out.push((a, b)), + ); + } + (false, true) => { + let mut stack = Vec::new(); + self.traverse_single_subtree::( + &mut stack, + &node.right, + left_child, + &mut |a, b| out.push((a, b)), + ); + } + (false, false) => { + out = self.traverse_two_branches_parallel::( + left_child, + right_child, + depth - 1, + ); + } + } + } + out + }, + ) + }, + ); + + left_pairs.append(&mut right_pairs); + left_pairs.append(&mut cross_pairs); + left_pairs + } + + /// Task-parallel mirror of `traverse_two_branches`, preserving its dispatch order + /// (left/left, left/right, right/left, right/right). + #[cfg(feature = "parallel")] + fn traverse_two_branches_parallel( + &self, + a: u32, + b: u32, + depth: u32, + ) -> Vec<(u32, u32)> { + const SEQ_LEAF_THRESHOLD: u32 = 512; + + let node1 = &self.nodes[a as usize]; + let node2 = &self.nodes[b as usize]; + let leaf_count = node1.left.leaf_count() + + node1.right.leaf_count() + + node2.left.leaf_count() + + node2.right.leaf_count(); + + if depth == 0 || leaf_count <= SEQ_LEAF_THRESHOLD { + let mut out = Vec::new(); + let mut stack = Vec::new(); + self.traverse_two_branches::(&mut stack, a, b, &mut |a, b| { + out.push((a, b)) + }); + return out; + } + + let dispatch = |child_a: &BvhNode, child_b: &BvhNode, check: bool| -> Vec<(u32, u32)> { + let mut out = Vec::new(); + if check { + match (child_a.is_leaf(), child_b.is_leaf()) { + (true, true) => out.push((child_a.children, child_b.children)), + (true, false) => { + let mut stack = Vec::new(); + self.traverse_single_subtree::( + &mut stack, + child_a, + child_b.children, + &mut |a, b| out.push((a, b)), + ); + } + (false, true) => { + let mut stack = Vec::new(); + self.traverse_single_subtree::( + &mut stack, + child_b, + child_a.children, + &mut |a, b| out.push((a, b)), + ); + } + (false, false) => { + out = self.traverse_two_branches_parallel::( + child_a.children, + child_b.children, + depth - 1, + ); + } + } + } + out + }; + + let left1 = &node1.left; + let right1 = &node1.right; + let left2 = &node2.left; + let right2 = &node2.right; + + let left_left = (!CHANGE_DETECTION || left1.is_changed() || left2.is_changed()) + && left1.intersects(left2); + let left_right = (!CHANGE_DETECTION || left1.is_changed() || right2.is_changed()) + && left1.intersects(right2); + let right_left = (!CHANGE_DETECTION || right1.is_changed() || left2.is_changed()) + && right1.intersects(left2); + let right_right = (!CHANGE_DETECTION || right1.is_changed() || right2.is_changed()) + && right1.intersects(right2); + + let ((mut ll, mut lr), (mut rl, mut rr)) = rayon::join( + || { + rayon::join( + || dispatch(left1, left2, left_left), + || dispatch(left1, right2, left_right), + ) + }, + || { + rayon::join( + || dispatch(right1, left2, right_left), + || dispatch(right1, right2, right_right), + ) + }, + ); + + ll.append(&mut lr); + ll.append(&mut rl); + ll.append(&mut rr); + ll + } + /// Performs a simultaneous traversal of the BVHs `self` and `other`, and yields the pairs /// of leaves it reached. /// diff --git a/src/partitioning/bvh/bvh_tree.rs b/src/partitioning/bvh/bvh_tree.rs index 07e0893e..89b623e6 100644 --- a/src/partitioning/bvh/bvh_tree.rs +++ b/src/partitioning/bvh/bvh_tree.rs @@ -144,7 +144,7 @@ pub struct BvhWorkspace { } /// A piece of data packing state flags as well as leaf counts for a BVH tree node. -#[derive(Default, Copy, Clone, Debug)] +#[derive(Default, Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", @@ -186,6 +186,17 @@ impl BvhNodeData { self.0 |= CHANGE_PENDING << 30; } + /// Collapses any change flag (pending or resolved) into the resolved `CHANGED` state. + /// + /// Used by partial refitting on internal nodes: a pending flag on an internal node + /// would otherwise read as "unchanged" (`is_changed() == false`) during traversals. + #[inline(always)] + pub(super) fn normalize_change_flag(&mut self) { + if self.0 >> 30 != 0 { + *self = Self((self.0 & 0x3fff_ffff) | (CHANGED << 30)); + } + } + #[inline(always)] pub(super) fn resolve_pending_change(&mut self) { if self.is_change_pending() { @@ -441,7 +452,7 @@ static_assertions::assert_eq_size!(BvhNode, BvhNodeSimd); /// /// - `BvhNodeWide` - Pair of nodes stored together /// - [`Bvh`] - The main BVH structure -#[derive(Copy, Clone, Debug)] +#[derive(Copy, Clone, Debug, PartialEq)] #[repr(C)] // SAFETY: needed to ensure SIMD aabb checks rely on the layout. #[cfg_attr(all(feature = "f32", feature = "dim3"), repr(align(16)))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -604,12 +615,40 @@ impl BvhNode { #[inline(always)] pub(super) fn merged(&self, other: &Self, children: u32) -> Self { - // TODO PERF: simd optimizations? - Self { - mins: self.mins.min(other.mins), - children, - maxs: self.maxs.max(other.maxs), - data: self.data.merged(other.data), + #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] + { + // Each node is two 16-byte rows: (mins, children) and (maxs, data). + // Min/max whole rows in one SIMD op each (the packed integer lanes + // produce garbage that is overwritten right after). This is the hot + // op of the refit passes, which rebuild every internal node. + let a = self.as_simd(); + let b = other.as_simd(); + let data = self.data.merged(other.data); + let mut out = Self { + mins: Vector::ZERO, + children, + maxs: Vector::ZERO, + data, + }; + { + let out_simd: &mut BvhNodeSimd = unsafe { core::mem::transmute(&mut out) }; + out_simd.mins = a.mins.min(b.mins); + out_simd.maxs = a.maxs.max(b.maxs); + } + // Restore the packed lanes clobbered by the row-wide min/max. + out.children = children; + out.data = data; + out + } + + #[cfg(not(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32")))] + { + Self { + mins: self.mins.min(other.mins), + children, + maxs: self.maxs.max(other.maxs), + data: self.data.merged(other.data), + } } } @@ -1758,6 +1797,16 @@ pub struct Bvh { // NOTE: this cannot be in the workspace as we need this to survive serialization/deserialization // to maintain determinism. pub(super) optimization: BvhIncrementalOptimizationState, + // Wide-node slots orphaned by leaf removals, reused by subsequent insertions + // so remove/insert cycles don't grow the node array unboundedly between + // compacting refits. The slots are zeroed when freed (a stale leaf copy left + // in an orphaned slot would be picked up by [`Bvh::rebuild`]'s raw node scan). + // Compacting refits and rebuilds (which recreate the node array and drop the + // orphaned slots) clear this list. + // NOTE: must survive serialization to maintain determinism (the slot reuse + // order affects the tree topology produced by later insertions). + #[cfg_attr(feature = "serde-serialize", serde(default))] + pub(super) free_wide_nodes: Vec, } impl Bvh { @@ -2193,10 +2242,12 @@ impl Bvh { parents, leaf_node_indices, optimization: _, + free_wide_nodes, } = self; nodes.capacity() * size_of::() + parents.capacity() * size_of::() + leaf_node_indices.capacity() * size_of::() + + free_wide_nodes.capacity() * size_of::() } /// Computes the depth of the subtree rooted at the specified node. @@ -2364,6 +2415,7 @@ impl Bvh { // We deleted the last leaf! Remove the root. self.nodes.clear(); self.parents.clear(); + self.free_wide_nodes.clear(); return; } @@ -2391,9 +2443,13 @@ impl Bvh { // nodes, which corrupts optimize_incremental. self.nodes.truncate(1); self.parents.truncate(1); + self.free_wide_nodes.clear(); } else { // The sibling isn’t a leaf. It becomes the new root at index 0. - self.nodes[0] = self.nodes[self.nodes[sibling].children as usize]; + let old_sibling_slot = self.nodes[sibling].children; + self.nodes[0] = self.nodes[old_sibling_slot as usize]; + self.nodes[old_sibling_slot as usize] = BvhNodeWide::zeros(); + self.free_wide_nodes.push(old_sibling_slot); // Both parent pointers need to be updated since both nodes moved to the root. let new_root = &mut self.nodes[0]; if new_root.left.is_leaf() { @@ -2423,6 +2479,12 @@ impl Bvh { self.nodes[parent] = *sibling; + // The removed leaf's wide node is now unreachable: zero it (so raw + // node scans like `Bvh::rebuild` can't pick up its stale leaf + // copies) and recycle its slot for later insertions. + self.nodes[wide_node_index] = BvhNodeWide::zeros(); + self.free_wide_nodes.push(wide_node_index as u32); + // TODO: we could use that propagation as an opportunity to // apply some rotations? let mut curr = parent.decompose().0; diff --git a/src/partitioning/bvh/mod.rs b/src/partitioning/bvh/mod.rs index b9c0c461..9a917b61 100644 --- a/src/partitioning/bvh/mod.rs +++ b/src/partitioning/bvh/mod.rs @@ -1,3 +1,4 @@ +pub use bvh_insert::BvhLeafUpdateStatus; pub use bvh_traverse::{BvhLeafCost, TraversalAction}; pub use bvh_tree::{Bvh, BvhBuildStrategy, BvhNode, BvhNodeIndex, BvhNodeWide, BvhWorkspace}; diff --git a/src/partitioning/mod.rs b/src/partitioning/mod.rs index 691a8175..12b475e3 100644 --- a/src/partitioning/mod.rs +++ b/src/partitioning/mod.rs @@ -2,8 +2,8 @@ #[cfg(feature = "alloc")] pub use self::bvh::{ - Bvh, BvhBuildStrategy, BvhLeafCost, BvhNode, BvhNodeIndex, BvhNodeWide, BvhWorkspace, - TraversalAction, + Bvh, BvhBuildStrategy, BvhLeafCost, BvhLeafUpdateStatus, BvhNode, BvhNodeIndex, BvhNodeWide, + BvhWorkspace, TraversalAction, }; #[cfg(feature = "alloc")] From 0cc2446edc0e189a444ec86ea927b9e69a16d4c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:27 +0200 Subject: [PATCH 02/12] perf: move the subshape_pos behind a Box --- .../contact_manifolds/contact_manifold.rs | 89 ++++++++++++++++--- ...nifolds_composite_shape_composite_shape.rs | 8 +- ...contact_manifolds_composite_shape_shape.rs | 4 +- ...t_manifolds_heightfield_composite_shape.rs | 4 +- ...ontact_manifolds_voxels_composite_shape.rs | 18 ++-- .../contact_manifolds_voxels_shape.rs | 12 +-- .../contact_manifolds_voxels_voxels.rs | 12 +-- 7 files changed, 103 insertions(+), 44 deletions(-) diff --git a/src/query/contact_manifolds/contact_manifold.rs b/src/query/contact_manifolds/contact_manifold.rs index ecbf0d5c..15e702df 100644 --- a/src/query/contact_manifolds/contact_manifold.rs +++ b/src/query/contact_manifolds/contact_manifold.rs @@ -492,16 +492,69 @@ pub struct ContactManifold { /// /// This is zero if the second shape is not a composite shape. pub subshape2: u32, - /// If the first shape involved is a composite shape, this contains the position of its subshape - /// involved in this contact. - pub subshape_pos1: Option, - /// If the second shape involved is a composite shape, this contains the position of its subshape - /// involved in this contact. - pub subshape_pos2: Option, + /// If either shape involved is a composite shape, this contains the positions of the + /// subshapes involved in this contact. + pub subshape_poses: Option>, /// Additional tracked data associated to this contact manifold. pub data: ManifoldData, } +/// The positions of the composite-shape subshapes involved in a contact manifold. +#[derive(Clone, Debug, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SubshapePoses { + /// The position of the first shape's subshape, if it is a composite shape. + pub pos1: Option, + /// The position of the second shape's subshape, if it is a composite shape. + pub pos2: Option, +} + +impl ContactManifold { + /// The position of the first shape's subshape involved in this contact, if the first + /// shape is a composite shape. + #[inline] + pub fn subshape_pos1(&self) -> Option<&Pose> { + self.subshape_poses.as_ref().and_then(|p| p.pos1.as_ref()) + } + + /// The position of the second shape's subshape involved in this contact, if the second + /// shape is a composite shape. + #[inline] + pub fn subshape_pos2(&self) -> Option<&Pose> { + self.subshape_poses.as_ref().and_then(|p| p.pos2.as_ref()) + } + + /// Sets the position of the first shape's subshape, reusing the existing allocation. + #[inline] + pub fn set_subshape_pos1(&mut self, pos: Option) { + match (&mut self.subshape_poses, pos) { + (Some(poses), pos) => poses.pos1 = pos, + (slot @ None, Some(pos)) => { + *slot = Some(alloc::boxed::Box::new(SubshapePoses { + pos1: Some(pos), + pos2: None, + })) + } + (None, None) => {} + } + } + + /// Sets the position of the second shape's subshape, reusing the existing allocation. + #[inline] + pub fn set_subshape_pos2(&mut self, pos: Option) { + match (&mut self.subshape_poses, pos) { + (Some(poses), pos) => poses.pos2 = pos, + (slot @ None, Some(pos)) => { + *slot = Some(alloc::boxed::Box::new(SubshapePoses { + pos1: None, + pos2: Some(pos), + })) + } + (None, None) => {} + } + } +} + impl ContactManifold { /// Create a new empty contact-manifold. pub fn new() -> Self @@ -522,8 +575,7 @@ impl ContactManifold ContactManifold ContactManifold ContactManifold dist_sq_threshold { return false; } - - pt.dist = dist; - pt.local_p1 = new_local_p1; } + self.update_separations(pos12); + true } + /// Refreshes every contact point's separation (`dist`) from the current + /// relative pose of the two shapes, keeping the contact points (anchors) + /// frozen at their captured material positions. + #[inline] + pub fn update_separations(&mut self, pos12: &Pose) { + for pt in &mut self.points { + let local_p2 = pos12 * pt.local_p2; + pt.dist = (local_p2 - pt.local_p1).dot(self.local_n1); + } + } + /// Transfers contact data from previous frame's contacts to current contacts based on feature IDs. /// /// This method is crucial for maintaining persistent contact information across frames. diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs index f6781130..7d9618cf 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_composite_shape.rs @@ -133,13 +133,13 @@ pub fn contact_manifolds_composite_shape_composite_shape<'a, ManifoldData, Conta if flipped { manifold.subshape1 = leaf2; manifold.subshape2 = leaf1; - manifold.subshape_pos1 = part_pos2.copied(); - manifold.subshape_pos2 = part_pos1.copied(); + manifold.set_subshape_pos1(part_pos2.copied()); + manifold.set_subshape_pos2(part_pos1.copied()); } else { manifold.subshape1 = leaf1; manifold.subshape2 = leaf2; - manifold.subshape_pos1 = part_pos1.copied(); - manifold.subshape_pos2 = part_pos2.copied(); + manifold.set_subshape_pos1(part_pos1.copied()); + manifold.set_subshape_pos2(part_pos2.copied()); }; manifolds.push(manifold); diff --git a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs index 711fbd62..1ca69958 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs @@ -105,11 +105,11 @@ pub fn contact_manifolds_composite_shape_shape( if flipped { manifold.subshape1 = 0; manifold.subshape2 = leaf1; - manifold.subshape_pos2 = part_pos1.copied(); + manifold.set_subshape_pos2(part_pos1.copied()); } else { manifold.subshape1 = leaf1; manifold.subshape2 = 0; - manifold.subshape_pos1 = part_pos1.copied(); + manifold.set_subshape_pos1(part_pos1.copied()); }; manifolds.push(manifold); diff --git a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs index fd30c6c8..01749f43 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs @@ -111,11 +111,11 @@ pub fn contact_manifolds_heightfield_composite_shape( if flipped { manifold.subshape1 = leaf2; manifold.subshape2 = leaf1; - manifold.subshape_pos1 = part_pos2.copied(); + manifold.set_subshape_pos1(part_pos2.copied()); } else { manifold.subshape1 = leaf1; manifold.subshape2 = leaf2; - manifold.subshape_pos2 = part_pos2.copied(); + manifold.set_subshape_pos2(part_pos2.copied()); }; manifolds.push(manifold); diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs index ba815adc..972ae620 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs @@ -168,13 +168,13 @@ pub fn contact_manifolds_voxels_composite_shape( // and keep the point at the same "canonical-shape-space" location as in the previous frame. let prev_center = if flipped { manifold - .subshape_pos2 + .subshape_pos2() .as_ref() .map(|p| p.translation) .unwrap_or_default() } else { manifold - .subshape_pos1 + .subshape_pos1() .as_ref() .map(|p| p.translation) .unwrap_or_default() @@ -193,9 +193,8 @@ pub fn contact_manifolds_voxels_composite_shape( // Update contacts. if flipped { - manifold.subshape_pos1 = part_pos2.copied(); - manifold.subshape_pos2 = - Some(Pose::from_translation(canonical_center1)); + manifold.set_subshape_pos1(part_pos2.copied()); + manifold.set_subshape_pos2(Some(Pose::from_translation(canonical_center1))); let _ = dispatcher.contact_manifold_convex_convex( &relative_pos12.inverse(), part_shape2, @@ -206,9 +205,8 @@ pub fn contact_manifolds_voxels_composite_shape( manifold, ); } else { - manifold.subshape_pos1 = - Some(Pose::from_translation(canonical_center1)); - manifold.subshape_pos2 = part_pos2.copied(); + manifold.set_subshape_pos1(Some(Pose::from_translation(canonical_center1))); + manifold.set_subshape_pos2(part_pos2.copied()); let _ = dispatcher.contact_manifold_convex_convex( &relative_pos12, &canonical_shape1, @@ -254,9 +252,9 @@ pub fn contact_manifolds_voxels_composite_shape( } let pt_in_voxel_space = if flipped { - manifold.subshape_pos2.transform_point(pt.local_p2) - vox1.center + manifold.subshape_pos2().transform_point(pt.local_p2) - vox1.center } else { - manifold.subshape_pos1.transform_point(pt.local_p1) - vox1.center + manifold.subshape_pos1().transform_point(pt.local_p1) - vox1.center }; sub_detector.selected_contacts |= (test_voxel.contains_local_point(pt_in_voxel_space) as u32) << i; diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs index 45c396e8..54033d4d 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs @@ -215,13 +215,13 @@ pub fn contact_manifolds_voxels_shape( // and keep the point at the same "canonica-shape-space" location as in the previous frame. let prev_center = if flipped { manifold - .subshape_pos2 + .subshape_pos2() .as_ref() .map(|p| p.translation) .unwrap_or_default() } else { manifold - .subshape_pos1 + .subshape_pos1() .as_ref() .map(|p| p.translation) .unwrap_or_default() @@ -240,7 +240,7 @@ pub fn contact_manifolds_voxels_shape( // Update contacts. if flipped { - manifold.subshape_pos2 = Some(Pose::from_translation(canonical_center1)); + manifold.set_subshape_pos2(Some(Pose::from_translation(canonical_center1))); let _ = dispatcher.contact_manifold_convex_convex( &canonical_pos12.inverse(), shape2, @@ -251,7 +251,7 @@ pub fn contact_manifolds_voxels_shape( manifold, ); } else { - manifold.subshape_pos1 = Some(Pose::from_translation(canonical_center1)); + manifold.set_subshape_pos1(Some(Pose::from_translation(canonical_center1))); let _ = dispatcher.contact_manifold_convex_convex( &canonical_pos12, canonical_shape1, @@ -296,9 +296,9 @@ pub fn contact_manifolds_voxels_shape( } let pt_in_voxel_space = if flipped { - manifold.subshape_pos2.transform_point(pt.local_p2) - vox1.center + manifold.subshape_pos2().transform_point(pt.local_p2) - vox1.center } else { - manifold.subshape_pos1.transform_point(pt.local_p1) - vox1.center + manifold.subshape_pos1().transform_point(pt.local_p1) - vox1.center }; sub_detector.selected_contacts |= (test_voxel.contains_local_point(pt_in_voxel_space) as u32) << i; diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs b/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs index 21f9c8db..630ab3a6 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_voxels.rs @@ -159,13 +159,13 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( // So we need to adjust the local points to account for the position difference // and keep the point at the same "canonical-shape-space" location as in the previous frame. let prev_center1 = manifold - .subshape_pos1 + .subshape_pos1() .as_ref() .map(|p| p.translation) .unwrap_or_default(); let delta_center1 = canonical_center1 - prev_center1; let prev_center2 = manifold - .subshape_pos2 + .subshape_pos2() .as_ref() .map(|p| p.translation) .unwrap_or_default(); @@ -177,8 +177,8 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( } // Update contacts. - manifold.subshape_pos1 = Some(Pose::from_translation(canonical_center1)); - manifold.subshape_pos2 = Some(Pose::from_translation(canonical_center2)); + manifold.set_subshape_pos1(Some(Pose::from_translation(canonical_center1))); + manifold.set_subshape_pos2(Some(Pose::from_translation(canonical_center2))); let _ = dispatcher.contact_manifold_convex_convex( &canonical_pos12, &canonical_pseudo_cube1, @@ -220,9 +220,9 @@ pub fn contact_manifolds_voxels_voxels<'a, ManifoldData, ContactData>( } let pt_in_voxel_space1 = - manifold.subshape_pos1.transform_point(pt.local_p1) - vox1.center; + manifold.subshape_pos1().transform_point(pt.local_p1) - vox1.center; let pt_in_voxel_space2 = - manifold.subshape_pos2.transform_point(pt.local_p2) - vox2.center; + manifold.subshape_pos2().transform_point(pt.local_p2) - vox2.center; sub_detector.selected_contacts |= ((test_voxel1.contains_local_point(pt_in_voxel_space1) as u32) << i) & ((test_voxel2.contains_local_point(pt_in_voxel_space2) as u32) << i); From 2a54565d9256aa1ace56b274de528e61174efadb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:28 +0200 Subject: [PATCH 03/12] perf: compute cuboid support-face feature ids with bit ops --- src/shape/cuboid.rs | 64 +++++++++++++++++++++------------------------ 1 file changed, 30 insertions(+), 34 deletions(-) diff --git a/src/shape/cuboid.rs b/src/shape/cuboid.rs index f3a2d5ed..6c768068 100644 --- a/src/shape/cuboid.rs +++ b/src/shape/cuboid.rs @@ -274,8 +274,7 @@ impl Cuboid { let sign = match imax { 0 => local_dir.x.copy_sign_to(1.0 as Real), 1 => local_dir.y.copy_sign_to(1.0 as Real), - 2 => local_dir.z.copy_sign_to(1.0 as Real), - _ => unreachable!(), + _ => local_dir.z.copy_sign_to(1.0 as Real), }; let vertices = match imax { @@ -291,13 +290,12 @@ impl Cuboid { Vector::new(-he.x, he.y * sign, -he.z), Vector::new(he.x, he.y * sign, -he.z), ], - 2 => [ + _ => [ Vector::new(he.x, he.y, he.z * sign), Vector::new(he.x, -he.y, he.z * sign), Vector::new(-he.x, -he.y, he.z * sign), Vector::new(-he.x, he.y, he.z * sign), - ], - _ => unreachable!(), + ] }; pub fn vid(i: u32) -> u32 { @@ -305,25 +303,24 @@ impl Cuboid { i * 2 } - let sign_index = ((sign as isize + 1) / 2) as usize; + let sign_index = ((sign as isize + 1) / 2) as u32; // The vertex id as numbered depending on the sign of the vertex // component. A + sign means the corresponding bit is 0 while a - // sign means the corresponding bit is 1. // For exampl the vertex [2.0, -1.0, -3.0] has the id 0b011 let vids = match imax { - 0 => [ - [vid(0b000), vid(0b010), vid(0b011), vid(0b001)], - [vid(0b100), vid(0b110), vid(0b111), vid(0b101)], - ][sign_index], - 1 => [ - [vid(0b000), vid(0b100), vid(0b101), vid(0b001)], - [vid(0b010), vid(0b110), vid(0b111), vid(0b011)], - ][sign_index], - 2 => [ - [vid(0b000), vid(0b010), vid(0b110), vid(0b100)], - [vid(0b001), vid(0b011), vid(0b111), vid(0b101)], - ][sign_index], - _ => unreachable!(), + 0 => { + let sbit = sign_index << 2; + [vid(0b000 | sbit), vid(0b010 | sbit), vid(0b011 | sbit), vid(0b001 | sbit)] + }, + 1 => { + let sbit = sign_index << 1; + [vid(0b000 | sbit), vid(0b100 | sbit), vid(0b101 | sbit), vid(0b001 | sbit)] + } + _ => { + let sbit = sign_index; + [vid(0b000 | sbit), vid(0b010 | sbit), vid(0b110 | sbit), vid(0b100 | sbit)] + } }; // The feature ids of edges is obtained from the vertex ids @@ -331,30 +328,29 @@ impl Cuboid { // Assuming vid1 > vid2, we do: (vid1 << 3) | vid2 | 0b11000000 // let eids = match imax { - 0 => [ - [0b11_010_000, 0b11_011_010, 0b11_011_001, 0b11_001_000], - [0b11_110_100, 0b11_111_110, 0b11_111_101, 0b11_101_100], - ][sign_index], - 1 => [ - [0b11_100_000, 0b11_101_100, 0b11_101_001, 0b11_001_000], - [0b11_110_010, 0b11_111_110, 0b11_111_011, 0b11_011_010], - ][sign_index], - 2 => [ - [0b11_010_000, 0b11_110_010, 0b11_110_100, 0b11_100_000], - [0b11_011_001, 0b11_111_011, 0b11_111_101, 0b11_101_001], - ][sign_index], - _ => unreachable!(), + 0 => { + let sbits = (sign_index << 2) | (sign_index << 5); // 0b00_100_100 + [0b11_010_000 | sbits, 0b11_011_010 | sbits, 0b11_011_001 | sbits, 0b11_001_000 | sbits] + } + 1 => { + let sbits = (sign_index << 1) | (sign_index << 4); // 0b00_010_010 + [0b11_100_000 | sbits, 0b11_101_100 | sbits, 0b11_101_001 | sbits, 0b11_001_000 | sbits] + } + _ => { + let sbits = (sign_index << 0) | (sign_index << 3); // 0b00_001_001 + [0b11_010_000 | sbits, 0b11_110_010 | sbits, 0b11_110_100 | sbits, 0b11_100_000 | sbits] + } }; // The face with normals [x, y, z] are numbered [10, 11, 12]. // The face with negated normals are numbered [13, 14, 15]. - let fid = imax + sign_index * 3 + 10; + let fid = imax as u32 + sign_index * 3 + 10; PolygonalFeature { vertices, vids: PackedFeatureId::vertices(vids), eids: PackedFeatureId::edges(eids), - fid: PackedFeatureId::face(fid as u32), + fid: PackedFeatureId::face(fid), num_vertices: 4, } } From 5d4059934e4355d1b31cb6c879b062dd200ebc00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:29 +0200 Subject: [PATCH 04/12] feat: sweep-based time-of-impact queries (query::sweep_toi) --- src/query/mod.rs | 6 + src/query/sweep_toi/composite.rs | 297 +++++++ src/query/sweep_toi/mod.rs | 25 + src/query/sweep_toi/proxy_distance.rs | 1027 +++++++++++++++++++++++++ src/query/sweep_toi/separation.rs | 583 ++++++++++++++ src/query/sweep_toi/sweep.rs | 110 +++ src/query/sweep_toi/sweep_toi.rs | 437 +++++++++++ src/query/sweep_toi/toi_proxy.rs | 183 +++++ 8 files changed, 2668 insertions(+) create mode 100644 src/query/sweep_toi/composite.rs create mode 100644 src/query/sweep_toi/mod.rs create mode 100644 src/query/sweep_toi/proxy_distance.rs create mode 100644 src/query/sweep_toi/separation.rs create mode 100644 src/query/sweep_toi/sweep.rs create mode 100644 src/query/sweep_toi/sweep_toi.rs create mode 100644 src/query/sweep_toi/toi_proxy.rs diff --git a/src/query/mod.rs b/src/query/mod.rs index 173039e9..b0af7bde 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -43,6 +43,11 @@ pub use self::query_dispatcher::{QueryDispatcher, QueryDispatcherChain}; pub use self::ray::{Ray, RayCast, RayIntersection, SimdRay}; pub use self::shape_cast::{cast_shapes, ShapeCastHit, ShapeCastOptions, ShapeCastStatus}; pub use self::split::{IntersectResult, SplitResult}; +#[cfg(feature = "alloc")] +pub use self::sweep_toi::{sweep_time_of_impact_composite, SweepCompositeFastShape}; +pub use self::sweep_toi::{ + sweep_time_of_impact, SimplexCache, Sweep, SweepToiOutput, SweepToiStatus, ToiProxy, +}; #[cfg(all(feature = "dim3", feature = "alloc"))] pub use self::ray::RayCullingMode; @@ -66,6 +71,7 @@ mod ray; pub mod sat; mod shape_cast; mod split; +pub mod sweep_toi; /// Queries dedicated to specific pairs of shapes. pub mod details { diff --git a/src/query/sweep_toi/composite.rs b/src/query/sweep_toi/composite.rs new file mode 100644 index 00000000..b0bd1005 --- /dev/null +++ b/src/query/sweep_toi/composite.rs @@ -0,0 +1,297 @@ +//! Sweep time-of-impact against composite shapes (meshes, polylines, heightfields, +//! compounds): the composite is assumed stationary, its acceleration structure is queried +//! with the swept bounds of the moving shape, and each candidate element runs the convex +//! TOI with a fallback sphere when the element reports an initial overlap. + +use super::sweep::Sweep; +use super::sweep_toi::{sweep_time_of_impact, SweepToiOutput, SweepToiStatus}; +use super::toi_proxy::ToiProxy; +use crate::bounding_volume::BoundingVolume; +use crate::math::{Pose, Real, Vector}; +use crate::shape::{Shape, TypedShape}; + +#[cfg(feature = "dim2")] +use crate::shape::{PolylineFlags, Segment}; +#[cfg(feature = "dim3")] +use crate::shape::{TriMeshFlags, Triangle}; + +/// Fraction of the fast shape’s minimum extent used for the initial-overlap fallback sphere. +pub const CORE_FRACTION: Real = 0.25; + +/// Parameters describing the fast (moving) shape for a composite TOI query. +#[derive(Copy, Clone)] +pub struct SweepCompositeFastShape<'a, 'b> { + /// Point-cloud proxy of the moving shape. + pub proxy: &'a ToiProxy<'b>, + /// Sweep of the moving shape. + pub sweep: &'a Sweep, + /// Centroid of the moving shape in its local frame. + pub local_centroid: Vector, + /// Smallest extent (inner radius) of the moving shape, used for fallback spheres and + /// one-sided early-outs. + pub min_extent: Real, +} + +struct CompositeToiContext<'a, 'b> { + fast: SweepCompositeFastShape<'a, 'b>, + // Centroid of the moving shape at the sweep endpoints, in the composite’s local frame. + local_centroid1: Vector, + local_centroid2: Vector, + fallback_radius: Real, + one_sided: bool, + #[cfg_attr(feature = "dim2", allow(dead_code))] + target_is_sensor: bool, + linear_slop: Real, + max_fraction: Real, + best: Option, +} + +impl CompositeToiContext<'_, '_> { + /// Runs the convex TOI of the moving shape against one composite element and keeps the + /// earliest hit, with a fallback-sphere retry on initial overlap. + fn toi_against_element(&mut self, element_proxy: &ToiProxy, element_sweep: &Sweep) { + let output = sweep_time_of_impact( + element_proxy, + element_sweep, + self.fast.proxy, + self.fast.sweep, + self.max_fraction, + self.linear_slop, + ); + + if 0.0 < output.fraction && output.fraction < self.max_fraction { + self.max_fraction = output.fraction; + self.best = Some(output); + } else if output.fraction == 0.0 { + // Fallback to the TOI of a small ball around the fast shape centroid. + #[cfg(feature = "dim2")] + let radius = self.fallback_radius; + #[cfg(feature = "dim3")] + let radius = self.fallback_radius + self.linear_slop; + + let fallback_proxy = ToiProxy::point(self.fast.local_centroid, radius); + let output = sweep_time_of_impact( + element_proxy, + element_sweep, + &fallback_proxy, + self.fast.sweep, + self.max_fraction, + self.linear_slop, + ); + + if 0.0 < output.fraction && output.fraction < self.max_fraction { + self.max_fraction = output.fraction; + self.best = Some(output); + } + } + } + + /// One-sided early-out for a 2D chain segment. + /// Returns `true` when the element can be skipped. + #[cfg(feature = "dim2")] + fn one_sided_early_out(&self, segment: &Segment) -> bool { + if !self.one_sided { + return false; + } + + let e = segment.b - segment.a; + let length = e.length(); + if length <= self.linear_slop { + return false; + } + let e = e / length; + + let separation1 = (self.local_centroid1 - segment.a).perp_dot(e); + let separation2 = (self.local_centroid2 - segment.a).perp_dot(e); + let core_distance = CORE_FRACTION * self.fast.min_extent; + + separation1 < 0.0 + || (separation1 - separation2 < core_distance && separation2 > core_distance) + } + + /// One-sided early-out for a 3D triangle. + /// Returns `true` when the element can be skipped. + #[cfg(feature = "dim3")] + fn one_sided_early_out(&self, triangle: &Triangle) -> bool { + if !self.one_sided { + return false; + } + + let n = (triangle.b - triangle.a) + .cross(triangle.c - triangle.a) + .normalize_or_zero(); + let offset1 = n.dot(self.local_centroid1 - triangle.a); + let offset2 = n.dot(self.local_centroid2 - triangle.a); + + if offset1 < 0.0 { + // Started behind. + return true; + } + + if !self.target_is_sensor + && offset1 - offset2 < self.fallback_radius + && offset2 > self.fallback_radius + { + // Finished in front. + return true; + } + + false + } +} + +/// Computes the time of impact between a stationary composite shape and a moving convex +/// shape. +/// +/// Returns `None` if `composite` is not a supported composite shape (triangle mesh, +/// polyline, heightfield, or compound). When supported but nothing is hit, the returned +/// output has status [`SweepToiStatus::Separated`] and `fraction == max_fraction`. +/// +/// `one_sided` enables the “started behind / finished in front” early-outs; it +/// should only be set for composites whose elements have meaningful outward normals +/// (oriented polylines, oriented meshes, heightfields). +#[allow(clippy::too_many_arguments)] +pub fn sweep_time_of_impact_composite( + composite: &dyn Shape, + composite_pose: &Pose, + fast: SweepCompositeFastShape, + one_sided: bool, + target_is_sensor: bool, + max_fraction: Real, + linear_slop: Real, +) -> Option { + let typed = composite.as_typed_shape(); + + // Fallback sphere radius (2D: core circle; 3D: mesh/compound fallback spheres). + #[cfg(feature = "dim2")] + let fallback_radius = CORE_FRACTION * fast.min_extent; + #[cfg(feature = "dim3")] + let fallback_radius = match typed { + TypedShape::Compound(_) => (0.75 * fast.min_extent).max(4.0 * linear_slop), + _ => (0.5 * fast.min_extent).max(linear_slop), + }; + + // Swept bounds of the fast shape, in the composite’s local frame. + let start_aabb = fast.proxy.compute_aabb(&fast.sweep.transform_at(0.0)); + let end_aabb = fast + .proxy + .compute_aabb(&fast.sweep.transform_at(max_fraction)); + let local_aabb = start_aabb + .merged(&end_aabb) + .transform_by(&composite_pose.inverse()); + + // Centroid of the fast shape at the sweep endpoints, in the composite’s local frame. + let centroid_world1 = fast + .sweep + .transform_at(0.0) + .transform_point(fast.local_centroid); + let centroid_world2 = fast + .sweep + .final_transform() + .transform_point(fast.local_centroid); + + let mut context = CompositeToiContext { + fast, + local_centroid1: composite_pose.inverse_transform_point(centroid_world1), + local_centroid2: composite_pose.inverse_transform_point(centroid_world2), + fallback_radius, + one_sided, + target_is_sensor, + linear_slop, + max_fraction, + best: None, + }; + + // The composite is stationary: every element sweep is degenerate at the composite pose + // (composed with the child pose for compounds). + let composite_sweep = Sweep::constant(composite_pose, Vector::ZERO); + + match typed { + #[cfg(feature = "dim3")] + TypedShape::TriMesh(mesh) => { + let one_sided_mesh = mesh.flags().contains(TriMeshFlags::ORIENTED); + context.one_sided = one_sided || one_sided_mesh; + for tri_id in mesh.bvh().intersect_aabb(&local_aabb) { + let triangle = mesh.triangle(tri_id); + if context.one_sided_early_out(&triangle) { + continue; + } + let proxy = ToiProxy::from_array([triangle.a, triangle.b, triangle.c], 0.0); + context.toi_against_element(&proxy, &composite_sweep); + } + } + #[cfg(feature = "dim2")] + TypedShape::TriMesh(mesh) => { + for tri_id in mesh.bvh().intersect_aabb(&local_aabb) { + let triangle = mesh.triangle(tri_id); + let proxy = ToiProxy::from_array([triangle.a, triangle.b, triangle.c], 0.0); + context.toi_against_element(&proxy, &composite_sweep); + } + } + TypedShape::Polyline(polyline) => { + #[cfg(feature = "dim2")] + { + let oriented = polyline.flags().contains(PolylineFlags::ORIENTED); + context.one_sided = one_sided || oriented; + } + for seg_id in polyline.bvh().intersect_aabb(&local_aabb) { + let segment = polyline.segment(seg_id); + #[cfg(feature = "dim2")] + if context.one_sided_early_out(&segment) { + continue; + } + let proxy = ToiProxy::from_array([segment.a, segment.b], 0.0); + context.toi_against_element(&proxy, &composite_sweep); + } + } + TypedShape::HeightField(heightfield) => { + #[cfg(feature = "dim2")] + heightfield.map_elements_in_local_aabb(&local_aabb, &mut |_, segment| { + if !context.one_sided_early_out(segment) { + let proxy = ToiProxy::from_array([segment.a, segment.b], 0.0); + context.toi_against_element(&proxy, &composite_sweep); + } + }); + #[cfg(feature = "dim3")] + heightfield.map_elements_in_local_aabb(&local_aabb, &mut |_, triangle| { + if !context.one_sided_early_out(triangle) { + let proxy = + ToiProxy::from_array([triangle.a, triangle.b, triangle.c], 0.0); + context.toi_against_element(&proxy, &composite_sweep); + } + }); + } + TypedShape::Compound(compound) => { + for child_id in compound.bvh().intersect_aabb(&local_aabb) { + let (child_pose, child_shape) = &compound.shapes()[child_id as usize]; + let child_world_pose = *composite_pose * *child_pose; + if let Some(child_proxy) = ToiProxy::from_shape(child_shape.as_ref()) { + let child_sweep = Sweep::constant(&child_world_pose, Vector::ZERO); + context.toi_against_element(&child_proxy, &child_sweep); + } else if let Some(hit) = sweep_time_of_impact_composite( + child_shape.as_ref(), + &child_world_pose, + context.fast, + context.one_sided, + target_is_sensor, + context.max_fraction, + linear_slop, + ) { + if 0.0 < hit.fraction && hit.fraction < context.max_fraction { + context.max_fraction = hit.fraction; + context.best = Some(hit); + } + } + // Children that are neither point-cloud shapes nor composites are skipped. + } + } + _ => return None, + } + + Some(context.best.unwrap_or(SweepToiOutput { + status: SweepToiStatus::Separated, + fraction: max_fraction, + point: Vector::ZERO, + normal: Vector::ZERO, + })) +} diff --git a/src/query/sweep_toi/mod.rs b/src/query/sweep_toi/mod.rs new file mode 100644 index 00000000..91f1ab3e --- /dev/null +++ b/src/query/sweep_toi/mod.rs @@ -0,0 +1,25 @@ +//! Time-of-impact computation on endpoint-interpolated sweeps. +//! +//! Unlike [`cast_shapes_nonlinear`](crate::query::cast_shapes_nonlinear), which models motion +//! as constant velocities, this module models a timestep as a [`Sweep`] between two endpoint +//! poses (linear center-of-mass interpolation + rotation nlerp) and computes the earliest +//! time at which two swept shapes reach a slop-based target separation, using conservative +//! advancement with separation functions. + +pub use self::proxy_distance::{proxy_distance, ProxyDistanceOutput, SimplexCache}; +pub use self::sweep::Sweep; +pub use self::sweep_toi::{sweep_time_of_impact, SweepToiOutput, SweepToiStatus}; +pub use self::toi_proxy::{ToiProxy, TOI_PROXY_INLINE_POINTS}; + +#[cfg(feature = "alloc")] +pub use self::composite::{ + sweep_time_of_impact_composite, SweepCompositeFastShape, CORE_FRACTION, +}; + +#[cfg(feature = "alloc")] +mod composite; +mod proxy_distance; +mod separation; +mod sweep; +mod sweep_toi; +mod toi_proxy; diff --git a/src/query/sweep_toi/proxy_distance.rs b/src/query/sweep_toi/proxy_distance.rs new file mode 100644 index 00000000..f895e827 --- /dev/null +++ b/src/query/sweep_toi/proxy_distance.rs @@ -0,0 +1,1027 @@ +//! GJK distance between two point-cloud proxies with an index-based simplex cache. +//! +//! The cache warm-starts successive distance queries on the same pair, which is the +//! backbone of the conservative-advancement time-of-impact loop. + +use super::toi_proxy::ToiProxy; +use crate::math::{Pose, Real, Vector}; + +/// Warm-starting simplex cache for [`proxy_distance`]. +#[derive(Copy, Clone, Debug, Default)] +pub struct SimplexCache { + /// Simplex size measure used to detect a stale cache (3D only). + #[cfg(feature = "dim3")] + pub metric: Real, + /// Number of cached simplex vertices (0 to 3). + pub count: u8, + /// Cached support indices on the first proxy. + pub index_a: [u32; 3], + /// Cached support indices on the second proxy. + pub index_b: [u32; 3], +} + +/// Result of [`proxy_distance`]. All geometric quantities are expressed in the local frame of +/// the first proxy. +#[derive(Copy, Clone, Debug, Default)] +pub struct ProxyDistanceOutput { + /// Closest point on the first proxy (frame A). + pub point_a: Vector, + /// Closest point on the second proxy (frame A). + pub point_b: Vector, + /// Separation direction pointing from A to B (frame A). Zero if overlapped. + pub normal: Vector, + /// Distance between the two proxies (0 if overlapped). + pub distance: Real, + /// Number of GJK iterations used. + pub iterations: u32, +} + +#[derive(Copy, Clone, Default)] +struct SimplexVertex { + wa: Vector, + wb: Vector, + w: Vector, + a: Real, + index_a: u32, + index_b: u32, +} + +fn cache_is_valid(cache: &SimplexCache, proxy_a: &ToiProxy, proxy_b: &ToiProxy) -> bool { + let (na, nb) = (proxy_a.points().len() as u32, proxy_b.points().len() as u32); + cache.count <= 3 + && cache.index_a[..cache.count as usize].iter().all(|i| *i < na) + && cache.index_b[..cache.count as usize].iter().all(|i| *i < nb) +} + +/// Computes the distance between two point-cloud proxies, warm-started by `cache`. +/// +/// `pos12` is the pose of the second proxy relative to the first; the query runs entirely in +/// the first proxy’s local frame. When `use_radii` is `false` the proxy radii are ignored +/// (core-shape distance), which is what the time-of-impact loop uses. +#[cfg(feature = "dim2")] +pub fn proxy_distance( + pos12: &Pose, + proxy_a: &ToiProxy, + proxy_b: &ToiProxy, + use_radii: bool, + cache: &mut SimplexCache, +) -> ProxyDistanceOutput { + let points_a = proxy_a.points(); + let points_b = proxy_b.points(); + let point_b_in_a = |i: u32| pos12.transform_point(points_b[i as usize]); + + let mut output = ProxyDistanceOutput::default(); + + // Initialize the simplex from the cache. + let mut simplex = [SimplexVertex::default(); 3]; + let mut count = if cache_is_valid(cache, proxy_a, proxy_b) { + cache.count as usize + } else { + 0 + }; + for i in 0..count { + let v = &mut simplex[i]; + v.index_a = cache.index_a[i]; + v.index_b = cache.index_b[i]; + v.wa = points_a[v.index_a as usize]; + v.wb = point_b_in_a(v.index_b); + v.w = v.wa - v.wb; + // Invalid coefficient; set by the simplex solvers. + v.a = -1.0; + } + + if count == 0 { + let v = &mut simplex[0]; + v.index_a = 0; + v.index_b = 0; + v.wa = points_a[0]; + v.wb = point_b_in_a(0); + v.w = v.wa - v.wb; + v.a = 1.0; + count = 1; + } + + let mut non_unit_normal = Vector::ZERO; + let mut save_a = [0u32; 3]; + let mut save_b = [0u32; 3]; + + // Main iteration loop. All computations are done in frame A. + const MAX_ITERATIONS: u32 = 20; + let mut iteration = 0; + while iteration < MAX_ITERATIONS { + // Copy simplex indices so we can identify duplicates. + let save_count = count; + for i in 0..save_count { + save_a[i] = simplex[i].index_a; + save_b[i] = simplex[i].index_b; + } + + let d = match count { + 1 => -simplex[0].w, + 2 => solve_simplex2(&mut simplex, &mut count), + 3 => solve_simplex3(&mut simplex, &mut count), + _ => unreachable!(), + }; + + // If we have 3 points, then the origin is in the corresponding triangle. + if count == 3 { + let (pa, pb) = witness_points(&simplex, count); + output.point_a = pa; + output.point_b = pb; + output.iterations = iteration; + return output; + } + + // Ensure the search direction is numerically fit; a degenerate direction means the + // origin is contained by a segment, i.e. the shapes are overlapped. + if d.dot(d) < Real::EPSILON * Real::EPSILON { + let (pa, pb) = witness_points(&simplex, count); + output.point_a = pa; + output.point_b = pb; + output.iterations = iteration; + return output; + } + + non_unit_normal = d; + + // Compute a tentative new simplex vertex using support points: + // support = support(a, d) - support(b, -d). + let index_a = proxy_a.support(d); + let index_b = proxy_b.support(pos12.rotation.inverse_transform_vector(-d)); + let vertex = &mut simplex[count]; + vertex.index_a = index_a; + vertex.wa = points_a[index_a as usize]; + vertex.index_b = index_b; + vertex.wb = point_b_in_a(index_b); + vertex.w = vertex.wa - vertex.wb; + + // Iteration count is equated to the number of support point calls. + iteration += 1; + + // Check for duplicate support points. This is the main termination criteria. + let duplicate = (0..save_count).any(|i| index_a == save_a[i] && index_b == save_b[i]); + if duplicate { + break; + } + + // New vertex is valid and needed. + count += 1; + } + + let normal = non_unit_normal.normalize_or_zero(); + let (pa, pb) = witness_points(&simplex, count); + output.normal = normal; + output.distance = pa.distance(pb); + output.point_a = pa; + output.point_b = pb; + output.iterations = iteration; + + // Cache the simplex. + cache.count = count as u8; + for i in 0..count { + cache.index_a[i] = simplex[i].index_a; + cache.index_b[i] = simplex[i].index_b; + } + + // Apply radii if requested. + if use_radii { + let radius_a = proxy_a.radius; + let radius_b = proxy_b.radius; + output.distance = (output.distance - radius_a - radius_b).max(0.0); + + // Keep closest points on perimeter even if overlapped, this way the points move + // smoothly. + output.point_a += radius_a * normal; + output.point_b -= radius_b * normal; + } + + output +} + +#[cfg(feature = "dim2")] +fn witness_points(simplex: &[SimplexVertex; 3], count: usize) -> (Vector, Vector) { + match count { + 1 => (simplex[0].wa, simplex[0].wb), + 2 => ( + simplex[0].a * simplex[0].wa + simplex[1].a * simplex[1].wa, + simplex[0].a * simplex[0].wb + simplex[1].a * simplex[1].wb, + ), + 3 => { + let pa = simplex[0].a * simplex[0].wa + + simplex[1].a * simplex[1].wa + + simplex[2].a * simplex[2].wa; + (pa, pa) + } + _ => unreachable!(), + } +} + +// Returns a vector pointing towards the origin, reducing the simplex if needed. +#[cfg(feature = "dim2")] +fn solve_simplex2(simplex: &mut [SimplexVertex; 3], count: &mut usize) -> Vector { + let w1 = simplex[0].w; + let w2 = simplex[1].w; + let e12 = w2 - w1; + + // w1 region + let d12_2 = -w1.dot(e12); + if d12_2 <= 0.0 { + // a2 <= 0, so we clamp it to 0 + simplex[0].a = 1.0; + *count = 1; + return -w1; + } + + // w2 region + let d12_1 = w2.dot(e12); + if d12_1 <= 0.0 { + // a1 <= 0, so we clamp it to 0 + simplex[1].a = 1.0; + *count = 1; + simplex[0] = simplex[1]; + return -w2; + } + + // Must be in e12 region. + let inv_d12 = 1.0 / (d12_1 + d12_2); + simplex[0].a = d12_1 * inv_d12; + simplex[1].a = d12_2 * inv_d12; + *count = 2; + // cross(cross(w1 + w2, e12), e12) + let s = (w1 + w2).perp_dot(e12); + Vector::new(-s * e12.y, s * e12.x) +} + +#[cfg(feature = "dim2")] +fn solve_simplex3(simplex: &mut [SimplexVertex; 3], count: &mut usize) -> Vector { + let w1 = simplex[0].w; + let w2 = simplex[1].w; + let w3 = simplex[2].w; + + // Edge12: a3 = 0 + let e12 = w2 - w1; + let w1e12 = w1.dot(e12); + let w2e12 = w2.dot(e12); + let d12_1 = w2e12; + let d12_2 = -w1e12; + + // Edge13: a2 = 0 + let e13 = w3 - w1; + let w1e13 = w1.dot(e13); + let w3e13 = w3.dot(e13); + let d13_1 = w3e13; + let d13_2 = -w1e13; + + // Edge23: a1 = 0 + let e23 = w3 - w2; + let w2e23 = w2.dot(e23); + let w3e23 = w3.dot(e23); + let d23_1 = w3e23; + let d23_2 = -w2e23; + + // Triangle123 + let n123 = e12.perp_dot(e13); + let d123_1 = n123 * w2.perp_dot(w3); + let d123_2 = n123 * w3.perp_dot(w1); + let d123_3 = n123 * w1.perp_dot(w2); + + // w1 region + if d12_2 <= 0.0 && d13_2 <= 0.0 { + simplex[0].a = 1.0; + *count = 1; + return -w1; + } + + // e12 + if d12_1 > 0.0 && d12_2 > 0.0 && d123_3 <= 0.0 { + let inv_d12 = 1.0 / (d12_1 + d12_2); + simplex[0].a = d12_1 * inv_d12; + simplex[1].a = d12_2 * inv_d12; + *count = 2; + let s = (w1 + w2).perp_dot(e12); + return Vector::new(-s * e12.y, s * e12.x); + } + + // e13 + if d13_1 > 0.0 && d13_2 > 0.0 && d123_2 <= 0.0 { + let inv_d13 = 1.0 / (d13_1 + d13_2); + simplex[0].a = d13_1 * inv_d13; + simplex[2].a = d13_2 * inv_d13; + *count = 2; + simplex[1] = simplex[2]; + let s = (w1 + w3).perp_dot(e13); + return Vector::new(-s * e13.y, s * e13.x); + } + + // w2 region + if d12_1 <= 0.0 && d23_2 <= 0.0 { + simplex[1].a = 1.0; + *count = 1; + simplex[0] = simplex[1]; + return -w2; + } + + // w3 region + if d13_1 <= 0.0 && d23_1 <= 0.0 { + simplex[2].a = 1.0; + *count = 1; + simplex[0] = simplex[2]; + return -w3; + } + + // e23 + if d23_1 > 0.0 && d23_2 > 0.0 && d123_1 <= 0.0 { + let inv_d23 = 1.0 / (d23_1 + d23_2); + simplex[1].a = d23_1 * inv_d23; + simplex[2].a = d23_2 * inv_d23; + *count = 2; + simplex[0] = simplex[2]; + let s = (w2 + w3).perp_dot(e23); + return Vector::new(-s * e23.y, s * e23.x); + } + + // Must be in triangle123 + let inv_d123 = 1.0 / (d123_1 + d123_2 + d123_3); + simplex[0].a = d123_1 * inv_d123; + simplex[1].a = d123_2 * inv_d123; + simplex[2].a = d123_3 * inv_d123; + *count = 3; + + // No search direction + Vector::ZERO +} + +// ==================== 3D ==================== + +#[cfg(feature = "dim3")] +const MAX_GJK_ITERATIONS: u32 = 32; + +#[cfg(feature = "dim3")] +fn barycentric_coords_edge(a: Vector, b: Vector) -> [Real; 3] { + let ab = b - a; + // Last element is divisor + [b.dot(ab), -a.dot(ab), ab.dot(ab)] +} + +#[cfg(feature = "dim3")] +fn barycentric_coords_tri(a: Vector, b: Vector, c: Vector) -> [Real; 4] { + let ab = b - a; + let ac = c - a; + + let b_x_c = b.cross(c); + let c_x_a = c.cross(a); + let a_x_b = a.cross(b); + + let ab_x_ac = ab.cross(ac); + + // Last element is divisor + [ + b_x_c.dot(ab_x_ac), + c_x_a.dot(ab_x_ac), + a_x_b.dot(ab_x_ac), + ab_x_ac.dot(ab_x_ac), + ] +} + +#[cfg(feature = "dim3")] +fn scalar_triple_product(a: Vector, b: Vector, c: Vector) -> Real { + a.cross(b).dot(c) +} + +#[cfg(feature = "dim3")] +fn barycentric_coords_tet(a: Vector, b: Vector, c: Vector, d: Vector) -> [Real; 5] { + let ab = b - a; + let ac = c - a; + let ad = d - a; + + // Last element is divisor (forced to be positive) + let divisor = scalar_triple_product(ab, ac, ad); + let sign = if divisor < 0.0 { -1.0 } else { 1.0 }; + + [ + sign * scalar_triple_product(b, c, d), + sign * scalar_triple_product(a, d, c), + sign * scalar_triple_product(a, b, d), + sign * scalar_triple_product(a, c, b), + sign * divisor, + ] +} + +#[cfg(feature = "dim3")] +fn simplex_metric(simplex: &[SimplexVertex; 4], count: usize) -> Real { + match count { + 1 => 0.0, + 2 => simplex[0].w.distance(simplex[1].w), + 3 => { + let a = simplex[0].w; + let b = simplex[1].w; + let c = simplex[2].w; + (b - a).cross(c - a).length() / 2.0 + } + 4 => { + let a = simplex[0].w; + let b = simplex[1].w; + let c = simplex[2].w; + let d = simplex[3].w; + scalar_triple_product(b - a, c - a, d - a) / 6.0 + } + _ => unreachable!(), + } +} + +#[cfg(feature = "dim3")] +fn witness_points3(simplex: &[SimplexVertex; 4], count: usize) -> (Vector, Vector) { + let vs = simplex; + match count { + 1 => (vs[0].wa, vs[0].wb), + 2 => ( + vs[0].a * vs[0].wa + vs[1].a * vs[1].wa, + vs[0].a * vs[0].wb + vs[1].a * vs[1].wb, + ), + 3 => ( + vs[0].a * vs[0].wa + vs[1].a * vs[1].wa + vs[2].a * vs[2].wa, + vs[0].a * vs[0].wb + vs[1].a * vs[1].wb + vs[2].a * vs[2].wb, + ), + 4 => { + // Force identical points and *zero* distance + let sum = vs[0].a * vs[0].wa + + vs[1].a * vs[1].wa + + vs[2].a * vs[2].wa + + vs[3].a * vs[3].wa; + (sum, sum) + } + _ => unreachable!(), + } +} + +/// Solves the 2-simplex. Returns `false` when the barycentric divisor degenerates. +#[cfg(feature = "dim3")] +fn solve_simplex2_3d(simplex: &mut [SimplexVertex; 4], count: &mut usize) -> bool { + let a = simplex[0].w; + let b = simplex[1].w; + let ab = b - a; + + let divisor = ab.dot(ab); + let u = b.dot(ab); + let v = -a.dot(ab); + + // V(A) + if v <= 0.0 { + *count = 1; + simplex[0].a = 1.0; + return true; + } + + // V(B) + if u <= 0.0 { + *count = 1; + simplex[0] = simplex[1]; + simplex[0].a = 1.0; + return true; + } + + // Edge region + if divisor <= 0.0 { + return false; + } + + let denominator = 1.0 / divisor; + simplex[0].a = denominator * u; + simplex[1].a = denominator * v; + true +} + +#[cfg(feature = "dim3")] +fn solve_simplex3_3d(simplex: &mut [SimplexVertex; 4], count: &mut usize) -> bool { + let v1 = simplex[0]; + let v2 = simplex[1]; + let v3 = simplex[2]; + + let w_ab = barycentric_coords_edge(v1.w, v2.w); + let w_bc = barycentric_coords_edge(v2.w, v3.w); + let w_ca = barycentric_coords_edge(v3.w, v1.w); + + // VR(A) + if w_ab[1] <= 0.0 && w_ca[0] <= 0.0 { + *count = 1; + simplex[0] = v1; + simplex[0].a = 1.0; + return true; + } + + // VR(B) + if w_bc[1] <= 0.0 && w_ab[0] <= 0.0 { + *count = 1; + simplex[0] = v2; + simplex[0].a = 1.0; + return true; + } + + // VR(C) + if w_ca[1] <= 0.0 && w_bc[0] <= 0.0 { + *count = 1; + simplex[0] = v3; + simplex[0].a = 1.0; + return true; + } + + let w_abc = barycentric_coords_tri(v1.w, v2.w, v3.w); + + // VR(AB) + if w_abc[2] <= 0.0 && w_ab[0] > 0.0 && w_ab[1] > 0.0 { + *count = 2; + simplex[0] = v1; + simplex[1] = v2; + let divisor = w_ab[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_ab[0] / divisor; + simplex[1].a = w_ab[1] / divisor; + return true; + } + + // VR(BC) + if w_abc[0] <= 0.0 && w_bc[0] > 0.0 && w_bc[1] > 0.0 { + *count = 2; + simplex[0] = v2; + simplex[1] = v3; + let divisor = w_bc[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_bc[0] / divisor; + simplex[1].a = w_bc[1] / divisor; + return true; + } + + // VR(CA) + if w_abc[1] <= 0.0 && w_ca[0] > 0.0 && w_ca[1] > 0.0 { + *count = 2; + simplex[0] = v3; + simplex[1] = v1; + let divisor = w_ca[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_ca[0] / divisor; + simplex[1].a = w_ca[1] / divisor; + return true; + } + + // Face region + let divisor = w_abc[3]; + if divisor <= 0.0 { + return false; + } + + // VR(ABC) + simplex[0].a = w_abc[0] / divisor; + simplex[1].a = w_abc[1] / divisor; + simplex[2].a = w_abc[2] / divisor; + true +} + +#[cfg(feature = "dim3")] +fn solve_simplex4_3d(simplex: &mut [SimplexVertex; 4], count: &mut usize) -> bool { + let vertex_a = simplex[0]; + let vertex_b = simplex[1]; + let vertex_c = simplex[2]; + let vertex_d = simplex[3]; + + let w_ab = barycentric_coords_edge(vertex_a.w, vertex_b.w); + let w_ac = barycentric_coords_edge(vertex_a.w, vertex_c.w); + let w_ad = barycentric_coords_edge(vertex_a.w, vertex_d.w); + let w_bc = barycentric_coords_edge(vertex_b.w, vertex_c.w); + let w_cd = barycentric_coords_edge(vertex_c.w, vertex_d.w); + let w_db = barycentric_coords_edge(vertex_d.w, vertex_b.w); + + // VR(A) + if w_ab[1] <= 0.0 && w_ac[1] <= 0.0 && w_ad[1] <= 0.0 { + *count = 1; + simplex[0] = vertex_a; + simplex[0].a = 1.0; + return true; + } + + // VR(B) + if w_ab[0] <= 0.0 && w_db[0] <= 0.0 && w_bc[1] <= 0.0 { + *count = 1; + simplex[0] = vertex_b; + simplex[0].a = 1.0; + return true; + } + + // VR(C) + if w_ac[0] <= 0.0 && w_bc[0] <= 0.0 && w_cd[1] <= 0.0 { + *count = 1; + simplex[0] = vertex_c; + simplex[0].a = 1.0; + return true; + } + + // VR(D) + if w_ad[0] <= 0.0 && w_cd[0] <= 0.0 && w_db[1] <= 0.0 { + *count = 1; + simplex[0] = vertex_d; + simplex[0].a = 1.0; + return true; + } + + let w_acb = barycentric_coords_tri(vertex_a.w, vertex_c.w, vertex_b.w); + let w_abd = barycentric_coords_tri(vertex_a.w, vertex_b.w, vertex_d.w); + let w_adc = barycentric_coords_tri(vertex_a.w, vertex_d.w, vertex_c.w); + let w_bcd = barycentric_coords_tri(vertex_b.w, vertex_c.w, vertex_d.w); + + // VR(AB) + if w_abd[2] <= 0.0 && w_acb[1] <= 0.0 && w_ab[0] > 0.0 && w_ab[1] > 0.0 { + *count = 2; + simplex[0] = vertex_a; + simplex[1] = vertex_b; + let divisor = w_ab[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_ab[0] / divisor; + simplex[1].a = w_ab[1] / divisor; + return true; + } + + // VR(AC) + if w_acb[2] <= 0.0 && w_adc[1] <= 0.0 && w_ac[0] > 0.0 && w_ac[1] > 0.0 { + *count = 2; + simplex[0] = vertex_a; + simplex[1] = vertex_c; + let divisor = w_ac[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_ac[0] / divisor; + simplex[1].a = w_ac[1] / divisor; + return true; + } + + // VR(AD) + if w_adc[2] <= 0.0 && w_abd[1] <= 0.0 && w_ad[0] > 0.0 && w_ad[1] > 0.0 { + *count = 2; + simplex[0] = vertex_a; + simplex[1] = vertex_d; + let divisor = w_ad[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_ad[0] / divisor; + simplex[1].a = w_ad[1] / divisor; + return true; + } + + // VR(BC) + if w_acb[0] <= 0.0 && w_bcd[2] <= 0.0 && w_bc[0] > 0.0 && w_bc[1] > 0.0 { + *count = 2; + simplex[0] = vertex_b; + simplex[1] = vertex_c; + let divisor = w_bc[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_bc[0] / divisor; + simplex[1].a = w_bc[1] / divisor; + return true; + } + + // VR(CD) + if w_adc[0] <= 0.0 && w_bcd[0] <= 0.0 && w_cd[0] > 0.0 && w_cd[1] > 0.0 { + *count = 2; + simplex[0] = vertex_c; + simplex[1] = vertex_d; + let divisor = w_cd[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_cd[0] / divisor; + simplex[1].a = w_cd[1] / divisor; + return true; + } + + // VR(DB) + if w_abd[0] <= 0.0 && w_bcd[1] <= 0.0 && w_db[0] > 0.0 && w_db[1] > 0.0 { + *count = 2; + simplex[0] = vertex_d; + simplex[1] = vertex_b; + let divisor = w_db[2]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_db[0] / divisor; + simplex[1].a = w_db[1] / divisor; + return true; + } + + let w_abcd = barycentric_coords_tet(vertex_a.w, vertex_b.w, vertex_c.w, vertex_d.w); + + // VR(ACB) + if w_abcd[3] < 0.0 && w_acb[0] > 0.0 && w_acb[1] > 0.0 && w_acb[2] > 0.0 { + *count = 3; + simplex[0] = vertex_a; + simplex[1] = vertex_c; + simplex[2] = vertex_b; + let divisor = w_acb[3]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_acb[0] / divisor; + simplex[1].a = w_acb[1] / divisor; + simplex[2].a = w_acb[2] / divisor; + return true; + } + + // VR(ABD) + if w_abcd[2] < 0.0 && w_abd[0] > 0.0 && w_abd[1] > 0.0 && w_abd[2] > 0.0 { + *count = 3; + simplex[0] = vertex_a; + simplex[1] = vertex_b; + simplex[2] = vertex_d; + let divisor = w_abd[3]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_abd[0] / divisor; + simplex[1].a = w_abd[1] / divisor; + simplex[2].a = w_abd[2] / divisor; + return true; + } + + // VR(ADC) + if w_abcd[1] < 0.0 && w_adc[0] > 0.0 && w_adc[1] > 0.0 && w_adc[2] > 0.0 { + *count = 3; + simplex[0] = vertex_a; + simplex[1] = vertex_d; + simplex[2] = vertex_c; + let divisor = w_adc[3]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_adc[0] / divisor; + simplex[1].a = w_adc[1] / divisor; + simplex[2].a = w_adc[2] / divisor; + return true; + } + + // VR(BCD) + if w_abcd[0] < 0.0 && w_bcd[0] > 0.0 && w_bcd[1] > 0.0 && w_bcd[2] > 0.0 { + *count = 3; + simplex[0] = vertex_b; + simplex[1] = vertex_c; + simplex[2] = vertex_d; + let divisor = w_bcd[3]; + if divisor <= 0.0 { + return false; + } + simplex[0].a = w_bcd[0] / divisor; + simplex[1].a = w_bcd[1] / divisor; + simplex[2].a = w_bcd[2] / divisor; + return true; + } + + // *** Inside tetrahedron *** + let divisor = w_abcd[4]; + if divisor <= 0.0 { + return false; + } + + // VR(ABCD) + simplex[0].a = w_abcd[0] / divisor; + simplex[1].a = w_abcd[1] / divisor; + simplex[2].a = w_abcd[2] / divisor; + simplex[3].a = w_abcd[3] / divisor; + true +} + +/// Computes the distance between two point-cloud proxies, warm-started by `cache`. +/// +/// `pos12` is the pose of the second proxy relative to the first; the query runs entirely in +/// the first proxy’s local frame. When `use_radii` is `false` the proxy radii are ignored +/// (core-shape distance), which is what the time-of-impact loop uses. +#[cfg(feature = "dim3")] +pub fn proxy_distance( + pos12: &Pose, + proxy_a: &ToiProxy, + proxy_b: &ToiProxy, + use_radii: bool, + cache: &mut SimplexCache, +) -> ProxyDistanceOutput { + let points_a = proxy_a.points(); + let points_b = proxy_b.points(); + let point_b_in_a = |i: u32| pos12.transform_point(points_b[i as usize]); + + let mut output = ProxyDistanceOutput::default(); + + // Compute initial simplex from cache. Note that in 3D the CSO points are w = wB - wA + // (the opposite of the 2D convention). + let mut simplex = [SimplexVertex::default(); 4]; + let mut count = if cache_is_valid(cache, proxy_a, proxy_b) { + cache.count as usize + } else { + 0 + }; + for i in 0..count { + let v = &mut simplex[i]; + v.index_a = cache.index_a[i]; + v.index_b = cache.index_b[i]; + v.wa = points_a[v.index_a as usize]; + v.wb = point_b_in_a(v.index_b); + v.w = v.wb - v.wa; + v.a = 0.0; + } + + // Compute the new simplex metric; if it is substantially different than the old metric + // flush the simplex. + if count > 0 { + let metric1 = cache.metric; + let metric2 = simplex_metric(&simplex, count); + if 2.0 * metric1 < metric2 || metric2 < 0.5 * metric1 || metric2 < Real::EPSILON { + count = 0; + } + } + + // If the cache is invalid or empty. + if count == 0 { + let v = &mut simplex[0]; + v.index_a = 0; + v.index_b = 0; + v.wa = points_a[0]; + v.wb = point_b_in_a(0); + v.w = v.wb - v.wa; + v.a = 0.0; + count = 1; + } + + let mut backup = simplex; + let mut backup_count = 0usize; + + // Keep track of squared distance. + let mut distance_sq = Real::MAX; + let mut normal = Vector::ZERO; + + // Run GJK. + let mut iteration = 0; + while iteration < MAX_GJK_ITERATIONS { + // Solve simplex. + let solved = match count { + 1 => { + simplex[0].a = 1.0; + true + } + 2 => solve_simplex2_3d(&mut simplex, &mut count), + 3 => solve_simplex3_3d(&mut simplex, &mut count), + 4 => solve_simplex4_3d(&mut simplex, &mut count), + _ => unreachable!(), + }; + + if !solved { + // No progress - reconstruct last simplex. + if backup_count == 0 { + break; + } + simplex = backup; + count = backup_count; + break; + } + + if count == 4 { + // Overlap + let (pa, pb) = witness_points3(&simplex, count); + output.point_a = pa; + output.point_b = pb; + output.iterations = iteration; + return output; + } + + // Assure distance progression. + let old_distance_sq = distance_sq; + + // Compute closest point. + let closest_point = match count { + 1 => simplex[0].w, + 2 => simplex[0].a * simplex[0].w + simplex[1].a * simplex[1].w, + 3 => { + simplex[0].a * simplex[0].w + + simplex[1].a * simplex[1].w + + simplex[2].a * simplex[2].w + } + _ => unreachable!(), + }; + + distance_sq = closest_point.dot(closest_point); + + if distance_sq >= old_distance_sq { + // No progress - reconstruct last simplex. + if backup_count == 0 { + break; + } + simplex = backup; + count = backup_count; + break; + } + + // Build new tentative support point. + let search_direction = match count { + 1 => -simplex[0].w, + 2 => { + // v = (AB x AO) x AB + let a = simplex[0].w; + let b = simplex[1].w; + let ab = b - a; + ab.cross(-a).cross(ab) + } + 3 => { + // v = AB x AC or v = AC x AB + let a = simplex[0].w; + let b = simplex[1].w; + let c = simplex[2].w; + let n = (b - a).cross(c - a); + if n.dot(a) < 0.0 { + n + } else { + -n + } + } + _ => unreachable!(), + }; + + if search_direction.length_squared() < 1000.0 * Real::MIN_POSITIVE { + // The origin is probably contained by a line segment or triangle. + // Thus the shapes are overlapped. + let (pa, pb) = witness_points3(&simplex, count); + output.point_a = pa; + output.point_b = pb; + output.iterations = iteration; + return output; + } + + normal = -search_direction; + + // Get new support points. + let index_a = proxy_a.support(-search_direction); + let support_a = points_a[index_a as usize]; + let index_b = proxy_b.support(pos12.rotation.inverse() * search_direction); + let support_b = point_b_in_a(index_b); + + // Save current simplex and add new vertex - this can fail if we detect cycling. + backup = simplex; + backup_count = count; + + // Check for duplicate support points. This is the main termination criteria. + let duplicate = + (0..count).any(|i| simplex[i].index_a == index_a && simplex[i].index_b == index_b); + if duplicate { + break; + } + + simplex[count].index_a = index_a; + simplex[count].index_b = index_b; + simplex[count].wa = support_a; + simplex[count].wb = support_b; + simplex[count].w = support_b - support_a; + count += 1; + + iteration += 1; + } + + let normal = normal.normalize_or_zero(); + if normal == Vector::ZERO { + // Treat as overlap. + output.iterations = iteration; + return output; + } + + // Build witness points and save cache. + let (pa, pb) = witness_points3(&simplex, count); + cache.metric = simplex_metric(&simplex, count); + cache.count = count.min(3) as u8; + for i in 0..count.min(3) { + cache.index_a[i] = simplex[i].index_a; + cache.index_b[i] = simplex[i].index_b; + } + + // Results stay in frame A. + output.point_a = pa; + output.point_b = pb; + output.distance = pa.distance(pb); + output.normal = normal; + output.iterations = iteration; + + // Apply radii if requested. + if use_radii { + let ra = proxy_a.radius; + let rb = proxy_b.radius; + output.distance = (output.distance - ra - rb).max(0.0); + + // Keep closest points on perimeter even if overlapped, this way the points move + // smoothly. + output.point_a += ra * normal; + output.point_b -= rb * normal; + } + + output +} diff --git a/src/query/sweep_toi/separation.rs b/src/query/sweep_toi/separation.rs new file mode 100644 index 00000000..cd45e618 --- /dev/null +++ b/src/query/sweep_toi/separation.rs @@ -0,0 +1,583 @@ +//! Separation functions used by the conservative-advancement time-of-impact loop. +//! +//! A separation function measures the signed separation of two swept proxies along a +//! progressively chosen axis, either re-picking the deepest support points +//! ([`SeparationFunction::find_min_separation`]) or holding witness features fixed for root +//! finding ([`SeparationFunction::evaluate`]). + +use super::proxy_distance::SimplexCache; +use super::sweep::{inv_rotate_vec, rotate_vec, Sweep}; +use super::toi_proxy::ToiProxy; +use crate::math::{Real, Vector}; + +/// Sentinel index meaning "no witness index" (face types keep a fixed local point instead). +pub(crate) const INVALID_INDEX: u32 = u32::MAX; + +#[cfg(feature = "dim2")] +#[derive(Copy, Clone, PartialEq)] +enum SeparationType { + Points, + FaceA, + FaceB, +} + +#[cfg(feature = "dim2")] +pub(crate) struct SeparationFunction<'a, 'b> { + proxy_a: &'a ToiProxy<'b>, + proxy_b: &'a ToiProxy<'b>, + sweep_a: Sweep, + sweep_b: Sweep, + local_point: Vector, + axis: Vector, + ty: SeparationType, +} + +#[cfg(feature = "dim2")] +impl<'a, 'b> SeparationFunction<'a, 'b> { + pub fn new( + cache: &SimplexCache, + proxy_a: &'a ToiProxy<'b>, + sweep_a: &Sweep, + proxy_b: &'a ToiProxy<'b>, + sweep_b: &Sweep, + _world_normal: Vector, + t1: Real, + ) -> Self { + let count = cache.count as usize; + debug_assert!(0 < count && count < 3); + + let xf_a = sweep_a.transform_at(t1); + let xf_b = sweep_b.transform_at(t1); + + if count == 1 { + let local_point_a = proxy_a.points()[cache.index_a[0] as usize]; + let local_point_b = proxy_b.points()[cache.index_b[0] as usize]; + let point_a = xf_a.transform_point(local_point_a); + let point_b = xf_b.transform_point(local_point_b); + return Self { + proxy_a, + proxy_b, + sweep_a: *sweep_a, + sweep_b: *sweep_b, + local_point: Vector::ZERO, + axis: (point_b - point_a).normalize_or_zero(), + ty: SeparationType::Points, + }; + } + + if cache.index_a[0] == cache.index_a[1] { + // Two points on B and one on A. + let local_point_b1 = proxy_b.points()[cache.index_b[0] as usize]; + let local_point_b2 = proxy_b.points()[cache.index_b[1] as usize]; + + // Perpendicular of the edge: cross(edge, 1.0). + let edge = local_point_b2 - local_point_b1; + let mut axis = Vector::new(edge.y, -edge.x).normalize_or_zero(); + let normal = rotate_vec(&xf_b.rotation, axis); + + let local_point = 0.5 * (local_point_b1 + local_point_b2); + let point_b = xf_b.transform_point(local_point); + + let local_point_a = proxy_a.points()[cache.index_a[0] as usize]; + let point_a = xf_a.transform_point(local_point_a); + + if (point_a - point_b).dot(normal) < 0.0 { + axis = -axis; + } + + Self { + proxy_a, + proxy_b, + sweep_a: *sweep_a, + sweep_b: *sweep_b, + local_point, + axis, + ty: SeparationType::FaceB, + } + } else { + // Two points on A and one or two points on B. + let local_point_a1 = proxy_a.points()[cache.index_a[0] as usize]; + let local_point_a2 = proxy_a.points()[cache.index_a[1] as usize]; + + let edge = local_point_a2 - local_point_a1; + let mut axis = Vector::new(edge.y, -edge.x).normalize_or_zero(); + let normal = rotate_vec(&xf_a.rotation, axis); + + let local_point = 0.5 * (local_point_a1 + local_point_a2); + let point_a = xf_a.transform_point(local_point); + + let local_point_b = proxy_b.points()[cache.index_b[0] as usize]; + let point_b = xf_b.transform_point(local_point_b); + + if (point_b - point_a).dot(normal) < 0.0 { + axis = -axis; + } + + Self { + proxy_a, + proxy_b, + sweep_a: *sweep_a, + sweep_b: *sweep_b, + local_point, + axis, + ty: SeparationType::FaceA, + } + } + } + + /// Finds the deepest support points at time `t` and returns their signed separation. + pub fn find_min_separation(&self, t: Real) -> (Real, u32, u32) { + let xf_a = self.sweep_a.transform_at(t); + let xf_b = self.sweep_b.transform_at(t); + + match self.ty { + SeparationType::Points => { + let axis_a = inv_rotate_vec(&xf_a.rotation, self.axis); + let axis_b = inv_rotate_vec(&xf_b.rotation, -self.axis); + + let index_a = self.proxy_a.support(axis_a); + let index_b = self.proxy_b.support(axis_b); + + let point_a = xf_a.transform_point(self.proxy_a.points()[index_a as usize]); + let point_b = xf_b.transform_point(self.proxy_b.points()[index_b as usize]); + + ((point_b - point_a).dot(self.axis), index_a, index_b) + } + SeparationType::FaceA => { + let normal = rotate_vec(&xf_a.rotation, self.axis); + let point_a = xf_a.transform_point(self.local_point); + + let axis_b = inv_rotate_vec(&xf_b.rotation, -normal); + let index_b = self.proxy_b.support(axis_b); + let point_b = xf_b.transform_point(self.proxy_b.points()[index_b as usize]); + + ((point_b - point_a).dot(normal), INVALID_INDEX, index_b) + } + SeparationType::FaceB => { + let normal = rotate_vec(&xf_b.rotation, self.axis); + let point_b = xf_b.transform_point(self.local_point); + + let axis_a = inv_rotate_vec(&xf_a.rotation, -normal); + let index_a = self.proxy_a.support(axis_a); + let point_a = xf_a.transform_point(self.proxy_a.points()[index_a as usize]); + + ((point_a - point_b).dot(normal), index_a, INVALID_INDEX) + } + } + } + + /// Evaluates the separation of fixed witness features at time `t`. + pub fn evaluate(&self, index_a: u32, index_b: u32, t: Real) -> Real { + let xf_a = self.sweep_a.transform_at(t); + let xf_b = self.sweep_b.transform_at(t); + + match self.ty { + SeparationType::Points => { + let point_a = xf_a.transform_point(self.proxy_a.points()[index_a as usize]); + let point_b = xf_b.transform_point(self.proxy_b.points()[index_b as usize]); + (point_b - point_a).dot(self.axis) + } + SeparationType::FaceA => { + let normal = rotate_vec(&xf_a.rotation, self.axis); + let point_a = xf_a.transform_point(self.local_point); + let point_b = xf_b.transform_point(self.proxy_b.points()[index_b as usize]); + (point_b - point_a).dot(normal) + } + SeparationType::FaceB => { + let normal = rotate_vec(&xf_b.rotation, self.axis); + let point_b = xf_b.transform_point(self.local_point); + let point_a = xf_a.transform_point(self.proxy_a.points()[index_a as usize]); + (point_a - point_b).dot(normal) + } + } + } + + /// Whether this function uses the edge/edge cross-product axis (3D only; always false in + /// 2D). + pub fn uses_edge_axis(&self) -> bool { + false + } + + /// Converts an edge-axis function to a fixed world axis (3D only; no-op in 2D). + pub fn force_fixed_axis(&mut self, _t: Real) {} +} + +// ==================== 3D ==================== + +#[cfg(feature = "dim3")] +#[derive(Copy, Clone, PartialEq)] +enum SeparationType { + Vertices, + Edges, + FaceA, + FaceB, +} + +#[cfg(feature = "dim3")] +pub(crate) struct SeparationFunction<'a, 'b> { + proxy_a: &'a ToiProxy<'b>, + proxy_b: &'a ToiProxy<'b>, + sweep_a: Sweep, + sweep_b: Sweep, + // These are associated with different bodies depending on the separation function type. + // It could be two local vectors/points on the same body (for example, both on bodyA). + witness1: Vector, + witness2: Vector, + ty: SeparationType, +} + +#[cfg(feature = "dim3")] +fn unique_count(count: usize, indices: &[u32; 3]) -> usize { + match count { + 1 => 1, + 2 => { + if indices[0] != indices[1] { + 2 + } else { + 1 + } + } + 3 => { + if indices[0] != indices[1] && indices[0] != indices[2] && indices[1] != indices[2] { + 3 + } else if indices[0] == indices[1] && indices[0] == indices[2] { + 1 + } else { + 2 + } + } + _ => unreachable!(), + } +} + +// This checks if the cross product of two edges switches direction over the sweep. +#[cfg(feature = "dim3")] +fn check_fast_edges( + sweep_a: &Sweep, + local_edge_a: Vector, + sweep_b: &Sweep, + local_edge_b: Vector, + axis0: Vector, +) -> bool { + // By taking the local witness axes we make sure that we get the correct orientations + // (e.g. if one axis was flipped)! + let xf_a2 = sweep_a.final_transform(); + let xf_b2 = sweep_b.final_transform(); + let edge_a = rotate_vec(&xf_a2.rotation, local_edge_a); + let edge_b = rotate_vec(&xf_b2.rotation, local_edge_b); + let axis = edge_a.cross(edge_b); + axis.dot(axis0) < 0.0 +} + +#[cfg(feature = "dim3")] +impl<'a, 'b> SeparationFunction<'a, 'b> { + pub fn new( + cache: &SimplexCache, + proxy_a: &'a ToiProxy<'b>, + sweep_a: &Sweep, + proxy_b: &'a ToiProxy<'b>, + sweep_b: &Sweep, + world_normal: Vector, + t1: Real, + ) -> Self { + let count = cache.count as usize; + debug_assert!(1 <= count && count <= 3); + + let mut index_a = cache.index_a; + let mut index_b = cache.index_b; + + let unique_count_a = unique_count(count, &index_a); + let unique_count_b = unique_count(count, &index_b); + + let xf_a1 = sweep_a.transform_at(t1); + let xf_b1 = sweep_b.transform_at(t1); + + let qa = xf_a1.rotation; + let qb = xf_b1.rotation; + + // Minimize round-off + let delta_p = xf_b1.translation - xf_a1.translation; + + let mut result = Self { + proxy_a, + proxy_b, + sweep_a: *sweep_a, + sweep_b: *sweep_b, + witness1: world_normal, + witness2: Vector::ZERO, + ty: SeparationType::Vertices, + }; + + match count { + 1 => { + // Witness is the world space direction + result.ty = SeparationType::Vertices; + result.witness1 = world_normal; + } + 2 => { + if unique_count_a == 2 && unique_count_b == 2 { + Self::init_edges( + &mut result, proxy_a, proxy_b, &index_a, &index_b, &qa, &qb, delta_p, + world_normal, 0.05, + ); + } else { + // Vertex versus edge, use world axis witness + result.ty = SeparationType::Vertices; + result.witness1 = world_normal; + } + } + 3 => { + if unique_count_a == 3 { + let va1 = proxy_a.points()[index_a[0] as usize]; + let va2 = proxy_a.points()[index_a[1] as usize]; + let va3 = proxy_a.points()[index_a[2] as usize]; + let mut local_axis_a = + (va2 - va1).cross(va3 - va1).normalize_or_zero(); + let axis_a = rotate_vec(&qa, local_axis_a); + + let local_point_a = (va1 + va2 + va3) / 3.0; + let local_point_b = proxy_b.points()[index_b[0] as usize]; + let delta = rotate_vec(&qb, local_point_b) - rotate_vec(&qa, local_point_a) + + delta_p; + + if delta.dot(axis_a) < 0.0 { + // Make axis point from A to B + local_axis_a = -local_axis_a; + } + + // Witness is the local plane of faceA + result.ty = SeparationType::FaceA; + result.witness1 = local_axis_a; + result.witness2 = local_point_a; + } else if unique_count_b == 3 { + let vb1 = proxy_b.points()[index_b[0] as usize]; + let vb2 = proxy_b.points()[index_b[1] as usize]; + let vb3 = proxy_b.points()[index_b[2] as usize]; + let mut local_axis_b = + (vb2 - vb1).cross(vb3 - vb1).normalize_or_zero(); + let axis_b = rotate_vec(&qb, local_axis_b); + + let local_point_a = proxy_a.points()[index_a[0] as usize]; + let local_point_b = (vb1 + vb2 + vb3) / 3.0; + let delta = rotate_vec(&qa, local_point_a) - rotate_vec(&qb, local_point_b) + - delta_p; + + if delta.dot(axis_b) < 0.0 { + // Make axis point from B to A + local_axis_b = -local_axis_b; + } + + // Witness is the local plane of faceB + result.ty = SeparationType::FaceB; + result.witness1 = local_axis_b; + result.witness2 = local_point_b; + } else { + debug_assert!(unique_count_a == 2 && unique_count_b == 2); + + if index_a[0] == index_a[1] { + // Make first two indices unique + index_a[1] = index_a[2]; + } + if index_b[0] == index_b[1] { + // Make first two indices unique + index_b[1] = index_b[2]; + } + + Self::init_edges( + &mut result, proxy_a, proxy_b, &index_a, &index_b, &qa, &qb, delta_p, + world_normal, 0.005, + ); + } + } + _ => unreachable!(), + } + + result + } + + #[allow(clippy::too_many_arguments)] + fn init_edges( + result: &mut Self, + proxy_a: &'a ToiProxy<'b>, + proxy_b: &'a ToiProxy<'b>, + index_a: &[u32; 3], + index_b: &[u32; 3], + qa: &crate::math::Rotation, + qb: &crate::math::Rotation, + delta_p: Vector, + world_normal: Vector, + parallel_tolerance: Real, + ) { + // Edge/Edge + let va1 = proxy_a.points()[index_a[0] as usize]; + let local_edge_a = (proxy_a.points()[index_a[1] as usize] - va1).normalize_or_zero(); + + let vb1 = proxy_b.points()[index_b[0] as usize]; + let mut local_edge_b = (proxy_b.points()[index_b[1] as usize] - vb1).normalize_or_zero(); + + let edge_a = rotate_vec(qa, local_edge_a); + let edge_b = rotate_vec(qb, local_edge_b); + + let mut axis = edge_a.cross(edge_b); + let length_squared = axis.length_squared(); + + // Skip near parallel edges: |e1 x e2| = sin(alpha) * |e1| * |e2| + let tolerance_squared = parallel_tolerance * parallel_tolerance; + if length_squared < tolerance_squared { + // The axis is not safe to normalize so we use a world axis instead! + result.ty = SeparationType::Vertices; + result.witness1 = world_normal; + return; + } + + let delta = rotate_vec(qb, vb1) - rotate_vec(qa, va1) + delta_p; + if delta.dot(axis) < 0.0 { + // Make axis point from A to B + axis = -axis; + local_edge_b = -local_edge_b; + } + + // Check for possible sign flip in edge/edge cross product + if check_fast_edges( + &result.sweep_a, + local_edge_a, + &result.sweep_b, + local_edge_b, + axis, + ) { + // Not safe to use local edges, fall back to initial world space axis instead + result.ty = SeparationType::Vertices; + result.witness1 = axis.normalize_or_zero(); + } else { + // Edge cross product is safe. This converges faster than a fixed axis. + result.ty = SeparationType::Edges; + result.witness1 = local_edge_a; + result.witness2 = local_edge_b; + } + } + + /// Finds the deepest support points at time `t` and returns their signed separation. + pub fn find_min_separation(&self, t: Real) -> (Real, u32, u32) { + let xf_a = self.sweep_a.transform_at(t); + let xf_b = self.sweep_b.transform_at(t); + + match self.ty { + SeparationType::Vertices => { + let axis = self.witness1; + + let local_axis_a = inv_rotate_vec(&xf_a.rotation, axis); + let local_axis_b = inv_rotate_vec(&xf_b.rotation, -axis); + + let index_a = self.proxy_a.support(local_axis_a); + let index_b = self.proxy_b.support(local_axis_b); + + let delta_p = xf_b.translation - xf_a.translation; + let local_point_a = self.proxy_a.points()[index_a as usize]; + let local_point_b = self.proxy_b.points()[index_b as usize]; + let delta = rotate_vec(&xf_b.rotation, local_point_b) + - rotate_vec(&xf_a.rotation, local_point_a) + + delta_p; + + (delta.dot(axis), index_a, index_b) + } + SeparationType::Edges => { + let edge_a = rotate_vec(&xf_a.rotation, self.witness1); + let edge_b = rotate_vec(&xf_b.rotation, self.witness2); + let axis = edge_a.cross(edge_b).normalize_or_zero(); + + let axis_a = inv_rotate_vec(&xf_a.rotation, axis); + let index_a = self.proxy_a.support(axis_a); + + let axis_b = inv_rotate_vec(&xf_b.rotation, axis); + let index_b = self.proxy_b.support(-axis_b); + + let delta_p = xf_b.translation - xf_a.translation; + let local_point_a = self.proxy_a.points()[index_a as usize]; + let local_point_b = self.proxy_b.points()[index_b as usize]; + let delta = rotate_vec(&xf_b.rotation, local_point_b) + - rotate_vec(&xf_a.rotation, local_point_a) + + delta_p; + + (delta.dot(axis), index_a, index_b) + } + SeparationType::FaceA => { + let normal = rotate_vec(&xf_a.rotation, self.witness1); + let point_a = xf_a.transform_point(self.witness2); + + let axis_b = inv_rotate_vec(&xf_b.rotation, normal); + let index_b = self.proxy_b.support(-axis_b); + let point_b = xf_b.transform_point(self.proxy_b.points()[index_b as usize]); + + ((point_b - point_a).dot(normal), INVALID_INDEX, index_b) + } + SeparationType::FaceB => { + let normal = rotate_vec(&xf_b.rotation, self.witness1); + + let axis_a = inv_rotate_vec(&xf_a.rotation, normal); + let index_a = self.proxy_a.support(-axis_a); + let point_a = xf_a.transform_point(self.proxy_a.points()[index_a as usize]); + + let point_b = xf_b.transform_point(self.witness2); + + ((point_a - point_b).dot(normal), index_a, INVALID_INDEX) + } + } + } + + /// Evaluates the separation of fixed witness features at time `t`. + pub fn evaluate(&self, index_a: u32, index_b: u32, t: Real) -> Real { + let xf_a = self.sweep_a.transform_at(t); + let xf_b = self.sweep_b.transform_at(t); + + match self.ty { + SeparationType::Vertices => { + let axis = self.witness1; + let point_a = xf_a.transform_point(self.proxy_a.points()[index_a as usize]); + let point_b = xf_b.transform_point(self.proxy_b.points()[index_b as usize]); + (point_b - point_a).dot(axis) + } + SeparationType::Edges => { + let edge_a = rotate_vec(&xf_a.rotation, self.witness1); + let edge_b = rotate_vec(&xf_b.rotation, self.witness2); + let axis = edge_a.cross(edge_b).normalize_or_zero(); + + let point_a = xf_a.transform_point(self.proxy_a.points()[index_a as usize]); + let point_b = xf_b.transform_point(self.proxy_b.points()[index_b as usize]); + (point_b - point_a).dot(axis) + } + SeparationType::FaceA => { + let axis = rotate_vec(&xf_a.rotation, self.witness1); + let point_a = xf_a.transform_point(self.witness2); + let point_b = xf_b.transform_point(self.proxy_b.points()[index_b as usize]); + (point_b - point_a).dot(axis) + } + SeparationType::FaceB => { + let axis = rotate_vec(&xf_b.rotation, self.witness1); + let point_a = xf_a.transform_point(self.proxy_a.points()[index_a as usize]); + let point_b = xf_b.transform_point(self.witness2); + (point_a - point_b).dot(axis) + } + } + } + + /// Whether this function uses the edge/edge cross-product axis. + pub fn uses_edge_axis(&self) -> bool { + self.ty == SeparationType::Edges + } + + /// Converts an edge-axis function to a frozen world axis evaluated at time `t`. + pub fn force_fixed_axis(&mut self, t: Real) { + debug_assert!(self.ty == SeparationType::Edges); + + let xf_a = self.sweep_a.transform_at(t); + let xf_b = self.sweep_b.transform_at(t); + + let edge_a = rotate_vec(&xf_a.rotation, self.witness1); + let edge_b = rotate_vec(&xf_b.rotation, self.witness2); + let axis = edge_a.cross(edge_b).normalize_or_zero(); + + self.ty = SeparationType::Vertices; + self.witness1 = axis; + self.witness2 = Vector::ZERO; + } +} diff --git a/src/query/sweep_toi/sweep.rs b/src/query/sweep_toi/sweep.rs new file mode 100644 index 00000000..22320dc9 --- /dev/null +++ b/src/query/sweep_toi/sweep.rs @@ -0,0 +1,110 @@ +use crate::math::{Pose, Real, Rotation, Vector}; + +/// Rotates a vector by a rotation. +#[inline] +pub(crate) fn rotate_vec(q: &Rotation, v: Vector) -> Vector { + #[cfg(feature = "dim2")] + { + q.transform_vector(v) + } + #[cfg(feature = "dim3")] + { + *q * v + } +} + +/// Rotates a vector by the inverse of a rotation. +#[inline] +pub(crate) fn inv_rotate_vec(q: &Rotation, v: Vector) -> Vector { + #[cfg(feature = "dim2")] + { + q.inverse_transform_vector(v) + } + #[cfg(feature = "dim3")] + { + q.inverse() * v + } +} + +/// Normalized linear interpolation between two rotations (3D: shortest arc). +#[inline] +pub(crate) fn nlerp(q1: &Rotation, q2: &Rotation, t: Real) -> Rotation { + #[cfg(feature = "dim2")] + { + q1.lerp(*q2, t).normalize() + } + #[cfg(feature = "dim3")] + { + let q1 = if q1.dot(*q2) < 0.0 { -*q1 } else { *q1 }; + (q1 * (1.0 - t) + *q2 * t).normalize() + } +} + +/// Describes the motion of a rigid body over a timestep as linear interpolation between two +/// endpoint poses: the center of mass moves on a straight line while the rotation is +/// interpolated with a normalized lerp (nlerp). +/// +/// This is a common motion model for continuous collision detection. It is exact +/// at both endpoints and a good approximation in between as long as the rotation delta stays +/// below ~45°. +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(serde::Serialize, serde::Deserialize))] +pub struct Sweep { + /// The center of mass expressed in the shape’s local frame. + pub local_center: Vector, + /// The world-space center of mass at the start of the sweep. + pub c1: Vector, + /// The world-space center of mass at the end of the sweep. + pub c2: Vector, + /// The rotation at the start of the sweep. + pub q1: Rotation, + /// The rotation at the end of the sweep. + pub q2: Rotation, +} + +impl Sweep { + /// Builds a sweep from the start and end poses of a shape’s local frame. + pub fn from_poses(start: &Pose, end: &Pose, local_center: Vector) -> Self { + Self { + local_center, + c1: start.transform_point(local_center), + c2: end.transform_point(local_center), + q1: start.rotation, + q2: end.rotation, + } + } + + /// A degenerate sweep holding the shape stationary at the given pose. + pub fn constant(pose: &Pose, local_center: Vector) -> Self { + Self::from_poses(pose, pose, local_center) + } + + /// The pose of the shape’s local frame at time `t ∈ [0, 1]`. + /// + /// The center of mass is lerped, the rotation is nlerped, and the local-frame origin is + /// recovered by un-shifting the local center. + pub fn transform_at(&self, t: Real) -> Pose { + let q = nlerp(&self.q1, &self.q2, t); + let p = self.c1.lerp(self.c2, t) - rotate_vec(&q, self.local_center); + Pose::from_parts(p, q) + } + + /// The pose of the shape’s local frame at the end of the sweep (`t = 1`), computed exactly. + pub fn final_transform(&self) -> Pose { + let p = self.c2 - rotate_vec(&self.q2, self.local_center); + Pose::from_parts(p, self.q2) + } + + /// Translates the entire sweep by `-origin`. + /// + /// Used to re-center the time-of-impact computation for better floating-point accuracy. + pub fn shifted(&self, origin: Vector) -> Self { + Self { + local_center: self.local_center, + c1: self.c1 - origin, + c2: self.c2 - origin, + q1: self.q1, + q2: self.q2, + } + } +} diff --git a/src/query/sweep_toi/sweep_toi.rs b/src/query/sweep_toi/sweep_toi.rs new file mode 100644 index 00000000..19183326 --- /dev/null +++ b/src/query/sweep_toi/sweep_toi.rs @@ -0,0 +1,437 @@ +//! Conservative-advancement time of impact on endpoint-interpolated sweeps. +//! +//! A GJK distance query (with a warm-started simplex cache) provides a separating axis, and a +//! push-back loop with a hybrid bisection/false-position root finder advances the earliest +//! time at which the shapes reach the target separation. + +use super::proxy_distance::{proxy_distance, SimplexCache}; +use super::separation::SeparationFunction; +use super::sweep::{rotate_vec, Sweep}; +use super::toi_proxy::ToiProxy; +use crate::math::{Real, Vector}; + +/// The outcome of a [`sweep_time_of_impact`] computation. +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +pub enum SweepToiStatus { + /// The shapes were overlapped at the start time; continuous collision gave up + /// (`fraction == 0`). + Overlapped, + /// The shapes reach the target separation at `fraction`. + Hit, + /// The shapes never come within the target separation over the sweep + /// (`fraction == max_fraction`). + Separated, + /// The root finder failed to converge; `fraction` holds the last safe time. + Failed, +} + +/// The result of a [`sweep_time_of_impact`] computation. +#[derive(Copy, Clone, Debug)] +pub struct SweepToiOutput { + /// How the computation terminated. + pub status: SweepToiStatus, + /// The normalized time of impact in `[0, max_fraction]`. + pub fraction: Real, + /// The averaged world-space hit point (only meaningful on `Hit`/`Failed`). + pub point: Vector, + /// The world-space separating axis at the hit time, pointing from the first shape to the + /// second (only meaningful on `Hit`/`Failed`). + pub normal: Vector, +} + +/// Computes the time of impact between two proxies following endpoint-interpolated sweeps. +/// +/// The shapes are advanced to the earliest time in `[0, max_fraction]` at which their +/// core-shape separation drops to `max(linear_slop, radius_a + radius_b - linear_slop)`. +/// `linear_slop` is typically 0.005 length units. +pub fn sweep_time_of_impact( + proxy_a: &ToiProxy, + sweep_a: &Sweep, + proxy_b: &ToiProxy, + sweep_b: &Sweep, + max_fraction: Real, + linear_slop: Real, +) -> SweepToiOutput { + let mut output = SweepToiOutput { + status: SweepToiStatus::Separated, + fraction: max_fraction, + point: Vector::ZERO, + normal: Vector::ZERO, + }; + + // Shift to the first sweep’s start center for better floating-point accuracy. + let origin = sweep_a.c1; + let sweep_a = sweep_a.shifted(origin); + let sweep_b = sweep_b.shifted(origin); + + #[cfg(feature = "dim2")] + let max_push_back_iterations = 8; // Maximum polygon vertex count. + #[cfg(feature = "dim3")] + let max_push_back_iterations = proxy_a.points().len() + proxy_b.points().len(); + + #[cfg(feature = "dim2")] + const MAX_DISTANCE_ITERATIONS: u32 = 20; + #[cfg(feature = "dim3")] + const MAX_DISTANCE_ITERATIONS: u32 = 25; + + let t_max = max_fraction; + + // Set up target distance and tolerance. + let total_radius = proxy_a.radius + proxy_b.radius; + let target = linear_slop.max(total_radius - linear_slop); + let tolerance = 0.25 * linear_slop; + debug_assert!(target > tolerance); + + let mut t1 = 0.0; + let mut distance_iterations = 0u32; + let mut cache = SimplexCache::default(); + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + loop { + // Get the distance between shapes. We can also use the results to get a separating + // axis. + let xf_a = sweep_a.transform_at(t1); + let xf_b = sweep_b.transform_at(t1); + let pos12 = xf_a.inv_mul(&xf_b); + let distance_output = proxy_distance(&pos12, proxy_a, proxy_b, false, &mut cache); + + // The distance query runs in frame A, project the witness data back to the (shifted) + // world. + let world_normal = rotate_vec(&xf_a.rotation, distance_output.normal); + let world_point_a = xf_a.transform_point(distance_output.point_a); + let world_point_b = xf_a.transform_point(distance_output.point_b); + + distance_iterations += 1; + + let averaged_hit_point = || { + let pa = world_point_a + proxy_a.radius * world_normal; + let pb = world_point_b - proxy_b.radius * world_normal; + 0.5 * (pa + pb) + origin + }; + + // If the shapes are overlapped, we give up on continuous collision. + if distance_output.distance <= 0.0 { + output.status = SweepToiStatus::Overlapped; + output.fraction = 0.0; + break; + } + + if distance_output.distance <= target + tolerance { + // Success! + output.status = SweepToiStatus::Hit; + output.point = averaged_hit_point(); + output.normal = world_normal; + output.fraction = t1; + break; + } + + // In 3D, check for slow progress before running the push-back loop… + #[cfg(feature = "dim3")] + if distance_iterations == MAX_DISTANCE_ITERATIONS { + // Progress too slow. This can happen when a capsule rotates around a triangle + // vertex. + output.status = SweepToiStatus::Failed; + output.fraction = t1; + output.point = averaged_hit_point(); + output.normal = world_normal; + break; + } + + // Initialize the separating axis. + let mut fcn = SeparationFunction::new( + &cache, + proxy_a, + &sweep_a, + proxy_b, + &sweep_b, + world_normal, + t1, + ); + + // Compute the TOI on the separating axis. We do this by successively resolving the + // deepest point. This loop is bounded by the number of vertices. + let mut done = false; + let mut t2 = t_max; + let mut push_back_iterations = 0; + loop { + // Find the deepest point at t2. Store the witness point indices. + let (mut s2, index_a, index_b) = fcn.find_min_separation(t2); + + // Is the final configuration separated? + if s2 - target > tolerance { + // Victory! + output.status = SweepToiStatus::Separated; + output.fraction = t_max; + done = true; + break; + } + + // Has the separation reached tolerance? + if s2 >= target - tolerance { + // Advance the sweeps. + t1 = t2; + break; + } + + // Compute the initial separation of the witness points. + let mut s1 = fcn.evaluate(index_a, index_b, t1); + + // Check for initial overlap. This might happen if the root finder runs out of + // iterations. + if s1 < target - tolerance { + output.status = SweepToiStatus::Failed; + output.fraction = t1; + done = true; + break; + } + + // Check for touching. + if s1 <= target + tolerance { + // Success! t1 should hold the TOI (could be 0.0). + output.status = SweepToiStatus::Hit; + output.point = averaged_hit_point(); + output.normal = world_normal; + output.fraction = t1; + done = true; + break; + } + + // Compute 1D root of: f(t) - target = 0. + let mut root_iteration_count = 0; + const MAX_ROOT_ITERATIONS: u32 = 50; + let mut a1 = t1; + let mut a2 = t2; + loop { + // Use a mix of false position and bisection. + let t = if root_iteration_count & 1 == 1 { + // False position to improve convergence. + a1 + (target - s1) * (a2 - a1) / (s2 - s1) + } else { + // Bisection to guarantee progress. + 0.5 * (a1 + a2) + }; + + root_iteration_count += 1; + + let s = fcn.evaluate(index_a, index_b, t); + + // Has the separation reached tolerance? + if (s - target).abs() <= tolerance { + // t2 holds a tentative value for t1. + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if s > target { + a1 = t; + s1 = s; + } else { + a2 = t; + s2 = s; + } + + if root_iteration_count == MAX_ROOT_ITERATIONS { + break; + } + } + + // Restart the inner loop if we have a failing edge case (3D edge/edge axis only). + if root_iteration_count == MAX_ROOT_ITERATIONS - 1 && fcn.uses_edge_axis() { + t2 = t_max; + fcn.force_fixed_axis(t1); + } + + push_back_iterations += 1; + if push_back_iterations == max_push_back_iterations { + break; + } + } + + if done { + break; + } + + // …while in 2D it is checked after the push-back loop, letting the last distance + // iteration still resolve an impact. + #[cfg(feature = "dim2")] + if distance_iterations == MAX_DISTANCE_ITERATIONS { + // Root finder got stuck. Semi-victory. + output.status = SweepToiStatus::Failed; + output.point = averaged_hit_point(); + output.normal = world_normal; + output.fraction = t1; + break; + } + } + + output +} + +#[cfg(test)] +mod tests { + use super::*; + use crate::math::{Pose, Rotation}; + + fn slop() -> Real { + 0.005 + } + + #[cfg(feature = "dim2")] + fn pose(x: Real, y: Real) -> Pose { + Pose::from_parts(Vector::new(x, y), Rotation::identity()) + } + #[cfg(feature = "dim3")] + fn pose(x: Real, y: Real) -> Pose { + Pose::from_parts(Vector::new(x, y, 0.0), Rotation::IDENTITY) + } + + fn ball_proxy(radius: Real) -> ToiProxy<'static> { + ToiProxy::point(Vector::ZERO, radius) + } + + fn cuboid_proxy(half_extent: Real) -> ToiProxy<'static> { + #[cfg(feature = "dim2")] + { + ToiProxy::from_array( + [ + Vector::new(-half_extent, -half_extent), + Vector::new(half_extent, -half_extent), + Vector::new(half_extent, half_extent), + Vector::new(-half_extent, half_extent), + ], + 0.0, + ) + } + #[cfg(feature = "dim3")] + { + let h = half_extent; + ToiProxy::from_array( + [ + Vector::new(-h, -h, -h), + Vector::new(h, -h, -h), + Vector::new(h, h, -h), + Vector::new(-h, h, -h), + Vector::new(-h, -h, h), + Vector::new(h, -h, h), + Vector::new(h, h, h), + Vector::new(-h, h, h), + ], + 0.0, + ) + } + } + + #[test] + fn ball_hits_static_box() { + // Static unit box at the origin; ball of radius 0.1 sweeping from x = -3 to x = +3. + let wall = cuboid_proxy(0.5); + let wall_sweep = Sweep::constant(&pose(0.0, 0.0), Vector::ZERO); + let ball = ball_proxy(0.1); + let ball_sweep = Sweep::from_poses(&pose(-3.0, 0.0), &pose(3.0, 0.0), Vector::ZERO); + + let result = sweep_time_of_impact(&wall, &wall_sweep, &ball, &ball_sweep, 1.0, slop()); + assert_eq!(result.status, SweepToiStatus::Hit); + + // The ball is stopped when its core (center) is `target = radius - slop` away from + // the box surface: center_x = -0.5 - (0.1 - 0.005), fraction = (start - x) / travel. + let expected = (3.0 - 0.5 - (0.1 - slop())) / 6.0; + assert!( + (result.fraction - expected).abs() < 0.001, + "fraction {} vs expected {expected}", + result.fraction + ); + } + + #[test] + fn ball_misses_box() { + // Ball sweeping parallel to the box, far away. + let wall = cuboid_proxy(0.5); + let wall_sweep = Sweep::constant(&pose(0.0, 0.0), Vector::ZERO); + let ball = ball_proxy(0.1); + let ball_sweep = Sweep::from_poses(&pose(-3.0, 5.0), &pose(3.0, 5.0), Vector::ZERO); + + let result = sweep_time_of_impact(&wall, &wall_sweep, &ball, &ball_sweep, 1.0, slop()); + assert_eq!(result.status, SweepToiStatus::Separated); + assert_eq!(result.fraction, 1.0); + } + + #[test] + fn overlapped_at_start_returns_fraction_zero() { + let wall = cuboid_proxy(0.5); + let wall_sweep = Sweep::constant(&pose(0.0, 0.0), Vector::ZERO); + let ball = ball_proxy(0.1); + let ball_sweep = Sweep::from_poses(&pose(0.0, 0.0), &pose(3.0, 0.0), Vector::ZERO); + + let result = sweep_time_of_impact(&wall, &wall_sweep, &ball, &ball_sweep, 1.0, slop()); + assert_eq!(result.status, SweepToiStatus::Overlapped); + assert_eq!(result.fraction, 0.0); + } + + #[test] + fn box_vs_box_face_impact() { + // Two unit boxes; one sweeps into the other along x. + let a = cuboid_proxy(0.5); + let a_sweep = Sweep::constant(&pose(0.0, 0.0), Vector::ZERO); + let b = cuboid_proxy(0.5); + let b_sweep = Sweep::from_poses(&pose(-4.0, 0.0), &pose(4.0, 0.0), Vector::ZERO); + + let result = sweep_time_of_impact(&a, &a_sweep, &b, &b_sweep, 1.0, slop()); + assert_eq!(result.status, SweepToiStatus::Hit); + + // Faces meet when the gap reaches target = slop: center_x = -(0.5 + 0.5 + slop). + let expected = (4.0 - 1.0 - slop()) / 8.0; + assert!( + (result.fraction - expected).abs() < 0.001, + "fraction {} vs expected {expected}", + result.fraction + ); + } + + #[test] + fn rotating_bar_hits_ball() { + // A long thin bar rotating 90° about the origin must catch a ball placed along its + // swept arc even though the bar’s endpoint poses don’t overlap the ball. + let half_length = 2.0; + #[cfg(feature = "dim2")] + let (bar, start, end) = ( + ToiProxy::from_array( + [Vector::new(-half_length, 0.0), Vector::new(half_length, 0.0)], + 0.05, + ), + Pose::from_parts(Vector::ZERO, Rotation::identity()), + Pose::from_parts(Vector::ZERO, Rotation::from_angle(core::f32::consts::FRAC_PI_2 as Real)), + ); + #[cfg(feature = "dim3")] + let (bar, start, end) = ( + ToiProxy::from_array( + [ + Vector::new(-half_length, 0.0, 0.0), + Vector::new(half_length, 0.0, 0.0), + ], + 0.05, + ), + Pose::from_parts(Vector::ZERO, Rotation::IDENTITY), + Pose::from_parts( + Vector::ZERO, + Rotation::from_rotation_z(core::f32::consts::FRAC_PI_2 as Real), + ), + ); + + let bar_sweep = Sweep::from_poses(&start, &end, Vector::ZERO); + let ball = ball_proxy(0.1); + // Place the ball at 45° on the arc of radius 1.5. + let d = 1.5 * (0.5_f64.sqrt() as Real); + let ball_sweep = Sweep::constant(&pose(d, d), Vector::ZERO); + + let result = sweep_time_of_impact(&bar, &bar_sweep, &ball, &ball_sweep, 1.0, slop()); + assert_eq!(result.status, SweepToiStatus::Hit); + // The bar reaches 45° at t = 0.5; it should hit slightly before. + assert!( + result.fraction > 0.3 && result.fraction < 0.5, + "fraction {}", + result.fraction + ); + } +} diff --git a/src/query/sweep_toi/toi_proxy.rs b/src/query/sweep_toi/toi_proxy.rs new file mode 100644 index 00000000..6d5cfece --- /dev/null +++ b/src/query/sweep_toi/toi_proxy.rs @@ -0,0 +1,183 @@ +use crate::bounding_volume::Aabb; +use crate::math::{Pose, Real, Vector}; +use crate::shape::{Shape, TypedShape}; + +/// Maximum number of proxy points stored inline (cuboid corners). +#[cfg(feature = "dim2")] +pub const TOI_PROXY_INLINE_POINTS: usize = 4; +/// Maximum number of proxy points stored inline (cuboid corners). +#[cfg(feature = "dim3")] +pub const TOI_PROXY_INLINE_POINTS: usize = 8; + +enum ProxyPoints<'a> { + Inline([Vector; TOI_PROXY_INLINE_POINTS], u8), + Borrowed(&'a [Vector]), +} + +/// A point cloud with a radius, approximating a convex shape for sweep-based time-of-impact +/// computations. +/// +/// Only shapes that decompose exactly into a point cloud plus an inflation radius can be +/// represented (balls, capsules, segments, triangles, cuboids, convex polygons/polyhedra and +/// their round variants). Other shapes (cylinders, cones, half-spaces, composites, custom +/// shapes) return `None` from [`ToiProxy::from_shape`]. +pub struct ToiProxy<'a> { + points: ProxyPoints<'a>, + /// The inflation radius around the point cloud. + pub radius: Real, +} + +impl<'a> ToiProxy<'a> { + /// A proxy made of a single point with a radius. + pub fn point(point: Vector, radius: Real) -> Self { + let mut buf = [Vector::ZERO; TOI_PROXY_INLINE_POINTS]; + buf[0] = point; + Self { + points: ProxyPoints::Inline(buf, 1), + radius, + } + } + + /// A proxy borrowing its point cloud. + pub fn from_points(points: &'a [Vector], radius: Real) -> Self { + assert!(!points.is_empty()); + Self { + points: ProxyPoints::Borrowed(points), + radius, + } + } + + /// A proxy from an inline array of points. + pub fn from_array(points: [Vector; N], radius: Real) -> Self { + assert!(N > 0 && N <= TOI_PROXY_INLINE_POINTS); + let mut buf = [Vector::ZERO; TOI_PROXY_INLINE_POINTS]; + buf[..N].copy_from_slice(&points); + Self { + points: ProxyPoints::Inline(buf, N as u8), + radius, + } + } + + /// Extracts a proxy from a shape, if the shape decomposes into points + radius. + pub fn from_shape(shape: &'a dyn Shape) -> Option { + match shape.as_typed_shape() { + TypedShape::Ball(ball) => Some(Self::point(Vector::ZERO, ball.radius)), + TypedShape::Cuboid(cuboid) => Some(Self::from_cuboid_half_extents( + cuboid.half_extents, + 0.0, + )), + TypedShape::RoundCuboid(round) => Some(Self::from_cuboid_half_extents( + round.inner_shape.half_extents, + round.border_radius, + )), + TypedShape::Capsule(capsule) => Some(Self::from_array( + [capsule.segment.a, capsule.segment.b], + capsule.radius, + )), + TypedShape::Segment(segment) => Some(Self::from_array([segment.a, segment.b], 0.0)), + TypedShape::Triangle(tri) => Some(Self::from_array([tri.a, tri.b, tri.c], 0.0)), + TypedShape::RoundTriangle(round) => { + let tri = &round.inner_shape; + Some(Self::from_array( + [tri.a, tri.b, tri.c], + round.border_radius, + )) + } + #[cfg(feature = "dim2")] + #[cfg(feature = "alloc")] + TypedShape::ConvexPolygon(poly) => Some(Self::from_points(poly.points(), 0.0)), + #[cfg(feature = "dim2")] + #[cfg(feature = "alloc")] + TypedShape::RoundConvexPolygon(round) => Some(Self::from_points( + round.inner_shape.points(), + round.border_radius, + )), + #[cfg(feature = "dim3")] + #[cfg(feature = "alloc")] + TypedShape::ConvexPolyhedron(poly) => Some(Self::from_points(poly.points(), 0.0)), + #[cfg(feature = "dim3")] + #[cfg(feature = "alloc")] + TypedShape::RoundConvexPolyhedron(round) => Some(Self::from_points( + round.inner_shape.points(), + round.border_radius, + )), + _ => None, + } + } + + fn from_cuboid_half_extents(he: Vector, radius: Real) -> Self { + #[cfg(feature = "dim2")] + { + Self::from_array( + [ + Vector::new(-he.x, -he.y), + Vector::new(he.x, -he.y), + Vector::new(he.x, he.y), + Vector::new(-he.x, he.y), + ], + radius, + ) + } + #[cfg(feature = "dim3")] + { + Self::from_array( + [ + Vector::new(-he.x, -he.y, -he.z), + Vector::new(he.x, -he.y, -he.z), + Vector::new(he.x, he.y, -he.z), + Vector::new(-he.x, he.y, -he.z), + Vector::new(-he.x, -he.y, he.z), + Vector::new(he.x, -he.y, he.z), + Vector::new(he.x, he.y, he.z), + Vector::new(-he.x, he.y, he.z), + ], + radius, + ) + } + } + + /// The proxy’s point cloud. + #[inline] + pub fn points(&self) -> &[Vector] { + match &self.points { + ProxyPoints::Inline(buf, len) => &buf[..*len as usize], + ProxyPoints::Borrowed(points) => points, + } + } + + /// The index of the proxy point with the greatest projection along `direction`. + /// + /// Projections are measured relative to the first point for better accuracy far from the + /// origin. + #[inline] + pub fn support(&self, direction: Vector) -> u32 { + let points = self.points(); + let origin = points[0]; + let mut best_index = 0; + let mut best_value = 0.0; + for (i, pt) in points.iter().enumerate().skip(1) { + let value = direction.dot(*pt - origin); + if value > best_value { + best_index = i; + best_value = value; + } + } + best_index as u32 + } + + /// The axis-aligned bounding box of this proxy under the given pose. + pub fn compute_aabb(&self, pose: &Pose) -> Aabb { + let points = self.points(); + let mut mins = pose.transform_point(points[0]); + let mut maxs = mins; + for pt in &points[1..] { + let p = pose.transform_point(*pt); + mins = mins.min(p); + maxs = maxs.max(p); + } + Aabb::new( + mins - Vector::splat(self.radius), + maxs + Vector::splat(self.radius), + ) + } +} From 7c95a23e5780d157be88a1002fe62cddfa394009 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:30 +0200 Subject: [PATCH 05/12] feat: opt-in 8-lane SIMD (simd8 feature) --- crates/parry2d-f64/Cargo.toml | 3 +++ crates/parry2d/Cargo.toml | 3 +++ crates/parry3d-f64/Cargo.toml | 3 +++ crates/parry3d/Cargo.toml | 3 +++ src/lib.rs | 34 +++++++++++++++++++++++++++------- 5 files changed, 39 insertions(+), 7 deletions(-) diff --git a/crates/parry2d-f64/Cargo.toml b/crates/parry2d-f64/Cargo.toml index 3ca3955b..1d0ec945 100644 --- a/crates/parry2d-f64/Cargo.toml +++ b/crates/parry2d-f64/Cargo.toml @@ -46,6 +46,9 @@ rkyv = ["dep:rkyv", "glamx/rkyv"] bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] +# No-op for f64 (simba has no 8-lane f64 type); declared so the shared +# `src/lib.rs` cfg resolves. f64 SIMD stays 4-lane. +simd8 = [] enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm"] parallel = ["rayon"] alloc = ["hashbrown"] diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 3acfc63b..8c49ffc3 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -46,6 +46,9 @@ rkyv = ["dep:rkyv", "glamx/rkyv"] bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] +# Widens SIMD from 4 to 8 lanes (f32 only). Modifier on top of +# simd-stable/simd-nightly; needs an AVX-enabled target to emit 256-bit code. +simd8 = [] enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm"] parallel = ["rayon"] alloc = ["hashbrown", "smallvec", "downcast-rs", "glamx/approx"] diff --git a/crates/parry3d-f64/Cargo.toml b/crates/parry3d-f64/Cargo.toml index 04b9c959..98921072 100644 --- a/crates/parry3d-f64/Cargo.toml +++ b/crates/parry3d-f64/Cargo.toml @@ -44,6 +44,9 @@ rkyv = ["dep:rkyv", "glamx/rkyv"] bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] +# No-op for f64 (simba has no 8-lane f64 type); declared so the shared +# `src/lib.rs` cfg resolves. f64 SIMD stays 4-lane. +simd8 = [] enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm", "glamx/scalar-math"] parallel = ["rayon"] # Adds `TriMesh:to_obj_file` function. diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index 03d0b9e4..aa6248c6 100644 --- a/crates/parry3d/Cargo.toml +++ b/crates/parry3d/Cargo.toml @@ -45,6 +45,9 @@ bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/portable_simd", "simd-is-enabled"] +# Widens SIMD from 4 to 8 lanes (f32 only). Modifier on top of +# simd-stable/simd-nightly; needs an AVX-enabled target to emit 256-bit code. +simd8 = [] enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm", "glamx/scalar-math"] parallel = ["rayon"] # Adds `TriMesh:to_obj_file` function. diff --git a/src/lib.rs b/src/lib.rs index 5b8177fa..a343ef3e 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -41,11 +41,10 @@ macro_rules! array( { #[inline(always)] #[allow(dead_code)] - fn create_arr(mut callback: impl FnMut(usize) -> T) -> [T; SIMD_WIDTH] { - #[cfg(not(feature = "simd-is-enabled"))] - return [callback(0usize)]; - #[cfg(feature = "simd-is-enabled")] - return [callback(0usize), callback(1usize), callback(2usize), callback(3usize)]; + fn create_arr(callback: impl FnMut(usize) -> T) -> [T; SIMD_WIDTH] { + // Width-agnostic: `N` is inferred from the `[T; SIMD_WIDTH]` return type, + // so this covers the 1-, 4-, and 8-lane builds alike. + core::array::from_fn(callback) } create_arr($callback) @@ -105,18 +104,39 @@ mod simd { #[cfg(feature = "simd-is-enabled")] mod simd { - #[cfg(all(feature = "simd-nightly", feature = "f32"))] + // 8-lane SIMD (f32 only; simba has no `WideF64x8`). Opt-in via `simd8` + // on top of `simd-stable`/`simd-nightly`. Requires an AVX-enabled target + // (`RUSTFLAGS="-C target-feature=+avx2,+fma"` or `-C target-cpu=native`) for + // the compiler to actually emit 256-bit instructions; otherwise it runs + // (correctly) as two 128-bit halves. + #[cfg(all(feature = "simd8", feature = "simd-nightly", feature = "f32"))] + pub use simba::simd::{f32x8 as SimdReal, mask32x8 as SimdBool}; + #[cfg(all(feature = "simd8", feature = "simd-stable", feature = "f32"))] + pub use simba::simd::{WideBoolF32x8 as SimdBool, WideF32x8 as SimdReal}; + + // 4-lane SIMD (default). + #[cfg(all(not(feature = "simd8"), feature = "simd-nightly", feature = "f32"))] pub use simba::simd::{f32x4 as SimdReal, mask32x4 as SimdBool}; - #[cfg(all(feature = "simd-stable", feature = "f32"))] + #[cfg(all(not(feature = "simd8"), feature = "simd-stable", feature = "f32"))] pub use simba::simd::{WideBoolF32x4 as SimdBool, WideF32x4 as SimdReal}; + // f64 stays 4-lane regardless of `simd8` (no 8-lane f64 type in simba). #[cfg(all(feature = "simd-nightly", feature = "f64"))] pub use simba::simd::{f64x4 as SimdReal, mask64x4 as SimdBool}; #[cfg(all(feature = "simd-stable", feature = "f64"))] pub use simba::simd::{WideBoolF64x4 as SimdBool, WideF64x4 as SimdReal}; /// The number of lanes of a SIMD number. + #[cfg(all(feature = "simd8", feature = "f32"))] + pub const SIMD_WIDTH: usize = 8; + /// SIMD_WIDTH - 1 + #[cfg(all(feature = "simd8", feature = "f32"))] + pub const SIMD_LAST_INDEX: usize = 7; + + /// The number of lanes of a SIMD number. + #[cfg(not(all(feature = "simd8", feature = "f32")))] pub const SIMD_WIDTH: usize = 4; /// SIMD_WIDTH - 1 + #[cfg(not(all(feature = "simd8", feature = "f32")))] pub const SIMD_LAST_INDEX: usize = 3; } From fe29690d6517834eda5d714fd2622416ef27d798 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:31 +0200 Subject: [PATCH 06/12] chore: simplify BvhNode::merged --- src/partitioning/bvh/bvh_tree.rs | 39 ++++---------------------------- 1 file changed, 5 insertions(+), 34 deletions(-) diff --git a/src/partitioning/bvh/bvh_tree.rs b/src/partitioning/bvh/bvh_tree.rs index 89b623e6..843d4150 100644 --- a/src/partitioning/bvh/bvh_tree.rs +++ b/src/partitioning/bvh/bvh_tree.rs @@ -615,40 +615,11 @@ impl BvhNode { #[inline(always)] pub(super) fn merged(&self, other: &Self, children: u32) -> Self { - #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] - { - // Each node is two 16-byte rows: (mins, children) and (maxs, data). - // Min/max whole rows in one SIMD op each (the packed integer lanes - // produce garbage that is overwritten right after). This is the hot - // op of the refit passes, which rebuild every internal node. - let a = self.as_simd(); - let b = other.as_simd(); - let data = self.data.merged(other.data); - let mut out = Self { - mins: Vector::ZERO, - children, - maxs: Vector::ZERO, - data, - }; - { - let out_simd: &mut BvhNodeSimd = unsafe { core::mem::transmute(&mut out) }; - out_simd.mins = a.mins.min(b.mins); - out_simd.maxs = a.maxs.max(b.maxs); - } - // Restore the packed lanes clobbered by the row-wide min/max. - out.children = children; - out.data = data; - out - } - - #[cfg(not(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32")))] - { - Self { - mins: self.mins.min(other.mins), - children, - maxs: self.maxs.max(other.maxs), - data: self.data.merged(other.data), - } + Self { + mins: self.mins.min(other.mins), + children, + maxs: self.maxs.max(other.maxs), + data: self.data.merged(other.data), } } From 432249a617406bfa92f0c6285ec9ea29cdfc13b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:32 +0200 Subject: [PATCH 07/12] =?UTF-8?q?feat:=E2=80=AFstart=20unifying=20simd/non?= =?UTF-8?q?-simd=20code=20paths?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crates/parry2d-f64/Cargo.toml | 2 +- crates/parry2d/Cargo.toml | 2 +- src/lib.rs | 33 ++++++++------------------------- src/utils/mod.rs | 1 - src/utils/wops.rs | 3 --- 5 files changed, 10 insertions(+), 31 deletions(-) diff --git a/crates/parry2d-f64/Cargo.toml b/crates/parry2d-f64/Cargo.toml index 1d0ec945..f324be0c 100644 --- a/crates/parry2d-f64/Cargo.toml +++ b/crates/parry2d-f64/Cargo.toml @@ -49,7 +49,7 @@ simd-nightly = ["simba/portable_simd", "simd-is-enabled"] # No-op for f64 (simba has no 8-lane f64 type); declared so the shared # `src/lib.rs` cfg resolves. f64 SIMD stays 4-lane. simd8 = [] -enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm"] +enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm", "glamx/scalar-math"] parallel = ["rayon"] alloc = ["hashbrown"] spade = ["dep:spade", "alloc"] diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 8c49ffc3..7c78f193 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -49,7 +49,7 @@ simd-nightly = ["simba/portable_simd", "simd-is-enabled"] # Widens SIMD from 4 to 8 lanes (f32 only). Modifier on top of # simd-stable/simd-nightly; needs an AVX-enabled target to emit 256-bit code. simd8 = [] -enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm"] +enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm", "glamx/scalar-math"] parallel = ["rayon"] alloc = ["hashbrown", "smallvec", "downcast-rs", "glamx/approx"] spade = ["dep:spade", "alloc"] diff --git a/src/lib.rs b/src/lib.rs index a343ef3e..0a9f1326 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -29,12 +29,11 @@ the rust programming language. not(feature = "simd-nightly") ))] std::compile_error!("The `simd-is-enabled` feature should not be enabled explicitly. Please enable the `simd-stable` or the `simd-nightly` feature instead."); -#[cfg(all(feature = "simd-is-enabled", feature = "enhanced-determinism"))] -std::compile_error!( - "SIMD cannot be enabled when the `enhanced-determinism` feature is also enabled." +#[cfg(all(feature = "simd8", feature = "enhanced-determinism"))] +core::compile_error!( + "8-lanes SIMD cannot be enabled when the `enhanced-determinism` feature is also enabled because it breaks cross-platform determinism." ); -#[cfg(feature = "simd-is-enabled")] #[allow(unused_macros)] macro_rules! array( ($callback: expr; SIMD_WIDTH) => { @@ -83,26 +82,6 @@ pub mod shape; pub mod transformation; pub mod utils; -#[cfg(not(feature = "simd-is-enabled"))] -mod simd { - /// The number of lanes of a SIMD number. - pub const SIMD_WIDTH: usize = 1; - /// SIMD_WIDTH - 1 - pub const SIMD_LAST_INDEX: usize = 0; - - /// A SIMD float with SIMD_WIDTH lanes. - #[cfg(feature = "f32")] - pub type SimdReal = f32; - - /// A SIMD float with SIMD_WIDTH lanes. - #[cfg(feature = "f64")] - pub type SimdReal = f64; - - /// A SIMD bool with SIMD_WIDTH lanes. - pub type SimdBool = bool; -} - -#[cfg(feature = "simd-is-enabled")] mod simd { // 8-lane SIMD (f32 only; simba has no `WideF64x8`). Opt-in via `simd8` // on top of `simd-stable`/`simd-nightly`. Requires an AVX-enabled target @@ -114,15 +93,19 @@ mod simd { #[cfg(all(feature = "simd8", feature = "simd-stable", feature = "f32"))] pub use simba::simd::{WideBoolF32x8 as SimdBool, WideF32x8 as SimdReal}; - // 4-lane SIMD (default). + // 4-lane SIMD (default width). #[cfg(all(not(feature = "simd8"), feature = "simd-nightly", feature = "f32"))] pub use simba::simd::{f32x4 as SimdReal, mask32x4 as SimdBool}; + #[cfg(all(not(feature = "simd-is-enabled"), feature = "f32"))] + pub use simba::simd::{AutoBoolx4 as SimdBool, AutoF32x4 as SimdReal}; #[cfg(all(not(feature = "simd8"), feature = "simd-stable", feature = "f32"))] pub use simba::simd::{WideBoolF32x4 as SimdBool, WideF32x4 as SimdReal}; // f64 stays 4-lane regardless of `simd8` (no 8-lane f64 type in simba). #[cfg(all(feature = "simd-nightly", feature = "f64"))] pub use simba::simd::{f64x4 as SimdReal, mask64x4 as SimdBool}; + #[cfg(all(not(feature = "simd-is-enabled"), feature = "f64"))] + pub use simba::simd::{AutoBoolx4 as SimdBool, AutoF64x4 as SimdReal}; #[cfg(all(feature = "simd-stable", feature = "f64"))] pub use simba::simd::{WideBoolF64x4 as SimdBool, WideF64x4 as SimdReal}; diff --git a/src/utils/mod.rs b/src/utils/mod.rs index c31b456d..12552ada 100644 --- a/src/utils/mod.rs +++ b/src/utils/mod.rs @@ -41,7 +41,6 @@ pub use self::sorted_pair::SortedPair; pub(crate) use self::spade::sanitize_spade_point; pub(crate) use self::wops::{WBasis, WCross, WSign}; -#[cfg(feature = "simd-is-enabled")] #[allow(unused_imports)] pub(crate) use self::wops::simd_swap; diff --git a/src/utils/wops.rs b/src/utils/wops.rs index da62fd68..535c5216 100644 --- a/src/utils/wops.rs +++ b/src/utils/wops.rs @@ -2,13 +2,11 @@ use crate::math::{Real, Vector2, Vector3}; -#[cfg(feature = "simd-is-enabled")] use { crate::math::{SimdBool, SimdReal}, simba::simd::SimdValue, }; -#[cfg(feature = "simd-is-enabled")] #[allow(dead_code)] /// Conditionally swaps each lanes of `a` with those of `b`. /// @@ -77,7 +75,6 @@ impl WSign for Vector3 { } } -#[cfg(feature = "simd-is-enabled")] impl WSign for SimdReal { fn copy_sign_to(self, to: SimdReal) -> SimdReal { use simba::simd::SimdRealField; From faefaddc72ccf67e9f165051e30bdc8771931042 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:33 +0200 Subject: [PATCH 08/12] chore: cargo fmt --- src/partitioning/bvh/bvh_insert.rs | 100 ++++++++++------ src/partitioning/bvh/bvh_tests.rs | 111 ++++++++++++++++++ ...ontact_manifolds_voxels_composite_shape.rs | 6 +- src/query/mod.rs | 4 +- src/query/sweep_toi/composite.rs | 3 +- src/query/sweep_toi/mod.rs | 4 +- src/query/sweep_toi/proxy_distance.rs | 14 ++- src/query/sweep_toi/separation.rs | 38 ++++-- src/query/sweep_toi/sweep.rs | 5 +- src/query/sweep_toi/sweep_toi.rs | 10 +- src/query/sweep_toi/toi_proxy.rs | 12 +- src/shape/cuboid.rs | 46 ++++++-- 12 files changed, 273 insertions(+), 80 deletions(-) diff --git a/src/partitioning/bvh/bvh_insert.rs b/src/partitioning/bvh/bvh_insert.rs index 3e8dc6b9..bd976520 100644 --- a/src/partitioning/bvh/bvh_insert.rs +++ b/src/partitioning/bvh/bvh_insert.rs @@ -1,5 +1,5 @@ -use super::BvhNode; use super::bvh_tree::{BvhNodeIndex, BvhNodeWide}; +use super::BvhNode; use crate::bounding_volume::{Aabb, BoundingVolume}; use crate::math::{Real, Vector}; use crate::partitioning::Bvh; @@ -232,28 +232,46 @@ impl Bvh { leaf_index: u32, change_detection_margin: Real, ) -> BvhLeafUpdateStatus { - if let Some(leaf) = self.leaf_node_indices.get(leaf_index as usize) { - let node = &mut self.nodes[*leaf]; + match self.update_partially_if_present(aabb, leaf_index, change_detection_margin) { + Some(status) => status, + None => { + self.insert_new_unchecked(aabb, leaf_index); + BvhLeafUpdateStatus::Inserted + } + } + } - if change_detection_margin > 0.0 { - if !node.contains_aabb(&aabb) { - node.mins = aabb.mins - Vector::splat(change_detection_margin); - node.maxs = aabb.maxs + Vector::splat(change_detection_margin); - node.data.set_change_pending(); - BvhLeafUpdateStatus::UpdatedInPlace - } else { - // The new AABB is still inside the leaf's fat AABB: the tree - // is left untouched. - BvhLeafUpdateStatus::Unchanged - } + /// [`Self::insert_or_update_partially`] restricted to leaves already in the tree: + /// returns `None`, leaving the tree untouched, when `leaf_index` has no leaf yet. + /// + /// Lets a caller apply every in-place update before any structural insertion — the + /// order [`Self::insert_or_update_batch_partially_parallel`] imposes, since its updates + /// run concurrently — without paying a second lookup to find out which updates are + /// insertions. + pub fn update_partially_if_present( + &mut self, + aabb: Aabb, + leaf_index: u32, + change_detection_margin: Real, + ) -> Option { + let leaf = *self.leaf_node_indices.get(leaf_index as usize)?; + let node = &mut self.nodes[leaf]; + + if change_detection_margin > 0.0 { + if !node.contains_aabb(&aabb) { + node.mins = aabb.mins - Vector::splat(change_detection_margin); + node.maxs = aabb.maxs + Vector::splat(change_detection_margin); + node.data.set_change_pending(); + Some(BvhLeafUpdateStatus::UpdatedInPlace) } else { - node.mins = aabb.mins; - node.maxs = aabb.maxs; - BvhLeafUpdateStatus::UpdatedInPlace + // The new AABB is still inside the leaf's fat AABB: the tree + // is left untouched. + Some(BvhLeafUpdateStatus::Unchanged) } } else { - self.insert_new_unchecked(aabb, leaf_index); - BvhLeafUpdateStatus::Inserted + node.mins = aabb.mins; + node.maxs = aabb.maxs; + Some(BvhLeafUpdateStatus::UpdatedInPlace) } } @@ -509,24 +527,38 @@ impl Bvh { leaf_index: u32, change_detection_margin: Real, ) -> BvhLeafUpdateStatus { - if let Some(leaf) = self.leaf_node_indices.get(leaf_index as usize) { - if self.nodes[*leaf].contains_aabb(&aabb) { - return BvhLeafUpdateStatus::Unchanged; + match self.reinsert_or_update_if_present(aabb, leaf_index, change_detection_margin) { + Some(status) => status, + None => { + self.insert_new_unchecked(aabb, leaf_index); + BvhLeafUpdateStatus::Inserted } + } + } - self.remove(leaf_index); - let fat_aabb = Aabb { - mins: aabb.mins - Vector::splat(change_detection_margin), - maxs: aabb.maxs + Vector::splat(change_detection_margin), - }; - // The new leaf is created with a pending change flag, exactly like an - // in-place update that escaped its previous fattened AABB. - self.insert_new_unchecked(fat_aabb, leaf_index); - BvhLeafUpdateStatus::UpdatedInPlace - } else { - self.insert_new_unchecked(aabb, leaf_index); - BvhLeafUpdateStatus::Inserted + /// [`Self::reinsert_or_update_with_change_detection`] restricted to leaves already in + /// the tree: returns `None`, leaving the tree untouched, when `leaf_index` has no leaf + /// yet. See [`Self::update_partially_if_present`]. + pub fn reinsert_or_update_if_present( + &mut self, + aabb: Aabb, + leaf_index: u32, + change_detection_margin: Real, + ) -> Option { + let leaf = *self.leaf_node_indices.get(leaf_index as usize)?; + if self.nodes[leaf].contains_aabb(&aabb) { + return Some(BvhLeafUpdateStatus::Unchanged); } + + self.remove(leaf_index); + let fat_aabb = Aabb { + mins: aabb.mins - Vector::splat(change_detection_margin), + maxs: aabb.maxs + Vector::splat(change_detection_margin), + }; + // The new leaf is created with a pending change flag, exactly like an + // in-place update that escaped its previous fattened AABB. + self.insert_new_unchecked(fat_aabb, leaf_index); + Some(BvhLeafUpdateStatus::UpdatedInPlace) } // Applies a tree rotation at the given `node` if this improves the SAH metric at that node. diff --git a/src/partitioning/bvh/bvh_tests.rs b/src/partitioning/bvh/bvh_tests.rs index 1b7c233e..d4e7d3b0 100644 --- a/src/partitioning/bvh/bvh_tests.rs +++ b/src/partitioning/bvh/bvh_tests.rs @@ -347,3 +347,114 @@ mod parallel_batch_update { } } } + +/// The contracts rapier's `parallel`-off ≡ `parallel`-on guarantee rests on: when rapier +/// picks the parallel variant of one of these passes, it must get the exact result the +/// sequential one would have produced — otherwise a build with the feature and a build +/// without it are two different simulations. +#[cfg(feature = "parallel")] +#[cfg(all(feature = "dim3", feature = "f32"))] +mod parallel_matches_sequential { + use crate::bounding_volume::Aabb; + use crate::math::Vector; + use crate::partitioning::{Bvh, BvhWorkspace}; + use alloc::vec::Vec; + + /// 17^3, past `refit_buffers_parallel`'s `SEQ_LEAF_THRESHOLD` so the parallel refit + /// really splits instead of delegating to the sequential one. + const SIDE: u32 = 17; + const LEAVES: u32 = SIDE * SIDE * SIDE; + + /// A jittered grid whose cells overlap their neighbours: the tree needs real internal + /// structure, and the traversal real pairs, for the comparisons to mean anything. + fn scene() -> Bvh { + let mut bvh = Bvh::new(); + for i in 0..LEAVES { + let (x, y, z) = (i % SIDE, (i / SIDE) % SIDE, i / (SIDE * SIDE)); + let jitter = (i % 7) as f32 * 0.03; + let mins = Vector::new(x as f32, y as f32, z as f32) * 1.0 + + Vector::new(jitter, jitter * 0.5, jitter * 0.25); + bvh.insert( + Aabb::new(mins.into(), (mins + Vector::new(1.5, 1.5, 1.5)).into()), + i, + ); + } + bvh + } + + /// Compares the node arrays themselves, not just the leaf AABBs: the refit rebuilds + /// them in depth-first order, and that order decides the traversal order downstream. + fn assert_same_nodes(seq: &Bvh, par: &Bvh) { + assert_eq!( + seq.nodes.len(), + par.nodes.len(), + "node array lengths differ after refit" + ); + for (i, (a, b)) in seq.nodes.iter().zip(par.nodes.iter()).enumerate() { + for (side, a, b) in [("left", &a.left, &b.left), ("right", &a.right, &b.right)] { + assert_eq!(a.aabb(), b.aabb(), "node {i} ({side}) aabb differs"); + assert_eq!( + a.leaf_count(), + b.leaf_count(), + "node {i} ({side}) leaf count differs" + ); + assert_eq!( + a.is_leaf(), + b.is_leaf(), + "node {i} ({side}) leafness differs" + ); + } + } + for i in 0..LEAVES { + assert_eq!( + seq.leaf_node(i).map(|n| n.aabb()), + par.leaf_node(i).map(|n| n.aabb()), + "leaf {i} differs after refit" + ); + } + } + + /// [`Bvh::refit_parallel`] documents that "only the work distribution differs". + #[test] + fn refit_parallel_matches_sequential() { + let (mut seq, mut par) = (scene(), scene()); + let (mut w1, mut w2) = (BvhWorkspace::default(), BvhWorkspace::default()); + seq.refit(&mut w1); + par.refit_parallel(&mut w2); + assert_same_nodes(&seq, &par); + } + + /// Same, for the flag-preserving refit rapier runs in its deferred optimization pass. + #[test] + fn refit_without_resolve_parallel_matches_sequential() { + let (mut seq, mut par) = (scene(), scene()); + let (mut w1, mut w2) = (BvhWorkspace::default(), BvhWorkspace::default()); + seq.optimize_incremental(&mut w1); + par.optimize_incremental(&mut w2); + seq.refit_without_resolve(&mut w1); + par.refit_without_resolve_parallel(&mut w2); + assert_same_nodes(&seq, &par); + } + + /// [`Bvh::traverse_bvtt_single_tree_parallel`] documents that it returns the pairs "in + /// the exact same order as the calls the sequential traversal would have made". + #[test] + fn bvtt_traversal_parallel_matches_sequential() { + let mut bvh = scene(); + let mut workspace = BvhWorkspace::default(); + bvh.refit(&mut workspace); + + let mut sequential = Vec::new(); + bvh.traverse_bvtt_single_tree::(&mut workspace, &mut |a, b| sequential.push((a, b))); + let parallel = bvh.traverse_bvtt_single_tree_parallel::(); + + assert!( + !sequential.is_empty(), + "the scene must actually report pairs" + ); + assert_eq!( + sequential, parallel, + "the parallel BVTT traversal reported a different pair sequence" + ); + } +} diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs index 972ae620..af948a09 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_composite_shape.rs @@ -194,7 +194,8 @@ pub fn contact_manifolds_voxels_composite_shape( // Update contacts. if flipped { manifold.set_subshape_pos1(part_pos2.copied()); - manifold.set_subshape_pos2(Some(Pose::from_translation(canonical_center1))); + manifold + .set_subshape_pos2(Some(Pose::from_translation(canonical_center1))); let _ = dispatcher.contact_manifold_convex_convex( &relative_pos12.inverse(), part_shape2, @@ -205,7 +206,8 @@ pub fn contact_manifolds_voxels_composite_shape( manifold, ); } else { - manifold.set_subshape_pos1(Some(Pose::from_translation(canonical_center1))); + manifold + .set_subshape_pos1(Some(Pose::from_translation(canonical_center1))); manifold.set_subshape_pos2(part_pos2.copied()); let _ = dispatcher.contact_manifold_convex_convex( &relative_pos12, diff --git a/src/query/mod.rs b/src/query/mod.rs index b0af7bde..fed9909f 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -43,11 +43,11 @@ pub use self::query_dispatcher::{QueryDispatcher, QueryDispatcherChain}; pub use self::ray::{Ray, RayCast, RayIntersection, SimdRay}; pub use self::shape_cast::{cast_shapes, ShapeCastHit, ShapeCastOptions, ShapeCastStatus}; pub use self::split::{IntersectResult, SplitResult}; -#[cfg(feature = "alloc")] -pub use self::sweep_toi::{sweep_time_of_impact_composite, SweepCompositeFastShape}; pub use self::sweep_toi::{ sweep_time_of_impact, SimplexCache, Sweep, SweepToiOutput, SweepToiStatus, ToiProxy, }; +#[cfg(feature = "alloc")] +pub use self::sweep_toi::{sweep_time_of_impact_composite, SweepCompositeFastShape}; #[cfg(all(feature = "dim3", feature = "alloc"))] pub use self::ray::RayCullingMode; diff --git a/src/query/sweep_toi/composite.rs b/src/query/sweep_toi/composite.rs index b0bd1005..4da1be51 100644 --- a/src/query/sweep_toi/composite.rs +++ b/src/query/sweep_toi/composite.rs @@ -255,8 +255,7 @@ pub fn sweep_time_of_impact_composite( #[cfg(feature = "dim3")] heightfield.map_elements_in_local_aabb(&local_aabb, &mut |_, triangle| { if !context.one_sided_early_out(triangle) { - let proxy = - ToiProxy::from_array([triangle.a, triangle.b, triangle.c], 0.0); + let proxy = ToiProxy::from_array([triangle.a, triangle.b, triangle.c], 0.0); context.toi_against_element(&proxy, &composite_sweep); } }); diff --git a/src/query/sweep_toi/mod.rs b/src/query/sweep_toi/mod.rs index 91f1ab3e..da15456c 100644 --- a/src/query/sweep_toi/mod.rs +++ b/src/query/sweep_toi/mod.rs @@ -12,9 +12,7 @@ pub use self::sweep_toi::{sweep_time_of_impact, SweepToiOutput, SweepToiStatus}; pub use self::toi_proxy::{ToiProxy, TOI_PROXY_INLINE_POINTS}; #[cfg(feature = "alloc")] -pub use self::composite::{ - sweep_time_of_impact_composite, SweepCompositeFastShape, CORE_FRACTION, -}; +pub use self::composite::{sweep_time_of_impact_composite, SweepCompositeFastShape, CORE_FRACTION}; #[cfg(feature = "alloc")] mod composite; diff --git a/src/query/sweep_toi/proxy_distance.rs b/src/query/sweep_toi/proxy_distance.rs index f895e827..43271be5 100644 --- a/src/query/sweep_toi/proxy_distance.rs +++ b/src/query/sweep_toi/proxy_distance.rs @@ -49,8 +49,12 @@ struct SimplexVertex { fn cache_is_valid(cache: &SimplexCache, proxy_a: &ToiProxy, proxy_b: &ToiProxy) -> bool { let (na, nb) = (proxy_a.points().len() as u32, proxy_b.points().len() as u32); cache.count <= 3 - && cache.index_a[..cache.count as usize].iter().all(|i| *i < na) - && cache.index_b[..cache.count as usize].iter().all(|i| *i < nb) + && cache.index_a[..cache.count as usize] + .iter() + .all(|i| *i < na) + && cache.index_b[..cache.count as usize] + .iter() + .all(|i| *i < nb) } /// Computes the distance between two point-cloud proxies, warm-started by `cache`. @@ -444,10 +448,8 @@ fn witness_points3(simplex: &[SimplexVertex; 4], count: usize) -> (Vector, Vecto ), 4 => { // Force identical points and *zero* distance - let sum = vs[0].a * vs[0].wa - + vs[1].a * vs[1].wa - + vs[2].a * vs[2].wa - + vs[3].a * vs[3].wa; + let sum = + vs[0].a * vs[0].wa + vs[1].a * vs[1].wa + vs[2].a * vs[2].wa + vs[3].a * vs[3].wa; (sum, sum) } _ => unreachable!(), diff --git a/src/query/sweep_toi/separation.rs b/src/query/sweep_toi/separation.rs index cd45e618..e6088e24 100644 --- a/src/query/sweep_toi/separation.rs +++ b/src/query/sweep_toi/separation.rs @@ -317,8 +317,16 @@ impl<'a, 'b> SeparationFunction<'a, 'b> { 2 => { if unique_count_a == 2 && unique_count_b == 2 { Self::init_edges( - &mut result, proxy_a, proxy_b, &index_a, &index_b, &qa, &qb, delta_p, - world_normal, 0.05, + &mut result, + proxy_a, + proxy_b, + &index_a, + &index_b, + &qa, + &qb, + delta_p, + world_normal, + 0.05, ); } else { // Vertex versus edge, use world axis witness @@ -331,14 +339,13 @@ impl<'a, 'b> SeparationFunction<'a, 'b> { let va1 = proxy_a.points()[index_a[0] as usize]; let va2 = proxy_a.points()[index_a[1] as usize]; let va3 = proxy_a.points()[index_a[2] as usize]; - let mut local_axis_a = - (va2 - va1).cross(va3 - va1).normalize_or_zero(); + let mut local_axis_a = (va2 - va1).cross(va3 - va1).normalize_or_zero(); let axis_a = rotate_vec(&qa, local_axis_a); let local_point_a = (va1 + va2 + va3) / 3.0; let local_point_b = proxy_b.points()[index_b[0] as usize]; - let delta = rotate_vec(&qb, local_point_b) - rotate_vec(&qa, local_point_a) - + delta_p; + let delta = + rotate_vec(&qb, local_point_b) - rotate_vec(&qa, local_point_a) + delta_p; if delta.dot(axis_a) < 0.0 { // Make axis point from A to B @@ -353,14 +360,13 @@ impl<'a, 'b> SeparationFunction<'a, 'b> { let vb1 = proxy_b.points()[index_b[0] as usize]; let vb2 = proxy_b.points()[index_b[1] as usize]; let vb3 = proxy_b.points()[index_b[2] as usize]; - let mut local_axis_b = - (vb2 - vb1).cross(vb3 - vb1).normalize_or_zero(); + let mut local_axis_b = (vb2 - vb1).cross(vb3 - vb1).normalize_or_zero(); let axis_b = rotate_vec(&qb, local_axis_b); let local_point_a = proxy_a.points()[index_a[0] as usize]; let local_point_b = (vb1 + vb2 + vb3) / 3.0; - let delta = rotate_vec(&qa, local_point_a) - rotate_vec(&qb, local_point_b) - - delta_p; + let delta = + rotate_vec(&qa, local_point_a) - rotate_vec(&qb, local_point_b) - delta_p; if delta.dot(axis_b) < 0.0 { // Make axis point from B to A @@ -384,8 +390,16 @@ impl<'a, 'b> SeparationFunction<'a, 'b> { } Self::init_edges( - &mut result, proxy_a, proxy_b, &index_a, &index_b, &qa, &qb, delta_p, - world_normal, 0.005, + &mut result, + proxy_a, + proxy_b, + &index_a, + &index_b, + &qa, + &qb, + delta_p, + world_normal, + 0.005, ); } } diff --git a/src/query/sweep_toi/sweep.rs b/src/query/sweep_toi/sweep.rs index 22320dc9..c70b50e9 100644 --- a/src/query/sweep_toi/sweep.rs +++ b/src/query/sweep_toi/sweep.rs @@ -48,7 +48,10 @@ pub(crate) fn nlerp(q1: &Rotation, q2: &Rotation, t: Real) -> Rotation { /// at both endpoints and a good approximation in between as long as the rotation delta stays /// below ~45°. #[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr( + feature = "serde-serialize", + derive(serde::Serialize, serde::Deserialize) +)] pub struct Sweep { /// The center of mass expressed in the shape’s local frame. pub local_center: Vector, diff --git a/src/query/sweep_toi/sweep_toi.rs b/src/query/sweep_toi/sweep_toi.rs index 19183326..6e55024b 100644 --- a/src/query/sweep_toi/sweep_toi.rs +++ b/src/query/sweep_toi/sweep_toi.rs @@ -397,11 +397,17 @@ mod tests { #[cfg(feature = "dim2")] let (bar, start, end) = ( ToiProxy::from_array( - [Vector::new(-half_length, 0.0), Vector::new(half_length, 0.0)], + [ + Vector::new(-half_length, 0.0), + Vector::new(half_length, 0.0), + ], 0.05, ), Pose::from_parts(Vector::ZERO, Rotation::identity()), - Pose::from_parts(Vector::ZERO, Rotation::from_angle(core::f32::consts::FRAC_PI_2 as Real)), + Pose::from_parts( + Vector::ZERO, + Rotation::from_angle(core::f32::consts::FRAC_PI_2 as Real), + ), ); #[cfg(feature = "dim3")] let (bar, start, end) = ( diff --git a/src/query/sweep_toi/toi_proxy.rs b/src/query/sweep_toi/toi_proxy.rs index 6d5cfece..4d91f1e4 100644 --- a/src/query/sweep_toi/toi_proxy.rs +++ b/src/query/sweep_toi/toi_proxy.rs @@ -62,10 +62,9 @@ impl<'a> ToiProxy<'a> { pub fn from_shape(shape: &'a dyn Shape) -> Option { match shape.as_typed_shape() { TypedShape::Ball(ball) => Some(Self::point(Vector::ZERO, ball.radius)), - TypedShape::Cuboid(cuboid) => Some(Self::from_cuboid_half_extents( - cuboid.half_extents, - 0.0, - )), + TypedShape::Cuboid(cuboid) => { + Some(Self::from_cuboid_half_extents(cuboid.half_extents, 0.0)) + } TypedShape::RoundCuboid(round) => Some(Self::from_cuboid_half_extents( round.inner_shape.half_extents, round.border_radius, @@ -78,10 +77,7 @@ impl<'a> ToiProxy<'a> { TypedShape::Triangle(tri) => Some(Self::from_array([tri.a, tri.b, tri.c], 0.0)), TypedShape::RoundTriangle(round) => { let tri = &round.inner_shape; - Some(Self::from_array( - [tri.a, tri.b, tri.c], - round.border_radius, - )) + Some(Self::from_array([tri.a, tri.b, tri.c], round.border_radius)) } #[cfg(feature = "dim2")] #[cfg(feature = "alloc")] diff --git a/src/shape/cuboid.rs b/src/shape/cuboid.rs index 6c768068..132ec379 100644 --- a/src/shape/cuboid.rs +++ b/src/shape/cuboid.rs @@ -295,7 +295,7 @@ impl Cuboid { Vector::new(he.x, -he.y, he.z * sign), Vector::new(-he.x, -he.y, he.z * sign), Vector::new(-he.x, he.y, he.z * sign), - ] + ], }; pub fn vid(i: u32) -> u32 { @@ -311,15 +311,30 @@ impl Cuboid { let vids = match imax { 0 => { let sbit = sign_index << 2; - [vid(0b000 | sbit), vid(0b010 | sbit), vid(0b011 | sbit), vid(0b001 | sbit)] - }, + [ + vid(0b000 | sbit), + vid(0b010 | sbit), + vid(0b011 | sbit), + vid(0b001 | sbit), + ] + } 1 => { let sbit = sign_index << 1; - [vid(0b000 | sbit), vid(0b100 | sbit), vid(0b101 | sbit), vid(0b001 | sbit)] + [ + vid(0b000 | sbit), + vid(0b100 | sbit), + vid(0b101 | sbit), + vid(0b001 | sbit), + ] } _ => { let sbit = sign_index; - [vid(0b000 | sbit), vid(0b010 | sbit), vid(0b110 | sbit), vid(0b100 | sbit)] + [ + vid(0b000 | sbit), + vid(0b010 | sbit), + vid(0b110 | sbit), + vid(0b100 | sbit), + ] } }; @@ -330,15 +345,30 @@ impl Cuboid { let eids = match imax { 0 => { let sbits = (sign_index << 2) | (sign_index << 5); // 0b00_100_100 - [0b11_010_000 | sbits, 0b11_011_010 | sbits, 0b11_011_001 | sbits, 0b11_001_000 | sbits] + [ + 0b11_010_000 | sbits, + 0b11_011_010 | sbits, + 0b11_011_001 | sbits, + 0b11_001_000 | sbits, + ] } 1 => { let sbits = (sign_index << 1) | (sign_index << 4); // 0b00_010_010 - [0b11_100_000 | sbits, 0b11_101_100 | sbits, 0b11_101_001 | sbits, 0b11_001_000 | sbits] + [ + 0b11_100_000 | sbits, + 0b11_101_100 | sbits, + 0b11_101_001 | sbits, + 0b11_001_000 | sbits, + ] } _ => { let sbits = (sign_index << 0) | (sign_index << 3); // 0b00_001_001 - [0b11_010_000 | sbits, 0b11_110_010 | sbits, 0b11_110_100 | sbits, 0b11_100_000 | sbits] + [ + 0b11_010_000 | sbits, + 0b11_110_010 | sbits, + 0b11_110_100 | sbits, + 0b11_100_000 | sbits, + ] } }; From 1315bf34ac7c9c609ff63c6252b4deb4d100f103 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:38:34 +0200 Subject: [PATCH 09/12] =?UTF-8?q?feat:=E2=80=AFkeep=20simd4=20always=20ena?= =?UTF-8?q?bled=20-=20remove=20simd-stable/simd-nightly?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/parry-ci-build.yml | 6 ++--- crates/parry2d-f64/Cargo.toml | 10 ++------ crates/parry2d/Cargo.toml | 14 ++++-------- crates/parry3d-f64/Cargo.toml | 8 +------ crates/parry3d/Cargo.toml | 12 +++------- src/bounding_volume/mod.rs | 2 -- src/lib.rs | 34 ++++++++-------------------- src/partitioning/bvh/bvh_queries.rs | 8 +++---- src/partitioning/bvh/bvh_tree.rs | 26 ++++++++++----------- 9 files changed, 39 insertions(+), 81 deletions(-) diff --git a/.github/workflows/parry-ci-build.yml b/.github/workflows/parry-ci-build.yml index 3755c368..1b36bb57 100644 --- a/.github/workflows/parry-ci-build.yml +++ b/.github/workflows/parry-ci-build.yml @@ -35,10 +35,8 @@ jobs: run: cargo build --verbose -p parry2d; - name: Build parry3d run: cargo build --verbose -p parry3d; - - name: Build parry2d SIMD - run: cd crates/parry2d; cargo build --verbose --features simd-stable; - - name: Build parry3d SIMD - run: cd crates/parry3d; cargo build --verbose --features simd-stable; + - name: Build parry3d 8-lanes SIMD + run: cd crates/parry3d; cargo build --verbose --features simd8; - name: Check serialization run: cargo check --features bytemuck-serialize,serde-serialize,rkyv; - name: Check enhanced-determinism diff --git a/crates/parry2d-f64/Cargo.toml b/crates/parry2d-f64/Cargo.toml index f324be0c..a2c3e429 100644 --- a/crates/parry2d-f64/Cargo.toml +++ b/crates/parry2d-f64/Cargo.toml @@ -44,8 +44,6 @@ serde-serialize = [ ] rkyv = ["dep:rkyv", "glamx/rkyv"] bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] -simd-stable = ["simba/wide", "simd-is-enabled"] -simd-nightly = ["simba/portable_simd", "simd-is-enabled"] # No-op for f64 (simba has no 8-lane f64 type); declared so the shared # `src/lib.rs` cfg resolves. f64 SIMD stays 4-lane. simd8 = [] @@ -55,10 +53,6 @@ alloc = ["hashbrown"] spade = ["dep:spade", "alloc"] improved_fixed_point_support = [] -# Do not enable this feature directly. It is automatically -# enabled with the "simd-stable" or "simd-nightly" feature. -simd-is-enabled = [] - [lib] name = "parry2d_f64" path = "../../src/lib.rs" @@ -71,7 +65,7 @@ downcast-rs = { workspace = true } num-traits = { workspace = true } slab = { workspace = true, optional = true } arrayvec = { workspace = true } -simba = { workspace = true } +simba = { workspace = true, features = ["wide"] } glamx = { workspace = true, features = ["approx", "f64", "i64"] } approx = { workspace = true } serde = { workspace = true, optional = true } @@ -91,7 +85,7 @@ smallvec = { workspace = true } foldhash = { workspace = true } [dev-dependencies] -simba = { workspace = true } +simba = { workspace = true, features = ["wide"] } oorandom = { workspace = true } ptree = { workspace = true } rand = { workspace = true } diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index 7c78f193..3100b23e 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -44,10 +44,8 @@ serde-serialize = [ ] rkyv = ["dep:rkyv", "glamx/rkyv"] bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] -simd-stable = ["simba/wide", "simd-is-enabled"] -simd-nightly = ["simba/portable_simd", "simd-is-enabled"] -# Widens SIMD from 4 to 8 lanes (f32 only). Modifier on top of -# simd-stable/simd-nightly; needs an AVX-enabled target to emit 256-bit code. +# Widens SIMD from 4 to 8 lanes (f32 only). Needs an AVX-enabled target to +# emit 256-bit code. simd8 = [] enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm", "glamx/scalar-math"] parallel = ["rayon"] @@ -56,10 +54,6 @@ spade = ["dep:spade", "alloc"] improved_fixed_point_support = [] encase = [ "dep:encase", "glamx/encase" ] -# Do not enable this feature directly. It is automatically -# enabled with the "simd-stable" or "simd-nightly" feature. -simd-is-enabled = [] - [lib] name = "parry2d" path = "../../src/lib.rs" @@ -72,7 +66,7 @@ downcast-rs = { workspace = true, optional = true } num-traits = { workspace = true } slab = { workspace = true, optional = true } arrayvec = { workspace = true } -simba = { workspace = true } +simba = { workspace = true, features = ["wide"] } glamx = { workspace = true, features = ["i32"] } approx = { workspace = true } serde = { workspace = true, optional = true } @@ -93,7 +87,7 @@ foldhash = { workspace = true } encase = { workspace = true, optional = true } [dev-dependencies] -simba = { workspace = true } +simba = { workspace = true, features = ["wide"] } oorandom = { workspace = true } ptree = { workspace = true } rand = { workspace = true } diff --git a/crates/parry3d-f64/Cargo.toml b/crates/parry3d-f64/Cargo.toml index 98921072..427f95d4 100644 --- a/crates/parry3d-f64/Cargo.toml +++ b/crates/parry3d-f64/Cargo.toml @@ -42,8 +42,6 @@ serde-serialize = [ ] rkyv = ["dep:rkyv", "glamx/rkyv"] bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] -simd-stable = ["simba/wide", "simd-is-enabled"] -simd-nightly = ["simba/portable_simd", "simd-is-enabled"] # No-op for f64 (simba has no 8-lane f64 type); declared so the shared # `src/lib.rs` cfg resolves. f64 SIMD stays 4-lane. simd8 = [] @@ -55,10 +53,6 @@ alloc = ["hashbrown"] spade = ["dep:spade", "alloc"] improved_fixed_point_support = [] -# Do not enable this feature directly. It is automatically -# enabled with the "simd-stable" or "simd-nightly" feature. -simd-is-enabled = [] - [lib] name = "parry3d_f64" path = "../../src/lib.rs" @@ -71,7 +65,7 @@ downcast-rs = { workspace = true } num-traits = { workspace = true } slab = { workspace = true, optional = true } arrayvec = { workspace = true } -simba = { workspace = true } +simba = { workspace = true, features = ["wide"] } glamx = { workspace = true, features = ["approx", "f64", "i64"] } approx = { workspace = true } serde = { workspace = true, optional = true, features = ["rc"] } diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index aa6248c6..c23827e0 100644 --- a/crates/parry3d/Cargo.toml +++ b/crates/parry3d/Cargo.toml @@ -43,10 +43,8 @@ serde-serialize = [ rkyv = ["dep:rkyv", "glamx/rkyv"] bytemuck-serialize = ["bytemuck", "glamx/bytemuck"] -simd-stable = ["simba/wide", "simd-is-enabled"] -simd-nightly = ["simba/portable_simd", "simd-is-enabled"] -# Widens SIMD from 4 to 8 lanes (f32 only). Modifier on top of -# simd-stable/simd-nightly; needs an AVX-enabled target to emit 256-bit code. +# Widens SIMD from 4 to 8 lanes (f32 only). Needs an AVX-enabled target to +# emit 256-bit code. simd8 = [] enhanced-determinism = ["simba/libm_force", "indexmap", "glamx/libm", "glamx/scalar-math"] parallel = ["rayon"] @@ -57,10 +55,6 @@ spade = ["dep:spade", "alloc"] improved_fixed_point_support = [] encase = [ "dep:encase", "glamx/encase" ] -# Do not enable this feature directly. It is automatically -# enabled with the "simd-stable" or "simd-nightly" feature. -simd-is-enabled = [] - [lib] name = "parry3d" path = "../../src/lib.rs" @@ -73,7 +67,7 @@ downcast-rs = { workspace = true, optional = true } num-traits = { workspace = true } slab = { workspace = true, optional = true } arrayvec = { workspace = true } -simba = { workspace = true } +simba = { workspace = true, features = ["wide"] } glamx = { workspace = true, features = ["i32"] } # , "approx"] } approx = { workspace = true } serde = { workspace = true, optional = true, features = ["rc"] } diff --git a/src/bounding_volume/mod.rs b/src/bounding_volume/mod.rs index 142ebcc4..a379ff03 100644 --- a/src/bounding_volume/mod.rs +++ b/src/bounding_volume/mod.rs @@ -3,7 +3,6 @@ #[doc(inline)] pub use crate::bounding_volume::aabb::Aabb; -// #[cfg(feature = "simd-is-enabled")] // pub use crate::bounding_volume::simd_aabb::SimdAabb; #[doc(inline)] @@ -62,7 +61,6 @@ mod bounding_sphere_utils; #[cfg(feature = "alloc")] mod bounding_sphere_voxels; -// #[cfg(feature = "simd-is-enabled")] // mod simd_aabb; /// Free functions for some special cases of bounding-volume computation. diff --git a/src/lib.rs b/src/lib.rs index 0a9f1326..2bdfac68 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -23,12 +23,6 @@ the rust programming language. #![cfg_attr(feature = "dim3", doc(html_root_url = "https://docs.rs/parry3d"))] #![no_std] -#[cfg(all( - feature = "simd-is-enabled", - not(feature = "simd-stable"), - not(feature = "simd-nightly") -))] -std::compile_error!("The `simd-is-enabled` feature should not be enabled explicitly. Please enable the `simd-stable` or the `simd-nightly` feature instead."); #[cfg(all(feature = "simd8", feature = "enhanced-determinism"))] core::compile_error!( "8-lanes SIMD cannot be enabled when the `enhanced-determinism` feature is also enabled because it breaks cross-platform determinism." @@ -83,30 +77,22 @@ pub mod transformation; pub mod utils; mod simd { - // 8-lane SIMD (f32 only; simba has no `WideF64x8`). Opt-in via `simd8` - // on top of `simd-stable`/`simd-nightly`. Requires an AVX-enabled target - // (`RUSTFLAGS="-C target-feature=+avx2,+fma"` or `-C target-cpu=native`) for - // the compiler to actually emit 256-bit instructions; otherwise it runs - // (correctly) as two 128-bit halves. - #[cfg(all(feature = "simd8", feature = "simd-nightly", feature = "f32"))] - pub use simba::simd::{f32x8 as SimdReal, mask32x8 as SimdBool}; - #[cfg(all(feature = "simd8", feature = "simd-stable", feature = "f32"))] + // The `wide` types fall back to scalar code on targets without SIMD, so + // they are always used, whatever the platform. + + // 8-lane SIMD (f32 only; simba has no `WideF64x8`). Opt-in via `simd8`. + // Requires an AVX-enabled target (`RUSTFLAGS="-C target-feature=+avx2,+fma"` + // or `-C target-cpu=native`) for the compiler to actually emit 256-bit + // instructions; otherwise it runs (correctly) as two 128-bit halves. + #[cfg(all(feature = "simd8", feature = "f32"))] pub use simba::simd::{WideBoolF32x8 as SimdBool, WideF32x8 as SimdReal}; // 4-lane SIMD (default width). - #[cfg(all(not(feature = "simd8"), feature = "simd-nightly", feature = "f32"))] - pub use simba::simd::{f32x4 as SimdReal, mask32x4 as SimdBool}; - #[cfg(all(not(feature = "simd-is-enabled"), feature = "f32"))] - pub use simba::simd::{AutoBoolx4 as SimdBool, AutoF32x4 as SimdReal}; - #[cfg(all(not(feature = "simd8"), feature = "simd-stable", feature = "f32"))] + #[cfg(all(not(feature = "simd8"), feature = "f32"))] pub use simba::simd::{WideBoolF32x4 as SimdBool, WideF32x4 as SimdReal}; // f64 stays 4-lane regardless of `simd8` (no 8-lane f64 type in simba). - #[cfg(all(feature = "simd-nightly", feature = "f64"))] - pub use simba::simd::{f64x4 as SimdReal, mask64x4 as SimdBool}; - #[cfg(all(not(feature = "simd-is-enabled"), feature = "f64"))] - pub use simba::simd::{AutoBoolx4 as SimdBool, AutoF64x4 as SimdReal}; - #[cfg(all(feature = "simd-stable", feature = "f64"))] + #[cfg(feature = "f64")] pub use simba::simd::{WideBoolF64x4 as SimdBool, WideF64x4 as SimdReal}; /// The number of lanes of a SIMD number. diff --git a/src/partitioning/bvh/bvh_queries.rs b/src/partitioning/bvh/bvh_queries.rs index 5b915a92..a5e9317e 100644 --- a/src/partitioning/bvh/bvh_queries.rs +++ b/src/partitioning/bvh/bvh_queries.rs @@ -6,7 +6,7 @@ use crate::query::PointProjection; use crate::query::{PointQuery, Ray}; use crate::shape::FeatureId; -#[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] +#[cfg(all(feature = "dim3", feature = "f32"))] pub(super) struct SimdInvRay { // TODO: we need to use `glam` here instead of `wide` because `wide` is lacking // operations for getting the min/max vector element. @@ -15,7 +15,7 @@ pub(super) struct SimdInvRay { pub inv_dir: glamx::Vec3A, } -#[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] +#[cfg(all(feature = "dim3", feature = "f32"))] impl From for SimdInvRay { fn from(ray: Ray) -> Self { let inv_dir = ray.dir.map(|r| { @@ -256,7 +256,7 @@ impl Bvh { /// is assumed to map a leaf index to an actual geometry to cast a ray on. The `Real` argument /// given to that closure is the distance to the closest ray hit found so far (or is equal to /// `max_time_of_impact` if no projection was found so far). - #[cfg(not(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32")))] + #[cfg(not(all(feature = "dim3", feature = "f32")))] pub fn cast_ray( &self, ray: &Ray, @@ -276,7 +276,7 @@ impl Bvh { /// is assumed to map a leaf index to an actual geometry to cast a ray on. The `Real` argument /// given to that closure is the distance to the closest ray hit found so far (or is equal to /// `max_time_of_impact` if no projection was found so far). - #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] + #[cfg(all(feature = "dim3", feature = "f32"))] pub fn cast_ray( &self, ray: &Ray, diff --git a/src/partitioning/bvh/bvh_tree.rs b/src/partitioning/bvh/bvh_tree.rs index 843d4150..678a1beb 100644 --- a/src/partitioning/bvh/bvh_tree.rs +++ b/src/partitioning/bvh/bvh_tree.rs @@ -397,16 +397,16 @@ impl BvhNodeWide { } #[repr(C)] // SAFETY: needed to ensure SIMD aabb checks rely on the layout. -#[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] +#[cfg(all(feature = "dim3", feature = "f32"))] pub(super) struct BvhNodeSimd { mins: glamx::Vec3A, maxs: glamx::Vec3A, } // SAFETY: compile-time assertions to ensure we can transmute between `BvhNode` and `BvhNodeSimd`. -#[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] +#[cfg(all(feature = "dim3", feature = "f32"))] static_assertions::assert_eq_align!(BvhNode, BvhNodeSimd); -#[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] +#[cfg(all(feature = "dim3", feature = "f32"))] static_assertions::assert_eq_size!(BvhNode, BvhNodeSimd); /// A single node (internal or leaf) of a BVH. @@ -606,7 +606,7 @@ impl BvhNode { } #[inline(always)] - #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] + #[cfg(all(feature = "dim3", feature = "f32"))] pub(super) fn as_simd(&self) -> &BvhNodeSimd { // SAFETY: BvhNode is declared with the alignment // and size of two SimdReal. @@ -934,8 +934,8 @@ impl BvhNode { /// /// # Performance /// - /// When SIMD is enabled (3D, f32, simd-is-enabled feature), this uses vectorized - /// comparisons for improved performance. + /// In 3D with f32, this uses vectorized comparisons for improved + /// performance. /// /// # Example /// @@ -961,7 +961,7 @@ impl BvhNode { /// # See Also /// /// - [`contains`](Self::contains) - Check full containment - #[cfg(not(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32")))] + #[cfg(not(all(feature = "dim3", feature = "f32")))] pub fn intersects(&self, other: &Self) -> bool { self.mins.cmple(other.maxs).all() && self.maxs.cmpge(other.mins).all() } @@ -1007,7 +1007,7 @@ impl BvhNode { /// # See Also /// /// - [`contains`](Self::contains) - Check full containment - #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] + #[cfg(all(feature = "dim3", feature = "f32"))] pub fn intersects(&self, other: &Self) -> bool { let simd_self = self.as_simd(); let simd_other = other.as_simd(); @@ -1029,8 +1029,8 @@ impl BvhNode { /// /// # Performance /// - /// When SIMD is enabled (3D, f32, simd-is-enabled feature), this uses vectorized - /// comparisons for improved performance. + /// In 3D with f32, this uses vectorized comparisons for improved + /// performance. /// /// # Example /// @@ -1055,7 +1055,7 @@ impl BvhNode { /// /// - [`intersects`](Self::intersects) - Check any overlap /// - [`contains_aabb`](Self::contains_aabb) - Contains an `Aabb` directly - #[cfg(not(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32")))] + #[cfg(not(all(feature = "dim3", feature = "f32")))] pub fn contains(&self, other: &Self) -> bool { self.mins.cmple(other.mins).all() && self.maxs.cmpge(other.maxs).all() } @@ -1100,7 +1100,7 @@ impl BvhNode { /// /// - [`intersects`](Self::intersects) - Check any overlap /// - [`contains_aabb`](Self::contains_aabb) - Contains an `Aabb` directly - #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] + #[cfg(all(feature = "dim3", feature = "f32"))] pub fn contains(&self, other: &Self) -> bool { let simd_self = self.as_simd(); let simd_other = other.as_simd(); @@ -1193,7 +1193,7 @@ impl BvhNode { /// Casts a ray on this AABB, with SIMD optimizations. /// /// Returns `Real::MAX` if there is no hit. - #[cfg(all(feature = "simd-is-enabled", feature = "dim3", feature = "f32"))] + #[cfg(all(feature = "dim3", feature = "f32"))] pub(super) fn cast_inv_ray_simd(&self, ray: &super::bvh_queries::SimdInvRay) -> f32 { let simd_self = self.as_simd(); let t1 = (simd_self.mins - ray.origin) * ray.inv_dir; From 5901c443b5e4a3942168c3e67ebb81c28f5c9a28 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 16:46:24 +0200 Subject: [PATCH 10/12] =?UTF-8?q?chore:=E2=80=AFadd=20attribution?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/query/sweep_toi/mod.rs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/query/sweep_toi/mod.rs b/src/query/sweep_toi/mod.rs index da15456c..1ed8d32c 100644 --- a/src/query/sweep_toi/mod.rs +++ b/src/query/sweep_toi/mod.rs @@ -5,6 +5,9 @@ //! poses (linear center-of-mass interpolation + rotation nlerp) and computes the earliest //! time at which two swept shapes reach a slop-based target separation, using conservative //! advancement with separation functions. +//! +//! NOTE: this is mostly ported from Box2D which had much better CCD quality than Rapier. +//! TODO: see how we can combine that with `nonlinear_shape_cast` since they serve similar goals. pub use self::proxy_distance::{proxy_distance, ProxyDistanceOutput, SimplexCache}; pub use self::sweep::Sweep; From 528b32e1ad8922e5f59767bdb38022690ba99fe3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 17:01:32 +0200 Subject: [PATCH 11/12] =?UTF-8?q?chore:=E2=80=AFfmt/clippy=20fixes?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Cargo.toml | 2 +- src/partitioning/bvh/bvh_insert.rs | 2 +- src/partitioning/bvh/bvh_queries.rs | 2 +- src/partitioning/bvh/bvh_refit.rs | 2 +- src/partitioning/bvh/bvh_tree.rs | 4 ++-- src/query/sweep_toi/proxy_distance.rs | 18 ++++++++---------- src/shape/cuboid.rs | 3 +++ 7 files changed, 17 insertions(+), 16 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index b534c6ed..9541ee89 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -41,7 +41,7 @@ downcast-rs = { version = "2", default-features = false, features = ["sync"] } num-traits = { version = "0.2", default-features = false } slab = "0.4" arrayvec = { version = "0.7", default-features = false } -simba = { version = "0.10", default-features = false } +simba = { version = "0.10.1", default-features = false } glamx = { version = "0.3", default-features = false, features = ["nostd-libm"] } approx = { version = "0.5", default-features = false } serde = { version = "1.0", features = ["derive"] } diff --git a/src/partitioning/bvh/bvh_insert.rs b/src/partitioning/bvh/bvh_insert.rs index bd976520..e0b9ae67 100644 --- a/src/partitioning/bvh/bvh_insert.rs +++ b/src/partitioning/bvh/bvh_insert.rs @@ -245,7 +245,7 @@ impl Bvh { /// returns `None`, leaving the tree untouched, when `leaf_index` has no leaf yet. /// /// Lets a caller apply every in-place update before any structural insertion — the - /// order [`Self::insert_or_update_batch_partially_parallel`] imposes, since its updates + /// order `insert_or_update_batch_partially_parallel` imposes, since its updates /// run concurrently — without paying a second lookup to find out which updates are /// insertions. pub fn update_partially_if_present( diff --git a/src/partitioning/bvh/bvh_queries.rs b/src/partitioning/bvh/bvh_queries.rs index a5e9317e..d1123771 100644 --- a/src/partitioning/bvh/bvh_queries.rs +++ b/src/partitioning/bvh/bvh_queries.rs @@ -311,7 +311,7 @@ impl Bvh { self.find_best( max_time_of_impact, |node: &BvhNode, _best_so_far| node.cast_inv_ray_simd(&simd_inv_ray), - |primitive, best_so_far| primitive_check(primitive, best_so_far), + primitive_check, ) } } diff --git a/src/partitioning/bvh/bvh_refit.rs b/src/partitioning/bvh/bvh_refit.rs index 1ec3d2af..d62c32a1 100644 --- a/src/partitioning/bvh/bvh_refit.rs +++ b/src/partitioning/bvh/bvh_refit.rs @@ -629,7 +629,7 @@ impl Bvh { /// flag inherited by the wide node that used to hold the insertion sibling, the /// raw-merged flags written by its SAH rotations) lies on the inserted leaf's /// ancestor path, which the walk below rewrites all the way to the root — see - /// [`Self::refit_path`]. + /// `refit_path`. /// /// [`BvhLeafUpdateStatus::Inserted`]: super::BvhLeafUpdateStatus::Inserted /// diff --git a/src/partitioning/bvh/bvh_tree.rs b/src/partitioning/bvh/bvh_tree.rs index 678a1beb..9fd69070 100644 --- a/src/partitioning/bvh/bvh_tree.rs +++ b/src/partitioning/bvh/bvh_tree.rs @@ -987,7 +987,7 @@ impl BvhNode { /// /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::partitioning::bvh::BvhNode; + /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; /// use parry3d::math::Vector; /// @@ -1081,7 +1081,7 @@ impl BvhNode { /// /// ``` /// # #[cfg(all(feature = "dim3", feature = "f32"))] { - /// use parry3d::partitioning::bvh::BvhNode; + /// use parry3d::partitioning::BvhNode; /// use parry3d::bounding_volume::Aabb; /// use parry3d::math::Vector; /// diff --git a/src/query/sweep_toi/proxy_distance.rs b/src/query/sweep_toi/proxy_distance.rs index 43271be5..6f541ff5 100644 --- a/src/query/sweep_toi/proxy_distance.rs +++ b/src/query/sweep_toi/proxy_distance.rs @@ -83,8 +83,7 @@ pub fn proxy_distance( } else { 0 }; - for i in 0..count { - let v = &mut simplex[i]; + for (i, v) in simplex.iter_mut().enumerate().take(count) { v.index_a = cache.index_a[i]; v.index_b = cache.index_b[i]; v.wa = points_a[v.index_a as usize]; @@ -182,9 +181,9 @@ pub fn proxy_distance( // Cache the simplex. cache.count = count as u8; - for i in 0..count { - cache.index_a[i] = simplex[i].index_a; - cache.index_b[i] = simplex[i].index_b; + for (i, v) in simplex.iter().enumerate().take(count) { + cache.index_a[i] = v.index_a; + cache.index_b[i] = v.index_b; } // Apply radii if requested. @@ -826,8 +825,7 @@ pub fn proxy_distance( } else { 0 }; - for i in 0..count { - let v = &mut simplex[i]; + for (i, v) in simplex.iter_mut().enumerate().take(count) { v.index_a = cache.index_a[i]; v.index_b = cache.index_b[i]; v.wa = points_a[v.index_a as usize]; @@ -1001,9 +999,9 @@ pub fn proxy_distance( let (pa, pb) = witness_points3(&simplex, count); cache.metric = simplex_metric(&simplex, count); cache.count = count.min(3) as u8; - for i in 0..count.min(3) { - cache.index_a[i] = simplex[i].index_a; - cache.index_b[i] = simplex[i].index_b; + for (i, v) in simplex.iter().enumerate().take(count.min(3)) { + cache.index_a[i] = v.index_a; + cache.index_b[i] = v.index_b; } // Results stay in frame A. diff --git a/src/shape/cuboid.rs b/src/shape/cuboid.rs index 132ec379..30395bb6 100644 --- a/src/shape/cuboid.rs +++ b/src/shape/cuboid.rs @@ -265,6 +265,9 @@ impl Cuboid { /// Computes the face with a normal that maximizes the dot-product with `local_dir`. #[cfg(feature = "dim3")] + // The identity ors/shifts below are kept so the bit patterns line up with the comments + // documenting the vertex/edge numbering. + #[allow(clippy::identity_op)] pub fn support_face(&self, local_dir: Vector) -> PolygonalFeature { // NOTE: can we use the orthonormal basis of local_dir // to make this AoSoA friendly? From 9805e9bf97a7be83cabefc4cb93e909c6a4a1dc5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 2 Aug 2026 17:03:57 +0200 Subject: [PATCH 12/12] chore: update changelog --- CHANGELOG.md | 24 +++++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 123366f0..2d3ba3b7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,10 +1,32 @@ ## Unreleased +### Breaking changes + +- The `simd-stable` and `simd-nightly` features were removed. 4-lane SIMD is now always enabled + (it falls back to scalar code on targets without SIMD support). The new opt-in `simd8` feature + widens SIMD to 8 lanes for `f32` builds; it requires an AVX-enabled target to actually emit + 256-bit instructions, and is incompatible with `enhanced-determinism`. +- `ContactManifold::subshape_pos1`/`subshape_pos2` are no longer public fields. They are replaced by + a single boxed `subshape_poses: Option>` field (to shrink `ContactManifold`), + accessed through the new `subshape_pos1()`/`subshape_pos2()` getters and + `set_subshape_pos1`/`set_subshape_pos2` setters. + ### Added +- `query::sweep_toi`: sweep-based time-of-impact queries. A timestep is modeled as a `Sweep` between + two endpoint poses (linear translation + rotation nlerp), and `sweep_time_of_impact` (plus + `sweep_time_of_impact_composite` for composite shapes) computes the earliest time the swept shapes + reach a slop-based target separation, using conservative advancement. Also exports `ToiProxy`, + `SimplexCache`, `SweepToiOutput`, `SweepToiStatus`, and `SweepCompositeFastShape`. - `Bvh` gains incremental and parallel update APIs: `refit_partial`, flag-preserving `refit_without_resolve` variants, `refit_parallel`, parallel BVTT traversal, and batched - parallel leaf updates. + parallel leaf updates (`insert_or_update_batch_partially_parallel`, + `reinsert_or_update_with_change_detection`, `reinsert_or_update_if_present`, + `update_partially_if_present`, and the `BvhLeafUpdateStatus` enum). + +### Modified + +- Cuboid support-face feature ids are now computed with bit operations instead of lookup tables. ## 0.29.0