From faec3d5d46c88e2949179dd2789899e5cf26ed48 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 12 Oct 2020 18:33:58 +0200 Subject: Start adding cylinders. --- src/dynamics/mass_properties_capsule.rs | 14 ++- src/geometry/collider.rs | 48 +++++++- .../ball_convex_contact_generator.rs | 2 + .../contact_generator/contact_dispatcher.rs | 14 ++- src/geometry/contact_generator/mod.rs | 6 + .../contact_generator/pfm_pfm_contact_generator.rs | 123 +++++++++++++++++++++ src/geometry/mod.rs | 7 ++ src/geometry/polygonal_feature_map.rs | 65 +++++++++++ src/geometry/polyhedron_feature3d.rs | 10 ++ src/utils.rs | 2 +- src_testbed/engine.rs | 9 ++ src_testbed/objects/cylinder.rs | 74 +++++++++++++ src_testbed/objects/mod.rs | 1 + src_testbed/objects/node.rs | 9 ++ 14 files changed, 375 insertions(+), 9 deletions(-) create mode 100644 src/geometry/contact_generator/pfm_pfm_contact_generator.rs create mode 100644 src/geometry/polygonal_feature_map.rs create mode 100644 src_testbed/objects/cylinder.rs diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index 5f08958..77ba96d 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -1,7 +1,7 @@ use crate::dynamics::MassProperties; #[cfg(feature = "dim3")] use crate::geometry::Capsule; -use crate::math::{Point, PrincipalAngularInertia, Vector}; +use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; impl MassProperties { fn cylinder_y_volume_unit_inertia( @@ -57,4 +57,16 @@ impl MassProperties { ) } } + + #[cfg(feature = "dim3")] + pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self { + let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); + + Self::with_principal_inertia_frame( + Point::origin(), + cyl_vol * density, + cyl_unit_i * density, + Rotation::identity(), + ) + } } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 7c293b6..fe42bd7 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,7 +1,9 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; +#[cfg(feature = "dim3")] +use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, + Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh, }; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; @@ -27,6 +29,9 @@ pub enum Shape { Trimesh(Trimesh), /// A heightfield shape. HeightField(HeightField), + #[cfg(feature = "dim3")] + /// A cylindrical shape. + Cylinder(Cylinder), } impl Shape { @@ -86,6 +91,25 @@ impl Shape { } } + /// Gets a reference to the underlying cylindrical shape, if `self` is one. + pub fn as_cylinder(&self) -> Option<&Cylinder> { + match self { + Shape::Cylinder(c) => Some(c), + _ => None, + } + } + + /// gets a reference to this shape seen as a PolygonalFeatureMap. + #[cfg(feature = "dim3")] + pub fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + match self { + Shape::Triangle(t) => Some(t), + Shape::Cuboid(c) => Some(c), + Shape::Cylinder(c) => Some(c), + _ => None, + } + } + /// Computes the axis-aligned bounding box of this shape. pub fn compute_aabb(&self, position: &Isometry) -> AABB { match self { @@ -96,6 +120,7 @@ impl Shape { Shape::Triangle(triangle) => triangle.bounding_volume(position), Shape::Trimesh(trimesh) => trimesh.aabb(position), Shape::HeightField(heightfield) => heightfield.bounding_volume(position), + Shape::Cylinder(cylinder) => cylinder.bounding_volume(position), } } @@ -139,6 +164,10 @@ impl Shape { Shape::HeightField(heightfield) => { heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) } + #[cfg(feature = "dim3")] + Shape::Cylinder(cylinder) => { + cylinder.toi_and_normal_with_ray(position, ray, max_toi, true) + } } } } @@ -242,9 +271,12 @@ impl Collider { Shape::Capsule(caps) => { MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius) } - Shape::Triangle(_) => MassProperties::zero(), - Shape::Trimesh(_) => MassProperties::zero(), - Shape::HeightField(_) => MassProperties::zero(), + Shape::Triangle(_) | Shape::Trimesh(_) | Shape::HeightField(_) => { + MassProperties::zero() + } + Shape::Cylinder(c) => { + MassProperties::from_cylinder(self.density, c.half_height, c.radius) + } } } } @@ -291,6 +323,12 @@ impl ColliderBuilder { Self::new(Shape::Ball(Ball::new(radius))) } + /// Initialize a new collider builder with a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. + pub fn cylinder(half_height: f32, radius: f32) -> Self { + Self::new(Shape::Cylinder(Cylinder::new(half_height, radius))) + } + /// Initialize a new collider builder with a cuboid shape defined by its half-extents. #[cfg(feature = "dim2")] pub fn cuboid(hx: f32, hy: f32) -> Self { diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index a187832..0856029 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -12,6 +12,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true), Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true), Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true), + Shape::Cylinder(cylinder2) => do_generate_contacts(cylinder2, ball1, ctxt, true), _ => unimplemented!(), } } else if let Shape::Ball(ball2) = ctxt.shape2 { @@ -19,6 +20,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false), Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false), Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false), + Shape::Cylinder(cylinder1) => do_generate_contacts(cylinder1, ball2, ctxt, false), _ => unimplemented!(), } } diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 8c846e0..e925fd5 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -1,6 +1,7 @@ use crate::geometry::contact_generator::{ ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace, - PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace, + PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator, + TrimeshShapeContactGeneratorWorkspace, }; use crate::geometry::Shape; use std::any::Any; @@ -73,7 +74,9 @@ impl ContactDispatcher for DefaultContactDispatcher { | (Shape::Triangle(_), Shape::Ball(_)) | (Shape::Ball(_), Shape::Triangle(_)) | (Shape::Capsule(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Capsule(_)) => ( + | (Shape::Ball(_), Shape::Capsule(_)) + | (Shape::Cylinder(_), Shape::Ball(_)) + | (Shape::Ball(_), Shape::Cylinder(_)) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() @@ -94,6 +97,13 @@ impl ContactDispatcher for DefaultContactDispatcher { }, None, ), + (Shape::Cylinder(_), _) | (_, Shape::Cylinder(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_pfm_pfm, + ..PrimitiveContactGenerator::default() + }, + Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())), + ), _ => (PrimitiveContactGenerator::default(), None), } } diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index ecd2540..a6bad05 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -18,6 +18,10 @@ pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triang 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::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, @@ -39,6 +43,8 @@ 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 trimesh_shape_contact_generator; diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs new file mode 100644 index 0000000..cfb4472 --- /dev/null +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -0,0 +1,123 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +use crate::geometry::{Contact, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace}; +use crate::math::{Isometry, Vector}; +use na::Unit; +use ncollide::query; +use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex}; + +pub struct PfmPfmContactManifoldGeneratorWorkspace { + simplex: VoronoiSimplex, + last_gjk_dir: Option>>, + last_optimal_dir: Option>>, + feature1: PolyhedronFace, + feature2: PolyhedronFace, +} + +impl Default for PfmPfmContactManifoldGeneratorWorkspace { + fn default() -> Self { + Self { + simplex: VoronoiSimplex::new(), + last_gjk_dir: None, + last_optimal_dir: None, + feature1: PolyhedronFace::new(), + feature2: PolyhedronFace::new(), + } + } +} + +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(), + ) { + do_generate_contacts(pfm1, pfm2, ctxt); + ctxt.manifold.update_warmstart_multiplier(); + ctxt.manifold.sort_contacts(ctxt.prediction_distance); + } +} + +fn do_generate_contacts( + 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) { + // return; + // } + + let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt + .workspace + .as_mut() + .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.") + .downcast_mut() + .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); + + let contact = query::contact_support_map_support_map_with_params( + &Isometry::identity(), + pfm1, + &pos12, + pfm2, + ctxt.prediction_distance, + &mut workspace.simplex, + workspace.last_gjk_dir, + ); + + let old_manifold_points = ctxt.manifold.points.clone(); + ctxt.manifold.points.clear(); + + match contact { + GJKResult::ClosestPoints(local_p1, local_p2, 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); + + // PolyhedronFace::contacts( + // ctxt.prediction_distance, + // &workspace.feature1, + // &normal1, + // &workspace.feature2, + // &pos21, + // ctxt.manifold, + // ); + + println!( + "Contact patatrac: {:?}, {:?}, {}, {}", + ctxt.manifold.points.len(), + ctxt.position1 * dir, + workspace.feature1.num_vertices, + workspace.feature2.num_vertices + ); + + 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.radius1 = 0.0; + ctxt.manifold.kinematics.radius2 = 0.0; + } + GJKResult::NoIntersection(dir) => { + workspace.last_gjk_dir = Some(dir); + } + _ => {} + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 562f962..1ccb2c8 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -30,6 +30,9 @@ pub type Triangle = ncollide::shape::Triangle; pub type Ball = ncollide::shape::Ball; /// A heightfield shape. pub type HeightField = ncollide::shape::HeightField; +/// A cylindrical shape. +#[cfg(feature = "dim3")] +pub type Cylinder = ncollide::shape::Cylinder; /// An axis-aligned bounding box. pub type AABB = ncollide::bounding_volume::AABB; /// Event triggered when two non-sensor colliders start or stop being in contact. @@ -51,6 +54,8 @@ pub(crate) use self::contact::WContact; pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal}; pub(crate) use self::narrow_phase::ContactManifoldIndex; #[cfg(feature = "dim3")] +pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap; +#[cfg(feature = "dim3")] pub(crate) use self::polyhedron_feature3d::PolyhedronFace; pub(crate) use self::waabb::{WRay, WAABB}; pub(crate) use self::wquadtree::WQuadtree; @@ -81,3 +86,5 @@ mod trimesh; mod waabb; mod wquadtree; //mod z_order; +#[cfg(feature = "dim3")] +mod polygonal_feature_map; diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs new file mode 100644 index 0000000..8b047fc --- /dev/null +++ b/src/geometry/polygonal_feature_map.rs @@ -0,0 +1,65 @@ +use crate::geometry::PolyhedronFace; +use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle}; +use crate::math::{Point, Vector}; +use approx::AbsDiffEq; +use na::{Unit, Vector2, Vector3}; +use ncollide::shape::Segment; +use ncollide::shape::SupportMap; + +/// Trait implemented by convex shapes with features with polyhedral approximations. +pub trait PolygonalFeatureMap: SupportMap { + fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace); +} + +impl PolygonalFeatureMap for Segment { + fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { + *out_feature = PolyhedronFace::from(*self); + } +} + +impl PolygonalFeatureMap for Triangle { + fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { + *out_feature = PolyhedronFace::from(*self); + } +} + +impl PolygonalFeatureMap for Cuboid { + fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace) { + let face = cuboid::support_face(self, **dir); + *out_feature = PolyhedronFace::from(face); + } +} + +impl PolygonalFeatureMap for Cylinder { + fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { + let dir2 = Vector2::new(dir.x, dir.z) + .try_normalize(f32::default_epsilon()) + .unwrap_or(Vector2::x()); + + if dir.y.abs() < 0.5 { + // We return a segment lying on the cylinder's curved part. + out_features.vertices[0] = Point::new( + dir2.x * self.radius, + -self.half_height, + dir2.y * self.radius, + ); + 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.num_vertices = 2; + out_features.vids = [0, 1, 1, 1]; // FIXME + } else { + // We return a square approximation of the cylinder cap. + let y = self.half_height.copysign(dir.y); + out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius); + 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 + } + } +} diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index dfeee29..5655f64 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -50,6 +50,16 @@ impl From> for PolyhedronFace { } impl PolyhedronFace { + pub fn new() -> Self { + Self { + vertices: [Point::origin(); 4], + vids: [0; 4], + eids: [0; 4], + fid: 0, + num_vertices: 0, + } + } + pub fn transform_by(&mut self, iso: &Isometry) { for v in &mut self.vertices[0..self.num_vertices] { *v = iso * *v; diff --git a/src/utils.rs b/src/utils.rs index ecdd4fd..04e6a3a 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -91,7 +91,7 @@ impl> WSign> for Vector3 { impl WSign for SimdFloat { fn copy_sign_to(self, to: SimdFloat) -> SimdFloat { - self.simd_copysign(to) + to.simd_copysign(self) } } diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 217da48..ca2d71f 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -26,6 +26,7 @@ use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape}; //#[cfg(feature = "fluids")] //use crate::objects::FluidRenderingMode; use crate::objects::capsule::Capsule; +use crate::objects::cylinder::Cylinder; use crate::objects::mesh::Mesh; use rand::{Rng, SeedableRng}; use rand_pcg::Pcg32; @@ -392,6 +393,14 @@ impl GraphicsManager { color, window, ))), + #[cfg(feature = "dim3")] + Shape::Cylinder(cylinder) => out.push(Node::Cylinder(Cylinder::new( + handle, + cylinder.half_height, + cylinder.radius, + color, + window, + ))), } } diff --git a/src_testbed/objects/cylinder.rs b/src_testbed/objects/cylinder.rs new file mode 100644 index 0000000..01a6737 --- /dev/null +++ b/src_testbed/objects/cylinder.rs @@ -0,0 +1,74 @@ +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window::Window; +use na::Point3; +use rapier::geometry::{ColliderHandle, ColliderSet}; +use rapier::math::Isometry; + +pub struct Cylinder { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl Cylinder { + pub fn new( + collider: ColliderHandle, + half_height: f32, + radius: f32, + color: Point3, + window: &mut Window, + ) -> Cylinder { + #[cfg(feature = "dim2")] + let node = window.add_rectangle(radius, half_height); + #[cfg(feature = "dim3")] + let node = window.add_cylinder(radius, half_height * 2.0); + + let mut res = Cylinder { + color, + base_color: color, + gfx: node, + collider, + }; + + // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten"); + res.gfx.set_color(color.x, color.y, color.z); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + self.color = color; + self.base_color = color; + } + + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.collider, + &self.color, + &Isometry::identity(), + ); + } + + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.collider + } +} diff --git a/src_testbed/objects/mod.rs b/src_testbed/objects/mod.rs index 82895b3..51db9d4 100644 --- a/src_testbed/objects/mod.rs +++ b/src_testbed/objects/mod.rs @@ -2,6 +2,7 @@ pub mod ball; pub mod box_node; pub mod capsule; pub mod convex; +pub mod cylinder; pub mod heightfield; pub mod mesh; pub mod node; diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs index 93b5eac..14668e8 100644 --- a/src_testbed/objects/node.rs +++ b/src_testbed/objects/node.rs @@ -10,6 +10,7 @@ use crate::objects::mesh::Mesh; use kiss3d::window::Window; use na::Point3; +use crate::objects::cylinder::Cylinder; use rapier::geometry::{ColliderHandle, ColliderSet}; use rapier::math::Isometry; @@ -28,6 +29,7 @@ pub enum Node { // Polyline(Polyline), Mesh(Mesh), Convex(Convex), + Cylinder(Cylinder), } impl Node { @@ -42,6 +44,7 @@ impl Node { // Node::Polyline(ref mut n) => n.select(), Node::Mesh(ref mut n) => n.select(), Node::Convex(ref mut n) => n.select(), + Node::Cylinder(ref mut n) => n.select(), } } @@ -56,6 +59,7 @@ impl Node { // Node::Polyline(ref mut n) => n.unselect(), Node::Mesh(ref mut n) => n.unselect(), Node::Convex(ref mut n) => n.unselect(), + Node::Cylinder(ref mut n) => n.unselect(), } } @@ -70,6 +74,7 @@ impl Node { // Node::Polyline(ref mut n) => n.update(colliders), Node::Mesh(ref mut n) => n.update(colliders), Node::Convex(ref mut n) => n.update(colliders), + Node::Cylinder(ref mut n) => n.update(colliders), } } @@ -97,6 +102,7 @@ impl Node { Node::HeightField(ref n) => Some(n.scene_node()), Node::Mesh(ref n) => Some(n.scene_node()), Node::Convex(ref n) => Some(n.scene_node()), + Node::Cylinder(ref n) => Some(n.scene_node()), #[cfg(feature = "dim2")] _ => None, } @@ -113,6 +119,7 @@ impl Node { Node::HeightField(ref mut n) => Some(n.scene_node_mut()), Node::Mesh(ref mut n) => Some(n.scene_node_mut()), Node::Convex(ref mut n) => Some(n.scene_node_mut()), + Node::Cylinder(ref mut n) => Some(n.scene_node_mut()), #[cfg(feature = "dim2")] _ => None, } @@ -129,6 +136,7 @@ impl Node { // Node::Polyline(ref n) => n.object(), Node::Mesh(ref n) => n.object(), Node::Convex(ref n) => n.object(), + Node::Cylinder(ref n) => n.object(), } } @@ -143,6 +151,7 @@ impl Node { // Node::Polyline(ref mut n) => n.set_color(color), Node::Mesh(ref mut n) => n.set_color(color), Node::Convex(ref mut n) => n.set_color(color), + Node::Cylinder(ref mut n) => n.set_color(color), } } } -- cgit From 8ee3c703d666785207c7db47e3881f2ca9723105 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 13 Oct 2020 18:39:45 +0200 Subject: Fix cylinder inertia tensor computation. --- src/dynamics/mass_properties_capsule.rs | 33 -------------------------- src/dynamics/mass_properties_cylinder.rs | 40 ++++++++++++++++++++++++++++++++ src/dynamics/mod.rs | 1 + 3 files changed, 41 insertions(+), 33 deletions(-) create mode 100644 src/dynamics/mass_properties_cylinder.rs diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index 77ba96d..647cfc7 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -4,27 +4,6 @@ use crate::geometry::Capsule; use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; impl MassProperties { - fn cylinder_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia) { - #[cfg(feature = "dim2")] - { - Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) - } - - #[cfg(feature = "dim3")] - { - let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; - - let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); - (volume, inertia) - } - } - pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { let half_height = (b - a).norm() / 2.0; let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); @@ -57,16 +36,4 @@ impl MassProperties { ) } } - - #[cfg(feature = "dim3")] - pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self { - let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); - - Self::with_principal_inertia_frame( - Point::origin(), - cyl_vol * density, - cyl_unit_i * density, - Rotation::identity(), - ) - } } diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs new file mode 100644 index 0000000..66a1343 --- /dev/null +++ b/src/dynamics/mass_properties_cylinder.rs @@ -0,0 +1,40 @@ +use crate::dynamics::MassProperties; +#[cfg(feature = "dim3")] +use crate::geometry::Capsule; +use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; + +impl MassProperties { + pub(crate) fn cylinder_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia) { + #[cfg(feature = "dim2")] + { + Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) + } + + #[cfg(feature = "dim3")] + { + let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; + + let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); + (volume, inertia) + } + } + + #[cfg(feature = "dim3")] + pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self { + let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); + let cyl_mass = cyl_vol * density; + + Self::with_principal_inertia_frame( + Point::origin(), + cyl_mass, + cyl_unit_i * cyl_mass, + Rotation::identity(), + ) + } +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 4499d95..512bd8b 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -23,6 +23,7 @@ mod mass_properties; mod mass_properties_ball; mod mass_properties_capsule; mod mass_properties_cuboid; +mod mass_properties_cylinder; #[cfg(feature = "dim2")] mod mass_properties_polygon; mod rigid_body; -- cgit From faf3e7e0f7f2b528da99343f9a3f8ce2b8fa6876 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 13 Oct 2020 18:40:59 +0200 Subject: Implement a special case for edge-edge 3D polygonal clipping. --- build/rapier2d/Cargo.toml | 2 +- build/rapier3d/Cargo.toml | 2 +- build/rapier_testbed2d/Cargo.toml | 4 +- build/rapier_testbed3d/Cargo.toml | 4 +- src/geometry/contact.rs | 8 +- .../cuboid_capsule_contact_generator.rs | 4 +- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 4 +- src/geometry/contact_generator/mod.rs | 5 +- .../contact_generator/pfm_pfm_contact_generator.rs | 159 +++++++++++++++++---- .../polygon_polygon_contact_generator.rs | 2 +- src/geometry/mod.rs | 3 +- src/geometry/polyhedron_feature3d.rs | 137 +++++++++++++++++- 13 files changed, 289 insertions(+), 47 deletions(-) diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 6d60b37..03ccd98 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -36,7 +36,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.22" -ncollide2d = "0.24" +ncollide2d = "0.25" simba = "^0.2.1" approx = "0.3" rayon = { version = "1", optional = true } diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index dcc2125..7a0139e 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -36,7 +36,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.22" -ncollide3d = "0.24" +ncollide3d = "0.25" simba = "^0.2.1" approx = "0.3" rayon = { version = "1", optional = true } diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index eeecb2a..95595bb 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -24,14 +24,14 @@ other-backends = [ "wrapped2d", "nphysics2d" ] [dependencies] nalgebra = "0.22" -kiss3d = { version = "0.25", features = [ "conrod" ] } +kiss3d = { version = "0.26", features = [ "conrod" ] } rand = "0.7" rand_pcg = "0.2" instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" num_cpus = { version = "1", optional = true } wrapped2d = { version = "0.4", optional = true } -ncollide2d = "0.24" +ncollide2d = "0.25" nphysics2d = { version = "0.17", optional = true } crossbeam = "0.7" bincode = "1" diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index 7675701..46edd3b 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -23,14 +23,14 @@ other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ] [dependencies] nalgebra = "0.22" -kiss3d = { version = "0.25", features = [ "conrod" ] } +kiss3d = { version = "0.26", features = [ "conrod" ] } rand = "0.7" rand_pcg = "0.2" instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" glam = { version = "0.8", optional = true } num_cpus = { version = "1", optional = true } -ncollide3d = "0.24" +ncollide3d = "0.25" nphysics3d = { version = "0.17", optional = true } physx = { version = "0.6", optional = true } physx-sys = { version = "0.4", optional = true } diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 7e235c2..782175a 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -429,7 +429,7 @@ impl ContactManifold { } #[inline] - pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry) -> bool { + pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry, early_stop: bool) -> bool { if self.points.len() == 0 { return false; } @@ -439,7 +439,7 @@ impl ContactManifold { let local_n2 = pos12 * self.local_n2; - if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD { + if early_stop && -self.local_n1.dot(&local_n2) < DOT_THRESHOLD { return false; } @@ -448,7 +448,7 @@ impl ContactManifold { let dpt = local_p2 - pt.local_p1; let dist = dpt.dot(&self.local_n1); - if dist * pt.dist < 0.0 { + if early_stop && dist * pt.dist < 0.0 { // We switched between penetrating/non-penetrating. // The may result in other contacts to appear. return false; @@ -456,7 +456,7 @@ impl ContactManifold { let new_local_p1 = local_p2 - self.local_n1 * dist; let dist_threshold = 0.001; // FIXME: this should not be hard-coded. - if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold { + if early_stop && na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold { return false; } diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index a7857a1..c94b300 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -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)) - || (swapped && manifold.try_update_contacts(&pos21)) + if (!swapped && manifold.try_update_contacts(&pos12, true)) + || (swapped && manifold.try_update_contacts(&pos21, true)) { return; } diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index d879a22..04ac43a 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -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) { + if manifold.try_update_contacts(&pos12, true) { return; } diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 1a0358d..d73e2eb 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -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)) - || (swapped && manifold.try_update_contacts(&pos21)) + if (!swapped && manifold.try_update_contacts(&pos12, true)) + || (swapped && manifold.try_update_contacts(&pos21, true)) { return; } diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index a6bad05..d8a523f 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -27,10 +27,9 @@ pub use self::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, }; +pub(crate) use self::polygon_polygon_contact_generator::clip_segments; #[cfg(feature = "dim2")] -pub(crate) use self::polygon_polygon_contact_generator::{ - clip_segments, clip_segments_with_normal, -}; +pub(crate) use self::polygon_polygon_contact_generator::clip_segments_with_normal; mod ball_ball_contact_generator; mod ball_convex_contact_generator; diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index cfb4472..a5e8014 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -1,6 +1,9 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Contact, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace}; +use crate::geometry::{ + Contact, ContactManifold, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace, +}; use crate::math::{Isometry, Vector}; +use crate::na::UnitQuaternion; use na::Unit; use ncollide::query; use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex}; @@ -44,7 +47,7 @@ fn do_generate_contacts( let pos12 = ctxt.position1.inverse() * ctxt.position2; let pos21 = pos12.inverse(); - // if ctxt.manifold.try_update_contacts(&pos12) { + // if ctxt.manifold.try_update_contacts(&pos12, true) { // return; // } @@ -77,27 +80,86 @@ fn do_generate_contacts( pfm2.local_support_feature(&normal2, &mut workspace.feature2); workspace.feature2.transform_by(&pos12); - // PolyhedronFace::contacts( - // ctxt.prediction_distance, - // &workspace.feature1, - // &normal1, - // &workspace.feature2, - // &pos21, - // ctxt.manifold, - // ); - - println!( - "Contact patatrac: {:?}, {:?}, {}, {}", - ctxt.manifold.points.len(), - ctxt.position1 * dir, - workspace.feature1.num_vertices, - workspace.feature2.num_vertices + PolyhedronFace::contacts( + ctxt.prediction_distance, + &workspace.feature1, + &normal1, + &workspace.feature2, + &pos21, + ctxt.manifold, ); - if ctxt.manifold.all_contacts().is_empty() { + // 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.radius1 = 0.0; + ctxt.manifold.kinematics.radius2 = 0.0; + } + GJKResult::NoIntersection(dir) => { + workspace.last_gjk_dir = Some(dir); + } + _ => {} + } +} + +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, + pos21: &Isometry, + prediction_distance: f32, + manifold: &mut ContactManifold, + workspace: &mut PfmPfmContactManifoldGeneratorWorkspace, + ) -> Option>> { + 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); - ctxt.manifold.points.push(Contact { + manifold.points.push(Contact { local_p1, local_p2: pos21 * local_p2, impulse: 0.0, @@ -106,18 +168,63 @@ fn do_generate_contacts( fid2: 0, // FIXME dist, }); + + Some(dir) } + GJKResult::NoIntersection(dir) => Some(dir), + _ => None, + } + } - // Adjust points to take the radius into account. - ctxt.manifold.local_n1 = *normal1; - ctxt.manifold.local_n2 = *normal2; + 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); } - GJKResult::NoIntersection(dir) => { - workspace.last_gjk_dir = Some(dir); - } - _ => {} } } diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 33b54e4..9fc1591 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -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) { + if manifold.try_update_contacts(&m12, true) { return; } diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 1ccb2c8..efbb35c 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -50,8 +50,9 @@ 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_generator::clip_segments; #[cfg(feature = "dim2")] -pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal}; +pub(crate) use self::contact_generator::clip_segments_with_normal; pub(crate) use self::narrow_phase::ContactManifoldIndex; #[cfg(feature = "dim3")] pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap; diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 5655f64..8f5de04 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -1,4 +1,5 @@ -use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle}; +use crate::approx::AbsDiffEq; +use crate::geometry::{self, Contact, ContactManifold, CuboidFeatureFace, Triangle}; use crate::math::{Isometry, Point, Vector}; use crate::utils::WBasis; use na::Point2; @@ -73,6 +74,140 @@ impl PolyhedronFace { face2: &PolyhedronFace, pos21: &Isometry, manifold: &mut ContactManifold, + ) { + match (face1.num_vertices, face2.num_vertices) { + (2, 2) => Self::contacts_edge_edge( + prediction_distance, + face1, + sep_axis1, + face2, + pos21, + manifold, + ), + _ => Self::contacts_face_face( + prediction_distance, + face1, + sep_axis1, + face2, + pos21, + manifold, + ), + } + } + + fn contacts_edge_edge( + prediction_distance: f32, + face1: &PolyhedronFace, + sep_axis1: &Vector, + face2: &PolyhedronFace, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + // Project the faces to a 2D plane for contact clipping. + // The plane they are projected onto has normal sep_axis1 + // and contains the origin (this is numerically OK because + // we are not working in world-space here). + let basis = sep_axis1.orthonormal_basis(); + let projected_edge1 = [ + Point2::new( + face1.vertices[0].coords.dot(&basis[0]), + face1.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[1].coords.dot(&basis[0]), + face1.vertices[1].coords.dot(&basis[1]), + ), + ]; + let projected_edge2 = [ + Point2::new( + face2.vertices[0].coords.dot(&basis[0]), + face2.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[1].coords.dot(&basis[0]), + face2.vertices[1].coords.dot(&basis[1]), + ), + ]; + + let tangent1 = + (projected_edge1[1] - projected_edge1[0]).try_normalize(f32::default_epsilon()); + let tangent2 = + (projected_edge2[1] - projected_edge2[0]).try_normalize(f32::default_epsilon()); + + // TODO: not sure what the best value for eps is. + // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small. + if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) { + let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8; + + if !parallel { + let seg1 = (&projected_edge1[0], &projected_edge1[1]); + let seg2 = (&projected_edge2[0], &projected_edge2[1]); + let (loc1, loc2) = + ncollide::query::closest_points_segment_segment_with_locations_nD(seg1, seg2); + + // Found a contact between the two edges. + let bcoords1 = loc1.barycentric_coordinates(); + let bcoords2 = loc2.barycentric_coordinates(); + + let edge1 = (face1.vertices[0], face1.vertices[1]); + let edge2 = (face2.vertices[0], face2.vertices[1]); + let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1]; + let local_p2 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1]; + let dist = (local_p2 - local_p1).dot(&sep_axis1); + + if dist <= prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.eids[0], + fid2: face2.eids[0], + dist, + }); + } + + return; + } + } + + // The lines are parallel so we are having a conformal contact. + // Let's use a range-based clipping to extract two contact points. + // TODO: would it be better and/or more efficient to do the + //clipping in 2D? + if let Some(clips) = geometry::clip_segments( + (face1.vertices[0], face1.vertices[1]), + (face2.vertices[0], face2.vertices[1]), + ) { + manifold.points.push(Contact { + local_p1: (clips.0).0, + local_p2: pos21 * (clips.0).1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: 0, // FIXME + fid2: 0, // FIXME + dist: ((clips.0).1 - (clips.0).0).dot(&sep_axis1), + }); + + manifold.points.push(Contact { + local_p1: (clips.1).0, + local_p2: pos21 * (clips.1).1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: 0, // FIXME + fid2: 0, // FIXME + dist: ((clips.1).1 - (clips.1).0).dot(&sep_axis1), + }); + } + } + + fn contacts_face_face( + prediction_distance: f32, + face1: &PolyhedronFace, + sep_axis1: &Vector, + face2: &PolyhedronFace, + pos21: &Isometry, + manifold: &mut ContactManifold, ) { // Project the faces to a 2D plane for contact clipping. // The plane they are projected onto has normal sep_axis1 -- cgit From 947c4813c9666fd8215743de298fe17780fa3ef2 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 19 Oct 2020 16:51:40 +0200 Subject: Complete the pfm/pfm contact generator. --- examples3d/all_examples3.rs | 2 + examples3d/debug_cylinder3.rs | 65 +++++++++ examples3d/heightfield3.rs | 14 +- examples3d/primitives3.rs | 25 ++-- examples3d/trimesh3.rs | 14 +- src/dynamics/rigid_body.rs | 3 +- src/geometry/contact.rs | 25 ++-- .../cuboid_capsule_contact_generator.rs | 4 +- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 4 +- .../contact_generator/pfm_pfm_contact_generator.rs | 147 +++------------------ .../polygon_polygon_contact_generator.rs | 2 +- src/geometry/polygonal_feature_map.rs | 36 ++++- src/utils.rs | 3 +- 14 files changed, 165 insertions(+), 181 deletions(-) create mode 100644 examples3d/debug_cylinder3.rs diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 64eb5b6..85b2504 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -13,6 +13,7 @@ use std::cmp::Ordering; mod add_remove3; mod compound3; mod debug_boxes3; +mod debug_cylinder3; mod debug_triangle3; mod debug_trimesh3; mod domino3; @@ -76,6 +77,7 @@ pub fn main() { ("(Debug) boxes", debug_boxes3::init_world), ("(Debug) triangle", debug_triangle3::init_world), ("(Debug) trimesh", debug_trimesh3::init_world), + ("(Debug) cylinder", debug_cylinder3::init_world), ]; // Lexicographic sort, with stress tests moved at the end of the list. diff --git a/examples3d/debug_cylinder3.rs b/examples3d/debug_cylinder3.rs new file mode 100644 index 0000000..4e7fae1 --- /dev/null +++ b/examples3d/debug_cylinder3.rs @@ -0,0 +1,65 @@ +use na::{Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +// This shows a bug when a cylinder is in contact with a very large +// but very thin cuboid. In this case the EPA returns an incorrect +// contact normal, resulting in the cylinder falling through the floor. +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 100.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 1; + let rad = 1.0; + + let shiftx = rad * 2.0 + rad; + let shifty = rad * 2.0 + rad; + let shiftz = rad * 2.0 + rad; + let centerx = shiftx * (num / 2) as f32; + let centery = shifty / 2.0; + let centerz = shiftz * (num / 2) as f32; + + let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; + + let x = -centerx + offset; + let y = centery + 3.0; + let z = -centerz + offset; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cylinder(rad, rad).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index 09df815..826df4c 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -54,13 +54,13 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); - colliders.insert(collider, handle, &mut bodies); - } else { - let collider = ColliderBuilder::ball(rad).build(); - colliders.insert(collider, handle, &mut bodies); - } + let collider = match j % 3 { + 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), + 1 => ColliderBuilder::ball(rad).build(), + _ => ColliderBuilder::cylinder(rad, rad).build(), + }; + + colliders.insert(collider, handle, &mut bodies); } } } diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index cf678b9..c3fa799 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -1,4 +1,4 @@ -use na::Point3; +use na::{Point3, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -15,7 +15,7 @@ pub fn init_world(testbed: &mut Testbed) { * Ground */ let ground_size = 100.1; - let ground_height = 0.1; + let ground_height = 2.1; let rigid_body = RigidBodyBuilder::new_static() .translation(0.0, -ground_height, 0.0) @@ -30,29 +30,30 @@ pub fn init_world(testbed: &mut Testbed) { let num = 8; let rad = 1.0; - let shift = rad * 2.0 + rad; - let centerx = shift * (num / 2) as f32; - let centery = shift / 2.0; - let centerz = shift * (num / 2) as f32; + let shiftx = rad * 2.0 + rad; + let shifty = rad * 2.0 + rad; + let shiftz = rad * 2.0 + rad; + let centerx = shiftx * (num / 2) as f32; + let centery = shifty / 2.0; + let centerz = shiftz * (num / 2) as f32; let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; for j in 0usize..20 { for i in 0..num { for k in 0usize..num { - let x = i as f32 * shift - centerx + offset; - let y = j as f32 * shift + centery + 3.0; - let z = k as f32 * shift - centerz + offset; + let x = i as f32 * shiftx - centerx + offset; + let y = j as f32 * shifty + centery + 3.0; + let z = k as f32 * shiftz - centerz + offset; // Build the rigid body. let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - let collider = match j % 2 { + let collider = match j % 3 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - // 2 => ColliderBuilder::capsule_y(rad, rad).build(), - _ => unreachable!(), + _ => ColliderBuilder::cylinder(rad, rad).build(), }; colliders.insert(collider, handle, &mut bodies); diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index d022a1f..40aede2 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -64,13 +64,13 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); - colliders.insert(collider, handle, &mut bodies); - } else { - let collider = ColliderBuilder::ball(rad).build(); - colliders.insert(collider, handle, &mut bodies); - } + let collider = match j % 3 { + 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), + 1 => ColliderBuilder::ball(rad).build(), + _ => ColliderBuilder::cylinder(rad, rad).build(), + }; + + colliders.insert(collider, handle, &mut bodies); } } } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index af1fb4a..9fa5a8e 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -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, linvel: Vector, diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 782175a..d211cf1 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -429,17 +429,27 @@ impl ContactManifold { } #[inline] - pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry, early_stop: bool) -> bool { + pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry) -> 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, + 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; } diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index c94b300..a7857a1 100644 --- a/src/