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

Move all the contact manifold computations out of Rapier.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-17 10:24:36 +01:00
parent cc6d1b9730
commit e231bacec6
49 changed files with 103 additions and 2511 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/data/arena.rs
View File

@@ -3,7 +3,7 @@
//! See https://github.com/fitzgen/generational-arena/blob/master/src/lib.rs.
//! This has been modified to have a fully deterministic deserialization (including for the order of
//! Index attribution after a deserialization of the arena.
use buckler::partitioning::IndexedData;
use eagl::partitioning::IndexedData;
use std::cmp;
use std::iter::{self, Extend, FromIterator, FusedIterator};
use std::mem;

137
src/data/hashmap.rs
View File

@@ -1,137 +0,0 @@
//! A hash-map that behaves deterministically when the
//! `enhanced-determinism` feature is enabled.
#[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))]
use indexmap::IndexMap as StdHashMap;
#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))]
use std::collections::HashMap as StdHashMap;
/// Serializes only the capacity of a hash-map instead of its actual content.
#[cfg(feature = "serde-serialize")]
pub fn serialize_hashmap_capacity<S: serde::Serializer, K, V, H: std::hash::BuildHasher>(
map: &StdHashMap<K, V, H>,
s: S,
) -> Result<S::Ok, S::Error> {
s.serialize_u64(map.capacity() as u64)
}
/// Creates a new hash-map with its capacity deserialized from `d`.
#[cfg(feature = "serde-serialize")]
pub fn deserialize_hashmap_capacity<
'de,
D: serde::Deserializer<'de>,
K,
V,
H: std::hash::BuildHasher + Default,
>(
d: D,
) -> Result<StdHashMap<K, V, H>, D::Error> {
struct CapacityVisitor;
impl<'de> serde::de::Visitor<'de> for CapacityVisitor {
type Value = u64;
fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(formatter, "an integer between 0 and 2^64")
}
fn visit_u64<E: serde::de::Error>(self, val: u64) -> Result<Self::Value, E> {
Ok(val)
}
}
let capacity = d.deserialize_u64(CapacityVisitor)? as usize;
Ok(StdHashMap::with_capacity_and_hasher(
capacity,
Default::default(),
))
}
/*
* FxHasher taken from rustc_hash, except that it does not depend on the pointer size.
*/
#[cfg(feature = "enhanced-determinism")]
pub type FxHashMap32<K, V> = indexmap::IndexMap<K, V, std::hash::BuildHasherDefault<FxHasher32>>;
#[cfg(feature = "enhanced-determinism")]
pub use {self::FxHashMap32 as HashMap, indexmap::map::Entry};
#[cfg(not(feature = "enhanced-determinism"))]
pub use {rustc_hash::FxHashMap as HashMap, std::collections::hash_map::Entry};
const K: u32 = 0x9e3779b9;
// Same as FxHasher, but with the guarantee that the internal hash is
// an u32 instead of something that depends on the platform.
pub struct FxHasher32 {
hash: u32,
}
impl Default for FxHasher32 {
#[inline]
fn default() -> FxHasher32 {
FxHasher32 { hash: 0 }
}
}
impl FxHasher32 {
#[inline]
fn add_to_hash(&mut self, i: u32) {
use std::ops::BitXor;
self.hash = self.hash.rotate_left(5).bitxor(i).wrapping_mul(K);
}
}
impl std::hash::Hasher for FxHasher32 {
#[inline]
fn write(&mut self, mut bytes: &[u8]) {
use std::convert::TryInto;
let read_u32 = |bytes: &[u8]| u32::from_ne_bytes(bytes[..4].try_into().unwrap());
let mut hash = FxHasher32 { hash: self.hash };
assert!(std::mem::size_of::<u32>() <= 8);
while bytes.len() >= std::mem::size_of::<u32>() {
hash.add_to_hash(read_u32(bytes) as u32);
bytes = &bytes[std::mem::size_of::<u32>()..];
}
if (std::mem::size_of::<u32>() > 4) && (bytes.len() >= 4) {
hash.add_to_hash(u32::from_ne_bytes(bytes[..4].try_into().unwrap()) as u32);
bytes = &bytes[4..];
}
if (std::mem::size_of::<u32>() > 2) && bytes.len() >= 2 {
hash.add_to_hash(u16::from_ne_bytes(bytes[..2].try_into().unwrap()) as u32);
bytes = &bytes[2..];
}
if (std::mem::size_of::<u32>() > 1) && bytes.len() >= 1 {
hash.add_to_hash(bytes[0] as u32);
}
self.hash = hash.hash;
}
#[inline]
fn write_u8(&mut self, i: u8) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u16(&mut self, i: u16) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u32(&mut self, i: u32) {
self.add_to_hash(i as u32);
}
#[inline]
fn write_u64(&mut self, i: u64) {
self.add_to_hash(i as u32);
self.add_to_hash((i >> 32) as u32);
}
#[inline]
fn write_usize(&mut self, i: usize) {
self.add_to_hash(i as u32);
}
#[inline]
fn finish(&self) -> u64 {
self.hash as u64
}
}

17
src/data/maybe_serializable_data.rs
View File

@@ -1,17 +0,0 @@
use downcast_rs::{impl_downcast, DowncastSync};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
/// Piece of data that may be serializable.
pub trait MaybeSerializableData: DowncastSync {
/// Convert this shape as a serializable entity.
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
None
}
/// Clones `self`.
fn clone_dyn(&self) -> Box<dyn MaybeSerializableData>;
}
impl_downcast!(sync MaybeSerializableData);

4
src/data/mod.rs
View File

@@ -1,11 +1,9 @@
//! Data structures modified with guaranteed deterministic behavior after deserialization.
pub use self::coarena::Coarena;
pub use self::maybe_serializable_data::MaybeSerializableData;
pub use eagl::utils::MaybeSerializableData;
pub mod arena;
mod coarena;
pub(crate) mod graph;
pub(crate) mod hashmap;
mod maybe_serializable_data;
pub mod pubsub;

2
src/dynamics/mod.rs
View File

@@ -9,7 +9,7 @@ pub use self::joint::{
};
pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder};
pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
pub use buckler::shape::MassProperties;
pub use eagl::shape::MassProperties;
// #[cfg(not(feature = "parallel"))]
pub(crate) use self::joint::JointGraphEdge;
pub(crate) use self::rigid_body::RigidBodyChanges;

2
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

@@ -13,7 +13,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix6, Vector6, U3};
#[cfg(feature = "dim2")]
use {
crate::utils::SdpMatrix3,
eagl::utils::SdpMatrix3,
na::{Matrix3, Vector3},
};

2
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -8,7 +8,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
#[cfg(feature = "dim2")]
use {
crate::utils::SdpMatrix2,
eagl::utils::SdpMatrix2,
na::{Matrix2, Vector2},
};

2
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
View File

@@ -12,7 +12,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3};
#[cfg(feature = "dim2")]
use {
crate::utils::SdpMatrix2,
eagl::utils::SdpMatrix2,
na::{Matrix2, Vector2},
};

8
src/geometry/broad_phase_multi_sap.rs
View File

@@ -1,10 +1,10 @@
use crate::data::hashmap::HashMap;
use crate::data::pubsub::Subscription;
use crate::dynamics::RigidBodySet;
use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider};
use crate::math::{Point, Vector, DIM};
use bit_vec::BitVec;
use buckler::bounding_volume::{BoundingVolume, AABB};
use eagl::bounding_volume::{BoundingVolume, AABB};
use eagl::utils::hashmap::HashMap;
use std::cmp::Ordering;
use std::ops::{Index, IndexMut};
@@ -477,8 +477,8 @@ pub struct BroadPhase {
#[cfg_attr(
feature = "serde-serialize",
serde(
serialize_with = "crate::data::hashmap::serialize_hashmap_capacity",
deserialize_with = "crate::data::hashmap::deserialize_hashmap_capacity"
serialize_with = "eagl::utils::hashmap::serialize_hashmap_capacity",
deserialize_with = "eagl::utils::hashmap::deserialize_hashmap_capacity"
)
)]
reporting: HashMap<(u32, u32), bool>, // Workspace

8
src/geometry/collider.rs
View File

@@ -1,13 +1,13 @@
use crate::buckler::shape::HalfSpace;
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
use crate::eagl::shape::HalfSpace;
use crate::geometry::InteractionGroups;
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use buckler::bounding_volume::AABB;
use buckler::shape::{
use eagl::bounding_volume::AABB;
use eagl::shape::{
Ball, Capsule, Cuboid, HeightField, Segment, Shape, ShapeType, TriMesh, Triangle,
};
#[cfg(feature = "dim3")]
use buckler::shape::{Cone, Cylinder, RoundCylinder};
use eagl::shape::{Cone, Cylinder, RoundCylinder};
use na::Point3;
use std::ops::Deref;
use std::sync::Arc;

103
src/geometry/contact_generator/ball_ball_contact_generator.rs
View File

@@ -1,103 +0,0 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Contact, ContactManifoldData, KinematicsCategory};
use crate::math::{Point, Vector};
#[cfg(feature = "simd-is-enabled")]
use {
crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd,
crate::geometry::{WBall, WContact},
crate::math::{Isometry, SimdReal, SIMD_WIDTH},
simba::simd::SimdValue,
};
#[cfg(feature = "simd-is-enabled")]
fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry<SimdReal>) -> WContact {
let dcenter = ball2.center - ball1.center;
let center_dist = dcenter.magnitude();
let normal = dcenter / center_dist;
WContact {
local_p1: ball1.center + normal * ball1.radius,
local_p2: pos21 * (ball2.center - normal * ball2.radius),
local_n1: normal,
local_n2: pos21 * -normal,
fid1: [0; SIMD_WIDTH],
fid2: [0; SIMD_WIDTH],
dist: center_dist - ball1.radius - ball2.radius,
}
}
#[cfg(feature = "simd-is-enabled")]
pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) {
let pos_ba = ctxt.positions2.inverse() * ctxt.positions1;
let radii_a =
SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
let radii_b =
SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]);
let wball_a = WBall::new(Point::origin(), radii_a);
let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b);
let contacts = generate_contacts_simd(&wball_a, &wball_b, &pos_ba);
for (i, manifold) in ctxt.manifolds.iter_mut().enumerate() {
// FIXME: compare the dist before extracting the contact.
let (contact, local_n1, local_n2) = contacts.extract(i);
if contact.dist <= ctxt.prediction_distance {
if manifold.points.len() != 0 {
manifold.points[0].copy_geometry_from(contact);
} else {
manifold.points.push(contact);
}
manifold.local_n1 = local_n1;
manifold.local_n2 = local_n2;
manifold.kinematics.category = KinematicsCategory::PointPoint;
manifold.kinematics.radius1 = radii_a.extract(i);
manifold.kinematics.radius2 = radii_b.extract(i);
ContactManifoldData::update_warmstart_multiplier(manifold);
} else {
manifold.points.clear();
}
manifold.sort_contacts(ctxt.prediction_distance);
}
}
pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) {
let pos_ba = ctxt.position2.inverse() * ctxt.position1;
let radius_a = ctxt.shape1.as_ball().unwrap().radius;
let radius_b = ctxt.shape2.as_ball().unwrap().radius;
let dcenter = pos_ba.inverse_transform_point(&Point::origin()).coords;
let center_dist = dcenter.magnitude();
let dist = center_dist - radius_a - radius_b;
if dist < ctxt.prediction_distance {
let local_n1 = if center_dist != 0.0 {
dcenter / center_dist
} else {
Vector::y()
};
let local_n2 = pos_ba.inverse_transform_vector(&-local_n1);
let local_p1 = local_n1 * radius_a;
let local_p2 = local_n2 * radius_b;
let contact = Contact::new(local_p1.into(), local_p2.into(), 0, 0, dist);
if ctxt.manifold.points.len() != 0 {
ctxt.manifold.points[0].copy_geometry_from(contact);
} else {
ctxt.manifold.points.push(contact);
}
ctxt.manifold.local_n1 = local_n1;
ctxt.manifold.local_n2 = local_n2;
ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint;
ctxt.manifold.kinematics.radius1 = radius_a;
ctxt.manifold.kinematics.radius2 = radius_b;
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
} else {
ctxt.manifold.points.clear();
}
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}

