nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First public release of Rapier.

Browse Source
This commit is contained in:
Sébastien Crozet
2020-08-25 22:10:25 +02:00
commit 754a48b7ff
175 changed files with 32819 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

485
src/geometry/contact.rs Normal file
View File

@@ -0,0 +1,485 @@
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,
};
#[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,
) -> (
&'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);
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,
// We put the friction and restitution here because
// this 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 following are set by the constraints solver.
pub(crate) constraint_index: usize,
pub(crate) position_constraint_index: usize,
}
impl ContactManifold {
pub(crate) fn new(
pair: ColliderPair,
subshapes: (usize, usize),
body_pair: BodyPair,
friction: f32,
restitution: f32,
) -> 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,
constraint_index: 0,
position_constraint_index: 0,
}
}
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,
constraint_index: self.constraint_index,
position_constraint_index: self.position_constraint_index,
}
}
pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self {
Self::with_subshape_indices(pair, coll1, coll2, 0, 0)
}
pub(crate) fn with_subshape_indices(
pair: ColliderPair,
coll1: &Collider,
coll2: &Collider,
subshape1: usize,
subshape2: usize,
) -> Self {
Self::new(
pair,
(subshape1, subshape2),
BodyPair::new(coll1.parent, coll2.parent),
(coll1.friction + coll2.friction) * 0.5,
(coll1.restitution + coll2.restitution) * 0.5,
)
}
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);
}
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 {
if self.points.len() == 0 {
return false;
}
// const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
let local_n2 = pos12 * self.local_n2;
if -self.local_n1.dot(&local_n2) < 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;
let dist_threshold = 0.001; // FIXME: this should not be hard-coded.
if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_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;
}
}
}
}
}
}