diff --git a/CHANGELOG.md b/CHANGELOG.md index e93ccb28..a43a9a50 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,31 @@ # Change Log +## Unreleased + +### Modified + +- Remove CUDA support to break free from the toolchain restriction required by cust. +- Rework internal edges resolution using normal cones. This implies the modification of the + `SimdCompositeShape::map_part_at`, `TypedSimdCompositeShape::map_typed_part`, and + `TypedSimdCompositeShape::map_untyped_part` trait functions so that the closure argument takes + an extra argument for the (optional) normal constraints. This argument can be safely ignored + by user code unless applying the normal collection is relevant to your use-case. +- Contact manifolds will now retain all contacts (including the ones further than the specified `prediction` + distance) whenever any contact is actually closer than this `prediction` distance. +- Typo fix: renamed `TopologyError::BadAdjascentTrianglesOrientation` to `BadAdjacentTrianglesOrientation`. + +### Fixed + +- Fix contacts between convex shapes being occasionally ignored due to some rounding errors. +- Remove crash when entering unreachable code in non-linear TOI calculation. +- Fix accuracy issue in triangle-mesh center-of-mass calculation when the mesh isn’t manifold. + +### Added + +- Add `SdpMatrix2::inverse_and_get_determinant_unchecked`. This is useful for computing the + inverse in a AoSoA SIMD setting. +- Add `Aabb::intersects_moving_aabb` to perform a swept test between two moving aabbs. + ## v0.13.8 ### Added diff --git a/crates/parry2d-f64/Cargo.toml b/crates/parry2d-f64/Cargo.toml index d481e6bc..79ab82bb 100644 --- a/crates/parry2d-f64/Cargo.toml +++ b/crates/parry2d-f64/Cargo.toml @@ -28,7 +28,6 @@ bytemuck-serialize = ["bytemuck", "nalgebra/convert-bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/packed_simd", "simd-is-enabled"] enhanced-determinism = ["simba/libm_force", "indexmap"] -cuda = ["cust_core", "cust", "nalgebra/cuda"] parallel = ["rayon"] # Do not enable this feature directly. It is automatically @@ -60,9 +59,7 @@ cust_core = { version = "0.1", optional = true } spade = { version = "2", optional = true } # Make this optional? rayon = { version = "1", optional = true } bytemuck = { version = "1", features = ["derive"], optional = true } - -[target.'cfg(not(target_os = "cuda"))'.dependencies] -cust = { version = "0.3", optional = true } +log = "0.4" [dev-dependencies] simba = { version = "0.8", default-features = false, features = ["partial_fixed_point_support"] } diff --git a/crates/parry2d/Cargo.toml b/crates/parry2d/Cargo.toml index e80264be..d6a0850d 100644 --- a/crates/parry2d/Cargo.toml +++ b/crates/parry2d/Cargo.toml @@ -28,7 +28,6 @@ bytemuck-serialize = ["bytemuck", "nalgebra/convert-bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/packed_simd", "simd-is-enabled"] enhanced-determinism = ["simba/libm_force", "indexmap"] -cuda = ["cust_core", "cust", "nalgebra/cuda"] parallel = ["rayon"] # Do not enable this feature directly. It is automatically @@ -60,9 +59,7 @@ cust_core = { version = "0.1", optional = true } spade = { version = "2", optional = true } rayon = { version = "1", optional = true } bytemuck = { version = "1", features = ["derive"], optional = true } - -[target.'cfg(not(target_os = "cuda"))'.dependencies] -cust = { version = "0.3", optional = true } +log = "0.4" [dev-dependencies] simba = { version = "0.8", default-features = false, features = ["partial_fixed_point_support"] } diff --git a/crates/parry2d/tests/geometry/epa2.rs b/crates/parry2d/tests/geometry/epa2.rs index fe9f9fed..47727488 100644 --- a/crates/parry2d/tests/geometry/epa2.rs +++ b/crates/parry2d/tests/geometry/epa2.rs @@ -42,7 +42,15 @@ fn cuboids_large_size_ratio_issue_181() { let pos_ab = pos_a.inv_mul(&pos_b); let mut manifold: ContactManifold<(), ()> = ContactManifold::new(); dispatcher - .contact_manifold_convex_convex(&pos_ab, &cuboid_a, &cuboid_b, 0.0, &mut manifold) + .contact_manifold_convex_convex( + &pos_ab, + &cuboid_a, + &cuboid_b, + None, + None, + 0.0, + &mut manifold, + ) .unwrap(); if let Some(deepest) = manifold.find_deepest_contact() { diff --git a/crates/parry3d-f64/Cargo.toml b/crates/parry3d-f64/Cargo.toml index 208b47a4..79969057 100644 --- a/crates/parry3d-f64/Cargo.toml +++ b/crates/parry3d-f64/Cargo.toml @@ -28,7 +28,6 @@ bytemuck-serialize = ["bytemuck", "nalgebra/convert-bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/packed_simd", "simd-is-enabled"] enhanced-determinism = ["simba/libm_force", "indexmap"] -cuda = ["cust_core", "cust", "nalgebra/cuda"] parallel = ["rayon"] # Do not enable this feature directly. It is automatically @@ -60,9 +59,7 @@ cust_core = { version = "0.1", optional = true } spade = { version = "2", optional = true } # Make this optional? rayon = { version = "1", optional = true } bytemuck = { version = "1", features = ["derive"], optional = true } - -[target.'cfg(not(target_os = "cuda"))'.dependencies] -cust = { version = "0.3", optional = true } +log = "0.4" [dev-dependencies] oorandom = "11" diff --git a/crates/parry3d/Cargo.toml b/crates/parry3d/Cargo.toml index bf257110..9b79dab5 100644 --- a/crates/parry3d/Cargo.toml +++ b/crates/parry3d/Cargo.toml @@ -29,7 +29,6 @@ bytemuck-serialize = ["bytemuck", "nalgebra/convert-bytemuck"] simd-stable = ["simba/wide", "simd-is-enabled"] simd-nightly = ["simba/packed_simd", "simd-is-enabled"] enhanced-determinism = ["simba/libm_force", "indexmap"] -cuda = ["cust_core", "cust", "nalgebra/cuda"] parallel = ["rayon"] # Do not enable this feature directly. It is automatically @@ -61,9 +60,7 @@ cust_core = { version = "0.1", optional = true } spade = { version = "2", optional = true } # Make this optional? rayon = { version = "1", optional = true } bytemuck = { version = "1", features = ["derive"], optional = true } - -[target.'cfg(not(target_os = "cuda"))'.dependencies] -cust = { version = "0.3", optional = true } +log = "0.4" [dev-dependencies] oorandom = "11" diff --git a/src/bounding_volume/aabb.rs b/src/bounding_volume/aabb.rs index f8b93d84..68c82d76 100644 --- a/src/bounding_volume/aabb.rs +++ b/src/bounding_volume/aabb.rs @@ -11,6 +11,7 @@ use num::Bounded; #[cfg(not(feature = "std"))] use na::ComplexField; // for .abs() +use crate::query::{Ray, RayCast}; #[cfg(feature = "rkyv")] use rkyv::{bytecheck, CheckBytes}; @@ -22,7 +23,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Debug, PartialEq, Copy, Clone)] #[repr(C)] pub struct Aabb { @@ -201,6 +201,7 @@ impl Aabb { BoundingSphere::new(center, radius) } + /// Does this AABB contains a point expressed in the same coordinate frame as `self`? #[inline] pub fn contains_local_point(&self, point: &Point) -> bool { for i in 0..DIM { @@ -212,6 +213,19 @@ impl Aabb { true } + /// Does this AABB intersects an AABB `aabb2` moving at velocity `vel12` relative to `self`? + #[inline] + pub fn intersects_moving_aabb(&self, aabb2: &Self, vel12: Vector) -> bool { + // Minkowski sum. + let msum = Aabb { + mins: self.mins - aabb2.maxs.coords, + maxs: self.maxs - aabb2.mins.coords, + }; + let ray = Ray::new(Point::origin(), vel12); + + msum.intersects_local_ray(&ray, 1.0) + } + /// Computes the intersection of this `Aabb` and another one. pub fn intersection(&self, other: &Aabb) -> Option { let result = Aabb { diff --git a/src/bounding_volume/aabb_heightfield.rs b/src/bounding_volume/aabb_heightfield.rs index 6338a8f3..c97ae3ce 100644 --- a/src/bounding_volume/aabb_heightfield.rs +++ b/src/bounding_volume/aabb_heightfield.rs @@ -1,8 +1,8 @@ use crate::bounding_volume::Aabb; use crate::math::{Isometry, Real}; -use crate::shape::{GenericHeightField, HeightFieldStorage}; +use crate::shape::HeightField; -impl GenericHeightField { +impl HeightField { /// Computes the world-space [`Aabb`] of this heightfield, transformed by `pos`. #[inline] pub fn aabb(&self, pos: &Isometry) -> Aabb { diff --git a/src/bounding_volume/bounding_sphere_heightfield.rs b/src/bounding_volume/bounding_sphere_heightfield.rs index 21dd0a3b..714e369f 100644 --- a/src/bounding_volume/bounding_sphere_heightfield.rs +++ b/src/bounding_volume/bounding_sphere_heightfield.rs @@ -1,8 +1,8 @@ use crate::bounding_volume::BoundingSphere; use crate::math::{Isometry, Real}; -use crate::shape::{GenericHeightField, HeightFieldStorage}; +use crate::shape::HeightField; -impl GenericHeightField { +impl HeightField { /// Computes the world-space bounding sphere of this height-field, transformed by `pos`. #[inline] pub fn bounding_sphere(&self, pos: &Isometry) -> BoundingSphere { diff --git a/src/bounding_volume/mod.rs b/src/bounding_volume/mod.rs index 4587f99f..53cfb658 100644 --- a/src/bounding_volume/mod.rs +++ b/src/bounding_volume/mod.rs @@ -23,6 +23,7 @@ mod aabb_convex_polygon; mod aabb_convex_polyhedron; mod aabb_cuboid; mod aabb_halfspace; +#[cfg(feature = "std")] mod aabb_heightfield; mod aabb_support_map; mod aabb_triangle; @@ -45,6 +46,7 @@ mod bounding_sphere_cuboid; #[cfg(feature = "dim3")] mod bounding_sphere_cylinder; mod bounding_sphere_halfspace; +#[cfg(feature = "std")] mod bounding_sphere_heightfield; #[cfg(feature = "std")] mod bounding_sphere_polyline; diff --git a/src/bounding_volume/simd_aabb.rs b/src/bounding_volume/simd_aabb.rs index 8ac51f2c..75bbfde7 100644 --- a/src/bounding_volume/simd_aabb.rs +++ b/src/bounding_volume/simd_aabb.rs @@ -12,7 +12,6 @@ use simba::simd::{SimdPartialOrd, SimdValue}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] pub struct SimdAabb { /// The min coordinates of the Aabbs. pub mins: Point, diff --git a/src/mass_properties/mass_properties_trimesh3d.rs b/src/mass_properties/mass_properties_trimesh3d.rs index 502fbc9e..aa7416d9 100644 --- a/src/mass_properties/mass_properties_trimesh3d.rs +++ b/src/mass_properties/mass_properties_trimesh3d.rs @@ -156,7 +156,7 @@ pub fn trimesh_signed_volume_and_center_of_mass( vertices: &[Point], indices: &[[u32; DIM]], ) -> (Real, Point) { - let geometric_center = Point::new(-10.0, -10.0, -10.0); // utils::center(vertices); + let geometric_center = crate::utils::center(vertices); let mut res = Point::origin(); let mut vol = 0.0; diff --git a/src/partitioning/mod.rs b/src/partitioning/mod.rs index 847129ec..cf86ef42 100644 --- a/src/partitioning/mod.rs +++ b/src/partitioning/mod.rs @@ -2,10 +2,8 @@ #[cfg(feature = "std")] pub use self::qbvh::{ - CenterDataSplitter, QbvhDataGenerator, QbvhNonOverlappingDataSplitter, QbvhUpdateWorkspace, -}; -pub use self::qbvh::{ - GenericQbvh, IndexedData, NodeIndex, Qbvh, QbvhNode, QbvhProxy, QbvhStorage, SimdNodeIndex, + CenterDataSplitter, IndexedData, NodeIndex, Qbvh, QbvhDataGenerator, QbvhNode, + QbvhNonOverlappingDataSplitter, QbvhProxy, QbvhUpdateWorkspace, SimdNodeIndex, }; #[cfg(feature = "parallel")] pub use self::visitor::{ParallelSimdSimultaneousVisitor, ParallelSimdVisitor}; @@ -16,7 +14,9 @@ pub use self::visitor::{ /// A quaternary bounding-volume-hierarchy. #[deprecated(note = "Renamed to Qbvh")] +#[cfg(feature = "std")] pub type SimdQbvh = Qbvh; +#[cfg(feature = "std")] mod qbvh; mod visitor; diff --git a/src/partitioning/qbvh/mod.rs b/src/partitioning/qbvh/mod.rs index 1995b25b..699f48f5 100644 --- a/src/partitioning/qbvh/mod.rs +++ b/src/partitioning/qbvh/mod.rs @@ -1,23 +1,15 @@ -#[cfg(feature = "std")] pub use self::{ build::{CenterDataSplitter, QbvhDataGenerator, QbvhNonOverlappingDataSplitter}, update::QbvhUpdateWorkspace, }; pub use self::qbvh::{ - GenericQbvh, IndexedData, NodeIndex, Qbvh, QbvhNode, QbvhNodeFlags, QbvhProxy, SimdNodeIndex, + IndexedData, NodeIndex, Qbvh, QbvhNode, QbvhNodeFlags, QbvhProxy, SimdNodeIndex, }; -pub use self::storage::QbvhStorage; mod qbvh; -mod storage; mod utils; -#[cfg(feature = "std")] mod build; -#[cfg(feature = "std")] mod traversal; -#[cfg(not(feature = "std"))] -mod traversal_no_std; -#[cfg(feature = "std")] mod update; diff --git a/src/partitioning/qbvh/qbvh.rs b/src/partitioning/qbvh/qbvh.rs index 3015c314..99913438 100644 --- a/src/partitioning/qbvh/qbvh.rs +++ b/src/partitioning/qbvh/qbvh.rs @@ -1,20 +1,11 @@ use crate::bounding_volume::{Aabb, SimdAabb}; use crate::math::{Real, Vector}; -use crate::partitioning::qbvh::storage::QbvhStorage; -use crate::utils::DefaultStorage; use bitflags::bitflags; use na::SimdValue; #[cfg(feature = "rkyv")] use rkyv::{bytecheck, CheckBytes}; -#[cfg(all(feature = "std", feature = "cuda"))] -use {crate::utils::CudaArray1, cust::error::CudaResult}; -#[cfg(feature = "cuda")] -use { - crate::utils::{CudaStorage, CudaStoragePtr}, - cust_core::DeviceCopy, -}; /// A data to which an index is associated. pub trait IndexedData: Copy { @@ -64,7 +55,6 @@ pub type SimdNodeIndex = u32; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] /// The index of one specific node of a Qbvh. pub struct NodeIndex { /// The index of the SIMD node containing the addressed node. @@ -97,7 +87,6 @@ bitflags! { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(as = "Self") )] - #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Default)] /// The status of a QBVH node. pub struct QbvhNodeFlags: u8 { @@ -120,7 +109,6 @@ bitflags! { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] pub struct QbvhNode { /// The Aabbs of the qbvh nodes represented by this node. pub simd_aabb: SimdAabb, @@ -194,7 +182,6 @@ impl QbvhNode { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] /// Combination of a leaf data and its associated node’s index. pub struct QbvhProxy { /// Index of the leaf node the leaf data is associated to. @@ -235,86 +222,22 @@ impl QbvhProxy { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[repr(C)] // Needed for Cuda. -#[derive(Debug)] -pub struct GenericQbvh> { +#[repr(C)] +#[derive(Debug, Clone)] +pub struct Qbvh { pub(super) root_aabb: Aabb, - pub(super) nodes: Storage::Nodes, - pub(super) dirty_nodes: Storage::ArrayU32, - pub(super) free_list: Storage::ArrayU32, - pub(super) proxies: Storage::ArrayProxies, + pub(super) nodes: Vec, + pub(super) dirty_nodes: Vec, + pub(super) free_list: Vec, + pub(super) proxies: Vec>, } -/// A quaternary bounding-volume-hierarchy. -/// -/// This is a bounding-volume-hierarchy where each node has either four children or none. -pub type Qbvh = GenericQbvh; -#[cfg(feature = "cuda")] -/// A Qbvh stored in CUDA memory. -pub type CudaQbvh = GenericQbvh; -#[cfg(feature = "cuda")] -/// A Qbvh accessible from CUDA kernels. -pub type CudaQbvhPtr = GenericQbvh; - -impl Clone for GenericQbvh -where - Storage: QbvhStorage, - Storage::Nodes: Clone, - Storage::ArrayU32: Clone, - Storage::ArrayProxies: Clone, -{ - fn clone(&self) -> Self { - Self { - root_aabb: self.root_aabb, - nodes: self.nodes.clone(), - dirty_nodes: self.dirty_nodes.clone(), - free_list: self.free_list.clone(), - proxies: self.proxies.clone(), - } - } -} - -impl Copy for GenericQbvh -where - Storage: QbvhStorage, - Storage::Nodes: Copy, - Storage::ArrayU32: Copy, - Storage::ArrayProxies: Copy, -{ -} - -#[cfg(feature = "cuda")] -unsafe impl DeviceCopy for GenericQbvh -where - Storage: QbvhStorage, - Storage::Nodes: DeviceCopy, - Storage::ArrayU32: DeviceCopy, - Storage::ArrayProxies: DeviceCopy, -{ -} - -#[cfg(all(feature = "std", feature = "cuda"))] -impl CudaQbvh { - /// Returns the qbvh usable from within a CUDA kernel. - pub fn as_device_ptr(&self) -> CudaQbvhPtr { - GenericQbvh { - root_aabb: self.root_aabb, - nodes: self.nodes.as_device_ptr(), - dirty_nodes: self.dirty_nodes.as_device_ptr(), - free_list: self.free_list.as_device_ptr(), - proxies: self.proxies.as_device_ptr(), - } - } -} - -#[cfg(feature = "std")] impl Default for Qbvh { fn default() -> Self { Self::new() } } -#[cfg(feature = "std")] impl Qbvh { /// Initialize an empty Qbvh. pub fn new() -> Self { @@ -327,21 +250,6 @@ impl Qbvh { } } - /// Converts this RAM-based qbvh to an qbvh based on CUDA memory. - #[cfg(feature = "cuda")] - pub fn to_cuda(&self) -> CudaResult> - where - LeafData: DeviceCopy, - { - Ok(CudaQbvh { - root_aabb: self.root_aabb, - nodes: CudaArray1::new(&self.nodes)?, - dirty_nodes: CudaArray1::new(&self.dirty_nodes)?, - free_list: CudaArray1::new(&self.free_list)?, - proxies: CudaArray1::new(&self.proxies)?, - }) - } - /// Iterates mutably through all the leaf data in this Qbvh. pub fn iter_data_mut(&mut self) -> impl Iterator { self.proxies.iter_mut().map(|p| (p.node, &mut p.data)) @@ -405,7 +313,7 @@ impl Qbvh { } } -impl> GenericQbvh { +impl Qbvh { /// The Aabb of the root of this tree. pub fn root_aabb(&self) -> &Aabb { &self.root_aabb diff --git a/src/partitioning/qbvh/storage.rs b/src/partitioning/qbvh/storage.rs deleted file mode 100644 index d54176c0..00000000 --- a/src/partitioning/qbvh/storage.rs +++ /dev/null @@ -1,38 +0,0 @@ -use crate::partitioning::{QbvhNode, QbvhProxy}; -use crate::utils::{Array1, DefaultStorage}; - -#[cfg(all(feature = "std", feature = "cuda"))] -use crate::utils::CudaArray1; -#[cfg(feature = "cuda")] -use crate::utils::{CudaArrayPointer1, CudaStorage, CudaStoragePtr}; - -/// Trait describing all the types needed for storing a Qbvh’s data. -pub trait QbvhStorage { - /// Type of the array containing the Qbvh nodes. - type Nodes: Array1; - /// Type of an array containing u32. - type ArrayU32: Array1; - /// Type of the array containing the Qbvh leaves. - type ArrayProxies: Array1>; -} - -#[cfg(feature = "std")] -impl QbvhStorage for DefaultStorage { - type Nodes = Vec; - type ArrayU32 = Vec; - type ArrayProxies = Vec>; -} - -#[cfg(all(feature = "std", feature = "cuda"))] -impl QbvhStorage for CudaStorage { - type Nodes = CudaArray1; - type ArrayU32 = CudaArray1; - type ArrayProxies = CudaArray1>; -} - -#[cfg(feature = "cuda")] -impl QbvhStorage for CudaStoragePtr { - type Nodes = CudaArrayPointer1; - type ArrayU32 = CudaArrayPointer1; - type ArrayProxies = CudaArrayPointer1>; -} diff --git a/src/partitioning/qbvh/traversal.rs b/src/partitioning/qbvh/traversal.rs index 49d12010..8c9e77de 100644 --- a/src/partitioning/qbvh/traversal.rs +++ b/src/partitioning/qbvh/traversal.rs @@ -4,11 +4,10 @@ use crate::bounding_volume::{Aabb, SimdAabb}; use crate::math::Real; use crate::partitioning::visitor::{SimdSimultaneousVisitStatus, SimdVisitorWithContext}; use crate::partitioning::{ - GenericQbvh, QbvhStorage, SimdBestFirstVisitStatus, SimdBestFirstVisitor, - SimdSimultaneousVisitor, SimdVisitStatus, SimdVisitor, + Qbvh, SimdBestFirstVisitStatus, SimdBestFirstVisitor, SimdSimultaneousVisitor, SimdVisitStatus, + SimdVisitor, }; use crate::simd::SIMD_WIDTH; -use crate::utils::Array1; use crate::utils::WeightedValue; use num::Bounded; use simba::simd::SimdBool; @@ -21,9 +20,9 @@ use { std::sync::atomic::{AtomicBool, Ordering as AtomicOrdering}, }; -use super::{IndexedData, NodeIndex, Qbvh}; +use super::{IndexedData, NodeIndex}; -impl> GenericQbvh { +impl Qbvh { /// Performs a depth-first traversal on the BVH. /// /// # Return @@ -79,7 +78,7 @@ impl> GenericQbvh> GenericQbvh> GenericQbvh> GenericQbvh> GenericQbvh> GenericQbvh> GenericQbvh> GenericQbvh> GenericQbvh Qbvh { let mut stack: ArrayVec = ArrayVec::new(); let node = &self.nodes[entry as usize]; let leaf_data = if node.is_leaf() { - Some( - array![|ii| Some(&self.proxies.get_at(node.children[ii] as usize)?.data); SIMD_WIDTH], - ) + Some(array![|ii| Some(&self.proxies.get(node.children[ii] as usize)?.data); SIMD_WIDTH]) } else { None }; @@ -610,7 +607,7 @@ impl Qbvh { let leaf_data1 = if node1.is_leaf() { Some( - array![|ii| Some(&qbvh1.proxies.get_at(node1.children[ii] as usize)?.data); SIMD_WIDTH], + array![|ii| Some(&qbvh1.proxies.get(node1.children[ii] as usize)?.data); SIMD_WIDTH], ) } else { None @@ -618,7 +615,7 @@ impl Qbvh { let leaf_data2 = if node2.is_leaf() { Some( - array![|ii| Some(&qbvh2.proxies.get_at(node2.children[ii] as usize)?.data); SIMD_WIDTH], + array![|ii| Some(&qbvh2.proxies.get(node2.children[ii] as usize)?.data); SIMD_WIDTH], ) } else { None diff --git a/src/partitioning/qbvh/traversal_no_std.rs b/src/partitioning/qbvh/traversal_no_std.rs deleted file mode 100644 index aea894ce..00000000 --- a/src/partitioning/qbvh/traversal_no_std.rs +++ /dev/null @@ -1,187 +0,0 @@ -use crate::bounding_volume::{Aabb, SimdAabb}; -use crate::math::Real; -use crate::partitioning::visitor::SimdSimultaneousVisitStatus; -use crate::partitioning::{ - GenericQbvh, QbvhStorage, SimdBestFirstVisitStatus, SimdBestFirstVisitor, - SimdSimultaneousVisitor, SimdVisitStatus, SimdVisitor, -}; -use crate::simd::SIMD_WIDTH; -use crate::utils::{Array1, WeightedValue}; -use arrayvec::ArrayVec; -use num::Bounded; -use simba::simd::SimdBool; - -use super::{IndexedData, NodeIndex, Qbvh}; - -impl> GenericQbvh { - /// Performs a depth-first traversal on the BVH. - /// - /// # Return - /// - /// Returns `false` if the traversal exited early, and `true` otherwise. - pub fn traverse_depth_first(&self, visitor: &mut impl SimdVisitor) -> bool { - self.traverse_depth_first_node(visitor, 0) - } - - /// Performs a depth-first traversal on the BVH starting at the given node. - /// - /// # Return - /// - /// Returns `false` if the traversal exited early, and `true` otherwise. - pub fn traverse_depth_first_node( - &self, - visitor: &mut impl SimdVisitor, - curr_node: u32, - ) -> bool { - let node = &self.nodes[curr_node as usize]; - let leaf_data = if node.is_leaf() { - Some( - array![|ii| Some(&self.proxies.get_at(node.children[ii] as usize)?.data); SIMD_WIDTH], - ) - } else { - None - }; - - match visitor.visit(&node.simd_aabb, leaf_data) { - SimdVisitStatus::ExitEarly => { - return false; - } - SimdVisitStatus::MaybeContinue(mask) => { - let bitmask = mask.bitmask(); - - for ii in 0..SIMD_WIDTH { - if (bitmask & (1 << ii)) != 0 { - if !node.is_leaf() { - // Internal node, visit the child. - // Unfortunately, we have this check because invalid Aabbs - // return a hit as well. - if node.children[ii] as usize <= self.nodes.len() { - if !self.traverse_depth_first_node(visitor, node.children[ii]) { - return false; - } - } - } - } - } - } - } - - true - } - - /// Performs a best-first-search on the BVH. - /// - /// Returns the content of the leaf with the smallest associated cost, and a result of - /// user-defined type. - pub fn traverse_best_first(&self, visitor: &mut BFS) -> Option<(NodeIndex, BFS::Result)> - where - BFS: SimdBestFirstVisitor, - BFS::Result: Clone, // Because we cannot move out of an array… - { - self.traverse_best_first_node(visitor, 0, Real::MAX) - } - - /// Performs a best-first-search on the BVH. - /// - /// Returns the content of the leaf with the smallest associated cost, and a result of - /// user-defined type. - pub fn traverse_best_first_node( - &self, - visitor: &mut BFS, - start_node: u32, - init_cost: Real, - ) -> Option<(NodeIndex, BFS::Result)> - where - BFS: SimdBestFirstVisitor, - BFS::Result: Clone, // Because we cannot move out of an array… - { - if self.nodes.is_empty() { - return None; - } - - let mut best_cost = init_cost; - let mut result = None; - - // NOTE: a stack with 64 elements is enough for a depth-first search - // on a tree with up to about 4.000.000.000 triangles. - // See https://math.stackexchange.com/a/2739663 for the max - // stack depth on a depth-first search. - let mut stack: ArrayVec<_, 64> = ArrayVec::new(); - stack.push(WeightedValue::new(start_node, -best_cost / 2.0)); - - self.traverse_best_first_node_recursive(visitor, &mut stack, &mut best_cost, &mut result); - result - } - - fn traverse_best_first_node_recursive( - &self, - visitor: &mut BFS, - stack: &mut ArrayVec, 64>, - best_cost: &mut Real, - best_result: &mut Option<(NodeIndex, BFS::Result)>, - ) where - BFS: SimdBestFirstVisitor, - BFS::Result: Clone, // Because we cannot move out of an array… - { - while let Some(entry) = stack.pop() { - // NOTE: since we are not actually allowed to allocate, we can’t use the binary heap. - // So we are really just running a recursive depth-first traversal, with early - // exit on the current best cost. - if -entry.cost >= *best_cost { - continue; - } - - let node = &self.nodes[entry.value as usize]; - let leaf_data = if node.is_leaf() { - Some( - array![|ii| Some(&self.proxies.get_at(node.children[ii] as usize)?.data); SIMD_WIDTH], - ) - } else { - None - }; - - match visitor.visit(*best_cost, &node.simd_aabb, leaf_data) { - SimdBestFirstVisitStatus::ExitEarly(result) => { - if result.is_some() { - *best_result = result.map(|r| (node.parent, r)); - return; - } - } - SimdBestFirstVisitStatus::MaybeContinue { - weights, - mask, - results, - } => { - let bitmask = mask.bitmask(); - let weights: [Real; SIMD_WIDTH] = weights.into(); - - for ii in 0..SIMD_WIDTH { - if (bitmask & (1 << ii)) != 0 { - if node.is_leaf() { - if weights[ii] < *best_cost && results[ii].is_some() { - // We found a leaf! - if let Some(proxy) = - self.proxies.get_at(node.children[ii] as usize) - { - *best_cost = weights[ii]; - *best_result = - Some((proxy.node, results[ii].clone().unwrap())) - } - } - } else { - // Internal node, visit the child. - // Unfortunately, we have this check because invalid Aabbs - // return a hit as well. - if (node.children[ii] as usize) < self.nodes.len() { - let child_node = - WeightedValue::new(node.children[ii], -weights[ii]); - stack.push(child_node); - } - } - } - } - } - } - } - } -} diff --git a/src/partitioning/qbvh/update/tests.rs b/src/partitioning/qbvh/update/tests.rs index 36ce24bd..2f122b5d 100644 --- a/src/partitioning/qbvh/update/tests.rs +++ b/src/partitioning/qbvh/update/tests.rs @@ -4,8 +4,7 @@ use std::borrow::Cow; use crate::{ bounding_volume::Aabb, math::{Point, Real}, - partitioning::{GenericQbvh, Qbvh}, - utils::DefaultStorage, + partitioning::Qbvh, }; use super::QbvhUpdateWorkspace; @@ -323,14 +322,14 @@ impl<'q> ptree::TreeItem for QbvhTreeIterator<'q> { #[derive(Clone)] struct QbvhTreeIterator<'q> { - qbvh: &'q GenericQbvh, + qbvh: &'q Qbvh, node: u32, aabb: Aabb, child_of_leaf: bool, } impl<'q> QbvhTreeIterator<'q> { - fn new(qbvh: &'q GenericQbvh) -> Self { + fn new(qbvh: &'q Qbvh) -> Self { let aabb = *qbvh.root_aabb(); let child_of_leaf = qbvh.raw_nodes().is_empty(); diff --git a/src/partitioning/visitor.rs b/src/partitioning/visitor.rs index 5d9b899b..dc751c94 100644 --- a/src/partitioning/visitor.rs +++ b/src/partitioning/visitor.rs @@ -1,5 +1,8 @@ use crate::math::{Real, SimdBool, SimdReal, SIMD_WIDTH}; + +#[cfg(feature = "std")] use crate::partitioning::qbvh::QbvhNode; +#[cfg(feature = "std")] use crate::partitioning::SimdNodeIndex; /// The next action to be taken by a BVH traversal algorithm after having visited a node with some data. @@ -116,6 +119,7 @@ pub trait SimdSimultaneousVisitor { */ /// Trait implemented by visitor called during the parallel traversal of a spatial partitioning data structure. +#[cfg(feature = "std")] pub trait ParallelSimdVisitor: Sync { /// Execute an operation on the content of a node of the spatial partitioning structure. /// @@ -129,6 +133,7 @@ pub trait ParallelSimdVisitor: Sync { ) -> SimdVisitStatus; } +#[cfg(feature = "std")] impl ParallelSimdVisitor for F where F: Sync + Fn(&QbvhNode, Option<[Option<&LeafData>; SIMD_WIDTH]>) -> SimdVisitStatus, @@ -146,6 +151,7 @@ where /// Trait implemented by visitor called during a parallel simultaneous spatial partitioning /// data structure traversal. #[cfg(feature = "parallel")] +#[cfg(feature = "std")] pub trait ParallelSimdSimultaneousVisitor: Sync { /// Visitor state data that will be passed down the recursion. type Data: Copy + Sync + Default; diff --git a/src/query/closest_points/closest_points_composite_shape_shape.rs b/src/query/closest_points/closest_points_composite_shape_shape.rs index 0d08d5c8..85220a94 100644 --- a/src/query/closest_points/closest_points_composite_shape_shape.rs +++ b/src/query/closest_points/closest_points_composite_shape_shape.rs @@ -3,7 +3,7 @@ use crate::math::{Isometry, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH}; use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; use crate::query::{ClosestPoints, QueryDispatcher}; use crate::shape::{Shape, TypedSimdCompositeShape}; -use crate::utils::{DefaultStorage, IsometryOpt}; +use crate::utils::IsometryOpt; use na; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; @@ -17,7 +17,7 @@ pub fn closest_points_composite_shape_shape( ) -> ClosestPoints where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { let mut visitor = CompositeShapeAgainstShapeClosestPointsVisitor::new(dispatcher, pos12, g1, g2, margin); @@ -39,7 +39,7 @@ pub fn closest_points_shape_composite_shape( ) -> ClosestPoints where D: QueryDispatcher, - G2: TypedSimdCompositeShape, + G2: TypedSimdCompositeShape, { closest_points_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1, margin).flipped() } @@ -59,7 +59,7 @@ pub struct CompositeShapeAgainstShapeClosestPointsVisitor<'a, D: ?Sized, G1: ?Si impl<'a, D: ?Sized, G1: ?Sized> CompositeShapeAgainstShapeClosestPointsVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { /// Initializes a visitor for computing the closest points between a composite-shape and a shape. pub fn new( @@ -87,7 +87,7 @@ impl<'a, D: ?Sized, G1: ?Sized> SimdBestFirstVisitor for CompositeShapeAgainstShapeClosestPointsVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { type Result = (G1::PartId, ClosestPoints); @@ -115,7 +115,7 @@ where for ii in 0..SIMD_WIDTH { if (bitmask & (1 << ii)) != 0 && data[ii].is_some() { let part_id = *data[ii].unwrap(); - self.g1.map_untyped_part_at(part_id, |part_pos1, g1| { + self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { let pts = self.dispatcher.closest_points( &part_pos1.inv_mul(self.pos12), g1, diff --git a/src/query/contact/contact_composite_shape_shape.rs b/src/query/contact/contact_composite_shape_shape.rs index 40418991..0656361d 100644 --- a/src/query/contact/contact_composite_shape_shape.rs +++ b/src/query/contact/contact_composite_shape_shape.rs @@ -22,7 +22,7 @@ where let mut res = None::; let mut leaf_callback = |i: &_| { - g1.map_part_at(*i, &mut |part_pos1, part1| { + g1.map_part_at(*i, &mut |part_pos1, part1, _| { if let Ok(Some(mut c)) = dispatcher.contact(&part_pos1.inv_mul(pos12), part1, g2, prediction) { 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 a653f7ff..4be384ab 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 @@ -98,81 +98,91 @@ pub fn contact_manifolds_composite_shape_composite_shape<'a, ManifoldData, Conta let mut old_manifolds = std::mem::take(manifolds); let mut leaf_fn1 = |leaf1: &u32| { - composite1.map_part_at(*leaf1, &mut |part_pos1, part_shape1| { - let pos211 = part_pos1.prepend_to(&pos21); // == pos21 * part_pos1 - let ls_part_aabb1_2 = part_shape1.compute_aabb(&pos211).loosened(prediction); - let mut leaf_fn2 = |leaf2: &u32| { - composite2.map_part_at(*leaf2, &mut |part_pos2, part_shape2| { - let pos2211 = part_pos2.inv_mul(&pos211); - let entry_key = if flipped { - (*leaf2, *leaf1) - } else { - (*leaf1, *leaf2) - }; - - let sub_detector = match workspace.sub_detectors.entry(entry_key) { - Entry::Occupied(entry) => { - let sub_detector = entry.into_mut(); - let manifold = old_manifolds[sub_detector.manifold_id].take(); - sub_detector.manifold_id = manifolds.len(); - sub_detector.timestamp = new_timestamp; - manifolds.push(manifold); - sub_detector - } - Entry::Vacant(entry) => { - let sub_detector = SubDetector { - manifold_id: manifolds.len(), - timestamp: new_timestamp, + composite1.map_part_at( + *leaf1, + &mut |part_pos1, part_shape1, normal_constraints1| { + let pos211 = part_pos1.prepend_to(&pos21); // == pos21 * part_pos1 + let ls_part_aabb1_2 = part_shape1.compute_aabb(&pos211).loosened(prediction); + let mut leaf_fn2 = |leaf2: &u32| { + composite2.map_part_at( + *leaf2, + &mut |part_pos2, part_shape2, normal_constraints2| { + let pos2211 = part_pos2.inv_mul(&pos211); + let entry_key = if flipped { + (*leaf2, *leaf1) + } else { + (*leaf1, *leaf2) + }; + + let sub_detector = match workspace.sub_detectors.entry(entry_key) { + Entry::Occupied(entry) => { + let sub_detector = entry.into_mut(); + let manifold = old_manifolds[sub_detector.manifold_id].take(); + sub_detector.manifold_id = manifolds.len(); + sub_detector.timestamp = new_timestamp; + manifolds.push(manifold); + sub_detector + } + Entry::Vacant(entry) => { + let sub_detector = SubDetector { + manifold_id: manifolds.len(), + timestamp: new_timestamp, + }; + + let mut manifold = ContactManifold::new(); + + if flipped { + manifold.subshape1 = *leaf2; + manifold.subshape2 = *leaf1; + manifold.subshape_pos1 = part_pos2.copied(); + manifold.subshape_pos2 = part_pos1.copied(); + } else { + manifold.subshape1 = *leaf1; + manifold.subshape2 = *leaf2; + manifold.subshape_pos1 = part_pos1.copied(); + manifold.subshape_pos2 = part_pos2.copied(); + }; + + manifolds.push(manifold); + entry.insert(sub_detector) + } }; - let mut manifold = ContactManifold::new(); + let manifold = &mut manifolds[sub_detector.manifold_id]; if flipped { - manifold.subshape1 = *leaf2; - manifold.subshape2 = *leaf1; - manifold.subshape_pos1 = part_pos2.copied(); - manifold.subshape_pos2 = part_pos1.copied(); + let _ = dispatcher.contact_manifold_convex_convex( + &pos2211, + part_shape2, + part_shape1, + normal_constraints2, + normal_constraints1, + prediction, + manifold, + ); } else { - manifold.subshape1 = *leaf1; - manifold.subshape2 = *leaf2; - manifold.subshape_pos1 = part_pos1.copied(); - manifold.subshape_pos2 = part_pos2.copied(); - }; - - manifolds.push(manifold); - entry.insert(sub_detector) - } - }; - - let manifold = &mut manifolds[sub_detector.manifold_id]; - - if flipped { - let _ = dispatcher.contact_manifold_convex_convex( - &pos2211, - part_shape2, - part_shape1, - prediction, - manifold, - ); - } else { - let _ = dispatcher.contact_manifold_convex_convex( - &pos2211.inverse(), - part_shape1, - part_shape2, - prediction, - manifold, - ); - } - }); - - true - }; - - let mut visitor2 = - BoundingVolumeIntersectionsVisitor::new(&ls_part_aabb1_2, &mut leaf_fn2); - - let _ = qbvh2.traverse_depth_first_with_stack(&mut visitor2, &mut stack2); - }); + let _ = dispatcher.contact_manifold_convex_convex( + &pos2211.inverse(), + part_shape1, + part_shape2, + normal_constraints1, + normal_constraints2, + prediction, + manifold, + ); + } + }, + ); + + true + }; + + let mut visitor2 = + BoundingVolumeIntersectionsVisitor::new(&ls_part_aabb1_2, &mut leaf_fn2); + + let _ = qbvh2.traverse_depth_first_with_stack(&mut visitor2, &mut stack2); + }, + ); true }; 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 16a68d63..a94c6193 100644 --- a/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_composite_shape_shape.rs @@ -84,59 +84,66 @@ pub fn contact_manifolds_composite_shape_shape( let mut old_manifolds = std::mem::take(manifolds); let mut leaf1_fn = |leaf1: &u32| { - composite1.map_part_at(*leaf1, &mut |part_pos1, part_shape1| { - let sub_detector = match workspace.sub_detectors.entry(*leaf1) { - Entry::Occupied(entry) => { - let sub_detector = entry.into_mut(); - let manifold = old_manifolds[sub_detector.manifold_id].take(); - sub_detector.manifold_id = manifolds.len(); - sub_detector.timestamp = new_timestamp; - manifolds.push(manifold); - sub_detector + composite1.map_part_at( + *leaf1, + &mut |part_pos1, part_shape1, normal_constraints1| { + let sub_detector = match workspace.sub_detectors.entry(*leaf1) { + Entry::Occupied(entry) => { + let sub_detector = entry.into_mut(); + let manifold = old_manifolds[sub_detector.manifold_id].take(); + sub_detector.manifold_id = manifolds.len(); + sub_detector.timestamp = new_timestamp; + manifolds.push(manifold); + sub_detector + } + Entry::Vacant(entry) => { + let sub_detector = SubDetector { + manifold_id: manifolds.len(), + timestamp: new_timestamp, + }; + + let mut manifold = ContactManifold::new(); + + if flipped { + manifold.subshape1 = 0; + manifold.subshape2 = *leaf1; + manifold.subshape_pos2 = part_pos1.copied(); + } else { + manifold.subshape1 = *leaf1; + manifold.subshape2 = 0; + manifold.subshape_pos1 = part_pos1.copied(); + }; + + manifolds.push(manifold); + entry.insert(sub_detector) + } + }; + + let manifold = &mut manifolds[sub_detector.manifold_id]; + + if flipped { + let _ = dispatcher.contact_manifold_convex_convex( + &part_pos1.prepend_to(&pos21), + shape2, + part_shape1, + None, + normal_constraints1, + prediction, + manifold, + ); + } else { + let _ = dispatcher.contact_manifold_convex_convex( + &part_pos1.inv_mul(&pos12), + part_shape1, + shape2, + normal_constraints1, + None, + prediction, + manifold, + ); } - Entry::Vacant(entry) => { - let sub_detector = SubDetector { - manifold_id: manifolds.len(), - timestamp: new_timestamp, - }; - - let mut manifold = ContactManifold::new(); - - if flipped { - manifold.subshape1 = 0; - manifold.subshape2 = *leaf1; - manifold.subshape_pos2 = part_pos1.copied(); - } else { - manifold.subshape1 = *leaf1; - manifold.subshape2 = 0; - manifold.subshape_pos1 = part_pos1.copied(); - }; - - manifolds.push(manifold); - entry.insert(sub_detector) - } - }; - - let manifold = &mut manifolds[sub_detector.manifold_id]; - - if flipped { - let _ = dispatcher.contact_manifold_convex_convex( - &part_pos1.prepend_to(&pos21), - shape2, - part_shape1, - prediction, - manifold, - ); - } else { - let _ = dispatcher.contact_manifold_convex_convex( - &part_pos1.inv_mul(&pos12), - part_shape1, - shape2, - prediction, - manifold, - ); - } - }); + }, + ); true }; diff --git a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs index 18c194c3..0138b61f 100644 --- a/src/query/contact_manifolds/contact_manifolds_convex_ball.rs +++ b/src/query/contact_manifolds/contact_manifolds_convex_ball.rs @@ -1,6 +1,8 @@ use crate::math::{Isometry, Point, Real, Vector}; +use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; use crate::query::{ContactManifold, TrackedContact}; use crate::shape::{Ball, PackedFeatureId, Shape}; +use crate::utils::COS_0_DOT_1_DEGREES; use na::Unit; /// Computes the contact manifold between a convex shape and a ball, both represented as a `Shape` trait-object. @@ -8,15 +10,35 @@ pub fn contact_manifold_convex_ball_shapes( pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, ) where ContactData: Default + Copy, { if let Some(ball1) = shape1.as_ball() { - contact_manifold_convex_ball(&pos12.inverse(), shape2, ball1, prediction, manifold, true); + contact_manifold_convex_ball( + &pos12.inverse(), + shape2, + ball1, + normal_constraints2, + normal_constraints1, + prediction, + manifold, + true, + ); } else if let Some(ball2) = shape2.as_ball() { - contact_manifold_convex_ball(pos12, shape1, ball2, prediction, manifold, false); + contact_manifold_convex_ball( + pos12, + shape1, + ball2, + normal_constraints1, + normal_constraints2, + prediction, + manifold, + false, + ); } } @@ -25,6 +47,8 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( pos12: &Isometry, shape1: &'a S1, ball2: &'a Ball, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, flipped: bool, @@ -51,7 +75,29 @@ pub fn contact_manifold_convex_ball<'a, ManifoldData, ContactData, S1>( } if dist <= ball2.radius + prediction { - let local_n2 = pos12.inverse_transform_vector(&-*local_n1); + let mut local_n2 = pos12.inverse_transform_vector(&-*local_n1); + let uncorrected_local_n2 = local_n2; + + if !(normal_constraints1, normal_constraints2).project_local_normals( + pos12, + local_n1.as_mut_unchecked(), + &mut local_n2, + ) { + // The contact got completely discarded by the normal correction. + manifold.clear(); + return; + } + + // Todo: have this check part of project_normals. + // Don’t allow over-correction for spheres. + let correction_dot_angle = uncorrected_local_n2.dot(&local_n2); + if uncorrected_local_n2.dot(&local_n2) < COS_0_DOT_1_DEGREES { + manifold.clear(); + return; + } + + dist *= correction_dot_angle; + let local_p2 = (local_n2 * ball2.radius).into(); let contact_point = TrackedContact::flipped( proj.point, diff --git a/src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs b/src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs index 2cbf6018..16be647d 100644 --- a/src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs +++ b/src/query/contact_manifolds/contact_manifolds_cuboid_cuboid.rs @@ -95,7 +95,6 @@ pub fn contact_manifold_cuboid_cuboid<'a, ManifoldData, ContactData: Default + C &local_n2, &feature1, &feature2, - prediction, manifold, false, ); diff --git a/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs b/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs index 25576328..3954d950 100644 --- a/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs +++ b/src/query/contact_manifolds/contact_manifolds_cuboid_triangle.rs @@ -1,6 +1,7 @@ #[cfg(feature = "dim2")] use crate::math::Vector; use crate::math::{Isometry, Real}; +use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; use crate::query::{sat, ContactManifold}; use crate::shape::PolygonalFeature; use crate::shape::{Cuboid, Shape, Triangle}; @@ -10,6 +11,8 @@ pub fn contact_manifold_cuboid_triangle_shapes( pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, ) where @@ -21,6 +24,8 @@ pub fn contact_manifold_cuboid_triangle_shapes( &pos12.inverse(), cuboid1, triangle2, + normal_constraints1, + normal_constraints2, prediction, manifold, false, @@ -31,6 +36,8 @@ pub fn contact_manifold_cuboid_triangle_shapes( pos12, cuboid2, triangle1, + normal_constraints2, + normal_constraints1, prediction, manifold, true, @@ -44,6 +51,8 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( pos21: &Isometry, cuboid1: &'a Cuboid, triangle2: &'a Triangle, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, flipped: bool, @@ -94,26 +103,40 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( * and get the polygons to clip. * */ - let mut best_sep = sep1; + let mut normal1 = sep1.1; + let mut dist = sep1.0; if sep2.0 > sep1.0 && sep2.0 > sep3.0 { - best_sep = (sep2.0, pos12 * -sep2.1); + normal1 = pos12 * -sep2.1; + dist = sep2.0; } else if sep3.0 > sep1.0 { - best_sep = sep3; + normal1 = sep3.1; + dist = sep3.0; + } + + // Apply any normal constraint to the separating axis. + let mut normal2 = pos21 * -normal1; + + if !(normal_constraints1, normal_constraints2).project_local_normals( + pos12, + &mut normal1, + &mut normal2, + ) { + manifold.clear(); + return; // The contact got completely discarded by normal correction. } let feature1; let feature2; - let normal2 = pos21 * -best_sep.1; #[cfg(feature = "dim2")] { - feature1 = cuboid1.support_face(best_sep.1); + feature1 = cuboid1.support_face(normal1); feature2 = triangle2.support_face(normal2); } #[cfg(feature = "dim3")] { - feature1 = cuboid1.support_face(best_sep.1); + feature1 = cuboid1.support_face(normal1); feature2 = PolygonalFeature::from(*triangle2); } @@ -123,22 +146,26 @@ pub fn contact_manifold_cuboid_triangle<'a, ManifoldData, ContactData>( manifold.clear(); PolygonalFeature::contacts( - pos12, - pos21, - &best_sep.1, - &normal2, - &feature1, - &feature2, - prediction, - manifold, - flipped, + pos12, pos21, &normal1, &normal2, &feature1, &feature2, manifold, flipped, ); + if normal_constraints1.is_some() || normal_constraints2.is_some() { + // HACK: some normal correction can lead to very incorrect penetration + // depth, e.g., if the other object extends very far toward that direction. + // This is caused by the locality of the convex/convex check. + // I haven’t found a good mathematically robust approach to account for + // that locally, so for now, we eliminate points that are large divergence + // relative to the unconstrained penetration distance. + manifold + .points + .retain(|pt| dist >= 0.0 || pt.dist >= 0.0 || pt.dist >= dist * 5.0); + } + if flipped { manifold.local_n1 = normal2; - manifold.local_n2 = best_sep.1; + manifold.local_n2 = normal1; } else { - manifold.local_n1 = best_sep.1; + manifold.local_n1 = normal1; manifold.local_n2 = normal2; } 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 87764c83..d5bcd58c 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_composite_shape.rs @@ -3,7 +3,7 @@ use crate::math::{Isometry, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; -use crate::query::contact_manifolds::ContactManifoldsWorkspace; +use crate::query::contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints}; use crate::query::query_dispatcher::PersistentQueryDispatcher; use crate::query::visitors::BoundingVolumeIntersectionsVisitor; use crate::query::ContactManifold; @@ -13,9 +13,6 @@ use crate::shape::{HeightField, Shape, SimdCompositeShape}; use crate::utils::hashmap::{Entry, HashMap}; use crate::utils::IsometryOpt; -#[cfg(feature = "dim3")] -use crate::query::contact_manifolds::InternalEdgesFixer; - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", @@ -33,8 +30,6 @@ struct SubDetector { pub struct HeightFieldCompositeShapeContactManifoldsWorkspace { timestamp: bool, sub_detectors: HashMap<(u32, u32), SubDetector>, - #[cfg(feature = "dim3")] - internal_edges: InternalEdgesFixer, } impl HeightFieldCompositeShapeContactManifoldsWorkspace { @@ -96,59 +91,74 @@ pub fn contact_manifolds_heightfield_composite_shape( let ls_aabb1_2 = part1.compute_aabb(pos21).loosened(prediction); let mut leaf_fn2 = |leaf2: &u32| { - composite2.map_part_at(*leaf2, &mut |part_pos2, part_shape2| { - let sub_detector = match workspace.sub_detectors.entry((leaf1, *leaf2)) { - Entry::Occupied(entry) => { - let sub_detector = entry.into_mut(); - let manifold = old_manifolds[sub_detector.manifold_id].take(); - sub_detector.manifold_id = manifolds.len(); - sub_detector.timestamp = new_timestamp; - manifolds.push(manifold); - sub_detector - } - Entry::Vacant(entry) => { - let sub_detector = SubDetector { - manifold_id: manifolds.len(), - timestamp: new_timestamp, - }; - - let mut manifold = ContactManifold::new(); - - if flipped { - manifold.subshape1 = *leaf2; - manifold.subshape2 = leaf1; - manifold.subshape_pos1 = part_pos2.copied(); - } else { - manifold.subshape1 = leaf1; - manifold.subshape2 = *leaf2; - manifold.subshape_pos2 = part_pos2.copied(); - }; - - manifolds.push(manifold); - entry.insert(sub_detector) + composite2.map_part_at( + *leaf2, + &mut |part_pos2, part_shape2, normal_constraints2| { + let sub_detector = match workspace.sub_detectors.entry((leaf1, *leaf2)) { + Entry::Occupied(entry) => { + let sub_detector = entry.into_mut(); + let manifold = old_manifolds[sub_detector.manifold_id].take(); + sub_detector.manifold_id = manifolds.len(); + sub_detector.timestamp = new_timestamp; + manifolds.push(manifold); + sub_detector + } + Entry::Vacant(entry) => { + let sub_detector = SubDetector { + manifold_id: manifolds.len(), + timestamp: new_timestamp, + }; + + let mut manifold = ContactManifold::new(); + + if flipped { + manifold.subshape1 = *leaf2; + manifold.subshape2 = leaf1; + manifold.subshape_pos1 = part_pos2.copied(); + } else { + manifold.subshape1 = leaf1; + manifold.subshape2 = *leaf2; + manifold.subshape_pos2 = part_pos2.copied(); + }; + + manifolds.push(manifold); + entry.insert(sub_detector) + } + }; + + let manifold = &mut manifolds[sub_detector.manifold_id]; + + #[cfg(feature = "dim2")] + let triangle_normals = None::<()>; + #[cfg(feature = "dim3")] + let triangle_normals = heightfield1.triangle_normal_constraints(leaf1); + let normal_constraints1 = triangle_normals + .as_ref() + .map(|proj| proj as &dyn NormalConstraints); + + if flipped { + let _ = dispatcher.contact_manifold_convex_convex( + &part_pos2.inv_mul(pos21), + part_shape2, + &sub_shape1, + normal_constraints2, + normal_constraints1, + prediction, + manifold, + ); + } else { + let _ = dispatcher.contact_manifold_convex_convex( + &part_pos2.prepend_to(pos12), + &sub_shape1, + part_shape2, + normal_constraints1, + normal_constraints2, + prediction, + manifold, + ); } - }; - - let manifold = &mut manifolds[sub_detector.manifold_id]; - - if flipped { - let _ = dispatcher.contact_manifold_convex_convex( - &part_pos2.inv_mul(pos21), - part_shape2, - &sub_shape1, - prediction, - manifold, - ); - } else { - let _ = dispatcher.contact_manifold_convex_convex( - &part_pos2.prepend_to(pos12), - &sub_shape1, - part_shape2, - prediction, - manifold, - ); - } - }); + }, + ); true }; @@ -160,16 +170,6 @@ pub fn contact_manifolds_heightfield_composite_shape( workspace .sub_detectors .retain(|_, detector| detector.timestamp == new_timestamp); - - #[cfg(feature = "dim3")] - { - workspace.internal_edges.remove_invalid_contacts( - manifolds, - flipped, - |id| heightfield1.triangle_at_id(id).unwrap(), - |id| heightfield1.triangle_vids_at_id(id).unwrap(), - ) - } } impl WorkspaceData for HeightFieldCompositeShapeContactManifoldsWorkspace { diff --git a/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs b/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs index 15e517b6..bf63ed58 100644 --- a/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_heightfield_shape.rs @@ -11,9 +11,6 @@ use crate::shape::Capsule; use crate::shape::{HeightField, Shape}; use crate::utils::hashmap::{Entry, HashMap}; -#[cfg(feature = "dim3")] -use crate::query::contact_manifolds::InternalEdgesFixer; - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", @@ -31,8 +28,6 @@ struct SubDetector { pub struct HeightFieldShapeContactManifoldsWorkspace { timestamp: bool, sub_detectors: HashMap, - #[cfg(feature = "dim3")] - internal_edges: InternalEdgesFixer, } impl HeightFieldShapeContactManifoldsWorkspace { @@ -156,11 +151,20 @@ pub fn contact_manifolds_heightfield_shape( let manifold = &mut manifolds[sub_detector.manifold_id]; + #[cfg(feature = "dim2")] + let pseudo_normals = None::<()>; + #[cfg(feature = "dim3")] + let pseudo_normals = heightfield1.triangle_normal_constraints(i); + + let normal_constraints1 = pseudo_normals.as_ref().map(|pn| pn as &_); + if flipped { let _ = dispatcher.contact_manifold_convex_convex( &pos12.inverse(), shape2, &sub_shape1, + None, + normal_constraints1, prediction, manifold, ); @@ -169,6 +173,8 @@ pub fn contact_manifolds_heightfield_shape( pos12, &sub_shape1, shape2, + normal_constraints1, + None, prediction, manifold, ); @@ -178,16 +184,6 @@ pub fn contact_manifolds_heightfield_shape( workspace .sub_detectors .retain(|_, detector| detector.timestamp == new_timestamp); - - #[cfg(feature = "dim3")] - { - workspace.internal_edges.remove_invalid_contacts( - manifolds, - flipped, - |id| heightfield1.triangle_at_id(id).unwrap(), - |id| heightfield1.triangle_vids_at_id(id).unwrap(), - ) - } } impl WorkspaceData for HeightFieldShapeContactManifoldsWorkspace { diff --git a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs index 11b5653e..bb13a838 100644 --- a/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs +++ b/src/query/contact_manifolds/contact_manifolds_pfm_pfm.rs @@ -1,4 +1,5 @@ use crate::math::{Isometry, Real}; +use crate::query::contact_manifolds::{NormalConstraints, NormalConstraintsPair}; use crate::query::{ self, gjk::{GJKResult, VoronoiSimplex}, @@ -13,6 +14,8 @@ pub fn contact_manifold_pfm_pfm_shapes( pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, ) where @@ -27,8 +30,10 @@ pub fn contact_manifold_pfm_pfm_shapes( pos12, pfm1, border_radius1, + normal_constraints1, pfm2, border_radius2, + normal_constraints2, prediction, manifold, ); @@ -40,8 +45,10 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( pos12: &Isometry, pfm1: &'a S1, border_radius1: Real, + normal_constraints1: Option<&dyn NormalConstraints>, pfm2: &'a S2, border_radius2: Real, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, ) where @@ -73,8 +80,19 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( match contact { GJKResult::ClosestPoints(p1, p2_1, dir) => { - let local_n1 = dir; - let local_n2 = pos12.inverse_transform_unit_vector(&-dir); + let mut local_n1 = dir; + let mut local_n2 = pos12.inverse_transform_unit_vector(&-dir); + let dist = (p2_1 - p1).dot(&local_n1); + + if !(normal_constraints1, normal_constraints2).project_local_normals( + pos12, + local_n1.as_mut_unchecked(), + local_n2.as_mut_unchecked(), + ) { + // The contact got completely discarded by the normal correction. + return; + } + let mut feature1 = PolygonalFeature::default(); let mut feature2 = PolygonalFeature::default(); pfm1.local_support_feature(&local_n1, &mut feature1); @@ -87,27 +105,36 @@ pub fn contact_manifold_pfm_pfm<'a, ManifoldData, ContactData, S1, S2>( &local_n2, &feature1, &feature2, - total_prediction, manifold, false, ); - if (cfg!(feature = "dim2") && manifold.points.is_empty()) - // TODO: this test is a workaround until we figure-out a way to - // determine the feature ids for the GJK/EPA contact. - || pfm1.is_convex_polyhedron() - || pfm2.is_convex_polyhedron() + if (cfg!(feature = "dim3") || cfg!(feature = "dim2") && manifold.points.is_empty()) + // If normal constraints changed the GJK direction, it is no longer valid so we cant use it for this additional contact. + && local_n1 == dir { let contact = TrackedContact::new( p1, pos12.inverse_transform_point(&p2_1), PackedFeatureId::UNKNOWN, // TODO: We don't know what features are involved. PackedFeatureId::UNKNOWN, - (p2_1 - p1).dot(&dir), + (p2_1 - p1).dot(&local_n1), ); manifold.points.push(contact); } + if normal_constraints1.is_some() || normal_constraints2.is_some() { + // HACK: some normal correction can lead to very incorrect penetration + // depth, e.g., if the other object extends very far toward that direction. + // This is caused by the locality of the convex/convex check. + // I haven’t found a good mathematically robust approach to account for + // that locally, so for now, we eliminate points that are large divergence + // relative to the unconstrained penetration distance. + manifold + .points + .retain(|pt| dist >= 0.0 || pt.dist >= 0.0 || pt.dist >= dist * 5.0); + } + // Adjust points to take the radius into account. if border_radius1 != 0.0 || border_radius2 != 0.0 { for contact in &mut manifold.points { diff --git a/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs b/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs index 3516c935..763eb61b 100644 --- a/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs +++ b/src/query/contact_manifolds/contact_manifolds_trimesh_shape.rs @@ -3,7 +3,8 @@ use crate::math::{Isometry, Real}; use crate::query::contact_manifolds::contact_manifolds_workspace::{ TypedWorkspaceData, WorkspaceData, }; -use crate::query::contact_manifolds::{ContactManifoldsWorkspace, InternalEdgesFixer}; +use crate::query::contact_manifolds::ContactManifoldsWorkspace; +use crate::query::details::NormalConstraints; use crate::query::query_dispatcher::PersistentQueryDispatcher; use crate::query::ContactManifold; use crate::shape::{Shape, TriMesh}; @@ -19,7 +20,6 @@ pub struct TriMeshShapeContactManifoldsWorkspace { interferences: Vec, local_aabb2: Aabb, old_interferences: Vec, - internal_edges: InternalEdgesFixer, } impl Default for TriMeshShapeContactManifoldsWorkspace { @@ -34,7 +34,6 @@ impl TriMeshShapeContactManifoldsWorkspace { interferences: Vec::new(), local_aabb2: Aabb::new_invalid(), old_interferences: Vec::new(), - internal_edges: InternalEdgesFixer::default(), } } } @@ -187,32 +186,33 @@ pub fn contact_manifolds_trimesh_shape( let manifold = &mut manifolds[i]; let triangle1 = trimesh1.triangle(*triangle_id); + let triangle_normals1 = trimesh1.triangle_normal_constraints(*triangle_id); + let normal_constraints1 = triangle_normals1 + .as_ref() + .map(|proj| proj as &dyn NormalConstraints); if flipped { let _ = dispatcher.contact_manifold_convex_convex( &pos12.inverse(), shape2, &triangle1, + None, + normal_constraints1, prediction, manifold, ); } else { - let _ = dispatcher - .contact_manifold_convex_convex(pos12, &triangle1, shape2, prediction, manifold); + let _ = dispatcher.contact_manifold_convex_convex( + pos12, + &triangle1, + shape2, + normal_constraints1, + None, + prediction, + manifold, + ); } } - - /* - * - * Deal with internal edges. - * - */ - workspace.internal_edges.remove_invalid_contacts( - manifolds, - flipped, - |id| trimesh1.triangle(id), - |id| trimesh1.indices()[id as usize], - ); } impl WorkspaceData for TriMeshShapeContactManifoldsWorkspace { diff --git a/src/query/contact_manifolds/internal_edges_fixer.rs b/src/query/contact_manifolds/internal_edges_fixer.rs deleted file mode 100644 index 7df434af..00000000 --- a/src/query/contact_manifolds/internal_edges_fixer.rs +++ /dev/null @@ -1,167 +0,0 @@ -use crate::query::ContactManifold; -use crate::shape::Triangle; -use crate::utils::hashmap::HashMap; - -#[cfg(feature = "dim3")] -use crate::math::Real; - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[cfg_attr( - feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) -)] -#[derive(Default, Clone)] -#[allow(dead_code)] // We will need these for 2D too in the future. -pub struct InternalEdgesFixer { - delayed_manifolds: Vec, - vertex_set: HashMap, -} - -impl InternalEdgesFixer { - #[cfg(feature = "dim2")] - pub fn remove_invalid_contacts( - &mut self, - _manifolds: &mut [ContactManifold], - _flipped: bool, - _get_triangle: impl Fn(u32) -> Triangle, - _get_triangle_indices: impl Fn(u32) -> [u32; 3], - ) where - ManifoldData: Default, - ContactData: Default + Copy, - { - // Not yet implemented. - } - - #[cfg(feature = "dim3")] - pub fn remove_invalid_contacts( - &mut self, - manifolds: &mut [ContactManifold], - flipped: bool, - get_triangle: impl Fn(u32) -> Triangle, - get_triangle_indices: impl Fn(u32) -> [u32; 3], - ) where - ManifoldData: Default, - ContactData: Default + Copy, - { - use crate::shape::FeatureId; - use std::cmp::Ordering; - - // 1. Ingest all the face contacts. - for (mid, manifold) in manifolds.iter().enumerate() { - if let Some(deepest) = manifold.find_deepest_contact() { - let tri_fid = if flipped { deepest.fid2 } else { deepest.fid1 }; - - if tri_fid.is_face() { - let (tri_idx, tri) = if flipped { - ( - get_triangle_indices(manifold.subshape2), - get_triangle(manifold.subshape2), - ) - } else { - ( - get_triangle_indices(manifold.subshape1), - get_triangle(manifold.subshape1), - ) - }; - - let normal = if flipped { - manifold.local_n2 - } else { - manifold.local_n1 - }; - - if let Some(tri_normal) = tri.normal() { - // We check normal collinearity with an epsilon because sometimes, - // because of rounding errors, a contact may be identified as a face - // contact where it’s really just an edge contact. - if normal.dot(&tri_normal).abs() > 1.0 - 1.0e-4 { - let _ = self.vertex_set.insert(tri_idx[0], ()); - let _ = self.vertex_set.insert(tri_idx[1], ()); - let _ = self.vertex_set.insert(tri_idx[2], ()); - // This as an actual face contact, continue without pushing to delayed_manifolds. - continue; - } - } - } - - // NOTE: if we reach this line, then the contact isn’t an actual face contact. - self.delayed_manifolds.push(mid as u32); - } - } - - // 2. Order by distance. - self.delayed_manifolds.sort_by(|a, b| { - let a = &manifolds[*a as usize]; - let b = &manifolds[*b as usize]; - - let dist_a = a - .find_deepest_contact() - .map(|c| c.dist) - .unwrap_or(Real::MAX); - let dist_b = b - .find_deepest_contact() - .map(|c| c.dist) - .unwrap_or(Real::MAX); - - dist_a.partial_cmp(&dist_b).unwrap_or(Ordering::Equal) - }); - - // 3. Deal with the edge/vertex contacts. - for mid in &self.delayed_manifolds { - let manifold = &mut manifolds[*mid as usize]; - - let tri_idx = if flipped { - get_triangle_indices(manifold.subshape2) - } else { - get_triangle_indices(manifold.subshape1) - }; - - manifold.points.retain(|pt| { - let tri_fid = if flipped { pt.fid2 } else { pt.fid1 }; - - match tri_fid.unpack() { - FeatureId::Face(_) => { - // This is a "false face" contact (a contact identified as face because - // of rounding errors but it’s probably just an edge contact. - // We don’t know exactly which edge it is, so we just ignore it - // if any of the vertices already exist in the set. - !self.vertex_set.contains_key(&tri_idx[0]) - && !self.vertex_set.contains_key(&tri_idx[1]) - && !self.vertex_set.contains_key(&tri_idx[2]) - } - FeatureId::Edge(id) => { - !self.vertex_set.contains_key(&tri_idx[id as usize]) - || !self - .vertex_set - .contains_key(&tri_idx[(id as usize + 1) % 3]) - } - FeatureId::Vertex(id) => !self.vertex_set.contains_key(&tri_idx[id as usize]), - FeatureId::Unknown => { - // We don’t know the feature type here, so we just ignore it - // if any of the vertices already exist in the set. - !self.vertex_set.contains_key(&tri_idx[0]) - && !self.vertex_set.contains_key(&tri_idx[1]) - && !self.vertex_set.contains_key(&tri_idx[2]) - } - } - }); - - // if !manifold.points.is_empty() { - // let normal = if flipped { - // manifold.local_n2 - // } else { - // manifold.local_n1 - // }; - // println!("Keeping other contact: {:?}, {:?}", tri_idx, normal); - // } - - let _ = self.vertex_set.insert(tri_idx[0], ()); - let _ = self.vertex_set.insert(tri_idx[1], ()); - let _ = self.vertex_set.insert(tri_idx[2], ()); - } - - self.vertex_set.clear(); - self.delayed_manifolds.clear(); - } -} diff --git a/src/query/contact_manifolds/mod.rs b/src/query/contact_manifolds/mod.rs index 3be0ed42..4adf9ea0 100644 --- a/src/query/contact_manifolds/mod.rs +++ b/src/query/contact_manifolds/mod.rs @@ -35,6 +35,7 @@ pub use self::contact_manifolds_trimesh_shape::{ pub use self::contact_manifolds_workspace::{ ContactManifoldsWorkspace, TypedWorkspaceData, WorkspaceData, }; +pub use self::normals_contraint::{NormalConstraints, NormalConstraintsPair}; use { self::contact_manifolds_composite_shape_composite_shape::CompositeShapeCompositeShapeContactManifoldsWorkspace, @@ -42,7 +43,6 @@ use { self::contact_manifolds_heightfield_composite_shape::HeightFieldCompositeShapeContactManifoldsWorkspace, self::contact_manifolds_heightfield_shape::HeightFieldShapeContactManifoldsWorkspace, self::contact_manifolds_trimesh_shape::TriMeshShapeContactManifoldsWorkspace, - self::internal_edges_fixer::InternalEdgesFixer, }; mod contact_manifold; @@ -60,4 +60,4 @@ mod contact_manifolds_heightfield_shape; mod contact_manifolds_pfm_pfm; mod contact_manifolds_trimesh_shape; mod contact_manifolds_workspace; -mod internal_edges_fixer; +mod normals_contraint; diff --git a/src/query/contact_manifolds/normals_contraint.rs b/src/query/contact_manifolds/normals_contraint.rs new file mode 100644 index 00000000..1ecc19ba --- /dev/null +++ b/src/query/contact_manifolds/normals_contraint.rs @@ -0,0 +1,126 @@ +use crate::math::{Isometry, Real, Vector}; + +// NOTE: the 'static requirement is only needed for the following impl to work: +// impl<'a> TypedSimdCompositeShape for dyn SimdCompositeShape +// We can probably work something out if that becomes too restrictive in +// the future. +/// Constraints of contact normals, generally for internal edges resolution. +/// +/// This trait used for applying constraints on normal direction for contact manifolds calculation. +/// Non-convex shapes will generally simplify collision-detection as a collection of simpler +/// convex-based collision-detection problems. However, that partial convex formulation allows +/// some contact normals that are theoretically impossible (in a convex analysis sense). The normal +/// constraints aims to correct/remove invalid normals, avoiding some artifacts in physics +/// simulations. In particular, this addresses the well-known "internal edges" problem on triangle +/// meshes and heightfields. +pub trait NormalConstraints: 'static { + /// Corrects in-place or discards the specified normal (assumed to be unit-sized) based on the + /// constraints encoded by `Self`. + /// + /// If this method returns `false` then the contacts associated to that normal should be + /// considered invalid and be ignored by the collision-detection pipeline. + fn project_local_normal_mut(&self, normal: &mut Vector) -> bool; + /// Corrects or discards the specified normal (assumed to be unit-sized) based on the constraints + /// encoded by `Self`. + /// + /// If this method returns `None` then the contacts associated to that normal should be + /// considered invalid and be ignored by the collision-detection pipeline. + fn project_local_normal(&self, mut normal: Vector) -> Option> { + self.project_local_normal_mut(&mut normal).then_some(normal) + } + + // NOTE: despite this not taking an UnitVector, the normal is + // assumed to be unit-sized. + /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the + /// assumption that `normal1` is in the same coordinates space as `Self`. + /// + /// The `normal2` will be modified to be equal to `-normal1` expressed in the local coordinate + /// space of the second shape. + /// + /// If this method returns `false` then the contacts associated to that normal should be + /// considered invalid and be ignored by the collision-detection pipeline. + fn project_local_normal1( + &self, + pos12: &Isometry, + normal1: &mut Vector, + normal2: &mut Vector, + ) -> bool { + if !self.project_local_normal_mut(normal1) { + return false; + } + + *normal2 = pos12.inverse_transform_vector(&-*normal1); + + true + } + + /// Applies normal correction to the unit vectors `normal1` and `normal2` based on the + /// assumption that `normal2` is in the same coordinates space as `Self`. + /// + /// The `normal1` will be modified to be equal to `-normal2` expressed in the local coordinate + /// space of the first shape. + /// + /// If this method returns `false` then the contacts associated to that normal should be + /// considered invalid and be ignored by the collision-detection pipeline. + fn project_local_normal2( + &self, + pos12: &Isometry, + normal1: &mut Vector, + normal2: &mut Vector, + ) -> bool { + if !self.project_local_normal_mut(normal2) { + return false; + } + + *normal1 = pos12 * (-*normal2); + + true + } +} + +impl NormalConstraints for () { + fn project_local_normal_mut(&self, _: &mut Vector) -> bool { + true + } +} + +/// A pair of [`NormalConstraints`]. +pub trait NormalConstraintsPair { + /// Applies the normal constraints to `normal1` and `normal2`. + /// + /// This trait is mostly used internally to combine two [`NormalConstraints`] conveniently. + fn project_local_normals( + &self, + pos12: &Isometry, + normal1: &mut Vector, + normal2: &mut Vector, + ) -> bool; +} + +// We generally use Option<&dyn NormalConstraints> instead of the naked +// trait-object in our codebase so providing this impl is convenient. +impl NormalConstraintsPair + for ( + Option<&dyn NormalConstraints>, + Option<&dyn NormalConstraints>, + ) +{ + fn project_local_normals( + &self, + pos12: &Isometry, + normal1: &mut Vector, + normal2: &mut Vector, + ) -> bool { + if let Some(proj) = self.0 { + if !proj.project_local_normal1(pos12, normal1, normal2) { + return false; + } + } + + if let Some(proj) = self.1 { + proj.project_local_normal2(pos12, normal1, normal2) + } else { + true + } + } +} diff --git a/src/query/default_query_dispatcher.rs b/src/query/default_query_dispatcher.rs index 121a34e6..2b826eca 100644 --- a/src/query/default_query_dispatcher.rs +++ b/src/query/default_query_dispatcher.rs @@ -5,7 +5,8 @@ use crate::query::{ }; #[cfg(feature = "std")] use crate::query::{ - contact_manifolds::ContactManifoldsWorkspace, query_dispatcher::PersistentQueryDispatcher, + contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints}, + query_dispatcher::PersistentQueryDispatcher, ContactManifold, }; use crate::shape::{HalfSpace, Segment, Shape, ShapeType}; @@ -525,6 +526,8 @@ where pos12, shape1, shape2, + None, + None, prediction, &mut manifolds[0], ); @@ -540,6 +543,8 @@ where pos12: &Isometry, shape1: &dyn Shape, shape2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, ) -> Result<(), Unsupported> { @@ -563,12 +568,12 @@ where contact_manifold_capsule_capsule_shapes(pos12, shape1, shape2, prediction, manifold) } (_, ShapeType::Ball) | (ShapeType::Ball, _) => { - contact_manifold_convex_ball_shapes(pos12, shape1, shape2, prediction, manifold) + contact_manifold_convex_ball_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold) } // (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => // contact_manifold_cuboid_capsule_shapes(pos12, shape1, shape2, prediction, manifold), (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => { - contact_manifold_cuboid_triangle_shapes(pos12, shape1, shape2, prediction, manifold) + contact_manifold_cuboid_triangle_shapes(pos12, shape1, shape2, normal_constraints1, normal_constraints2, prediction, manifold) } (ShapeType::HalfSpace, _) => { if let Some((pfm2, border_radius2)) = shape2.as_polygonal_feature_map() { @@ -606,7 +611,7 @@ where shape2.as_polygonal_feature_map(), ) { contact_manifold_pfm_pfm( - pos12, pfm1.0, pfm1.1, pfm2.0, pfm2.1, prediction, manifold, + pos12, pfm1.0, pfm1.1, normal_constraints1, pfm2.0, pfm2.1, normal_constraints2, prediction, manifold, ) } else { return Err(Unsupported); diff --git a/src/query/distance/distance_composite_shape_shape.rs b/src/query/distance/distance_composite_shape_shape.rs index 1e7b2e18..7e65916b 100644 --- a/src/query/distance/distance_composite_shape_shape.rs +++ b/src/query/distance/distance_composite_shape_shape.rs @@ -3,7 +3,7 @@ use crate::math::{Isometry, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH}; use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; use crate::query::QueryDispatcher; use crate::shape::{Shape, TypedSimdCompositeShape}; -use crate::utils::{DefaultStorage, IsometryOpt}; +use crate::utils::IsometryOpt; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; /// Smallest distance between a composite shape and any other shape. @@ -15,7 +15,7 @@ pub fn distance_composite_shape_shape( ) -> Real where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { let mut visitor = CompositeShapeAgainstAnyDistanceVisitor::new(dispatcher, pos12, g1, g2); g1.typed_qbvh() @@ -34,7 +34,7 @@ pub fn distance_shape_composite_shape( ) -> Real where D: QueryDispatcher, - G2: TypedSimdCompositeShape, + G2: TypedSimdCompositeShape, { distance_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1) } @@ -75,7 +75,7 @@ impl<'a, D: ?Sized, G1: ?Sized> SimdBestFirstVisitor for CompositeShapeAgainstAnyDistanceVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { type Result = (G1::PartId, Real); @@ -103,7 +103,7 @@ where if (bitmask & (1 << ii)) != 0 && data[ii].is_some() { let part_id = *data[ii].unwrap(); let mut dist = Ok(0.0); - self.g1.map_untyped_part_at(part_id, |part_pos1, g1| { + self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { dist = self.dispatcher .distance(&part_pos1.inv_mul(self.pos12), g1, self.g2); diff --git a/src/query/intersection_test/intersection_test_composite_shape_shape.rs b/src/query/intersection_test/intersection_test_composite_shape_shape.rs index 9cc4d90d..04779327 100644 --- a/src/query/intersection_test/intersection_test_composite_shape_shape.rs +++ b/src/query/intersection_test/intersection_test_composite_shape_shape.rs @@ -7,7 +7,7 @@ use crate::partitioning::{ }; use crate::query::QueryDispatcher; use crate::shape::{Shape, TypedSimdCompositeShape}; -use crate::utils::{DefaultStorage, IsometryOpt}; +use crate::utils::IsometryOpt; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; /// Intersection test between a composite shape (`Mesh`, `Compound`) and any other shape. @@ -19,7 +19,7 @@ pub fn intersection_test_composite_shape_shape( ) -> bool where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { let mut visitor = IntersectionCompositeShapeShapeVisitor::new(dispatcher, pos12, g1, g2); @@ -36,7 +36,7 @@ pub fn intersection_test_shape_composite_shape( ) -> bool where D: QueryDispatcher, - G2: TypedSimdCompositeShape, + G2: TypedSimdCompositeShape, { intersection_test_composite_shape_shape(dispatcher, &pos12.inverse(), g2, g1) } @@ -56,7 +56,7 @@ pub struct IntersectionCompositeShapeShapeVisitor<'a, D: ?Sized, G1: ?Sized + 'a impl<'a, D: ?Sized, G1: ?Sized> IntersectionCompositeShapeShapeVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { /// Initialize a visitor for checking if a composite-shape and a shape intersect. pub fn new( @@ -82,7 +82,7 @@ impl<'a, D: ?Sized, G1: ?Sized> SimdVisitor for IntersectionCompositeShapeShapeVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { fn visit( &mut self, @@ -99,7 +99,7 @@ where if (bitmask & (1 << ii)) != 0 { let Some(data) = data else { continue }; let part_id = *data; - self.g1.map_untyped_part_at(part_id, |part_pos1, g1| { + self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { found_intersection = self.dispatcher.intersection_test( &part_pos1.inv_mul(self.pos12), g1, @@ -134,7 +134,7 @@ pub struct IntersectionCompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?S impl<'a, D: ?Sized, G1: ?Sized> IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { /// Initialize a visitor for checking if a composite-shape and a shape intersect. pub fn new( @@ -160,7 +160,7 @@ impl<'a, D: ?Sized, G1: ?Sized> SimdBestFirstVisitor for IntersectionCompositeShapeShapeBestFirstVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { type Result = (G1::PartId, bool); @@ -185,7 +185,7 @@ where for (ii, data) in data.into_iter().enumerate() { if (bitmask & (1 << ii)) != 0 && data.is_some() { let part_id = *data.unwrap(); - self.g1.map_untyped_part_at(part_id, |part_pos1, g1| { + self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { found_intersection = self.dispatcher.intersection_test( &part_pos1.inv_mul(self.pos12), g1, diff --git a/src/query/mod.rs b/src/query/mod.rs index f95dc1cf..763d8cd0 100644 --- a/src/query/mod.rs +++ b/src/query/mod.rs @@ -63,6 +63,7 @@ mod ray; pub mod sat; mod split; mod time_of_impact; +#[cfg(feature = "std")] pub mod visitors; /// Queries dedicated to specific pairs of shapes. diff --git a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_composite_shape_shape.rs b/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_composite_shape_shape.rs index 4ec67a9b..5a337d67 100644 --- a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_composite_shape_shape.rs +++ b/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_composite_shape_shape.rs @@ -3,7 +3,6 @@ use crate::math::{Real, SimdBool, SimdReal, SIMD_WIDTH}; use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; use crate::query::{self, details::NonlinearTOIMode, NonlinearRigidMotion, QueryDispatcher, TOI}; use crate::shape::{Ball, Shape, TypedSimdCompositeShape}; -use crate::utils::DefaultStorage; use simba::simd::SimdValue; /// Time Of Impact of a composite shape with any other shape, under a rigid motion (translation + rotation). @@ -19,7 +18,7 @@ pub fn nonlinear_time_of_impact_composite_shape_shape( ) -> Option where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new( dispatcher, @@ -50,7 +49,7 @@ pub fn nonlinear_time_of_impact_shape_composite_shape( ) -> Option where D: QueryDispatcher, - G2: TypedSimdCompositeShape, + G2: TypedSimdCompositeShape, { nonlinear_time_of_impact_composite_shape_shape( dispatcher, @@ -82,7 +81,7 @@ pub struct NonlinearTOICompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?S impl<'a, D: ?Sized, G1: ?Sized> NonlinearTOICompositeShapeShapeBestFirstVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { /// Initializes visitor used to determine the non-linear time of impact between /// a composite shape and another shape. @@ -114,7 +113,7 @@ impl<'a, D: ?Sized, G1: ?Sized> SimdBestFirstVisitor for NonlinearTOICompositeShapeShapeBestFirstVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { type Result = (G1::PartId, TOI); @@ -155,7 +154,7 @@ where if let Some(data) = data { if toi.toi < best && data[ii].is_some() { let part_id = *data[ii].unwrap(); - self.g1.map_untyped_part_at(part_id, |part_pos1, g1| { + self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { let toi = if let Some(part_pos1) = part_pos1 { self.dispatcher .nonlinear_time_of_impact( diff --git a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_support_map_support_map.rs b/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_support_map_support_map.rs index 3d55f4c8..d906dd1e 100644 --- a/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_support_map_support_map.rs +++ b/src/query/nonlinear_time_of_impact/nonlinear_time_of_impact_support_map_support_map.rs @@ -198,7 +198,15 @@ where break; } } - ClosestPoints::Disjoint => unreachable!(), + ClosestPoints::Disjoint => { + // TODO: this case should be unreachable and needs some debugging + // see: https://github.com/dimforge/parry/issues/176 + log::error!( + "Closest points not found despite setting the max distance to infinity." + ); + result.status = TOIStatus::Failed; + break; + } } } diff --git a/src/query/point/mod.rs b/src/query/point/mod.rs index f59160b1..2e3e3067 100644 --- a/src/query/point/mod.rs +++ b/src/query/point/mod.rs @@ -14,6 +14,7 @@ mod point_aabb; mod point_ball; mod point_bounding_sphere; mod point_capsule; +#[cfg(feature = "std")] mod point_composite_shape; #[cfg(feature = "dim3")] mod point_cone; @@ -21,6 +22,7 @@ mod point_cuboid; #[cfg(feature = "dim3")] mod point_cylinder; mod point_halfspace; +#[cfg(feature = "std")] mod point_heightfield; #[doc(hidden)] pub mod point_query; diff --git a/src/query/point/point_composite_shape.rs b/src/query/point/point_composite_shape.rs index f50ca15b..d28dd5b5 100644 --- a/src/query/point/point_composite_shape.rs +++ b/src/query/point/point_composite_shape.rs @@ -6,19 +6,13 @@ use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; use crate::query::visitors::CompositePointContainmentTest; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; use crate::shape::{ - FeatureId, GenericTriMesh, SegmentPointLocation, TriMeshStorage, TrianglePointLocation, - TypedSimdCompositeShape, + FeatureId, SegmentPointLocation, TriMesh, TrianglePointLocation, TypedSimdCompositeShape, }; use na; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; -#[cfg(feature = "dim3")] -use crate::utils::Array1; - -#[cfg(feature = "std")] use crate::shape::{Compound, Polyline}; -#[cfg(feature = "std")] impl PointQuery for Polyline { #[inline] fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { @@ -48,7 +42,7 @@ impl PointQuery for Polyline { } } -impl PointQuery for GenericTriMesh { +impl PointQuery for TriMesh { #[inline] fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { self.project_local_point_and_get_location(point, solid).0 @@ -104,7 +98,6 @@ impl PointQuery for GenericTriMesh { } } -#[cfg(feature = "std")] impl PointQuery for Compound { #[inline] fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { @@ -128,7 +121,6 @@ impl PointQuery for Compound { } } -#[cfg(feature = "std")] impl PointQueryWithLocation for Polyline { type Location = (u32, SegmentPointLocation); @@ -144,7 +136,7 @@ impl PointQueryWithLocation for Polyline { } } -impl PointQueryWithLocation for GenericTriMesh { +impl PointQueryWithLocation for TriMesh { type Location = (u32, TrianglePointLocation); #[inline] @@ -181,13 +173,13 @@ impl PointQueryWithLocation for GenericTriMesh } TrianglePointLocation::OnEdge(i, _) => pseudo_normals .edges_pseudo_normal - .get_at(part_id as usize) + .get(part_id as usize) .map(|pn| pn[i as usize]), TrianglePointLocation::OnVertex(i) => { let idx = self.indices()[part_id as usize]; pseudo_normals .vertices_pseudo_normal - .get_at(idx[i as usize] as usize) + .get(idx[i as usize] as usize) .copied() } }; @@ -255,7 +247,7 @@ macro_rules! gen_visitor( if (bitmask & (1 << ii)) != 0 && data[ii].is_some() { let mut is_inside = false; let subshape_id = *data[ii].unwrap(); - self.shape.map_typed_part_at(subshape_id, |part_pos, part_shape| { + self.shape.map_typed_part_at(subshape_id, |part_pos, part_shape, _| { let (proj $(, $extra_info)*) = if let Some(part_pos) = part_pos { part_shape.$project_point( part_pos, diff --git a/src/query/point/point_heightfield.rs b/src/query/point/point_heightfield.rs index 10c1da3e..25507d24 100644 --- a/src/query/point/point_heightfield.rs +++ b/src/query/point/point_heightfield.rs @@ -1,11 +1,11 @@ use crate::bounding_volume::Aabb; use crate::math::{Point, Real, Vector}; use crate::query::{PointProjection, PointQuery, PointQueryWithLocation}; -use crate::shape::{FeatureId, GenericHeightField, HeightFieldStorage, TrianglePointLocation}; +use crate::shape::{FeatureId, HeightField, TrianglePointLocation}; #[cfg(not(feature = "std"))] use na::ComplexField; // For sqrt. -impl PointQuery for GenericHeightField { +impl PointQuery for HeightField { fn project_local_point_with_max_dist( &self, pt: &Point, @@ -71,7 +71,7 @@ impl PointQuery for GenericHeightField { } } -impl PointQueryWithLocation for GenericHeightField { +impl PointQueryWithLocation for HeightField { type Location = (usize, TrianglePointLocation); #[inline] diff --git a/src/query/query_dispatcher.rs b/src/query/query_dispatcher.rs index 8fb5891a..203e73f0 100644 --- a/src/query/query_dispatcher.rs +++ b/src/query/query_dispatcher.rs @@ -1,6 +1,9 @@ use crate::math::{Isometry, Real, Vector}; #[cfg(feature = "std")] -use crate::query::{contact_manifolds::ContactManifoldsWorkspace, ContactManifold}; +use crate::query::{ + contact_manifolds::{ContactManifoldsWorkspace, NormalConstraints}, + ContactManifold, +}; use crate::query::{ClosestPoints, Contact, NonlinearRigidMotion, Unsupported, TOI}; use crate::shape::Shape; @@ -30,6 +33,8 @@ pub trait PersistentQueryDispatcher: QueryD pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, ) -> Result<(), Unsupported>; @@ -222,6 +227,8 @@ where pos12: &Isometry, g1: &dyn Shape, g2: &dyn Shape, + normal_constraints1: Option<&dyn NormalConstraints>, + normal_constraints2: Option<&dyn NormalConstraints>, prediction: Real, manifold: &mut ContactManifold, ) -> ()); diff --git a/src/query/ray/mod.rs b/src/query/ray/mod.rs index 654a0479..71c90f7b 100644 --- a/src/query/ray/mod.rs +++ b/src/query/ray/mod.rs @@ -22,6 +22,7 @@ mod ray_bounding_sphere; mod ray_composite_shape; mod ray_cuboid; mod ray_halfspace; +#[cfg(feature = "std")] mod ray_heightfield; mod ray_round_shape; mod ray_support_map; diff --git a/src/query/ray/ray.rs b/src/query/ray/ray.rs index 7efe94ac..e6a4adc2 100644 --- a/src/query/ray/ray.rs +++ b/src/query/ray/ray.rs @@ -14,7 +14,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[repr(C)] pub struct Ray { /// Starting point of the ray. diff --git a/src/query/ray/ray_composite_shape.rs b/src/query/ray/ray_composite_shape.rs index d241d159..06188d0f 100644 --- a/src/query/ray/ray_composite_shape.rs +++ b/src/query/ray/ray_composite_shape.rs @@ -3,7 +3,6 @@ use crate::math::{Real, SimdBool, SimdReal, SIMD_WIDTH}; use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; use crate::query::{Ray, RayCast, RayIntersection, SimdRay}; use crate::shape::{Compound, FeatureId, Polyline, TriMesh, TypedSimdCompositeShape}; -use crate::utils::DefaultStorage; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; impl RayCast for TriMesh { @@ -121,7 +120,7 @@ impl<'a, S> RayCompositeShapeToiBestFirstVisitor<'a, S> { impl<'a, S> SimdBestFirstVisitor for RayCompositeShapeToiBestFirstVisitor<'a, S> where - S: TypedSimdCompositeShape, + S: TypedSimdCompositeShape, { type Result = (S::PartId, Real); @@ -146,7 +145,7 @@ where if (bitmask & (1 << ii)) != 0 && data[ii].is_some() { let part_id = *data[ii].unwrap(); self.shape - .map_typed_part_at(part_id, |part_pos, part_shape| { + .map_typed_part_at(part_id, |part_pos, part_shape, _| { let toi = if let Some(part_pos) = part_pos { part_shape.cast_ray(part_pos, self.ray, self.max_toi, self.solid) } else { @@ -201,7 +200,7 @@ impl<'a, S> RayCompositeShapeToiAndNormalBestFirstVisitor<'a, S> { impl<'a, S> SimdBestFirstVisitor for RayCompositeShapeToiAndNormalBestFirstVisitor<'a, S> where - S: TypedSimdCompositeShape, + S: TypedSimdCompositeShape, { type Result = (S::PartId, RayIntersection); @@ -225,7 +224,7 @@ where for ii in 0..SIMD_WIDTH { if (bitmask & (1 << ii)) != 0 && data[ii].is_some() { self.shape - .map_typed_part_at(*data[ii].unwrap(), |part_pos, part_shape| { + .map_typed_part_at(*data[ii].unwrap(), |part_pos, part_shape, _| { let result = if let Some(part_pos) = part_pos { part_shape.cast_ray_and_get_normal( part_pos, diff --git a/src/query/ray/ray_heightfield.rs b/src/query/ray/ray_heightfield.rs index cfd80eee..da313acb 100644 --- a/src/query/ray/ray_heightfield.rs +++ b/src/query/ray/ray_heightfield.rs @@ -4,10 +4,10 @@ use crate::query; use crate::query::{Ray, RayCast, RayIntersection}; #[cfg(feature = "dim2")] use crate::shape::FeatureId; -use crate::shape::{GenericHeightField, HeightFieldStorage}; +use crate::shape::HeightField; #[cfg(feature = "dim2")] -impl RayCast for GenericHeightField { +impl RayCast for HeightField { #[inline] fn cast_local_ray_and_get_normal( &self, @@ -119,7 +119,7 @@ impl RayCast for GenericHeightField { } #[cfg(feature = "dim3")] -impl RayCast for GenericHeightField { +impl RayCast for HeightField { #[inline] fn cast_local_ray_and_get_normal( &self, diff --git a/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs b/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs index 64613fe6..420365a3 100644 --- a/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs +++ b/src/query/time_of_impact/time_of_impact_composite_shape_shape.rs @@ -3,7 +3,6 @@ use crate::math::{Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH} use crate::partitioning::{SimdBestFirstVisitStatus, SimdBestFirstVisitor}; use crate::query::{QueryDispatcher, Ray, SimdRay, TOI}; use crate::shape::{Shape, TypedSimdCompositeShape}; -use crate::utils::DefaultStorage; use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; /// Time Of Impact of a composite shape with any other shape, under translational movement. @@ -18,7 +17,7 @@ pub fn time_of_impact_composite_shape_shape( ) -> Option where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { let mut visitor = TOICompositeShapeShapeBestFirstVisitor::new( dispatcher, @@ -46,7 +45,7 @@ pub fn time_of_impact_shape_composite_shape( ) -> Option where D: QueryDispatcher, - G2: TypedSimdCompositeShape, + G2: TypedSimdCompositeShape, { time_of_impact_composite_shape_shape( dispatcher, @@ -78,7 +77,7 @@ pub struct TOICompositeShapeShapeBestFirstVisitor<'a, D: ?Sized, G1: ?Sized + 'a impl<'a, D: ?Sized, G1: ?Sized> TOICompositeShapeShapeBestFirstVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { /// Creates a new visitor used to find the time-of-impact between a composite shape and a shape. pub fn new( @@ -112,7 +111,7 @@ impl<'a, D: ?Sized, G1: ?Sized> SimdBestFirstVisitor for TOICompositeShapeShapeBestFirstVisitor<'a, D, G1> where D: QueryDispatcher, - G1: TypedSimdCompositeShape, + G1: TypedSimdCompositeShape, { type Result = (G1::PartId, TOI); @@ -143,7 +142,7 @@ where if (bitmask & (1 << ii)) != 0 && data[ii].is_some() { let part_id = *data[ii].unwrap(); let mut toi = None; - self.g1.map_untyped_part_at(part_id, |part_pos1, g1| { + self.g1.map_untyped_part_at(part_id, |part_pos1, g1, _| { if let Some(part_pos1) = part_pos1 { toi = self .dispatcher diff --git a/src/query/time_of_impact/time_of_impact_heightfield_shape.rs b/src/query/time_of_impact/time_of_impact_heightfield_shape.rs index 92611b4b..c48eda35 100644 --- a/src/query/time_of_impact/time_of_impact_heightfield_shape.rs +++ b/src/query/time_of_impact/time_of_impact_heightfield_shape.rs @@ -1,22 +1,21 @@ use crate::math::{Isometry, Real, Vector}; use crate::query::{QueryDispatcher, Ray, Unsupported, TOI}; -use crate::shape::{GenericHeightField, HeightFieldStorage, Shape}; +use crate::shape::{HeightField, Shape}; #[cfg(feature = "dim3")] use crate::{bounding_volume::Aabb, query::RayCast}; /// Time Of Impact between a moving shape and a heightfield. #[cfg(feature = "dim2")] -pub fn time_of_impact_heightfield_shape( +pub fn time_of_impact_heightfield_shape( dispatcher: &D, pos12: &Isometry, vel12: &Vector, - heightfield1: &GenericHeightField, + heightfield1: &HeightField, g2: &dyn Shape, max_toi: Real, stop_at_penetration: bool, ) -> Result, Unsupported> where - Storage: HeightFieldStorage, D: QueryDispatcher, { let aabb2_1 = g2.compute_aabb(pos12); @@ -104,17 +103,16 @@ where /// Time Of Impact between a moving shape and a heightfield. #[cfg(feature = "dim3")] -pub fn time_of_impact_heightfield_shape( +pub fn time_of_impact_heightfield_shape( dispatcher: &D, pos12: &Isometry, vel12: &Vector, - heightfield1: &GenericHeightField, + heightfield1: &HeightField, g2: &dyn Shape, max_toi: Real, stop_at_penetration: bool, ) -> Result, Unsupported> where - Storage: HeightFieldStorage, D: QueryDispatcher, { let aabb1 = heightfield1.local_aabb(); @@ -279,17 +277,16 @@ where } /// Time Of Impact between a moving shape and a heightfield. -pub fn time_of_impact_shape_heightfield( +pub fn time_of_impact_shape_heightfield( dispatcher: &D, pos12: &Isometry, vel12: &Vector, g1: &dyn Shape, - heightfield2: &GenericHeightField, + heightfield2: &HeightField, max_toi: Real, stop_at_penetration: bool, ) -> Result, Unsupported> where - Storage: HeightFieldStorage, D: QueryDispatcher, { Ok(time_of_impact_heightfield_shape( diff --git a/src/query/visitors/composite_closest_point_visitor.rs b/src/query/visitors/composite_closest_point_visitor.rs index 39329800..21eebdfd 100644 --- a/src/query/visitors/composite_closest_point_visitor.rs +++ b/src/query/visitors/composite_closest_point_visitor.rs @@ -50,7 +50,7 @@ impl<'a, S: SimdCompositeShape + PointQuery> SimdBestFirstVisitor for ii in 0..SIMD_WIDTH { if (bitmask & (1 << ii)) != 0 && data[ii].is_some() { self.shape - .map_part_at(*data[ii].unwrap(), &mut |part_pos, obj| { + .map_part_at(*data[ii].unwrap(), &mut |part_pos, obj, _| { let proj = if let Some(part_pos) = part_pos { obj.project_point(part_pos, self.point, self.solid) } else { diff --git a/src/query/visitors/composite_point_containment_test.rs b/src/query/visitors/composite_point_containment_test.rs index b2c5f346..5c5a8cec 100644 --- a/src/query/visitors/composite_point_containment_test.rs +++ b/src/query/visitors/composite_point_containment_test.rs @@ -46,7 +46,7 @@ impl<'a, S: TypedSimdCompositeShape> SimdVisitor for (ii, data) in data.into_iter().enumerate() { if (bitmask & (1 << ii)) != 0 { let Some(data) = data else { continue }; - self.shape.map_typed_part_at(*data, |part_pos, obj| { + self.shape.map_typed_part_at(*data, |part_pos, obj, _| { if obj.contains_local_point(&part_pos.inverse_transform_point(self.point)) { self.found = true; } diff --git a/src/query/visitors/mod.rs b/src/query/visitors/mod.rs index 80b40e25..6b5a3e34 100644 --- a/src/query/visitors/mod.rs +++ b/src/query/visitors/mod.rs @@ -1,29 +1,17 @@ //! Visitors for performing geometric queries exploiting spatial partitioning data structures. -#[cfg(feature = "std")] pub use self::aabb_sets_interferences_collector::AabbSetsInterferencesCollector; -#[cfg(feature = "std")] pub use self::bounding_volume_intersections_simultaneous_visitor::BoundingVolumeIntersectionsSimultaneousVisitor; -#[cfg(feature = "std")] pub use self::bounding_volume_intersections_visitor::BoundingVolumeIntersectionsVisitor; -#[cfg(feature = "std")] pub use self::composite_closest_point_visitor::CompositeClosestPointVisitor; pub use self::composite_point_containment_test::CompositePointContainmentTest; -#[cfg(feature = "std")] pub use self::point_intersections_visitor::PointIntersectionsVisitor; -#[cfg(feature = "std")] pub use self::ray_intersections_visitor::RayIntersectionsVisitor; -#[cfg(feature = "std")] mod aabb_sets_interferences_collector; -#[cfg(feature = "std")] mod bounding_volume_intersections_simultaneous_visitor; -#[cfg(feature = "std")] mod bounding_volume_intersections_visitor; -#[cfg(feature = "std")] mod composite_closest_point_visitor; mod composite_point_containment_test; -#[cfg(feature = "std")] mod point_intersections_visitor; -#[cfg(feature = "std")] mod ray_intersections_visitor; diff --git a/src/shape/ball.rs b/src/shape/ball.rs index 7d2a9220..677246ab 100644 --- a/src/shape/ball.rs +++ b/src/shape/ball.rs @@ -13,7 +13,6 @@ use crate::shape::SupportMap; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] pub struct Ball { diff --git a/src/shape/capsule.rs b/src/shape/capsule.rs index 54b595e4..357e2571 100644 --- a/src/shape/capsule.rs +++ b/src/shape/capsule.rs @@ -16,7 +16,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[repr(C)] /// A capsule shape defined as a round segment. pub struct Capsule { diff --git a/src/shape/composite_shape.rs b/src/shape/composite_shape.rs index be0bda0e..95571fc2 100644 --- a/src/shape/composite_shape.rs +++ b/src/shape/composite_shape.rs @@ -1,7 +1,7 @@ use crate::math::{Isometry, Real}; -use crate::partitioning::{GenericQbvh, IndexedData, Qbvh, QbvhStorage}; +use crate::partitioning::{IndexedData, Qbvh}; +use crate::query::details::NormalConstraints; use crate::shape::Shape; -use crate::utils::DefaultStorage; /// Trait implemented by shapes composed of multiple simpler shapes. /// @@ -10,45 +10,54 @@ use crate::utils::DefaultStorage; #[cfg(feature = "std")] pub trait SimdCompositeShape { /// Applies a function to one sub-shape of this composite shape. - fn map_part_at(&self, shape_id: u32, f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape)); + fn map_part_at( + &self, + shape_id: u32, + f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + ); /// Gets the acceleration structure of the composite shape. fn qbvh(&self) -> &Qbvh; } +#[cfg(feature = "std")] pub trait TypedSimdCompositeShape { type PartShape: ?Sized + Shape; + type PartNormalConstraints: ?Sized + NormalConstraints; type PartId: IndexedData; - type QbvhStorage: QbvhStorage; fn map_typed_part_at( &self, shape_id: Self::PartId, - f: impl FnMut(Option<&Isometry>, &Self::PartShape), + f: impl FnMut(Option<&Isometry>, &Self::PartShape, Option<&Self::PartNormalConstraints>), ); // TODO: we need this method because the compiler won't want // to cast `&Self::PartShape` to `&dyn Shape` because it complains - // that `PairtShape` is not `Sized`. + // that `PartShape` is not `Sized`. fn map_untyped_part_at( &self, shape_id: Self::PartId, - f: impl FnMut(Option<&Isometry>, &dyn Shape), + f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), ); - fn typed_qbvh(&self) -> &GenericQbvh; + fn typed_qbvh(&self) -> &Qbvh; } #[cfg(feature = "std")] impl<'a> TypedSimdCompositeShape for dyn SimdCompositeShape + 'a { type PartShape = dyn Shape; + type PartNormalConstraints = dyn NormalConstraints; type PartId = u32; - type QbvhStorage = DefaultStorage; fn map_typed_part_at( &self, shape_id: u32, - mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), + mut f: impl FnMut( + Option<&Isometry>, + &Self::PartShape, + Option<&Self::PartNormalConstraints>, + ), ) { self.map_part_at(shape_id, &mut f) } @@ -56,12 +65,12 @@ impl<'a> TypedSimdCompositeShape for dyn SimdCompositeShape + 'a { fn map_untyped_part_at( &self, shape_id: u32, - mut f: impl FnMut(Option<&Isometry>, &dyn Shape), + mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), ) { self.map_part_at(shape_id, &mut f) } - fn typed_qbvh(&self) -> &GenericQbvh { + fn typed_qbvh(&self) -> &Qbvh { self.qbvh() } } diff --git a/src/shape/compound.rs b/src/shape/compound.rs index 9e6c1ca4..7be11a95 100644 --- a/src/shape/compound.rs +++ b/src/shape/compound.rs @@ -5,12 +5,12 @@ use crate::bounding_volume::{Aabb, BoundingSphere, BoundingVolume}; use crate::math::{Isometry, Real}; use crate::partitioning::Qbvh; +use crate::query::details::NormalConstraints; #[cfg(feature = "dim2")] use crate::shape::{ConvexPolygon, TriMesh, Triangle}; use crate::shape::{Shape, SharedShape, SimdCompositeShape, TypedSimdCompositeShape}; #[cfg(feature = "dim2")] use crate::transformation::hertel_mehlhorn; -use crate::utils::DefaultStorage; /// A compound shape with an aabb bounding volume. /// @@ -123,9 +123,13 @@ impl Compound { impl SimdCompositeShape for Compound { #[inline] - fn map_part_at(&self, shape_id: u32, f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape)) { + fn map_part_at( + &self, + shape_id: u32, + f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + ) { if let Some(shape) = self.shapes.get(shape_id as usize) { - f(Some(&shape.0), &*shape.1) + f(Some(&shape.0), &*shape.1, None) } } @@ -137,17 +141,21 @@ impl SimdCompositeShape for Compound { impl TypedSimdCompositeShape for Compound { type PartShape = dyn Shape; + type PartNormalConstraints = (); type PartId = u32; - type QbvhStorage = DefaultStorage; #[inline(always)] fn map_typed_part_at( &self, i: u32, - mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), + mut f: impl FnMut( + Option<&Isometry>, + &Self::PartShape, + Option<&Self::PartNormalConstraints>, + ), ) { if let Some((part_pos, part)) = self.shapes.get(i as usize) { - f(Some(part_pos), &**part) + f(Some(part_pos), &**part, None) } } @@ -155,10 +163,10 @@ impl TypedSimdCompositeShape for Compound { fn map_untyped_part_at( &self, i: u32, - mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), + mut f: impl FnMut(Option<&Isometry>, &Self::PartShape, Option<&dyn NormalConstraints>), ) { if let Some((part_pos, part)) = self.shapes.get(i as usize) { - f(Some(part_pos), &**part) + f(Some(part_pos), &**part, None) } } diff --git a/src/shape/cone.rs b/src/shape/cone.rs index 8cead502..beeabb6a 100644 --- a/src/shape/cone.rs +++ b/src/shape/cone.rs @@ -22,7 +22,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] pub struct Cone { diff --git a/src/shape/cuboid.rs b/src/shape/cuboid.rs index 835020b6..ef922e3a 100644 --- a/src/shape/cuboid.rs +++ b/src/shape/cuboid.rs @@ -21,7 +21,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] pub struct Cuboid { diff --git a/src/shape/cylinder.rs b/src/shape/cylinder.rs index c9905fd0..2c1e514e 100644 --- a/src/shape/cylinder.rs +++ b/src/shape/cylinder.rs @@ -22,7 +22,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] pub struct Cylinder { diff --git a/src/shape/half_space.rs b/src/shape/half_space.rs index 0dda578e..cdcbf50a 100644 --- a/src/shape/half_space.rs +++ b/src/shape/half_space.rs @@ -13,7 +13,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[repr(C)] pub struct HalfSpace { /// The halfspace planar boundary's outward normal. diff --git a/src/shape/heightfield2.rs b/src/shape/heightfield2.rs index b3943b5e..aa860ea7 100644 --- a/src/shape/heightfield2.rs +++ b/src/shape/heightfield2.rs @@ -4,114 +4,35 @@ use na::ComplexField; use na::DVector; use std::ops::Range; -use crate::utils::DefaultStorage; - -#[cfg(feature = "cuda")] -use crate::utils::{CudaArrayPointer1, CudaStorage, CudaStoragePtr}; - -#[cfg(all(feature = "std", feature = "cuda"))] -use {crate::utils::CudaArray1, cust::error::CudaResult}; - use na::Point2; use crate::bounding_volume::Aabb; use crate::math::{Real, Vector}; use crate::shape::Segment; -use crate::utils::Array1; -/// Indicates if a cell of an heightfield is removed or not. Set this to `false` for +/// Indicates if a cell of a heightfield is removed or not. Set this to `false` for /// a removed cell. pub type HeightFieldCellStatus = bool; -/// Trait describing all the types needed for storing an heightfield’s data. -pub trait HeightFieldStorage { - /// Type of the array containing the heightfield’s heights. - type Heights: Array1; - /// Type of the array containing the heightfield’s cells status. - type Status: Array1; -} - -#[cfg(feature = "std")] -impl HeightFieldStorage for DefaultStorage { - type Heights = DVector; - type Status = DVector; -} - -#[cfg(all(feature = "std", feature = "cuda"))] -impl HeightFieldStorage for CudaStorage { - type Heights = CudaArray1; - type Status = CudaArray1; -} - -#[cfg(feature = "cuda")] -impl HeightFieldStorage for CudaStoragePtr { - type Heights = CudaArrayPointer1; - type Status = CudaArrayPointer1; -} - #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -#[cfg_attr( - feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) -)] -#[derive(Debug)] -#[repr(C)] // Needed for Cuda. +// TODO: Archive isn’t implemented for VecStorage yet. +// #[cfg_attr( +// feature = "rkyv", +// derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), +// archive(check_bytes) +// )] +#[derive(Debug, Clone)] +#[repr(C)] /// A 2D heightfield with a generic storage buffer for its heights. -pub struct GenericHeightField { - heights: Storage::Heights, - status: Storage::Status, +pub struct HeightField { + heights: DVector, + status: DVector, scale: Vector, aabb: Aabb, } -impl Clone for GenericHeightField -where - Storage: HeightFieldStorage, - Storage::Heights: Clone, - Storage::Status: Clone, -{ - fn clone(&self) -> Self { - Self { - heights: self.heights.clone(), - status: self.status.clone(), - scale: self.scale, - aabb: self.aabb, - } - } -} - -impl Copy for GenericHeightField -where - Storage: HeightFieldStorage, - Storage::Heights: Copy, - Storage::Status: Copy, -{ -} - -#[cfg(feature = "cuda")] -unsafe impl cust_core::DeviceCopy for GenericHeightField -where - Storage: HeightFieldStorage, - Storage::Heights: cust_core::DeviceCopy + Copy, - Storage::Status: cust_core::DeviceCopy + Copy, -{ -} - -/// A 2D heightfield. -#[cfg(feature = "std")] -pub type HeightField = GenericHeightField; - -/// A 2D heightfield stored in the CUDA memory, initializable from the host. -#[cfg(all(feature = "std", feature = "cuda"))] -pub type CudaHeightField = GenericHeightField; - -/// A 2D heightfield stored in the CUDA memory, accessible from within a Cuda kernel. -#[cfg(feature = "cuda")] -pub type CudaHeightFieldPtr = GenericHeightField; - #[cfg(feature = "std")] impl HeightField { /// Creates a new 2D heightfield with the given heights and scale factor. @@ -137,40 +58,16 @@ impl HeightField { aabb, } } - - /// Converts this RAM-based heightfield to an heightfield based on CUDA memory. - #[cfg(feature = "cuda")] - pub fn to_cuda(&self) -> CudaResult { - Ok(CudaHeightField { - heights: CudaArray1::from_vector(&self.heights)?, - status: CudaArray1::from_vector(&self.status)?, - scale: self.scale, - aabb: self.aabb, - }) - } } -#[cfg(all(feature = "std", feature = "cuda"))] -impl CudaHeightField { - /// Returns the heightfield usable from within a CUDA kernel. - pub fn as_device_ptr(&self) -> CudaHeightFieldPtr { - CudaHeightFieldPtr { - heights: self.heights.as_device_ptr(), - status: self.status.as_device_ptr(), - aabb: self.aabb, - scale: self.scale, - } - } -} - -impl GenericHeightField { +impl HeightField { /// The number of cells of this heightfield. pub fn num_cells(&self) -> usize { self.heights.len() - 1 } /// The height at each cell endpoint. - pub fn heights(&self) -> &Storage::Heights { + pub fn heights(&self) -> &DVector { &self.heights } diff --git a/src/shape/heightfield3.rs b/src/shape/heightfield3.rs index 67064f9a..356b90c4 100644 --- a/src/shape/heightfield3.rs +++ b/src/shape/heightfield3.rs @@ -1,18 +1,11 @@ -use crate::utils::DefaultStorage; #[cfg(feature = "std")] use na::DMatrix; use std::ops::Range; -#[cfg(feature = "cuda")] -use crate::utils::{CudaArrayPointer2, CudaStorage, CudaStoragePtr}; -#[cfg(all(feature = "std", feature = "cuda"))] -use {crate::utils::CudaArray2, cust::error::CudaResult}; - use crate::bounding_volume::Aabb; use crate::math::{Real, Vector}; -use crate::shape::{FeatureId, Triangle}; -use crate::utils::Array2; -use na::Point3; +use crate::shape::{FeatureId, Triangle, TrianglePseudoNormals}; +use na::{Point3, Unit}; #[cfg(not(feature = "std"))] use na::ComplexField; @@ -24,7 +17,6 @@ bitflags! { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(as = "Self"), )] - #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Default)] /// The status of the cell of an heightfield. pub struct HeightFieldCellStatus: u8 { @@ -39,100 +31,60 @@ bitflags! { } } -/// Trait describing all the types needed for storing an heightfield’s data. -pub trait HeightFieldStorage { - /// Type of the array containing the heightfield’s heights. - type Heights: Array2; - /// Type of the array containing the heightfield’s cells status. - type Status: Array2; -} - -#[cfg(feature = "std")] -impl HeightFieldStorage for DefaultStorage { - type Heights = DMatrix; - type Status = DMatrix; -} - -#[cfg(all(feature = "std", feature = "cuda"))] -impl HeightFieldStorage for CudaStorage { - type Heights = CudaArray2; - type Status = CudaArray2; -} - -#[cfg(feature = "cuda")] -impl HeightFieldStorage for CudaStoragePtr { - type Heights = CudaArrayPointer2; - type Status = CudaArrayPointer2; +bitflags::bitflags! { + #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] + #[cfg_attr( + feature = "rkyv", + derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), + archive(as = "Self"), + )] + #[repr(C)] + #[derive(Default)] + /// The status of the cell of an heightfield. + pub struct HeightFieldFlags: u8 { + /// If set, a special treatment will be applied to contact manifold calculation to eliminate + /// or fix contacts normals that could lead to incorrect bumps in physics simulation (especially + /// on flat surfaces). + /// + /// This is achieved by taking into account adjacent triangle normals when computing contact + /// points for a given triangle. + const FIX_INTERNAL_EDGES = 1 << 0; + } } #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] -#[cfg_attr( - feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) -)] -#[derive(Debug)] -#[repr(C)] // Needed for Cuda. -/// A 3D heightfield with a generic storage buffer for its height grid. -pub struct GenericHeightField { - heights: Storage::Heights, - status: Storage::Status, +// TODO: Archive isn’t implemented for VecStorage yet. +// #[cfg_attr( +// feature = "rkyv", +// derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), +// archive(check_bytes) +// )] +#[derive(Debug, Clone)] +#[repr(C)] +/// A 3D heightfield. +pub struct HeightField { + heights: DMatrix, + status: DMatrix, scale: Vector, aabb: Aabb, num_triangles: usize, + flags: HeightFieldFlags, } -impl Clone for GenericHeightField -where - Storage: HeightFieldStorage, - Storage::Heights: Clone, - Storage::Status: Clone, -{ - fn clone(&self) -> Self { - Self { - heights: self.heights.clone(), - status: self.status.clone(), - scale: self.scale, - aabb: self.aabb, - num_triangles: self.num_triangles, - } - } -} - -impl Copy for GenericHeightField -where - Storage: HeightFieldStorage, - Storage::Heights: Copy, - Storage::Status: Copy, -{ -} - -#[cfg(feature = "cuda")] -unsafe impl cust_core::DeviceCopy for GenericHeightField -where - Storage: HeightFieldStorage, - Storage::Heights: cust_core::DeviceCopy + Copy, - Storage::Status: cust_core::DeviceCopy + Copy, -{ -} - -/// A 3D heightfield. -#[cfg(feature = "std")] -pub type HeightField = GenericHeightField; - -/// A 3D heightfield stored in the CUDA memory, initializable from the host. -#[cfg(all(feature = "std", feature = "cuda"))] -pub type CudaHeightField = GenericHeightField; - -/// A 3D heightfield stored in the CUDA memory, accessible from within a Cuda kernel. -#[cfg(feature = "cuda")] -pub type CudaHeightFieldPtr = GenericHeightField; - #[cfg(feature = "std")] impl HeightField { - /// Initializes a new heightfield with the given heights and a scaling factor. + /// Initializes a new heightfield with the given heights, scaling factor, and flags. pub fn new(heights: DMatrix, scale: Vector) -> Self { + Self::with_flags(heights, scale, HeightFieldFlags::empty()) + } + + /// Initializes a new heightfield with the given heights and a scaling factor. + pub fn with_flags( + heights: DMatrix, + scale: Vector, + flags: HeightFieldFlags, + ) -> Self { assert!( heights.nrows() > 1 && heights.ncols() > 1, "A heightfield heights must have at least 2 rows and columns." @@ -157,37 +109,12 @@ impl HeightField { aabb, num_triangles, status, + flags, } } - - /// Converts this RAM-based heightfield to an heightfield based on CUDA memory. - #[cfg(feature = "cuda")] - pub fn to_cuda(&self) -> CudaResult { - Ok(CudaHeightField { - heights: CudaArray2::from_matrix(&self.heights)?, - status: CudaArray2::from_matrix(&self.status)?, - aabb: self.aabb, - num_triangles: self.num_triangles, - scale: self.scale, - }) - } } -#[cfg(all(feature = "std", feature = "cuda"))] -impl CudaHeightField { - /// Returns the heightfield usable from within a CUDA kernel. - pub fn as_device_ptr(&self) -> CudaHeightFieldPtr { - CudaHeightFieldPtr { - heights: self.heights.as_device_ptr(), - status: self.status.as_device_ptr(), - aabb: self.aabb, - num_triangles: self.num_triangles, - scale: self.scale, - } - } -} - -impl GenericHeightField { +impl HeightField { /// The number of rows of this heightfield. pub fn nrows(&self) -> usize { self.heights.nrows() - 1 @@ -320,10 +247,7 @@ impl GenericHeightField { } /// An iterator through all the triangles around the given point, after vertical projection on the heightfield. - pub fn triangles_around_point( - &self, - point: &Point3, - ) -> HeightFieldRadialTriangles { + pub fn triangles_around_point(&self, point: &Point3) -> HeightFieldRadialTriangles { let center = self.closest_cell_at_point(point); HeightFieldRadialTriangles { heightfield: self, @@ -334,7 +258,7 @@ impl GenericHeightField { } } - /// Gets the the vertices of the triangle identified by `id`. + /// Gets the vertices of the triangle identified by `id`. pub fn triangle_at_id(&self, id: u32) -> Option { let (i, j, left) = self.split_triangle_id(id); if left { @@ -360,7 +284,7 @@ impl GenericHeightField { return (None, None); } - let status = self.status.get(i, j); + let status = self.status[(i, j)]; if status.contains( HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED @@ -414,7 +338,7 @@ impl GenericHeightField { return (None, None); } - let status = self.status.get(i, j); + let status = self.status[(i, j)]; if status.contains( HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED @@ -432,10 +356,10 @@ impl GenericHeightField { let x0 = -0.5 + cell_width * (j as Real); let x1 = -0.5 + cell_width * ((j + 1) as Real); - let y00 = self.heights.get(i, j); - let y10 = self.heights.get(i + 1, j); - let y01 = self.heights.get(i, j + 1); - let y11 = self.heights.get(i + 1, j + 1); + let y00 = self.heights[(i, j)]; + let y10 = self.heights[(i + 1, j)]; + let y01 = self.heights[(i, j + 1)]; + let y11 = self.heights[(i + 1, j + 1)]; let mut p00 = Point3::new(x0, y00, z0); let mut p10 = Point3::new(x0, y10, z1); @@ -479,6 +403,105 @@ impl GenericHeightField { } } + /// Computes the pseudo-normals of the triangle identified by the given id. + /// + /// Returns `None` if the heightfield’s [`HeightfieldFlags::FIX_INTERNAL_EDGES`] isn’t set, or + /// if the triangle doesn’t exist due to it being removed by its status flag + /// (`HeightFieldCellStatus::LEFT_TRIANGLE_REMOVED` or + /// `HeightFieldCellStatus::RIGHT_TRIANGLE_REMOVED`). + pub fn triangle_normal_constraints(&self, id: u32) -> Option { + if self.flags.contains(HeightFieldFlags::FIX_INTERNAL_EDGES) { + let (i, j, left) = self.split_triangle_id(id); + let status = self.status[(i, j)]; + + let (tri_left, tri_right) = self.triangles_at(i, j); + let tri_normal = if left { + *tri_left?.normal()? + } else { + *tri_right?.normal()? + }; + + // TODO: we only compute bivectors where v is a specific direction + // (+/-X, +/-Z, or a combination of both). So this bivector + // calculation could be simplified/optimized quite a bit. + // Computes the pseudo-normal of an edge where the adjacent triangle is missing. + let bivector = |v: Vector| tri_normal.cross(&v).cross(&tri_normal).normalize(); + // Pseudo-normal computed from an adjacent triangle’s normal and the current triangle’s normal. + let adj_pseudo_normal = |adj: Option| { + adj.map(|adj| adj.normal().map(|n| *n).unwrap_or(tri_normal)) + }; + + let diag_dir = if status.contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) { + Vector::new(-1.0, 0.0, 1.0) + } else { + Vector::new(1.0, 0.0, 1.0) + }; + + let (left_pseudo_normal, right_pseudo_normal) = if left { + let adj_left = adj_pseudo_normal(self.triangles_at(i.overflowing_sub(1).0, j).1) + .unwrap_or_else(|| bivector(-Vector::z())); + let adj_right = adj_pseudo_normal(tri_right).unwrap_or_else(|| bivector(diag_dir)); + (adj_left, adj_right) + } else { + let adj_left = adj_pseudo_normal(tri_left).unwrap_or_else(|| bivector(-diag_dir)); + let adj_right = adj_pseudo_normal(self.triangles_at(i + 1, j).0) + .unwrap_or_else(|| bivector(Vector::z())); + (adj_left, adj_right) + }; + + // The third neighbor depends on the combination of zigzag scheme + // and right/left position. + let top_or_bottom_pseudo_normal = if left + != status.contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) + { + // The neighbor is below. + let ((bot_left, bot_right), bot_status) = if j > 0 { + (self.triangles_at(i, j - 1), self.status[(i, j - 1)]) + } else { + ((None, None), HeightFieldCellStatus::empty()) + }; + + let bot_tri = if bot_status.contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) { + bot_left + } else { + bot_right + }; + + adj_pseudo_normal(bot_tri).unwrap_or_else(|| bivector(-Vector::x())) + } else { + // The neighbor is above. + let ((top_left, top_right), top_status) = if j < self.heights.ncols() - 2 { + (self.triangles_at(i, j + 1), self.status[(i, j + 1)]) + } else { + ((None, None), HeightFieldCellStatus::empty()) + }; + + let top_tri = if top_status.contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) { + top_right + } else { + top_left + }; + + adj_pseudo_normal(top_tri).unwrap_or_else(|| bivector(Vector::x())) + }; + + // NOTE: the normalization can only succeed due to the heightfield’s definition. + let pseudo_normal1 = Unit::new_normalize((tri_normal + left_pseudo_normal) / 2.0); + let pseudo_normal2 = Unit::new_normalize((tri_normal + right_pseudo_normal) / 2.0); + let pseudo_normal3 = + Unit::new_normalize((tri_normal + top_or_bottom_pseudo_normal) / 2.0); + + Some(TrianglePseudoNormals { + face: Unit::new_unchecked(tri_normal), // No need to re-normalize. + // TODO: the normals are given in no particular order. So they are **not** + // guaranteed to be provided in the same order as the triangle’s edge. + edges: [pseudo_normal1, pseudo_normal2, pseudo_normal3], + }) + } else { + None + } + } + /// The number of cells of this heightfield along each dimension. pub fn num_cells_ij(&self) -> (usize, usize) { (self.nrows(), self.ncols()) @@ -486,26 +509,26 @@ impl GenericHeightField { /// The status of the `(i, j)`-th cell. pub fn cell_status(&self, i: usize, j: usize) -> HeightFieldCellStatus { - self.status.get(i, j) + self.status[(i, j)] } /// Set the status of the `(i, j)`-th cell. pub fn set_cell_status(&mut self, i: usize, j: usize, status: HeightFieldCellStatus) { - self.status.set(i, j, status) + self.status[(i, j)] = status; } /// The statuses of all the cells of this heightfield. - pub fn cells_statuses(&self) -> &Storage::Status { + pub fn cells_statuses(&self) -> &DMatrix { &self.status } /// The mutable statuses of all the cells of this heightfield. - pub fn cells_statuses_mut(&mut self) -> &mut Storage::Status { + pub fn cells_statuses_mut(&mut self) -> &mut DMatrix { &mut self.status } /// The heights of this heightfield. - pub fn heights(&self) -> &Storage::Heights { + pub fn heights(&self) -> &DMatrix { &self.heights } @@ -567,11 +590,7 @@ impl GenericHeightField { let nrows = self.heights.nrows(); let ij = i + j * nrows; - if self - .status - .get(i, j) - .contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) - { + if self.status[(i, j)].contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) { if left { FeatureId::Vertex([ij, ij + 1, ij + 1 + nrows][ivertex as usize] as u32) } else { @@ -594,11 +613,7 @@ impl GenericHeightField { let ileft = vshift + i + j * (nrows - 1); let iright = ileft + nrows - 1; - if self - .status - .get(i, j) - .contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) - { + if self.status[(i, j)].contains(HeightFieldCellStatus::ZIGZAG_SUBDIVISION) { if left { // Triangle: // @@ -684,7 +699,7 @@ impl GenericHeightField { // multiple times. for j in min_x..max_x { for i in min_z..max_z { - let status = self.status.get(i, j); + let status = self.status[(i, j)]; if status.contains(HeightFieldCellStatus::CELL_REMOVED) { continue; @@ -696,10 +711,10 @@ impl GenericHeightField { let x0 = -0.5 + cell_width * (j as Real); let x1 = x0 + cell_width; - let y00 = self.heights.get(i, j); - let y10 = self.heights.get(i + 1, j); - let y01 = self.heights.get(i, j + 1); - let y11 = self.heights.get(i + 1, j + 1); + let y00 = self.heights[(i, j)]; + let y10 = self.heights[(i + 1, j)]; + let y01 = self.heights[(i, j + 1)]; + let y11 = self.heights[(i + 1, j + 1)]; if (y00 > ref_maxs.y && y10 > ref_maxs.y && y01 > ref_maxs.y && y11 > ref_maxs.y) || (y00 < ref_mins.y @@ -747,13 +762,13 @@ impl GenericHeightField { } } -struct HeightFieldTriangles<'a, Storage: HeightFieldStorage> { - heightfield: &'a GenericHeightField, +struct HeightFieldTriangles<'a> { + heightfield: &'a HeightField, curr: (usize, usize), tris: (Option, Option), } -impl<'a, Storage: HeightFieldStorage> Iterator for HeightFieldTriangles<'a, Storage> { +impl<'a> Iterator for HeightFieldTriangles<'a> { type Item = Triangle; fn next(&mut self) -> Option { @@ -782,15 +797,15 @@ impl<'a, Storage: HeightFieldStorage> Iterator for HeightFieldTriangles<'a, Stor } /// An iterator through all the triangles around the given point, after vertical projection on the heightfield. -pub struct HeightFieldRadialTriangles<'a, Storage: HeightFieldStorage> { - heightfield: &'a GenericHeightField, +pub struct HeightFieldRadialTriangles<'a> { + heightfield: &'a HeightField, center: (usize, usize), curr_radius: usize, curr_element: usize, tris: (Option, Option), } -impl<'a, Storage: HeightFieldStorage> HeightFieldRadialTriangles<'a, Storage> { +impl<'a> HeightFieldRadialTriangles<'a> { /// Returns the next triangle in this iterator. /// /// Returns `None` no triangle closest than `max_dist` remain diff --git a/src/shape/mod.rs b/src/shape/mod.rs index d1b69c7f..3e115dd7 100644 --- a/src/shape/mod.rs +++ b/src/shape/mod.rs @@ -2,8 +2,6 @@ pub use self::ball::Ball; pub use self::capsule::Capsule; -#[doc(inline)] -pub use self::composite_shape::TypedSimdCompositeShape; pub use self::cuboid::Cuboid; pub use self::feature_id::{FeatureId, PackedFeatureId}; pub use self::half_space::HalfSpace; @@ -20,7 +18,9 @@ pub use self::triangle::{Triangle, TriangleOrientation, TrianglePointLocation}; #[cfg(feature = "std")] pub use self::{ - composite_shape::SimdCompositeShape, compound::Compound, polyline::Polyline, + composite_shape::{SimdCompositeShape, TypedSimdCompositeShape}, + compound::Compound, + polyline::Polyline, shared_shape::SharedShape, }; @@ -28,6 +28,7 @@ pub use self::{ #[cfg(feature = "std")] pub use self::convex_polygon::ConvexPolygon; #[cfg(feature = "dim2")] +#[cfg(feature = "std")] pub use self::heightfield2::*; #[cfg(feature = "dim2")] pub use self::polygonal_feature2d::PolygonalFeature; @@ -40,13 +41,15 @@ pub use self::convex_polyhedron::ConvexPolyhedron; #[cfg(feature = "dim3")] pub use self::cylinder::Cylinder; #[cfg(feature = "dim3")] +#[cfg(feature = "std")] pub use self::heightfield3::*; #[cfg(feature = "dim3")] pub use self::polygonal_feature3d::PolygonalFeature; #[cfg(feature = "dim3")] pub use self::tetrahedron::{Tetrahedron, TetrahedronPointLocation}; +pub use self::triangle_pseudo_normals::TrianglePseudoNormals; +#[cfg(feature = "std")] pub use self::trimesh::*; -pub use self::trimesh_storage::TriMeshStorage; /// A cylinder dilated by a sphere (so it has round corners). #[cfg(feature = "dim3")] @@ -69,6 +72,7 @@ pub type RoundConvexPolygon = RoundShape; mod ball; mod capsule; +#[cfg(feature = "std")] #[doc(hidden)] pub mod composite_shape; #[cfg(feature = "std")] @@ -89,6 +93,7 @@ mod triangle; #[cfg(feature = "std")] mod convex_polygon; #[cfg(feature = "dim2")] +#[cfg(feature = "std")] mod heightfield2; #[cfg(feature = "dim3")] @@ -99,12 +104,14 @@ mod convex_polyhedron; #[cfg(feature = "dim3")] mod cylinder; #[cfg(feature = "dim3")] +#[cfg(feature = "std")] mod heightfield3; #[cfg(feature = "dim3")] mod polygonal_feature3d; mod polygonal_feature_map; #[cfg(feature = "dim3")] mod tetrahedron; +#[cfg(feature = "std")] pub(crate) mod trimesh; // TODO: move this elsewhere? mod feature_id; @@ -112,4 +119,4 @@ mod feature_id; mod polygonal_feature2d; #[cfg(feature = "std")] mod shared_shape; -mod trimesh_storage; +mod triangle_pseudo_normals; diff --git a/src/shape/polygonal_feature2d.rs b/src/shape/polygonal_feature2d.rs index 89edac49..65e33b45 100644 --- a/src/shape/polygonal_feature2d.rs +++ b/src/shape/polygonal_feature2d.rs @@ -55,20 +55,19 @@ impl PolygonalFeature { sep_axis2: &Vector, feature1: &Self, feature2: &Self, - prediction: Real, manifold: &mut ContactManifold, flipped: bool, ) { match (feature1.num_vertices == 2, feature2.num_vertices == 2) { - (true, true) => Self::face_face_contacts( - pos12, feature1, sep_axis1, feature2, prediction, manifold, flipped, - ), - (true, false) => Self::face_vertex_contacts( - pos12, feature1, sep_axis1, feature2, prediction, manifold, flipped, - ), - (false, true) => Self::face_vertex_contacts( - pos21, feature2, sep_axis2, feature1, prediction, manifold, !flipped, - ), + (true, true) => { + Self::face_face_contacts(pos12, feature1, sep_axis1, feature2, manifold, flipped) + } + (true, false) => { + Self::face_vertex_contacts(pos12, feature1, sep_axis1, feature2, manifold, flipped) + } + (false, true) => { + Self::face_vertex_contacts(pos21, feature2, sep_axis2, feature1, manifold, !flipped) + } (false, false) => unimplemented!(), } } @@ -82,7 +81,6 @@ impl PolygonalFeature { face1: &Self, sep_axis1: &Vector, vertex2: &Self, - _prediction: Real, manifold: &mut ContactManifold, flipped: bool, ) { @@ -113,7 +111,6 @@ impl PolygonalFeature { face1: &Self, normal1: &Vector, face2: &Self, - _prediction: Real, manifold: &mut ContactManifold, flipped: bool, ) { @@ -126,32 +123,26 @@ impl PolygonalFeature { let fids2 = [face2.vids[0], face2.fid, face2.vids[1]]; let dist = (clip_a.1 - clip_a.0).dot(normal1); - if true { - // dist < prediction { - let contact = TrackedContact::flipped( - clip_a.0, - pos12.inverse_transform_point(&clip_a.1), - fids1[clip_a.2], - fids2[clip_a.3], - dist, - flipped, - ); - manifold.points.push(contact); - } + let contact = TrackedContact::flipped( + clip_a.0, + pos12.inverse_transform_point(&clip_a.1), + fids1[clip_a.2], + fids2[clip_a.3], + dist, + flipped, + ); + manifold.points.push(contact); let dist = (clip_b.1 - clip_b.0).dot(normal1); - if true { - // dist < prediction { - let contact = TrackedContact::flipped( - clip_b.0, - pos12.inverse_transform_point(&clip_b.1), - fids1[clip_b.2], - fids2[clip_b.3], - dist, - flipped, - ); - manifold.points.push(contact); - } + let contact = TrackedContact::flipped( + clip_b.0, + pos12.inverse_transform_point(&clip_b.1), + fids1[clip_b.2], + fids2[clip_b.3], + dist, + flipped, + ); + manifold.points.push(contact); } } } diff --git a/src/shape/polygonal_feature3d.rs b/src/shape/polygonal_feature3d.rs index 108c91df..ccc195f1 100644 --- a/src/shape/polygonal_feature3d.rs +++ b/src/shape/polygonal_feature3d.rs @@ -80,17 +80,14 @@ impl PolygonalFeature { _sep_axis2: &Vector, feature1: &Self, feature2: &Self, - prediction: Real, manifold: &mut ContactManifold, flipped: bool, ) { match (feature1.num_vertices, feature2.num_vertices) { - (2, 2) => Self::contacts_edge_edge( - pos12, feature1, sep_axis1, feature2, prediction, manifold, flipped, - ), - _ => Self::contacts_face_face( - pos12, feature1, sep_axis1, feature2, prediction, manifold, flipped, - ), + (2, 2) => { + Self::contacts_edge_edge(pos12, feature1, sep_axis1, feature2, manifold, flipped) + } + _ => Self::contacts_face_face(pos12, feature1, sep_axis1, feature2, manifold, flipped), } } @@ -100,7 +97,6 @@ impl PolygonalFeature { face1: &PolygonalFeature, sep_axis1: &Vector, face2: &PolygonalFeature, - prediction: Real, manifold: &mut ContactManifold, flipped: bool, ) { @@ -160,16 +156,14 @@ impl PolygonalFeature { let local_p2_1 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1]; let dist = (local_p2_1 - local_p1).dot(sep_axis1); - if dist <= prediction { - manifold.points.push(TrackedContact::flipped( - local_p1, - pos12.inverse_transform_point(&local_p2_1), - face1.eids[0], - face2.eids[0], - dist, - flipped, - )); - } + manifold.points.push(TrackedContact::flipped( + local_p1, + pos12.inverse_transform_point(&local_p2_1), + face1.eids[0], + face2.eids[0], + dist, + flipped, + )); return; } @@ -216,7 +210,6 @@ impl PolygonalFeature { face1: &PolygonalFeature, sep_axis1: &Vector, face2: &PolygonalFeature, - prediction: Real, manifold: &mut ContactManifold, flipped: bool, ) { @@ -302,16 +295,14 @@ impl PolygonalFeature { let local_p1 = face1.vertices[i]; let local_p2_1 = face1.vertices[i] + dist * sep_axis1; - if dist <= prediction { - manifold.points.push(TrackedContact::flipped( - local_p1, - pos12.inverse_transform_point(&local_p2_1), - face1.vids[i], - face2.fid, - dist, - flipped, - )); - } + manifold.points.push(TrackedContact::flipped( + local_p1, + pos12.inverse_transform_point(&local_p2_1), + face1.vids[i], + face2.fid, + dist, + flipped, + )); } } } @@ -346,17 +337,14 @@ impl PolygonalFeature { let local_p2_1 = vertices2_1[i]; let local_p1 = vertices2_1[i] - dist * sep_axis1; - if true { - // dist <= prediction { - manifold.points.push(TrackedContact::flipped( - local_p1, - pos12.inverse_transform_point(&local_p2_1), - face1.fid, - face2.vids[i], - dist, - flipped, - )); - } + manifold.points.push(TrackedContact::flipped( + local_p1, + pos12.inverse_transform_point(&local_p2_1), + face1.fid, + face2.vids[i], + dist, + flipped, + )); } } } @@ -386,16 +374,14 @@ impl PolygonalFeature { let local_p2_1 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1; let dist = (local_p2_1 - local_p1).dot(sep_axis1); - if dist <= prediction { - manifold.points.push(TrackedContact::flipped( - local_p1, - pos12.inverse_transform_point(&local_p2_1), - face1.eids[i], - face2.eids[j], - dist, - flipped, - )); - } + manifold.points.push(TrackedContact::flipped( + local_p1, + pos12.inverse_transform_point(&local_p2_1), + face1.eids[i], + face2.eids[j], + dist, + flipped, + )); } } } diff --git a/src/shape/polyline.rs b/src/shape/polyline.rs index ee29839f..f78b812f 100644 --- a/src/shape/polyline.rs +++ b/src/shape/polyline.rs @@ -5,7 +5,7 @@ use crate::query::{PointProjection, PointQueryWithLocation}; use crate::shape::composite_shape::SimdCompositeShape; use crate::shape::{FeatureId, Segment, SegmentPointLocation, Shape, TypedSimdCompositeShape}; -use crate::utils::DefaultStorage; +use crate::query::details::NormalConstraints; #[cfg(not(feature = "std"))] use na::ComplexField; // for .abs() @@ -278,9 +278,13 @@ impl Polyline { } impl SimdCompositeShape for Polyline { - fn map_part_at(&self, i: u32, f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape)) { + fn map_part_at( + &self, + i: u32, + f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + ) { let tri = self.segment(i); - f(None, &tri) + f(None, &tri, None) } fn qbvh(&self) -> &Qbvh { @@ -290,23 +294,31 @@ impl SimdCompositeShape for Polyline { impl TypedSimdCompositeShape for Polyline { type PartShape = Segment; + type PartNormalConstraints = (); type PartId = u32; - type QbvhStorage = DefaultStorage; #[inline(always)] fn map_typed_part_at( &self, i: u32, - mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), + mut f: impl FnMut( + Option<&Isometry>, + &Self::PartShape, + Option<&Self::PartNormalConstraints>, + ), ) { let seg = self.segment(i); - f(None, &seg) + f(None, &seg, None) } #[inline(always)] - fn map_untyped_part_at(&self, i: u32, mut f: impl FnMut(Option<&Isometry>, &dyn Shape)) { + fn map_untyped_part_at( + &self, + i: u32, + mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + ) { let seg = self.segment(i); - f(None, &seg) + f(None, &seg, None) } fn typed_qbvh(&self) -> &Qbvh { diff --git a/src/shape/round_shape.rs b/src/shape/round_shape.rs index 96c05130..318f61cd 100644 --- a/src/shape/round_shape.rs +++ b/src/shape/round_shape.rs @@ -8,7 +8,6 @@ use na::Unit; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Copy, Clone, Debug)] #[repr(C)] /// A shape with rounded borders. diff --git a/src/shape/segment.rs b/src/shape/segment.rs index f883f33d..41b598cc 100644 --- a/src/shape/segment.rs +++ b/src/shape/segment.rs @@ -17,7 +17,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone)] #[repr(C)] pub struct Segment { diff --git a/src/shape/shared_shape.rs b/src/shape/shared_shape.rs index e445be74..c14ca424 100644 --- a/src/shape/shared_shape.rs +++ b/src/shape/shared_shape.rs @@ -3,6 +3,8 @@ use crate::math::{Isometry, Point, Real, Vector, DIM}; use crate::shape::ConvexPolygon; #[cfg(feature = "serde-serialize")] use crate::shape::DeserializableTypedShape; +#[cfg(feature = "dim3")] +use crate::shape::HeightFieldFlags; use crate::shape::{ Ball, Capsule, Compound, Cuboid, HalfSpace, HeightField, Polyline, RoundShape, Segment, Shape, TriMesh, TriMeshFlags, Triangle, TypedShape, @@ -355,19 +357,30 @@ impl SharedShape { }) } - /// Initializes an heightfield shape defined by its set of height and a scale + /// Initializes a heightfield shape defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim2")] pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { SharedShape(Arc::new(HeightField::new(heights, scale))) } - /// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale + /// Initializes a heightfield shape on the x-z plane defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim3")] pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { SharedShape(Arc::new(HeightField::new(heights, scale))) } + + /// Initializes a heightfield shape on the x-z plane defined by its set of height, a scale + /// factor along each coordinate axis, and [`HeightFieldFlags`]. + #[cfg(feature = "dim3")] + pub fn heightfield_with_flags( + heights: na::DMatrix, + scale: Vector, + flags: HeightFieldFlags, + ) -> Self { + SharedShape(Arc::new(HeightField::with_flags(heights, scale, flags))) + } } #[cfg(feature = "serde-serialize")] diff --git a/src/shape/tetrahedron.rs b/src/shape/tetrahedron.rs index 94149781..d232635e 100644 --- a/src/shape/tetrahedron.rs +++ b/src/shape/tetrahedron.rs @@ -20,7 +20,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(Copy, Clone, Debug)] #[repr(C)] pub struct Tetrahedron { diff --git a/src/shape/triangle.rs b/src/shape/triangle.rs index d3931460..7dc2e761 100644 --- a/src/shape/triangle.rs +++ b/src/shape/triangle.rs @@ -25,7 +25,6 @@ use rkyv::{bytecheck, CheckBytes}; derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] #[derive(PartialEq, Debug, Copy, Clone, Default)] #[repr(C)] pub struct Triangle { diff --git a/src/shape/triangle_pseudo_normals.rs b/src/shape/triangle_pseudo_normals.rs new file mode 100644 index 00000000..927d7333 --- /dev/null +++ b/src/shape/triangle_pseudo_normals.rs @@ -0,0 +1,228 @@ +use crate::math::{Real, UnitVector, Vector}; +use na::Vector3; + +#[cfg(feature = "std")] +use crate::query::details::NormalConstraints; + +// NOTE: ideally, the normal cone should take into account the point where the normal cone is +// considered. But as long as we assume that the triangles are one-way we can get away with +// just relying on the normal directions. +// Taking the point into account would be technically doable (and desirable if we wanted +// to define, e.g., a one-way mesh) but requires: +// 1. To make sure the edge pseudo-normals are given in the correct edge order. +// 2. To have access to the contact feature. +// We can have access to both during the narrow-phase, but leave that as a future +// potential improvements. +// NOTE: this isn’t equal to the "true" normal cones since concave edges will have pseudo-normals +// still pointing outward (instead of inward or being empty). +/// The pseudo-normals of a triangle providing approximations of its feature’s normal cones. +#[derive(Clone, Debug)] +pub struct TrianglePseudoNormals { + /// The triangle’s face normal. + pub face: UnitVector, + // TODO: if we switch this to providing pseudo-normals in a specific order + // (e.g. in the same order as the triangle’s edges), then we should + // think of fixing that order in the heightfield + // triangle_pseudo_normals code. + /// The edges pseudo-normals, in no particular order. + pub edges: [UnitVector; 3], +} + +#[cfg(feature = "std")] +impl NormalConstraints for TrianglePseudoNormals { + /// Projects the given direction to it is contained in the polygonal + /// cone defined `self`. + fn project_local_normal_mut(&self, dir: &mut Vector) -> bool { + let dot_face = dir.dot(&self.face); + + // Find the closest pseudo-normal. + let dots = Vector3::new( + dir.dot(&self.edges[0]), + dir.dot(&self.edges[1]), + dir.dot(&self.edges[2]), + ); + let closest_dot = dots.imax(); + let closest_edge = &self.edges[closest_dot]; + + // Apply the projection. Note that this isn’t 100% accurate since this approximates the + // vertex normal cone using the closest edge’s normal cone instead of the + // true polygonal cone on S² (but taking into account this polygonal cone exactly + // would be quite computationally expensive). + + if *closest_edge == self.face { + // The normal cone is degenerate, there is only one possible direction. + *dir = *self.face; + return dot_face >= 0.0; + } + + // TODO: take into account the two closest edges instead for better continuity + // of vertex normals? + let dot_edge_face = self.face.dot(closest_edge); + let dot_dir_face = self.face.dot(dir); + let dot_corrected_dir_face = 2.0 * dot_edge_face * dot_edge_face - 1.0; // cos(2 * angle(closest_edge, face)) + + if dot_dir_face >= dot_corrected_dir_face { + // The direction is in the pseudo-normal cone. No correction to apply. + return true; + } + + // We need to correct. + let edge_on_normal = *self.face * dot_edge_face; + let edge_orthogonal_to_normal = **closest_edge - edge_on_normal; + + let dir_on_normal = *self.face * dot_dir_face; + let dir_orthogonal_to_normal = *dir - dir_on_normal; + let Some(unit_dir_orthogonal_to_normal) = dir_orthogonal_to_normal.try_normalize(1.0e-6) + else { + return dot_face >= 0.0; + }; + + // NOTE: the normalization might be redundant as the result vector is guaranteed to be + // unit sized. Though some rounding errors could throw it off. + let Some(adjusted_pseudo_normal) = (edge_on_normal + + unit_dir_orthogonal_to_normal * edge_orthogonal_to_normal.norm()) + .try_normalize(1.0e-6) else { + return dot_face >= 0.0; + }; + + // The reflection of the face normal wrt. the adjusted pseudo-normal gives us the + // second end of the pseudo-normal cone the direction is projected on. + *dir = adjusted_pseudo_normal * (2.0 * self.face.dot(&adjusted_pseudo_normal)) - *self.face; + dot_face >= 0.0 + } +} + +#[cfg(test)] +#[cfg(feature = "dim3")] +mod test { + use crate::math::{Real, Vector}; + use crate::shape::TrianglePseudoNormals; + use na::Unit; + + use super::NormalConstraints; + + fn bisector(v1: Vector, v2: Vector) -> Vector { + (v1 + v2).normalize() + } + + fn bisector_y(v: Vector) -> Vector { + bisector(v, Vector::y()) + } + + #[test] + fn trivial_pseudo_normals_projection() { + let pn = TrianglePseudoNormals { + face: Vector::y_axis(), + edges: [Vector::y_axis(); 3], + }; + + assert_eq!( + pn.project_local_normal(Vector::new(1.0, 1.0, 1.0)), + Some(Vector::y()) + ); + assert!(pn.project_local_normal(-Vector::y()).is_none()); + } + + #[test] + fn edge_pseudo_normals_projection_strictly_positive() { + let bisector = |v1: Vector, v2: Vector| (v1 + v2).normalize(); + let bisector_y = |v: Vector| bisector(v, Vector::y()); + + // The normal cones for this test will be fully contained in the +Y half-space. + let cones_ref_dir = [ + -Vector::z(), + -Vector::x(), + Vector::new(1.0, 0.0, 1.0).normalize(), + ]; + let cones_ends = cones_ref_dir.map(bisector_y); + let cones_axes = cones_ends.map(bisector_y); + + let pn = TrianglePseudoNormals { + face: Vector::y_axis(), + edges: cones_axes.map(Unit::new_normalize), + }; + + for i in 0..3 { + assert_relative_eq!( + pn.project_local_normal(cones_ends[i]).unwrap(), + cones_ends[i], + epsilon = 1.0e-5 + ); + assert_eq!(pn.project_local_normal(cones_axes[i]), Some(cones_axes[i])); + + // Guaranteed to be inside the normal cone of edge i. + let subdivs = 100; + + for k in 1..100 { + let v = Vector::y() + .lerp(&cones_ends[i], k as Real / (subdivs as Real)) + .normalize(); + assert_eq!(pn.project_local_normal(v).unwrap(), v); + } + + // Guaranteed to be outside the normal cone of edge i. + for k in 1..subdivs { + let v = cones_ref_dir[i] + .lerp(&cones_ends[i], k as Real / (subdivs as Real)) + .normalize(); + assert_relative_eq!( + pn.project_local_normal(v).unwrap(), + cones_ends[i], + epsilon = 1.0e-5 + ); + } + + // Guaranteed to be outside the normal cone, and in the -Y half-space. + for k in 1..subdivs { + let v = cones_ref_dir[i] + .lerp(&(-Vector::y()), k as Real / (subdivs as Real)) + .normalize(); + assert!(pn.project_local_normal(v).is_none(),); + } + } + } + + #[test] + fn edge_pseudo_normals_projection_negative() { + // The normal cones for this test will be fully contained in the +Y half-space. + let cones_ref_dir = [ + -Vector::z(), + -Vector::x(), + Vector::new(1.0, 0.0, 1.0).normalize(), + ]; + let cones_ends = cones_ref_dir.map(|v| bisector(v, -Vector::y())); + let cones_axes = [ + bisector(bisector_y(cones_ref_dir[0]), cones_ref_dir[0]), + bisector(bisector_y(cones_ref_dir[1]), cones_ref_dir[1]), + bisector(bisector_y(cones_ref_dir[2]), cones_ref_dir[2]), + ]; + + let pn = TrianglePseudoNormals { + face: Vector::y_axis(), + edges: cones_axes.map(Unit::new_normalize), + }; + + for i in 0..3 { + assert_eq!(pn.project_local_normal(cones_axes[i]), Some(cones_axes[i])); + + // Guaranteed to be inside the normal cone of edge i. + let subdivs = 100; + + for k in 1..subdivs { + let v = Vector::y() + .lerp(&cones_ends[i], k as Real / (subdivs as Real)) + .normalize(); + assert_eq!(pn.project_local_normal(v).unwrap(), v); + } + + // Guaranteed to be outside the normal cone of edge i. + // Since it is additionally guaranteed to be in the -Y half-space, we should get None. + for k in 1..subdivs { + let v = (-Vector::y()) + .lerp(&cones_ends[i], k as Real / (subdivs as Real)) + .normalize(); + assert!(pn.project_local_normal(v).is_none()); + } + } + } +} diff --git a/src/shape/trimesh.rs b/src/shape/trimesh.rs index 514bae5d..c0d55240 100644 --- a/src/shape/trimesh.rs +++ b/src/shape/trimesh.rs @@ -1,30 +1,23 @@ use crate::bounding_volume::Aabb; use crate::math::{Isometry, Point, Real, Vector}; -use crate::partitioning::QbvhStorage; -use crate::partitioning::{GenericQbvh, Qbvh}; -use crate::shape::trimesh_storage::TriMeshStorage; -use crate::shape::{FeatureId, Shape, Triangle, TypedSimdCompositeShape}; +use crate::partitioning::Qbvh; +use crate::shape::{FeatureId, Shape, Triangle, TrianglePseudoNormals, TypedSimdCompositeShape}; use std::fmt; -use crate::utils::{Array1, DefaultStorage, HashablePartialEq}; +use crate::utils::HashablePartialEq; #[cfg(feature = "dim3")] -use {crate::shape::Cuboid, crate::shape::HeightFieldStorage, crate::utils::SortedPair}; +use {crate::shape::Cuboid, crate::utils::SortedPair, na::Unit}; -#[cfg(feature = "std")] use { crate::shape::composite_shape::SimdCompositeShape, crate::utils::hashmap::{Entry, HashMap}, std::collections::HashSet, }; -#[cfg(all(feature = "dim2", feature = "std"))] +#[cfg(feature = "dim2")] use crate::transformation::ear_clipping::triangulate_ear_clipping; -#[cfg(feature = "cuda")] -use crate::utils::{CudaStorage, CudaStoragePtr}; -#[cfg(all(feature = "std", feature = "cuda"))] -use {crate::utils::CudaArray1, cust::error::CudaResult}; - +use crate::query::details::NormalConstraints; #[cfg(feature = "rkyv")] use rkyv::{bytecheck, CheckBytes}; @@ -34,7 +27,7 @@ pub enum TopologyError { /// Found a triangle with two or three identical vertices. BadTriangle(u32), /// At least two adjacent triangles have opposite orientations. - BadAdjascentTrianglesOrientation { + BadAdjacentTrianglesOrientation { /// The first triangle, with an orientation opposite to the second triangle. triangle1: u32, /// The second triangle, with an orientation opposite to the first triangle. @@ -44,14 +37,13 @@ pub enum TopologyError { }, } -#[cfg(feature = "std")] impl std::fmt::Display for TopologyError { fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result { match self { Self::BadTriangle(fid) => { f.pad(&format!("the triangle {fid} has at least two identical vertices.")) } - Self::BadAdjascentTrianglesOrientation { + Self::BadAdjacentTrianglesOrientation { triangle1, triangle2, edge, @@ -60,7 +52,6 @@ impl std::fmt::Display for TopologyError { } } -#[cfg(feature = "std")] impl std::error::Error for TopologyError {} /// The set of pseudo-normals of a triangle mesh. @@ -69,93 +60,49 @@ impl std::error::Error for TopologyError {} /// point on the triangle, as described in the paper: /// "Signed distance computation using the angle weighted pseudonormal", Baerentzen, et al. /// DOI: 10.1109/TVCG.2005.49 -#[derive(Default)] +#[derive(Default, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[repr(C)] // Needed for Cuda. +#[repr(C)] #[cfg(feature = "dim3")] -pub struct TriMeshPseudoNormals { +pub struct TriMeshPseudoNormals { /// The pseudo-normals of the vertices. - pub vertices_pseudo_normal: Storage::ArrayVector, + pub vertices_pseudo_normal: Vec>, /// The pseudo-normals of the edges. - pub edges_pseudo_normal: Storage::ArrayVectorTriple, -} - -#[cfg(all(feature = "dim3", feature = "std", feature = "cuda"))] -impl TriMeshPseudoNormals { - /// Returns the heightfield usable from within a CUDA kernel. - fn as_device_ptr(&self) -> TriMeshPseudoNormals { - TriMeshPseudoNormals { - vertices_pseudo_normal: self.vertices_pseudo_normal.as_device_ptr(), - edges_pseudo_normal: self.edges_pseudo_normal.as_device_ptr(), - } - } -} - -#[cfg(all(feature = "dim3", feature = "std", feature = "cuda"))] -impl TriMeshPseudoNormals { - fn to_cuda(&self) -> CudaResult> { - Ok(TriMeshPseudoNormals { - vertices_pseudo_normal: CudaArray1::new(&self.vertices_pseudo_normal)?, - edges_pseudo_normal: CudaArray1::new(&self.edges_pseudo_normal)?, - }) - } + pub edges_pseudo_normal: Vec<[Vector; 3]>, } /// The connected-components of a triangle mesh. -#[derive(Debug)] +#[derive(Debug, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[repr(C)] // Needed for Cuda. -pub struct TriMeshConnectedComponents { +#[repr(C)] +pub struct TriMeshConnectedComponents { /// The `face_colors[i]` gives the connected-component index /// of the i-th face. - pub face_colors: Storage::ArrayU32, + pub face_colors: Vec, /// The set of faces grouped by connected components. - pub grouped_faces: Storage::ArrayU32, + pub grouped_faces: Vec, /// The range of connected components. `self.grouped_faces[self.ranges[i]..self.ranges[i + 1]]` /// contains the indices of all the faces part of the i-th connected component. - pub ranges: Storage::ArrayUsize, + pub ranges: Vec, } -impl TriMeshConnectedComponents { +impl TriMeshConnectedComponents { /// The total number of connected components. pub fn num_connected_components(&self) -> usize { self.ranges.len() - 1 } } -#[cfg(all(feature = "std", feature = "cuda"))] -impl TriMeshConnectedComponents { - /// Returns the heightfield usable from within a CUDA kernel. - fn as_device_ptr(&self) -> TriMeshConnectedComponents { - TriMeshConnectedComponents { - face_colors: self.face_colors.as_device_ptr(), - grouped_faces: self.grouped_faces.as_device_ptr(), - ranges: self.ranges.as_device_ptr(), - } - } -} - -#[cfg(all(feature = "std", feature = "cuda"))] -impl TriMeshConnectedComponents { - fn to_cuda(&self) -> CudaResult> { - Ok(TriMeshConnectedComponents { - face_colors: CudaArray1::new(&self.face_colors)?, - grouped_faces: CudaArray1::new(&self.grouped_faces)?, - ranges: CudaArray1::new(&self.ranges)?, - }) - } -} - /// A vertex of a triangle-mesh’s half-edge topology. #[derive(Clone, Copy, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -164,8 +111,7 @@ impl TriMeshConnectedComponents { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] -#[repr(C)] // Needed for Cuda. +#[repr(C)] pub struct TopoVertex { /// One of the half-edge with this vertex as endpoint. pub half_edge: u32, @@ -179,8 +125,7 @@ pub struct TopoVertex { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] -#[repr(C)] // Needed for Cuda. +#[repr(C)] pub struct TopoFace { /// The half-edge adjacent to this face, with a starting point equal /// to the first point of this face. @@ -195,8 +140,7 @@ pub struct TopoFace { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize, CheckBytes), archive(as = "Self") )] -#[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] -#[repr(C)] // Needed for Cuda. +#[repr(C)] pub struct TopoHalfEdge { /// The next half-edge. pub next: u32, @@ -211,35 +155,24 @@ pub struct TopoHalfEdge { } /// The half-edge topology information of a triangle mesh. -#[derive(Default)] +#[derive(Default, Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr( feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[repr(C)] // Needed for Cuda. -pub struct TriMeshTopology { +#[repr(C)] +pub struct TriMeshTopology { /// The vertices of this half-edge representation. - pub vertices: Storage::ArrayTopoVertex, + pub vertices: Vec, /// The faces of this half-edge representation. - pub faces: Storage::ArrayTopoFace, + pub faces: Vec, /// The half-edges of this half-edge representation. - pub half_edges: Storage::ArrayTopoHalfEdge, + pub half_edges: Vec, } -#[cfg(all(feature = "std", feature = "cuda"))] -impl TriMeshTopology { - fn as_device_ptr(&self) -> TriMeshTopology { - TriMeshTopology { - vertices: self.vertices.as_device_ptr(), - faces: self.faces.as_device_ptr(), - half_edges: self.half_edges.as_device_ptr(), - } - } -} - -impl TriMeshTopology { +impl TriMeshTopology { #[cfg(feature = "dim3")] pub(crate) fn face_half_edges_ids(&self, fid: u32) -> [u32; 3] { let first_half_edge = self.faces[fid as usize].half_edge; @@ -254,17 +187,6 @@ impl TriMeshTopology { } } -#[cfg(all(feature = "std", feature = "cuda"))] -impl TriMeshTopology { - fn to_cuda(&self) -> CudaResult> { - Ok(TriMeshTopology { - vertices: CudaArray1::new(&self.vertices)?, - faces: CudaArray1::new(&self.faces)?, - half_edges: CudaArray1::new(&self.half_edges)?, - }) - } -} - bitflags::bitflags! { #[cfg_attr(feature = "serde", derive(Serialize, Deserialize))] #[cfg_attr( @@ -272,162 +194,84 @@ bitflags::bitflags! { derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(as = "Self"), )] - #[cfg_attr(feature = "cuda", derive(cust_core::DeviceCopy))] - #[repr(C)] // Needed for Cuda. + #[repr(C)] #[derive(Default)] /// The status of the cell of an heightfield. - pub struct TriMeshFlags: u8 { + pub struct TriMeshFlags: u16 { /// If set, the half-edge topology of the trimesh will be computed if possible. - const HALF_EDGE_TOPOLOGY = 0b0000_0001; + const HALF_EDGE_TOPOLOGY = 1; /// If set, the half-edge topology and connected components of the trimesh will be computed if possible. /// /// Because of the way it is currently implemented, connected components can only be computed on /// a mesh where the half-edge topology computation succeeds. It will no longer be the case in the /// future once we decouple the computations. - const CONNECTED_COMPONENTS = 0b0000_0010; + const CONNECTED_COMPONENTS = 1 << 1; /// If set, any triangle that results in a failing half-hedge topology computation will be deleted. - const DELETE_BAD_TOPOLOGY_TRIANGLES = 0b0000_0100; + const DELETE_BAD_TOPOLOGY_TRIANGLES = 1 << 2; /// If set, the trimesh will be assumed to be oriented (with outward normals). /// /// The pseudo-normals of its vertices and edges will be computed. - const ORIENTED = 0b0000_1000; + const ORIENTED = 1 << 3; /// If set, the duplicate vertices of the trimesh will be merged. /// /// Two vertices with the exact same coordinates will share the same entry on the /// vertex buffer and the index buffer is adjusted accordingly. - const MERGE_DUPLICATE_VERTICES = 0b0001_0000; + const MERGE_DUPLICATE_VERTICES = 1 << 4; /// If set, the triangles sharing two vertices with identical index values will be removed. /// /// Because of the way it is currently implemented, this methods implies that duplicate /// vertices will be merged. It will no longer be the case in the future once we decouple /// the computations. - const DELETE_DEGENERATE_TRIANGLES = 0b0010_0000; - /// If set, two triangles sharing three vertices with identical index values (in any order) will be removed. + const DELETE_DEGENERATE_TRIANGLES = 1 << 5; + /// If set, two triangles sharing three vertices with identical index values (in any order) + /// will be removed. /// /// Because of the way it is currently implemented, this methods implies that duplicate /// vertices will be merged. It will no longer be the case in the future once we decouple /// the computations. - const DELETE_DUPLICATE_TRIANGLES = 0b0100_0000; + const DELETE_DUPLICATE_TRIANGLES = 1 << 6; + /// If set, a special treatment will be applied to contact manifold calculation to eliminate + /// or fix contacts normals that could lead to incorrect bumps in physics simulation + /// (especially on flat surfaces). + /// + /// This is achieved by taking into account adjacent triangle normals when computing contact + /// points for a given triangle. + const FIX_INTERNAL_EDGES = 1 << 7 | Self::ORIENTED.bits() | Self::MERGE_DUPLICATE_VERTICES.bits(); } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[cfg_attr( - feature = "serde-serialize", - serde(bound( - serialize = ">::Nodes: serde::Serialize, \ - >::ArrayU32: serde::Serialize, \ - >::ArrayProxies: serde::Serialize,\ - Storage::ArrayTopoVertex: serde::Serialize,\ - Storage::ArrayTopoFace: serde::Serialize,\ - Storage::ArrayTopoHalfEdge: serde::Serialize,\ - Storage::ArrayU32: serde::Serialize,\ - Storage::ArrayUsize: serde::Serialize,\ - Storage::ArrayVector: serde::Serialize,\ - Storage::ArrayPoint: serde::Serialize,\ - Storage::ArrayIdx: serde::Serialize,\ - Storage::ArrayVectorTriple: serde::Serialize", - deserialize = ">::Nodes: serde::Deserialize<'de>, \ - >::ArrayU32: serde::Deserialize<'de>, \ - >::ArrayProxies: serde::Deserialize<'de>,\ - Storage::ArrayTopoVertex: serde::Deserialize<'de>,\ - Storage::ArrayTopoFace: serde::Deserialize<'de>,\ - Storage::ArrayTopoHalfEdge: serde::Deserialize<'de>,\ - Storage::ArrayU32: serde::Deserialize<'de>,\ - Storage::ArrayUsize: serde::Deserialize<'de>,\ - Storage::ArrayVector: serde::Deserialize<'de>,\ - Storage::ArrayPoint: serde::Deserialize<'de>,\ - Storage::ArrayIdx: serde::Deserialize<'de>,\ - Storage::ArrayVectorTriple: serde::Deserialize<'de>", - )) -)] #[cfg_attr( feature = "rkyv", derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), archive(check_bytes) )] -#[repr(C)] // Needed for Cuda. +#[repr(C)] +#[derive(Clone)] /// A triangle mesh. -pub struct GenericTriMesh { - qbvh: GenericQbvh, - vertices: Storage::ArrayPoint, - indices: Storage::ArrayIdx, +pub struct TriMesh { + qbvh: Qbvh, + vertices: Vec>, + indices: Vec<[u32; 3]>, #[cfg(feature = "dim3")] - pub(crate) pseudo_normals: Option>, - topology: Option>, - connected_components: Option>, + pub(crate) pseudo_normals: Option, + topology: Option, + connected_components: Option, flags: TriMeshFlags, } -impl fmt::Debug for GenericTriMesh { +impl fmt::Debug for TriMesh { fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { write!(f, "GenericTriMesh") } } -/// A triangle-mesh. -pub type TriMesh = GenericTriMesh; -#[cfg(feature = "cuda")] -/// A triangle-mesh stored on CUDA memory. -pub type CudaTriMesh = GenericTriMesh; -#[cfg(feature = "cuda")] -/// A triangle-mesh accessible from CUDA kernels. -pub type CudaTriMeshPtr = GenericTriMesh; - -#[cfg(all(feature = "std", feature = "cuda"))] -impl CudaTriMesh { - /// Returns the heightfield usable from within a CUDA kernel. - pub fn as_device_ptr(&self) -> CudaTriMeshPtr { - GenericTriMesh { - qbvh: self.qbvh.as_device_ptr(), - vertices: self.vertices.as_device_ptr(), - indices: self.indices.as_device_ptr(), - #[cfg(feature = "dim3")] - pseudo_normals: self.pseudo_normals.as_ref().map(|pn| pn.as_device_ptr()), - topology: self.topology.as_ref().map(|topo| topo.as_device_ptr()), - connected_components: self - .connected_components - .as_ref() - .map(|cc| cc.as_device_ptr()), - flags: self.flags, - } - } -} - -#[cfg(feature = "std")] impl TriMesh { /// Creates a new triangle mesh from a vertex buffer and an index buffer. pub fn new(vertices: Vec>, indices: Vec<[u32; 3]>) -> Self { Self::with_flags(vertices, indices, TriMeshFlags::empty()) } - /// Converts this RAM-based heightfield to an heightfield based on CUDA memory. - #[cfg(feature = "cuda")] - pub fn to_cuda(&self) -> CudaResult { - Ok(CudaTriMesh { - qbvh: self.qbvh.to_cuda()?, - vertices: CudaArray1::new(&self.vertices)?, - indices: CudaArray1::new(&self.indices)?, - #[cfg(feature = "dim3")] - pseudo_normals: self - .pseudo_normals - .as_ref() - .map(|pn| pn.to_cuda()) - .transpose()?, - topology: self - .topology - .as_ref() - .map(|topo| topo.to_cuda()) - .transpose()?, - connected_components: self - .connected_components - .as_ref() - .map(|cc| cc.to_cuda()) - .transpose()?, - flags: self.flags, - }) - } - /// Creates a new triangle mesh from a vertex buffer and an index buffer, and flags controlling optional properties. pub fn with_flags( vertices: Vec>, @@ -861,7 +705,7 @@ impl TriMesh { self.delete_bad_topology_triangles(); } - let mut topology = TriMeshTopology::::default(); + let mut topology = TriMeshTopology::default(); let mut half_edge_map = HashMap::default(); topology.vertices.resize( @@ -897,7 +741,7 @@ impl TriMesh { if let Some(existing) = half_edge_map.insert(edge_key, half_edge_base_id + k) { // If the same edge already exists (with the same vertex order) then // we have two triangles sharing the same but with opposite incompatible orientations. - return Err(TopologyError::BadAdjascentTrianglesOrientation { + return Err(TopologyError::BadAdjacentTrianglesOrientation { edge: edge_key, triangle1: topology.half_edges[existing as usize].face, triangle2: fid as u32, @@ -1019,7 +863,7 @@ impl TriMesh { } } -impl GenericTriMesh { +impl TriMesh { /// The flags of this triangle mesh. pub fn flags(&self) -> TriMeshFlags { self.flags @@ -1036,7 +880,7 @@ impl GenericTriMesh { } /// The acceleration structure used by this triangle-mesh. - pub fn qbvh(&self) -> &GenericQbvh { + pub fn qbvh(&self) -> &Qbvh { &self.qbvh } @@ -1064,91 +908,75 @@ impl GenericTriMesh { ) } + /// Returns the pseudo-normals of one of this mesh’s triangles, if it was computed. + /// + /// This returns `None` if the pseudo-normals of this triangle were not computed. + /// To have its pseudo-normals computed, be sure to set the [`TriMeshFlags`] so that + /// they contain the [`TriMeshFlags::FIX_INTERNAL_EDGES`] flag. + #[cfg(feature = "dim3")] + pub fn triangle_normal_constraints(&self, i: u32) -> Option { + if self.flags.contains(TriMeshFlags::FIX_INTERNAL_EDGES) { + let triangle = self.triangle(i); + let pseudo_normals = self.pseudo_normals.as_ref()?; + let edges_pseudo_normals = pseudo_normals.edges_pseudo_normal[i as usize]; + + // TODO: could the pseudo-normal be pre-normalized instead of having to renormalize + // every time we need them? + Some(TrianglePseudoNormals { + face: triangle.normal()?, + edges: [ + Unit::try_new(edges_pseudo_normals[0], 1.0e-6)?, + Unit::try_new(edges_pseudo_normals[1], 1.0e-6)?, + Unit::try_new(edges_pseudo_normals[2], 1.0e-6)?, + ], + }) + } else { + None + } + } + + #[cfg(feature = "dim2")] + #[doc(hidden)] + pub fn triangle_normal_constraints(&self, _i: u32) -> Option { + None + } + /// The vertex buffer of this mesh. - pub fn vertices(&self) -> &Storage::ArrayPoint { + pub fn vertices(&self) -> &[Point] { &self.vertices } /// The index buffer of this mesh. - pub fn indices(&self) -> &Storage::ArrayIdx { + pub fn indices(&self) -> &[[u32; 3]] { &self.indices } /// Returns the topology information of this trimesh, if it has been computed. - pub fn topology(&self) -> Option<&TriMeshTopology> { + pub fn topology(&self) -> Option<&TriMeshTopology> { self.topology.as_ref() } /// Returns the connected-component information of this trimesh, if it has been computed. - pub fn connected_components(&self) -> Option<&TriMeshConnectedComponents> { + pub fn connected_components(&self) -> Option<&TriMeshConnectedComponents> { self.connected_components.as_ref() } /// The pseudo-normals of this triangle mesh, if they have been computed. #[cfg(feature = "dim3")] - pub fn pseudo_normals(&self) -> Option<&TriMeshPseudoNormals> { + pub fn pseudo_normals(&self) -> Option<&TriMeshPseudoNormals> { self.pseudo_normals.as_ref() } } -/* -#[cfg(feature = "dim3")] -impl RayCast for TriMesh { - fn cast_local_ray_and_get_normal( - &self, - ray: &Ray, - max_toi: Real, - solid: bool, - ) -> Option { - // FIXME: do a best-first search. - let mut intersections = Vec::new(); - self.qbvh.cast_ray(&ray, max_toi, &mut intersections); - let mut best: Option = None; - - for inter in intersections { - let tri = self.triangle(inter); - if let Some(inter) = tri.cast_local_ray_and_get_normal(ray, max_toi, solid) { - if let Some(curr) = &mut best { - if curr.toi > inter.toi { - *curr = inter; - } - } else { - best = Some(inter); - } - } - } - - best - } - - fn intersects_local_ray(&self, ray: &Ray, max_toi: Real) -> bool { - // FIXME: do a best-first search. - let mut intersections = Vec::new(); - self.qbvh.cast_ray(&ray, max_toi, &mut intersections); - - for inter in intersections { - let tri = self.triangle(inter); - if tri.intersects_local_ray(ray, max_toi) { - return true; - } - } - - false - } -} -*/ - #[cfg(feature = "dim3")] -#[cfg(feature = "std")] -impl From> for TriMesh { - fn from(heightfield: crate::shape::GenericHeightField) -> Self { +impl From for TriMesh { + fn from(heightfield: crate::shape::HeightField) -> Self { let (vtx, idx) = heightfield.to_trimesh(); TriMesh::new(vtx, idx) } } #[cfg(feature = "dim3")] -#[cfg(feature = "std")] impl From for TriMesh { fn from(cuboid: Cuboid) -> Self { let (vtx, idx) = cuboid.to_trimesh(); @@ -1156,11 +984,19 @@ impl From for TriMesh { } } -#[cfg(feature = "std")] impl SimdCompositeShape for TriMesh { - fn map_part_at(&self, i: u32, f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape)) { + fn map_part_at( + &self, + i: u32, + f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + ) { let tri = self.triangle(i); - f(None, &tri) + let normals = self.triangle_normal_constraints(i); + f( + None, + &tri, + normals.as_ref().map(|n| n as &dyn NormalConstraints), + ) } fn qbvh(&self) -> &Qbvh { @@ -1168,202 +1004,42 @@ impl SimdCompositeShape for TriMesh { } } -impl TypedSimdCompositeShape for GenericTriMesh { +impl TypedSimdCompositeShape for TriMesh { type PartShape = Triangle; + type PartNormalConstraints = TrianglePseudoNormals; type PartId = u32; - type QbvhStorage = Storage::QbvhStorage; #[inline(always)] fn map_typed_part_at( &self, i: u32, - mut f: impl FnMut(Option<&Isometry>, &Self::PartShape), + mut f: impl FnMut( + Option<&Isometry>, + &Self::PartShape, + Option<&Self::PartNormalConstraints>, + ), ) { let tri = self.triangle(i); - f(None, &tri) + let pseudo_normals = self.triangle_normal_constraints(i); + f(None, &tri, pseudo_normals.as_ref()) } #[inline(always)] - fn map_untyped_part_at(&self, i: u32, mut f: impl FnMut(Option<&Isometry>, &dyn Shape)) { + fn map_untyped_part_at( + &self, + i: u32, + mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + ) { let tri = self.triangle(i); - f(None, &tri) + let pseudo_normals = self.triangle_normal_constraints(i); + f( + None, + &tri, + pseudo_normals.as_ref().map(|n| n as &dyn NormalConstraints), + ) } - fn typed_qbvh(&self) -> &GenericQbvh { + fn typed_qbvh(&self) -> &Qbvh { &self.qbvh } } - -/******************************************* - * - * BOILERPLACE Copy/Clone implementations - * - * - ******************************************/ -#[cfg(feature = "dim3")] -impl Clone for TriMeshPseudoNormals -where - Storage: TriMeshStorage, - Storage::ArrayVector: Clone, - Storage::ArrayVectorTriple: Clone, -{ - fn clone(&self) -> Self { - Self { - vertices_pseudo_normal: self.vertices_pseudo_normal.clone(), - edges_pseudo_normal: self.edges_pseudo_normal.clone(), - } - } -} - -#[cfg(feature = "dim3")] -impl Copy for TriMeshPseudoNormals -where - Storage: TriMeshStorage, - Storage::ArrayVector: Copy, - Storage::ArrayVectorTriple: Copy, -{ -} - -#[cfg(feature = "dim3")] -#[cfg(feature = "cuda")] -unsafe impl cust_core::DeviceCopy for TriMeshPseudoNormals -where - Storage: TriMeshStorage, - Storage::ArrayVector: cust_core::DeviceCopy + Copy, - Storage::ArrayVectorTriple: cust_core::DeviceCopy + Copy, -{ -} - -impl Clone for TriMeshConnectedComponents -where - Storage: TriMeshStorage, - Storage::ArrayU32: Clone, - Storage::ArrayUsize: Clone, -{ - fn clone(&self) -> Self { - Self { - face_colors: self.face_colors.clone(), - grouped_faces: self.grouped_faces.clone(), - ranges: self.ranges.clone(), - } - } -} - -impl Copy for TriMeshConnectedComponents -where - Storage: TriMeshStorage, - Storage::ArrayU32: Copy, - Storage::ArrayUsize: Copy, -{ -} - -#[cfg(feature = "cuda")] -unsafe impl cust_core::DeviceCopy for TriMeshConnectedComponents -where - Storage: TriMeshStorage, - Storage::ArrayU32: cust_core::DeviceCopy + Copy, - Storage::ArrayUsize: cust_core::DeviceCopy + Copy, -{ -} - -impl Clone for TriMeshTopology -where - Storage: TriMeshStorage, - Storage::ArrayTopoVertex: Clone, - Storage::ArrayTopoFace: Clone, - Storage::ArrayTopoHalfEdge: Clone, -{ - fn clone(&self) -> Self { - Self { - vertices: self.vertices.clone(), - faces: self.faces.clone(), - half_edges: self.half_edges.clone(), - } - } -} - -impl Copy for TriMeshTopology -where - Storage: TriMeshStorage, - Storage::ArrayTopoVertex: Copy, - Storage::ArrayTopoFace: Copy, - Storage::ArrayTopoHalfEdge: Copy, -{ -} - -#[cfg(feature = "cuda")] -unsafe impl cust_core::DeviceCopy for TriMeshTopology -where - Storage: TriMeshStorage, - Storage::ArrayTopoVertex: cust_core::DeviceCopy + Copy, - Storage::ArrayTopoFace: cust_core::DeviceCopy + Copy, - Storage::ArrayTopoHalfEdge: cust_core::DeviceCopy + Copy, -{ -} - -impl Clone for GenericTriMesh -where - Storage: TriMeshStorage, - >::Nodes: Clone, - >::ArrayU32: Clone, - >::ArrayProxies: Clone, - Storage::ArrayTopoVertex: Clone, - Storage::ArrayTopoFace: Clone, - Storage::ArrayTopoHalfEdge: Clone, - Storage::ArrayU32: Clone, - Storage::ArrayUsize: Clone, - Storage::ArrayVector: Clone, - Storage::ArrayPoint: Clone, - Storage::ArrayIdx: Clone, - Storage::ArrayVectorTriple: Clone, -{ - fn clone(&self) -> Self { - Self { - qbvh: self.qbvh.clone(), - vertices: self.vertices.clone(), - indices: self.indices.clone(), - #[cfg(feature = "dim3")] - pseudo_normals: self.pseudo_normals.clone(), - topology: self.topology.clone(), - connected_components: self.connected_components.clone(), - flags: self.flags, - } - } -} - -impl Copy for GenericTriMesh -where - Storage: TriMeshStorage, - >::Nodes: Copy, - >::ArrayU32: Copy, - >::ArrayProxies: Copy, - Storage::ArrayTopoVertex: Copy, - Storage::ArrayTopoFace: Copy, - Storage::ArrayTopoHalfEdge: Copy, - Storage::ArrayU32: Copy, - Storage::ArrayUsize: Copy, - Storage::ArrayVector: Copy, - Storage::ArrayPoint: Copy, - Storage::ArrayIdx: Copy, - Storage::ArrayVectorTriple: Copy, -{ -} - -#[cfg(feature = "cuda")] -unsafe impl cust_core::DeviceCopy for GenericTriMesh -where - Storage: TriMeshStorage, - >::Nodes: cust_core::DeviceCopy + Copy, - >::ArrayU32: cust_core::DeviceCopy + Copy, - >::ArrayProxies: cust_core::DeviceCopy + Copy, - Storage::ArrayTopoVertex: cust_core::DeviceCopy + Copy, - Storage::ArrayTopoFace: cust_core::DeviceCopy + Copy, - Storage::ArrayTopoHalfEdge: cust_core::DeviceCopy + Copy, - Storage::ArrayU32: cust_core::DeviceCopy + Copy, - Storage::ArrayUsize: cust_core::DeviceCopy + Copy, - Storage::ArrayVector: cust_core::DeviceCopy + Copy, - Storage::ArrayPoint: cust_core::DeviceCopy + Copy, - Storage::ArrayIdx: cust_core::DeviceCopy + Copy, - Storage::ArrayVectorTriple: cust_core::DeviceCopy + Copy, -{ -} diff --git a/src/shape/trimesh_storage.rs b/src/shape/trimesh_storage.rs deleted file mode 100644 index 914ad142..00000000 --- a/src/shape/trimesh_storage.rs +++ /dev/null @@ -1,75 +0,0 @@ -use crate::math::{Point, Real, Vector}; -use crate::partitioning::QbvhStorage; -use crate::shape::{TopoFace, TopoHalfEdge, TopoVertex}; -use crate::utils::{Array1, DefaultStorage}; - -#[cfg(all(feature = "std", feature = "cuda"))] -use crate::utils::CudaArray1; -#[cfg(feature = "cuda")] -use crate::utils::{CudaArrayPointer1, CudaStorage, CudaStoragePtr}; - -/// Trait describing all the types needed for storing a triangle mesh’s data. -pub trait TriMeshStorage { - /// Storage needed to store a Qbvh. - type QbvhStorage: QbvhStorage; - /// Storage needed to store topology vertices. - type ArrayTopoVertex: Array1; - /// Storage needed to store topology faces. - type ArrayTopoFace: Array1; - /// Storage needed to store topology half-edges. - type ArrayTopoHalfEdge: Array1; - /// Storage needed to store u32 - type ArrayU32: Array1; - /// Storage needed to store usize. - type ArrayUsize: Array1; - /// Storage needed to store vectors. - type ArrayVector: Array1>; - /// Storage needed to store points. - type ArrayPoint: Array1>; - /// Storage needed to store triangle indices. - type ArrayIdx: Array1<[u32; 3]>; - /// Storage needed to store triples of vectors. - type ArrayVectorTriple: Array1<[Vector; 3]>; -} - -#[cfg(feature = "std")] -impl TriMeshStorage for DefaultStorage { - type QbvhStorage = Self; - type ArrayTopoVertex = Vec; - type ArrayTopoFace = Vec; - type ArrayTopoHalfEdge = Vec; - type ArrayU32 = Vec; - type ArrayUsize = Vec; - type ArrayVector = Vec>; - type ArrayPoint = Vec>; - type ArrayIdx = Vec<[u32; 3]>; - type ArrayVectorTriple = Vec<[Vector; 3]>; -} - -#[cfg(all(feature = "std", feature = "cuda"))] -impl TriMeshStorage for CudaStorage { - type QbvhStorage = Self; - type ArrayTopoVertex = CudaArray1; - type ArrayTopoFace = CudaArray1; - type ArrayTopoHalfEdge = CudaArray1; - type ArrayU32 = CudaArray1; - type ArrayUsize = CudaArray1; - type ArrayVector = CudaArray1>; - type ArrayPoint = CudaArray1>; - type ArrayIdx = CudaArray1<[u32; 3]>; - type ArrayVectorTriple = CudaArray1<[Vector; 3]>; -} - -#[cfg(feature = "cuda")] -impl TriMeshStorage for CudaStoragePtr { - type QbvhStorage = Self; - type ArrayTopoVertex = CudaArrayPointer1; - type ArrayTopoFace = CudaArrayPointer1; - type ArrayTopoHalfEdge = CudaArrayPointer1; - type ArrayU32 = CudaArrayPointer1; - type ArrayUsize = CudaArrayPointer1; - type ArrayVector = CudaArrayPointer1>; - type ArrayPoint = CudaArrayPointer1>; - type ArrayIdx = CudaArrayPointer1<[u32; 3]>; - type ArrayVectorTriple = CudaArrayPointer1<[Vector; 3]>; -} diff --git a/src/transformation/to_outline/heightfield_to_outline.rs b/src/transformation/to_outline/heightfield_to_outline.rs index 13e7a597..9f79664c 100644 --- a/src/transformation/to_outline/heightfield_to_outline.rs +++ b/src/transformation/to_outline/heightfield_to_outline.rs @@ -1,8 +1,8 @@ use crate::math::Real; -use crate::shape::{GenericHeightField, HeightFieldCellStatus, HeightFieldStorage}; +use crate::shape::{HeightField, HeightFieldCellStatus}; use na::Point3; -impl GenericHeightField { +impl HeightField { /// Outlines this heightfield’s shape using polylines. pub fn to_outline(&self) -> (Vec>, Vec<[u32; 2]>) { todo!() diff --git a/src/transformation/to_trimesh/heightfield_to_trimesh.rs b/src/transformation/to_trimesh/heightfield_to_trimesh.rs index 6e615d38..28d27cc3 100644 --- a/src/transformation/to_trimesh/heightfield_to_trimesh.rs +++ b/src/transformation/to_trimesh/heightfield_to_trimesh.rs @@ -1,8 +1,8 @@ use crate::math::Real; -use crate::shape::{GenericHeightField, HeightFieldStorage}; +use crate::shape::HeightField; use na::Point3; -impl GenericHeightField { +impl HeightField { /// Discretize the boundary of this heightfield as a triangle-mesh. pub fn to_trimesh(&self) -> (Vec>, Vec<[u32; 3]>) { let mut vertices = Vec::new(); diff --git a/src/utils/array.rs b/src/utils/array.rs deleted file mode 100644 index 1eead7e2..00000000 --- a/src/utils/array.rs +++ /dev/null @@ -1,99 +0,0 @@ -use core::ops::IndexMut; -#[cfg(feature = "std")] -use na::{DMatrix, DVector, Scalar}; - -/// Abstraction over a 1D array. -pub trait Array1: IndexMut { - /// The number of heights on this storage. - fn len(&self) -> usize; - - /// Is this array empty? - #[inline] - fn is_empty(&self) -> bool { - self.len() == 0 - } - - // NOTE: we don’t name it just `get` to avoid clashes with pre-existing `get` methods. - /// Gets the i-th element of this array, if it exists. - #[inline] - fn get_at(&self, i: usize) -> Option<&T> { - if i < self.len() { - Some(&self[i]) - } else { - None - } - } -} - -#[cfg(feature = "std")] -impl Array1 for Vec { - #[inline] - fn len(&self) -> usize { - self.len() - } -} - -#[cfg(feature = "std")] -impl Array1 for DVector { - #[inline] - fn len(&self) -> usize { - self.len() - } -} - -/// Abstraction over a 2D array. -pub trait Array2 { - /// The type of heights. - type Item; - /// The number of rows of the heights grid. - fn nrows(&self) -> usize; - /// The number of columns of the heights grid. - fn ncols(&self) -> usize; - /// Gets the height on the `(i, j)`-th cell of the height grid. - fn get(&self, i: usize, j: usize) -> Self::Item; - /// Sets the height on the `(i, j)`-th cell of the height grid. - fn set(&mut self, i: usize, j: usize, val: Self::Item); -} - -#[cfg(feature = "std")] -impl Array2 for DMatrix { - type Item = T; - - #[inline] - fn nrows(&self) -> usize { - self.nrows() - } - - #[inline] - fn ncols(&self) -> usize { - self.ncols() - } - - #[inline] - fn get(&self, i: usize, j: usize) -> Self::Item { - self[(i, j)].clone() - } - - #[inline] - fn set(&mut self, i: usize, j: usize, val: Self::Item) { - self[(i, j)] = val - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[cfg_attr( - feature = "rkyv", - derive(rkyv::Archive, rkyv::Deserialize, rkyv::Serialize), - archive(check_bytes) -)] -#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] -/// Default data storage based on `Vec`, `DVector`, and `DMatrix`. -pub struct DefaultStorage; -#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] -#[cfg(feature = "cuda")] -/// Data storage residing in CUDA memory. -pub struct CudaStorage; -#[derive(Copy, Clone, Debug, PartialEq, Eq, Default)] -#[cfg(feature = "cuda")] -/// Data storage for accessing data from CUDA kernels. -pub struct CudaStoragePtr; diff --git a/src/utils/consts.rs b/src/utils/consts.rs index 2d0797c1..cca57281 100644 --- a/src/utils/consts.rs +++ b/src/utils/consts.rs @@ -6,6 +6,7 @@ use crate::math::Real; // pub(crate) const COS_10_DEGREES: Real = 0.98480775301; // pub(crate) const COS_45_DEGREES: Real = 0.70710678118; // pub(crate) const SIN_45_DEGREES: Real = COS_45_DEGREES; +pub(crate) const COS_0_DOT_1_DEGREES: Real = 0.99999847691; pub(crate) const COS_1_DEGREES: Real = 0.99984769515; // pub(crate) const COS_5_DEGREES: Real = 0.99619469809; pub(crate) const COS_FRAC_PI_8: Real = 0.92387953251; diff --git a/src/utils/cuda_array.rs b/src/utils/cuda_array.rs deleted file mode 100644 index 751199b1..00000000 --- a/src/utils/cuda_array.rs +++ /dev/null @@ -1,270 +0,0 @@ -#[cfg(target_os = "cuda")] -use crate::shape::HeightFieldStorage; -use crate::utils::{Array1, Array2, DevicePointer}; - -#[cfg(feature = "std")] -use cust::{error::CudaResult, memory::DeviceBuffer}; -use cust_core::DeviceCopy; -use std::ops::{Index, IndexMut}; - -/* - * - * 2D array. - * -*/ -#[cfg(feature = "std")] -/// A 2D array residing on GPU memory. -pub struct CudaArray2 { - data: DeviceBuffer, - nrows: usize, - ncols: usize, -} - -#[cfg(feature = "std")] -impl CudaArray2 { - /// Initialize a 2D cuda array on the GPU. - pub fn new(data: &[T], nrows: usize, ncols: usize) -> CudaResult { - assert_eq!( - data.len(), - nrows * ncols, - "The data length much match the array length." - ); - DeviceBuffer::from_slice(data).map(|data| Self { data, nrows, ncols }) - } - - /// Initialize, using a matrix, a 2D cuda array on the GPU. - pub fn from_matrix(mat: &na::DMatrix) -> CudaResult { - Self::new(mat.as_slice(), mat.nrows(), mat.ncols()) - } - - /// Gets the device pointer to the CUDA memory. - pub fn as_device_ptr(&self) -> CudaArrayPointer2 { - CudaArrayPointer2 { - data: self.data.as_device_ptr(), - nrows: self.nrows, - ncols: self.ncols, - } - } -} - -#[cfg(feature = "std")] -impl Array2 for CudaArray2 { - type Item = T; - - fn nrows(&self) -> usize { - panic!("Cuda arrays cannot be read directly."); - } - - fn ncols(&self) -> usize { - panic!("Cuda arrays cannot be read directly."); - } - - fn get(&self, i: usize, j: usize) -> Self::Item { - panic!("Cuda arrays cannot be read directly."); - } - - fn set(&mut self, i: usize, j: usize, val: Self::Item) { - panic!("Cuda arrays cannot be read directly."); - } -} - -#[repr(C)] -#[derive(Copy, Clone, cust_core::DeviceCopy)] -/// A pointer to a 2D CUDA array. -pub struct CudaArrayPointer2 { - data: DevicePointer, - nrows: usize, - ncols: usize, -} - -#[cfg(all(feature = "dim3", target_os = "cuda"))] -impl Array2 for CudaArrayPointer2 { - type Item = T; - - #[inline] - fn nrows(&self) -> usize { - self.nrows - } - - #[inline] - fn ncols(&self) -> usize { - self.ncols - } - - #[inline] - fn get(&self, i: usize, j: usize) -> Self::Item { - let linear_index = i + j * self.nrows; - assert!(linear_index < self.nrows * self.ncols); - unsafe { *self.data.as_ptr().add(linear_index) } - } - - #[inline] - fn set(&mut self, i: usize, j: usize, val: Self::Item) { - let linear_index = i + j * self.nrows; - assert!(linear_index < self.nrows * self.ncols); - unsafe { - *self.data.as_mut_ptr().add(linear_index) = val; - } - } -} - -#[cfg(all(feature = "dim3", not(target_os = "cuda")))] -impl Array2 for CudaArrayPointer2 { - type Item = T; - - fn nrows(&self) -> usize { - panic!("Cuda pointers can only be read from of a cuda kernel."); - } - - fn ncols(&self) -> usize { - panic!("Cuda pointers can only be read from of a cuda kernel."); - } - - fn get(&self, i: usize, j: usize) -> Self::Item { - panic!("Cuda pointers can only be read from of a cuda kernel."); - } - - fn set(&mut self, i: usize, j: usize, val: Self::Item) { - panic!("Cuda pointers can only be read from of a cuda kernel."); - } -} - -/* - * - * 1D array. - * - */ -#[cfg(feature = "std")] -/// A 1D array residing on GPU memory. -pub struct CudaArray1 { - data: DeviceBuffer, -} - -#[cfg(feature = "std")] -impl CudaArray1 { - /// Initialize a 1D cuda array on the GPU. - pub fn new(data: &[T]) -> CudaResult { - DeviceBuffer::from_slice(data).map(|data| Self { data }) - } - - /// Initialize a 1D cuda array on the GPU using a dynamically-sized vector. - pub fn from_vector(vect: &na::DVector) -> CudaResult { - Self::new(vect.as_slice()) - } - - /// Gets the device pointer to the CUDA memory. - pub fn as_device_ptr(&self) -> CudaArrayPointer1 { - CudaArrayPointer1 { - data: self.data.as_device_ptr(), - len: self.data.len(), - } - } -} - -#[cfg(feature = "std")] -impl Array1 for CudaArray1 { - fn len(&self) -> usize { - panic!("Cuda arrays cannot be read directly."); - } -} - -#[cfg(feature = "std")] -impl Index for CudaArray1 { - type Output = T; - - #[inline] - fn index(&self, _: usize) -> &T { - panic!("Cuda arrays cannot be read directly."); - } -} - -#[cfg(feature = "std")] -impl IndexMut for CudaArray1 { - #[inline] - fn index_mut(&mut self, _: usize) -> &mut T { - panic!("Cuda arrays cannot be read directly."); - } -} - -#[repr(C)] -#[derive(Copy, Clone, cust_core::DeviceCopy)] -/// A pointer to a 2D CUDA array. -pub struct CudaArrayPointer1 { - data: DevicePointer, - len: usize, -} - -#[cfg(target_os = "cuda")] -impl CudaArrayPointer1 { - #[inline] - pub fn len(&self) -> usize { - self.len - } - - #[inline] - pub fn get(&self, i: usize) -> T { - assert!(i < self.len); - unsafe { *self.data.as_ptr().add(i) } - } - - #[inline] - pub fn set(&mut self, i: usize, val: T) { - assert!(i < self.len); - unsafe { - *self.data.as_mut_ptr().add(i) = val; - } - } -} - -#[cfg(not(target_os = "cuda"))] -impl Array1 for CudaArrayPointer1 { - fn len(&self) -> usize { - panic!("Cuda pointers can only be read from of a cuda kernel."); - } -} - -#[cfg(target_os = "cuda")] -impl Array1 for CudaArrayPointer1 { - #[inline] - fn len(&self) -> usize { - self.len - } -} - -#[cfg(not(target_os = "cuda"))] -impl Index for CudaArrayPointer1 { - type Output = T; - - #[inline] - fn index(&self, i: usize) -> &T { - panic!("Cuda pointers can only be read from of a cuda kernel."); - } -} - -#[cfg(not(target_os = "cuda"))] -impl IndexMut for CudaArrayPointer1 { - #[inline] - fn index_mut(&mut self, i: usize) -> &mut T { - panic!("Cuda pointers can only be read from of a cuda kernel."); - } -} - -#[cfg(target_os = "cuda")] -impl Index for CudaArrayPointer1 { - type Output = T; - - #[inline] - fn index(&self, i: usize) -> &T { - assert!(i < self.len); - unsafe { &*self.data.as_ptr().add(i) } - } -} - -#[cfg(target_os = "cuda")] -impl IndexMut for CudaArrayPointer1 { - #[inline] - fn index_mut(&mut self, i: usize) -> &mut T { - assert!(i < self.len); - unsafe { &mut *self.data.as_mut_ptr().add(i) } - } -} diff --git a/src/utils/cuda_device_pointer.rs b/src/utils/cuda_device_pointer.rs deleted file mode 100644 index b5a73611..00000000 --- a/src/utils/cuda_device_pointer.rs +++ /dev/null @@ -1,33 +0,0 @@ -#[cfg(not(target_os = "cuda"))] -pub use cust::memory::DevicePointer; - -#[cfg(target_os = "cuda")] -pub use self::cuda_utils::*; - -#[cfg(target_os = "cuda")] -pub mod cuda_utils { - #[repr(transparent)] - #[derive(PartialEq)] - pub struct DevicePointer(*mut T); - - impl Clone for DevicePointer { - fn clone(&self) -> Self { - Self(self.0) - } - } - - impl Copy for DevicePointer {} - unsafe impl cust_core::DeviceCopy for DevicePointer {} - - impl DevicePointer { - pub fn null() -> Self { - Self(core::ptr::null_mut()) - } - pub fn as_ptr(self) -> *const T { - self.0 - } - pub fn as_mut_ptr(&self) -> *mut T { - self.0 - } - } -} diff --git a/src/utils/mod.rs b/src/utils/mod.rs index 6dbfe227..ce43163a 100644 --- a/src/utils/mod.rs +++ b/src/utils/mod.rs @@ -17,7 +17,6 @@ pub use self::point_cloud_support_point::{ pub use self::point_in_poly2d::{point_in_convex_poly2d, point_in_poly2d}; pub use self::sdp_matrix::{SdpMatrix2, SdpMatrix3}; -pub use self::array::{Array1, Array2, DefaultStorage}; pub use self::as_bytes::AsBytes; pub(crate) use self::consts::*; pub use self::cov::{center_cov, cov}; @@ -33,16 +32,6 @@ pub use self::sorted_pair::SortedPair; pub(crate) use self::weighted_value::WeightedValue; pub(crate) use self::wops::{simd_swap, WBasis, WCross, WSign}; -#[cfg(all(feature = "cuda", feature = "std"))] -pub use self::cuda_array::{CudaArray1, CudaArray2}; -#[cfg(feature = "cuda")] -pub use { - self::array::{CudaStorage, CudaStoragePtr}, - self::cuda_array::{CudaArrayPointer1, CudaArrayPointer2}, - self::cuda_device_pointer::DevicePointer, -}; - -mod array; mod as_bytes; mod ccw_face_normal; mod center; @@ -51,10 +40,6 @@ mod center; mod cleanup; mod consts; mod cov; -#[cfg(feature = "cuda")] -mod cuda_array; -#[cfg(feature = "cuda")] -mod cuda_device_pointer; #[cfg(feature = "std")] mod deterministic_state; mod hashable_partial_eq; diff --git a/src/utils/sdp_matrix.rs b/src/utils/sdp_matrix.rs index 0de2cfe8..50b4130b 100644 --- a/src/utils/sdp_matrix.rs +++ b/src/utils/sdp_matrix.rs @@ -70,12 +70,17 @@ impl SdpMatrix2 { /// Compute the inverse of this SDP matrix without performing any inversibility check. pub fn inverse_unchecked(&self) -> Self { + self.inverse_and_get_determinant_unchecked().0 + } + + /// Compute the inverse of this SDP matrix without performing any inversibility check. + pub fn inverse_and_get_determinant_unchecked(&self) -> (Self, N) { let determinant = self.m11 * self.m22 - self.m12 * self.m12; let m11 = self.m22 / determinant; let m12 = -self.m12 / determinant; let m22 = self.m11 / determinant; - Self { m11, m12, m22 } + (Self { m11, m12, m22 }, determinant) } /// Convert this SDP matrix to a regular matrix representation.