70
src/geometry/contact_generator/ball_convex_contact_generator.rs
View File

@@ -1,70 +0,0 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Ball, Contact, ContactManifoldData, KinematicsCategory};
use crate::math::Isometry;
use buckler::query::PointQuery;
use na::Unit;
pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) {
if let Some(ball1) = ctxt.shape1.as_ball() {
ctxt.manifold.swap_identifiers();
do_generate_contacts(ctxt.shape2, ball1, ctxt, true);
} else if let Some(ball2) = ctxt.shape2.as_ball() {
do_generate_contacts(ctxt.shape1, ball2, ctxt, false);
}
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
fn do_generate_contacts<P: ?Sized + PointQuery>(
point_query1: &P,
ball2: &Ball,
ctxt: &mut PrimitiveContactGenerationContext,
swapped: bool,
) {
let position1;
let position2;
if swapped {
position1 = ctxt.position2;
position2 = ctxt.position1;
} else {
position1 = ctxt.position1;
position2 = ctxt.position2;
}
let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into());
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) {
#[cfg(feature = "dim2")]
if proj.is_inside {
local_n1 = -local_n1;
dist = -dist;
}
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);
if ctxt.manifold.points.len() != 1 {
ctxt.manifold.points.clear();
ctxt.manifold.points.push(contact_point);
} else {
// Copy only the geometry so we keep the warmstart impulses.
ctxt.manifold.points[0].copy_geometry_from(contact_point);
}
ctxt.manifold.local_n1 = *local_n1;
ctxt.manifold.local_n2 = local_n2;
ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint;
ctxt.manifold.kinematics.radius1 = 0.0;
ctxt.manifold.kinematics.radius2 = ball2.radius;
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
} else {
ctxt.manifold.points.clear();
}
}
}

1
src/geometry/contact_generator/ball_polygon_contact_generator.rs
View File

@@ -1 +0,0 @@

202
src/geometry/contact_generator/capsule_capsule_contact_generator.rs
View File

@@ -1,202 +0,0 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Capsule, Contact, ContactManifold, ContactManifoldData, KinematicsCategory};
use crate::math::Isometry;
use crate::math::Vector;
use approx::AbsDiffEq;
#[cfg(feature = "dim2")]
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()) {
generate_contacts(
ctxt.prediction_distance,
capsule1,
ctxt.position1,
capsule2,
ctxt.position2,
ctxt.manifold,
);
}
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
#[cfg(feature = "dim2")]
pub fn generate_contacts<'a>(
prediction_distance: f32,
capsule1: &'a Capsule,
pos1: &'a Isometry<f32>,
capsule2: &'a Capsule,
pos2: &'a Isometry<f32>,
manifold: &mut ContactManifold,
) {
// FIXME: the contact kinematics is not correctly set here.
// We use the common "Point-Plane" kinematics with zero radius everytime.
// Instead we should select point/point ore point-plane (with non-zero
// radius for the point) depending on the features involved in the contact.
let pos12 = pos1.inverse() * pos2;
let pos21 = pos12.inverse();
let seg1 = capsule1.segment;
let seg2_1 = capsule2.segment.transformed(&pos12);
let (loc1, loc2) = buckler::query::details::closest_points_segment_segment_with_locations_nD(
(&seg1.a, &seg1.b),
(&seg2_1.a, &seg2_1.b),
);
// We do this clone to perform contact tracking and transfer impulses.
// FIXME: find a more efficient way of doing this.
let old_manifold_points = manifold.points.clone();
manifold.points.clear();
let swapped = false;
let fid1 = if let SegmentPointLocation::OnVertex(v1) = loc1 {
v1 as u8 * 2
} else {
1
};
let fid2 = if let SegmentPointLocation::OnVertex(v2) = loc2 {
v2 as u8 * 2
} else {
1
};
let bcoords1 = loc1.barycentric_coordinates();
let bcoords2 = loc2.barycentric_coordinates();
let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
let local_n1 =
Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());
let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius;
if dist <= prediction_distance {
let local_n2 = pos21 * -local_n1;
let contact = Contact::new(local_p1, pos21 * local_p2, fid1, fid2, dist);
manifold.points.push(contact);
manifold.local_n1 = *local_n1;
manifold.local_n2 = *local_n2;
manifold.kinematics.category = KinematicsCategory::PlanePoint;
manifold.kinematics.radius1 = 0.0;
manifold.kinematics.radius2 = 0.0;
} else {
// No contact within tolerance.
return;
}
if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.direction()) {
if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8
&& dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8
{
// Capsules axes are almost parallel and are almost perpendicular to the normal.
// Find a second contact point.
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.
Contact::new(
clip_a.0,
pos21 * clip_a.1,
clip_a.2 as u8,
clip_a.3 as u8,
(clip_a.1 - clip_a.0).dot(&local_n1),
)
} else {
// Use clip_b as the second contact.
Contact::new(
clip_b.0,
pos21 * clip_b.1,
clip_b.2 as u8,
clip_b.3 as u8,
(clip_b.1 - clip_b.0).dot(&local_n1),
)
};
manifold.points.push(contact);
}
}
}
if swapped {
for point in &mut manifold.points {
point.local_p1 += manifold.local_n1 * capsule2.radius;
point.local_p2 += manifold.local_n2 * capsule1.radius;
point.dist -= capsule1.radius + capsule2.radius;
}
} else {
for point in &mut manifold.points {
point.local_p1 += manifold.local_n1 * capsule1.radius;
point.local_p2 += manifold.local_n2 * capsule2.radius;
point.dist -= capsule1.radius + capsule2.radius;
}
}
manifold.match_contacts(&old_manifold_points, swapped);
}
#[cfg(feature = "dim3")]
pub fn generate_contacts<'a>(
prediction_distance: f32,
capsule1: &'a Capsule,
pos1: &'a Isometry<f32>,
capsule2: &'a Capsule,
pos2: &'a Isometry<f32>,
manifold: &mut ContactManifold,
) {
let pos12 = pos1.inverse() * pos2;
let pos21 = pos12.inverse();
let seg1 = capsule1.segment;
let seg2_1 = capsule2.segment.transformed(&pos12);
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();
let bcoords2 = loc2.barycentric_coordinates();
let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
let local_n1 =
Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());
let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius;
if dist <= prediction_distance {
let local_n2 = pos21 * -local_n1;
let contact = Contact::new(
local_p1 + *local_n1 * capsule1.radius,
pos21 * local_p2 + *local_n2 * capsule2.radius,
0,
0,
dist,
);
if manifold.points.len() != 0 {
manifold.points[0].copy_geometry_from(contact);
} else {
manifold.points.push(contact);
}
manifold.local_n1 = *local_n1;
manifold.local_n2 = *local_n2;
manifold.kinematics.category = KinematicsCategory::PlanePoint;
manifold.kinematics.radius1 = 0.0;
manifold.kinematics.radius2 = 0.0;
} else {
manifold.points.clear();
}
}
}

141
src/geometry/contact_generator/contact_dispatcher.rs
View File

