diff --git a/CHANGELOG.md b/CHANGELOG.md index 59d35eec..4eef481a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,9 @@ +## Unreleased + +### Fixed + +- Fix contact manifold voxels cached key overflowing. ([#346](https://github.com/dimforge/parry/pull/346)) + # 0.22.0-beta.1 ### Fixed diff --git a/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs b/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs index 9532b5df..e38f214a 100644 --- a/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_voxels_shape.rs @@ -316,6 +316,7 @@ pub fn contact_manifolds_voxels_shape( #[derive(Copy, Clone, Debug)] pub(crate) struct CanonicalVoxelShape { pub range: [Point; 2], + /// See [Self::linear_index_for_dimensions] pub workspace_key: Vector2, } @@ -348,15 +349,32 @@ impl CanonicalVoxelShape { adjust_canon(AxisMask::Z_NEG, 2, &mut key_low, mins[2]); } + let mut domain_dilated: [Point; 2] = voxels.domain().map(|v| *v); + domain_dilated[0].coords -= Vector::repeat(1); Self { range: [key_low, key_high], workspace_key: Vector2::new( - voxels.linear_index(key_low), - voxels.linear_index(key_high), + Self::linear_index_for_dimensions(key_low, domain_dilated[0], domain_dilated[1]), + Self::linear_index_for_dimensions(key_high, domain_dilated[0], domain_dilated[1]), ), } } + /// The linearized index associated to the given voxel key. + /// + /// This function allows to override the dimensions, that would lead to out of bounds index. + /// + /// It can be useful to keep a cache on voxels information when it would not fit the original dimension. + pub fn linear_index_for_dimensions( + voxel_key: Point, + domain_mins: Point, + domain_maxs: Point, + ) -> u32 { + let dims = (domain_maxs - domain_mins).map(|e| e as u32); + let rel_key = voxel_key - domain_mins; + (rel_key.x + rel_key.y * dims[0] as i32) as u32 + } + pub fn cuboid( &self, voxels: &Voxels,