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

Complete the pfm/pfm contact generator.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-10-19 16:51:40 +02:00
parent faf3e7e0f7
commit 947c4813c9
14 changed files with 166 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/dynamics/rigid_body.rs
View File

@@ -218,6 +218,7 @@ impl RigidBody {
let shift = Translation::from(com.coords);
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
}
pub(crate) fn integrate(&mut self, dt: f32) {
self.position = self.integrate_velocity(dt) * self.position;
}
@@ -334,7 +335,7 @@ impl RigidBody {
}
}
/// A builder for rigid-bodies.
/// A builder for rigid-bodies.
pub struct RigidBodyBuilder {
position: Isometry<f32>,
linvel: Vector<f32>,

25
src/geometry/contact.rs
View File

@@ -429,17 +429,27 @@ impl ContactManifold {
}
#[inline]
pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>, early_stop: bool) -> bool {
pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>) -> bool {
// const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
const DIST_SQ_THRESHOLD: f32 = 0.001; // FIXME: this should not be hard-coded.
self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD)
}
#[inline]
pub(crate) fn try_update_contacts_eps(
&mut self,
pos12: &Isometry<f32>,
angle_dot_threshold: f32,
dist_sq_threshold: f32,
) -> bool {
if self.points.len() == 0 {
return false;
}
// const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
let local_n2 = pos12 * self.local_n2;
if early_stop && -self.local_n1.dot(&local_n2) < DOT_THRESHOLD {
if -self.local_n1.dot(&local_n2) < angle_dot_threshold {
return false;
}
@@ -448,15 +458,14 @@ impl ContactManifold {
let dpt = local_p2 - pt.local_p1;
let dist = dpt.dot(&self.local_n1);
if early_stop && dist * pt.dist < 0.0 {
if dist * pt.dist < 0.0 {
// We switched between penetrating/non-penetrating.
// The may result in other contacts to appear.
return false;
}
let new_local_p1 = local_p2 - self.local_n1 * dist;
let dist_threshold = 0.001; // FIXME: this should not be hard-coded.
if early_stop && na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold {
if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold {
return false;
}

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

@@ -47,8 +47,8 @@ pub fn generate_contacts<'a>(
let mut pos12 = pos1.inverse() * pos2;
let mut pos21 = pos12.inverse();
if (!swapped && manifold.try_update_contacts(&pos12, true))
|| (swapped && manifold.try_update_contacts(&pos21, true))
if (!swapped && manifold.try_update_contacts(&pos12))
|| (swapped && manifold.try_update_contacts(&pos21))
{
return;
}

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

@@ -34,7 +34,7 @@ pub fn generate_contacts<'a>(
let mut pos12 = pos1.inverse() * pos2;
let mut pos21 = pos12.inverse();
if manifold.try_update_contacts(&pos12, true) {
if manifold.try_update_contacts(&pos12) {
return;
}

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

@@ -48,8 +48,8 @@ pub fn generate_contacts<'a>(
let mut pos12 = pos1.inverse() * pos2;
let mut pos21 = pos12.inverse();
if (!swapped && manifold.try_update_contacts(&pos12, true))
|| (swapped && manifold.try_update_contacts(&pos21, true))
if (!swapped && manifold.try_update_contacts(&pos12))
|| (swapped && manifold.try_update_contacts(&pos21))
{
return;
}

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

@@ -30,8 +30,8 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace {
pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Some(pfm1), Some(pfm2)) = (
ctxt.collider1.shape().as_polygonal_feature_map(),
ctxt.collider2.shape().as_polygonal_feature_map(),
ctxt.shape1.as_polygonal_feature_map(),
ctxt.shape2.as_polygonal_feature_map(),
) {
do_generate_contacts(pfm1, pfm2, ctxt);
ctxt.manifold.update_warmstart_multiplier();
@@ -47,9 +47,15 @@ fn do_generate_contacts(
let pos12 = ctxt.position1.inverse() * ctxt.position2;
let pos21 = pos12.inverse();
// if ctxt.manifold.try_update_contacts(&pos12, true) {
// return;
// }
// 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
@@ -72,7 +78,7 @@ fn do_generate_contacts(
ctxt.manifold.points.clear();
match contact {
GJKResult::ClosestPoints(local_p1, local_p2, dir) => {
GJKResult::ClosestPoints(_, _, dir) => {
workspace.last_gjk_dir = Some(dir);
let normal1 = dir;
let normal2 = pos21 * -dir;
@@ -89,24 +95,10 @@ fn do_generate_contacts(
ctxt.manifold,
);
// if ctxt.manifold.all_contacts().is_empty() {
// // Add at least the deepest contact.
// let dist = (local_p2 - local_p1).dot(&dir);
// ctxt.manifold.points.push(Contact {
// local_p1,
// local_p2: pos21 * local_p2,
// impulse: 0.0,
// tangent_impulse: Contact::zero_tangent_impulse(),
// fid1: 0, // FIXME
// fid2: 0, // FIXME
// dist,
// });
// }
// Adjust points to take the radius into account.
ctxt.manifold.local_n1 = *normal1;
ctxt.manifold.local_n2 = *normal2;
ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME
ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // TODO: is this the more appropriate?
ctxt.manifold.kinematics.radius1 = 0.0;
ctxt.manifold.kinematics.radius2 = 0.0;
}
@@ -115,116 +107,7 @@ fn do_generate_contacts(
}
_ => {}
}
}
fn do_generate_contacts2(
pfm1: &dyn PolygonalFeatureMap,
pfm2: &dyn PolygonalFeatureMap,
ctxt: &mut PrimitiveContactGenerationContext,
) {
let pos12 = ctxt.position1.inverse() * ctxt.position2;
let pos21 = pos12.inverse();
// if ctxt.manifold.try_update_contacts(&pos12, true) {
// return;
// }
let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt
.workspace
.as_mut()
.expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.")
.downcast_mut()
.expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
fn generate_single_contact_pair(
pfm1: &dyn PolygonalFeatureMap,
pfm2: &dyn PolygonalFeatureMap,
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
prediction_distance: f32,
manifold: &mut ContactManifold,
workspace: &mut PfmPfmContactManifoldGeneratorWorkspace,
) -> Option<Unit<Vector<f32>>> {
let contact = query::contact_support_map_support_map_with_params(
&Isometry::identity(),
pfm1,
&pos12,
pfm2,
prediction_distance,
&mut workspace.simplex,
workspace.last_gjk_dir,
);
match contact {
GJKResult::ClosestPoints(local_p1, local_p2, dir) => {
// Add at least the deepest contact.
let dist = (local_p2 - local_p1).dot(&dir);
manifold.points.push(Contact {
local_p1,
local_p2: pos21 * local_p2,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: 0, // FIXME
fid2: 0, // FIXME
dist,
});
Some(dir)
}
GJKResult::NoIntersection(dir) => Some(dir),
_ => None,
}
}
let old_manifold_points = ctxt.manifold.points.clone();
ctxt.manifold.points.clear();
if let Some(local_n1) = generate_single_contact_pair(
pfm1,
pfm2,
&pos12,
&pos21,
ctxt.prediction_distance,
ctxt.manifold,
workspace,
) {
workspace.last_gjk_dir = Some(local_n1);
if !ctxt.manifold.points.is_empty() {
use crate::utils::WBasis;
// Use perturbations to generate other contact points.
let basis = local_n1.orthonormal_basis();
let perturbation_angle = std::f32::consts::PI / 180.0 * 15.0; // FIXME: this should be a function of the shape size.
let perturbations = [
UnitQuaternion::new(basis[0] * perturbation_angle),
UnitQuaternion::new(basis[0] * -perturbation_angle),
UnitQuaternion::new(basis[1] * perturbation_angle),
UnitQuaternion::new(basis[1] * -perturbation_angle),
];
for rot in &perturbations {
let new_pos12 = pos12 * rot;
let new_pos21 = new_pos12.inverse();
generate_single_contact_pair(
pfm1,
pfm2,
&new_pos12,
&new_pos21,
ctxt.prediction_distance,
ctxt.manifold,
workspace,
);
println!("After perturbation: {}", ctxt.manifold.points.len());
}
// Set manifold normal.
ctxt.manifold.local_n1 = *local_n1;
ctxt.manifold.local_n2 = pos21 * -*local_n1;
ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME
ctxt.manifold.kinematics.radius1 = 0.0;
ctxt.manifold.kinematics.radius2 = 0.0;
ctxt.manifold.try_update_contacts(&pos12, false);
}
}
// Transfer impulses.
super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false);
}

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

@@ -31,7 +31,7 @@ fn generate_contacts<'a>(
let mut m12 = m1.inverse() * m2;
let mut m21 = m12.inverse();
if manifold.try_update_contacts(&m12, true) {
if manifold.try_update_contacts(&m12) {
return;
}

36
src/geometry/polygonal_feature_map.rs
View File

@@ -32,6 +32,20 @@ impl PolygonalFeatureMap for Cuboid {
impl PolygonalFeatureMap for Cylinder {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
// About feature ids.
// At all times, we consider our cylinder to be approximated as follows:
// - The curved part is approximated by a single segment.
// - Each flat cap of the cylinder is approximated by a square.
// - The curved-part segment has a feature ID of 0, and its endpoint with negative
// `y` coordinate has an ID of 1.
// - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
// when looking at the cap with an eye looking towards +y).
// - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
// - The bottom cap has its face feature ID of 9.
// - The feature IDs of the top cap are the same as the bottom cap to which we add 10.
// So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19.
// - Note that at all times, one of each cap's vertices are the same as the curved-part
// segment endpoints.
let dir2 = Vector2::new(dir.x, dir.z)
.try_normalize(f32::default_epsilon())
.unwrap_or(Vector2::x());
@@ -45,10 +59,10 @@ impl PolygonalFeatureMap for Cylinder {
);
out_features.vertices[1] =
Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
out_features.eids = [0, 0, 0, 0]; // FIXME
out_features.fid = 1;
out_features.eids = [0, 0, 0, 0];
out_features.fid = 0;
out_features.num_vertices = 2;
out_features.vids = [0, 1, 1, 1]; // FIXME
out_features.vids = [1, 11, 11, 11];
} else {
// We return a square approximation of the cylinder cap.
let y = self.half_height.copysign(dir.y);
@@ -56,10 +70,18 @@ impl PolygonalFeatureMap for Cylinder {
out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
out_features.eids = [0, 1, 2, 3]; // FIXME
out_features.fid = if dir.y < 0.0 { 0 } else { 2 };
out_features.num_vertices = 4;
out_features.vids = [0, 1, 2, 3]; // FIXME
if dir.y < 0.0 {
out_features.eids = [2, 4, 6, 8];
out_features.fid = 9;
out_features.num_vertices = 4;
out_features.vids = [1, 3, 5, 7];
} else {
out_features.eids = [12, 14, 16, 18];
out_features.fid = 19;
out_features.num_vertices = 4;
out_features.vids = [11, 13, 15, 17];
}
}
}
}

3
src/utils.rs
View File

@@ -19,8 +19,9 @@ use {
// pub(crate) const COS_10_DEGREES: f32 = 0.98480775301;
// pub(crate) const COS_45_DEGREES: f32 = 0.70710678118;
// pub(crate) const SIN_45_DEGREES: f32 = COS_45_DEGREES;
pub(crate) const COS_1_DEGREES: f32 = 0.99984769515;
pub(crate) const COS_5_DEGREES: f32 = 0.99619469809;
#[cfg(feature = "dim2")]
// #[cfg(feature = "dim2")]
pub(crate) const COS_FRAC_PI_8: f32 = 0.92387953251;
#[cfg(feature = "dim2")]
pub(crate) const SIN_FRAC_PI_8: f32 = 0.38268343236;