@@ -1,141 +0,0 @@
#[cfg(feature = "dim3")]
use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
use crate::geometry::contact_generator::{
ContactGenerator, ContactGeneratorWorkspace, ContactPhase,
HeightFieldShapeContactGeneratorWorkspace, PrimitiveContactGenerator,
TriMeshShapeContactGeneratorWorkspace,
};
use buckler::shape::ShapeType;
/// Trait implemented by structures responsible for selecting a collision-detection algorithm
/// for a given pair of shapes.
pub trait ContactDispatcher {
/// Select the collision-detection algorithm for the given pair of primitive shapes.
fn dispatch_primitives(
&self,
shape1: ShapeType,
shape2: ShapeType,
) -> (PrimitiveContactGenerator, Option<ContactGeneratorWorkspace>);
/// Select the collision-detection algorithm for the given pair of non-primitive shapes.
fn dispatch(
&self,
shape1: ShapeType,
shape2: ShapeType,
) -> (ContactPhase, Option<ContactGeneratorWorkspace>);
}
/// The default contact dispatcher used by Rapier.
pub struct DefaultContactDispatcher;
impl ContactDispatcher for DefaultContactDispatcher {
fn dispatch_primitives(
&self,
shape1: ShapeType,
shape2: ShapeType,
) -> (PrimitiveContactGenerator, Option<ContactGeneratorWorkspace>) {
match (shape1, shape2) {
(ShapeType::Ball, ShapeType::Ball) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_ball_ball,
#[cfg(feature = "simd-is-enabled")]
generate_contacts_simd: super::generate_contacts_ball_ball_simd,
..PrimitiveContactGenerator::default()
},
None,
),
(ShapeType::Cuboid, ShapeType::Cuboid) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_cuboid_cuboid,
..PrimitiveContactGenerator::default()
},
None,
),
// (ShapeType::Polygon, ShapeType::Polygon) => (
// PrimitiveContactGenerator {
// generate_contacts: super::generate_contacts_polygon_polygon,
// ..PrimitiveContactGenerator::default()
// },
// None,
// ),
(ShapeType::Capsule, ShapeType::Capsule) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_capsule_capsule,
..PrimitiveContactGenerator::default()
},
None,
),
(_, ShapeType::Ball) | (ShapeType::Ball, _) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_ball_convex,
..PrimitiveContactGenerator::default()
},
None,
),
(ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_cuboid_capsule,
..PrimitiveContactGenerator::default()
},
None,
),
(ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => {
(
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_cuboid_triangle,
..PrimitiveContactGenerator::default()
},
None,
)
}
#[cfg(feature = "dim3")]
(ShapeType::Cylinder, _)
| (_, ShapeType::Cylinder)
| (ShapeType::Cone, _)
| (_, ShapeType::Cone)
| (ShapeType::RoundCylinder, _)
| (_, ShapeType::RoundCylinder)
| (ShapeType::Capsule, _)
| (_, ShapeType::Capsule) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_pfm_pfm,
..PrimitiveContactGenerator::default()
},
Some(ContactGeneratorWorkspace::from(
PfmPfmContactManifoldGeneratorWorkspace::default(),
)),
),
_ => (PrimitiveContactGenerator::default(), None),
}
}
fn dispatch(
&self,
shape1: ShapeType,
shape2: ShapeType,
) -> (ContactPhase, Option<ContactGeneratorWorkspace>) {
match (shape1, shape2) {
(ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => (
ContactPhase::NearPhase(ContactGenerator {
generate_contacts: super::generate_contacts_trimesh_shape,
..ContactGenerator::default()
}),
Some(ContactGeneratorWorkspace::from(
TriMeshShapeContactGeneratorWorkspace::new(),
)),
),
(ShapeType::HeightField, _) | (_, ShapeType::HeightField) => (
ContactPhase::NearPhase(ContactGenerator {
generate_contacts: super::generate_contacts_heightfield_shape,
..ContactGenerator::default()
}),
Some(ContactGeneratorWorkspace::from(
HeightFieldShapeContactGeneratorWorkspace::new(),
)),
),
_ => {
let (gen, workspace) = self.dispatch_primitives(shape1, shape2);
(ContactPhase::ExactPhase(gen), workspace)
}
}
}
}

228
src/geometry/contact_generator/contact_generator.rs
View File

@@ -1,228 +0,0 @@
use crate::data::MaybeSerializableData;
use crate::geometry::{
Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape,
ShapeType, SolverFlags,
};
use crate::math::Isometry;
#[cfg(feature = "simd-is-enabled")]
use crate::math::{SimdReal, SIMD_WIDTH};
use crate::pipeline::EventHandler;
#[derive(Copy, Clone)]
pub enum ContactPhase {
NearPhase(ContactGenerator),
ExactPhase(PrimitiveContactGenerator),
}
impl ContactPhase {
#[inline]
pub fn generate_contacts(
self,
mut context: ContactGenerationContext,
events: &dyn EventHandler,
) {
let had_contacts_before = context.pair.has_any_active_contact();
match self {
Self::NearPhase(gen) => (gen.generate_contacts)(&mut context),
Self::ExactPhase(gen) => {
// Build the primitive context from the non-primitive context and dispatch.
let (collider1, collider2, manifold, workspace) = context
.pair
.single_manifold(context.colliders, context.solver_flags);
let mut context2 = PrimitiveContactGenerationContext {
prediction_distance: context.prediction_distance,
collider1,
collider2,
shape1: collider1.shape(),
shape2: collider2.shape(),
position1: collider1.position(),
position2: collider2.position(),
manifold,
workspace,
};
(gen.generate_contacts)(&mut context2)
}
}
if had_contacts_before != context.pair.has_any_active_contact() {
if had_contacts_before {
events.handle_contact_event(ContactEvent::Stopped(
context.pair.pair.collider1,
context.pair.pair.collider2,
));
} else {
events.handle_contact_event(ContactEvent::Started(
context.pair.pair.collider1,
context.pair.pair.collider2,
))
}
}
}
#[cfg(feature = "simd-is-enabled")]
#[inline]
pub fn generate_contacts_simd(
self,
mut context: ContactGenerationContextSimd,
events: &dyn EventHandler,
) {
let mut had_contacts_before = [false; SIMD_WIDTH];
for (i, pair) in context.pairs.iter().enumerate() {
had_contacts_before[i] = pair.has_any_active_contact()
}
match self {
Self::NearPhase(gen) => (gen.generate_contacts_simd)(&mut context),
Self::ExactPhase(gen) => {
// Build the primitive context from the non-primitive context and dispatch.
use arrayvec::ArrayVec;
let mut colliders_arr: ArrayVec<[(&Collider, &Collider); SIMD_WIDTH]> =
ArrayVec::new();
let mut manifold_arr: ArrayVec<[&mut ContactManifold; SIMD_WIDTH]> =
ArrayVec::new();
let mut workspace_arr: ArrayVec<
[Option<&mut (dyn MaybeSerializableData)>; SIMD_WIDTH],
> = ArrayVec::new();
for (pair, solver_flags) in
context.pairs.iter_mut().zip(context.solver_flags.iter())
{
let (collider1, collider2, manifold, workspace) =
pair.single_manifold(context.colliders, *solver_flags);
colliders_arr.push((collider1, collider2));
manifold_arr.push(manifold);
workspace_arr.push(workspace);
}
let max_index = colliders_arr.len() - 1;
let colliders1 = array![|ii| colliders_arr[ii.min(max_index)].0; SIMD_WIDTH];
let colliders2 = array![|ii| colliders_arr[ii.min(max_index)].1; SIMD_WIDTH];
let mut context2 = PrimitiveContactGenerationContextSimd {
prediction_distance: context.prediction_distance,
colliders1,
colliders2,
shapes1: array![|ii| colliders1[ii].shape(); SIMD_WIDTH],
shapes2: array![|ii| colliders2[ii].shape(); SIMD_WIDTH],
positions1: &Isometry::from(
array![|ii| *colliders1[ii].position(); SIMD_WIDTH],
),
positions2: &Isometry::from(
array![|ii| *colliders2[ii].position(); SIMD_WIDTH],
),
manifolds: manifold_arr.as_mut_slice(),
workspaces: workspace_arr.as_mut_slice(),
};
(gen.generate_contacts_simd)(&mut context2)
}
}
for (i, pair) in context.pairs.iter().enumerate() {
if had_contacts_before[i] != pair.has_any_active_contact() {
if had_contacts_before[i] {
events.handle_contact_event(ContactEvent::Stopped(
pair.pair.collider1,
pair.pair.collider2,
))
} else {
events.handle_contact_event(ContactEvent::Started(
pair.pair.collider1,
pair.pair.collider2,
))
}
}
}
}
}
pub struct PrimitiveContactGenerationContext<'a> {
pub prediction_distance: f32,
pub collider1: &'a Collider,
pub collider2: &'a Collider,
pub shape1: &'a dyn Shape,
pub shape2: &'a dyn Shape,
pub position1: &'a Isometry<f32>,
pub position2: &'a Isometry<f32>,
pub manifold: &'a mut ContactManifold,
pub workspace: Option<&'a mut (dyn MaybeSerializableData)>,
}
#[cfg(feature = "simd-is-enabled")]
pub struct PrimitiveContactGenerationContextSimd<'a, 'b> {
pub prediction_distance: f32,
pub colliders1: [&'a Collider; SIMD_WIDTH],
pub colliders2: [&'a Collider; SIMD_WIDTH],
pub shapes1: [&'a dyn Shape; SIMD_WIDTH],
pub shapes2: [&'a dyn Shape; SIMD_WIDTH],
pub positions1: &'a Isometry<SimdReal>,
pub positions2: &'a Isometry<SimdReal>,
pub manifolds: &'a mut [&'b mut ContactManifold],
pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>],
}
#[derive(Copy, Clone)]
pub struct PrimitiveContactGenerator {
pub generate_contacts: fn(&mut PrimitiveContactGenerationContext),
#[cfg(feature = "simd-is-enabled")]
pub generate_contacts_simd: fn(&mut PrimitiveContactGenerationContextSimd),
}
impl PrimitiveContactGenerator {
fn unimplemented_fn(_ctxt: &mut PrimitiveContactGenerationContext) {}
#[cfg(feature = "simd-is-enabled")]
fn unimplemented_simd_fn(_ctxt: &mut PrimitiveContactGenerationContextSimd) {}
}
impl Default for PrimitiveContactGenerator {
fn default() -> Self {
Self {
generate_contacts: Self::unimplemented_fn,
#[cfg(feature = "simd-is-enabled")]
generate_contacts_simd: Self::unimplemented_simd_fn,
}
}
}
pub struct ContactGenerationContext<'a> {
pub dispatcher: &'a dyn ContactDispatcher,
pub prediction_distance: f32,
pub colliders: &'a ColliderSet,
pub pair: &'a mut ContactPair,
pub solver_flags: SolverFlags,
}
#[cfg(feature = "simd-is-enabled")]
pub struct ContactGenerationContextSimd<'a, 'b> {
pub dispatcher: &'a dyn ContactDispatcher,
pub prediction_distance: f32,
pub colliders: &'a ColliderSet,
pub pairs: &'a mut [&'b mut ContactPair],
pub solver_flags: &'a [SolverFlags],
}
#[derive(Copy, Clone)]
pub struct ContactGenerator {
pub generate_contacts: fn(&mut ContactGenerationContext),
#[cfg(feature = "simd-is-enabled")]
pub generate_contacts_simd: fn(&mut ContactGenerationContextSimd),
}
impl ContactGenerator {
fn unimplemented_fn(_ctxt: &mut ContactGenerationContext) {}
#[cfg(feature = "simd-is-enabled")]
fn unimplemented_simd_fn(_ctxt: &mut ContactGenerationContextSimd) {}
}
impl Default for ContactGenerator {
fn default() -> Self {
Self {
generate_contacts: Self::unimplemented_fn,
#[cfg(feature = "simd-is-enabled")]
generate_contacts_simd: Self::unimplemented_simd_fn,
}
}
}

104
src/geometry/contact_generator/contact_generator_workspace.rs
View File

