From e231bacec608fa5efd24f7a876572927dbd6c9c4 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 17 Dec 2020 10:24:36 +0100 Subject: Move all the contact manifold computations out of Rapier. --- src/geometry/broad_phase_multi_sap.rs | 8 +- src/geometry/collider.rs | 8 +- src/geometry/contact.rs | 308 --------------------- .../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 ---- .../contact_generator/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_pair.rs | 292 +++++++++++++++++++ src/geometry/mod.rs | 53 ++-- src/geometry/narrow_phase.rs | 58 ++-- src/geometry/polygon.rs | 2 +- src/geometry/proximity.rs | 43 --- .../ball_convex_proximity_detector.rs | 2 +- .../cuboid_cuboid_proximity_detector.rs | 4 +- .../cuboid_triangle_proximity_detector.rs | 2 +- .../trimesh_shape_proximity_detector.rs | 4 +- src/geometry/proximity_pair.rs | 43 +++ src/geometry/triangle.rs | 7 - src/geometry/z_order.rs | 70 ----- 33 files changed, 397 insertions(+), 2647 deletions(-) delete mode 100644 src/geometry/contact.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 create mode 100644 src/geometry/contact_pair.rs delete mode 100644 src/geometry/proximity.rs create mode 100644 src/geometry/proximity_pair.rs delete mode 100644 src/geometry/triangle.rs delete mode 100644 src/geometry/z_order.rs (limited to 'src/geometry') 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.rs b/src/geometry/contact.rs deleted file mode 100644 index 14646d9..0000000 --- a/src/geometry/contact.rs +++ /dev/null @@ -1,308 +0,0 @@ -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}; -#[cfg(feature = "simd-is-enabled")] -use { - crate::math::{SimdReal, SIMD_WIDTH}, - simba::simd::SimdValue, -}; - -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - /// Flags affecting the behavior of the constraints solver for a given contact manifold. - pub struct SolverFlags: u32 { - /// The constraint solver will take this contact manifold into - /// account for force computation. - const COMPUTE_IMPULSES = 0b01; - } -} - -#[cfg(feature = "simd-is-enabled")] -pub(crate) struct WContact { - pub local_p1: Point, - pub local_p2: Point, - pub local_n1: Vector, - pub local_n2: Vector, - pub dist: SimdReal, - pub fid1: [u8; SIMD_WIDTH], - pub fid2: [u8; SIMD_WIDTH], -} - -#[cfg(feature = "simd-is-enabled")] -impl WContact { - pub fn extract(&self, i: usize) -> (Contact, Vector, Vector) { - let c = Contact { - local_p1: self.local_p1.extract(i), - local_p2: self.local_p2.extract(i), - dist: self.dist.extract(i), - fid1: self.fid1[i], - fid2: self.fid2[i], - data: ContactData::default(), - }; - - (c, self.local_n1.extract(i), self.local_n2.extract(i)) - } -} - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A single contact between two collider. -pub struct ContactData { - /// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body. - /// - /// The impulse applied to the second collider's rigid-body is given by `-impulse`. - pub impulse: f32, - /// The friction impulse along the vector orthonormal to the contact normal, applied to the first - /// collider's rigid-body. - #[cfg(feature = "dim2")] - pub tangent_impulse: f32, - /// The friction impulses along the basis orthonormal to the contact normal, applied to the first - /// collider's rigid-body. - #[cfg(feature = "dim3")] - pub tangent_impulse: [f32; 2], -} - -impl ContactData { - #[cfg(feature = "dim2")] - pub(crate) fn zero_tangent_impulse() -> f32 { - 0.0 - } - - #[cfg(feature = "dim3")] - pub(crate) fn zero_tangent_impulse() -> [f32; 2] { - [0.0, 0.0] - } -} - -impl Default for ContactData { - fn default() -> Self { - Self { - impulse: 0.0, - tangent_impulse: Self::zero_tangent_impulse(), - } - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone)] -/// The description of all the contacts between a pair of colliders. -pub struct ContactPair { - /// The pair of colliders involved. - pub pair: ColliderPair, - /// The set of contact manifolds between the two colliders. - /// - /// 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, -} - -impl ContactPair { - pub(crate) fn new( - pair: ColliderPair, - generator: ContactPhase, - generator_workspace: Option, - ) -> Self { - Self { - pair, - manifolds: Vec::new(), - generator: Some(generator), - generator_workspace, - } - } - - /// Does this contact pair have any active contact? - /// - /// An active contact is a contact that may result in a non-zero contact force. - pub fn has_any_active_contact(&self) -> bool { - for manifold in &self.manifolds { - if manifold.num_active_contacts != 0 { - return true; - } - } - - false - } - - pub(crate) fn single_manifold<'a, 'b>( - &'a mut self, - colliders: &'b ColliderSet, - flags: SolverFlags, - ) -> ( - &'b Collider, - &'b Collider, - &'a mut ContactManifold, - Option<&'a mut (dyn MaybeSerializableData)>, - ) { - let coll1 = &colliders[self.pair.collider1]; - let coll2 = &colliders[self.pair.collider2]; - - if self.manifolds.len() == 0 { - let manifold_data = ContactManifoldData::from_colliders(self.pair, coll1, coll2, flags); - self.manifolds - .push(ContactManifold::with_data((0, 0), manifold_data)); - } - - // We have to make sure the order of the returned collider - // match the order of the pair stored inside of the manifold. - // (This order can be modified by the contact determination algorithm). - let manifold = &mut self.manifolds[0]; - if manifold.data.pair.collider1 == self.pair.collider1 { - ( - coll1, - coll2, - manifold, - self.generator_workspace.as_mut().map(|w| &mut *w.0), - ) - } else { - ( - coll2, - coll1, - manifold, - self.generator_workspace.as_mut().map(|w| &mut *w.0), - ) - } - } -} - -#[derive(Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A contact manifold between two colliders. -/// -/// A contact manifold describes a set of contacts between two colliders. All the contact -/// part of the same contact manifold share the same contact normal and contact kinematics. -pub struct ContactManifoldData { - // The following are set by the narrow-phase. - /// The pair of colliders involved in this contact manifold. - pub pair: ColliderPair, - /// The pair of body involved in this contact manifold. - pub body_pair: BodyPair, - pub(crate) warmstart_multiplier: f32, - // The two following are set by the constraints solver. - pub(crate) constraint_index: usize, - pub(crate) position_constraint_index: usize, - // We put the following fields here to avoids reading the colliders inside of the - // contact preparation method. - /// The friction coefficient for of all the contacts on this contact manifold. - pub friction: f32, - /// The restitution coefficient for all the contacts on this contact manifold. - pub restitution: f32, - /// The relative position between the first collider and its parent at the time the - /// contact points were generated. - pub delta1: Isometry, - /// The relative position between the second collider and its parent at the time the - /// contact points were generated. - pub delta2: Isometry, - /// Flags used to control some aspects of the constraints solver for this contact manifold. - pub solver_flags: SolverFlags, -} - -impl Default for ContactManifoldData { - fn default() -> Self { - Self::new( - ColliderPair::new(ColliderSet::invalid_handle(), ColliderSet::invalid_handle()), - BodyPair::new( - RigidBodySet::invalid_handle(), - RigidBodySet::invalid_handle(), - ), - Isometry::identity(), - Isometry::identity(), - 0.0, - 0.0, - SolverFlags::empty(), - ) - } -} - -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, - body_pair: BodyPair, - delta1: Isometry, - delta2: Isometry, - friction: f32, - restitution: f32, - solver_flags: SolverFlags, - ) -> ContactManifoldData { - Self { - pair, - body_pair, - warmstart_multiplier: Self::min_warmstart_multiplier(), - friction, - restitution, - delta1, - delta2, - constraint_index: 0, - position_constraint_index: 0, - solver_flags, - } - } - - pub(crate) fn from_colliders( - pair: ColliderPair, - coll1: &Collider, - coll2: &Collider, - flags: SolverFlags, - ) -> Self { - Self::with_subshape_indices(pair, coll1, coll2, flags) - } - - pub(crate) fn with_subshape_indices( - pair: ColliderPair, - coll1: &Collider, - coll2: &Collider, - solver_flags: SolverFlags, - ) -> Self { - Self::new( - pair, - BodyPair::new(coll1.parent, coll2.parent), - *coll1.position_wrt_parent(), - *coll2.position_wrt_parent(), - (coll1.friction + coll2.friction) * 0.5, - (coll1.restitution + coll2.restitution) * 0.5, - solver_flags, - ) - } - - pub(crate) fn min_warmstart_multiplier() -> f32 { - // Multiplier used to reduce the amount of warm-starting. - // 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 - } - - pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) { - // In 2D, tall stacks will actually suffer from this - // because oscillation due to inaccuracies in 2D often - // cause contacts to break, which would result in - // a reset of the warmstart multiplier. - if cfg!(feature = "dim2") { - manifold.data.warmstart_multiplier = 1.0; - return; - } - - for pt in &manifold.points { - if pt.data.impulse != 0.0 { - manifold.data.warmstart_multiplier = - (manifold.data.warmstart_multiplier * 2.0).min(1.0); - return; - } - } - - // Reset the multiplier. - manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() - } -} 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,