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))]
pub enum KinematicsCategory {
PointPoint,
PlanePoint,
}
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct ContactKinematics {
pub category: KinematicsCategory,
pub radius1: f32,
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))]
pub struct Contact {
pub local_p1: Point<f32>,
pub local_p2: Point<f32>,
pub impulse: f32,
#[cfg(feature = "dim2")]
pub tangent_impulse: f32,
#[cfg(feature = "dim3")]
pub tangent_impulse: [f32; 2],
pub fid1: u8,
pub fid2: u8,
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;
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct ContactPair {
pub pair: ColliderPair,
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,
}
}
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);
}
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))]
pub struct ContactManifold {
#[cfg(feature = "dim2")]
pub(super) points: arrayvec::ArrayVec<[Contact; 2]>,
#[cfg(feature = "dim3")]
pub(super) points: Vec<Contact>,
pub num_active_contacts: usize,
pub local_n1: Vector<f32>,
pub local_n2: Vector<f32>,
pub kinematics: ContactKinematics,
pub body_pair: BodyPair,
pub pair: ColliderPair,
pub subshape_index_pair: (usize, usize),
pub(crate) warmstart_multiplier: f32,
pub(crate) constraint_index: usize,
pub(crate) position_constraint_index: usize,
pub friction: f32,
pub restitution: f32,
pub delta1: Isometry<f32>,
pub delta2: Isometry<f32>,
}
impl ContactManifold {
pub(crate) fn new(
pair: ColliderPair,
subshapes: (usize, usize),
body_pair: BodyPair,
delta1: Isometry<f32>,
delta2: Isometry<f32>,
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,
delta1,
delta2,
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,
delta1: self.delta1,
delta2: self.delta2,
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.position_wrt_parent(),
*coll2.position_wrt_parent(),
(coll1.friction + coll2.friction) * 0.5,
(coll1.restitution + coll2.restitution) * 0.5,
)
}
pub(crate) fn min_warmstart_multiplier() -> f32 {
0.01
}
#[inline]
pub fn num_active_contacts(&self) -> usize {
self.num_active_contacts
}
#[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]
}
#[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) {
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;
}
}
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 = 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 {
return false;
}
let new_local_p1 = local_p2 - self.local_n1 * dist;
let dist_threshold = 0.001;
if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold {
return false;
}
pt.dist = dist;
pt.local_p1 = new_local_p1;
}
true
}
#[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 {
self.points
.swap(self.num_active_contacts, first_inactive_index - 1);
first_inactive_index -= 1;
} else {
self.num_active_contacts += 1;
}
}
}
}
}
}