@@ -1,104 +0,0 @@
use crate::data::MaybeSerializableData;
#[cfg(feature = "dim3")]
use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
use crate::geometry::contact_generator::{
HeightFieldShapeContactGeneratorWorkspace, TriMeshShapeContactGeneratorWorkspace,
WorkspaceSerializationTag,
};
// Note we have this newtype because it simplifies the serialization/deserialization code.
pub struct ContactGeneratorWorkspace(pub Box<dyn MaybeSerializableData>);
impl Clone for ContactGeneratorWorkspace {
fn clone(&self) -> Self {
ContactGeneratorWorkspace(self.0.clone_dyn())
}
}
impl<T: MaybeSerializableData> From<T> for ContactGeneratorWorkspace {
fn from(data: T) -> Self {
Self(Box::new(data) as Box<dyn MaybeSerializableData>)
}
}
#[cfg(feature = "serde-serialize")]
impl serde::Serialize for ContactGeneratorWorkspace {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: serde::Serializer,
{
use crate::serde::ser::SerializeStruct;
if let Some((tag, ser)) = self.0.as_serialize() {
let mut state = serializer.serialize_struct("ContactGeneratorWorkspace", 2)?;
state.serialize_field("tag", &tag)?;
state.serialize_field("inner", ser)?;
state.end()
} else {
Err(serde::ser::Error::custom(
"Found a non-serializable contact generator workspace.",
))
}
}
}
#[cfg(feature = "serde-serialize")]
impl<'de> serde::Deserialize<'de> for ContactGeneratorWorkspace {
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where
D: serde::Deserializer<'de>,
{
struct Visitor {};
impl<'de> serde::de::Visitor<'de> for Visitor {
type Value = ContactGeneratorWorkspace;
fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(formatter, "one shape type tag and the inner shape data")
}
fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
use num::cast::FromPrimitive;
let tag: u32 = seq
.next_element()?
.ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
fn deser<'de, A, S: MaybeSerializableData + serde::Deserialize<'de>>(
seq: &mut A,
) -> Result<Box<dyn MaybeSerializableData>, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
let workspace: S = seq.next_element()?.ok_or_else(|| {
serde::de::Error::custom("Failed to deserialize builtin workspace.")
})?;
Ok(Box::new(workspace) as Box<dyn MaybeSerializableData>)
}
let workspace = match WorkspaceSerializationTag::from_u32(tag) {
Some(WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace) => {
deser::<A, HeightFieldShapeContactGeneratorWorkspace>(&mut seq)?
}
Some(WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace) => {
deser::<A, TriMeshShapeContactGeneratorWorkspace>(&mut seq)?
}
#[cfg(feature = "dim3")]
Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => {
deser::<A, PfmPfmContactManifoldGeneratorWorkspace>(&mut seq)?
}
None => {
return Err(serde::de::Error::custom(
"found invalid contact generator workspace type to deserialize",
))
}
};
Ok(ContactGeneratorWorkspace(workspace))
}
}
deserializer.deserialize_struct("ContactGeneratorWorkspace", &["tag", "inner"], Visitor {})
}
}

190
src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
View File

@@ -1,190 +0,0 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
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()) {
generate_contacts(
ctxt.prediction_distance,
cube1,
ctxt.position1,
capsule2,
ctxt.position2,
ctxt.manifold,
false,
);
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
} else if let (Some(capsule1), Some(cube2)) =
(ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid())
{
generate_contacts(
ctxt.prediction_distance,
cube2,
ctxt.position2,
capsule1,
ctxt.position1,
ctxt.manifold,
true,
);
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
}
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
pub fn generate_contacts<'a>(
prediction_distance: f32,
cube1: &'a Cuboid,
mut pos1: &'a Isometry<f32>,
capsule2: &'a Capsule,
mut pos2: &'a Isometry<f32>,
manifold: &mut ContactManifold,
swapped: bool,
) {
let mut pos12 = pos1.inverse() * pos2;
let mut pos21 = pos12.inverse();
if (!swapped && manifold.try_update_contacts(&pos12))
|| (swapped && manifold.try_update_contacts(&pos21))
{
return;
}
let segment2 = capsule2.segment;
/*
*
* Point-Face cases.
*
*/
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;
}
#[cfg(feature = "dim3")]
let sep2 = (-f32::MAX, Vector::x());
#[cfg(feature = "dim2")]
let sep2 = sat::segment_cuboid_find_local_separating_normal_oneway(&segment2, cube1, &pos21);
if sep2.0 > capsule2.radius + prediction_distance {
manifold.points.clear();
return;
}
/*
*
* Edge-Edge cases.
*
*/
#[cfg(feature = "dim2")]
let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D.
#[cfg(feature = "dim3")]
let sep3 = sat::cuboid_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12);
if sep3.0 > capsule2.radius + prediction_distance {
manifold.points.clear();
return;
}
/*
*
* Select the best combination of features
* and get the polygons to clip.
*
*/
let mut swapped_reference = false;
let mut best_sep = sep1;
if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
// The reference shape will be the second shape.
// std::mem::swap(&mut cube1, &mut capsule2);
std::mem::swap(&mut pos1, &mut pos2);
std::mem::swap(&mut pos12, &mut pos21);
best_sep = sep2;
swapped_reference = true;
} else if sep3.0 > sep1.0 {
best_sep = sep3;
}
let feature1;
let mut feature2;
#[cfg(feature = "dim2")]
{
if swapped_reference {
feature1 = CuboidFeatureFace::from(segment2);
feature2 = cube1.support_face(pos21 * -best_sep.1);
} else {
feature1 = cube1.support_face(best_sep.1);
feature2 = CuboidFeatureFace::from(segment2);
}
}
#[cfg(feature = "dim3")]
{
if swapped_reference {
feature1 = PolyhedronFeature::from(segment2);
feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1);
} else {
feature1 = cube1.polyhedron_support_face(best_sep.1);
feature2 = PolyhedronFeature::from(segment2);
}
}
feature2.transform_by(&pos12);
if swapped ^ swapped_reference {
manifold.swap_identifiers();
}
// We do this clone to perform contact tracking and transfer impulses.
// FIXME: find a more efficient way of doing this.
let old_manifold_points = manifold.points.clone();
manifold.points.clear();
#[cfg(feature = "dim2")]
CuboidFeature::face_face_contacts(
prediction_distance + capsule2.radius,
&feature1,
&best_sep.1,
&feature2,
&pos21,
manifold,
);
#[cfg(feature = "dim3")]
PolyhedronFeature::contacts(
prediction_distance + capsule2.radius,
&feature1,
&best_sep.1,
&feature2,
&pos21,
manifold,
);
// Adjust points to take the radius into account.
manifold.local_n1 = best_sep.1;
manifold.local_n2 = pos21 * -best_sep.1;
manifold.kinematics.category = KinematicsCategory::PlanePoint;
manifold.kinematics.radius1 = 0.0;
manifold.kinematics.radius2 = 0.0;
if swapped_reference {
for point in &mut manifold.points {
point.local_p1 += manifold.local_n1 * capsule2.radius;
point.dist -= capsule2.radius;
}
} else {
for point in &mut manifold.points {
point.local_p2 += manifold.local_n2 * capsule2.radius;
point.dist -= capsule2.radius;
}
}
// Transfer impulses.
manifold.match_contacts(&old_manifold_points, swapped ^ swapped_reference);
}

156
src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs
View File

@@ -1,156 +0,0 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{ContactManifold, ContactManifoldData, KinematicsCategory};
use crate::math::Isometry;
#[cfg(feature = "dim2")]
use crate::math::Vector;
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()) {
generate_contacts(
ctxt.prediction_distance,
cube1,
ctxt.position1,
cube2,
ctxt.position2,
ctxt.manifold,
);
} else {
unreachable!()
}
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 pos1: &'a Isometry<f32>,
mut cube2: &'a Cuboid,
mut pos2: &'a Isometry<f32>,
manifold: &mut ContactManifold,
) {
let mut pos12 = pos1.inverse() * pos2;
let mut pos21 = pos12.inverse();
if manifold.try_update_contacts(&pos12) {
return;
}
/*
*
* Point-Face
*
*/
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);
if sep2.0 > prediction_distance {
manifold.points.clear();
return;
}
/*
*
* Edge-Edge cases
*
*/
#[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);
if sep3.0 > prediction_distance {
manifold.points.clear();
return;
}
/*
*
* Select the best combination of features
* and get the polygons to clip.
*
*/
let mut swapped = false;
let mut best_sep = sep1;
if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
// The reference shape will be the second shape.
std::mem::swap(&mut cube1, &mut cube2);
std::mem::swap(&mut pos1, &mut pos2);
std::mem::swap(&mut pos12, &mut pos21);
manifold.swap_identifiers();
best_sep = sep2;
swapped = true;
} else if sep3.0 > sep1.0 {
best_sep = sep3;
}
// We do this clone to perform contact tracking and transfer impulses.
// FIXME: find a more efficient way of doing this.
let old_manifold_points = manifold.points.clone();
manifold.points.clear();
// 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 = cube1.support_feature(best_sep.1);
let mut feature2 = cube2.support_feature(pos21 * -best_sep.1);
feature2.transform_by(&pos12);
match (&feature1, &feature2) {
(CuboidFeature::Face(f1), CuboidFeature::Vertex(v2)) => {
CuboidFeature::face_vertex_contacts(f1, &best_sep.1, v2, &pos21, manifold)
}
#[cfg(feature = "dim3")]
(CuboidFeature::Face(f1), CuboidFeature::Edge(e2)) => CuboidFeature::face_edge_contacts(
prediction_distance,
f1,
&best_sep.1,
e2,
&pos21,
manifold,
false,
),
(CuboidFeature::Face(f1), CuboidFeature::Face(f2)) => CuboidFeature::face_face_contacts(
prediction_distance,
f1,
&best_sep.1,
f2,
&pos21,
manifold,
),
#[cfg(feature = "dim3")]
(CuboidFeature::Edge(e1), CuboidFeature::Edge(e2)) => {
CuboidFeature::edge_edge_contacts(e1, &best_sep.1, e2, &pos21, manifold)
}
#[cfg(feature = "dim3")]
(CuboidFeature::Edge(e1), CuboidFeature::Face(f2)) => {
// Since f2 is also expressed in the local-space of the first
// feature, the position we provide here is pos21.
CuboidFeature::face_edge_contacts(
prediction_distance,
f2,
&-best_sep.1,
e1,
&pos21,
manifold,
true,
)
}
_ => unreachable!(), // The other cases are not possible.
}
manifold.local_n1 = best_sep.1;
manifold.local_n2 = pos21 * -best_sep.1;
manifold.kinematics.category = KinematicsCategory::PlanePoint;
manifold.kinematics.radius1 = 0.0;
manifold.kinematics.radius2 = 0.0;
// Transfer impulses.
manifold.match_contacts(&old_manifold_points, swapped);
}

