From 9bf1321f8f1d2e116f44c2461a53f302c4ef4171 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 8 Dec 2020 17:31:49 +0100 Subject: Outsource the contact manifold, SAT, and some shapes. --- src/geometry/broad_phase_multi_sap.rs | 12 +- src/geometry/capsule.rs | 192 -------- src/geometry/collider.rs | 6 +- src/geometry/contact.rs | 333 +++---------- .../ball_ball_contact_generator.rs | 6 +- .../ball_convex_contact_generator.rs | 18 +- .../capsule_capsule_contact_generator.rs | 33 +- .../cuboid_capsule_contact_generator.rs | 37 +- .../cuboid_cuboid_contact_generator.rs | 23 +- .../cuboid_triangle_contact_generator.rs | 38 +- .../heightfield_shape_contact_generator.rs | 14 +- src/geometry/contact_generator/mod.rs | 30 -- .../contact_generator/pfm_pfm_contact_generator.rs | 30 +- .../polygon_polygon_contact_generator.rs | 175 +------ .../trimesh_shape_contact_generator.rs | 18 +- src/geometry/cuboid.rs | 234 ---------- src/geometry/cuboid_feature2d.rs | 128 ----- src/geometry/cuboid_feature3d.rs | 516 --------------------- src/geometry/mod.rs | 110 +++-- src/geometry/narrow_phase.rs | 7 +- src/geometry/polygon.rs | 4 +- src/geometry/polygonal_feature_map.rs | 132 ------ src/geometry/polyhedron_feature3d.rs | 445 ------------------ .../ball_convex_proximity_detector.rs | 11 +- .../cuboid_cuboid_proximity_detector.rs | 17 +- .../cuboid_triangle_proximity_detector.rs | 8 +- .../trimesh_shape_proximity_detector.rs | 4 +- src/geometry/round_cylinder.rs | 49 +- src/geometry/sat.rs | 344 +------------- src/geometry/shape.rs | 49 +- src/geometry/triangle.rs | 4 +- src/geometry/trimesh.rs | 63 +-- src/geometry/waabb.rs | 12 +- src/geometry/wquadtree.rs | 2 +- 34 files changed, 368 insertions(+), 2736 deletions(-) delete mode 100644 src/geometry/capsule.rs delete mode 100644 src/geometry/cuboid.rs delete mode 100644 src/geometry/cuboid_feature2d.rs delete mode 100644 src/geometry/cuboid_feature3d.rs delete mode 100644 src/geometry/polygonal_feature_map.rs delete mode 100644 src/geometry/polyhedron_feature3d.rs (limited to 'src/geometry') diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 863990d..56c05df 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -4,7 +4,7 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; use bit_vec::BitVec; -use ncollide::bounding_volume::{BoundingVolume, AABB}; +use buckler::bounding_volume::{BoundingVolume, AABB}; use std::cmp::Ordering; use std::ops::{Index, IndexMut}; @@ -67,7 +67,7 @@ fn point_key(point: Point) -> Point { (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() } -fn region_aabb(index: Point) -> AABB { +fn region_aabb(index: Point) -> AABB { let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into(); let maxs = mins + Vector::repeat(CELL_WIDTH); AABB::new(mins, maxs) @@ -345,7 +345,7 @@ struct SAPRegion { } impl SAPRegion { - pub fn new(bounds: AABB) -> Self { + pub fn new(bounds: AABB) -> Self { let axes = [ SAPAxis::new(bounds.mins.x, bounds.maxs.x), SAPAxis::new(bounds.mins.y, bounds.maxs.y), @@ -361,7 +361,7 @@ impl SAPRegion { } } - pub fn recycle(bounds: AABB, mut old: Self) -> Self { + pub fn recycle(bounds: AABB, mut old: Self) -> Self { // Correct the bounds for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) { axis.min_bound = bound; @@ -381,7 +381,7 @@ impl SAPRegion { old } - pub fn recycle_or_new(bounds: AABB, pool: &mut Vec) -> Self { + pub fn recycle_or_new(bounds: AABB, pool: &mut Vec) -> Self { if let Some(old) = pool.pop() { Self::recycle(bounds, old) } else { @@ -488,7 +488,7 @@ pub struct BroadPhase { #[derive(Clone)] pub(crate) struct BroadPhaseProxy { handle: ColliderHandle, - aabb: AABB, + aabb: AABB, next_free: u32, } diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs deleted file mode 100644 index 54736cc..0000000 --- a/src/geometry/capsule.rs +++ /dev/null @@ -1,192 +0,0 @@ -use crate::geometry::{Ray, RayIntersection, AABB}; -use crate::math::{Isometry, Point, Rotation, Vector}; -use approx::AbsDiffEq; -use na::Unit; -use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast}; -use ncollide::shape::{FeatureId, Segment, SupportMap}; - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A capsule shape defined as a round segment. -pub struct Capsule { - /// The axis and endpoint of the capsule. - pub segment: Segment, - /// The radius of the capsule. - pub radius: f32, -} - -impl Capsule { - /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius. - pub fn new_x(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::x() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius. - pub fn new_y(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::y() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius. - #[cfg(feature = "dim3")] - pub fn new_z(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::z() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. - pub fn new(a: Point, b: Point, radius: f32) -> Self { - let segment = Segment::new(a, b); - Self { segment, radius } - } - - /// The axis-aligned bounding box of this capsule. - pub fn aabb(&self, pos: &Isometry) -> AABB { - let a = pos * self.segment.a; - let b = pos * self.segment.b; - let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius); - let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius); - AABB::new(mins.into(), maxs.into()) - } - - /// The height of this capsule. - pub fn height(&self) -> f32 { - (self.segment.b - self.segment.a).norm() - } - - /// The half-height of this capsule. - pub fn half_height(&self) -> f32 { - self.height() / 2.0 - } - - /// The center of this capsule. - pub fn center(&self) -> Point { - na::center(&self.segment.a, &self.segment.b) - } - - /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. - pub fn transform_by(&self, pos: &Isometry) -> Self { - Self::new(pos * self.segment.a, pos * self.segment.b, self.radius) - } - - /// The rotation `r` such that `r * Y` is collinear with `b - a`. - pub fn rotation_wrt_y(&self) -> Rotation { - let mut dir = self.segment.b - self.segment.a; - if dir.y < 0.0 { - dir = -dir; - } - - #[cfg(feature = "dim2")] - { - Rotation::rotation_between(&Vector::y(), &dir) - } - - #[cfg(feature = "dim3")] - { - Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()) - } - } - - /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`. - pub fn transform_wrt_y(&self) -> Isometry { - let rot = self.rotation_wrt_y(); - Isometry::from_parts(self.center().coords.into(), rot) - } -} - -impl SupportMap for Capsule { - fn local_support_point(&self, dir: &Vector) -> Point { - let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis()); - self.local_support_point_toward(&dir) - } - - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) { - self.segment.a + **dir * self.radius - } else { - self.segment.b + **dir * self.radius - } - } -} - -impl RayCast for Capsule { - fn toi_and_normal_with_ray( - &self, - m: &Isometry, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option { - let ls_ray = ray.inverse_transform_by(m); - - ncollide::query::ray_intersection_with_support_map_with_params( - &Isometry::identity(), - self, - &mut VoronoiSimplex::new(), - &ls_ray, - max_toi, - solid, - ) - .map(|mut res| { - res.normal = m * res.normal; - res - }) - } -} - -// TODO: this code has been extracted from ncollide and added here -// so we can modify it to fit with our new definition of capsule. -// We should find a way to avoid this code duplication. -impl PointQuery for Capsule { - #[inline] - fn project_point( - &self, - m: &Isometry, - pt: &Point, - solid: bool, - ) -> PointProjection { - let seg = Segment::new(self.segment.a, self.segment.b); - let proj = seg.project_point(m, pt, solid); - let dproj = *pt - proj.point; - - if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) { - let inside = dist <= self.radius; - if solid && inside { - return PointProjection::new(true, *pt); - } else { - return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius); - } - } else if solid { - return PointProjection::new(true, *pt); - } - - #[cfg(feature = "dim2")] - if let Some(dir) = seg.normal() { - let dir = m * *dir; - PointProjection::new(true, proj.point + dir * self.radius) - } else { - // The segment has no normal, likely because it degenerates to a point. - PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) - } - - #[cfg(feature = "dim3")] - if let Some(dir) = seg.direction() { - use crate::utils::WBasis; - let dir = m * dir.orthonormal_basis()[0]; - PointProjection::new(true, proj.point + dir * self.radius) - } else { - // The segment has no normal, likely because it degenerates to a point. - PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) - } - } - - #[inline] - fn project_point_with_feature( - &self, - m: &Isometry, - pt: &Point, - ) -> (PointProjection, FeatureId) { - (self.project_point(m, pt, false), FeatureId::Face(0)) - } -} diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index c04be35..8154554 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -6,8 +6,8 @@ use crate::geometry::{ #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, RoundCylinder}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; +use buckler::bounding_volume::AABB; use na::Point3; -use ncollide::bounding_volume::AABB; use std::ops::Deref; use std::sync::Arc; @@ -270,11 +270,11 @@ impl Collider { } /// Compute the axis-aligned bounding box of this collider. - pub fn compute_aabb(&self) -> AABB { + pub fn compute_aabb(&self) -> AABB { self.shape.compute_aabb(&self.position) } - // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB { + // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB { // let aabb1 = self.shape.compute_aabb(&self.position); // let aabb2 = self.shape.compute_aabb(&self.predicted_position); // aabb1.merged(&aabb2) diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index a4a176e..1f0a902 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -1,7 +1,8 @@ +use crate::buckler::query::TrackedData; use crate::data::MaybeSerializableData; -use crate::dynamics::BodyPair; +use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet}; use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase}; -use crate::geometry::{Collider, ColliderPair, ColliderSet}; +use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold}; use crate::math::{Isometry, Point, Vector}; #[cfg(feature = "simd-is-enabled")] use { @@ -19,39 +20,6 @@ bitflags::bitflags! { } } -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// The type local linear approximation of the neighborhood of a pair contact points on two shapes -pub enum KinematicsCategory { - /// Both neighborhoods are assimilated to a single point. - PointPoint, - /// The first shape's neighborhood at the contact point is assimilated to a plane while - /// the second is assimilated to a point. - PlanePoint, -} - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// Local contact geometry at the neighborhood of a pair of contact points. -pub struct ContactKinematics { - /// The local contact geometry. - pub category: KinematicsCategory, - /// The dilation applied to the first contact geometry. - pub radius1: f32, - /// The dilation applied to the second contact geometry. - pub radius2: f32, -} - -impl Default for ContactKinematics { - fn default() -> Self { - ContactKinematics { - category: KinematicsCategory::PointPoint, - radius1: 0.0, - radius2: 0.0, - } - } -} - #[cfg(feature = "simd-is-enabled")] pub(crate) struct WContact { pub local_p1: Point, @@ -70,10 +38,9 @@ impl WContact { local_p1: self.local_p1.extract(i), local_p2: self.local_p2.extract(i), dist: self.dist.extract(i), - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), fid1: self.fid1[i], fid2: self.fid2[i], + data: ContactData::default(), }; (c, self.local_n1.extract(i), self.local_n2.extract(i)) @@ -83,11 +50,7 @@ impl WContact { #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A single contact between two collider. -pub struct Contact { - /// The contact point in the local-space of the first collider. - pub local_p1: Point, - /// The contact point in the local-space of the second collider. - pub local_p2: Point, +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`. @@ -100,46 +63,9 @@ pub struct Contact { /// collider's rigid-body. #[cfg(feature = "dim3")] pub tangent_impulse: [f32; 2], - /// The identifier of the subshape of the first collider involved in this contact. - /// - /// For primitive shapes like cuboid, ball, etc., this is 0. - /// For shapes like trimesh and heightfield this identifies the specific triangle - /// involved in the contact. - pub fid1: u8, - /// The identifier of the subshape of the second collider involved in this contact. - /// - /// For primitive shapes like cuboid, ball, etc., this is 0. - /// For shapes like trimesh and heightfield this identifies the specific triangle - /// involved in the contact. - pub fid2: u8, - /// The distance between the two colliders along the contact normal. - /// - /// If this is negative, the colliders are penetrating. - pub dist: f32, } -impl Contact { - pub(crate) fn new( - local_p1: Point, - local_p2: Point, - fid1: u8, - fid2: u8, - dist: f32, - ) -> Self { - Self { - local_p1, - local_p2, - impulse: 0.0, - #[cfg(feature = "dim2")] - tangent_impulse: 0.0, - #[cfg(feature = "dim3")] - tangent_impulse: [0.0; 2], - fid1, - fid2, - dist, - } - } - +impl ContactData { #[cfg(feature = "dim2")] pub(crate) fn zero_tangent_impulse() -> f32 { 0.0 @@ -149,26 +75,15 @@ impl Contact { pub(crate) fn zero_tangent_impulse() -> [f32; 2] { [0.0, 0.0] } +} - pub(crate) fn copy_geometry_from(&mut self, contact: Contact) { - self.local_p1 = contact.local_p1; - self.local_p2 = contact.local_p2; - self.fid1 = contact.fid1; - self.fid2 = contact.fid2; - self.dist = contact.dist; +impl Default for ContactData { + fn default() -> Self { + Self { + impulse: 0.0, + tangent_impulse: Self::zero_tangent_impulse(), + } } - - // pub(crate) fn swap(self) -> Self { - // Self { - // local_p1: self.local_p2, - // local_p2: self.local_p1, - // impulse: self.impulse, - // tangent_impulse: self.tangent_impulse, - // fid1: self.fid2, - // fid2: self.fid1, - // dist: self.dist, - // } - // } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -227,15 +142,16 @@ impl ContactPair { let coll2 = &colliders[self.pair.collider2]; if self.manifolds.len() == 0 { - let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags); - self.manifolds.push(manifold); + 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.pair.collider1 == self.pair.collider1 { + if manifold.data.pair.collider1 == self.pair.collider1 { ( coll1, coll2, @@ -259,30 +175,12 @@ impl ContactPair { /// /// 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 ContactManifold { - // NOTE: use a SmallVec instead? - // And for 2D use an ArrayVec since there will never be more than 2 contacts anyways. - #[cfg(feature = "dim2")] - pub(super) points: arrayvec::ArrayVec<[Contact; 2]>, - #[cfg(feature = "dim3")] - pub(super) points: Vec, - /// The number of active contacts on this contact manifold. - /// - /// Active contacts are these that may result in contact forces. - pub num_active_contacts: usize, - /// The contact normal of all the contacts of this manifold, expressed in the local space of the first collider. - pub local_n1: Vector, - /// The contact normal of all the contacts of this manifold, expressed in the local space of the second collider. - pub local_n2: Vector, - /// The contact kinematics of all the contacts of this manifold. - pub kinematics: ContactKinematics, +pub struct ContactManifoldData { // The following are set by the narrow-phase. - /// The pair of body involved in this contact manifold. - pub body_pair: BodyPair, /// The pair of colliders involved in this contact manifold. pub pair: ColliderPair, - /// The pair of subshapes involved in this contact manifold. - pub subshape_index_pair: (usize, usize), + /// 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, @@ -303,29 +201,44 @@ pub struct ContactManifold { pub solver_flags: SolverFlags, } -impl ContactManifold { +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, - subshapes: (usize, usize), body_pair: BodyPair, delta1: Isometry, delta2: Isometry, friction: f32, restitution: f32, solver_flags: SolverFlags, - ) -> ContactManifold { + ) -> ContactManifoldData { Self { - #[cfg(feature = "dim2")] - points: arrayvec::ArrayVec::new(), - #[cfg(feature = "dim3")] - points: Vec::new(), - num_active_contacts: 0, - local_n1: Vector::zeros(), - local_n2: Vector::zeros(), pair, - subshape_index_pair: subshapes, body_pair, - kinematics: ContactKinematics::default(), warmstart_multiplier: Self::min_warmstart_multiplier(), friction, restitution, @@ -337,50 +250,23 @@ impl ContactManifold { } } - pub(crate) fn take(&mut self) -> Self { - ContactManifold { - #[cfg(feature = "dim2")] - points: self.points.clone(), - #[cfg(feature = "dim3")] - points: std::mem::replace(&mut self.points, Vec::new()), - num_active_contacts: self.num_active_contacts, - local_n1: self.local_n1, - local_n2: self.local_n2, - kinematics: self.kinematics, - body_pair: self.body_pair, - pair: self.pair, - subshape_index_pair: self.subshape_index_pair, - warmstart_multiplier: self.warmstart_multiplier, - friction: self.friction, - restitution: self.restitution, - delta1: self.delta1, - delta2: self.delta2, - constraint_index: self.constraint_index, - position_constraint_index: self.position_constraint_index, - solver_flags: self.solver_flags, - } - } - pub(crate) fn from_colliders( pair: ColliderPair, coll1: &Collider, coll2: &Collider, flags: SolverFlags, ) -> Self { - Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags) + Self::with_subshape_indices(pair, coll1, coll2, flags) } pub(crate) fn with_subshape_indices( pair: ColliderPair, coll1: &Collider, coll2: &Collider, - subshape1: usize, - subshape2: usize, solver_flags: SolverFlags, ) -> Self { Self::new( pair, - (subshape1, subshape2), BodyPair::new(coll1.parent, coll2.parent), *coll1.position_wrt_parent(), *coll2.position_wrt_parent(), @@ -398,134 +284,25 @@ impl ContactManifold { 0.01 } - /// Number of active contacts on this contact manifold. - #[inline] - pub fn num_active_contacts(&self) -> usize { - self.num_active_contacts - } - - /// The slice of all the active contacts on this contact manifold. - /// - /// Active contacts are contacts that may end up generating contact forces. - #[inline] - pub fn active_contacts(&self) -> &[Contact] { - &self.points[..self.num_active_contacts] - } - - #[inline] - pub(crate) fn active_contacts_mut(&mut self) -> &mut [Contact] { - &mut self.points[..self.num_active_contacts] - } - - /// The slice of all the contacts, active or not, on this contact manifold. - #[inline] - pub fn all_contacts(&self) -> &[Contact] { - &self.points - } - - pub(crate) fn swap_identifiers(&mut self) { - self.pair = self.pair.swap(); - self.body_pair = self.body_pair.swap(); - self.subshape_index_pair = (self.subshape_index_pair.1, self.subshape_index_pair.0); - std::mem::swap(&mut self.delta1, &mut self.delta2); - } - - pub(crate) fn update_warmstart_multiplier(&mut self) { + 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") { - self.warmstart_multiplier = 1.0; + manifold.data.warmstart_multiplier = 1.0; return; } - for pt in &self.points { - if pt.impulse != 0.0 { - self.warmstart_multiplier = (self.warmstart_multiplier * 2.0).min(1.0); + 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. - self.warmstart_multiplier = Self::min_warmstart_multiplier() - } - - #[inline] - pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry) -> bool { - // const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES; - const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES; - const DIST_SQ_THRESHOLD: f32 = 0.001; // FIXME: this should not be hard-coded. - self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD) - } - - #[inline] - pub(crate) fn try_update_contacts_eps( - &mut self, - pos12: &Isometry, - angle_dot_threshold: f32, - dist_sq_threshold: f32, - ) -> bool { - if self.points.len() == 0 { - return false; - } - - let local_n2 = pos12 * self.local_n2; - - if -self.local_n1.dot(&local_n2) < angle_dot_threshold { - return false; - } - - for pt in &mut self.points { - let local_p2 = pos12 * pt.local_p2; - let dpt = local_p2 - pt.local_p1; - let dist = dpt.dot(&self.local_n1); - - if dist * pt.dist < 0.0 { - // We switched between penetrating/non-penetrating. - // The may result in other contacts to appear. - return false; - } - let new_local_p1 = local_p2 - self.local_n1 * dist; - - if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold { - return false; - } - - pt.dist = dist; - pt.local_p1 = new_local_p1; - } - - true - } - - /// Sort the contacts of this contact manifold such that the active contacts are in the first - /// positions of the array. - #[inline] - pub(crate) fn sort_contacts(&mut self, prediction_distance: f32) { - let num_contacts = self.points.len(); - match num_contacts { - 0 => { - self.num_active_contacts = 0; - } - 1 => { - self.num_active_contacts = (self.points[0].dist < prediction_distance) as usize; - } - _ => { - let mut first_inactive_index = num_contacts; - - self.num_active_contacts = 0; - while self.num_active_contacts != first_inactive_index { - if self.points[self.num_active_contacts].dist >= prediction_distance { - // Swap with the last contact. - self.points - .swap(self.num_active_contacts, first_inactive_index - 1); - first_inactive_index -= 1; - } else { - self.num_active_contacts += 1; - } - } - } - } + 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 index 96ac235..7122998 100644 --- a/src/geometry/contact_generator/ball_ball_contact_generator.rs +++ b/src/geometry/contact_generator/ball_ball_contact_generator.rs @@ -1,5 +1,5 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Contact, KinematicsCategory}; +use crate::geometry::{Contact, ContactManifoldData, KinematicsCategory}; use crate::math::{Point, Vector}; #[cfg(feature = "simd-is-enabled")] use { @@ -53,7 +53,7 @@ pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationCon manifold.kinematics.category = KinematicsCategory::PointPoint; manifold.kinematics.radius1 = radii_a.extract(i); manifold.kinematics.radius2 = radii_b.extract(i); - manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(manifold); } else { manifold.points.clear(); } @@ -94,7 +94,7 @@ pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint; ctxt.manifold.kinematics.radius1 = radius_a; ctxt.manifold.kinematics.radius2 = radius_b; - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } else { ctxt.manifold.points.clear(); } diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index 69bc5e3..88f1912 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -1,8 +1,8 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Ball, Contact, KinematicsCategory}; +use crate::geometry::{Ball, Contact, ContactManifoldData, KinematicsCategory}; use crate::math::Isometry; +use buckler::query::PointQuery; use na::Unit; -use ncollide::query::PointQuery; pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { if let Some(ball1) = ctxt.shape1.as_ball() { @@ -15,7 +15,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex ctxt.manifold.sort_contacts(ctxt.prediction_distance); } -fn do_generate_contacts>( +fn do_generate_contacts( point_query1: &P, ball2: &Ball, ctxt: &mut PrimitiveContactGenerationContext, @@ -33,12 +33,8 @@ fn do_generate_contacts>( } let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into()); - - // TODO: add a `project_local_point` to the PointQuery trait to avoid - // the identity isometry. - let proj = - point_query1.project_point(&Isometry::identity(), &local_p2_1, cfg!(feature = "dim3")); - let dpos = local_p2_1 - proj.point; + 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) { @@ -51,8 +47,8 @@ fn do_generate_contacts>( 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); - let contact_point = Contact::new(proj.point, local_p2, 0, 0, dist - ball2.radius); if ctxt.manifold.points.len() != 1 { ctxt.manifold.points.clear(); ctxt.manifold.points.push(contact_point); @@ -66,7 +62,7 @@ fn do_generate_contacts>( ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; ctxt.manifold.kinematics.radius1 = 0.0; ctxt.manifold.kinematics.radius2 = ball2.radius; - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } else { ctxt.manifold.points.clear(); } diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index 3104496..9090b36 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -1,11 +1,11 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory}; +use crate::geometry::{Capsule, Contact, ContactManifold, ContactManifoldData, KinematicsCategory}; use crate::math::Isometry; use crate::math::Vector; use approx::AbsDiffEq; -use na::Unit; #[cfg(feature = "dim2")] -use ncollide::shape::SegmentPointLocation; +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()) { @@ -19,7 +19,7 @@ pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationCo ); } - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -41,7 +41,7 @@ pub fn generate_contacts<'a>( let seg1 = capsule1.segment; let seg2_1 = capsule2.segment.transformed(&pos12); - let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( + let (loc1, loc2) = buckler::query::details::closest_points_segment_segment_with_locations_nD( (&seg1.a, &seg1.b), (&seg2_1.a, &seg2_1.b), ); @@ -94,11 +94,13 @@ pub fn generate_contacts<'a>( { // Capsules axes are almost parallel and are almost perpendicular to the normal. // Find a second contact point. - if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal( - (seg1.a, seg1.b), - (seg2_1.a, seg2_1.b), - *local_n1, - ) { + 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. @@ -139,7 +141,7 @@ pub fn generate_contacts<'a>( } } - super::match_contacts(manifold, &old_manifold_points, swapped); + manifold.match_contacts(&old_manifold_points, swapped); } #[cfg(feature = "dim3")] @@ -156,10 +158,11 @@ pub fn generate_contacts<'a>( let seg1 = capsule1.segment; let seg2_1 = capsule2.segment.transformed(&pos12); - let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( - (&seg1.a, &seg1.b), - (&seg2_1.a, &seg2_1.b), - ); + 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(); diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index 3fd4a17..9f5d856 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -1,11 +1,12 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -#[cfg(feature = "dim3")] -use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory}; -#[cfg(feature = "dim2")] -use crate::geometry::{CuboidFeature, CuboidFeatureFace}; +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()) { @@ -18,7 +19,7 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon ctxt.manifold, false, ); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } else if let (Some(capsule1), Some(cube2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid()) { @@ -31,7 +32,7 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon ctxt.manifold, true, ); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -61,7 +62,8 @@ pub fn generate_contacts<'a>( * Point-Face cases. * */ - let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, &segment2, &pos12); + 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; @@ -84,8 +86,7 @@ pub fn generate_contacts<'a>( #[cfg(feature = "dim2")] let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. #[cfg(feature = "dim3")] - let sep3 = - sat::cube_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12, &pos21); + let sep3 = sat::cuboid_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12); if sep3.0 > capsule2.radius + prediction_distance { manifold.points.clear(); return; @@ -118,20 +119,20 @@ pub fn generate_contacts<'a>( { if swapped_reference { feature1 = CuboidFeatureFace::from(segment2); - feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1); + feature2 = cube1.support_face(pos21 * -best_sep.1); } else { - feature1 = cuboid::support_face(cube1, best_sep.1); + feature1 = cube1.support_face(best_sep.1); feature2 = CuboidFeatureFace::from(segment2); } } #[cfg(feature = "dim3")] { if swapped_reference { - feature1 = PolyhedronFace::from(segment2); - feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1); + feature1 = PolyhedronFeature::from(segment2); + feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1); } else { - feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1); - feature2 = PolyhedronFace::from(segment2); + feature1 = cube1.polyhedron_support_face(best_sep.1); + feature2 = PolyhedronFeature::from(segment2); } } @@ -156,7 +157,7 @@ pub fn generate_contacts<'a>( manifold, ); #[cfg(feature = "dim3")] - PolyhedronFace::contacts( + PolyhedronFeature::contacts( prediction_distance + capsule2.radius, &feature1, &best_sep.1, @@ -185,5 +186,5 @@ pub fn generate_contacts<'a>( } // Transfer impulses. - super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference); + 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 index 5be5af3..1d750ef 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -1,9 +1,10 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory}; +use crate::geometry::{ContactManifold, ContactManifoldData, KinematicsCategory}; use crate::math::Isometry; #[cfg(feature = "dim2")] use crate::math::Vector; -use ncollide::shape::Cuboid; +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()) { @@ -19,15 +20,15 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont unreachable!() } - ctxt.manifold.update_warmstart_multiplier(); + 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 cube1: &'a Cuboid, mut pos1: &'a Isometry, - mut cube2: &'a Cuboid, + mut cube2: &'a Cuboid, mut pos2: &'a Isometry, manifold: &mut ContactManifold, ) { @@ -43,13 +44,13 @@ pub fn generate_contacts<'a>( * Point-Face * */ - let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21); + 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, &pos12); + let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21); if sep2.0 > prediction_distance { manifold.points.clear(); return; @@ -63,7 +64,7 @@ pub fn generate_contacts<'a>( #[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, &pos21); + let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12); if sep3.0 > prediction_distance { manifold.points.clear(); return; @@ -97,8 +98,8 @@ pub fn generate_contacts<'a>( // 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 = cuboid::support_feature(cube1, best_sep.1); - let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1); + 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) { @@ -151,5 +152,5 @@ pub fn generate_contacts<'a>( manifold.kinematics.radius2 = 0.0; // Transfer impulses. - super::match_contacts(manifold, &old_manifold_points, swapped); + manifold.match_contacts(&old_manifold_points, swapped); } diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 562d7d6..06dc6f0 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -1,13 +1,11 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -#[cfg(feature = "dim3")] -use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Triangle}; +use crate::geometry::{ContactManifold, ContactManifoldData, Cuboid, KinematicsCategory, Triangle}; use crate::math::Isometry; #[cfg(feature = "dim2")] -use crate::{ - geometry::{triangle, CuboidFeature}, - math::Vector, -}; +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()) { @@ -20,7 +18,7 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo ctxt.manifold, false, ); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } else if let (Some(triangle1), Some(cube2)) = (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) { @@ -33,7 +31,7 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo ctxt.manifold, true, ); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -61,7 +59,8 @@ pub fn generate_contacts<'a>( * Point-Face cases. * */ - let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12); + let sep1 = + sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12); if sep1.0 > prediction_distance { manifold.points.clear(); return; @@ -81,8 +80,7 @@ pub fn generate_contacts<'a>( #[cfg(feature = "dim2")] let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. #[cfg(feature = "dim3")] - let sep3 = - sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21); + let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12); if sep3.0 > prediction_distance { manifold.points.clear(); return; @@ -115,20 +113,20 @@ pub fn generate_contacts<'a>( { if swapped_reference { feature1 = triangle::support_face(triangle2, best_sep.1); - feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1); + feature2 = cube1.support_face(pos21 * -best_sep.1); } else { - feature1 = cuboid::support_face(cube1, best_sep.1); + feature1 = cube1.support_face(best_sep.1); feature2 = triangle::support_face(triangle2, pos21 * -best_sep.1); } } #[cfg(feature = "dim3")] { if swapped_reference { - feature1 = PolyhedronFace::from(*triangle2); - feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1); + feature1 = PolyhedronFeature::from(*triangle2); + feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1); } else { - feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1); - feature2 = PolyhedronFace::from(*triangle2); + feature1 = cube1.polyhedron_support_face(best_sep.1); + feature2 = PolyhedronFeature::from(*triangle2); } } @@ -153,7 +151,7 @@ pub fn generate_contacts<'a>( manifold, ); #[cfg(feature = "dim3")] - PolyhedronFace::contacts( + PolyhedronFeature::contacts( prediction_distance, &feature1, &best_sep.1, @@ -169,5 +167,5 @@ pub fn generate_contacts<'a>( manifold.kinematics.radius2 = 0.0; // Transfer impulses. - super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference); + 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 index 70360c9..358ac84 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -1,3 +1,4 @@ +use crate::buckler::bounding_volume::BoundingVolume; use crate::data::hashmap::{Entry, HashMap}; use crate::data::MaybeSerializableData; use crate::geometry::contact_generator::{ @@ -6,8 +7,7 @@ use crate::geometry::contact_generator::{ }; #[cfg(feature = "dim2")] use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, HeightField, Shape}; -use crate::ncollide::bounding_volume::BoundingVolume; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData, HeightField, Shape}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; @@ -88,7 +88,7 @@ fn do_generate_contacts( let solver_flags = ctxt.solver_flags; let shape_type2 = collider2.shape().shape_type(); - heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| { + 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. @@ -113,15 +113,13 @@ fn do_generate_contacts( timestamp: new_timestamp, workspace: workspace2, }; - let manifold = ContactManifold::with_subshape_indices( + let manifold_data = ContactManifoldData::from_colliders( coll_pair, collider1, collider2, - i, - 0, solver_flags, ); - manifolds.push(manifold); + manifolds.push(ContactManifold::with_data((i, 0), manifold_data)); entry.insert(sub_detector) } @@ -142,7 +140,7 @@ fn do_generate_contacts( let manifold = &mut manifolds[sub_detector.manifold_id]; - let mut ctxt2 = if coll_pair.collider1 != manifold.pair.collider1 { + let mut ctxt2 = if coll_pair.collider1 != manifold.data.pair.collider1 { PrimitiveContactGenerationContext { prediction_distance, collider1: collider2, diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index 9b14711..f539d7a 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -28,10 +28,6 @@ pub use self::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, }; -pub(crate) use self::polygon_polygon_contact_generator::clip_segments; -#[cfg(feature = "dim2")] -pub(crate) use self::polygon_polygon_contact_generator::clip_segments_with_normal; - pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag; mod ball_ball_contact_generator; @@ -53,29 +49,3 @@ mod serializable_workspace_tag; mod trimesh_shape_contact_generator; use crate::geometry::{Contact, ContactManifold}; - -pub(crate) fn match_contacts( - manifold: &mut ContactManifold, - old_contacts: &[Contact], - swapped: bool, -) { - for contact in &mut manifold.points { - if !swapped { - for old_contact in old_contacts { - if contact.fid1 == old_contact.fid1 && contact.fid2 == old_contact.fid2 { - // Transfer impulse cache. - contact.impulse = old_contact.impulse; - contact.tangent_impulse = old_contact.tangent_impulse; - } - } - } else { - for old_contact in old_contacts { - if contact.fid1 == old_contact.fid2 && contact.fid2 == old_contact.fid1 { - // Transfer impulse cache. - contact.impulse = old_contact.impulse; - contact.tangent_impulse = old_contact.tangent_impulse; - } - } - } - } -} diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index 5bc213e..721d5dc 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -1,12 +1,15 @@ use crate::data::MaybeSerializableData; use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{KinematicsCategory, PolygonalFeatureMap, PolyhedronFace}; +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; -use ncollide::query; -use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] @@ -15,12 +18,12 @@ pub struct PfmPfmContactManifoldGeneratorWorkspace { feature = "serde-serialize", serde(skip, default = "VoronoiSimplex::new") )] - simplex: VoronoiSimplex, + simplex: VoronoiSimplex, last_gjk_dir: Option>>, #[cfg_attr(feature = "serde-serialize", serde(skip))] - feature1: PolyhedronFace, + feature1: PolyhedronFeature, #[cfg_attr(feature = "serde-serialize", serde(skip))] - feature2: PolyhedronFace, + feature2: PolyhedronFeature, } impl Default for PfmPfmContactManifoldGeneratorWorkspace { @@ -28,8 +31,8 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace { Self { simplex: VoronoiSimplex::new(), last_gjk_dir: None, - feature1: PolyhedronFace::new(), - feature2: PolyhedronFace::new(), + feature1: PolyhedronFeature::new(), + feature2: PolyhedronFeature::new(), } } } @@ -40,7 +43,7 @@ pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { ctxt.shape2.as_polygonal_feature_map(), ) { do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); ctxt.manifold.sort_contacts(ctxt.prediction_distance); } } @@ -73,10 +76,9 @@ fn do_generate_contacts( .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); let total_prediction = ctxt.prediction_distance + border_radius1 + border_radius2; - let contact = query::contact_support_map_support_map_with_params( - &Isometry::identity(), - pfm1, + let contact = query::contact::contact_support_map_support_map_with_params( &pos12, + pfm1, pfm2, total_prediction, &mut workspace.simplex, @@ -95,7 +97,7 @@ fn do_generate_contacts( pfm2.local_support_feature(&normal2, &mut workspace.feature2); workspace.feature2.transform_by(&pos12); - PolyhedronFace::contacts( + PolyhedronFeature::contacts( total_prediction, &workspace.feature1, &normal1, @@ -126,7 +128,7 @@ fn do_generate_contacts( } // Transfer impulses. - super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false); + ctxt.manifold.match_contacts(&old_manifold_points, false); } impl MaybeSerializableData for PfmPfmContactManifoldGeneratorWorkspace { diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 0e7543d..7e80fe7 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -1,10 +1,13 @@ #![allow(dead_code)] // TODO: remove this once we support polygons. use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon}; +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!() @@ -16,7 +19,7 @@ pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationC // &ctxt.position2, // ctxt.manifold, // ); - // ctxt.manifold.update_warmstart_multiplier(); + // ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); // } else { // unreachable!() // } @@ -75,12 +78,12 @@ fn generate_contacts<'a>( m12 * p2.vertices[support_face2], m12 * p2.vertices[(support_face2 + 1) % len2], ); - if let Some((clip_a, clip_b)) = clip_segments(seg1, seg2) { + 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 impulses_a = (0.0, Contact::zero_tangent_impulse()); - let mut impulses_b = (0.0, Contact::zero_tangent_impulse()); + 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, @@ -111,26 +114,15 @@ fn generate_contacts<'a>( } if fids_a == original_fids_a { - impulses_a = ( - manifold.points[0].impulse, - manifold.points[0].tangent_impulse, - ); + data_a = manifold.points[0].data; } else if fids_a == original_fids_b { - impulses_a = ( - manifold.points[1].impulse, - manifold.points[1].tangent_impulse, - ); + data_a = manifold.points[1].data; } + if fids_b == original_fids_a { - impulses_b = ( - manifold.points[0].impulse, - manifold.points[0].tangent_impulse, - ); + data_b = manifold.points[0].data; } else if fids_b == original_fids_b { - impulses_b = ( - manifold.points[1].impulse, - manifold.points[1].tangent_impulse, - ); + data_b = manifold.points[1].data; } } @@ -138,21 +130,19 @@ fn generate_contacts<'a>( manifold.points.push(Contact { local_p1: clip_a.0, local_p2: m21 * clip_a.1, - impulse: impulses_a.0, - tangent_impulse: impulses_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, - impulse: impulses_b.0, - tangent_impulse: impulses_b.1, fid1: fids_b.0, fid2: fids_b.1, dist: dist_b, + data: data_b, }); manifold.local_n1 = local_n1; @@ -164,138 +154,3 @@ fn generate_contacts<'a>( manifold.points.clear(); } } - -// Features in clipping points are: -// 0 = First vertex. -// 1 = On the face. -// 2 = Second vertex. -pub(crate) type ClippingPoints = (Point, Point, usize, usize); - -#[cfg(feature = "dim2")] -pub(crate) fn clip_segments_with_normal( - mut seg1: (Point, Point), - mut seg2: (Point, Point), - normal: Vector, -) -> Option<(ClippingPoints, ClippingPoints)> { - use crate::utils::WBasis; - let tangent = normal.orthonormal_basis()[0]; - - let mut range1 = [seg1.0.coords.dot(&tangent), seg1.1.coords.dot(&tangent)]; - let mut range2 = [seg2.0.coords.dot(&tangent), seg2.1.coords.dot(&tangent)]; - let mut features1 = [0, 2]; - let mut features2 = [0, 2]; - - if range1[1] < range1[0] { - range1.swap(0, 1); - features1.swap(0, 1); - std::mem::swap(&mut seg1.0, &mut seg1.1); - } - - if range2[1] < range2[0] { - range2.swap(0, 1); - features2.swap(0, 1); - std::mem::swap(&mut seg2.0, &mut seg2.1); - } - - if range2[0] > range1[1] || range1[0] > range2[1] { - // No clip point. - return None; - } - - let ca = if range2[0] > range1[0] { - let bcoord = (range2[0] - range1[0]) * utils::inv(range1[1] - range1[0]); - let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord; - let p2 = seg2.0; - - (p1, p2, 1, features2[0]) - } else { - let bcoord = (range1[0] - range2[0]) * utils::inv(range2[1] - range2[0]); - let p1 = seg1.0; - let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; - - (p1, p2, features1[0], 1) - }; - - let cb = if range2[1] < range1[1] { - let bcoord = (range2[1] - range1[0]) * utils::inv(range1[1] - range1[0]); - let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord; - let p2 = seg2.1; - - (p1, p2, 1, features2[1]) - } else { - let bcoord = (range1[1] - range2[0]) * utils::inv(range2[1] - range2[0]); - let p1 = seg1.1; - let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; - - (p1, p2, features1[1], 1) - }; - - Some((ca, cb)) -} - -pub(crate) fn clip_segments( - mut seg1: (Point, Point), - mut seg2: (Point, Point), -) -> Option<(ClippingPoints, ClippingPoints)> { - // NOTE: no need to normalize the tangent. - let tangent1 = seg1.1 - seg1.0; - let sqnorm_tangent1 = tangent1.norm_squared(); - - let mut range1 = [0.0, sqnorm_tangent1]; - let mut range2 = [ - (seg2.0 - seg1.0).dot(&tangent1), - (seg2.1 - seg1.0).dot(&tangent1), - ]; - let mut features1 = [0, 2]; - let mut features2 = [0, 2]; - - if range1[1] < range1[0] { - range1.swap(0, 1); - features1.swap(0, 1); - std::mem::swap(&mut seg1.0, &mut seg1.1); - } - - if range2[1] < range2[0] { - range2.swap(0, 1); - features2.swap(0, 1); - std::mem::swap(&mut seg2.0, &mut seg2.1); -