diff options
Diffstat (limited to 'src/geometry')
34 files changed, 368 insertions, 2736 deletions
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<f32>) -> Point<i32> { (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() } -fn region_aabb(index: Point<i32>) -> AABB<f32> { +fn region_aabb(index: Point<i32>) -> 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<f32>) -> 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<f32>, 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<f32>, pool: &mut Vec<Self>) -> Self { + pub fn recycle_or_new(bounds: AABB, pool: &mut Vec<Self>) -> 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<f32>, + 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<f32>, - /// 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<f32>, b: Point<f32>, 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<f32>) -> 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<f32> { - 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<f32>) -> 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<f32> { - 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<f32> { - let rot = self.rotation_wrt_y(); - Isometry::from_parts(self.center().coords.into(), rot) - } -} - -impl SupportMap<f32> for Capsule { - fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> { - 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<Vector<f32>>) -> Point<f32> { - 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<f32> for Capsule { - fn toi_and_normal_with_ray( - &self, - m: &Isometry<f32>, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option<RayIntersection> { - 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<f32> for Capsule { - #[inline] - fn project_point( - &self, - m: &Isometry<f32>, - pt: &Point<f32>, - solid: bool, - ) -> PointProjection<f32> { - 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<f32>, - pt: &Point<f32>, - ) -> (PointProjection<f32>, 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<f32> { + pub fn compute_aabb(&self) -> AABB { self.shape.compute_aabb(&self.position) } - // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB<f32> { + // 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<SimdFloat>, @@ -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<f32>, - /// The contact point in the local-space of the second collider. - pub local_p2: Point<f32>, +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<f32>, - local_p2: Point<f32>, - 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<Contact>, - /// 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<f32>, - /// The contact normal of all the contacts of this manifold, expressed in the local space of the second collider. - pub local_n2: Vector<f32>, - /// 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<f32>, delta2: Isometry<f32>, 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<f32>) -> 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<f32>, - 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() |