1
src/geometry/contact_generator/cuboid_polygon_contact_generator.rs
View File

@@ -1 +0,0 @@

171
src/geometry/contact_generator/cuboid_triangle_contact_generator.rs
View File

@@ -1,171 +0,0 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{ContactManifold, ContactManifoldData, Cuboid, KinematicsCategory, Triangle};
use crate::math::Isometry;
#[cfg(feature = "dim2")]
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()) {
generate_contacts(
ctxt.prediction_distance,
cube1,
ctxt.position1,
triangle2,
ctxt.position2,
ctxt.manifold,
false,
);
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
} else if let (Some(triangle1), Some(cube2)) =
(ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid())
{
generate_contacts(
ctxt.prediction_distance,
cube2,
ctxt.position2,
triangle1,
ctxt.position1,
ctxt.manifold,
true,
);
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
}
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
pub fn generate_contacts<'a>(
prediction_distance: f32,
cube1: &'a Cuboid,
mut pos1: &'a Isometry<f32>,
triangle2: &'a Triangle,
mut pos2: &'a Isometry<f32>,
manifold: &mut ContactManifold,
swapped: bool,
) {
let mut pos12 = pos1.inverse() * pos2;
let mut pos21 = pos12.inverse();
if (!swapped && manifold.try_update_contacts(&pos12))
|| (swapped && manifold.try_update_contacts(&pos21))
{
return;
}
/*
*
* Point-Face cases.
*
*/
let sep1 =
sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12);
if sep1.0 > prediction_distance {
manifold.points.clear();
return;
}
let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cube1, &pos21);
if sep2.0 > prediction_distance {
manifold.points.clear();
return;
}
/*
*
* Edge-Edge cases.
*
*/
#[cfg(feature = "dim2")]
let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D.
#[cfg(feature = "dim3")]
let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12);
if sep3.0 > prediction_distance {
manifold.points.clear();
return;
}
/*
*
* Select the best combination of features
* and get the polygons to clip.
*
*/
let mut swapped_reference = false;
let mut best_sep = sep1;
if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
// The reference shape will be the second shape.
// std::mem::swap(&mut cube1, &mut triangle2);
std::mem::swap(&mut pos1, &mut pos2);
std::mem::swap(&mut pos12, &mut pos21);
best_sep = sep2;
swapped_reference = true;
} else if sep3.0 > sep1.0 {
best_sep = sep3;
}
let feature1;
let mut feature2;
#[cfg(feature = "dim2")]
{
if swapped_reference {
feature1 = triangle::support_face(triangle2, best_sep.1);
feature2 = cube1.support_face(pos21 * -best_sep.1);
} else {
feature1 = cube1.support_face(best_sep.1);
feature2 = triangle::support_face(triangle2, pos21 * -best_sep.1);
}
}
#[cfg(feature = "dim3")]
{
if swapped_reference {
feature1 = PolyhedronFeature::from(*triangle2);
feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1);
} else {
feature1 = cube1.polyhedron_support_face(best_sep.1);
feature2 = PolyhedronFeature::from(*triangle2);
}
}
feature2.transform_by(&pos12);
if swapped ^ swapped_reference {
manifold.swap_identifiers();
}
// We do this clone to perform contact tracking and transfer impulses.
// FIXME: find a more efficient way of doing this.
let old_manifold_points = manifold.points.clone();
manifold.points.clear();
#[cfg(feature = "dim2")]
CuboidFeature::face_face_contacts(
prediction_distance,
&feature1,
&best_sep.1,
&feature2,
&pos21,
manifold,
);
#[cfg(feature = "dim3")]
PolyhedronFeature::contacts(
prediction_distance,
&feature1,
&best_sep.1,
&feature2,
&pos21,
manifold,
);
manifold.local_n1 = best_sep.1;
manifold.local_n2 = pos21 * -best_sep.1;
manifold.kinematics.category = KinematicsCategory::PlanePoint;
manifold.kinematics.radius1 = 0.0;
manifold.kinematics.radius2 = 0.0;
// Transfer impulses.
manifold.match_contacts(&old_manifold_points, swapped ^ swapped_reference);
}

190
src/geometry/contact_generator/heightfield_shape_contact_generator.rs
View File

@@ -1,190 +0,0 @@
use crate::buckler::bounding_volume::BoundingVolume;
use crate::data::hashmap::{Entry, HashMap};
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::{
ContactGenerationContext, ContactGeneratorWorkspace, PrimitiveContactGenerationContext,
PrimitiveContactGenerator,
};
use crate::geometry::{Collider, ContactManifold, ContactManifoldData};
#[cfg(feature = "dim2")]
use buckler::shape::Capsule;
use buckler::shape::{HeightField, Shape};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
struct SubDetector {
#[cfg_attr(feature = "serde-serialize", serde(skip))]
generator: Option<PrimitiveContactGenerator>,
manifold_id: usize,
timestamp: bool,
workspace: Option<ContactGeneratorWorkspace>,
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct HeightFieldShapeContactGeneratorWorkspace {
timestamp: bool,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
old_manifolds: Vec<ContactManifold>,
sub_detectors: HashMap<usize, SubDetector>,
}
impl HeightFieldShapeContactGeneratorWorkspace {
pub fn new() -> Self {
Self {
timestamp: false,
old_manifolds: Vec::new(),
sub_detectors: HashMap::default(),
}
}
}
pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext) {
let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
if let Some(heightfield1) = collider1.shape().as_heightfield() {
do_generate_contacts(heightfield1, collider1, collider2, ctxt, false)
} else if let Some(heightfield2) = collider2.shape().as_heightfield() {
do_generate_contacts(heightfield2, collider2, collider1, ctxt, true)
}
}
fn do_generate_contacts(
heightfield1: &HeightField,
collider1: &Collider,
collider2: &Collider,
ctxt: &mut ContactGenerationContext,
_flipped: bool,
) {
let workspace: &mut HeightFieldShapeContactGeneratorWorkspace = ctxt
.pair
.generator_workspace
.as_mut()
.expect("The HeightFieldShapeContactGeneratorWorkspace is missing.")
.0
.downcast_mut()
.expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace.");
let new_timestamp = !workspace.timestamp;
workspace.timestamp = new_timestamp;
/*
* Compute interferences.
*/
let pos12 = collider1.position.inverse() * collider2.position;
// TODO: somehow precompute the AABB and reuse it?
let ls_aabb2 = collider2
.shape()
.compute_aabb(&pos12)
.loosened(ctxt.prediction_distance);
std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds);
ctxt.pair.manifolds.clear();
let coll_pair = ctxt.pair.pair;
let manifolds = &mut ctxt.pair.manifolds;
let prediction_distance = ctxt.prediction_distance;
let dispatcher = ctxt.dispatcher;
let solver_flags = ctxt.solver_flags;
let shape_type2 = collider2.shape().shape_type();
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.
#[cfg(feature = "dim3")]
let sub_shape1 = *part1;
let sub_detector = match workspace.sub_detectors.entry(i as usize) {
Entry::Occupied(entry) => {
let sub_detector = entry.into_mut();
let manifold = workspace.old_manifolds[sub_detector.manifold_id].take();
sub_detector.manifold_id = manifolds.len();
sub_detector.timestamp = new_timestamp;
manifolds.push(manifold);
sub_detector
}
Entry::Vacant(entry) => {
let (generator, workspace2) =
dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2);
let sub_detector = SubDetector {
generator: Some(generator),
manifold_id: manifolds.len(),
timestamp: new_timestamp,
workspace: workspace2,
};
let manifold_data = ContactManifoldData::from_colliders(
coll_pair,
collider1,
collider2,
solver_flags,
);
manifolds.push(ContactManifold::with_data((i as usize, 0), manifold_data));
entry.insert(sub_detector)
}
};
if sub_detector.generator.is_none() {
// We probably lost the generator after deserialization.
// So we need to dispatch again.
let (generator, workspace2) =
dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2);
sub_detector.generator = Some(generator);
// Don't overwrite the workspace if we already deserialized one.
if sub_detector.workspace.is_none() {
sub_detector.workspace = workspace2;
}
}
let manifold = &mut manifolds[sub_detector.manifold_id];
let mut ctxt2 = if coll_pair.collider1 != manifold.data.pair.collider1 {
PrimitiveContactGenerationContext {
prediction_distance,
collider1: collider2,
collider2: collider1,
shape1: collider2.shape(),
shape2: &sub_shape1,
position1: collider2.position(),
position2: position1,
manifold,
workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0),
}
} else {
PrimitiveContactGenerationContext {
prediction_distance,
collider1,
collider2,
shape1: &sub_shape1,
shape2: collider2.shape(),
position1,
position2: collider2.position(),
manifold,
workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0),
}
};
(sub_detector.generator.unwrap().generate_contacts)(&mut ctxt2)
});
workspace
.sub_detectors
.retain(|_, detector| detector.timestamp == new_timestamp)
}
impl MaybeSerializableData for HeightFieldShapeContactGeneratorWorkspace {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
Some((
super::WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace as u32,
self,
))
}
fn clone_dyn(&self) -> Box<dyn MaybeSerializableData> {
Box::new(self.clone())
}
}

51
src/geometry/contact_generator/mod.rs
View File

@@ -1,51 +0,0 @@
pub use self::ball_ball_contact_generator::generate_contacts_ball_ball;
#[cfg(feature = "simd-is-enabled")]
pub use self::ball_ball_contact_generator::generate_contacts_ball_ball_simd;
pub use self::ball_convex_contact_generator::generate_contacts_ball_convex;
pub use self::capsule_capsule_contact_generator::generate_contacts_capsule_capsule;
pub use self::contact_dispatcher::{ContactDispatcher, DefaultContactDispatcher};
pub use self::contact_generator::{
ContactGenerationContext, ContactGenerator, ContactPhase, PrimitiveContactGenerationContext,
PrimitiveContactGenerator,
};
#[cfg(feature = "simd-is-enabled")]
pub use self::contact_generator::{
ContactGenerationContextSimd, PrimitiveContactGenerationContextSimd,
};
pub use self::cuboid_capsule_contact_generator::generate_contacts_cuboid_capsule;
pub use self::cuboid_cuboid_contact_generator::generate_contacts_cuboid_cuboid;
pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triangle;
pub use self::heightfield_shape_contact_generator::{
generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace,
};
#[cfg(feature = "dim3")]
pub use self::pfm_pfm_contact_generator::{
generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace,
};
// pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
pub use self::contact_generator_workspace::ContactGeneratorWorkspace;
pub use self::trimesh_shape_contact_generator::{
generate_contacts_trimesh_shape, TriMeshShapeContactGeneratorWorkspace,
};
pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag;
mod ball_ball_contact_generator;
mod ball_convex_contact_generator;
mod ball_polygon_contact_generator;
mod capsule_capsule_contact_generator;
mod contact_dispatcher;
mod contact_generator;
mod contact_generator_workspace;
mod cuboid_capsule_contact_generator;
mod cuboid_cuboid_contact_generator;
mod cuboid_polygon_contact_generator;
mod cuboid_triangle_contact_generator;
mod heightfield_shape_contact_generator;
#[cfg(feature = "dim3")]
mod pfm_pfm_contact_generator;
mod polygon_polygon_contact_generator;
mod serializable_workspace_tag;
mod trimesh_shape_contact_generator;
use crate::geometry::{Contact, ContactManifold};

