532 lines
18 KiB
Rust
532 lines
18 KiB
Rust
use crate::dynamics::BodyPair;
|
|
use crate::geometry::contact_generator::ContactPhase;
|
|
use crate::geometry::{Collider, ColliderPair, ColliderSet};
|
|
use crate::math::{Isometry, Point, Vector};
|
|
use std::any::Any;
|
|
#[cfg(feature = "simd-is-enabled")]
|
|
use {
|
|
crate::math::{SimdFloat, 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_FORCES = 0b01;
|
|
}
|
|
}
|
|
|
|
#[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>,
|
|
pub local_p2: Point<SimdFloat>,
|
|
pub local_n1: Vector<SimdFloat>,
|
|
pub local_n2: Vector<SimdFloat>,
|
|
pub dist: SimdFloat,
|
|
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<f32>, Vector<f32>) {
|
|
let c = Contact {
|
|
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],
|
|
};
|
|
|
|
(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 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>,
|
|
/// 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],
|
|
/// 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,
|
|
}
|
|
}
|
|
|
|
#[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]
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
// 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))]
|
|
/// 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<ContactManifold>,
|
|
#[cfg_attr(feature = "serde-serialize", serde(skip))]
|
|
pub(crate) generator: Option<ContactPhase>,
|
|
#[cfg_attr(feature = "serde-serialize", serde(skip))]
|
|
pub(crate) generator_workspace: Option<Box<dyn Any + Send + Sync>>,
|
|
}
|
|
|
|
impl ContactPair {
|
|
pub(crate) fn new(
|
|
pair: ColliderPair,
|
|
generator: ContactPhase,
|
|
generator_workspace: Option<Box<dyn Any + Send + Sync>>,
|
|
) -> 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 Any + Send + Sync)>,
|
|
) {
|
|
let coll1 = &colliders[self.pair.collider1];
|
|
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);
|
|
}
|
|
|
|
// 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 {
|
|
(
|
|
coll1,
|
|
coll2,
|
|
manifold,
|
|
self.generator_workspace.as_mut().map(|w| &mut **w),
|
|
)
|
|
} else {
|
|
(
|
|
coll2,
|
|
coll1,
|
|
manifold,
|
|
self.generator_workspace.as_mut().map(|w| &mut **w),
|
|
)
|
|
}
|
|
}
|
|
}
|
|
|
|
#[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 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,
|
|
// 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),
|
|
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<f32>,
|
|
/// The relative position between the second collider and its parent at the time the
|
|
/// contact points were generated.
|
|
pub delta2: Isometry<f32>,
|
|
/// Flags used to control some aspects of the constraints solver for this contact manifold.
|
|
pub solver_flags: SolverFlags,
|
|
}
|
|
|
|
impl ContactManifold {
|
|
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 {
|
|
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,
|
|
delta1,
|
|
delta2,
|
|
constraint_index: 0,
|
|
position_constraint_index: 0,
|
|
solver_flags,
|
|
}
|
|
}
|
|
|
|
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)
|
|
}
|
|
|
|
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(),
|
|
(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
|
|
}
|
|
|
|
/// 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) {
|
|
// 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;
|
|
return;
|
|
}
|
|
|
|
for pt in &self.points {
|
|
if pt.impulse != 0.0 {
|
|
self.warmstart_multiplier = (self.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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|