From e231bacec608fa5efd24f7a876572927dbd6c9c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Thu, 17 Dec 2020 10:24:36 +0100 Subject: [PATCH] Move all the contact manifold computations out of Rapier. --- Cargo.toml | 4 +- build/rapier2d/Cargo.toml | 6 +- build/rapier3d/Cargo.toml | 11 +- build/rapier_testbed2d/Cargo.toml | 2 +- build/rapier_testbed3d/Cargo.toml | 2 +- src/data/arena.rs | 2 +- src/data/hashmap.rs | 137 ----------- src/data/maybe_serializable_data.rs | 17 -- src/data/mod.rs | 4 +- src/dynamics/mod.rs | 2 +- .../fixed_velocity_constraint_wide.rs | 2 +- .../prismatic_velocity_constraint.rs | 2 +- .../prismatic_velocity_constraint_wide.rs | 2 +- src/geometry/broad_phase_multi_sap.rs | 8 +- src/geometry/collider.rs | 8 +- .../ball_ball_contact_generator.rs | 103 -------- .../ball_convex_contact_generator.rs | 70 ------ .../ball_polygon_contact_generator.rs | 1 - .../capsule_capsule_contact_generator.rs | 202 ---------------- .../contact_generator/contact_dispatcher.rs | 141 ----------- .../contact_generator/contact_generator.rs | 228 ------------------ .../contact_generator_workspace.rs | 104 -------- .../cuboid_capsule_contact_generator.rs | 190 --------------- .../cuboid_cuboid_contact_generator.rs | 156 ------------ .../cuboid_polygon_contact_generator.rs | 1 - .../cuboid_triangle_contact_generator.rs | 171 ------------- .../heightfield_shape_contact_generator.rs | 190 --------------- src/geometry/contact_generator/mod.rs | 51 ---- .../pfm_pfm_contact_generator.rs | 146 ----------- .../polygon_polygon_contact_generator.rs | 156 ------------ .../serializable_workspace_tag.rs | 9 - .../trimesh_shape_contact_generator.rs | 221 ----------------- .../voxels_shape_contact_generator.rs | 0 src/geometry/{contact.rs => contact_pair.rs} | 32 +-- src/geometry/mod.rs | 53 ++-- src/geometry/narrow_phase.rs | 56 ++--- src/geometry/polygon.rs | 2 +- .../ball_convex_proximity_detector.rs | 2 +- .../cuboid_cuboid_proximity_detector.rs | 4 +- .../cuboid_triangle_proximity_detector.rs | 2 +- .../trimesh_shape_proximity_detector.rs | 4 +- .../{proximity.rs => proximity_pair.rs} | 0 src/geometry/triangle.rs | 7 - src/geometry/z_order.rs | 70 ------ src/lib.rs | 10 +- src/utils.rs | 15 +- src_testbed/lib.rs | 4 +- src_testbed/objects/heightfield.rs | 2 +- src_testbed/objects/polyline.rs | 2 +- 49 files changed, 103 insertions(+), 2511 deletions(-) delete mode 100644 src/data/hashmap.rs delete mode 100644 src/data/maybe_serializable_data.rs delete mode 100644 src/geometry/contact_generator/ball_ball_contact_generator.rs delete mode 100644 src/geometry/contact_generator/ball_convex_contact_generator.rs delete mode 100644 src/geometry/contact_generator/ball_polygon_contact_generator.rs delete mode 100644 src/geometry/contact_generator/capsule_capsule_contact_generator.rs delete mode 100644 src/geometry/contact_generator/contact_dispatcher.rs delete mode 100644 src/geometry/contact_generator/contact_generator.rs delete mode 100644 src/geometry/contact_generator/contact_generator_workspace.rs delete mode 100644 src/geometry/contact_generator/cuboid_capsule_contact_generator.rs delete mode 100644 src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs delete mode 100644 src/geometry/contact_generator/cuboid_polygon_contact_generator.rs delete mode 100644 src/geometry/contact_generator/cuboid_triangle_contact_generator.rs delete mode 100644 src/geometry/contact_generator/heightfield_shape_contact_generator.rs delete mode 100644 src/geometry/contact_generator/mod.rs delete mode 100644 src/geometry/contact_generator/pfm_pfm_contact_generator.rs delete mode 100644 src/geometry/contact_generator/polygon_polygon_contact_generator.rs delete mode 100644 src/geometry/contact_generator/serializable_workspace_tag.rs delete mode 100644 src/geometry/contact_generator/trimesh_shape_contact_generator.rs delete mode 100644 src/geometry/contact_generator/voxels_shape_contact_generator.rs rename src/geometry/{contact.rs => contact_pair.rs} (90%) rename src/geometry/{proximity.rs => proximity_pair.rs} (100%) delete mode 100644 src/geometry/triangle.rs delete mode 100644 src/geometry/z_order.rs diff --git a/Cargo.toml b/Cargo.toml index 55e9b43..eaafe91 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,8 +10,8 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark #nphysics2d = { path = "../nphysics/build/nphysics2d" } #nphysics3d = { path = "../nphysics/build/nphysics3d" } #kiss3d = { path = "../kiss3d" } -buckler2d = { path = "../buckler/build/buckler2d" } -buckler3d = { path = "../buckler/build/buckler3d" } +eagl2d = { path = "../eagl/build/eagl2d" } +eagl3d = { path = "../eagl/build/eagl3d" } [profile.release] #debug = true diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 987fa2f..c20833c 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -21,8 +21,8 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] -enhanced-determinism = [ "simba/libm_force", "indexmap" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "eagl2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +enhanced-determinism = [ "simba/libm_force", "eagl2d/enhanced-determinism", "indexmap" ] [lib] name = "rapier2d" @@ -35,7 +35,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.23" -buckler2d = "0.1" +eagl2d = "0.1" simba = "0.3" approx = "0.4" rayon = { version = "1", optional = true } diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index 7d5673b..a9abef8 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -15,14 +15,14 @@ edition = "2018" default = [ "dim3" ] dim3 = [ ] parallel = [ "rayon" ] -simd-stable = [ "buckler3d/simd-stable", "simba/wide", "simd-is-enabled" ] -simd-nightly = [ "buckler3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] +simd-stable = [ "eagl3d/simd-stable", "simba/wide", "simd-is-enabled" ] +simd-nightly = [ "eagl3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] -enhanced-determinism = [ "simba/libm_force", "indexmap" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "eagl3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +enhanced-determinism = [ "simba/libm_force", "eagl3d/enhanced-determinism" ] [lib] name = "rapier3d" @@ -35,7 +35,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.23" -buckler3d = "0.1" +eagl3d = "0.1" simba = "0.3" approx = "0.4" rayon = { version = "1", optional = true } @@ -46,7 +46,6 @@ bit-vec = "0.6" rustc-hash = "1" serde = { version = "1", features = [ "derive" ], optional = true } erased-serde = { version = "0.3", optional = true } -indexmap = { version = "1", features = [ "serde-1" ], optional = true } downcast-rs = "1.2" num-derive = "0.3" bitflags = "1" diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index 72ead41..23be92e 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -31,7 +31,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" num_cpus = { version = "1", optional = true } wrapped2d = { version = "0.4", optional = true } -buckler2d = "0.1" +eagl2d = "0.1" ncollide2d = "0.26" nphysics2d = { version = "0.18", optional = true } crossbeam = "0.8" diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index d4ad764..2820a7c 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -30,7 +30,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" glam = { version = "0.10", optional = true } num_cpus = { version = "1", optional = true } -buckler3d = "0.1" +eagl3d = "0.1" ncollide3d = "0.26" nphysics3d = { version = "0.18", optional = true } physx = { version = "0.8", optional = true } diff --git a/src/data/arena.rs b/src/data/arena.rs index a3af45c..3a24cd1 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -3,7 +3,7 @@ //! See https://github.com/fitzgen/generational-arena/blob/master/src/lib.rs. //! This has been modified to have a fully deterministic deserialization (including for the order of //! Index attribution after a deserialization of the arena. -use buckler::partitioning::IndexedData; +use eagl::partitioning::IndexedData; use std::cmp; use std::iter::{self, Extend, FromIterator, FusedIterator}; use std::mem; diff --git a/src/data/hashmap.rs b/src/data/hashmap.rs deleted file mode 100644 index d2ea980..0000000 --- a/src/data/hashmap.rs +++ /dev/null @@ -1,137 +0,0 @@ -//! A hash-map that behaves deterministically when the -//! `enhanced-determinism` feature is enabled. - -#[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))] -use indexmap::IndexMap as StdHashMap; -#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))] -use std::collections::HashMap as StdHashMap; - -/// Serializes only the capacity of a hash-map instead of its actual content. -#[cfg(feature = "serde-serialize")] -pub fn serialize_hashmap_capacity( - map: &StdHashMap, - s: S, -) -> Result { - s.serialize_u64(map.capacity() as u64) -} - -/// Creates a new hash-map with its capacity deserialized from `d`. -#[cfg(feature = "serde-serialize")] -pub fn deserialize_hashmap_capacity< - 'de, - D: serde::Deserializer<'de>, - K, - V, - H: std::hash::BuildHasher + Default, ->( - d: D, -) -> Result, D::Error> { - struct CapacityVisitor; - impl<'de> serde::de::Visitor<'de> for CapacityVisitor { - type Value = u64; - - fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { - write!(formatter, "an integer between 0 and 2^64") - } - - fn visit_u64(self, val: u64) -> Result { - Ok(val) - } - } - - let capacity = d.deserialize_u64(CapacityVisitor)? as usize; - Ok(StdHashMap::with_capacity_and_hasher( - capacity, - Default::default(), - )) -} - -/* - * FxHasher taken from rustc_hash, except that it does not depend on the pointer size. - */ -#[cfg(feature = "enhanced-determinism")] -pub type FxHashMap32 = indexmap::IndexMap>; -#[cfg(feature = "enhanced-determinism")] -pub use {self::FxHashMap32 as HashMap, indexmap::map::Entry}; -#[cfg(not(feature = "enhanced-determinism"))] -pub use {rustc_hash::FxHashMap as HashMap, std::collections::hash_map::Entry}; - -const K: u32 = 0x9e3779b9; - -// Same as FxHasher, but with the guarantee that the internal hash is -// an u32 instead of something that depends on the platform. -pub struct FxHasher32 { - hash: u32, -} - -impl Default for FxHasher32 { - #[inline] - fn default() -> FxHasher32 { - FxHasher32 { hash: 0 } - } -} - -impl FxHasher32 { - #[inline] - fn add_to_hash(&mut self, i: u32) { - use std::ops::BitXor; - self.hash = self.hash.rotate_left(5).bitxor(i).wrapping_mul(K); - } -} - -impl std::hash::Hasher for FxHasher32 { - #[inline] - fn write(&mut self, mut bytes: &[u8]) { - use std::convert::TryInto; - let read_u32 = |bytes: &[u8]| u32::from_ne_bytes(bytes[..4].try_into().unwrap()); - let mut hash = FxHasher32 { hash: self.hash }; - assert!(std::mem::size_of::() <= 8); - while bytes.len() >= std::mem::size_of::() { - hash.add_to_hash(read_u32(bytes) as u32); - bytes = &bytes[std::mem::size_of::()..]; - } - if (std::mem::size_of::() > 4) && (bytes.len() >= 4) { - hash.add_to_hash(u32::from_ne_bytes(bytes[..4].try_into().unwrap()) as u32); - bytes = &bytes[4..]; - } - if (std::mem::size_of::() > 2) && bytes.len() >= 2 { - hash.add_to_hash(u16::from_ne_bytes(bytes[..2].try_into().unwrap()) as u32); - bytes = &bytes[2..]; - } - if (std::mem::size_of::() > 1) && bytes.len() >= 1 { - hash.add_to_hash(bytes[0] as u32); - } - self.hash = hash.hash; - } - - #[inline] - fn write_u8(&mut self, i: u8) { - self.add_to_hash(i as u32); - } - - #[inline] - fn write_u16(&mut self, i: u16) { - self.add_to_hash(i as u32); - } - - #[inline] - fn write_u32(&mut self, i: u32) { - self.add_to_hash(i as u32); - } - - #[inline] - fn write_u64(&mut self, i: u64) { - self.add_to_hash(i as u32); - self.add_to_hash((i >> 32) as u32); - } - - #[inline] - fn write_usize(&mut self, i: usize) { - self.add_to_hash(i as u32); - } - - #[inline] - fn finish(&self) -> u64 { - self.hash as u64 - } -} diff --git a/src/data/maybe_serializable_data.rs b/src/data/maybe_serializable_data.rs deleted file mode 100644 index 8b14e1a..0000000 --- a/src/data/maybe_serializable_data.rs +++ /dev/null @@ -1,17 +0,0 @@ -use downcast_rs::{impl_downcast, DowncastSync}; -#[cfg(feature = "serde-serialize")] -use erased_serde::Serialize; - -/// Piece of data that may be serializable. -pub trait MaybeSerializableData: DowncastSync { - /// Convert this shape as a serializable entity. - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> { - None - } - - /// Clones `self`. - fn clone_dyn(&self) -> Box; -} - -impl_downcast!(sync MaybeSerializableData); diff --git a/src/data/mod.rs b/src/data/mod.rs index 672bf94..eb0a229 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -1,11 +1,9 @@ //! Data structures modified with guaranteed deterministic behavior after deserialization. pub use self::coarena::Coarena; -pub use self::maybe_serializable_data::MaybeSerializableData; +pub use eagl::utils::MaybeSerializableData; pub mod arena; mod coarena; pub(crate) mod graph; -pub(crate) mod hashmap; -mod maybe_serializable_data; pub mod pubsub; diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 28f149a..76e9de2 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -9,7 +9,7 @@ pub use self::joint::{ }; pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; -pub use buckler::shape::MassProperties; +pub use eagl::shape::MassProperties; // #[cfg(not(feature = "parallel"))] pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::rigid_body::RigidBodyChanges; diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index 79c69c6..ce42da8 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -13,7 +13,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix6, Vector6, U3}; #[cfg(feature = "dim2")] use { - crate::utils::SdpMatrix3, + eagl::utils::SdpMatrix3, na::{Matrix3, Vector3}, }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 49cfc7a..a2c7c2c 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -8,7 +8,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] use { - crate::utils::SdpMatrix2, + eagl::utils::SdpMatrix2, na::{Matrix2, Vector2}, }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index c05c08e..86e0c78 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -12,7 +12,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] use { - crate::utils::SdpMatrix2, + eagl::utils::SdpMatrix2, na::{Matrix2, Vector2}, }; diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 56c05df..4242d77 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -1,10 +1,10 @@ -use crate::data::hashmap::HashMap; use crate::data::pubsub::Subscription; use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; use bit_vec::BitVec; -use buckler::bounding_volume::{BoundingVolume, AABB}; +use eagl::bounding_volume::{BoundingVolume, AABB}; +use eagl::utils::hashmap::HashMap; use std::cmp::Ordering; use std::ops::{Index, IndexMut}; @@ -477,8 +477,8 @@ pub struct BroadPhase { #[cfg_attr( feature = "serde-serialize", serde( - serialize_with = "crate::data::hashmap::serialize_hashmap_capacity", - deserialize_with = "crate::data::hashmap::deserialize_hashmap_capacity" + serialize_with = "eagl::utils::hashmap::serialize_hashmap_capacity", + deserialize_with = "eagl::utils::hashmap::deserialize_hashmap_capacity" ) )] reporting: HashMap<(u32, u32), bool>, // Workspace diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 1275358..db418a3 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,13 +1,13 @@ -use crate::buckler::shape::HalfSpace; use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; +use crate::eagl::shape::HalfSpace; use crate::geometry::InteractionGroups; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; -use buckler::bounding_volume::AABB; -use buckler::shape::{ +use eagl::bounding_volume::AABB; +use eagl::shape::{ Ball, Capsule, Cuboid, HeightField, Segment, Shape, ShapeType, TriMesh, Triangle, }; #[cfg(feature = "dim3")] -use buckler::shape::{Cone, Cylinder, RoundCylinder}; +use eagl::shape::{Cone, Cylinder, RoundCylinder}; use na::Point3; use std::ops::Deref; use std::sync::Arc; diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs deleted file mode 100644 index f2ca7af..0000000 --- a/src/geometry/contact_generator/ball_ball_contact_generator.rs +++ /dev/null @@ -1,103 +0,0 @@ -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Contact, ContactManifoldData, KinematicsCategory}; -use crate::math::{Point, Vector}; -#[cfg(feature = "simd-is-enabled")] -use { - crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd, - crate::geometry::{WBall, WContact}, - crate::math::{Isometry, SimdReal, SIMD_WIDTH}, - simba::simd::SimdValue, -}; - -#[cfg(feature = "simd-is-enabled")] -fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { - let dcenter = ball2.center - ball1.center; - let center_dist = dcenter.magnitude(); - let normal = dcenter / center_dist; - - WContact { - local_p1: ball1.center + normal * ball1.radius, - local_p2: pos21 * (ball2.center - normal * ball2.radius), - local_n1: normal, - local_n2: pos21 * -normal, - fid1: [0; SIMD_WIDTH], - fid2: [0; SIMD_WIDTH], - dist: center_dist - ball1.radius - ball2.radius, - } -} - -#[cfg(feature = "simd-is-enabled")] -pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) { - let pos_ba = ctxt.positions2.inverse() * ctxt.positions1; - let radii_a = - SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); - let radii_b = - SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); - - let wball_a = WBall::new(Point::origin(), radii_a); - let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b); - let contacts = generate_contacts_simd(&wball_a, &wball_b, &pos_ba); - - for (i, manifold) in ctxt.manifolds.iter_mut().enumerate() { - // FIXME: compare the dist before extracting the contact. - let (contact, local_n1, local_n2) = contacts.extract(i); - if contact.dist <= ctxt.prediction_distance { - if manifold.points.len() != 0 { - manifold.points[0].copy_geometry_from(contact); - } else { - manifold.points.push(contact); - } - - manifold.local_n1 = local_n1; - manifold.local_n2 = local_n2; - manifold.kinematics.category = KinematicsCategory::PointPoint; - manifold.kinematics.radius1 = radii_a.extract(i); - manifold.kinematics.radius2 = radii_b.extract(i); - ContactManifoldData::update_warmstart_multiplier(manifold); - } else { - manifold.points.clear(); - } - - manifold.sort_contacts(ctxt.prediction_distance); - } -} - -pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) { - let pos_ba = ctxt.position2.inverse() * ctxt.position1; - let radius_a = ctxt.shape1.as_ball().unwrap().radius; - let radius_b = ctxt.shape2.as_ball().unwrap().radius; - - let dcenter = pos_ba.inverse_transform_point(&Point::origin()).coords; - let center_dist = dcenter.magnitude(); - let dist = center_dist - radius_a - radius_b; - - if dist < ctxt.prediction_distance { - let local_n1 = if center_dist != 0.0 { - dcenter / center_dist - } else { - Vector::y() - }; - - let local_n2 = pos_ba.inverse_transform_vector(&-local_n1); - let local_p1 = local_n1 * radius_a; - let local_p2 = local_n2 * radius_b; - let contact = Contact::new(local_p1.into(), local_p2.into(), 0, 0, dist); - - if ctxt.manifold.points.len() != 0 { - ctxt.manifold.points[0].copy_geometry_from(contact); - } else { - ctxt.manifold.points.push(contact); - } - - ctxt.manifold.local_n1 = local_n1; - ctxt.manifold.local_n2 = local_n2; - ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint; - ctxt.manifold.kinematics.radius1 = radius_a; - ctxt.manifold.kinematics.radius2 = radius_b; - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - } else { - ctxt.manifold.points.clear(); - } - - ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs deleted file mode 100644 index 88f1912..0000000 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ /dev/null @@ -1,70 +0,0 @@ -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Ball, Contact, ContactManifoldData, KinematicsCategory}; -use crate::math::Isometry; -use buckler::query::PointQuery; -use na::Unit; - -pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { - if let Some(ball1) = ctxt.shape1.as_ball() { - ctxt.manifold.swap_identifiers(); - do_generate_contacts(ctxt.shape2, ball1, ctxt, true); - } else if let Some(ball2) = ctxt.shape2.as_ball() { - do_generate_contacts(ctxt.shape1, ball2, ctxt, false); - } - - ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} - -fn do_generate_contacts( - point_query1: &P, - ball2: &Ball, - ctxt: &mut PrimitiveContactGenerationContext, - swapped: bool, -) { - let position1; - let position2; - - if swapped { - position1 = ctxt.position2; - position2 = ctxt.position1; - } else { - position1 = ctxt.position1; - position2 = ctxt.position2; - } - - let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into()); - let proj = point_query1.project_local_point(&local_p2_1, cfg!(feature = "dim3")); - let dpos = local_p2_1 - proj.local_point; - - #[allow(unused_mut)] // Because `mut local_n1, mut dist` is needed in 2D but not in 3D. - if let Some((mut local_n1, mut dist)) = Unit::try_new_and_get(dpos, 0.0) { - #[cfg(feature = "dim2")] - if proj.is_inside { - local_n1 = -local_n1; - dist = -dist; - } - - if dist <= ball2.radius + ctxt.prediction_distance { - let local_n2 = position2.inverse_transform_vector(&(position1 * -*local_n1)); - let local_p2 = (local_n2 * ball2.radius).into(); - let contact_point = Contact::new(proj.local_point, local_p2, 0, 0, dist - ball2.radius); - - if ctxt.manifold.points.len() != 1 { - ctxt.manifold.points.clear(); - ctxt.manifold.points.push(contact_point); - } else { - // Copy only the geometry so we keep the warmstart impulses. - ctxt.manifold.points[0].copy_geometry_from(contact_point); - } - - ctxt.manifold.local_n1 = *local_n1; - ctxt.manifold.local_n2 = local_n2; - ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; - ctxt.manifold.kinematics.radius1 = 0.0; - ctxt.manifold.kinematics.radius2 = ball2.radius; - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - } else { - ctxt.manifold.points.clear(); - } - } -} diff --git a/src/geometry/contact_generator/ball_polygon_contact_generator.rs b/src/geometry/contact_generator/ball_polygon_contact_generator.rs deleted file mode 100644 index 8b13789..0000000 --- a/src/geometry/contact_generator/ball_polygon_contact_generator.rs +++ /dev/null @@ -1 +0,0 @@ - diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs deleted file mode 100644 index 9090b36..0000000 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ /dev/null @@ -1,202 +0,0 @@ -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Capsule, Contact, ContactManifold, ContactManifoldData, KinematicsCategory}; -use crate::math::Isometry; -use crate::math::Vector; -use approx::AbsDiffEq; -#[cfg(feature = "dim2")] -use buckler::shape::SegmentPointLocation; -use na::Unit; - -pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) { - generate_contacts( - ctxt.prediction_distance, - capsule1, - ctxt.position1, - capsule2, - ctxt.position2, - ctxt.manifold, - ); - } - - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} - -#[cfg(feature = "dim2")] -pub fn generate_contacts<'a>( - prediction_distance: f32, - capsule1: &'a Capsule, - pos1: &'a Isometry, - capsule2: &'a Capsule, - pos2: &'a Isometry, - manifold: &mut ContactManifold, -) { - // FIXME: the contact kinematics is not correctly set here. - // We use the common "Point-Plane" kinematics with zero radius everytime. - // Instead we should select point/point ore point-plane (with non-zero - // radius for the point) depending on the features involved in the contact. - let pos12 = pos1.inverse() * pos2; - let pos21 = pos12.inverse(); - - let seg1 = capsule1.segment; - let seg2_1 = capsule2.segment.transformed(&pos12); - let (loc1, loc2) = buckler::query::details::closest_points_segment_segment_with_locations_nD( - (&seg1.a, &seg1.b), - (&seg2_1.a, &seg2_1.b), - ); - - // We do this clone to perform contact tracking and transfer impulses. - // FIXME: find a more efficient way of doing this. - let old_manifold_points = manifold.points.clone(); - manifold.points.clear(); - - let swapped = false; - - let fid1 = if let SegmentPointLocation::OnVertex(v1) = loc1 { - v1 as u8 * 2 - } else { - 1 - }; - let fid2 = if let SegmentPointLocation::OnVertex(v2) = loc2 { - v2 as u8 * 2 - } else { - 1 - }; - - let bcoords1 = loc1.barycentric_coordinates(); - let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1]; - let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1]; - - let local_n1 = - Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis()); - let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius; - - if dist <= prediction_distance { - let local_n2 = pos21 * -local_n1; - let contact = Contact::new(local_p1, pos21 * local_p2, fid1, fid2, dist); - manifold.points.push(contact); - - manifold.local_n1 = *local_n1; - manifold.local_n2 = *local_n2; - manifold.kinematics.category = KinematicsCategory::PlanePoint; - manifold.kinematics.radius1 = 0.0; - manifold.kinematics.radius2 = 0.0; - } else { - // No contact within tolerance. - return; - } - - if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.direction()) { - if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8 - && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8 - { - // Capsules axes are almost parallel and are almost perpendicular to the normal. - // Find a second contact point. - if let Some((clip_a, clip_b)) = - buckler::query::details::clip_segment_segment_with_normal( - (seg1.a, seg1.b), - (seg2_1.a, seg2_1.b), - *local_n1, - ) - { - let contact = - if (clip_a.0 - local_p1).norm_squared() > f32::default_epsilon() * 100.0 { - // Use clip_a as the second contact. - Contact::new( - clip_a.0, - pos21 * clip_a.1, - clip_a.2 as u8, - clip_a.3 as u8, - (clip_a.1 - clip_a.0).dot(&local_n1), - ) - } else { - // Use clip_b as the second contact. - Contact::new( - clip_b.0, - pos21 * clip_b.1, - clip_b.2 as u8, - clip_b.3 as u8, - (clip_b.1 - clip_b.0).dot(&local_n1), - ) - }; - - manifold.points.push(contact); - } - } - } - - if swapped { - for point in &mut manifold.points { - point.local_p1 += manifold.local_n1 * capsule2.radius; - point.local_p2 += manifold.local_n2 * capsule1.radius; - point.dist -= capsule1.radius + capsule2.radius; - } - } else { - for point in &mut manifold.points { - point.local_p1 += manifold.local_n1 * capsule1.radius; - point.local_p2 += manifold.local_n2 * capsule2.radius; - point.dist -= capsule1.radius + capsule2.radius; - } - } - - manifold.match_contacts(&old_manifold_points, swapped); -} - -#[cfg(feature = "dim3")] -pub fn generate_contacts<'a>( - prediction_distance: f32, - capsule1: &'a Capsule, - pos1: &'a Isometry, - capsule2: &'a Capsule, - pos2: &'a Isometry, - manifold: &mut ContactManifold, -) { - let pos12 = pos1.inverse() * pos2; - let pos21 = pos12.inverse(); - - let seg1 = capsule1.segment; - let seg2_1 = capsule2.segment.transformed(&pos12); - let (loc1, loc2) = - buckler::query::closest_points::closest_points_segment_segment_with_locations_nD( - (&seg1.a, &seg1.b), - (&seg2_1.a, &seg2_1.b), - ); - - { - let bcoords1 = loc1.barycentric_coordinates(); - let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1]; - let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1]; - - let local_n1 = - Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis()); - let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius; - - if dist <= prediction_distance { - let local_n2 = pos21 * -local_n1; - let contact = Contact::new( - local_p1 + *local_n1 * capsule1.radius, - pos21 * local_p2 + *local_n2 * capsule2.radius, - 0, - 0, - dist, - ); - - if manifold.points.len() != 0 { - manifold.points[0].copy_geometry_from(contact); - } else { - manifold.points.push(contact); - } - - manifold.local_n1 = *local_n1; - manifold.local_n2 = *local_n2; - manifold.kinematics.category = KinematicsCategory::PlanePoint; - manifold.kinematics.radius1 = 0.0; - manifold.kinematics.radius2 = 0.0; - } else { - manifold.points.clear(); - } - } -} diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs deleted file mode 100644 index 9b247f3..0000000 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ /dev/null @@ -1,141 +0,0 @@ -#[cfg(feature = "dim3")] -use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace; -use crate::geometry::contact_generator::{ - ContactGenerator, ContactGeneratorWorkspace, ContactPhase, - HeightFieldShapeContactGeneratorWorkspace, PrimitiveContactGenerator, - TriMeshShapeContactGeneratorWorkspace, -}; -use buckler::shape::ShapeType; - -/// Trait implemented by structures responsible for selecting a collision-detection algorithm -/// for a given pair of shapes. -pub trait ContactDispatcher { - /// Select the collision-detection algorithm for the given pair of primitive shapes. - fn dispatch_primitives( - &self, - shape1: ShapeType, - shape2: ShapeType, - ) -> (PrimitiveContactGenerator, Option); - /// Select the collision-detection algorithm for the given pair of non-primitive shapes. - fn dispatch( - &self, - shape1: ShapeType, - shape2: ShapeType, - ) -> (ContactPhase, Option); -} - -/// The default contact dispatcher used by Rapier. -pub struct DefaultContactDispatcher; - -impl ContactDispatcher for DefaultContactDispatcher { - fn dispatch_primitives( - &self, - shape1: ShapeType, - shape2: ShapeType, - ) -> (PrimitiveContactGenerator, Option) { - match (shape1, shape2) { - (ShapeType::Ball, ShapeType::Ball) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_ball_ball, - #[cfg(feature = "simd-is-enabled")] - generate_contacts_simd: super::generate_contacts_ball_ball_simd, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (ShapeType::Cuboid, ShapeType::Cuboid) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_cuboid_cuboid, - ..PrimitiveContactGenerator::default() - }, - None, - ), - // (ShapeType::Polygon, ShapeType::Polygon) => ( - // PrimitiveContactGenerator { - // generate_contacts: super::generate_contacts_polygon_polygon, - // ..PrimitiveContactGenerator::default() - // }, - // None, - // ), - (ShapeType::Capsule, ShapeType::Capsule) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_capsule_capsule, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (_, ShapeType::Ball) | (ShapeType::Ball, _) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_ball_convex, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_cuboid_capsule, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => { - ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_cuboid_triangle, - ..PrimitiveContactGenerator::default() - }, - None, - ) - } - #[cfg(feature = "dim3")] - (ShapeType::Cylinder, _) - | (_, ShapeType::Cylinder) - | (ShapeType::Cone, _) - | (_, ShapeType::Cone) - | (ShapeType::RoundCylinder, _) - | (_, ShapeType::RoundCylinder) - | (ShapeType::Capsule, _) - | (_, ShapeType::Capsule) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_pfm_pfm, - ..PrimitiveContactGenerator::default() - }, - Some(ContactGeneratorWorkspace::from( - PfmPfmContactManifoldGeneratorWorkspace::default(), - )), - ), - _ => (PrimitiveContactGenerator::default(), None), - } - } - - fn dispatch( - &self, - shape1: ShapeType, - shape2: ShapeType, - ) -> (ContactPhase, Option) { - match (shape1, shape2) { - (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => ( - ContactPhase::NearPhase(ContactGenerator { - generate_contacts: super::generate_contacts_trimesh_shape, - ..ContactGenerator::default() - }), - Some(ContactGeneratorWorkspace::from( - TriMeshShapeContactGeneratorWorkspace::new(), - )), - ), - (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => ( - ContactPhase::NearPhase(ContactGenerator { - generate_contacts: super::generate_contacts_heightfield_shape, - ..ContactGenerator::default() - }), - Some(ContactGeneratorWorkspace::from( - HeightFieldShapeContactGeneratorWorkspace::new(), - )), - ), - _ => { - let (gen, workspace) = self.dispatch_primitives(shape1, shape2); - (ContactPhase::ExactPhase(gen), workspace) - } - } - } -} diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs deleted file mode 100644 index 06ab265..0000000 --- a/src/geometry/contact_generator/contact_generator.rs +++ /dev/null @@ -1,228 +0,0 @@ -use crate::data::MaybeSerializableData; -use crate::geometry::{ - Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape, - ShapeType, SolverFlags, -}; -use crate::math::Isometry; -#[cfg(feature = "simd-is-enabled")] -use crate::math::{SimdReal, SIMD_WIDTH}; -use crate::pipeline::EventHandler; - -#[derive(Copy, Clone)] -pub enum ContactPhase { - NearPhase(ContactGenerator), - ExactPhase(PrimitiveContactGenerator), -} - -impl ContactPhase { - #[inline] - pub fn generate_contacts( - self, - mut context: ContactGenerationContext, - events: &dyn EventHandler, - ) { - let had_contacts_before = context.pair.has_any_active_contact(); - - match self { - Self::NearPhase(gen) => (gen.generate_contacts)(&mut context), - Self::ExactPhase(gen) => { - // Build the primitive context from the non-primitive context and dispatch. - let (collider1, collider2, manifold, workspace) = context - .pair - .single_manifold(context.colliders, context.solver_flags); - let mut context2 = PrimitiveContactGenerationContext { - prediction_distance: context.prediction_distance, - collider1, - collider2, - shape1: collider1.shape(), - shape2: collider2.shape(), - position1: collider1.position(), - position2: collider2.position(), - manifold, - workspace, - }; - - (gen.generate_contacts)(&mut context2) - } - } - - if had_contacts_before != context.pair.has_any_active_contact() { - if had_contacts_before { - events.handle_contact_event(ContactEvent::Stopped( - context.pair.pair.collider1, - context.pair.pair.collider2, - )); - } else { - events.handle_contact_event(ContactEvent::Started( - context.pair.pair.collider1, - context.pair.pair.collider2, - )) - } - } - } - - #[cfg(feature = "simd-is-enabled")] - #[inline] - pub fn generate_contacts_simd( - self, - mut context: ContactGenerationContextSimd, - events: &dyn EventHandler, - ) { - let mut had_contacts_before = [false; SIMD_WIDTH]; - - for (i, pair) in context.pairs.iter().enumerate() { - had_contacts_before[i] = pair.has_any_active_contact() - } - - match self { - Self::NearPhase(gen) => (gen.generate_contacts_simd)(&mut context), - Self::ExactPhase(gen) => { - // Build the primitive context from the non-primitive context and dispatch. - use arrayvec::ArrayVec; - let mut colliders_arr: ArrayVec<[(&Collider, &Collider); SIMD_WIDTH]> = - ArrayVec::new(); - let mut manifold_arr: ArrayVec<[&mut ContactManifold; SIMD_WIDTH]> = - ArrayVec::new(); - let mut workspace_arr: ArrayVec< - [Option<&mut (dyn MaybeSerializableData)>; SIMD_WIDTH], - > = ArrayVec::new(); - - for (pair, solver_flags) in - context.pairs.iter_mut().zip(context.solver_flags.iter()) - { - let (collider1, collider2, manifold, workspace) = - pair.single_manifold(context.colliders, *solver_flags); - colliders_arr.push((collider1, collider2)); - manifold_arr.push(manifold); - workspace_arr.push(workspace); - } - - let max_index = colliders_arr.len() - 1; - let colliders1 = array![|ii| colliders_arr[ii.min(max_index)].0; SIMD_WIDTH]; - let colliders2 = array![|ii| colliders_arr[ii.min(max_index)].1; SIMD_WIDTH]; - - let mut context2 = PrimitiveContactGenerationContextSimd { - prediction_distance: context.prediction_distance, - colliders1, - colliders2, - shapes1: array![|ii| colliders1[ii].shape(); SIMD_WIDTH], - shapes2: array![|ii| colliders2[ii].shape(); SIMD_WIDTH], - positions1: &Isometry::from( - array![|ii| *colliders1[ii].position(); SIMD_WIDTH], - ), - positions2: &Isometry::from( - array![|ii| *colliders2[ii].position(); SIMD_WIDTH], - ), - manifolds: manifold_arr.as_mut_slice(), - workspaces: workspace_arr.as_mut_slice(), - }; - - (gen.generate_contacts_simd)(&mut context2) - } - } - - for (i, pair) in context.pairs.iter().enumerate() { - if had_contacts_before[i] != pair.has_any_active_contact() { - if had_contacts_before[i] { - events.handle_contact_event(ContactEvent::Stopped( - pair.pair.collider1, - pair.pair.collider2, - )) - } else { - events.handle_contact_event(ContactEvent::Started( - pair.pair.collider1, - pair.pair.collider2, - )) - } - } - } - } -} - -pub struct PrimitiveContactGenerationContext<'a> { - pub prediction_distance: f32, - pub collider1: &'a Collider, - pub collider2: &'a Collider, - pub shape1: &'a dyn Shape, - pub shape2: &'a dyn Shape, - pub position1: &'a Isometry, - pub position2: &'a Isometry, - pub manifold: &'a mut ContactManifold, - pub workspace: Option<&'a mut (dyn MaybeSerializableData)>, -} - -#[cfg(feature = "simd-is-enabled")] -pub struct PrimitiveContactGenerationContextSimd<'a, 'b> { - pub prediction_distance: f32, - pub colliders1: [&'a Collider; SIMD_WIDTH], - pub colliders2: [&'a Collider; SIMD_WIDTH], - pub shapes1: [&'a dyn Shape; SIMD_WIDTH], - pub shapes2: [&'a dyn Shape; SIMD_WIDTH], - pub positions1: &'a Isometry, - pub positions2: &'a Isometry, - pub manifolds: &'a mut [&'b mut ContactManifold], - pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>], -} - -#[derive(Copy, Clone)] -pub struct PrimitiveContactGenerator { - pub generate_contacts: fn(&mut PrimitiveContactGenerationContext), - #[cfg(feature = "simd-is-enabled")] - pub generate_contacts_simd: fn(&mut PrimitiveContactGenerationContextSimd), -} - -impl PrimitiveContactGenerator { - fn unimplemented_fn(_ctxt: &mut PrimitiveContactGenerationContext) {} - #[cfg(feature = "simd-is-enabled")] - fn unimplemented_simd_fn(_ctxt: &mut PrimitiveContactGenerationContextSimd) {} -} - -impl Default for PrimitiveContactGenerator { - fn default() -> Self { - Self { - generate_contacts: Self::unimplemented_fn, - #[cfg(feature = "simd-is-enabled")] - generate_contacts_simd: Self::unimplemented_simd_fn, - } - } -} - -pub struct ContactGenerationContext<'a> { - pub dispatcher: &'a dyn ContactDispatcher, - pub prediction_distance: f32, - pub colliders: &'a ColliderSet, - pub pair: &'a mut ContactPair, - pub solver_flags: SolverFlags, -} - -#[cfg(feature = "simd-is-enabled")] -pub struct ContactGenerationContextSimd<'a, 'b> { - pub dispatcher: &'a dyn ContactDispatcher, - pub prediction_distance: f32, - pub colliders: &'a ColliderSet, - pub pairs: &'a mut [&'b mut ContactPair], - pub solver_flags: &'a [SolverFlags], -} - -#[derive(Copy, Clone)] -pub struct ContactGenerator { - pub generate_contacts: fn(&mut ContactGenerationContext), - #[cfg(feature = "simd-is-enabled")] - pub generate_contacts_simd: fn(&mut ContactGenerationContextSimd), -} - -impl ContactGenerator { - fn unimplemented_fn(_ctxt: &mut ContactGenerationContext) {} - #[cfg(feature = "simd-is-enabled")] - fn unimplemented_simd_fn(_ctxt: &mut ContactGenerationContextSimd) {} -} - -impl Default for ContactGenerator { - fn default() -> Self { - Self { - generate_contacts: Self::unimplemented_fn, - #[cfg(feature = "simd-is-enabled")] - generate_contacts_simd: Self::unimplemented_simd_fn, - } - } -} diff --git a/src/geometry/contact_generator/contact_generator_workspace.rs b/src/geometry/contact_generator/contact_generator_workspace.rs deleted file mode 100644 index 7aac592..0000000 --- a/src/geometry/contact_generator/contact_generator_workspace.rs +++ /dev/null @@ -1,104 +0,0 @@ -use crate::data::MaybeSerializableData; -#[cfg(feature = "dim3")] -use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace; -use crate::geometry::contact_generator::{ - HeightFieldShapeContactGeneratorWorkspace, TriMeshShapeContactGeneratorWorkspace, - WorkspaceSerializationTag, -}; - -// Note we have this newtype because it simplifies the serialization/deserialization code. -pub struct ContactGeneratorWorkspace(pub Box); - -impl Clone for ContactGeneratorWorkspace { - fn clone(&self) -> Self { - ContactGeneratorWorkspace(self.0.clone_dyn()) - } -} - -impl From for ContactGeneratorWorkspace { - fn from(data: T) -> Self { - Self(Box::new(data) as Box) - } -} - -#[cfg(feature = "serde-serialize")] -impl serde::Serialize for ContactGeneratorWorkspace { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - use crate::serde::ser::SerializeStruct; - - if let Some((tag, ser)) = self.0.as_serialize() { - let mut state = serializer.serialize_struct("ContactGeneratorWorkspace", 2)?; - state.serialize_field("tag", &tag)?; - state.serialize_field("inner", ser)?; - state.end() - } else { - Err(serde::ser::Error::custom( - "Found a non-serializable contact generator workspace.", - )) - } - } -} - -#[cfg(feature = "serde-serialize")] -impl<'de> serde::Deserialize<'de> for ContactGeneratorWorkspace { - fn deserialize(deserializer: D) -> Result - where - D: serde::Deserializer<'de>, - { - struct Visitor {}; - impl<'de> serde::de::Visitor<'de> for Visitor { - type Value = ContactGeneratorWorkspace; - fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { - write!(formatter, "one shape type tag and the inner shape data") - } - - fn visit_seq(self, mut seq: A) -> Result - where - A: serde::de::SeqAccess<'de>, - { - use num::cast::FromPrimitive; - - let tag: u32 = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - - fn deser<'de, A, S: MaybeSerializableData + serde::Deserialize<'de>>( - seq: &mut A, - ) -> Result, A::Error> - where - A: serde::de::SeqAccess<'de>, - { - let workspace: S = seq.next_element()?.ok_or_else(|| { - serde::de::Error::custom("Failed to deserialize builtin workspace.") - })?; - Ok(Box::new(workspace) as Box) - } - - let workspace = match WorkspaceSerializationTag::from_u32(tag) { - Some(WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace) => { - deser::(&mut seq)? - } - Some(WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace) => { - deser::(&mut seq)? - } - #[cfg(feature = "dim3")] - Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => { - deser::(&mut seq)? - } - None => { - return Err(serde::de::Error::custom( - "found invalid contact generator workspace type to deserialize", - )) - } - }; - - Ok(ContactGeneratorWorkspace(workspace)) - } - } - - deserializer.deserialize_struct("ContactGeneratorWorkspace", &["tag", "inner"], Visitor {}) - } -} diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs deleted file mode 100644 index 9f5d856..0000000 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ /dev/null @@ -1,190 +0,0 @@ -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Capsule, ContactManifold, ContactManifoldData, Cuboid, KinematicsCategory}; -use crate::math::Isometry; -use crate::math::Vector; -use buckler::query::sat; -#[cfg(feature = "dim3")] -use buckler::shape::PolyhedronFeature; -#[cfg(feature = "dim2")] -use buckler::shape::{CuboidFeature, CuboidFeatureFace}; - -pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) { - generate_contacts( - ctxt.prediction_distance, - cube1, - ctxt.position1, - capsule2, - ctxt.position2, - ctxt.manifold, - false, - ); - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - } else if let (Some(capsule1), Some(cube2)) = - (ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid()) - { - generate_contacts( - ctxt.prediction_distance, - cube2, - ctxt.position2, - capsule1, - ctxt.position1, - ctxt.manifold, - true, - ); - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - } - ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} - -pub fn generate_contacts<'a>( - prediction_distance: f32, - cube1: &'a Cuboid, - mut pos1: &'a Isometry, - capsule2: &'a Capsule, - mut pos2: &'a Isometry, - manifold: &mut ContactManifold, - swapped: bool, -) { - let mut pos12 = pos1.inverse() * pos2; - let mut pos21 = pos12.inverse(); - - if (!swapped && manifold.try_update_contacts(&pos12)) - || (swapped && manifold.try_update_contacts(&pos21)) - { - return; - } - - let segment2 = capsule2.segment; - - /* - * - * Point-Face cases. - * - */ - let sep1 = - sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, &segment2, &pos12); - if sep1.0 > capsule2.radius + prediction_distance { - manifold.points.clear(); - return; - } - - #[cfg(feature = "dim3")] - let sep2 = (-f32::MAX, Vector::x()); - #[cfg(feature = "dim2")] - let sep2 = sat::segment_cuboid_find_local_separating_normal_oneway(&segment2, cube1, &pos21); - if sep2.0 > capsule2.radius + prediction_distance { - manifold.points.clear(); - return; - } - - /* - * - * Edge-Edge cases. - * - */ - #[cfg(feature = "dim2")] - let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. - #[cfg(feature = "dim3")] - let sep3 = sat::cuboid_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12); - if sep3.0 > capsule2.radius + prediction_distance { - manifold.points.clear(); - return; - } - - /* - * - * Select the best combination of features - * and get the polygons to clip. - * - */ - let mut swapped_reference = false; - let mut best_sep = sep1; - - if sep2.0 > sep1.0 && sep2.0 > sep3.0 { - // The reference shape will be the second shape. - // std::mem::swap(&mut cube1, &mut capsule2); - std::mem::swap(&mut pos1, &mut pos2); - std::mem::swap(&mut pos12, &mut pos21); - best_sep = sep2; - swapped_reference = true; - } else if sep3.0 > sep1.0 { - best_sep = sep3; - } - - let feature1; - let mut feature2; - - #[cfg(feature = "dim2")] - { - if swapped_reference { - feature1 = CuboidFeatureFace::from(segment2); - feature2 = cube1.support_face(pos21 * -best_sep.1); - } else { - feature1 = cube1.support_face(best_sep.1); - feature2 = CuboidFeatureFace::from(segment2); - } - } - #[cfg(feature = "dim3")] - { - if swapped_reference { - feature1 = PolyhedronFeature::from(segment2); - feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1); - } else { - feature1 = cube1.polyhedron_support_face(best_sep.1); - feature2 = PolyhedronFeature::from(segment2); - } - } - - feature2.transform_by(&pos12); - - if swapped ^ swapped_reference { - manifold.swap_identifiers(); - } - - // We do this clone to perform contact tracking and transfer impulses. - // FIXME: find a more efficient way of doing this. - let old_manifold_points = manifold.points.clone(); - manifold.points.clear(); - - #[cfg(feature = "dim2")] - CuboidFeature::face_face_contacts( - prediction_distance + capsule2.radius, - &feature1, - &best_sep.1, - &feature2, - &pos21, - manifold, - ); - #[cfg(feature = "dim3")] - PolyhedronFeature::contacts( - prediction_distance + capsule2.radius, - &feature1, - &best_sep.1, - &feature2, - &pos21, - manifold, - ); - - // Adjust points to take the radius into account. - manifold.local_n1 = best_sep.1; - manifold.local_n2 = pos21 * -best_sep.1; - manifold.kinematics.category = KinematicsCategory::PlanePoint; - manifold.kinematics.radius1 = 0.0; - manifold.kinematics.radius2 = 0.0; - - if swapped_reference { - for point in &mut manifold.points { - point.local_p1 += manifold.local_n1 * capsule2.radius; - point.dist -= capsule2.radius; - } - } else { - for point in &mut manifold.points { - point.local_p2 += manifold.local_n2 * capsule2.radius; - point.dist -= capsule2.radius; - } - } - - // Transfer impulses. - manifold.match_contacts(&old_manifold_points, swapped ^ swapped_reference); -} diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs deleted file mode 100644 index 1d750ef..0000000 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ /dev/null @@ -1,156 +0,0 @@ -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{ContactManifold, ContactManifoldData, KinematicsCategory}; -use crate::math::Isometry; -#[cfg(feature = "dim2")] -use crate::math::Vector; -use buckler::query::sat; -use buckler::shape::{Cuboid, CuboidFeature}; - -pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { - generate_contacts( - ctxt.prediction_distance, - cube1, - ctxt.position1, - cube2, - ctxt.position2, - ctxt.manifold, - ); - } else { - unreachable!() - } - - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} - -pub fn generate_contacts<'a>( - prediction_distance: f32, - mut cube1: &'a Cuboid, - mut pos1: &'a Isometry, - mut cube2: &'a Cuboid, - mut pos2: &'a Isometry, - manifold: &mut ContactManifold, -) { - let mut pos12 = pos1.inverse() * pos2; - let mut pos21 = pos12.inverse(); - - if manifold.try_update_contacts(&pos12) { - return; - } - - /* - * - * Point-Face - * - */ - let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12); - if sep1.0 > prediction_distance { - manifold.points.clear(); - return; - } - - let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21); - if sep2.0 > prediction_distance { - manifold.points.clear(); - return; - } - - /* - * - * Edge-Edge cases - * - */ - #[cfg(feature = "dim2")] - let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. - #[cfg(feature = "dim3")] - let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12); - if sep3.0 > prediction_distance { - manifold.points.clear(); - return; - } - - /* - * - * Select the best combination of features - * and get the polygons to clip. - * - */ - let mut swapped = false; - let mut best_sep = sep1; - - if sep2.0 > sep1.0 && sep2.0 > sep3.0 { - // The reference shape will be the second shape. - std::mem::swap(&mut cube1, &mut cube2); - std::mem::swap(&mut pos1, &mut pos2); - std::mem::swap(&mut pos12, &mut pos21); - manifold.swap_identifiers(); - best_sep = sep2; - swapped = true; - } else if sep3.0 > sep1.0 { - best_sep = sep3; - } - - // We do this clone to perform contact tracking and transfer impulses. - // FIXME: find a more efficient way of doing this. - let old_manifold_points = manifold.points.clone(); - manifold.points.clear(); - - // Now the reference feature is from `cube1` and the best separation is `best_sep`. - // Everything must be expressed in the local-space of `cube1` for contact clipping. - let feature1 = cube1.support_feature(best_sep.1); - let mut feature2 = cube2.support_feature(pos21 * -best_sep.1); - feature2.transform_by(&pos12); - - match (&feature1, &feature2) { - (CuboidFeature::Face(f1), CuboidFeature::Vertex(v2)) => { - CuboidFeature::face_vertex_contacts(f1, &best_sep.1, v2, &pos21, manifold) - } - #[cfg(feature = "dim3")] - (CuboidFeature::Face(f1), CuboidFeature::Edge(e2)) => CuboidFeature::face_edge_contacts( - prediction_distance, - f1, - &best_sep.1, - e2, - &pos21, - manifold, - false, - ), - (CuboidFeature::Face(f1), CuboidFeature::Face(f2)) => CuboidFeature::face_face_contacts( - prediction_distance, - f1, - &best_sep.1, - f2, - &pos21, - manifold, - ), - #[cfg(feature = "dim3")] - (CuboidFeature::Edge(e1), CuboidFeature::Edge(e2)) => { - CuboidFeature::edge_edge_contacts(e1, &best_sep.1, e2, &pos21, manifold) - } - #[cfg(feature = "dim3")] - (CuboidFeature::Edge(e1), CuboidFeature::Face(f2)) => { - // Since f2 is also expressed in the local-space of the first - // feature, the position we provide here is pos21. - CuboidFeature::face_edge_contacts( - prediction_distance, - f2, - &-best_sep.1, - e1, - &pos21, - manifold, - true, - ) - } - _ => unreachable!(), // The other cases are not possible. - } - - manifold.local_n1 = best_sep.1; - manifold.local_n2 = pos21 * -best_sep.1; - manifold.kinematics.category = KinematicsCategory::PlanePoint; - manifold.kinematics.radius1 = 0.0; - manifold.kinematics.radius2 = 0.0; - - // Transfer impulses. - manifold.match_contacts(&old_manifold_points, swapped); -} diff --git a/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs b/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs deleted file mode 100644 index 8b13789..0000000 --- a/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs +++ /dev/null @@ -1 +0,0 @@ - diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs deleted file mode 100644 index 06dc6f0..0000000 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ /dev/null @@ -1,171 +0,0 @@ -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{ContactManifold, ContactManifoldData, Cuboid, KinematicsCategory, Triangle}; -use crate::math::Isometry; -#[cfg(feature = "dim2")] -use crate::{buckler::shape::CuboidFeature, geometry::triangle, math::Vector}; -use buckler::query::sat; -#[cfg(feature = "dim3")] -use buckler::shape::PolyhedronFeature; - -pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { - generate_contacts( - ctxt.prediction_distance, - cube1, - ctxt.position1, - triangle2, - ctxt.position2, - ctxt.manifold, - false, - ); - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - } else if let (Some(triangle1), Some(cube2)) = - (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) - { - generate_contacts( - ctxt.prediction_distance, - cube2, - ctxt.position2, - triangle1, - ctxt.position1, - ctxt.manifold, - true, - ); - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - } - ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} - -pub fn generate_contacts<'a>( - prediction_distance: f32, - cube1: &'a Cuboid, - mut pos1: &'a Isometry, - triangle2: &'a Triangle, - mut pos2: &'a Isometry, - manifold: &mut ContactManifold, - swapped: bool, -) { - let mut pos12 = pos1.inverse() * pos2; - let mut pos21 = pos12.inverse(); - - if (!swapped && manifold.try_update_contacts(&pos12)) - || (swapped && manifold.try_update_contacts(&pos21)) - { - return; - } - - /* - * - * Point-Face cases. - * - */ - let sep1 = - sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12); - if sep1.0 > prediction_distance { - manifold.points.clear(); - return; - } - - let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cube1, &pos21); - if sep2.0 > prediction_distance { - manifold.points.clear(); - return; - } - - /* - * - * Edge-Edge cases. - * - */ - #[cfg(feature = "dim2")] - let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. - #[cfg(feature = "dim3")] - let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12); - if sep3.0 > prediction_distance { - manifold.points.clear(); - return; - } - - /* - * - * Select the best combination of features - * and get the polygons to clip. - * - */ - let mut swapped_reference = false; - let mut best_sep = sep1; - - if sep2.0 > sep1.0 && sep2.0 > sep3.0 { - // The reference shape will be the second shape. - // std::mem::swap(&mut cube1, &mut triangle2); - std::mem::swap(&mut pos1, &mut pos2); - std::mem::swap(&mut pos12, &mut pos21); - best_sep = sep2; - swapped_reference = true; - } else if sep3.0 > sep1.0 { - best_sep = sep3; - } - - let feature1; - let mut feature2; - - #[cfg(feature = "dim2")] - { - if swapped_reference { - feature1 = triangle::support_face(triangle2, best_sep.1); - feature2 = cube1.support_face(pos21 * -best_sep.1); - } else { - feature1 = cube1.support_face(best_sep.1); - feature2 = triangle::support_face(triangle2, pos21 * -best_sep.1); - } - } - #[cfg(feature = "dim3")] - { - if swapped_reference { - feature1 = PolyhedronFeature::from(*triangle2); - feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1); - } else { - feature1 = cube1.polyhedron_support_face(best_sep.1); - feature2 = PolyhedronFeature::from(*triangle2); - } - } - - feature2.transform_by(&pos12); - - if swapped ^ swapped_reference { - manifold.swap_identifiers(); - } - - // We do this clone to perform contact tracking and transfer impulses. - // FIXME: find a more efficient way of doing this. - let old_manifold_points = manifold.points.clone(); - manifold.points.clear(); - - #[cfg(feature = "dim2")] - CuboidFeature::face_face_contacts( - prediction_distance, - &feature1, - &best_sep.1, - &feature2, - &pos21, - manifold, - ); - #[cfg(feature = "dim3")] - PolyhedronFeature::contacts( - prediction_distance, - &feature1, - &best_sep.1, - &feature2, - &pos21, - manifold, - ); - - manifold.local_n1 = best_sep.1; - manifold.local_n2 = pos21 * -best_sep.1; - manifold.kinematics.category = KinematicsCategory::PlanePoint; - manifold.kinematics.radius1 = 0.0; - manifold.kinematics.radius2 = 0.0; - - // Transfer impulses. - manifold.match_contacts(&old_manifold_points, swapped ^ swapped_reference); -} diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs deleted file mode 100644 index 125ac34..0000000 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ /dev/null @@ -1,190 +0,0 @@ -use crate::buckler::bounding_volume::BoundingVolume; -use crate::data::hashmap::{Entry, HashMap}; -use crate::data::MaybeSerializableData; -use crate::geometry::contact_generator::{ - ContactGenerationContext, ContactGeneratorWorkspace, PrimitiveContactGenerationContext, - PrimitiveContactGenerator, -}; -use crate::geometry::{Collider, ContactManifold, ContactManifoldData}; -#[cfg(feature = "dim2")] -use buckler::shape::Capsule; -use buckler::shape::{HeightField, Shape}; -#[cfg(feature = "serde-serialize")] -use erased_serde::Serialize; - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone)] -struct SubDetector { - #[cfg_attr(feature = "serde-serialize", serde(skip))] - generator: Option, - manifold_id: usize, - timestamp: bool, - workspace: Option, -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone)] -pub struct HeightFieldShapeContactGeneratorWorkspace { - timestamp: bool, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - old_manifolds: Vec, - sub_detectors: HashMap, -} - -impl HeightFieldShapeContactGeneratorWorkspace { - pub fn new() -> Self { - Self { - timestamp: false, - old_manifolds: Vec::new(), - sub_detectors: HashMap::default(), - } - } -} - -pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext) { - let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; - let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - - if let Some(heightfield1) = collider1.shape().as_heightfield() { - do_generate_contacts(heightfield1, collider1, collider2, ctxt, false) - } else if let Some(heightfield2) = collider2.shape().as_heightfield() { - do_generate_contacts(heightfield2, collider2, collider1, ctxt, true) - } -} - -fn do_generate_contacts( - heightfield1: &HeightField, - collider1: &Collider, - collider2: &Collider, - ctxt: &mut ContactGenerationContext, - _flipped: bool, -) { - let workspace: &mut HeightFieldShapeContactGeneratorWorkspace = ctxt - .pair - .generator_workspace - .as_mut() - .expect("The HeightFieldShapeContactGeneratorWorkspace is missing.") - .0 - .downcast_mut() - .expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace."); - let new_timestamp = !workspace.timestamp; - workspace.timestamp = new_timestamp; - - /* - * Compute interferences. - */ - let pos12 = collider1.position.inverse() * collider2.position; - // TODO: somehow precompute the AABB and reuse it? - let ls_aabb2 = collider2 - .shape() - .compute_aabb(&pos12) - .loosened(ctxt.prediction_distance); - - std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds); - ctxt.pair.manifolds.clear(); - let coll_pair = ctxt.pair.pair; - let manifolds = &mut ctxt.pair.manifolds; - let prediction_distance = ctxt.prediction_distance; - let dispatcher = ctxt.dispatcher; - let solver_flags = ctxt.solver_flags; - let shape_type2 = collider2.shape().shape_type(); - - heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1| { - let position1 = collider1.position(); - #[cfg(feature = "dim2")] - let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead. - #[cfg(feature = "dim3")] - let sub_shape1 = *part1; - - let sub_detector = match workspace.sub_detectors.entry(i as usize) { - Entry::Occupied(entry) => { - let sub_detector = entry.into_mut(); - let manifold = workspace.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 (generator, workspace2) = - dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2); - let sub_detector = SubDetector { - generator: Some(generator), - manifold_id: manifolds.len(), - timestamp: new_timestamp, - workspace: workspace2, - }; - let manifold_data = ContactManifoldData::from_colliders( - coll_pair, - collider1, - collider2, - solver_flags, - ); - manifolds.push(ContactManifold::with_data((i as usize, 0), manifold_data)); - - entry.insert(sub_detector) - } - }; - - if sub_detector.generator.is_none() { - // We probably lost the generator after deserialization. - // So we need to dispatch again. - let (generator, workspace2) = - dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2); - sub_detector.generator = Some(generator); - - // Don't overwrite the workspace if we already deserialized one. - if sub_detector.workspace.is_none() { - sub_detector.workspace = workspace2; - } - } - - let manifold = &mut manifolds[sub_detector.manifold_id]; - - let mut ctxt2 = if coll_pair.collider1 != manifold.data.pair.collider1 { - PrimitiveContactGenerationContext { - prediction_distance, - collider1: collider2, - collider2: collider1, - shape1: collider2.shape(), - shape2: &sub_shape1, - position1: collider2.position(), - position2: position1, - manifold, - workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0), - } - } else { - PrimitiveContactGenerationContext { - prediction_distance, - collider1, - collider2, - shape1: &sub_shape1, - shape2: collider2.shape(), - position1, - position2: collider2.position(), - manifold, - workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0), - } - }; - - (sub_detector.generator.unwrap().generate_contacts)(&mut ctxt2) - }); - - workspace - .sub_detectors - .retain(|_, detector| detector.timestamp == new_timestamp) -} - -impl MaybeSerializableData for HeightFieldShapeContactGeneratorWorkspace { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> { - Some(( - super::WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace as u32, - self, - )) - } - - fn clone_dyn(&self) -> Box { - Box::new(self.clone()) - } -} diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs deleted file mode 100644 index 4c0716a..0000000 --- a/src/geometry/contact_generator/mod.rs +++ /dev/null @@ -1,51 +0,0 @@ -pub use self::ball_ball_contact_generator::generate_contacts_ball_ball; -#[cfg(feature = "simd-is-enabled")] -pub use self::ball_ball_contact_generator::generate_contacts_ball_ball_simd; -pub use self::ball_convex_contact_generator::generate_contacts_ball_convex; -pub use self::capsule_capsule_contact_generator::generate_contacts_capsule_capsule; -pub use self::contact_dispatcher::{ContactDispatcher, DefaultContactDispatcher}; -pub use self::contact_generator::{ - ContactGenerationContext, ContactGenerator, ContactPhase, PrimitiveContactGenerationContext, - PrimitiveContactGenerator, -}; -#[cfg(feature = "simd-is-enabled")] -pub use self::contact_generator::{ - ContactGenerationContextSimd, PrimitiveContactGenerationContextSimd, -}; -pub use self::cuboid_capsule_contact_generator::generate_contacts_cuboid_capsule; -pub use self::cuboid_cuboid_contact_generator::generate_contacts_cuboid_cuboid; -pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triangle; -pub use self::heightfield_shape_contact_generator::{ - generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace, -}; -#[cfg(feature = "dim3")] -pub use self::pfm_pfm_contact_generator::{ - generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace, -}; -// pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; -pub use self::contact_generator_workspace::ContactGeneratorWorkspace; -pub use self::trimesh_shape_contact_generator::{ - generate_contacts_trimesh_shape, TriMeshShapeContactGeneratorWorkspace, -}; - -pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag; - -mod ball_ball_contact_generator; -mod ball_convex_contact_generator; -mod ball_polygon_contact_generator; -mod capsule_capsule_contact_generator; -mod contact_dispatcher; -mod contact_generator; -mod contact_generator_workspace; -mod cuboid_capsule_contact_generator; -mod cuboid_cuboid_contact_generator; -mod cuboid_polygon_contact_generator; -mod cuboid_triangle_contact_generator; -mod heightfield_shape_contact_generator; -#[cfg(feature = "dim3")] -mod pfm_pfm_contact_generator; -mod polygon_polygon_contact_generator; -mod serializable_workspace_tag; -mod trimesh_shape_contact_generator; - -use crate::geometry::{Contact, ContactManifold}; diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs deleted file mode 100644 index 721d5dc..0000000 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ /dev/null @@ -1,146 +0,0 @@ -use crate::data::MaybeSerializableData; -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{ContactManifoldData, KinematicsCategory}; -use crate::math::{Isometry, Vector}; -use buckler::query::{ - self, - gjk::{GJKResult, VoronoiSimplex}, -}; -use buckler::shape::{PolygonalFeatureMap, PolyhedronFeature}; -#[cfg(feature = "serde-serialize")] -use erased_serde::Serialize; -use na::Unit; - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone)] -pub struct PfmPfmContactManifoldGeneratorWorkspace { - #[cfg_attr( - feature = "serde-serialize", - serde(skip, default = "VoronoiSimplex::new") - )] - simplex: VoronoiSimplex, - last_gjk_dir: Option>>, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - feature1: PolyhedronFeature, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - feature2: PolyhedronFeature, -} - -impl Default for PfmPfmContactManifoldGeneratorWorkspace { - fn default() -> Self { - Self { - simplex: VoronoiSimplex::new(), - last_gjk_dir: None, - feature1: PolyhedronFeature::new(), - feature2: PolyhedronFeature::new(), - } - } -} - -pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Some((pfm1, border_radius1)), Some((pfm2, border_radius2))) = ( - ctxt.shape1.as_polygonal_feature_map(), - ctxt.shape2.as_polygonal_feature_map(), - ) { - do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt); - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - ctxt.manifold.sort_contacts(ctxt.prediction_distance); - } -} - -fn do_generate_contacts( - pfm1: &dyn PolygonalFeatureMap, - border_radius1: f32, - pfm2: &dyn PolygonalFeatureMap, - border_radius2: f32, - ctxt: &mut PrimitiveContactGenerationContext, -) { - let pos12 = ctxt.position1.inverse() * ctxt.position2; - let pos21 = pos12.inverse(); - - // We use very small thresholds for the manifold update because something to high would - // cause numerical drifts with the effect of introducing bumps in - // what should have been smooth rolling motions. - if ctxt - .manifold - .try_update_contacts_eps(&pos12, crate::utils::COS_1_DEGREES, 1.0e-6) - { - return; - } - - let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt - .workspace - .as_mut() - .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.") - .downcast_mut() - .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); - - let total_prediction = ctxt.prediction_distance + border_radius1 + border_radius2; - let contact = query::contact::contact_support_map_support_map_with_params( - &pos12, - pfm1, - pfm2, - total_prediction, - &mut workspace.simplex, - workspace.last_gjk_dir, - ); - - let old_manifold_points = ctxt.manifold.points.clone(); - ctxt.manifold.points.clear(); - - match contact { - GJKResult::ClosestPoints(_, _, dir) => { - workspace.last_gjk_dir = Some(dir); - let normal1 = dir; - let normal2 = pos21 * -dir; - pfm1.local_support_feature(&normal1, &mut workspace.feature1); - pfm2.local_support_feature(&normal2, &mut workspace.feature2); - workspace.feature2.transform_by(&pos12); - - PolyhedronFeature::contacts( - total_prediction, - &workspace.feature1, - &normal1, - &workspace.feature2, - &pos21, - ctxt.manifold, - ); - - if border_radius1 != 0.0 || border_radius2 != 0.0 { - for contact in &mut ctxt.manifold.points { - contact.local_p1 += *normal1 * border_radius1; - contact.local_p2 += *normal2 * border_radius2; - contact.dist -= border_radius1 + border_radius2; - } - } - - // Adjust points to take the radius into account. - ctxt.manifold.local_n1 = *normal1; - ctxt.manifold.local_n2 = *normal2; - ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // TODO: is this the more appropriate? - ctxt.manifold.kinematics.radius1 = 0.0; - ctxt.manifold.kinematics.radius2 = 0.0; - } - GJKResult::NoIntersection(dir) => { - workspace.last_gjk_dir = Some(dir); - } - _ => {} - } - - // Transfer impulses. - ctxt.manifold.match_contacts(&old_manifold_points, false); -} - -impl MaybeSerializableData for PfmPfmContactManifoldGeneratorWorkspace { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> { - Some(( - super::WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace as u32, - self, - )) - } - - fn clone_dyn(&self) -> Box { - Box::new(self.clone()) - } -} diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs deleted file mode 100644 index 7e80fe7..0000000 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ /dev/null @@ -1,156 +0,0 @@ -#![allow(dead_code)] // TODO: remove this once we support polygons. - -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{ - sat, Contact, ContactData, ContactManifold, ContactManifoldData, KinematicsCategory, Polygon, -}; -use crate::math::{Isometry, Point}; -#[cfg(feature = "dim2")] -use crate::{math::Vector, utils}; -use buckler::query; - -pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationContext) { - unimplemented!() - // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { - // generate_contacts( - // polygon1, - // &ctxt.position1, - // polygon2, - // &ctxt.position2, - // ctxt.manifold, - // ); - // ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - // } else { - // unreachable!() - // } - // - // ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} - -fn generate_contacts<'a>( - mut p1: &'a Polygon, - mut m1: &'a Isometry, - mut p2: &'a Polygon, - mut m2: &'a Isometry, - manifold: &'a mut ContactManifold, -) { - let mut m12 = m1.inverse() * m2; - let mut m21 = m12.inverse(); - - if manifold.try_update_contacts(&m12) { - return; - } - - let mut sep1 = sat::polygon_polygon_compute_separation_features(p1, p2, &m12); - if sep1.0 > 0.0 { - manifold.points.clear(); - return; - } - - let mut sep2 = sat::polygon_polygon_compute_separation_features(p2, p1, &m21); - if sep2.0 > 0.0 { - manifold.points.clear(); - return; - } - - let mut swapped = false; - if sep2.0 > sep1.0 { - std::mem::swap(&mut sep1, &mut sep2); - std::mem::swap(&mut m1, &mut m2); - std::mem::swap(&mut p1, &mut p2); - std::mem::swap(&mut m12, &mut m21); - manifold.swap_identifiers(); - swapped = true; - } - - let support_face1 = sep1.1; - let local_n1 = p1.normals[support_face1]; - let local_n2 = m21 * -local_n1; - let support_face2 = p2.support_face(&local_n2); - let len1 = p1.vertices.len(); - let len2 = p2.vertices.len(); - - let seg1 = ( - p1.vertices[support_face1], - p1.vertices[(support_face1 + 1) % len1], - ); - let seg2 = ( - m12 * p2.vertices[support_face2], - m12 * p2.vertices[(support_face2 + 1) % len2], - ); - if let Some((clip_a, clip_b)) = query::details::clip_segment_segment(seg1, seg2) { - let dist_a = (clip_a.1 - clip_a.0).dot(&local_n1); - let dist_b = (clip_b.1 - clip_b.0).dot(&local_n1); - - let mut data_a = ContactData::default(); - let mut data_b = ContactData::default(); - - let fids_a = ( - ((support_face1 * 2 + clip_a.2) % (len1 * 2)) as u8, - ((support_face2 * 2 + clip_a.3) % (len2 * 2)) as u8, - ); - - let fids_b = ( - ((support_face1 * 2 + clip_b.2) % (len1 * 2)) as u8, - ((support_face2 * 2 + clip_b.3) % (len2 * 2)) as u8, - ); - - if manifold.points.len() != 0 { - assert_eq!(manifold.points.len(), 2); - - // We already had 2 points in the previous iteration. - // Match the features to see if we keep the cached impulse. - let original_fids_a; - let original_fids_b; - - // NOTE: the previous manifold may have its bodies swapped wrt. our new manifold. - // So we have to adjust accordingly the features we will be comparing. - if swapped { - original_fids_a = (manifold.points[0].fid1, manifold.points[0].fid2); - original_fids_b = (manifold.points[1].fid1, manifold.points[1].fid2); - } else { - original_fids_a = (manifold.points[0].fid2, manifold.points[0].fid1); - original_fids_b = (manifold.points[1].fid2, manifold.points[1].fid1); - } - - if fids_a == original_fids_a { - data_a = manifold.points[0].data; - } else if fids_a == original_fids_b { - data_a = manifold.points[1].data; - } - - if fids_b == original_fids_a { - data_b = manifold.points[0].data; - } else if fids_b == original_fids_b { - data_b = manifold.points[1].data; - } - } - - manifold.points.clear(); - manifold.points.push(Contact { - local_p1: clip_a.0, - local_p2: m21 * clip_a.1, - fid1: fids_a.0, - fid2: fids_a.1, - dist: dist_a, - data: data_a, - }); - - manifold.points.push(Contact { - local_p1: clip_b.0, - local_p2: m21 * clip_b.1, - fid1: fids_b.0, - fid2: fids_b.1, - dist: dist_b, - data: data_b, - }); - - manifold.local_n1 = local_n1; - manifold.local_n2 = local_n2; - manifold.kinematics.category = KinematicsCategory::PlanePoint; - manifold.kinematics.radius1 = 0.0; - manifold.kinematics.radius2 = 0.0; - } else { - manifold.points.clear(); - } -} diff --git a/src/geometry/contact_generator/serializable_workspace_tag.rs b/src/geometry/contact_generator/serializable_workspace_tag.rs deleted file mode 100644 index 4bdb902..0000000 --- a/src/geometry/contact_generator/serializable_workspace_tag.rs +++ /dev/null @@ -1,9 +0,0 @@ -use num_derive::FromPrimitive; - -#[derive(Copy, Clone, Debug, FromPrimitive)] -pub(super) enum WorkspaceSerializationTag { - TriMeshShapeContactGeneratorWorkspace = 0, - #[cfg(feature = "dim3")] - PfmPfmContactGeneratorWorkspace, - HeightfieldShapeContactGeneratorWorkspace, -} diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs deleted file mode 100644 index 209ac42..0000000 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ /dev/null @@ -1,221 +0,0 @@ -use crate::buckler::bounding_volume::{BoundingVolume, AABB}; -use crate::data::MaybeSerializableData; -use crate::geometry::contact_generator::{ - ContactGenerationContext, PrimitiveContactGenerationContext, -}; -use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, TriMesh}; -#[cfg(feature = "serde-serialize")] -use erased_serde::Serialize; - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone)] -pub struct TriMeshShapeContactGeneratorWorkspace { - interferences: Vec, - local_aabb2: AABB, - old_interferences: Vec, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - old_manifolds: Vec, -} - -impl TriMeshShapeContactGeneratorWorkspace { - pub fn new() -> Self { - Self { - interferences: Vec::new(), - local_aabb2: AABB::new_invalid(), - old_interferences: Vec::new(), - old_manifolds: Vec::new(), - } - } -} - -pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) { - let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; - let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - - if let Some(trimesh1) = collider1.shape().as_trimesh() { - do_generate_contacts(trimesh1, collider1, collider2, ctxt, false) - } else if let Some(trimesh2) = collider2.shape().as_trimesh() { - do_generate_contacts(trimesh2, collider2, collider1, ctxt, true) - } -} - -fn do_generate_contacts( - trimesh1: &TriMesh, - collider1: &Collider, - collider2: &Collider, - ctxt: &mut ContactGenerationContext, - flipped: bool, -) { - let ctxt_pair_pair = if flipped { - ctxt.pair.pair.swap() - } else { - ctxt.pair.pair - }; - - let workspace: &mut TriMeshShapeContactGeneratorWorkspace = ctxt - .pair - .generator_workspace - .as_mut() - .expect("The TriMeshShapeContactGeneratorWorkspace is missing.") - .0 - .downcast_mut() - .expect("Invalid workspace type, expected a TriMeshShapeContactGeneratorWorkspace."); - - /* - * Compute interferences. - */ - let pos12 = collider1.position.inverse() * collider2.position; - // TODO: somehow precompute the AABB and reuse it? - let mut new_local_aabb2 = collider2 - .shape() - .compute_aabb(&pos12) - .loosened(ctxt.prediction_distance); - let same_local_aabb2 = workspace.local_aabb2.contains(&new_local_aabb2); - - if !same_local_aabb2 { - let extra_margin = - (new_local_aabb2.maxs - new_local_aabb2.mins).map(|e| (e / 10.0).min(0.1)); - new_local_aabb2.mins -= extra_margin; - new_local_aabb2.maxs += extra_margin; - - let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value? - std::mem::swap( - &mut workspace.old_interferences, - &mut workspace.interferences, - ); - std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds); - ctxt.pair.manifolds.clear(); - - if workspace.old_interferences.is_empty() && !workspace.old_manifolds.is_empty() { - // This happens if for some reasons the contact generator context was lost - // and rebuilt. In this case, we hate to reconstruct the `old_interferences` - // array using the subshape ids from the contact manifolds. - // TODO: always rely on the subshape ids instead of maintaining `.old_interferences` ? - let ctxt_collider1 = ctxt_pair_pair.collider1; - workspace.old_interferences = workspace - .old_manifolds - .iter() - .map(|manifold| { - if manifold.data.pair.collider1 == ctxt_collider1 { - manifold.subshape_index_pair.0 as u32 - } else { - manifold.subshape_index_pair.1 as u32 - } - }) - .collect(); - } - - // This assertion may fire due to the invalid triangle_ids that the - // near-phase may return (due to SIMD sentinels). - // - // assert_eq!( - // workspace - // .old_interferences - // .len() - // .min(trimesh1.num_triangles()), - // workspace.old_manifolds.len() - // ); - - workspace.interferences.clear(); - trimesh1 - .quadtree() - .intersect_aabb(&local_aabb2, &mut workspace.interferences); - workspace.local_aabb2 = local_aabb2; - } - - /* - * Dispatch to the specific solver by keeping the previous manifold if we already had one. - */ - let new_interferences = &workspace.interferences; - let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); - let mut old_manifolds_it = workspace.old_manifolds.drain(..); - let shape_type2 = collider2.shape().shape_type(); - - // TODO: don't redispatch at each frame (we should probably do the same as - // the heightfield). - for (i, triangle_id) in new_interferences.iter().enumerate() { - if *triangle_id >= trimesh1.num_triangles() as u32 { - // Because of SIMD padding, the broad-phase may return tiangle indices greater - // than the max. - continue; - } - - if !same_local_aabb2 { - loop { - match old_inter_it.peek() { - Some(old_triangle_id) if *old_triangle_id < *triangle_id => { - old_inter_it.next(); - old_manifolds_it.next(); - } - _ => break, - } - } - - let manifold = if old_inter_it.peek() != Some(triangle_id) { - // We don't have a manifold for this triangle yet. - let data = ContactManifoldData::from_colliders( - ctxt_pair_pair, - collider1, - collider2, - ctxt.solver_flags, - ); - - ContactManifold::with_data((*triangle_id as usize, 0), data) - } else { - // We already have a manifold for this triangle. - old_inter_it.next(); - old_manifolds_it.next().unwrap() - }; - - ctxt.pair.manifolds.push(manifold); - } - - let manifold = &mut ctxt.pair.manifolds[i]; - let triangle1 = trimesh1.triangle(*triangle_id); - let (generator, mut workspace2) = ctxt - .dispatcher - .dispatch_primitives(ShapeType::Triangle, shape_type2); - - let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.data.pair.collider1 { - PrimitiveContactGenerationContext { - prediction_distance: ctxt.prediction_distance, - collider1: collider2, - collider2: collider1, - shape1: collider2.shape(), - shape2: &triangle1, - position1: collider2.position(), - position2: collider1.position(), - manifold, - workspace: workspace2.as_mut().map(|w| &mut *w.0), - } - } else { - PrimitiveContactGenerationContext { - prediction_distance: ctxt.prediction_distance, - collider1, - collider2, - shape1: &triangle1, - shape2: collider2.shape(), - position1: collider1.position(), - position2: collider2.position(), - manifold, - workspace: workspace2.as_mut().map(|w| &mut *w.0), - } - }; - - (generator.generate_contacts)(&mut ctxt2); - } -} - -impl MaybeSerializableData for TriMeshShapeContactGeneratorWorkspace { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> { - Some(( - super::WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace as u32, - self, - )) - } - - fn clone_dyn(&self) -> Box { - Box::new(self.clone()) - } -} diff --git a/src/geometry/contact_generator/voxels_shape_contact_generator.rs b/src/geometry/contact_generator/voxels_shape_contact_generator.rs deleted file mode 100644 index e69de29..0000000 diff --git a/src/geometry/contact.rs b/src/geometry/contact_pair.rs similarity index 90% rename from src/geometry/contact.rs rename to src/geometry/contact_pair.rs index 14646d9..ba5da2a 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact_pair.rs @@ -1,9 +1,8 @@ -use crate::buckler::query::TrackedData; -use crate::data::MaybeSerializableData; use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet}; -use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase}; use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold}; use crate::math::{Isometry, Point, Vector}; +use eagl::query::ContactManifoldsWorkspace; +use eagl::utils::MaybeSerializableData; #[cfg(feature = "simd-is-enabled")] use { crate::math::{SimdReal, SIMD_WIDTH}, @@ -96,22 +95,15 @@ pub struct ContactPair { /// /// All contact manifold contain themselves contact points between the colliders. pub manifolds: Vec, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) generator: Option, - pub(crate) generator_workspace: Option, + pub(crate) workspace: Option, } impl ContactPair { - pub(crate) fn new( - pair: ColliderPair, - generator: ContactPhase, - generator_workspace: Option, - ) -> Self { + pub(crate) fn new(pair: ColliderPair) -> Self { Self { pair, manifolds: Vec::new(), - generator: Some(generator), - generator_workspace, + workspace: None, } } @@ -156,14 +148,14 @@ impl ContactPair { coll1, coll2, manifold, - self.generator_workspace.as_mut().map(|w| &mut *w.0), + self.workspace.as_mut().map(|w| &mut *w.0), ) } else { ( coll2, coll1, manifold, - self.generator_workspace.as_mut().map(|w| &mut *w.0), + self.workspace.as_mut().map(|w| &mut *w.0), ) } } @@ -218,14 +210,6 @@ impl Default for ContactManifoldData { } } -impl TrackedData for ContactManifoldData { - fn flip(&mut self) { - std::mem::swap(&mut self.pair.collider1, &mut self.pair.collider2); - std::mem::swap(&mut self.body_pair.body1, &mut self.body_pair.body2); - std::mem::swap(&mut self.delta1, &mut self.delta2); - } -} - impl ContactManifoldData { pub(crate) fn new( pair: ColliderPair, @@ -281,7 +265,7 @@ impl ContactManifoldData { // This coefficient increases exponentially over time, until it reaches 1.0. // This will reduce significant overshoot at the timesteps that // follow a timestep involving high-velocity impacts. - 0.01 + 1.0 // 0.01 } pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) { diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 46fe360..9f32a7f 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -3,49 +3,49 @@ pub use self::broad_phase_multi_sap::BroadPhase; pub use self::collider::{Collider, ColliderBuilder, ColliderShape}; pub use self::collider_set::{ColliderHandle, ColliderSet}; -pub use self::contact::{ContactData, ContactManifoldData}; -pub use self::contact::{ContactPair, SolverFlags}; -pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher}; +// pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher}; +pub use self::contact_pair::{ContactData, ContactManifoldData}; +pub use self::contact_pair::{ContactPair, SolverFlags}; pub use self::interaction_graph::{ ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex, }; pub use self::narrow_phase::NarrowPhase; pub use self::polygon::Polygon; -pub use self::proximity::ProximityPair; pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; +pub use self::proximity_pair::ProximityPair; pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; -pub use buckler::query::Proximity; +pub use eagl::query::Proximity; -pub use buckler::query::{KinematicsCategory, TrackedContact}; +pub use eagl::query::{KinematicsCategory, TrackedContact}; -pub type Contact = buckler::query::TrackedContact; -pub type ContactManifold = buckler::query::ContactManifold; +pub type Contact = eagl::query::TrackedContact; +pub type ContactManifold = eagl::query::ContactManifold; /// A segment shape. -pub type Segment = buckler::shape::Segment; +pub type Segment = eagl::shape::Segment; /// A cuboid shape. -pub type Cuboid = buckler::shape::Cuboid; +pub type Cuboid = eagl::shape::Cuboid; /// A triangle shape. -pub type Triangle = buckler::shape::Triangle; +pub type Triangle = eagl::shape::Triangle; /// A ball shape. -pub type Ball = buckler::shape::Ball; +pub type Ball = eagl::shape::Ball; /// A capsule shape. -pub type Capsule = buckler::shape::Capsule; +pub type Capsule = eagl::shape::Capsule; /// A heightfield shape. -pub type HeightField = buckler::shape::HeightField; +pub type HeightField = eagl::shape::HeightField; /// A cylindrical shape. #[cfg(feature = "dim3")] -pub type Cylinder = buckler::shape::Cylinder; +pub type Cylinder = eagl::shape::Cylinder; /// A cone shape. #[cfg(feature = "dim3")] -pub type Cone = buckler::shape::Cone; +pub type Cone = eagl::shape::Cone; /// An axis-aligned bounding box. -pub type AABB = buckler::bounding_volume::AABB; +pub type AABB = eagl::bounding_volume::AABB; /// A ray that can be cast against colliders. -pub type Ray = buckler::query::Ray; +pub type Ray = eagl::query::Ray; /// The intersection between a ray and a collider. -pub type RayIntersection = buckler::query::RayIntersection; +pub type RayIntersection = eagl::query::RayIntersection; /// The the projection of a point on a collider. -pub type PointProjection = buckler::query::PointProjection; +pub type PointProjection = eagl::query::PointProjection; #[derive(Copy, Clone, Hash, Debug)] /// Events occurring when two collision objects start or stop being in contact (or penetration). @@ -101,26 +101,25 @@ pub(crate) use self::ball::WBall; pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] -pub(crate) use self::contact::WContact; +pub(crate) use self::contact_pair::WContact; pub(crate) use self::narrow_phase::ContactManifoldIndex; -pub(crate) use buckler::partitioning::WQuadtree; +pub(crate) use eagl::partitioning::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; pub use self::interaction_groups::InteractionGroups; -pub use buckler::shape::*; +pub use eagl::shape::*; mod ball; mod broad_phase_multi_sap; mod collider; mod collider_set; -mod contact; -mod contact_generator; +// mod contact_generator; +mod contact_pair; mod interaction_graph; mod narrow_phase; mod polygon; -mod proximity; mod proximity_detector; +mod proximity_pair; pub(crate) mod sat; -pub(crate) mod triangle; //mod z_order; mod interaction_groups; mod user_callbacks; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index ad2d514..240ed31 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -2,28 +2,26 @@ use rayon::prelude::*; use crate::dynamics::RigidBodySet; -use crate::geometry::contact_generator::{ - ContactDispatcher, ContactGenerationContext, DefaultContactDispatcher, -}; use crate::geometry::proximity_detector::{ DefaultProximityDispatcher, ProximityDetectionContext, ProximityDispatcher, }; +use eagl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; //#[cfg(feature = "simd-is-enabled")] //use crate::geometry::{ // contact_generator::ContactGenerationContextSimd, // proximity_detector::ProximityDetectionContextSimd, WBall, //}; use crate::geometry::{ - BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactPairFilter, - PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, RemovedCollider, - SolverFlags, + BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactManifoldData, + ContactPairFilter, PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, + RemovedCollider, SolverFlags, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] //use crate::math::{SimdReal, SIMD_WIDTH}; -use crate::buckler::query::Proximity; use crate::data::pubsub::Subscription; use crate::data::Coarena; +use crate::eagl::query::Proximity; use crate::pipeline::EventHandler; use std::collections::HashMap; //use simba::simd::SimdValue; @@ -324,10 +322,7 @@ impl NarrowPhase { .find_edge(gid1.contact_graph_index, gid2.contact_graph_index) .is_none() { - let dispatcher = DefaultContactDispatcher; - let generator = dispatcher - .dispatch(co1.shape().shape_type(), co2.shape().shape_type()); - let interaction = ContactPair::new(*pair, generator.0, generator.1); + let interaction = ContactPair::new(*pair); let _ = self.contact_graph.add_edge( gid1.contact_graph_index, gid2.contact_graph_index, @@ -523,33 +518,22 @@ impl NarrowPhase { solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); } - let dispatcher = DefaultContactDispatcher; - if pair.generator.is_none() { - // We need a redispatch for this generator. - // This can happen, e.g., after restoring a snapshot of the narrow-phase. - let (generator, workspace) = - dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type()); - pair.generator = Some(generator); - - // Keep the workspace if one already exists. - if pair.generator_workspace.is_none() { - pair.generator_workspace = workspace; - } - } - - let context = ContactGenerationContext { - dispatcher: &dispatcher, + let dispatcher = DefaultQueryDispatcher; + let pos12 = co1.position().inverse() * co2.position(); + dispatcher.contact_manifolds( + &pos12, + co1.shape(), + co2.shape(), prediction_distance, - colliders, - pair, - solver_flags, - }; + &mut pair.manifolds, + &mut pair.workspace, + ); - context - .pair - .generator - .unwrap() - .generate_contacts(context, events); + // TODO: don't write this everytime? + for manifold in &mut pair.manifolds { + manifold.data = + ContactManifoldData::from_colliders(pair.pair, co1, co2, solver_flags); + } }); } diff --git a/src/geometry/polygon.rs b/src/geometry/polygon.rs index 0bc3e56..15cf005 100644 --- a/src/geometry/polygon.rs +++ b/src/geometry/polygon.rs @@ -1,7 +1,7 @@ #![allow(dead_code)] // TODO: remove this once we support polygons. use crate::math::{Isometry, Point, Vector}; -use buckler::bounding_volume::AABB; +use eagl::bounding_volume::AABB; #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs index 5c1a105..19ead5c 100644 --- a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs @@ -1,7 +1,7 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; use crate::geometry::{Ball, Proximity}; use crate::math::Isometry; -use buckler::query::PointQuery; +use eagl::query::PointQuery; pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { if let Some(ball1) = ctxt.shape1.as_ball() { diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs index 93a5103..b43f53d 100644 --- a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs @@ -1,8 +1,8 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; use crate::geometry::Proximity; use crate::math::Isometry; -use buckler::query::sat; -use buckler::shape::Cuboid; +use eagl::query::sat; +use eagl::shape::Cuboid; pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs index 922855a..5ac12a8 100644 --- a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs @@ -1,7 +1,7 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; use crate::geometry::{Cuboid, Proximity, Triangle}; use crate::math::Isometry; -use buckler::query::sat; +use eagl::query::sat; pub fn detect_proximity_cuboid_triangle( ctxt: &mut PrimitiveProximityDetectionContext, diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index dbb20f5..961de3b 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -1,4 +1,4 @@ -use crate::buckler::bounding_volume::{BoundingVolume, AABB}; +use crate::eagl::bounding_volume::{BoundingVolume, AABB}; use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; @@ -87,7 +87,7 @@ fn do_detect_proximity( for triangle_id in new_interferences.iter() { if *triangle_id >= trimesh1.num_triangles() as u32 { - // Because of SIMD padding, the broad-phase may return tiangle indices greater + // Because of SIMD padding, the broad-phase may return triangle indices greater // than the max. continue; } diff --git a/src/geometry/proximity.rs b/src/geometry/proximity_pair.rs similarity index 100% rename from src/geometry/proximity.rs rename to src/geometry/proximity_pair.rs diff --git a/src/geometry/triangle.rs b/src/geometry/triangle.rs deleted file mode 100644 index 1f2bc92..0000000 --- a/src/geometry/triangle.rs +++ /dev/null @@ -1,7 +0,0 @@ -#[cfg(feature = "dim2")] -use crate::{buckler::shape::CuboidFeatureFace, geometry::Triangle, math::Vector}; - -#[cfg(feature = "dim2")] -pub fn support_face(_triangle: &Triangle, _local_dir: Vector) -> CuboidFeatureFace { - unimplemented!() -} diff --git a/src/geometry/z_order.rs b/src/geometry/z_order.rs deleted file mode 100644 index 6693547..0000000 --- a/src/geometry/z_order.rs +++ /dev/null @@ -1,70 +0,0 @@ -use num_traits::float::FloatCore; -use std::cmp::Ordering; - -#[allow(dead_code)] // We don't use this currently, but migth in the future. -pub fn z_cmp_ints(lhs: &[usize], rhs: &[usize]) -> Ordering { - assert_eq!( - lhs.len(), - rhs.len(), - "Cannot compare array with different lengths." - ); - - let mut msd = 0; - - for dim in 1..rhs.len() { - if less_msb(lhs[msd] ^ rhs[msd], lhs[dim] ^ rhs[dim]) { - msd = dim; - } - } - - lhs[msd].cmp(&rhs[msd]) -} - -fn less_msb(x: usize, y: usize) -> bool { - x < y && x < (x ^ y) -} - -// Fast construction of k-Nearest Neighbor Graphs for Point Clouds -// Michael Connor, Piyush Kumar -// Algorithm 1 -// -// http://compgeom.com/~piyush/papers/tvcg_stann.pdf -pub fn z_cmp_floats(p1: &[f32], p2: &[f32]) -> Option { - assert_eq!( - p1.len(), - p2.len(), - "Cannot compare array with different lengths." - ); - let mut x = 0; - let mut dim = 0; - - for j in 0..p1.len() { - let y = xor_msb_float(p1[j], p2[j]); - if x < y { - x = y; - dim = j; - } - } - - p1[dim].partial_cmp(&p2[dim]) -} - -fn xor_msb_float(fa: f32, fb: f32) -> i16 { - let (mantissa1, exponent1, _sign1) = fa.integer_decode(); - let (mantissa2, exponent2, _sign2) = fb.integer_decode(); - let x = exponent1; // To keep the same notation as the paper. - let y = exponent2; // To keep the same notation as the paper. - - if x == y { - let z = msdb(mantissa1, mantissa2); - x - z - } else if y < x { - x - } else { - y - } -} - -fn msdb(x: u64, y: u64) -> i16 { - 64i16 - (x ^ y).leading_zeros() as i16 -} diff --git a/src/lib.rs b/src/lib.rs index 0cd76a1..2dc770a 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -11,11 +11,11 @@ // FIXME: deny that #![allow(missing_docs)] -#[cfg(feature = "dim2")] -pub extern crate buckler2d as buckler; -#[cfg(feature = "dim3")] -pub extern crate buckler3d as buckler; pub extern crate crossbeam; +#[cfg(feature = "dim2")] +pub extern crate eagl2d as eagl; +#[cfg(feature = "dim3")] +pub extern crate eagl3d as eagl; pub extern crate nalgebra as na; #[cfg(feature = "serde")] #[macro_use] @@ -128,4 +128,4 @@ pub mod dynamics; pub mod geometry; pub mod pipeline; pub mod utils; -pub use buckler::math; +pub use eagl::math; diff --git a/src/utils.rs b/src/utils.rs index 4089631..48414bc 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -5,6 +5,7 @@ use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vec use num::Zero; use simba::simd::SimdValue; +use eagl::utils::SdpMatrix3; use std::ops::{Add, Mul}; use { crate::math::{AngularInertia, SimdBool, SimdReal}, @@ -468,8 +469,7 @@ impl WAngularInertia for SimdReal { } } -#[cfg(feature = "dim3")] -impl WAngularInertia for AngularInertia { +impl WAngularInertia for SdpMatrix3 { type AngVector = Vector3; type LinVector = Vector3; type AngMatrix = Matrix3; @@ -485,7 +485,7 @@ impl WAngularInertia for AngularInertia { if determinant.is_zero() { Self::zero() } else { - AngularInertia { + SdpMatrix3 { m11: minor_m12_m23 / determinant, m12: -minor_m11_m23 / determinant, m13: minor_m11_m22 / determinant, @@ -497,7 +497,7 @@ impl WAngularInertia for AngularInertia { } fn squared(&self) -> Self { - AngularInertia { + SdpMatrix3 { m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13, m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23, m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33, @@ -533,8 +533,7 @@ impl WAngularInertia for AngularInertia { } } -#[cfg(feature = "dim3")] -impl WAngularInertia for AngularInertia { +impl WAngularInertia for SdpMatrix3 { type AngVector = Vector3; type LinVector = Vector3; type AngMatrix = Matrix3; @@ -551,7 +550,7 @@ impl WAngularInertia for AngularInertia { let is_zero = determinant.simd_eq(zero); let inv_det = (::one() / determinant).select(is_zero, zero); - AngularInertia { + SdpMatrix3 { m11: minor_m12_m23 * inv_det, m12: -minor_m11_m23 * inv_det, m13: minor_m11_m22 * inv_det, @@ -573,7 +572,7 @@ impl WAngularInertia for AngularInertia { } fn squared(&self) -> Self { - AngularInertia { + SdpMatrix3 { m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13, m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23, m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33, diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index 0d1483e..0880419 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -1,9 +1,9 @@ #[macro_use] extern crate kiss3d; #[cfg(feature = "dim2")] -extern crate buckler2d as buckler; +extern crate eagl2d as eagl; #[cfg(feature = "dim3")] -extern crate buckler3d as buckler; +extern crate eagl3d as eagl; extern crate nalgebra as na; #[cfg(feature = "dim2")] extern crate ncollide2d as ncollide; diff --git a/src_testbed/objects/heightfield.rs b/src_testbed/objects/heightfield.rs index 977bcca..a631b71 100644 --- a/src_testbed/objects/heightfield.rs +++ b/src_testbed/objects/heightfield.rs @@ -1,6 +1,6 @@ #[cfg(feature = "dim3")] use crate::objects::node::{self, GraphicsNode}; -use buckler::shape; +use eagl::shape; use kiss3d::resource::Mesh; use kiss3d::window::Window; use na::{self, Point3}; diff --git a/src_testbed/objects/polyline.rs b/src_testbed/objects/polyline.rs index efb5e95..62edc69 100644 --- a/src_testbed/objects/polyline.rs +++ b/src_testbed/objects/polyline.rs @@ -1,4 +1,4 @@ -use buckler2d::shape; +use eagl2d::shape; use kiss3d::window::Window; use na::{Isometry2, Point2, Point3}; use nphysics2d::object::{ColliderAnchor, DefaultColliderHandle, DefaultColliderSet};