146
src/geometry/contact_generator/pfm_pfm_contact_generator.rs
View File

@@ -1,146 +0,0 @@
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
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;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct PfmPfmContactManifoldGeneratorWorkspace {
#[cfg_attr(
feature = "serde-serialize",
serde(skip, default = "VoronoiSimplex::new")
)]
simplex: VoronoiSimplex,
last_gjk_dir: Option<Unit<Vector<f32>>>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
feature1: PolyhedronFeature,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
feature2: PolyhedronFeature,
}
impl Default for PfmPfmContactManifoldGeneratorWorkspace {
fn default() -> Self {
Self {
simplex: VoronoiSimplex::new(),
last_gjk_dir: None,
feature1: PolyhedronFeature::new(),
feature2: PolyhedronFeature::new(),
}
}
}
pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Some((pfm1, border_radius1)), Some((pfm2, border_radius2))) = (
ctxt.shape1.as_polygonal_feature_map(),
ctxt.shape2.as_polygonal_feature_map(),
) {
do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt);
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
}
fn do_generate_contacts(
pfm1: &dyn PolygonalFeatureMap,
border_radius1: f32,
pfm2: &dyn PolygonalFeatureMap,
border_radius2: f32,
ctxt: &mut PrimitiveContactGenerationContext,
) {
let pos12 = ctxt.position1.inverse() * ctxt.position2;
let pos21 = pos12.inverse();
// We use very small thresholds for the manifold update because something to high would
// cause numerical drifts with the effect of introducing bumps in
// what should have been smooth rolling motions.
if ctxt
.manifold
.try_update_contacts_eps(&pos12, crate::utils::COS_1_DEGREES, 1.0e-6)
{
return;
}
let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt
.workspace
.as_mut()
.expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.")
.downcast_mut()
.expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
let total_prediction = ctxt.prediction_distance + border_radius1 + border_radius2;
let contact = query::contact::contact_support_map_support_map_with_params(
&pos12,
pfm1,
pfm2,
total_prediction,
&mut workspace.simplex,
workspace.last_gjk_dir,
);
let old_manifold_points = ctxt.manifold.points.clone();
ctxt.manifold.points.clear();
match contact {
GJKResult::ClosestPoints(_, _, dir) => {
workspace.last_gjk_dir = Some(dir);
let normal1 = dir;
let normal2 = pos21 * -dir;
pfm1.local_support_feature(&normal1, &mut workspace.feature1);
pfm2.local_support_feature(&normal2, &mut workspace.feature2);
workspace.feature2.transform_by(&pos12);
PolyhedronFeature::contacts(
total_prediction,
&workspace.feature1,
&normal1,
&workspace.feature2,
&pos21,
ctxt.manifold,
);
if border_radius1 != 0.0 || border_radius2 != 0.0 {
for contact in &mut ctxt.manifold.points {
contact.local_p1 += *normal1 * border_radius1;
contact.local_p2 += *normal2 * border_radius2;
contact.dist -= border_radius1 + border_radius2;
}
}
// Adjust points to take the radius into account.
ctxt.manifold.local_n1 = *normal1;
ctxt.manifold.local_n2 = *normal2;
ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // TODO: is this the more appropriate?
ctxt.manifold.kinematics.radius1 = 0.0;
ctxt.manifold.kinematics.radius2 = 0.0;
}
GJKResult::NoIntersection(dir) => {
workspace.last_gjk_dir = Some(dir);
}
_ => {}
}
// Transfer impulses.
ctxt.manifold.match_contacts(&old_manifold_points, false);
}
impl MaybeSerializableData for PfmPfmContactManifoldGeneratorWorkspace {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
Some((
super::WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace as u32,
self,
))
}
fn clone_dyn(&self) -> Box<dyn MaybeSerializableData> {
Box::new(self.clone())
}
}

156
src/geometry/contact_generator/polygon_polygon_contact_generator.rs
View File

@@ -1,156 +0,0 @@
#![allow(dead_code)] // TODO: remove this once we support polygons.
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
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!()
// if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
// generate_contacts(
// polygon1,
// &ctxt.position1,
// polygon2,
// &ctxt.position2,
// ctxt.manifold,
// );
// ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
// } else {
// unreachable!()
// }
//
// ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
fn generate_contacts<'a>(
mut p1: &'a Polygon,
mut m1: &'a Isometry<f32>,
mut p2: &'a Polygon,
mut m2: &'a Isometry<f32>,
manifold: &'a mut ContactManifold,
) {
let mut m12 = m1.inverse() * m2;
let mut m21 = m12.inverse();
if manifold.try_update_contacts(&m12) {
return;
}
let mut sep1 = sat::polygon_polygon_compute_separation_features(p1, p2, &m12);
if sep1.0 > 0.0 {
manifold.points.clear();
return;
}
let mut sep2 = sat::polygon_polygon_compute_separation_features(p2, p1, &m21);
if sep2.0 > 0.0 {
manifold.points.clear();
return;
}
let mut swapped = false;
if sep2.0 > sep1.0 {
std::mem::swap(&mut sep1, &mut sep2);
std::mem::swap(&mut m1, &mut m2);
std::mem::swap(&mut p1, &mut p2);
std::mem::swap(&mut m12, &mut m21);
manifold.swap_identifiers();
swapped = true;
}
let support_face1 = sep1.1;
let local_n1 = p1.normals[support_face1];
let local_n2 = m21 * -local_n1;
let support_face2 = p2.support_face(&local_n2);
let len1 = p1.vertices.len();
let len2 = p2.vertices.len();
let seg1 = (
p1.vertices[support_face1],
p1.vertices[(support_face1 + 1) % len1],
);
let seg2 = (
m12 * p2.vertices[support_face2],
m12 * p2.vertices[(support_face2 + 1) % len2],
);
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 data_a = ContactData::default();
let mut data_b = ContactData::default();
let fids_a = (
((support_face1 * 2 + clip_a.2) % (len1 * 2)) as u8,
((support_face2 * 2 + clip_a.3) % (len2 * 2)) as u8,
);
let fids_b = (
((support_face1 * 2 + clip_b.2) % (len1 * 2)) as u8,
((support_face2 * 2 + clip_b.3) % (len2 * 2)) as u8,
);
if manifold.points.len() != 0 {
assert_eq!(manifold.points.len(), 2);
// We already had 2 points in the previous iteration.
// Match the features to see if we keep the cached impulse.
let original_fids_a;
let original_fids_b;
// NOTE: the previous manifold may have its bodies swapped wrt. our new manifold.
// So we have to adjust accordingly the features we will be comparing.
if swapped {
original_fids_a = (manifold.points[0].fid1, manifold.points[0].fid2);
original_fids_b = (manifold.points[1].fid1, manifold.points[1].fid2);
} else {
original_fids_a = (manifold.points[0].fid2, manifold.points[0].fid1);
original_fids_b = (manifold.points[1].fid2, manifold.points[1].fid1);
}
if fids_a == original_fids_a {
data_a = manifold.points[0].data;
} else if fids_a == original_fids_b {
data_a = manifold.points[1].data;
}
if fids_b == original_fids_a {
data_b = manifold.points[0].data;
} else if fids_b == original_fids_b {
data_b = manifold.points[1].data;
}
}
manifold.points.clear();
manifold.points.push(Contact {
local_p1: clip_a.0,
local_p2: m21 * clip_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,
fid1: fids_b.0,
fid2: fids_b.1,
dist: dist_b,
data: data_b,
});
manifold.local_n1 = local_n1;
manifold.local_n2 = local_n2;
manifold.kinematics.category = KinematicsCategory::PlanePoint;
manifold.kinematics.radius1 = 0.0;
manifold.kinematics.radius2 = 0.0;
} else {
manifold.points.clear();
}
}

9
src/geometry/contact_generator/serializable_workspace_tag.rs
View File

@@ -1,9 +0,0 @@
use num_derive::FromPrimitive;
#[derive(Copy, Clone, Debug, FromPrimitive)]
pub(super) enum WorkspaceSerializationTag {
TriMeshShapeContactGeneratorWorkspace = 0,
#[cfg(feature = "dim3")]
PfmPfmContactGeneratorWorkspace,
HeightfieldShapeContactGeneratorWorkspace,
}

221
src/geometry/contact_generator/trimesh_shape_contact_generator.rs
View File

@@ -1,221 +0,0 @@
use crate::buckler::bounding_volume::{BoundingVolume, AABB};
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::{
ContactGenerationContext, PrimitiveContactGenerationContext,
};
use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, TriMesh};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct TriMeshShapeContactGeneratorWorkspace {
interferences: Vec<u32>,
local_aabb2: AABB,
old_interferences: Vec<u32>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
old_manifolds: Vec<ContactManifold>,
}
impl TriMeshShapeContactGeneratorWorkspace {
pub fn new() -> Self {
Self {
interferences: Vec::new(),
local_aabb2: AABB::new_invalid(),
old_interferences: Vec::new(),
old_manifolds: Vec::new(),
}
}
}
pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) {
let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
if let Some(trimesh1) = collider1.shape().as_trimesh() {
do_generate_contacts(trimesh1, collider1, collider2, ctxt, false)
} else if let Some(trimesh2) = collider2.shape().as_trimesh() {
do_generate_contacts(trimesh2, collider2, collider1, ctxt, true)
}
}
fn do_generate_contacts(
trimesh1: &TriMesh,
collider1: &Collider,
collider2: &Collider,
ctxt: &mut ContactGenerationContext,
flipped: bool,
) {
let ctxt_pair_pair = if flipped {
ctxt.pair.pair.swap()
} else {
ctxt.pair.pair
};
let workspace: &mut TriMeshShapeContactGeneratorWorkspace = ctxt
.pair
.generator_workspace
.as_mut()
.expect("The TriMeshShapeContactGeneratorWorkspace is missing.")
.0
.downcast_mut()
.expect("Invalid workspace type, expected a TriMeshShapeContactGeneratorWorkspace.");
/*
* Compute interferences.
*/
let pos12 = collider1.position.inverse() * collider2.position;
// TODO: somehow precompute the AABB and reuse it?
let mut new_local_aabb2 = collider2
.shape()
.compute_aabb(&pos12)
.loosened(ctxt.prediction_distance);
let same_local_aabb2 = workspace.local_aabb2.contains(&new_local_aabb2);
if !same_local_aabb2 {
let extra_margin =
(new_local_aabb2.maxs - new_local_aabb2.mins).map(|e| (e / 10.0).min(0.1));
new_local_aabb2.mins -= extra_margin;
new_local_aabb2.maxs += extra_margin;
let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value?
std::mem::swap(
&mut workspace.old_interferences,
&mut workspace.interferences,
);
std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds);
ctxt.pair.manifolds.clear();
if workspace.old_interferences.is_empty() && !workspace.old_manifolds.is_empty() {
// This happens if for some reasons the contact generator context was lost
// and rebuilt. In this case, we hate to reconstruct the `old_interferences`
// array using the subshape ids from the contact manifolds.
// TODO: always rely on the subshape ids instead of maintaining `.old_interferences` ?
let ctxt_collider1 = ctxt_pair_pair.collider1;
workspace.old_interferences = workspace
.old_manifolds
.iter()
.map(|manifold| {
if manifold.data.pair.collider1 == ctxt_collider1 {
manifold.subshape_index_pair.0 as u32
} else {
manifold.subshape_index_pair.1 as u32
}
})
.collect();
}
// This assertion may fire due to the invalid triangle_ids that the
// near-phase may return (due to SIMD sentinels).
//
// assert_eq!(
// workspace
// .old_interferences
// .len()
// .min(trimesh1.num_triangles()),
// workspace.old_manifolds.len()
// );
workspace.interferences.clear();
trimesh1
.quadtree()
.intersect_aabb(&local_aabb2, &mut workspace.interferences);
workspace.local_aabb2 = local_aabb2;
}
/*
* Dispatch to the specific solver by keeping the previous manifold if we already had one.
*/
let new_interferences = &workspace.interferences;
let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
let mut old_manifolds_it = workspace.old_manifolds.drain(..);
let shape_type2 = collider2.shape().shape_type();
// TODO: don't redispatch at each frame (we should probably do the same as
// the heightfield).
for (i, triangle_id) in new_interferences.iter().enumerate() {
if *triangle_id >= trimesh1.num_triangles() as u32 {
// Because of SIMD padding, the broad-phase may return tiangle indices greater
// than the max.
continue;
}
if !same_local_aabb2 {
loop {
match old_inter_it.peek() {
Some(old_triangle_id) if *old_triangle_id < *triangle_id => {
old_inter_it.next();
old_manifolds_it.next();
}
_ => break,
}
}
let manifold = if old_inter_it.peek() != Some(triangle_id) {
// We don't have a manifold for this triangle yet.
let data = ContactManifoldData::from_colliders(
ctxt_pair_pair,
collider1,
collider2,
ctxt.solver_flags,
);
ContactManifold::with_data((*triangle_id as usize, 0), data)
} else {
// We already have a manifold for this triangle.
old_inter_it.next();
old_manifolds_it.next().unwrap()
};
ctxt.pair.manifolds.push(manifold);
}
let manifold = &mut ctxt.pair.manifolds[i];
let triangle1 = trimesh1.triangle(*triangle_id);
let (generator, mut workspace2) = ctxt
.dispatcher
.dispatch_primitives(ShapeType::Triangle, shape_type2);
let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.data.pair.collider1 {
PrimitiveContactGenerationContext {
prediction_distance: ctxt.prediction_distance,
collider1: collider2,
collider2: collider1,
shape1: collider2.shape(),
shape2: &triangle1,
position1: collider2.position(),
position2: collider1.position(),
manifold,
workspace: workspace2.as_mut().map(|w| &mut *w.0),
}
} else {
PrimitiveContactGenerationContext {
prediction_distance: ctxt.prediction_distance,
collider1,
collider2,
shape1: &triangle1,
shape2: collider2.shape(),
position1: collider1.position(),
position2: collider2.position(),
manifold,
workspace: workspace2.as_mut().map(|w| &mut *w.0),
}
};
(generator.generate_contacts)(&mut ctxt2);
}
}
impl MaybeSerializableData for TriMeshShapeContactGeneratorWorkspace {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> {
Some((
super::WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace as u32,
self,
))
}
fn clone_dyn(&self) -> Box<dyn MaybeSerializableData> {
Box::new(self.clone())
}
}

0
src/geometry/contact_generator/voxels_shape_contact_generator.rs
View File

32
src/geometry/contact.rs → src/geometry/contact_pair.rs
View File

@@ -1,9 +1,8 @@
use crate::buckler::query::TrackedData;
use crate::data::MaybeSerializableData;
use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet};
use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase};
use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold};
use crate::math::{Isometry, Point, Vector};
use eagl::query::ContactManifoldsWorkspace;
use eagl::utils::MaybeSerializableData;
#[cfg(feature = "simd-is-enabled")]
use {
crate::math::{SimdReal, SIMD_WIDTH},
@@ -96,22 +95,15 @@ pub struct ContactPair {
///
/// 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>,
pub(crate) generator_workspace: Option<ContactGeneratorWorkspace>,
pub(crate) workspace: Option<ContactManifoldsWorkspace>,
}
impl ContactPair {
pub(crate) fn new(
pair: ColliderPair,
generator: ContactPhase,
generator_workspace: Option<ContactGeneratorWorkspace>,
) -> Self {
pub(crate) fn new(pair: ColliderPair) -> Self {
Self {
pair,
manifolds: Vec::new(),
generator: Some(generator),
generator_workspace,
workspace: None,
}
}
@@ -156,14 +148,14 @@ impl ContactPair {
coll1,
coll2,
manifold,
self.generator_workspace.as_mut().map(|w| &mut *w.0),
self.workspace.as_mut().map(|w| &mut *w.0),
)
} else {
(
coll2,
coll1,
manifold,
self.generator_workspace.as_mut().map(|w| &mut *w.0),
self.workspace.as_mut().map(|w| &mut *w.0),
)
}
}
@@ -218,14 +210,6 @@ impl Default for ContactManifoldData {
}
}
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,
@@ -281,7 +265,7 @@ impl ContactManifoldData {
// 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
1.0 // 0.01
}
pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) {

53
src/geometry/mod.rs
View File

@@ -3,49 +3,49 @@
pub use self::broad_phase_multi_sap::BroadPhase;
pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
pub use self::collider_set::{ColliderHandle, ColliderSet};
pub use self::contact::{ContactData, ContactManifoldData};
pub use self::contact::{ContactPair, SolverFlags};
pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher};
// pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher};
pub use self::contact_pair::{ContactData, ContactManifoldData};
pub use self::contact_pair::{ContactPair, SolverFlags};
pub use self::interaction_graph::{
ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex,
};
pub use self::narrow_phase::NarrowPhase;
pub use self::polygon::Polygon;
pub use self::proximity::ProximityPair;
pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher};
pub use self::proximity_pair::ProximityPair;
pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter};
pub use buckler::query::Proximity;
pub use eagl::query::Proximity;
pub use buckler::query::{KinematicsCategory, TrackedContact};
pub use eagl::query::{KinematicsCategory, TrackedContact};
pub type Contact = buckler::query::TrackedContact<ContactData>;
pub type ContactManifold = buckler::query::ContactManifold<ContactManifoldData, ContactData>;
pub type Contact = eagl::query::TrackedContact<ContactData>;
pub type ContactManifold = eagl::query::ContactManifold<ContactManifoldData, ContactData>;
/// A segment shape.
pub type Segment = buckler::shape::Segment;
pub type Segment = eagl::shape::Segment;
/// A cuboid shape.
pub type Cuboid = buckler::shape::Cuboid;
pub type Cuboid = eagl::shape::Cuboid;
/// A triangle shape.
pub type Triangle = buckler::shape::Triangle;
pub type Triangle = eagl::shape::Triangle;
/// A ball shape.
pub type Ball = buckler::shape::Ball;
pub type Ball = eagl::shape::Ball;
/// A capsule shape.
pub type Capsule = buckler::shape::Capsule;
pub type Capsule = eagl::shape::Capsule;
/// A heightfield shape.
pub type HeightField = buckler::shape::HeightField;
pub type HeightField = eagl::shape::HeightField;
/// A cylindrical shape.
#[cfg(feature = "dim3")]
pub type Cylinder = buckler::shape::Cylinder;
pub type Cylinder = eagl::shape::Cylinder;
/// A cone shape.
#[cfg(feature = "dim3")]
pub type Cone = buckler::shape::Cone;
pub type Cone = eagl::shape::Cone;
/// An axis-aligned bounding box.
pub type AABB = buckler::bounding_volume::AABB;
pub type AABB = eagl::bounding_volume::AABB;
/// A ray that can be cast against colliders.
pub type Ray = buckler::query::Ray;
pub type Ray = eagl::query::Ray;
/// The intersection between a ray and a collider.
pub type RayIntersection = buckler::query::RayIntersection;
pub type RayIntersection = eagl::query::RayIntersection;
/// The the projection of a point on a collider.
pub type PointProjection = buckler::query::PointProjection;
pub type PointProjection = eagl::query::PointProjection;
#[derive(Copy, Clone, Hash, Debug)]
/// Events occurring when two collision objects start or stop being in contact (or penetration).
@@ -101,26 +101,25 @@ pub(crate) use self::ball::WBall;
pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair};
pub(crate) use self::collider_set::RemovedCollider;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use self::contact::WContact;
pub(crate) use self::contact_pair::WContact;
pub(crate) use self::narrow_phase::ContactManifoldIndex;
pub(crate) use buckler::partitioning::WQuadtree;
pub(crate) use eagl::partitioning::WQuadtree;
//pub(crate) use self::z_order::z_cmp_floats;
pub use self::interaction_groups::InteractionGroups;
pub use buckler::shape::*;
pub use eagl::shape::*;
mod ball;
mod broad_phase_multi_sap;
mod collider;
mod collider_set;
mod contact;
mod contact_generator;
// mod contact_generator;
mod contact_pair;
mod interaction_graph;
mod narrow_phase;
mod polygon;
mod proximity;
mod proximity_detector;
mod proximity_pair;
pub(crate) mod sat;
pub(crate) mod triangle;
//mod z_order;
mod interaction_groups;
mod user_callbacks;

56
src/geometry/narrow_phase.rs
View File

@@ -2,28 +2,26 @@
use rayon::prelude::*;
use crate::dynamics::RigidBodySet;
use crate::geometry::contact_generator::{
ContactDispatcher, ContactGenerationContext, DefaultContactDispatcher,
};
use crate::geometry::proximity_detector::{
DefaultProximityDispatcher, ProximityDetectionContext, ProximityDispatcher,
};
use eagl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
//#[cfg(feature = "simd-is-enabled")]
//use crate::geometry::{
// contact_generator::ContactGenerationContextSimd,
// proximity_detector::ProximityDetectionContextSimd, WBall,
//};
use crate::geometry::{
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactPairFilter,
PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, RemovedCollider,
SolverFlags,
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactManifoldData,
ContactPairFilter, PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter,
RemovedCollider, SolverFlags,
};
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
//#[cfg(feature = "simd-is-enabled")]
//use crate::math::{SimdReal, SIMD_WIDTH};
use crate::buckler::query::Proximity;
use crate::data::pubsub::Subscription;
use crate::data::Coarena;
use crate::eagl::query::Proximity;
use crate::pipeline::EventHandler;
use std::collections::HashMap;
//use simba::simd::SimdValue;
@@ -324,10 +322,7 @@ impl NarrowPhase {
.find_edge(gid1.contact_graph_index, gid2.contact_graph_index)
.is_none()
{
let dispatcher = DefaultContactDispatcher;
let generator = dispatcher
.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
let interaction = ContactPair::new(*pair, generator.0, generator.1);
let interaction = ContactPair::new(*pair);
let _ = self.contact_graph.add_edge(
gid1.contact_graph_index,
gid2.contact_graph_index,
@@ -523,33 +518,22 @@ impl NarrowPhase {
solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
}
let dispatcher = DefaultContactDispatcher;
if pair.generator.is_none() {
// We need a redispatch for this generator.
// This can happen, e.g., after restoring a snapshot of the narrow-phase.
let (generator, workspace) =
dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
pair.generator = Some(generator);
// Keep the workspace if one already exists.
if pair.generator_workspace.is_none() {
pair.generator_workspace = workspace;
}
}
let context = ContactGenerationContext {
dispatcher: &dispatcher,
let dispatcher = DefaultQueryDispatcher;
let pos12 = co1.position().inverse() * co2.position();
dispatcher.contact_manifolds(
&pos12,
co1.shape(),
co2.shape(),
prediction_distance,
colliders,
pair,
solver_flags,
};
&mut pair.manifolds,
&mut pair.workspace,
);
context
.pair
.generator
.unwrap()
.generate_contacts(context, events);
// TODO: don't write this everytime?
for manifold in &mut pair.manifolds {
manifold.data =
ContactManifoldData::from_colliders(pair.pair, co1, co2, solver_flags);
}
});
}

2
src/geometry/polygon.rs
View File

@@ -1,7 +1,7 @@
#![allow(dead_code)] // TODO: remove this once we support polygons.
use crate::math::{Isometry, Point, Vector};
use buckler::bounding_volume::AABB;
use eagl::bounding_volume::AABB;
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]

2
src/geometry/proximity_detector/ball_convex_proximity_detector.rs
View File

@@ -1,7 +1,7 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{Ball, Proximity};
use crate::math::Isometry;
use buckler::query::PointQuery;
use eagl::query::PointQuery;
pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
if let Some(ball1) = ctxt.shape1.as_ball() {

4
src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs
View File

@@ -1,8 +1,8 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::Proximity;
use crate::math::Isometry;
use buckler::query::sat;
use buckler::shape::Cuboid;
use eagl::query::sat;
use eagl::shape::Cuboid;
pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) {

2
src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs
View File

@@ -1,7 +1,7 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{Cuboid, Proximity, Triangle};
use crate::math::Isometry;
use buckler::query::sat;
use eagl::query::sat;
pub fn detect_proximity_cuboid_triangle(
ctxt: &mut PrimitiveProximityDetectionContext,

4
src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs
View File

@@ -1,4 +1,4 @@
use crate::buckler::bounding_volume::{BoundingVolume, AABB};
use crate::eagl::bounding_volume::{BoundingVolume, AABB};
use crate::geometry::proximity_detector::{
PrimitiveProximityDetectionContext, ProximityDetectionContext,
};
@@ -87,7 +87,7 @@ fn do_detect_proximity(
for triangle_id in new_interferences.iter() {
if *triangle_id >= trimesh1.num_triangles() as u32 {
// Because of SIMD padding, the broad-phase may return tiangle indices greater
// Because of SIMD padding, the broad-phase may return triangle indices greater
// than the max.
continue;
}

0
src/geometry/proximity.rs → src/geometry/proximity_pair.rs
View File

7
src/geometry/triangle.rs
View File

@@ -1,7 +0,0 @@
#[cfg(feature = "dim2")]
use crate::{buckler::shape::CuboidFeatureFace, geometry::Triangle, math::Vector};
#[cfg(feature = "dim2")]
pub fn support_face(_triangle: &Triangle, _local_dir: Vector<f32>) -> CuboidFeatureFace {
unimplemented!()
}

70
src/geometry/z_order.rs
View File

@@ -1,70 +0,0 @@
use num_traits::float::FloatCore;
use std::cmp::Ordering;
#[allow(dead_code)] // We don't use this currently, but migth in the future.
pub fn z_cmp_ints(lhs: &[usize], rhs: &[usize]) -> Ordering {
assert_eq!(
lhs.len(),
rhs.len(),
"Cannot compare array with different lengths."
);
let mut msd = 0;
for dim in 1..rhs.len() {
if less_msb(lhs[msd] ^ rhs[msd], lhs[dim] ^ rhs[dim]) {
msd = dim;
}
}
lhs[msd].cmp(&rhs[msd])
}
fn less_msb(x: usize, y: usize) -> bool {
x < y && x < (x ^ y)
}
// Fast construction of k-Nearest Neighbor Graphs for Point Clouds
// Michael Connor, Piyush Kumar
// Algorithm 1
//
// http://compgeom.com/~piyush/papers/tvcg_stann.pdf
pub fn z_cmp_floats(p1: &[f32], p2: &[f32]) -> Option<Ordering> {
assert_eq!(
p1.len(),
p2.len(),
"Cannot compare array with different lengths."
);
let mut x = 0;
let mut dim = 0;
for j in 0..p1.len() {
let y = xor_msb_float(p1[j], p2[j]);
if x < y {
x = y;
dim = j;
}
}
p1[dim].partial_cmp(&p2[dim])
}
fn xor_msb_float(fa: f32, fb: f32) -> i16 {
let (mantissa1, exponent1, _sign1) = fa.integer_decode();
let (mantissa2, exponent2, _sign2) = fb.integer_decode();
let x = exponent1; // To keep the same notation as the paper.
let y = exponent2; // To keep the same notation as the paper.
if x == y {
let z = msdb(mantissa1, mantissa2);
x - z
} else if y < x {
x
} else {
y
}
}
fn msdb(x: u64, y: u64) -> i16 {
64i16 - (x ^ y).leading_zeros() as i16
}

10
src/lib.rs
View File

@@ -11,11 +11,11 @@
// FIXME: deny that
#![allow(missing_docs)]
#[cfg(feature = "dim2")]
pub extern crate buckler2d as buckler;
#[cfg(feature = "dim3")]
pub extern crate buckler3d as buckler;
pub extern crate crossbeam;
#[cfg(feature = "dim2")]
pub extern crate eagl2d as eagl;
#[cfg(feature = "dim3")]
pub extern crate eagl3d as eagl;
pub extern crate nalgebra as na;
#[cfg(feature = "serde")]
#[macro_use]
@@ -128,4 +128,4 @@ pub mod dynamics;
pub mod geometry;
pub mod pipeline;
pub mod utils;
pub use buckler::math;
pub use eagl::math;

15
src/utils.rs
View File

@@ -5,6 +5,7 @@ use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vec
use num::Zero;
use simba::simd::SimdValue;
use eagl::utils::SdpMatrix3;
use std::ops::{Add, Mul};
use {
crate::math::{AngularInertia, SimdBool, SimdReal},
@@ -468,8 +469,7 @@ impl WAngularInertia<SimdReal> for SimdReal {
}
}
#[cfg(feature = "dim3")]
impl WAngularInertia<f32> for AngularInertia<f32> {
impl WAngularInertia<f32> for SdpMatrix3<f32> {
type AngVector = Vector3<f32>;
type LinVector = Vector3<f32>;
type AngMatrix = Matrix3<f32>;
@@ -485,7 +485,7 @@ impl WAngularInertia<f32> for AngularInertia<f32> {
if determinant.is_zero() {
Self::zero()
} else {
AngularInertia {
SdpMatrix3 {
m11: minor_m12_m23 / determinant,
m12: -minor_m11_m23 / determinant,
m13: minor_m11_m22 / determinant,
@@ -497,7 +497,7 @@ impl WAngularInertia<f32> for AngularInertia<f32> {
}
fn squared(&self) -> Self {
AngularInertia {
SdpMatrix3 {
m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13,
m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23,
m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33,
@@ -533,8 +533,7 @@ impl WAngularInertia<f32> for AngularInertia<f32> {
}
}
#[cfg(feature = "dim3")]
impl WAngularInertia<SimdReal> for AngularInertia<SimdReal> {
impl WAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
type AngVector = Vector3<SimdReal>;
type LinVector = Vector3<SimdReal>;
type AngMatrix = Matrix3<SimdReal>;
@@ -551,7 +550,7 @@ impl WAngularInertia<SimdReal> for AngularInertia<SimdReal> {
let is_zero = determinant.simd_eq(zero);
let inv_det = (<SimdReal>::one() / determinant).select(is_zero, zero);
AngularInertia {
SdpMatrix3 {
m11: minor_m12_m23 * inv_det,
m12: -minor_m11_m23 * inv_det,
m13: minor_m11_m22 * inv_det,
@@ -573,7 +572,7 @@ impl WAngularInertia<SimdReal> for AngularInertia<SimdReal> {
}
fn squared(&self) -> Self {
AngularInertia {
SdpMatrix3 {
m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13,
m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23,
m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33,