From 76118d6885a0aa6420b4a5a13271e8087bde3a42 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Sat, 10 Oct 2020 12:15:43 +0200 Subject: [PATCH 01/41] WQuadtree: fix stack overflow caused by more than 4 AABB with the same center. --- examples3d/debug_boxes3.rs | 32 ++++++++++++++++++-------------- src/geometry/wquadtree.rs | 33 ++++++++++++++++++++++++++++++--- 2 files changed, 48 insertions(+), 17 deletions(-) diff --git a/examples3d/debug_boxes3.rs b/examples3d/debug_boxes3.rs index 7237fd9..919cdd6 100644 --- a/examples3d/debug_boxes3.rs +++ b/examples3d/debug_boxes3.rs @@ -17,22 +17,26 @@ pub fn init_world(testbed: &mut Testbed) { 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); + for _ in 0..6 { + 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); + } // Build the dynamic box rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(1.1, 0.0, 0.0) - .rotation(Vector3::new(0.8, 0.2, 0.1)) - .can_sleep(false) - .build(); - let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build(); - colliders.insert(collider, handle, &mut bodies); + for _ in 0..6 { + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(1.1, 0.0, 0.0) + .rotation(Vector3::new(0.8, 0.2, 0.1)) + .can_sleep(false) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } /* * Set up the testbed. diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index fce04eb..deab4a2 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -539,7 +539,7 @@ fn split_indices_wrt_dim<'a>( dim: usize, ) -> (&'a mut [usize], &'a mut [usize]) { let mut icurr = 0; - let mut ilast = indices.len() - 1; + let mut ilast = indices.len(); // The loop condition we can just do 0..indices.len() // instead of the test icurr < ilast because we know @@ -549,12 +549,39 @@ fn split_indices_wrt_dim<'a>( let center = aabbs[i].center(); if center[dim] > split_point[dim] { - indices.swap(icurr, ilast); ilast -= 1; + indices.swap(icurr, ilast); } else { icurr += 1; } } - indices.split_at_mut(icurr) + if icurr == 0 || icurr == indices.len() { + // We don't want to return one empty set. But + // this can happen if all the coordinates along the + // given dimension are equal. + // In this is the case, we just split in the middle. + let half = indices.len() / 2; + indices.split_at_mut(half) + } else { + indices.split_at_mut(icurr) + } +} + +#[cfg(test)] +mod test { + use crate::geometry::{WQuadtree, AABB}; + use crate::math::{Point, Vector}; + + #[test] + fn multiple_identical_AABB_stack_overflow() { + // A stack overflow was caused during the construction of the + // WAABB tree with more than four AABB with the same center. + let aabb = AABB::new(Point::origin(), Vector::repeat(1.0).into()); + + for k in 0..20 { + let mut tree = WQuadtree::new(); + tree.clear_and_rebuild((0..k).map(|i| (i, aabb)), 0.0); + } + } } From f8acf6a5e9d3ba537dac6502b0e0541236b418c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 12 Oct 2020 09:47:40 +0200 Subject: [PATCH 02/41] Release v0.2.1 --- CHANGELOG | 6 +++++- build/rapier2d/Cargo.toml | 2 +- build/rapier3d/Cargo.toml | 2 +- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/CHANGELOG b/CHANGELOG index 70b20e5..06eea7d 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -1,4 +1,8 @@ -## v0.2.0 - WIP + +## v0.2.1 +- Fix panic in TriMesh construction and QueryPipeline update caused by a stack overflow or a subtraction underflow. + +## v0.2.0 The most significant change on this version is the addition of the `QueryPipeline` responsible for performing scene-wide queries. So far only ray-casting has been implemented. diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index ff5253b..6d60b37 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -1,7 +1,7 @@ # Name idea: bident for 2D and trident for 3D [package] name = "rapier2d" -version = "0.2.0" +version = "0.2.1" authors = [ "Sébastien Crozet " ] description = "2-dimensional physics engine in Rust." documentation = "http://docs.rs/rapier2d" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index 130e5dd..dcc2125 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -1,7 +1,7 @@ # Name idea: bident for 2D and trident for 3D [package] name = "rapier3d" -version = "0.2.0" +version = "0.2.1" authors = [ "Sébastien Crozet " ] description = "3-dimensional physics engine in Rust." documentation = "http://docs.rs/rapier3d" From faec3d5d46c88e2949179dd2789899e5cf26ed48 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 12 Oct 2020 18:33:58 +0200 Subject: [PATCH 03/41] 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 + .../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), } } } From 8ee3c703d666785207c7db47e3881f2ca9723105 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 13 Oct 2020 18:39:45 +0200 Subject: [PATCH 04/41] 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; From faf3e7e0f7f2b528da99343f9a3f8ce2b8fa6876 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 13 Oct 2020 18:40:59 +0200 Subject: [PATCH 05/41] 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 +- .../pfm_pfm_contact_generator.rs | 167 ++++++++++++++---- .../polygon_polygon_contact_generator.rs | 2 +- src/geometry/mod.rs | 3 +- src/geometry/polyhedron_feature3d.rs | 137 +++++++++++++- 13 files changed, 293 insertions(+), 51 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,36 +80,28 @@ 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() { - // 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, - }); - } + // 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; @@ -121,3 +116,115 @@ 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, + 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); + 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); + } + } +} 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 From 947c4813c9666fd8215743de298fe17780fa3ef2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 19 Oct 2020 16:51:40 +0200 Subject: [PATCH 06/41] 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 +- .../pfm_pfm_contact_generator.rs | 149 ++---------------- .../polygon_polygon_contact_generator.rs | 2 +- src/geometry/polygonal_feature_map.rs | 36 ++++- src/utils.rs | 3 +- 14 files changed, 166 insertions(+), 182 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/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, true)) - || (swapped && manifold.try_update_contacts(&pos21, true)) + if (!swapped && manifold.try_update_contacts(&pos12)) + || (swapped && manifold.try_update_contacts(&pos21)) { return; } diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index 04ac43a..d879a22 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, true) { + if manifold.try_update_contacts(&pos12) { return; } diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index d73e2eb..1a0358d 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, true)) - || (swapped && manifold.try_update_contacts(&pos21, true)) + if (!swapped && manifold.try_update_contacts(&pos12)) + || (swapped && manifold.try_update_contacts(&pos21)) { return; } diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index a5e8014..c3815dd 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -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, - 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); - 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); } diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 9fc1591..33b54e4 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, true) { + if manifold.try_update_contacts(&m12) { return; } diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index 8b047fc..70be5d0 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -32,6 +32,20 @@ impl PolygonalFeatureMap for Cuboid { impl PolygonalFeatureMap for Cylinder { fn local_support_feature(&self, dir: &Unit>, 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]; + } } } } diff --git a/src/utils.rs b/src/utils.rs index 04e6a3a..bd972b8 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -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; From 865ce8a8e5301b23ca474adaaffe8b43e725803e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 20 Oct 2020 11:55:33 +0200 Subject: [PATCH 07/41] Collider shape: use a trait-object instead of an enum. --- build/rapier2d/Cargo.toml | 5 +- build/rapier3d/Cargo.toml | 6 +- src/dynamics/mass_properties.rs | 4 +- src/dynamics/mass_properties_capsule.rs | 11 +- src/geometry/broad_phase_multi_sap.rs | 16 +- src/geometry/collider.rs | 395 +++++++++--------- .../ball_convex_contact_generator.rs | 23 +- .../capsule_capsule_contact_generator.rs | 15 +- .../contact_generator/contact_dispatcher.rs | 78 ++-- .../cuboid_capsule_contact_generator.rs | 8 +- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 6 +- .../heightfield_shape_contact_generator.rs | 35 +- src/geometry/contact_generator/mod.rs | 2 +- .../polygon_polygon_contact_generator.rs | 29 +- .../trimesh_shape_contact_generator.rs | 11 +- src/geometry/mod.rs | 12 +- src/geometry/narrow_phase.rs | 12 +- .../ball_convex_proximity_detector.rs | 18 +- .../cuboid_cuboid_proximity_detector.rs | 2 +- .../cuboid_triangle_proximity_detector.rs | 6 +- .../polygon_polygon_proximity_detector.rs | 23 +- .../proximity_dispatcher.rs | 40 +- .../trimesh_shape_proximity_detector.rs | 11 +- src/geometry/rounded.rs | 7 + src/geometry/sat.rs | 20 +- src/geometry/shape.rs | 275 ++++++++++++ src/geometry/trimesh.rs | 45 +- src/pipeline/query_pipeline.rs | 12 +- src/utils.rs | 26 ++ src_testbed/engine.rs | 65 +-- 31 files changed, 782 insertions(+), 438 deletions(-) create mode 100644 src/geometry/rounded.rs create mode 100644 src/geometry/shape.rs diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 03ccd98..44038e6 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -22,7 +22,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -46,7 +46,10 @@ arrayvec = "0.5" bit-vec = "0.6" rustc-hash = "1" serde = { version = "1", features = [ "derive" ], optional = true } +erased-serde = { version = "0.3", optional = true } indexmap = { version = "1", features = [ "serde-1" ], optional = true } +downcast-rs = "1.2" +num-derive = "0.3" [dev-dependencies] bincode = "1" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index 7a0139e..adc453c 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -22,7 +22,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -46,7 +46,11 @@ arrayvec = "0.5" bit-vec = "0.6" rustc-hash = "1" serde = { version = "1", features = [ "derive" ], optional = true } +erased-serde = { version = "0.3", optional = true } indexmap = { version = "1", features = [ "serde-1" ], optional = true } +downcast-rs = "1.2" +num-derive = "0.3" + [dev-dependencies] bincode = "1" diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index 22e7da5..d64839c 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -91,7 +91,7 @@ impl MassProperties { } #[cfg(feature = "dim3")] - /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii. + /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes. pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3 { let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); self.principal_inertia_local_frame.to_rotation_matrix() @@ -103,7 +103,7 @@ impl MassProperties { } #[cfg(feature = "dim3")] - /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. + /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes. pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); self.principal_inertia_local_frame.to_rotation_matrix() diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index 647cfc7..c4e039c 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -4,21 +4,19 @@ use crate::geometry::Capsule; use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; impl MassProperties { - pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { - let half_height = (b - a).norm() / 2.0; + pub(crate) fn from_capsule(density: f32, half_height: f32, radius: f32) -> Self { let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); let cap_vol = cyl_vol + ball_vol; let cap_mass = cap_vol * density; let mut cap_unit_i = cyl_unit_i + ball_unit_i; - let local_com = na::center(&a, &b); #[cfg(feature = "dim2")] { let h = half_height * 2.0; let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; cap_unit_i += extra; - Self::new(local_com, cap_mass, cap_unit_i * cap_mass) + Self::new(Point::origin(), cap_mass, cap_unit_i * cap_mass) } #[cfg(feature = "dim3")] @@ -27,12 +25,11 @@ impl MassProperties { let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; cap_unit_i.x += extra; cap_unit_i.z += extra; - let local_frame = Capsule::new(a, b, radius).rotation_wrt_y(); Self::with_principal_inertia_frame( - local_com, + Point::origin(), cap_mass, cap_unit_i * cap_mass, - local_frame, + Rotation::identity(), ) } } diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 8e69d24..3562c2e 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -335,7 +335,7 @@ impl SAPAxis { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct SAPRegion { - axii: [SAPAxis; DIM], + axes: [SAPAxis; DIM], existing_proxies: BitVec, #[cfg_attr(feature = "serde-serialize", serde(skip))] to_insert: Vec, // Workspace @@ -344,14 +344,14 @@ struct SAPRegion { impl SAPRegion { pub fn new(bounds: AABB) -> Self { - let axii = [ + let axes = [ SAPAxis::new(bounds.mins.x, bounds.maxs.x), SAPAxis::new(bounds.mins.y, bounds.maxs.y), #[cfg(feature = "dim3")] SAPAxis::new(bounds.mins.z, bounds.maxs.z), ]; SAPRegion { - axii, + axes, existing_proxies: BitVec::new(), to_insert: Vec::new(), need_update: false, @@ -386,15 +386,15 @@ impl SAPRegion { // Update endpoints. let mut deleted_any = false; for dim in 0..DIM { - self.axii[dim].update_endpoints(dim, proxies, reporting); - deleted_any = self.axii[dim] + self.axes[dim].update_endpoints(dim, proxies, reporting); + deleted_any = self.axes[dim] .delete_out_of_bounds_proxies(&mut self.existing_proxies) || deleted_any; } if deleted_any { for dim in 0..DIM { - self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); + self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); } } @@ -404,9 +404,9 @@ impl SAPRegion { if !self.to_insert.is_empty() { // Insert new proxies. for dim in 1..DIM { - self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None); + self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None); } - self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); + self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); self.to_insert.clear(); } } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index fe42bd7..c6cf6d0 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -3,172 +3,186 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, - Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh, + Polygon, Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, }; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; +use downcast_rs::{impl_downcast, DowncastSync}; +use erased_serde::Serialize; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; use ncollide::query::RayCast; use num::Zero; +use std::any::Any; +use std::ops::Deref; +use std::sync::Arc; +/// The shape of a collider. #[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// An enum grouping all the possible shape of a collider. -pub enum Shape { - /// A ball shape. - Ball(Ball), - /// A convex polygon shape. - Polygon(Polygon), - /// A cuboid shape. - Cuboid(Cuboid), - /// A capsule shape. - Capsule(Capsule), - /// A triangle shape. - Triangle(Triangle), - /// A triangle mesh shape. - Trimesh(Trimesh), - /// A heightfield shape. - HeightField(HeightField), - #[cfg(feature = "dim3")] - /// A cylindrical shape. - Cylinder(Cylinder), +pub struct ColliderShape(pub Arc); + +impl Deref for ColliderShape { + type Target = Shape; + fn deref(&self) -> &Shape { + &*self.0 + } } -impl Shape { - /// Gets a reference to the underlying ball shape, if `self` is one. - pub fn as_ball(&self) -> Option<&Ball> { - match self { - Shape::Ball(b) => Some(b), - _ => None, - } +impl ColliderShape { + /// Initialize a ball shape defined by its radius. + pub fn ball(radius: f32) -> Self { + ColliderShape(Arc::new(Ball::new(radius))) } - /// Gets a reference to the underlying polygon shape, if `self` is one. - pub fn as_polygon(&self) -> Option<&Polygon> { - match self { - Shape::Polygon(p) => Some(p), - _ => None, - } - } - - /// Gets a reference to the underlying cuboid shape, if `self` is one. - pub fn as_cuboid(&self) -> Option<&Cuboid> { - match self { - Shape::Cuboid(c) => Some(c), - _ => None, - } - } - - /// Gets a reference to the underlying capsule shape, if `self` is one. - pub fn as_capsule(&self) -> Option<&Capsule> { - match self { - Shape::Capsule(c) => Some(c), - _ => None, - } - } - - /// Gets a reference to the underlying triangle mesh shape, if `self` is one. - pub fn as_trimesh(&self) -> Option<&Trimesh> { - match self { - Shape::Trimesh(c) => Some(c), - _ => None, - } - } - - /// Gets a reference to the underlying heightfield shape, if `self` is one. - pub fn as_heightfield(&self) -> Option<&HeightField> { - match self { - Shape::HeightField(h) => Some(h), - _ => None, - } - } - - /// Gets a reference to the underlying triangle shape, if `self` is one. - pub fn as_triangle(&self) -> Option<&Triangle> { - match self { - Shape::Triangle(c) => Some(c), - _ => None, - } - } - - /// 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. + /// Initialize a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. #[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, - } + pub fn cylinder(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } - /// Computes the axis-aligned bounding box of this shape. - pub fn compute_aabb(&self, position: &Isometry) -> AABB { - match self { - Shape::Ball(ball) => ball.bounding_volume(position), - Shape::Polygon(poly) => poly.aabb(position), - Shape::Capsule(caps) => caps.aabb(position), - Shape::Cuboid(cuboid) => cuboid.bounding_volume(position), - 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), - } + /// Initialize a cuboid shape defined by its half-extents. + pub fn cuboid(half_extents: Vector) -> Self { + ColliderShape(Arc::new(Cuboid::new(half_extents))) } - /// Computes the first intersection point between a ray in this collider. - /// - /// Some shapes are not supported yet and will always return `None`. - /// - /// # Parameters - /// - `position`: the position of this shape. - /// - `ray`: the ray to cast. - /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. - pub fn cast_ray( - &self, - position: &Isometry, - ray: &Ray, - max_toi: f32, - ) -> Option { - match self { - Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true), - Shape::Polygon(_poly) => None, - Shape::Capsule(caps) => { - let pos = position * caps.transform_wrt_y(); - let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius); - caps.toi_and_normal_with_ray(&pos, ray, max_toi, true) + /// Initialize a capsule shape aligned with the `y` axis. + pub fn capsule(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Capsule::new(half_height, radius))) + } + + /// Initializes a triangle shape. + pub fn triangle(a: Point, b: Point, c: Point) -> Self { + ColliderShape(Arc::new(Triangle::new(a, b, c))) + } + + /// Initializes a triangle mesh shape defined by its vertex and index buffers. + pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { + ColliderShape(Arc::new(Trimesh::new(vertices, indices))) + } + + /// Initializes an heightfield shape defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim2")] + pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) + } + + /// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim3")] + pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) + } +} + +#[cfg(feature = "serde-serialize")] +impl serde::Serialize for ColliderShape { + fn serialize(&self, serializer: S) -> Result + where + S: serde::Serializer, + { + use crate::serde::ser::SerializeStruct; + + if let Some(ser) = self.0.as_serialize() { + let typ = self.0.shape_type(); + let mut state = serializer.serialize_struct("ColliderShape", 2)?; + state.serialize_field("tag", &(typ as i32))?; + state.serialize_field("inner", ser)?; + state.end() + } else { + Err(serde::ser::Error::custom( + "Found a non-serializable custom shape.", + )) + } + } +} + +#[cfg(feature = "serde-serialize")] +impl<'de> serde::Deserialize<'de> for ColliderShape { + fn deserialize(deserializer: D) -> Result + where + D: serde::Deserializer<'de>, + { + struct Visitor {}; + impl<'de> serde::de::Visitor<'de> for Visitor { + type Value = ColliderShape; + fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { + write!(formatter, "one shape type tag and the inner shape data") } - Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true), - #[cfg(feature = "dim2")] - Shape::Triangle(_) | Shape::Trimesh(_) => { - // This is not implemented yet in 2D. - None - } - #[cfg(feature = "dim3")] - Shape::Triangle(triangle) => { - triangle.toi_and_normal_with_ray(position, ray, max_toi, true) - } - #[cfg(feature = "dim3")] - Shape::Trimesh(trimesh) => { - trimesh.toi_and_normal_with_ray(position, ray, max_toi, true) - } - 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) + + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + use num::cast::FromPrimitive; + + let tag: i32 = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + + let shape = match ShapeType::from_i32(tag) { + Some(ShapeType::Ball) => { + let shape: Ball = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::Polygon) => { + unimplemented!() + // let shape: Polygon = seq + // .next_element()? + // .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + // Arc::new(shape) as Arc + } + Some(ShapeType::Cuboid) => { + let shape: Cuboid = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::Capsule) => { + let shape: Capsule = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::Triangle) => { + let shape: Triangle = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::Trimesh) => { + let shape: Trimesh = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::HeightField) => { + let shape: HeightField = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + #[cfg(feature = "dim3")] + Some(ShapeType::Cylinder) => { + let shape: Cylinder = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + None => { + return Err(serde::de::Error::custom( + "found invalid shape type to deserialize", + )) + } + }; + + Ok(ColliderShape(shape)) } } + + deserializer.deserialize_struct("ColliderShape", &["tag", "inner"], Visitor {}) } } @@ -177,7 +191,7 @@ impl Shape { /// /// To build a new collider, use the `ColliderBuilder` structure. pub struct Collider { - shape: Shape, + shape: ColliderShape, density: f32, is_sensor: bool, pub(crate) parent: RigidBodyHandle, @@ -245,7 +259,7 @@ impl Collider { /// The geometric shape of this collider. pub fn shape(&self) -> &Shape { - &self.shape + &*self.shape.0 } /// Compute the axis-aligned bounding box of this collider. @@ -261,23 +275,7 @@ impl Collider { /// Compute the local-space mass properties of this collider. pub fn mass_properties(&self) -> MassProperties { - match &self.shape { - Shape::Ball(ball) => MassProperties::from_ball(self.density, ball.radius), - #[cfg(feature = "dim2")] - Shape::Polygon(p) => MassProperties::from_polygon(self.density, p.vertices()), - #[cfg(feature = "dim3")] - Shape::Polygon(_p) => unimplemented!(), - Shape::Cuboid(c) => MassProperties::from_cuboid(self.density, c.half_extents), - Shape::Capsule(caps) => { - MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius) - } - Shape::Triangle(_) | Shape::Trimesh(_) | Shape::HeightField(_) => { - MassProperties::zero() - } - Shape::Cylinder(c) => { - MassProperties::from_cylinder(self.density, c.half_height, c.radius) - } - } + self.shape.mass_properties(self.density) } } @@ -286,7 +284,7 @@ impl Collider { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct ColliderBuilder { /// The shape of the collider to be built. - pub shape: Shape, + pub shape: ColliderShape, /// The density of the collider to be built. density: Option, /// The friction coefficient of the collider to be built. @@ -301,7 +299,7 @@ pub struct ColliderBuilder { impl ColliderBuilder { /// Initialize a new collider builder with the given shape. - pub fn new(shape: Shape) -> Self { + pub fn new(shape: ColliderShape) -> Self { Self { shape, density: None, @@ -320,102 +318,81 @@ impl ColliderBuilder { /// Initialize a new collider builder with a ball shape defined by its radius. pub fn ball(radius: f32) -> Self { - Self::new(Shape::Ball(Ball::new(radius))) + Self::new(ColliderShape::ball(radius)) } /// Initialize a new collider builder with a cylindrical shape defined by its half-height /// (along along the y axis) and its radius. + #[cfg(feature = "dim3")] pub fn cylinder(half_height: f32, radius: f32) -> Self { - Self::new(Shape::Cylinder(Cylinder::new(half_height, radius))) + Self::new(ColliderShape::cylinder(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 { - let cuboid = Cuboid { - half_extents: Vector::new(hx, hy), - }; - - Self::new(Shape::Cuboid(cuboid)) - - /* - use crate::math::Point; - let vertices = vec![ - Point::new(hx, -hy), - Point::new(hx, hy), - Point::new(-hx, hy), - Point::new(-hx, -hy), - ]; - let normals = vec![Vector::x(), Vector::y(), -Vector::x(), -Vector::y()]; - let polygon = Polygon::new(vertices, normals); - - Self::new(Shape::Polygon(polygon)) - */ + Self::new(ColliderShape::cuboid(Vector::new(hx, hy))) } /// Initialize a new collider builder with a capsule shape aligned with the `x` axis. pub fn capsule_x(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_x(half_height, radius); - Self::new(Shape::Capsule(capsule)) + #[cfg(feature = "dim2")] + let rot = -std::f32::consts::FRAC_PI_2; + #[cfg(feature = "dim3")] + let rot = Vector::z() * -std::f32::consts::FRAC_PI_2; + Self::new(ColliderShape::capsule(half_height, radius)) + .position(Isometry::new(na::zero(), rot)) } /// Initialize a new collider builder with a capsule shape aligned with the `y` axis. pub fn capsule_y(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_y(half_height, radius); - Self::new(Shape::Capsule(capsule)) + Self::new(ColliderShape::capsule(half_height, radius)) } /// Initialize a new collider builder with a capsule shape aligned with the `z` axis. #[cfg(feature = "dim3")] pub fn capsule_z(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_z(half_height, radius); - Self::new(Shape::Capsule(capsule)) + let rot = Vector::x() * std::f32::consts::FRAC_PI_2; + Self::new(ColliderShape::capsule(half_height, radius)) + .position(Isometry::new(na::zero(), rot)) } /// Initialize a new collider builder with a cuboid shape defined by its half-extents. #[cfg(feature = "dim3")] pub fn cuboid(hx: f32, hy: f32, hz: f32) -> Self { - let cuboid = Cuboid { - half_extents: Vector::new(hx, hy, hz), - }; - - Self::new(Shape::Cuboid(cuboid)) + Self::new(ColliderShape::cuboid(Vector::new(hx, hy, hz))) } /// Initializes a collider builder with a segment shape. /// /// A segment shape is modeled by a capsule with a 0 radius. pub fn segment(a: Point, b: Point) -> Self { - let capsule = Capsule::new(a, b, 0.0); - Self::new(Shape::Capsule(capsule)) + let (pos, half_height) = crate::utils::segment_to_capsule(&a, &b); + Self::new(ColliderShape::capsule(half_height, 0.0)).position(pos) } /// Initializes a collider builder with a triangle shape. pub fn triangle(a: Point, b: Point, c: Point) -> Self { - let triangle = Triangle::new(a, b, c); - Self::new(Shape::Triangle(triangle)) + Self::new(ColliderShape::triangle(a, b, c)) } /// Initializes a collider builder with a triangle mesh shape defined by its vertex and index buffers. pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { - let trimesh = Trimesh::new(vertices, indices); - Self::new(Shape::Trimesh(trimesh)) + Self::new(ColliderShape::trimesh(vertices, indices)) } /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim2")] pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { - let heightfield = HeightField::new(heights, scale); - Self::new(Shape::HeightField(heightfield)) + Self::new(ColliderShape::heightfield(heights, scale)) } /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim3")] pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { - let heightfield = HeightField::new(heights, scale); - Self::new(Shape::HeightField(heightfield)) + Self::new(ColliderShape::heightfield(heights, scale)) } /// The default friction coefficient used by the collider builder. diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index 0856029..62ebfab 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -5,30 +5,17 @@ use na::Unit; use ncollide::query::PointQuery; pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { - if let Shape::Ball(ball1) = ctxt.shape1 { + if let Some(ball1) = ctxt.shape1.as_ball() { ctxt.manifold.swap_identifiers(); - - match ctxt.shape2 { - 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 { - match ctxt.shape1 { - 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!(), - } + 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>( +fn do_generate_contacts>( point_query1: &P, ball2: &Ball, ctxt: &mut PrimitiveContactGenerationContext, diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index 3800ce6..4d9bbc7 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -8,7 +8,7 @@ use na::Unit; use ncollide::shape::{Segment, SegmentPointLocation}; pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Capsule(capsule1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) { generate_contacts( ctxt.prediction_distance, capsule1, @@ -94,7 +94,7 @@ pub fn generate_contacts<'a>( if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8 && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8 { - // Capsules axii are almost parallel and are almost perpendicular to the normal. + // Capsules axes are almost parallel and are almost perpendicular to the normal. // Find a second contact point. if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal( (capsule1.a, capsule1.b), @@ -156,17 +156,18 @@ pub fn generate_contacts<'a>( let pos12 = pos1.inverse() * pos2; let pos21 = pos12.inverse(); - let capsule2_1 = capsule1.transform_by(&pos12); + let seg1 = capsule1.segment(); + let seg2_1 = capsule2.segment().transformed(&pos12); let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( - (&capsule1.a, &capsule1.b), - (&capsule2_1.a, &capsule2_1.b), + (&seg1.a, &seg1.b), + (&seg2_1.a, &seg2_1.b), ); { let bcoords1 = loc1.barycentric_coordinates(); let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1]; - let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1]; + 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()); diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index e925fd5..01bbc46 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -3,7 +3,7 @@ use crate::geometry::contact_generator::{ PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace, }; -use crate::geometry::Shape; +use crate::geometry::{Shape, ShapeType}; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm @@ -12,8 +12,8 @@ pub trait ContactDispatcher { /// Select the collision-detection algorithm for the given pair of primitive shapes. fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveContactGenerator, Option>, @@ -21,8 +21,8 @@ pub trait ContactDispatcher { /// Select the collision-detection algorithm for the given pair of non-primitive shapes. fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ContactPhase, Option>); } @@ -32,14 +32,14 @@ pub struct DefaultContactDispatcher; impl ContactDispatcher for DefaultContactDispatcher { fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveContactGenerator, Option>, ) { match (shape1, shape2) { - (Shape::Ball(_), Shape::Ball(_)) => ( + (ShapeType::Ball, ShapeType::Ball) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_ball, #[cfg(feature = "simd-is-enabled")] @@ -48,56 +48,58 @@ impl ContactDispatcher for DefaultContactDispatcher { }, None, ), - (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + (ShapeType::Cuboid, ShapeType::Cuboid) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_cuboid_cuboid, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Polygon(_), Shape::Polygon(_)) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_polygon_polygon, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (Shape::Capsule(_), Shape::Capsule(_)) => ( + // (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, ), - (Shape::Cuboid(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Cuboid(_)) - | (Shape::Triangle(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Triangle(_)) - | (Shape::Capsule(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Capsule(_)) - | (Shape::Cylinder(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Cylinder(_)) => ( + (ShapeType::Cuboid, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Cuboid) + | (ShapeType::Triangle, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Triangle) + | (ShapeType::Capsule, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Capsule) + | (ShapeType::Cylinder, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Cylinder) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => ( + (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_cuboid_capsule, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_cuboid_triangle, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (Shape::Cylinder(_), _) | (_, Shape::Cylinder(_)) => ( + (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => { + ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_cuboid_triangle, + ..PrimitiveContactGenerator::default() + }, + None, + ) + } + (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() @@ -110,18 +112,18 @@ impl ContactDispatcher for DefaultContactDispatcher { fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ContactPhase, Option>) { match (shape1, shape2) { - (Shape::Trimesh(_), _) | (_, Shape::Trimesh(_)) => ( + (ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_trimesh_shape, ..ContactGenerator::default() }), Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())), ), - (Shape::HeightField(_), _) | (_, Shape::HeightField(_)) => ( + (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_heightfield_shape, ..ContactGenerator::default() diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index a7857a1..c3cf588 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -9,7 +9,7 @@ use crate::math::Vector; use ncollide::shape::Segment; pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) { generate_contacts( ctxt.prediction_distance, cube1, @@ -20,7 +20,9 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon false, ); ctxt.manifold.update_warmstart_multiplier(); - } else if let (Shape::Capsule(capsule1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(capsule1), Some(cube2)) = + (ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid()) + { generate_contacts( ctxt.prediction_distance, cube2, @@ -53,7 +55,7 @@ pub fn generate_contacts<'a>( return; } - let segment2 = Segment::new(capsule2.a, capsule2.b); + let segment2 = capsule2.segment(); /* * diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index d879a22..596cfb1 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -6,7 +6,7 @@ use crate::math::Vector; use ncollide::shape::Cuboid; pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { generate_contacts( ctxt.prediction_distance, cube1, diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 1a0358d..6744c0e 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -10,7 +10,7 @@ use crate::{ }; pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { generate_contacts( ctxt.prediction_distance, cube1, @@ -21,7 +21,9 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo false, ); ctxt.manifold.update_warmstart_multiplier(); - } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(triangle1), Some(cube2)) = + (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) + { generate_contacts( ctxt.prediction_distance, cube2, diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 04afc65..f507caf 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -3,7 +3,7 @@ use crate::geometry::contact_generator::{ }; #[cfg(feature = "dim2")] use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, HeightField, Shape}; +use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType}; use crate::ncollide::bounding_volume::BoundingVolume; #[cfg(feature = "dim3")] use crate::{geometry::Triangle, math::Point}; @@ -38,9 +38,9 @@ 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 Shape::HeightField(heightfield1) = collider1.shape() { + if let Some(heightfield1) = collider1.shape().as_heightfield() { do_generate_contacts(heightfield1, collider1, collider2, ctxt, false) - } else if let Shape::HeightField(heightfield2) = collider2.shape() { + } else if let Some(heightfield2) = collider2.shape().as_heightfield() { do_generate_contacts(heightfield2, collider2, collider1, ctxt, true) } } @@ -59,6 +59,7 @@ fn do_generate_contacts( .expect("The HeightFieldShapeContactGeneratorWorkspace is missing.") .downcast_mut() .expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace."); + let shape_type2 = collider2.shape().shape_type(); /* * Detect if the detector context has been reset. @@ -76,19 +77,9 @@ fn do_generate_contacts( // subshape_id, manifold.subshape_index_pair // ); - // Use dummy shapes for the dispatch. - #[cfg(feature = "dim2")] - let sub_shape1 = - Shape::Capsule(Capsule::new(na::Point::origin(), na::Point::origin(), 0.0)); - #[cfg(feature = "dim3")] - let sub_shape1 = Shape::Triangle(Triangle::new( - Point::origin(), - Point::origin(), - Point::origin(), - )); let (generator, workspace2) = ctxt .dispatcher - .dispatch_primitives(&sub_shape1, collider2.shape()); + .dispatch_primitives(ShapeType::Capsule, shape_type2); let sub_detector = SubDetector { generator, @@ -120,12 +111,18 @@ fn do_generate_contacts( let manifolds = &mut ctxt.pair.manifolds; let prediction_distance = ctxt.prediction_distance; let dispatcher = ctxt.dispatcher; + 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 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0)); + let (position1, sub_shape1) = { + let (dpos, height) = crate::utils::segment_to_capsule(&part1.a, &part1.b); + (position1 * dpos, Capsule::new(height, 0.0)); + }; #[cfg(feature = "dim3")] - let sub_shape1 = Shape::Triangle(*part1); + let sub_shape1 = *part1; + let sub_detector = match workspace.sub_detectors.entry(i) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); @@ -137,7 +134,7 @@ fn do_generate_contacts( } Entry::Vacant(entry) => { let (generator, workspace2) = - dispatcher.dispatch_primitives(&sub_shape1, collider2.shape()); + dispatcher.dispatch_primitives(ShapeType::Triangle, shape_type2); let sub_detector = SubDetector { generator, manifold_id: manifolds.len(), @@ -162,7 +159,7 @@ fn do_generate_contacts( shape1: collider2.shape(), shape2: &sub_shape1, position1: collider2.position(), - position2: collider1.position(), + position2: &position1, manifold, workspace: sub_detector.workspace.as_deref_mut(), } @@ -173,7 +170,7 @@ fn do_generate_contacts( collider2, shape1: &sub_shape1, shape2: collider2.shape(), - position1: collider1.position(), + position1: &position1, position2: collider2.position(), manifold, workspace: sub_detector.workspace.as_deref_mut(), diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index d8a523f..0549420 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -22,7 +22,7 @@ pub use self::heightfield_shape_contact_generator::{ 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::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; pub use self::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, }; diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 33b54e4..c150e83 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -5,20 +5,21 @@ use crate::math::{Isometry, Point}; use crate::{math::Vector, utils}; pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { - generate_contacts( - polygon1, - &ctxt.position1, - polygon2, - &ctxt.position2, - ctxt.manifold, - ); - ctxt.manifold.update_warmstart_multiplier(); - } else { - unreachable!() - } - - ctxt.manifold.sort_contacts(ctxt.prediction_distance); + unimplemented!() + // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { + // generate_contacts( + // polygon1, + // &ctxt.position1, + // polygon2, + // &ctxt.position2, + // ctxt.manifold, + // ); + // ctxt.manifold.update_warmstart_multiplier(); + // } else { + // unreachable!() + // } + // + // ctxt.manifold.sort_contacts(ctxt.prediction_distance); } fn generate_contacts<'a>( diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 52ba9b7..49e9b40 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -1,7 +1,7 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, Shape, Trimesh}; +use crate::geometry::{Collider, ContactManifold, Shape, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeContactGeneratorWorkspace { @@ -26,9 +26,9 @@ 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 Shape::Trimesh(trimesh1) = collider1.shape() { + if let Some(trimesh1) = collider1.shape().as_trimesh() { do_generate_contacts(trimesh1, collider1, collider2, ctxt, false) - } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + } else if let Some(trimesh2) = collider2.shape().as_trimesh() { do_generate_contacts(trimesh2, collider2, collider1, ctxt, true) } } @@ -121,6 +121,7 @@ fn do_generate_contacts( 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(); for (i, triangle_id) in new_interferences.iter().enumerate() { if *triangle_id >= trimesh1.num_triangles() { @@ -159,10 +160,10 @@ fn do_generate_contacts( } let manifold = &mut ctxt.pair.manifolds[i]; - let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let triangle1 = trimesh1.triangle(*triangle_id); let (generator, mut workspace2) = ctxt .dispatcher - .dispatch_primitives(&triangle1, collider2.shape()); + .dispatch_primitives(ShapeType::Triangle, shape_type2); let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 { PrimitiveContactGenerationContext { diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index efbb35c..71573ed 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -1,8 +1,7 @@ //! Structures related to geometry: colliders, shapes, etc. pub use self::broad_phase_multi_sap::BroadPhase; -pub use self::capsule::Capsule; -pub use self::collider::{Collider, ColliderBuilder, Shape}; +pub use self::collider::{Collider, ColliderBuilder, ColliderShape}; pub use self::collider_set::{ColliderHandle, ColliderSet}; pub use self::contact::{ Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, @@ -19,9 +18,12 @@ 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::rounded::Rounded; pub use self::trimesh::Trimesh; pub use ncollide::query::Proximity; +/// A capsule shape. +pub type Capsule = ncollide::shape::Capsule; /// A cuboid shape. pub type Cuboid = ncollide::shape::Cuboid; /// A triangle shape. @@ -43,6 +45,8 @@ pub type ProximityEvent = ncollide::pipeline::ProximityEvent; pub type Ray = ncollide::query::Ray; /// The intersection between a ray and a collider. pub type RayIntersection = ncollide::query::RayIntersection; +/// The the projection of a point on a collider. +pub type PointProjection = ncollide::query::PointProjection; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::ball::WBall; @@ -61,10 +65,10 @@ pub(crate) use self::polyhedron_feature3d::PolyhedronFace; pub(crate) use self::waabb::{WRay, WAABB}; pub(crate) use self::wquadtree::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; +pub use self::shape::{Shape, ShapeType}; mod ball; mod broad_phase_multi_sap; -mod capsule; mod collider; mod collider_set; mod contact; @@ -89,3 +93,5 @@ mod wquadtree; //mod z_order; #[cfg(feature = "dim3")] mod polygonal_feature_map; +mod rounded; +mod shape; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index ebe0a79..e95709c 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -197,7 +197,8 @@ impl NarrowPhase { if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() { let dispatcher = DefaultProximityDispatcher; - let generator = dispatcher.dispatch(co1.shape(), co2.shape()); + let generator = dispatcher + .dispatch(co1.shape().shape_type(), co2.shape().shape_type()); let interaction = ProximityPair::new(*pair, generator.0, generator.1); let _ = self.proximity_graph.add_edge( @@ -226,7 +227,8 @@ impl NarrowPhase { if self.contact_graph.graph.find_edge(gid1, gid2).is_none() { let dispatcher = DefaultContactDispatcher; - let generator = dispatcher.dispatch(co1.shape(), co2.shape()); + let generator = dispatcher + .dispatch(co1.shape().shape_type(), co2.shape().shape_type()); let interaction = ContactPair::new(*pair, generator.0, generator.1); let _ = self.contact_graph.add_edge( co1.contact_graph_index, @@ -308,7 +310,8 @@ impl NarrowPhase { if pair.detector.is_none() { // We need a redispatch for this detector. // This can happen, e.g., after restoring a snapshot of the narrow-phase. - let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape()); + let (detector, workspace) = + dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type()); pair.detector = Some(detector); pair.detector_workspace = workspace; } @@ -418,7 +421,8 @@ impl NarrowPhase { 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(), co2.shape()); + let (generator, workspace) = + dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type()); pair.generator = Some(generator); pair.generator_workspace = workspace; } diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs index b00337d..d7c5d02 100644 --- a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs @@ -4,24 +4,16 @@ use crate::math::Isometry; use ncollide::query::PointQuery; pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - if let Shape::Ball(ball1) = ctxt.shape1 { - match ctxt.shape2 { - Shape::Triangle(tri2) => do_detect_proximity(tri2, ball1, &ctxt), - Shape::Cuboid(cube2) => do_detect_proximity(cube2, ball1, &ctxt), - _ => unimplemented!(), - } - } else if let Shape::Ball(ball2) = ctxt.shape2 { - match ctxt.shape1 { - Shape::Triangle(tri1) => do_detect_proximity(tri1, ball2, &ctxt), - Shape::Cuboid(cube1) => do_detect_proximity(cube1, ball2, &ctxt), - _ => unimplemented!(), - } + if let Some(ball1) = ctxt.shape1.as_ball() { + do_detect_proximity(ctxt.shape2, ball1, &ctxt) + } else if let Some(ball2) = ctxt.shape2.as_ball() { + do_detect_proximity(ctxt.shape1, ball2, &ctxt) } else { panic!("Invalid shape types provide.") } } -fn do_detect_proximity>( +fn do_detect_proximity>( point_query1: &P, ball2: &Ball, ctxt: &PrimitiveProximityDetectionContext, diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs index b68ebf9..2462fc9 100644 --- a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs @@ -4,7 +4,7 @@ use crate::math::Isometry; use ncollide::shape::Cuboid; pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { detect_proximity( ctxt.prediction_distance, cube1, diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs index 12f3b4a..45991c7 100644 --- a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs @@ -5,7 +5,7 @@ use crate::math::Isometry; pub fn detect_proximity_cuboid_triangle( ctxt: &mut PrimitiveProximityDetectionContext, ) -> Proximity { - if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { detect_proximity( ctxt.prediction_distance, cube1, @@ -13,7 +13,9 @@ pub fn detect_proximity_cuboid_triangle( triangle2, ctxt.position2, ) - } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(triangle1), Some(cube2)) = + (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) + { detect_proximity( ctxt.prediction_distance, cube2, diff --git a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs index f0e049f..5b89dc5 100644 --- a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs +++ b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs @@ -5,17 +5,18 @@ use crate::math::Isometry; pub fn detect_proximity_polygon_polygon( ctxt: &mut PrimitiveProximityDetectionContext, ) -> Proximity { - if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { - detect_proximity( - ctxt.prediction_distance, - polygon1, - &ctxt.position1, - polygon2, - &ctxt.position2, - ) - } else { - unreachable!() - } + unimplemented!() + // if let (Some(polygon1), Some(polygon2)) = (ctxt.shape1.as_polygon(), ctxt.shape2.as_polygon()) { + // detect_proximity( + // ctxt.prediction_distance, + // polygon1, + // &ctxt.position1, + // polygon2, + // &ctxt.position2, + // ) + // } else { + // unreachable!() + // } } fn detect_proximity<'a>( diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs index 6d6b4c5..62f50f7 100644 --- a/src/geometry/proximity_detector/proximity_dispatcher.rs +++ b/src/geometry/proximity_detector/proximity_dispatcher.rs @@ -2,7 +2,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetector, ProximityDetector, ProximityPhase, TrimeshShapeProximityDetectorWorkspace, }; -use crate::geometry::Shape; +use crate::geometry::{Shape, ShapeType}; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm @@ -11,8 +11,8 @@ pub trait ProximityDispatcher { /// Select the proximity detection algorithm for the given pair of primitive shapes. fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveProximityDetector, Option>, @@ -20,8 +20,8 @@ pub trait ProximityDispatcher { /// Select the proximity detection algorithm for the given pair of non-primitive shapes. fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ProximityPhase, Option>); } @@ -31,14 +31,14 @@ pub struct DefaultProximityDispatcher; impl ProximityDispatcher for DefaultProximityDispatcher { fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveProximityDetector, Option>, ) { match (shape1, shape2) { - (Shape::Ball(_), Shape::Ball(_)) => ( + (ShapeType::Ball, ShapeType::Ball) => ( PrimitiveProximityDetector { #[cfg(feature = "simd-is-enabled")] detect_proximity_simd: super::detect_proximity_ball_ball_simd, @@ -47,56 +47,56 @@ impl ProximityDispatcher for DefaultProximityDispatcher { }, None, ), - (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + (ShapeType::Cuboid, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_cuboid, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Polygon(_), Shape::Polygon(_)) => ( + (ShapeType::Polygon, ShapeType::Polygon) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_polygon_polygon, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Triangle(_), Shape::Ball(_)) => ( + (ShapeType::Triangle, ShapeType::Ball) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Ball(_), Shape::Triangle(_)) => ( + (ShapeType::Ball, ShapeType::Triangle) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Cuboid(_), Shape::Ball(_)) => ( + (ShapeType::Cuboid, ShapeType::Ball) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Ball(_), Shape::Cuboid(_)) => ( + (ShapeType::Ball, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Triangle(_), Shape::Cuboid(_)) => ( + (ShapeType::Triangle, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_triangle, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Cuboid(_), Shape::Triangle(_)) => ( + (ShapeType::Cuboid, ShapeType::Triangle) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_triangle, ..PrimitiveProximityDetector::default() @@ -109,18 +109,18 @@ impl ProximityDispatcher for DefaultProximityDispatcher { fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ProximityPhase, Option>) { match (shape1, shape2) { - (Shape::Trimesh(_), _) => ( + (ShapeType::Trimesh, _) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() }), Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), ), - (_, Shape::Trimesh(_)) => ( + (_, ShapeType::Trimesh) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index edf3085..cce46d2 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -1,7 +1,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; -use crate::geometry::{Collider, Proximity, Shape, Trimesh}; +use crate::geometry::{Collider, Proximity, Shape, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeProximityDetectorWorkspace { @@ -24,9 +24,9 @@ pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> P let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::Trimesh(trimesh1) = collider1.shape() { + if let Some(trimesh1) = collider1.shape().as_trimesh() { do_detect_proximity(trimesh1, collider1, collider2, ctxt) - } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + } else if let Some(trimesh2) = collider2.shape().as_trimesh() { do_detect_proximity(trimesh2, collider2, collider1, ctxt) } else { panic!("Invalid shape types provided.") @@ -83,6 +83,7 @@ fn do_detect_proximity( let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut best_proximity = Proximity::Disjoint; + let shape_type2 = collider2.shape().shape_type(); for triangle_id in new_interferences.iter() { if *triangle_id >= trimesh1.num_triangles() { @@ -107,10 +108,10 @@ fn do_detect_proximity( }; } - let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let triangle1 = trimesh1.triangle(*triangle_id); let (proximity_detector, mut workspace2) = ctxt .dispatcher - .dispatch_primitives(&triangle1, collider2.shape()); + .dispatch_primitives(ShapeType::Triangle, shape_type2); let mut ctxt2 = PrimitiveProximityDetectionContext { prediction_distance: ctxt.prediction_distance, diff --git a/src/geometry/rounded.rs b/src/geometry/rounded.rs new file mode 100644 index 0000000..615d408 --- /dev/null +++ b/src/geometry/rounded.rs @@ -0,0 +1,7 @@ +/// A rounded shape. +pub struct Rounded { + /// The shape being rounded. + pub shape: S, + /// The rounding radius. + pub radius: f32, +} diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 0666c04..e2548dd 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -58,8 +58,8 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( let y2 = pos12 * Vector::y(); let z2 = pos12 * Vector::z(); - // We have 3 * 3 = 9 axii to test. - let axii = [ + // We have 3 * 3 = 9 axes to test. + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), @@ -74,7 +74,7 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( Vector::new(-z2.y, z2.x, 0.0), ]; - for axis1 in &axii { + for axis1 in &axes { let norm1 = axis1.norm(); if norm1 > f32::default_epsilon() { let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line( @@ -149,7 +149,7 @@ pub fn cube_support_map_compute_separation_wrt_local_line>( pub fn cube_support_map_find_local_separating_edge_twoway( cube1: &Cuboid, shape2: &impl SupportMap, - axii: &[Vector], + axes: &[Vector], pos12: &Isometry, pos21: &Isometry, ) -> (f32, Vector) { @@ -157,7 +157,7 @@ pub fn cube_support_map_find_local_separating_edge_twoway( let mut best_separation = -std::f32::MAX; let mut best_dir = Vector::zeros(); - for axis1 in axii { + for axis1 in axes { if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) { let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line( cube1, shape2, pos12, pos21, &axis1, @@ -184,8 +184,8 @@ pub fn cube_triangle_find_local_separating_edge_twoway( let y2 = pos12 * (triangle2.c - triangle2.b); let z2 = pos12 * (triangle2.a - triangle2.c); - // We have 3 * 3 = 3 axii to test. - let axii = [ + // We have 3 * 3 = 3 axes to test. + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), @@ -200,7 +200,7 @@ pub fn cube_triangle_find_local_separating_edge_twoway( Vector::new(-z2.y, z2.x, 0.0), ]; - cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axii, pos12, pos21) + cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axes, pos12, pos21) } #[cfg(feature = "dim3")] @@ -212,14 +212,14 @@ pub fn cube_segment_find_local_separating_edge_twoway( ) -> (f32, Vector) { let x2 = pos12 * (segment2.b - segment2.a); - let axii = [ + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), Vector::new(-x2.y, x2.x, 0.0), ]; - cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axii, pos12, pos21) + cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axes, pos12, pos21) } pub fn cube_support_map_find_local_separating_normal_oneway>( diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs new file mode 100644 index 0000000..c9e0a3a --- /dev/null +++ b/src/geometry/shape.rs @@ -0,0 +1,275 @@ +use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; +#[cfg(feature = "dim3")] +use crate::geometry::PolygonalFeatureMap; +use crate::geometry::{ + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, + Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh, +}; +use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; +use downcast_rs::{impl_downcast, DowncastSync}; +use erased_serde::Serialize; +use na::Point3; +use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use ncollide::query::{PointQuery, RayCast}; +use num::Zero; +use num_derive::FromPrimitive; +use std::any::Any; + +#[derive(Copy, Clone, Debug, FromPrimitive)] +/// Enum representing the type of a shape. +pub enum ShapeType { + /// A ball shape. + Ball = 1, + /// A convex polygon shape. + Polygon, + /// A cuboid shape. + Cuboid, + /// A capsule shape. + Capsule, + /// A triangle shape. + Triangle, + /// A triangle mesh shape. + Trimesh, + /// A heightfield shape. + HeightField, + #[cfg(feature = "dim3")] + /// A cylindrical shape. + Cylinder, + // /// A custom shape type. + // Custom(u8), +} + +/// Trait implemented by shapes usable by Rapier. +pub trait Shape: RayCast + PointQuery + DowncastSync { + /// Convert this shape as a serializable entity. + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + None + } + + /// Computes the AABB of this shape. + fn compute_aabb(&self, position: &Isometry) -> AABB; + + /// Compute the mass-properties of this shape given its uniform density. + fn mass_properties(&self, density: f32) -> MassProperties; + + /// Gets the type tag of this shape. + fn shape_type(&self) -> ShapeType; + + /// Converts this shape to a polygonal feature-map, if it is one. + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + None + } + + // fn as_rounded(&self) -> Option<&Rounded>> { + // None + // } +} + +impl_downcast!(sync Shape); + +impl dyn Shape { + /// Converts this abstract shape to a ball, if it is one. + pub fn as_ball(&self) -> Option<&Ball> { + self.downcast_ref() + } + + /// Converts this abstract shape to a cuboid, if it is one. + pub fn as_cuboid(&self) -> Option<&Cuboid> { + self.downcast_ref() + } + + /// Converts this abstract shape to a capsule, if it is one. + pub fn as_capsule(&self) -> Option<&Capsule> { + self.downcast_ref() + } + + /// Converts this abstract shape to a triangle, if it is one. + pub fn as_triangle(&self) -> Option<&Triangle> { + self.downcast_ref() + } + + /// Converts this abstract shape to a triangle mesh, if it is one. + pub fn as_trimesh(&self) -> Option<&Trimesh> { + self.downcast_ref() + } + + /// Converts this abstract shape to a heightfield, if it is one. + pub fn as_heightfield(&self) -> Option<&HeightField> { + self.downcast_ref() + } + + /// Converts this abstract shape to a cylinder, if it is one. + pub fn as_cylinder(&self) -> Option<&Cylinder> { + self.downcast_ref() + } +} + +impl Shape for Ball { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_ball(density, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Ball + } +} + +// impl Shape for Polygon { +// #[cfg(feature = "serde-serialize")] +// fn as_serialize(&self) -> Option<&dyn Serialize> { +// Some(self as &dyn Serialize) +// } +// +// fn compute_aabb(&self, position: &Isometry) -> AABB { +// self.aabb(position) +// } +// +// fn mass_properties(&self, _density: f32) -> MassProperties { +// unimplemented!() +// } +// +// fn shape_type(&self) -> ShapeType { +// ShapeType::Polygon +// } +// } + +impl Shape for Cuboid { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cuboid(density, self.half_extents) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cuboid + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + Some(self as &dyn PolygonalFeatureMap) + } +} + +impl Shape for Capsule { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_capsule(density, self.half_height, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Capsule + } +} + +impl Shape for Triangle { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Triangle + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + Some(self as &dyn PolygonalFeatureMap) + } +} + +impl Shape for Trimesh { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Trimesh + } +} + +impl Shape for HeightField { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::HeightField + } +} + +#[cfg(feature = "dim3")] +impl Shape for Cylinder { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cylinder(density, self.half_height, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cylinder + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + Some(self as &dyn PolygonalFeatureMap) + } +} diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index b6e23e7..38ce0a3 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -1,13 +1,9 @@ -use crate::geometry::{Triangle, WQuadtree}; +use crate::geometry::{PointProjection, Ray, RayIntersection, Triangle, WQuadtree}; use crate::math::{Isometry, Point}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; - -#[cfg(feature = "dim3")] -use { - crate::geometry::{Ray, RayIntersection}, - ncollide::query::RayCast, -}; +use ncollide::query::{PointQuery, RayCast}; +use ncollide::shape::FeatureId; #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -110,6 +106,41 @@ impl Trimesh { } } +impl PointQuery for Trimesh { + fn project_point(&self, m: &Isometry, pt: &Point, solid: bool) -> PointProjection { + // TODO + unimplemented!() + } + + fn project_point_with_feature( + &self, + m: &Isometry, + pt: &Point, + ) -> (PointProjection, FeatureId) { + // TODO + unimplemented!() + } +} + +#[cfg(feature = "dim2")] +impl RayCast for Trimesh { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option { + // TODO + None + } + + fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_toi: f32) -> bool { + // TODO + false + } +} + #[cfg(feature = "dim3")] impl RayCast for Trimesh { fn toi_and_normal_with_ray( diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 32f59fc..03103be 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -69,7 +69,11 @@ impl QueryPipeline { for handle in inter { let collider = &colliders[handle]; - if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if let Some(inter) = + collider + .shape() + .toi_and_normal_with_ray(collider.position(), ray, max_toi, true) + { if inter.toi < best { best = inter.toi; result = Some((handle, collider, inter)); @@ -103,7 +107,11 @@ impl QueryPipeline { for handle in inter { let collider = &colliders[handle]; - if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if let Some(inter) = + collider + .shape() + .toi_and_normal_with_ray(collider.position(), ray, max_toi, true) + { if !callback(handle, collider, inter) { return; } diff --git a/src/utils.rs b/src/utils.rs index bd972b8..48f4ef7 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,6 +1,7 @@ //! Miscellaneous utilities. use crate::dynamics::RigidBodyHandle; +use crate::math::{Isometry, Point, Rotation, Vector}; #[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))] use indexmap::IndexMap as HashMap; use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; @@ -1332,3 +1333,28 @@ pub(crate) fn other_handle( pair.0 } } + +/// Returns the rotation that aligns the y axis to the segment direction. +pub(crate) fn rotation_wrt_y(a: &Point, b: &Point) -> Rotation { + let mut dir = b - a; + + if dir.y < 0.0 { + dir = -dir; + } + + #[cfg(feature = "dim2")] + return Rotation::rotation_between(&Vector::y(), &dir); + + #[cfg(feature = "dim3")] + return Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()); +} + +// Return the transform that aligns the y axis to the segment and move the origin to the segment middle, +// and the capsule's half-height. +pub(crate) fn segment_to_capsule(a: &Point, b: &Point) -> (Isometry, f32) { + let rot = rotation_wrt_y(a, b); + let half_height = (b - a).norm() / 2.0; + let center = na::center(a, b); + let pos = Isometry::from_parts(center.coords.into(), rot); + (pos, half_height) +} diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index ca2d71f..ca1a2b8 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -350,33 +350,44 @@ impl GraphicsManager { color: Point3, out: &mut Vec, ) { - match collider.shape() { - Shape::Ball(ball) => { - out.push(Node::Ball(Ball::new(handle, ball.radius, color, window))) - } - Shape::Polygon(poly) => out.push(Node::Convex(Convex::new( - handle, - poly.vertices().to_vec(), - color, - window, - ))), - Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new( + let shape = collider.shape(); + + if let Some(ball) = shape.as_ball() { + out.push(Node::Ball(Ball::new(handle, ball.radius, color, window))) + } + + // Shape::Polygon(poly) => out.push(Node::Convex(Convex::new( + // handle, + // poly.vertices().to_vec(), + // color, + // window, + // ))), + + if let Some(cuboid) = shape.as_cuboid() { + out.push(Node::Box(BoxNode::new( handle, cuboid.half_extents, color, window, - ))), - Shape::Capsule(capsule) => { - out.push(Node::Capsule(Capsule::new(handle, capsule, color, window))) - } - Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new( + ))) + } + + if let Some(capsule) = shape.as_capsule() { + out.push(Node::Capsule(Capsule::new(handle, capsule, color, window))) + } + + if let Some(triangle) = shape.as_triangle() { + out.push(Node::Mesh(Mesh::new( handle, vec![triangle.a, triangle.b, triangle.c], vec![Point3::new(0, 1, 2)], color, window, - ))), - Shape::Trimesh(trimesh) => out.push(Node::Mesh(Mesh::new( + ))) + } + + if let Some(trimesh) = shape.as_trimesh() { + out.push(Node::Mesh(Mesh::new( handle, trimesh.vertices().to_vec(), trimesh @@ -386,21 +397,27 @@ impl GraphicsManager { .collect(), color, window, - ))), - Shape::HeightField(heightfield) => out.push(Node::HeightField(HeightField::new( + ))) + } + + if let Some(heightfield) = shape.as_heightfield() { + out.push(Node::HeightField(HeightField::new( handle, heightfield, color, window, - ))), - #[cfg(feature = "dim3")] - Shape::Cylinder(cylinder) => out.push(Node::Cylinder(Cylinder::new( + ))) + } + + #[cfg(feature = "dim3")] + if let Some(cylinder) = shape.as_cylinder() { + out.push(Node::Cylinder(Cylinder::new( handle, cylinder.half_height, cylinder.radius, color, window, - ))), + ))) } } From d513c22d33ab44b0048355bcfd1db4173b3f7ece Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 20 Oct 2020 14:16:01 +0200 Subject: [PATCH 08/41] Add cone support. --- examples3d/heightfield3.rs | 5 +- examples3d/primitives3.rs | 5 +- examples3d/trimesh3.rs | 5 +- src/dynamics/mass_properties_cone.rs | 30 ++++++++ src/dynamics/mod.rs | 2 + src/geometry/collider.rs | 29 +++++++- .../contact_generator/contact_dispatcher.rs | 9 ++- src/geometry/mod.rs | 3 + src/geometry/polygonal_feature_map.rs | 48 +++++++++++- src/geometry/shape.rs | 41 +++++++++- src_testbed/engine.rs | 12 +++ src_testbed/objects/capsule.rs | 2 +- src_testbed/objects/cone.rs | 74 +++++++++++++++++++ src_testbed/objects/mod.rs | 1 + src_testbed/objects/node.rs | 9 +++ 15 files changed, 257 insertions(+), 18 deletions(-) create mode 100644 src/dynamics/mass_properties_cone.rs create mode 100644 src_testbed/objects/cone.rs diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index 826df4c..2f26dde 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -54,10 +54,11 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - let collider = match j % 3 { + let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - _ => ColliderBuilder::cylinder(rad, rad).build(), + 2 => ColliderBuilder::cylinder(rad, rad).build(), + _ => ColliderBuilder::cone(rad, rad).build(), }; colliders.insert(collider, handle, &mut bodies); diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index c3fa799..4e2fc19 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -50,10 +50,11 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - let collider = match j % 3 { + let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - _ => ColliderBuilder::cylinder(rad, rad).build(), + 2 => ColliderBuilder::cylinder(rad, rad).build(), + _ => ColliderBuilder::cone(rad, rad).build(), }; colliders.insert(collider, handle, &mut bodies); diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index 40aede2..2d6c0bb 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -64,10 +64,11 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - let collider = match j % 3 { + let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - _ => ColliderBuilder::cylinder(rad, rad).build(), + 2 => ColliderBuilder::cylinder(rad, rad).build(), + _ => ColliderBuilder::cone(rad, rad).build(), }; colliders.insert(collider, handle, &mut bodies); diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs new file mode 100644 index 0000000..0fb61b6 --- /dev/null +++ b/src/dynamics/mass_properties_cone.rs @@ -0,0 +1,30 @@ +use crate::dynamics::MassProperties; +use crate::geometry::Cone; +use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; + +impl MassProperties { + pub(crate) fn cone_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia) { + let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0; + let principal = sq_radius * 3.0 / 10.0; + + (volume, Vector::new(off_principal, principal, off_principal)) + } + + pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self { + let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius); + let cyl_mass = cyl_vol * density; + + Self::with_principal_inertia_frame( + Point::new(0.0, -half_height / 2.0, 0.0), + cyl_mass, + cyl_unit_i * cyl_mass, + Rotation::identity(), + ) + } +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 512bd8b..10cdd29 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -22,6 +22,8 @@ mod joint; mod mass_properties; mod mass_properties_ball; mod mass_properties_capsule; +#[cfg(feature = "dim3")] +mod mass_properties_cone; mod mass_properties_cuboid; mod mass_properties_cylinder; #[cfg(feature = "dim2")] diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index c6cf6d0..f952b15 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,10 +1,10 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; -#[cfg(feature = "dim3")] -use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, - Polygon, Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, + Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, }; +#[cfg(feature = "dim3")] +use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use downcast_rs::{impl_downcast, DowncastSync}; use erased_serde::Serialize; @@ -40,6 +40,13 @@ impl ColliderShape { ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } + /// Initialize a cone shape defined by its half-height + /// (along along the y axis) and its basis radius. + #[cfg(feature = "dim3")] + pub fn cone(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cone::new(half_height, radius))) + } + /// Initialize a cuboid shape defined by its half-extents. pub fn cuboid(half_extents: Vector) -> Self { ColliderShape(Arc::new(Cuboid::new(half_extents))) @@ -171,6 +178,13 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; Arc::new(shape) as Arc } + #[cfg(feature = "dim3")] + Some(ShapeType::Cone) => { + let shape: Cone = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } None => { return Err(serde::de::Error::custom( "found invalid shape type to deserialize", @@ -328,6 +342,13 @@ impl ColliderBuilder { Self::new(ColliderShape::cylinder(half_height, radius)) } + /// Initialize a new collider builder with a cone shape defined by its half-height + /// (along along the y axis) and its basis radius. + #[cfg(feature = "dim3")] + pub fn cone(half_height: f32, radius: f32) -> Self { + Self::new(ColliderShape::cone(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/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 01bbc46..70ac84c 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -76,7 +76,9 @@ impl ContactDispatcher for DefaultContactDispatcher { | (ShapeType::Capsule, ShapeType::Ball) | (ShapeType::Ball, ShapeType::Capsule) | (ShapeType::Cylinder, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Cylinder) => ( + | (ShapeType::Ball, ShapeType::Cylinder) + | (ShapeType::Cone, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Cone) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() @@ -99,7 +101,10 @@ impl ContactDispatcher for DefaultContactDispatcher { None, ) } - (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) => ( + (ShapeType::Cylinder, _) + | (_, ShapeType::Cylinder) + | (ShapeType::Cone, _) + | (_, ShapeType::Cone) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 71573ed..d569248 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -35,6 +35,9 @@ pub type HeightField = ncollide::shape::HeightField; /// A cylindrical shape. #[cfg(feature = "dim3")] pub type Cylinder = ncollide::shape::Cylinder; +/// A cone shape. +#[cfg(feature = "dim3")] +pub type Cone = ncollide::shape::Cone; /// 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. diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index 70be5d0..fc5e066 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -1,5 +1,5 @@ use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle}; +use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle}; use crate::math::{Point, Vector}; use approx::AbsDiffEq; use na::{Unit, Vector2, Vector3}; @@ -85,3 +85,49 @@ impl PolygonalFeatureMap for Cylinder { } } } + +impl PolygonalFeatureMap for Cone { + fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { + // About feature ids. It is very similar to the feature ids of cylinders. + // At all times, we consider our cone to be approximated as follows: + // - The curved part is approximated by a single segment. + // - The flat cap of the cone 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. + // - Note that at all times, one of the 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()); + + if dir.y > 0.0 { + // We return a segment lying on the cone'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(0.0, self.half_height, 0.0); + out_features.eids = [0, 0, 0, 0]; + out_features.fid = 0; + out_features.num_vertices = 2; + out_features.vids = [1, 11, 11, 11]; + } else { + // We return a square approximation of the cone cap. + let y = -self.half_height; + 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 = [2, 4, 6, 8]; + out_features.fid = 9; + out_features.num_vertices = 4; + out_features.vids = [1, 3, 5, 7]; + } + } +} diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index c9e0a3a..b972a3e 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,10 +1,10 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; -#[cfg(feature = "dim3")] -use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, - Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, + Proximity, Ray, RayIntersection, Triangle, Trimesh, }; +#[cfg(feature = "dim3")] +use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use downcast_rs::{impl_downcast, DowncastSync}; use erased_serde::Serialize; @@ -35,6 +35,9 @@ pub enum ShapeType { #[cfg(feature = "dim3")] /// A cylindrical shape. Cylinder, + #[cfg(feature = "dim3")] + /// A cylindrical shape. + Cone, // /// A custom shape type. // Custom(u8), } @@ -104,6 +107,11 @@ impl dyn Shape { pub fn as_cylinder(&self) -> Option<&Cylinder> { self.downcast_ref() } + + /// Converts this abstract shape to a cone, if it is one. + pub fn as_cone(&self) -> Option<&Cone> { + self.downcast_ref() + } } impl Shape for Ball { @@ -273,3 +281,28 @@ impl Shape for Cylinder { Some(self as &dyn PolygonalFeatureMap) } } + +#[cfg(feature = "dim3")] +impl Shape for Cone { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cone(density, self.half_height, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cone + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + Some(self as &dyn PolygonalFeatureMap) + } +} diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index ca1a2b8..1908745 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::cone::Cone; use crate::objects::cylinder::Cylinder; use crate::objects::mesh::Mesh; use rand::{Rng, SeedableRng}; @@ -419,6 +420,17 @@ impl GraphicsManager { window, ))) } + + #[cfg(feature = "dim3")] + if let Some(cone) = shape.as_cone() { + out.push(Node::Cone(Cone::new( + handle, + cone.half_height, + cone.radius, + color, + window, + ))) + } } /* diff --git a/src_testbed/objects/capsule.rs b/src_testbed/objects/capsule.rs index 23160be..f285b81 100644 --- a/src_testbed/objects/capsule.rs +++ b/src_testbed/objects/capsule.rs @@ -19,7 +19,7 @@ impl Capsule { window: &mut window::Window, ) -> Capsule { let r = capsule.radius; - let h = capsule.half_height() * 2.0; + let h = capsule.half_height * 2.0; #[cfg(feature = "dim2")] let node = window.add_planar_capsule(r, h); #[cfg(feature = "dim3")] diff --git a/src_testbed/objects/cone.rs b/src_testbed/objects/cone.rs new file mode 100644 index 0000000..58b014f --- /dev/null +++ b/src_testbed/objects/cone.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 Cone { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl Cone { + pub fn new( + collider: ColliderHandle, + half_height: f32, + radius: f32, + color: Point3, + window: &mut Window, + ) -> Cone { + #[cfg(feature = "dim2")] + let node = window.add_rectangle(radius, half_height); + #[cfg(feature = "dim3")] + let node = window.add_cone(radius, half_height * 2.0); + + let mut res = Cone { + 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 51db9d4..e4d7bc4 100644 --- a/src_testbed/objects/mod.rs +++ b/src_testbed/objects/mod.rs @@ -1,6 +1,7 @@ pub mod ball; pub mod box_node; pub mod capsule; +pub mod cone; pub mod convex; pub mod cylinder; pub mod heightfield; diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs index 14668e8..d1de799 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::cone::Cone; use crate::objects::cylinder::Cylinder; use rapier::geometry::{ColliderHandle, ColliderSet}; use rapier::math::Isometry; @@ -30,6 +31,7 @@ pub enum Node { Mesh(Mesh), Convex(Convex), Cylinder(Cylinder), + Cone(Cone), } impl Node { @@ -45,6 +47,7 @@ impl Node { Node::Mesh(ref mut n) => n.select(), Node::Convex(ref mut n) => n.select(), Node::Cylinder(ref mut n) => n.select(), + Node::Cone(ref mut n) => n.select(), } } @@ -60,6 +63,7 @@ impl Node { Node::Mesh(ref mut n) => n.unselect(), Node::Convex(ref mut n) => n.unselect(), Node::Cylinder(ref mut n) => n.unselect(), + Node::Cone(ref mut n) => n.unselect(), } } @@ -75,6 +79,7 @@ impl Node { Node::Mesh(ref mut n) => n.update(colliders), Node::Convex(ref mut n) => n.update(colliders), Node::Cylinder(ref mut n) => n.update(colliders), + Node::Cone(ref mut n) => n.update(colliders), } } @@ -103,6 +108,7 @@ impl 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()), + Node::Cone(ref n) => Some(n.scene_node()), #[cfg(feature = "dim2")] _ => None, } @@ -120,6 +126,7 @@ impl Node { 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()), + Node::Cone(ref mut n) => Some(n.scene_node_mut()), #[cfg(feature = "dim2")] _ => None, } @@ -137,6 +144,7 @@ impl Node { Node::Mesh(ref n) => n.object(), Node::Convex(ref n) => n.object(), Node::Cylinder(ref n) => n.object(), + Node::Cone(ref n) => n.object(), } } @@ -152,6 +160,7 @@ impl Node { 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), + Node::Cone(ref mut n) => n.set_color(color), } } } From 64958470950cd9832a669b1bd5d70a2aeb6a85ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 20 Oct 2020 15:57:54 +0200 Subject: [PATCH 09/41] Add rounded cylinder. --- examples3d/heightfield3.rs | 4 +- examples3d/primitives3.rs | 4 +- examples3d/trimesh3.rs | 4 +- src/geometry/collider.rs | 95 ++++++++------- .../contact_generator/contact_dispatcher.rs | 15 +-- .../pfm_pfm_contact_generator.rs | 19 ++- src/geometry/mod.rs | 2 +- src/geometry/rounded.rs | 110 +++++++++++++++++- src/geometry/shape.rs | 68 +++++++++-- src_testbed/engine.rs | 2 +- 10 files changed, 242 insertions(+), 81 deletions(-) diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index 2f26dde..8c3386a 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -57,7 +57,9 @@ pub fn init_world(testbed: &mut Testbed) { let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - 2 => ColliderBuilder::cylinder(rad, rad).build(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index 4e2fc19..daabd23 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -53,7 +53,9 @@ pub fn init_world(testbed: &mut Testbed) { let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - 2 => ColliderBuilder::cylinder(rad, rad).build(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index 2d6c0bb..8fee784 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -67,7 +67,9 @@ pub fn init_world(testbed: &mut Testbed) { let collider = match j % 4 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - 2 => ColliderBuilder::cylinder(rad, rad).build(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index f952b15..4b3b115 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,7 +1,7 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, + Proximity, Ray, RayIntersection, Rounded, Shape, ShapeType, Triangle, Trimesh, }; #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; @@ -40,6 +40,17 @@ impl ColliderShape { ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } + /// Initialize a rounded cylindrical shape defined by its half-height + /// (along along the y axis), its radius, and its roundedness (the + /// radius of the sphere used for dilating the cylinder). + #[cfg(feature = "dim3")] + pub fn rounded_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { + ColliderShape(Arc::new(Rounded::new( + Cylinder::new(half_height, radius), + rounding_radius, + ))) + } + /// Initialize a cone shape defined by its half-height /// (along along the y axis) and its basis radius. #[cfg(feature = "dim3")] @@ -127,13 +138,20 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { .next_element()? .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + fn deser<'de, A, S: Shape + serde::Deserialize<'de>>( + seq: &mut A, + ) -> Result, A::Error> + where + A: serde::de::SeqAccess<'de>, + { + let shape: S = seq.next_element()?.ok_or_else(|| { + serde::de::Error::custom("Failed to deserialize builtin shape.") + })?; + Ok(Arc::new(shape) as Arc) + } + let shape = match ShapeType::from_i32(tag) { - Some(ShapeType::Ball) => { - let shape: Ball = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } + Some(ShapeType::Ball) => deser::(&mut seq)?, Some(ShapeType::Polygon) => { unimplemented!() // let shape: Polygon = seq @@ -141,50 +159,17 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { // .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; // Arc::new(shape) as Arc } - Some(ShapeType::Cuboid) => { - let shape: Cuboid = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } - Some(ShapeType::Capsule) => { - let shape: Capsule = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } - Some(ShapeType::Triangle) => { - let shape: Triangle = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } - Some(ShapeType::Trimesh) => { - let shape: Trimesh = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } - Some(ShapeType::HeightField) => { - let shape: HeightField = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } + Some(ShapeType::Cuboid) => deser::(&mut seq)?, + Some(ShapeType::Capsule) => deser::(&mut seq)?, + Some(ShapeType::Triangle) => deser::(&mut seq)?, + Some(ShapeType::Trimesh) => deser::(&mut seq)?, + Some(ShapeType::HeightField) => deser::(&mut seq)?, #[cfg(feature = "dim3")] - Some(ShapeType::Cylinder) => { - let shape: Cylinder = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } + Some(ShapeType::Cylinder) => deser::(&mut seq)?, #[cfg(feature = "dim3")] - Some(ShapeType::Cone) => { - let shape: Cone = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - Arc::new(shape) as Arc - } + Some(ShapeType::Cone) => deser::(&mut seq)?, + #[cfg(feature = "dim3")] + Some(ShapeType::RoundedCylinder) => deser::>(&mut seq)?, None => { return Err(serde::de::Error::custom( "found invalid shape type to deserialize", @@ -342,6 +327,18 @@ impl ColliderBuilder { Self::new(ColliderShape::cylinder(half_height, radius)) } + /// Initialize a new collider builder with a rounded cylindrical shape defined by its half-height + /// (along along the y axis), its radius, and its roundedness (the + /// radius of the sphere used for dilating the cylinder). + #[cfg(feature = "dim3")] + pub fn rounded_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { + Self::new(ColliderShape::rounded_cylinder( + half_height, + radius, + rounding_radius, + )) + } + /// Initialize a new collider builder with a cone shape defined by its half-height /// (along along the y axis) and its basis radius. #[cfg(feature = "dim3")] diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 70ac84c..14b7f79 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -69,16 +69,7 @@ impl ContactDispatcher for DefaultContactDispatcher { }, None, ), - (ShapeType::Cuboid, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Cuboid) - | (ShapeType::Triangle, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Triangle) - | (ShapeType::Capsule, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Capsule) - | (ShapeType::Cylinder, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Cylinder) - | (ShapeType::Cone, ShapeType::Ball) - | (ShapeType::Ball, ShapeType::Cone) => ( + (_, ShapeType::Ball) | (ShapeType::Ball, _) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() @@ -104,7 +95,9 @@ impl ContactDispatcher for DefaultContactDispatcher { (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) | (ShapeType::Cone, _) - | (_, ShapeType::Cone) => ( + | (_, ShapeType::Cone) + | (ShapeType::RoundedCylinder, _) + | (_, ShapeType::RoundedCylinder) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index c3815dd..37f8629 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -29,11 +29,11 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace { } pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Some(pfm1), Some(pfm2)) = ( + if let (Some((pfm1, round_radius1)), Some((pfm2, round_radius2))) = ( ctxt.shape1.as_polygonal_feature_map(), ctxt.shape2.as_polygonal_feature_map(), ) { - do_generate_contacts(pfm1, pfm2, ctxt); + do_generate_contacts(pfm1, round_radius1, pfm2, round_radius2, ctxt); ctxt.manifold.update_warmstart_multiplier(); ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -41,7 +41,9 @@ pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { fn do_generate_contacts( pfm1: &dyn PolygonalFeatureMap, + round_radius1: f32, pfm2: &dyn PolygonalFeatureMap, + round_radius2: f32, ctxt: &mut PrimitiveContactGenerationContext, ) { let pos12 = ctxt.position1.inverse() * ctxt.position2; @@ -64,12 +66,13 @@ fn do_generate_contacts( .downcast_mut() .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); + let total_prediction = ctxt.prediction_distance + round_radius1 + round_radius2; let contact = query::contact_support_map_support_map_with_params( &Isometry::identity(), pfm1, &pos12, pfm2, - ctxt.prediction_distance, + total_prediction, &mut workspace.simplex, workspace.last_gjk_dir, ); @@ -87,7 +90,7 @@ fn do_generate_contacts( workspace.feature2.transform_by(&pos12); PolyhedronFace::contacts( - ctxt.prediction_distance, + total_prediction, &workspace.feature1, &normal1, &workspace.feature2, @@ -95,6 +98,14 @@ fn do_generate_contacts( ctxt.manifold, ); + if round_radius1 != 0.0 || round_radius2 != 0.0 { + for contact in &mut ctxt.manifold.points { + contact.local_p1 += *normal1 * round_radius1; + contact.local_p2 += *normal2 * round_radius2; + contact.dist -= round_radius1 + round_radius2; + } + } + // Adjust points to take the radius into account. ctxt.manifold.local_n1 = *normal1; ctxt.manifold.local_n2 = *normal2; diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index d569248..f73de98 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -18,7 +18,7 @@ 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::rounded::Rounded; +pub use self::rounded::{Roundable, Rounded}; pub use self::trimesh::Trimesh; pub use ncollide::query::Proximity; diff --git a/src/geometry/rounded.rs b/src/geometry/rounded.rs index 615d408..59c6a72 100644 --- a/src/geometry/rounded.rs +++ b/src/geometry/rounded.rs @@ -1,7 +1,115 @@ +use crate::geometry::{Cylinder, ShapeType}; +use crate::math::{Isometry, Point, Vector}; +use na::Unit; +use ncollide::query::{ + algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection, +}; +use ncollide::shape::{FeatureId, SupportMap}; + +/// A shape which corner can be rounded. +pub trait Roundable { + /// The ShapeType fo this shape after rounding its corners. + fn rounded_shape_type() -> ShapeType; +} + +impl Roundable for Cylinder { + fn rounded_shape_type() -> ShapeType { + ShapeType::RoundedCylinder + } +} + /// A rounded shape. -pub struct Rounded { +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct Rounded { /// The shape being rounded. pub shape: S, /// The rounding radius. pub radius: f32, } + +impl Rounded { + /// Create sa new shape where all its edges and vertices are rounded by a radius of `radius`. + /// + /// This is done by applying a dilation of the given radius to the shape. + pub fn new(shape: S, radius: f32) -> Self { + Self { shape, radius } + } +} + +impl> SupportMap for Rounded { + fn local_support_point(&self, dir: &Vector) -> Point { + self.local_support_point_toward(&Unit::new_normalize(*dir)) + } + + fn local_support_point_toward(&self, dir: &Unit>) -> Point { + self.shape.local_support_point_toward(dir) + **dir * self.radius + } + + fn support_point(&self, transform: &Isometry, dir: &Vector) -> Point { + let local_dir = transform.inverse_transform_vector(dir); + transform * self.local_support_point(&local_dir) + } + + fn support_point_toward( + &self, + transform: &Isometry, + dir: &Unit>, + ) -> Point { + let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir)); + transform * self.local_support_point_toward(&local_dir) + } +} + +impl> RayCast for Rounded { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option> { + let ls_ray = ray.inverse_transform_by(m); + + ncollide::query::ray_intersection_with_support_map_with_params( + &Isometry::identity(), + self, + &mut VoronoiSimplex::new(), + &ls_ray, + max_toi, + solid, + ) + .map(|mut res| { + res.normal = m * res.normal; + res + }) + } +} + +// TODO: if PointQuery had a `project_point_with_normal` method, we could just +// call this and adjust the projected point accordingly. +impl> PointQuery for Rounded { + #[inline] + fn project_point( + &self, + m: &Isometry, + point: &Point, + solid: bool, + ) -> PointProjection { + ncollide::query::point_projection_on_support_map( + m, + self, + &mut VoronoiSimplex::new(), + point, + solid, + ) + } + + #[inline] + fn project_point_with_feature( + &self, + m: &Isometry, + point: &Point, + ) -> (PointProjection, FeatureId) { + (self.project_point(m, point, false), FeatureId::Unknown) + } +} diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index b972a3e..822a5b0 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,7 +1,7 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Triangle, Trimesh, + Proximity, Ray, RayIntersection, Roundable, Rounded, Triangle, Trimesh, }; #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; @@ -9,7 +9,7 @@ use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use downcast_rs::{impl_downcast, DowncastSync}; use erased_serde::Serialize; use na::Point3; -use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use ncollide::bounding_volume::{BoundingVolume, HasBoundingVolume, AABB}; use ncollide::query::{PointQuery, RayCast}; use num::Zero; use num_derive::FromPrimitive; @@ -40,6 +40,18 @@ pub enum ShapeType { Cone, // /// A custom shape type. // Custom(u8), + // /// A cuboid with rounded corners. + // RoundedCuboid, + // /// A triangle with rounded corners. + // RoundedTriangle, + // /// A triangle-mesh with rounded corners. + // RoundedTrimesh, + // /// An heightfield with rounded corners. + // RoundedHeightField, + /// A cylinder with rounded corners. + RoundedCylinder, + // /// A cone with rounded corners. + // RoundedCone, } /// Trait implemented by shapes usable by Rapier. @@ -61,7 +73,7 @@ pub trait Shape: RayCast + PointQuery + DowncastSync { /// Converts this shape to a polygonal feature-map, if it is one. #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { None } @@ -112,6 +124,15 @@ impl dyn Shape { pub fn as_cone(&self) -> Option<&Cone> { self.downcast_ref() } + + /// Converts this abstract shape to a cone, if it is one. + pub fn as_rounded(&self) -> Option<&Rounded> + where + S: Roundable, + Rounded: Shape, + { + self.downcast_ref() + } } impl Shape for Ball { @@ -171,8 +192,8 @@ impl Shape for Cuboid { } #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - Some(self as &dyn PolygonalFeatureMap) + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) } } @@ -214,8 +235,8 @@ impl Shape for Triangle { } #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - Some(self as &dyn PolygonalFeatureMap) + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) } } @@ -277,8 +298,8 @@ impl Shape for Cylinder { } #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - Some(self as &dyn PolygonalFeatureMap) + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) } } @@ -302,7 +323,32 @@ impl Shape for Cone { } #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - Some(self as &dyn PolygonalFeatureMap) + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) + } +} + +#[cfg(feature = "dim3")] +impl Shape for Rounded { + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.shape.compute_aabb(position).loosened(self.radius) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + // We ignore the margin here. + self.shape.mass_properties(density) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::RoundedCylinder + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((&self.shape as &dyn PolygonalFeatureMap, self.radius)) } } diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 1908745..7bdc812 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -411,7 +411,7 @@ impl GraphicsManager { } #[cfg(feature = "dim3")] - if let Some(cylinder) = shape.as_cylinder() { + if let Some(cylinder) = shape.as_cylinder().or(shape.as_rounded().map(|r| &r.shape)) { out.push(Node::Cylinder(Cylinder::new( handle, cylinder.half_height, From 949e3f5384a366c3bff5415c5db4635e811a580e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 20 Oct 2020 16:22:53 +0200 Subject: [PATCH 10/41] Fix many warnings. --- src/dynamics/mass_properties_capsule.rs | 4 +-- src/dynamics/mass_properties_cylinder.rs | 7 ++++-- src/dynamics/mass_properties_polygon.rs | 2 ++ src/geometry/collider.rs | 19 ++++++-------- .../ball_convex_contact_generator.rs | 2 +- .../capsule_capsule_contact_generator.rs | 24 ++++++++---------- .../contact_generator/contact_dispatcher.rs | 8 +++--- .../contact_generator/contact_generator.rs | 8 +++--- .../cuboid_capsule_contact_generator.rs | 3 +-- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 2 +- .../heightfield_shape_contact_generator.rs | 6 ++--- .../polygon_polygon_contact_generator.rs | 4 +-- .../trimesh_shape_contact_generator.rs | 2 +- src/geometry/polygonal_feature_map.rs | 2 +- src/geometry/polyhedron_feature3d.rs | 2 -- .../ball_convex_proximity_detector.rs | 2 +- .../cuboid_cuboid_proximity_detector.rs | 2 +- .../cuboid_triangle_proximity_detector.rs | 2 +- .../polygon_polygon_proximity_detector.rs | 4 +-- .../proximity_detector/proximity_detector.rs | 8 +++--- .../proximity_dispatcher.rs | 2 +- .../trimesh_shape_proximity_detector.rs | 2 +- src/geometry/rounded.rs | 5 +++- src/geometry/shape.rs | 25 ++++++++++--------- src/geometry/trimesh.rs | 16 ++++++------ src/utils.rs | 1 + src_testbed/engine.rs | 5 ++-- 28 files changed, 87 insertions(+), 84 deletions(-) diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index c4e039c..cfd4345 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -1,7 +1,7 @@ use crate::dynamics::MassProperties; +use crate::math::Point; #[cfg(feature = "dim3")] -use crate::geometry::Capsule; -use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; +use crate::math::Rotation; impl MassProperties { pub(crate) fn from_capsule(density: f32, half_height: f32, radius: f32) -> Self { diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs index 66a1343..8d4f254 100644 --- a/src/dynamics/mass_properties_cylinder.rs +++ b/src/dynamics/mass_properties_cylinder.rs @@ -1,7 +1,10 @@ use crate::dynamics::MassProperties; +use crate::math::{PrincipalAngularInertia, Vector}; #[cfg(feature = "dim3")] -use crate::geometry::Capsule; -use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; +use { + crate::geometry::Capsule, + crate::math::{Point, Rotation}, +}; impl MassProperties { pub(crate) fn cylinder_y_volume_unit_inertia( diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs index c87e888..8b0b811 100644 --- a/src/dynamics/mass_properties_polygon.rs +++ b/src/dynamics/mass_properties_polygon.rs @@ -1,3 +1,5 @@ +#![allow(dead_code)] // TODO: remove this + use crate::dynamics::MassProperties; use crate::math::Point; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 4b3b115..4785d62 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,18 +1,13 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Rounded, Shape, ShapeType, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Proximity, + Shape, ShapeType, Triangle, Trimesh, }; #[cfg(feature = "dim3")] -use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; +use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, Rounded}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; -use downcast_rs::{impl_downcast, DowncastSync}; -use erased_serde::Serialize; use na::Point3; -use ncollide::bounding_volume::{HasBoundingVolume, AABB}; -use ncollide::query::RayCast; -use num::Zero; -use std::any::Any; +use ncollide::bounding_volume::AABB; use std::ops::Deref; use std::sync::Arc; @@ -21,8 +16,8 @@ use std::sync::Arc; pub struct ColliderShape(pub Arc); impl Deref for ColliderShape { - type Target = Shape; - fn deref(&self) -> &Shape { + type Target = dyn Shape; + fn deref(&self) -> &dyn Shape { &*self.0 } } @@ -257,7 +252,7 @@ impl Collider { } /// The geometric shape of this collider. - pub fn shape(&self) -> &Shape { + pub fn shape(&self) -> &dyn Shape { &*self.shape.0 } diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index 62ebfab..69bc5e3 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -1,5 +1,5 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Ball, Contact, KinematicsCategory, Shape}; +use crate::geometry::{Ball, Contact, KinematicsCategory}; use crate::math::Isometry; use na::Unit; use ncollide::query::PointQuery; diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index 4d9bbc7..b24227a 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -1,11 +1,11 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory, Shape}; +use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory}; use crate::math::Isometry; use crate::math::Vector; use approx::AbsDiffEq; use na::Unit; #[cfg(feature = "dim2")] -use ncollide::shape::{Segment, SegmentPointLocation}; +use ncollide::shape::SegmentPointLocation; pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) { if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) { @@ -39,10 +39,11 @@ pub fn generate_contacts<'a>( let pos12 = pos1.inverse() * pos2; let pos21 = pos12.inverse(); - let capsule2_1 = capsule2.transform_by(&pos12); + let seg1 = capsule1.segment(); + let seg2_1 = capsule2.segment().transformed(&pos12); let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( - (&capsule1.a, &capsule1.b), - (&capsule2_1.a, &capsule2_1.b), + (&seg1.a, &seg1.b), + (&seg2_1.a, &seg2_1.b), ); // We do this clone to perform contact tracking and transfer impulses. @@ -65,8 +66,8 @@ pub fn generate_contacts<'a>( let bcoords1 = loc1.barycentric_coordinates(); let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1]; - let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1]; + 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()); @@ -87,18 +88,15 @@ pub fn generate_contacts<'a>( return; } - let seg1 = Segment::new(capsule1.a, capsule1.b); - let seg2 = Segment::new(capsule2_1.a, capsule2_1.b); - - if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2.direction()) { + 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)) = crate::geometry::clip_segments_with_normal( - (capsule1.a, capsule1.b), - (capsule2_1.a, capsule2_1.b), + (seg1.a, seg1.b), + (seg2_1.a, seg2_1.b), *local_n1, ) { let contact = diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 14b7f79..53165ee 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -1,9 +1,10 @@ +#[cfg(feature = "dim3")] +use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace; use crate::geometry::contact_generator::{ ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace, - PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator, - TrimeshShapeContactGeneratorWorkspace, + PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace, }; -use crate::geometry::{Shape, ShapeType}; +use crate::geometry::ShapeType; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm @@ -92,6 +93,7 @@ impl ContactDispatcher for DefaultContactDispatcher { None, ) } + #[cfg(feature = "dim3")] (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) | (ShapeType::Cone, _) diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index 9dd0050..b034760 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -139,8 +139,8 @@ pub struct PrimitiveContactGenerationContext<'a> { pub prediction_distance: f32, pub collider1: &'a Collider, pub collider2: &'a Collider, - pub shape1: &'a Shape, - pub shape2: &'a Shape, + pub shape1: &'a dyn Shape, + pub shape2: &'a dyn Shape, pub position1: &'a Isometry, pub position2: &'a Isometry, pub manifold: &'a mut ContactManifold, @@ -152,8 +152,8 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> { pub prediction_distance: f32, pub colliders1: [&'a Collider; SIMD_WIDTH], pub colliders2: [&'a Collider; SIMD_WIDTH], - pub shapes1: [&'a Shape; SIMD_WIDTH], - pub shapes2: [&'a Shape; SIMD_WIDTH], + pub shapes1: [&'a dyn Shape; SIMD_WIDTH], + pub shapes2: [&'a dyn Shape; SIMD_WIDTH], pub positions1: &'a Isometry, pub positions2: &'a Isometry, pub manifolds: &'a mut [&'b mut ContactManifold], diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index c3cf588..8650a78 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -1,12 +1,11 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; #[cfg(feature = "dim3")] use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory, Shape}; +use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory}; #[cfg(feature = "dim2")] use crate::geometry::{CuboidFeature, CuboidFeatureFace}; use crate::math::Isometry; use crate::math::Vector; -use ncollide::shape::Segment; pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) { if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) { diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index 596cfb1..5be5af3 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -1,5 +1,5 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory, Shape}; +use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory}; use crate::math::Isometry; #[cfg(feature = "dim2")] use crate::math::Vector; diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 6744c0e..562d7d6 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -1,7 +1,7 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; #[cfg(feature = "dim3")] use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Shape, Triangle}; +use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Triangle}; use crate::math::Isometry; #[cfg(feature = "dim2")] use crate::{ diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index f507caf..820caa3 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -3,7 +3,7 @@ use crate::geometry::contact_generator::{ }; #[cfg(feature = "dim2")] use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType}; +use crate::geometry::{Collider, ContactManifold, HeightField, ShapeType}; use crate::ncollide::bounding_volume::BoundingVolume; #[cfg(feature = "dim3")] use crate::{geometry::Triangle, math::Point}; @@ -117,8 +117,8 @@ fn do_generate_contacts( let position1 = *collider1.position(); #[cfg(feature = "dim2")] let (position1, sub_shape1) = { - let (dpos, height) = crate::utils::segment_to_capsule(&part1.a, &part1.b); - (position1 * dpos, Capsule::new(height, 0.0)); + let (dpos, half_height) = crate::utils::segment_to_capsule(&part1.a, &part1.b); + (position1 * dpos, Capsule::new(half_height, 0.0)) }; #[cfg(feature = "dim3")] let sub_shape1 = *part1; diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index c150e83..e1e6e7e 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -1,10 +1,10 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon, Shape}; +use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon}; use crate::math::{Isometry, Point}; #[cfg(feature = "dim2")] use crate::{math::Vector, utils}; -pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) { +pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationContext) { unimplemented!() // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { // generate_contacts( diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 49e9b40..502658d 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -1,7 +1,7 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, Shape, ShapeType, Trimesh}; +use crate::geometry::{Collider, ContactManifold, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeContactGeneratorWorkspace { diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index fc5e066..2586826 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -2,7 +2,7 @@ use crate::geometry::PolyhedronFace; use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle}; use crate::math::{Point, Vector}; use approx::AbsDiffEq; -use na::{Unit, Vector2, Vector3}; +use na::{Unit, Vector2}; use ncollide::shape::Segment; use ncollide::shape::SupportMap; diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 8f5de04..5a86ef2 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -387,8 +387,6 @@ impl PolyhedronFace { /// Compute the barycentric coordinates of the intersection between the two given lines. /// Returns `None` if the lines are parallel. fn closest_points_line2d(edge1: [Point2; 2], edge2: [Point2; 2]) -> Option<(f32, f32)> { - use approx::AbsDiffEq; - // Inspired by Real-time collision detection by Christer Ericson. let dir1 = edge1[1] - edge1[0]; let dir2 = edge2[1] - edge2[0]; diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs index d7c5d02..eda6547 100644 --- a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs @@ -1,5 +1,5 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{Ball, Proximity, Shape}; +use crate::geometry::{Ball, Proximity}; use crate::math::Isometry; use ncollide::query::PointQuery; diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs index 2462fc9..ae885b3 100644 --- a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs @@ -1,5 +1,5 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Proximity, Shape}; +use crate::geometry::{sat, Proximity}; use crate::math::Isometry; use ncollide::shape::Cuboid; diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs index 45991c7..532ab36 100644 --- a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs @@ -1,5 +1,5 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Cuboid, Proximity, Shape, Triangle}; +use crate::geometry::{sat, Cuboid, Proximity, Triangle}; use crate::math::Isometry; pub fn detect_proximity_cuboid_triangle( diff --git a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs index 5b89dc5..30a02fa 100644 --- a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs +++ b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs @@ -1,9 +1,9 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Polygon, Proximity, Shape}; +use crate::geometry::{sat, Polygon, Proximity}; use crate::math::Isometry; pub fn detect_proximity_polygon_polygon( - ctxt: &mut PrimitiveProximityDetectionContext, + _ctxt: &mut PrimitiveProximityDetectionContext, ) -> Proximity { unimplemented!() // if let (Some(polygon1), Some(polygon2)) = (ctxt.shape1.as_polygon(), ctxt.shape2.as_polygon()) { diff --git a/src/geometry/proximity_detector/proximity_detector.rs b/src/geometry/proximity_detector/proximity_detector.rs index 76e8cd7..7c8ad20 100644 --- a/src/geometry/proximity_detector/proximity_detector.rs +++ b/src/geometry/proximity_detector/proximity_detector.rs @@ -120,8 +120,8 @@ pub struct PrimitiveProximityDetectionContext<'a> { pub prediction_distance: f32, pub collider1: &'a Collider, pub collider2: &'a Collider, - pub shape1: &'a Shape, - pub shape2: &'a Shape, + pub shape1: &'a dyn Shape, + pub shape2: &'a dyn Shape, pub position1: &'a Isometry, pub position2: &'a Isometry, pub workspace: Option<&'a mut (dyn Any + Send + Sync)>, @@ -132,8 +132,8 @@ pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> { pub prediction_distance: f32, pub colliders1: [&'a Collider; SIMD_WIDTH], pub colliders2: [&'a Collider; SIMD_WIDTH], - pub shapes1: [&'a Shape; SIMD_WIDTH], - pub shapes2: [&'a Shape; SIMD_WIDTH], + pub shapes1: [&'a dyn Shape; SIMD_WIDTH], + pub shapes2: [&'a dyn Shape; SIMD_WIDTH], pub positions1: &'a Isometry, pub positions2: &'a Isometry, pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>], diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs index 62f50f7..768aca6 100644 --- a/src/geometry/proximity_detector/proximity_dispatcher.rs +++ b/src/geometry/proximity_detector/proximity_dispatcher.rs @@ -2,7 +2,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetector, ProximityDetector, ProximityPhase, TrimeshShapeProximityDetectorWorkspace, }; -use crate::geometry::{Shape, ShapeType}; +use crate::geometry::ShapeType; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index cce46d2..b469637 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -1,7 +1,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; -use crate::geometry::{Collider, Proximity, Shape, ShapeType, Trimesh}; +use crate::geometry::{Collider, Proximity, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeProximityDetectorWorkspace { diff --git a/src/geometry/rounded.rs b/src/geometry/rounded.rs index 59c6a72..ce3fc96 100644 --- a/src/geometry/rounded.rs +++ b/src/geometry/rounded.rs @@ -1,4 +1,6 @@ -use crate::geometry::{Cylinder, ShapeType}; +#[cfg(feature = "dim3")] +use crate::geometry::Cylinder; +use crate::geometry::ShapeType; use crate::math::{Isometry, Point, Vector}; use na::Unit; use ncollide::query::{ @@ -12,6 +14,7 @@ pub trait Roundable { fn rounded_shape_type() -> ShapeType; } +#[cfg(feature = "dim3")] impl Roundable for Cylinder { fn rounded_shape_type() -> ShapeType { ShapeType::RoundedCylinder diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index 822a5b0..b6350e9 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,19 +1,17 @@ -use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; -use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Roundable, Rounded, Triangle, Trimesh, -}; -#[cfg(feature = "dim3")] -use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}; -use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; +use crate::dynamics::MassProperties; +use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Roundable, Rounded, Triangle, Trimesh}; +use crate::math::Isometry; use downcast_rs::{impl_downcast, DowncastSync}; use erased_serde::Serialize; -use na::Point3; -use ncollide::bounding_volume::{BoundingVolume, HasBoundingVolume, AABB}; +use ncollide::bounding_volume::{HasBoundingVolume, AABB}; use ncollide::query::{PointQuery, RayCast}; use num::Zero; use num_derive::FromPrimitive; -use std::any::Any; +#[cfg(feature = "dim3")] +use { + crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}, + ncollide::bounding_volume::BoundingVolume, +}; #[derive(Copy, Clone, Debug, FromPrimitive)] /// Enum representing the type of a shape. @@ -49,6 +47,7 @@ pub enum ShapeType { // /// An heightfield with rounded corners. // RoundedHeightField, /// A cylinder with rounded corners. + #[cfg(feature = "dim3")] RoundedCylinder, // /// A cone with rounded corners. // RoundedCone, @@ -116,11 +115,13 @@ impl dyn Shape { } /// Converts this abstract shape to a cylinder, if it is one. + #[cfg(feature = "dim3")] pub fn as_cylinder(&self) -> Option<&Cylinder> { self.downcast_ref() } /// Converts this abstract shape to a cone, if it is one. + #[cfg(feature = "dim3")] pub fn as_cone(&self) -> Option<&Cone> { self.downcast_ref() } @@ -226,7 +227,7 @@ impl Shape for Triangle { self.bounding_volume(position) } - fn mass_properties(&self, density: f32) -> MassProperties { + fn mass_properties(&self, _density: f32) -> MassProperties { MassProperties::zero() } diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index 38ce0a3..b4e8518 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -107,15 +107,15 @@ impl Trimesh { } impl PointQuery for Trimesh { - fn project_point(&self, m: &Isometry, pt: &Point, solid: bool) -> PointProjection { + fn project_point(&self, _m: &Isometry, _pt: &Point, _solid: bool) -> PointProjection { // TODO unimplemented!() } fn project_point_with_feature( &self, - m: &Isometry, - pt: &Point, + _m: &Isometry, + _pt: &Point, ) -> (PointProjection, FeatureId) { // TODO unimplemented!() @@ -126,16 +126,16 @@ impl PointQuery for Trimesh { impl RayCast for Trimesh { fn toi_and_normal_with_ray( &self, - m: &Isometry, - ray: &Ray, - max_toi: f32, - solid: bool, + _m: &Isometry, + _ray: &Ray, + _max_toi: f32, + _solid: bool, ) -> Option { // TODO None } - fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_toi: f32) -> bool { + fn intersects_ray(&self, _m: &Isometry, _ray: &Ray, _max_toi: f32) -> bool { // TODO false } diff --git a/src/utils.rs b/src/utils.rs index 48f4ef7..0bc9e17 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -20,6 +20,7 @@ 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; +#[cfg(feature = "dim3")] pub(crate) const COS_1_DEGREES: f32 = 0.99984769515; pub(crate) const COS_5_DEGREES: f32 = 0.99619469809; // #[cfg(feature = "dim2")] diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 7bdc812..d6edea5 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -9,11 +9,10 @@ use na::Point3; use crate::math::Point; use crate::objects::ball::Ball; use crate::objects::box_node::Box as BoxNode; -use crate::objects::convex::Convex; use crate::objects::heightfield::HeightField; use crate::objects::node::{GraphicsNode, Node}; use rapier::dynamics::{RigidBodyHandle, RigidBodySet}; -use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape}; +use rapier::geometry::{Collider, ColliderHandle, ColliderSet}; //use crate::objects::capsule::Capsule; //use crate::objects::convex::Convex; //#[cfg(feature = "fluids")] @@ -26,7 +25,9 @@ use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape}; //#[cfg(feature = "fluids")] //use crate::objects::FluidRenderingMode; use crate::objects::capsule::Capsule; +#[cfg(feature = "dim3")] use crate::objects::cone::Cone; +#[cfg(feature = "dim3")] use crate::objects::cylinder::Cylinder; use crate::objects::mesh::Mesh; use rand::{Rng, SeedableRng}; From f7a6f433d62ea427f6e2233365a6f534ca7e1c63 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 20 Oct 2020 18:14:20 +0200 Subject: [PATCH 11/41] Rename rounded -> round. --- examples3d/heightfield3.rs | 2 +- examples3d/primitives3.rs | 2 +- examples3d/trimesh3.rs | 2 +- src/geometry/collider.rs | 8 ++++---- src/geometry/contact_generator/contact_dispatcher.rs | 4 ++-- src/geometry/rounded.rs | 2 +- src/geometry/shape.rs | 4 ++-- 7 files changed, 12 insertions(+), 12 deletions(-) diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index 8c3386a..1213020 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -59,7 +59,7 @@ pub fn init_world(testbed: &mut Testbed) { 1 => ColliderBuilder::ball(rad).build(), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. - 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index daabd23..96636a4 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -55,7 +55,7 @@ pub fn init_world(testbed: &mut Testbed) { 1 => ColliderBuilder::ball(rad).build(), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. - 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index 8fee784..5ada899 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -69,7 +69,7 @@ pub fn init_world(testbed: &mut Testbed) { 1 => ColliderBuilder::ball(rad).build(), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. - 2 => ColliderBuilder::rounded_cylinder(rad, rad, rad / 10.0).build(), + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), _ => ColliderBuilder::cone(rad, rad).build(), }; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 4785d62..755ae5a 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -39,7 +39,7 @@ impl ColliderShape { /// (along along the y axis), its radius, and its roundedness (the /// radius of the sphere used for dilating the cylinder). #[cfg(feature = "dim3")] - pub fn rounded_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { + pub fn round_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { ColliderShape(Arc::new(Rounded::new( Cylinder::new(half_height, radius), rounding_radius, @@ -164,7 +164,7 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { #[cfg(feature = "dim3")] Some(ShapeType::Cone) => deser::(&mut seq)?, #[cfg(feature = "dim3")] - Some(ShapeType::RoundedCylinder) => deser::>(&mut seq)?, + Some(ShapeType::RoundCylinder) => deser::>(&mut seq)?, None => { return Err(serde::de::Error::custom( "found invalid shape type to deserialize", @@ -326,8 +326,8 @@ impl ColliderBuilder { /// (along along the y axis), its radius, and its roundedness (the /// radius of the sphere used for dilating the cylinder). #[cfg(feature = "dim3")] - pub fn rounded_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { - Self::new(ColliderShape::rounded_cylinder( + pub fn round_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { + Self::new(ColliderShape::round_cylinder( half_height, radius, rounding_radius, diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 53165ee..62a1f71 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -98,8 +98,8 @@ impl ContactDispatcher for DefaultContactDispatcher { | (_, ShapeType::Cylinder) | (ShapeType::Cone, _) | (_, ShapeType::Cone) - | (ShapeType::RoundedCylinder, _) - | (_, ShapeType::RoundedCylinder) => ( + | (ShapeType::RoundCylinder, _) + | (_, ShapeType::RoundCylinder) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() diff --git a/src/geometry/rounded.rs b/src/geometry/rounded.rs index ce3fc96..bfbab3b 100644 --- a/src/geometry/rounded.rs +++ b/src/geometry/rounded.rs @@ -17,7 +17,7 @@ pub trait Roundable { #[cfg(feature = "dim3")] impl Roundable for Cylinder { fn rounded_shape_type() -> ShapeType { - ShapeType::RoundedCylinder + ShapeType::RoundCylinder } } diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index b6350e9..80c9b51 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -48,7 +48,7 @@ pub enum ShapeType { // RoundedHeightField, /// A cylinder with rounded corners. #[cfg(feature = "dim3")] - RoundedCylinder, + RoundCylinder, // /// A cone with rounded corners. // RoundedCone, } @@ -345,7 +345,7 @@ impl Shape for Rounded { } fn shape_type(&self) -> ShapeType { - ShapeType::RoundedCylinder + ShapeType::RoundCylinder } #[cfg(feature = "dim3")] From b9156302d331a1eaf9f40b84d8c1ffd68b9040ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 20 Oct 2020 18:57:53 +0200 Subject: [PATCH 12/41] Replace rounding -> round. --- src/geometry/collider.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 755ae5a..838dda2 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -39,10 +39,10 @@ impl ColliderShape { /// (along along the y axis), its radius, and its roundedness (the /// radius of the sphere used for dilating the cylinder). #[cfg(feature = "dim3")] - pub fn round_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { + pub fn round_cylinder(half_height: f32, radius: f32, round_radius: f32) -> Self { ColliderShape(Arc::new(Rounded::new( Cylinder::new(half_height, radius), - rounding_radius, + round_radius, ))) } @@ -326,11 +326,11 @@ impl ColliderBuilder { /// (along along the y axis), its radius, and its roundedness (the /// radius of the sphere used for dilating the cylinder). #[cfg(feature = "dim3")] - pub fn round_cylinder(half_height: f32, radius: f32, rounding_radius: f32) -> Self { + pub fn round_cylinder(half_height: f32, radius: f32, round_radius: f32) -> Self { Self::new(ColliderShape::round_cylinder( half_height, radius, - rounding_radius, + round_radius, )) } From 9315da2b6c959303a4c103d1fa3c6653adb53143 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 14:12:31 +0100 Subject: [PATCH 13/41] Fix bug in 2D heightfield collision detection. --- .../heightfield_shape_contact_generator.rs | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 820caa3..e66bbac 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -3,7 +3,7 @@ use crate::geometry::contact_generator::{ }; #[cfg(feature = "dim2")] use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, HeightField, ShapeType}; +use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType}; use crate::ncollide::bounding_volume::BoundingVolume; #[cfg(feature = "dim3")] use crate::{geometry::Triangle, math::Point}; @@ -72,11 +72,6 @@ fn do_generate_contacts( } else { manifold.subshape_index_pair.1 }; - // println!( - // "Restoring for {} [chosen with {:?}]", - // subshape_id, manifold.subshape_index_pair - // ); - let (generator, workspace2) = ctxt .dispatcher .dispatch_primitives(ShapeType::Capsule, shape_type2); @@ -116,9 +111,9 @@ fn do_generate_contacts( heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| { let position1 = *collider1.position(); #[cfg(feature = "dim2")] - let (position1, sub_shape1) = { + let (position1, dpos1, sub_shape1) = { let (dpos, half_height) = crate::utils::segment_to_capsule(&part1.a, &part1.b); - (position1 * dpos, Capsule::new(half_height, 0.0)) + (position1 * dpos, dpos, Capsule::new(half_height, 0.0)) }; #[cfg(feature = "dim3")] let sub_shape1 = *part1; @@ -134,7 +129,7 @@ fn do_generate_contacts( } Entry::Vacant(entry) => { let (generator, workspace2) = - dispatcher.dispatch_primitives(ShapeType::Triangle, shape_type2); + dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2); let sub_detector = SubDetector { generator, manifold_id: manifolds.len(), @@ -177,6 +172,13 @@ fn do_generate_contacts( } }; + #[cfg(feature = "dim2")] + if coll_pair.collider1 != ctxt2.manifold.pair.collider1 { + ctxt2.manifold.delta2 = collider1.position_wrt_parent() * dpos1; + } else { + ctxt2.manifold.delta1 = collider1.position_wrt_parent() * dpos1; + } + (sub_detector.generator.generate_contacts)(&mut ctxt2) }); From e028f4504065b228bea8b736265e8b1bfb58f7f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 14:33:31 +0100 Subject: [PATCH 14/41] Update dependencies. --- Cargo.toml | 5 +++++ benchmarks2d/Cargo.toml | 2 +- benchmarks3d/Cargo.toml | 2 +- build/rapier2d/Cargo.toml | 11 +++++------ build/rapier3d/Cargo.toml | 11 +++++------ build/rapier_testbed2d/Cargo.toml | 10 +++++----- build/rapier_testbed3d/Cargo.toml | 14 +++++++------- examples2d/Cargo.toml | 2 +- examples3d/Cargo.toml | 2 +- 9 files changed, 31 insertions(+), 28 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 944fa40..cfb166a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -5,6 +5,11 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark [patch.crates-io] #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" } #simba = { path = "../simba" } +#ncollide2d = { path = "../ncollide/build/ncollide2d" } +#ncollide3d = { path = "../ncollide/build/ncollide3d" } +#nphysics2d = { path = "../nphysics/build/nphysics2d" } +#nphysics3d = { path = "../nphysics/build/nphysics3d" } +#kiss3d = { path = "../kiss3d" } [profile.release] #debug = true diff --git a/benchmarks2d/Cargo.toml b/benchmarks2d/Cargo.toml index b1ee8db..90a6326 100644 --- a/benchmarks2d/Cargo.toml +++ b/benchmarks2d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.22" +nalgebra = "0.23" [dependencies.rapier_testbed2d] path = "../build/rapier_testbed2d" diff --git a/benchmarks3d/Cargo.toml b/benchmarks3d/Cargo.toml index 81c843a..03194df 100644 --- a/benchmarks3d/Cargo.toml +++ b/benchmarks3d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.22" +nalgebra = "0.23" [dependencies.rapier_testbed3d] path = "../build/rapier_testbed3d" diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 44038e6..d6e5a29 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -1,4 +1,3 @@ -# Name idea: bident for 2D and trident for 3D [package] name = "rapier2d" version = "0.2.1" @@ -35,12 +34,12 @@ required-features = [ "dim2" ] vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" -nalgebra = "0.22" -ncollide2d = "0.25" -simba = "^0.2.1" -approx = "0.3" +nalgebra = "0.23" +ncollide2d = "0.26" +simba = "0.3" +approx = "0.4" rayon = { version = "1", optional = true } -crossbeam = "0.7" +crossbeam = "0.8" generational-arena = "0.2" arrayvec = "0.5" bit-vec = "0.6" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index adc453c..e9b9808 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -1,4 +1,3 @@ -# Name idea: bident for 2D and trident for 3D [package] name = "rapier3d" version = "0.2.1" @@ -35,12 +34,12 @@ required-features = [ "dim3" ] vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" -nalgebra = "0.22" -ncollide3d = "0.25" -simba = "^0.2.1" -approx = "0.3" +nalgebra = "0.23" +ncollide3d = "0.26" +simba = "0.3" +approx = "0.4" rayon = { version = "1", optional = true } -crossbeam = "0.7" +crossbeam = "0.8" generational-arena = "0.2" arrayvec = "0.5" bit-vec = "0.6" diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index 95595bb..87610c3 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -23,17 +23,17 @@ other-backends = [ "wrapped2d", "nphysics2d" ] [dependencies] -nalgebra = "0.22" -kiss3d = { version = "0.26", features = [ "conrod" ] } +nalgebra = "0.23" +kiss3d = { version = "0.27", 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.25" -nphysics2d = { version = "0.17", optional = true } -crossbeam = "0.7" +ncollide2d = "0.26" +nphysics2d = { version = "0.18", optional = true } +crossbeam = "0.8" bincode = "1" flexbuffers = "0.1" Inflector = "0.11" diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index 46edd3b..e99d741 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -22,19 +22,19 @@ parallel = [ "rapier3d/parallel", "num_cpus" ] other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ] [dependencies] -nalgebra = "0.22" -kiss3d = { version = "0.26", features = [ "conrod" ] } +nalgebra = "0.23" +kiss3d = { version = "0.27", 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 } +glam = { version = "0.9", optional = true } num_cpus = { version = "1", optional = true } -ncollide3d = "0.25" -nphysics3d = { version = "0.17", optional = true } -physx = { version = "0.6", optional = true } +ncollide3d = "0.26" +nphysics3d = { version = "0.18", optional = true } +physx = { version = "0.7", optional = true } physx-sys = { version = "0.4", optional = true } -crossbeam = "0.7" +crossbeam = "0.8" bincode = "1" flexbuffers = "0.1" serde_cbor = "0.11" diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml index ad63958..f1ed728 100644 --- a/examples2d/Cargo.toml +++ b/examples2d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.22" +nalgebra = "0.23" [dependencies.rapier_testbed2d] path = "../build/rapier_testbed2d" diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index efc3cce..5362554 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.22" +nalgebra = "0.23" [dependencies.rapier_testbed3d] path = "../build/rapier_testbed3d" From 3da333f11c93898808eb9233c0cf333743bbf906 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 15:12:10 +0100 Subject: [PATCH 15/41] Fix testbed compilation with other backends. --- .github/workflows/rapier-ci-bench.yml | 4 +- src_testbed/box2d_backend.rs | 70 +++++++++++++-------------- src_testbed/nphysics_backend.rs | 48 ++++++++++-------- src_testbed/physx_backend.rs | 38 ++++++--------- 4 files changed, 81 insertions(+), 79 deletions(-) diff --git a/.github/workflows/rapier-ci-bench.yml b/.github/workflows/rapier-ci-bench.yml index 9cce1b2..8a43818 100644 --- a/.github/workflows/rapier-ci-bench.yml +++ b/.github/workflows/rapier-ci-bench.yml @@ -18,15 +18,17 @@ jobs: BENCHBOT_TARGET_COMMIT: ${{ github.event.pull_request.head.sha }} BENCHBOT_SHA: ${{ github.sha }} BENCHBOT_HEAD_REF: ${{github.head_ref}} + BENCHBOT_OTHER_BACKENDS: 'true' runs-on: ubuntu-latest steps: - name: Find commit SHA if: github.ref == 'refs/heads/master' run: | echo "::set-env name=BENCHBOT_TARGET_COMMIT::$BENCHBOT_SHA" + echo "::set-env name=BENCHBOT_OTHER_BACKENDS::false" - name: Send 3D bench message shell: bash run: curl -u $BENCHBOT_AMQP_USER:$BENCHBOT_AMQP_PASS -i -H "content-type:application/json" -X POST https://$BENCHBOT_AMQP_HOST/api/exchanges/$BENCHBOT_AMQP_VHOST//publish - -d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\" }","payload_encoding":"string"}' \ No newline at end of file + -d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\", \"other_backends\":\"'$BENCHBOT_OTHER_BACKENDS'\" }","payload_encoding":"string"}' \ No newline at end of file diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index c25ff1f..9156c2a 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -167,44 +167,42 @@ impl Box2dWorld { fixture_def.is_sensor = collider.is_sensor(); fixture_def.filter = b2::Filter::new(); - match collider.shape() { - Shape::Ball(b) => { - let mut b2_shape = b2::CircleShape::new(); - b2_shape.set_radius(b.radius); - b2_shape.set_position(center); - body.create_fixture(&b2_shape, &mut fixture_def); - } - Shape::Cuboid(c) => { - let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y); - body.create_fixture(&b2_shape, &mut fixture_def); - } - Shape::Polygon(poly) => { - let points: Vec<_> = poly - .vertices() - .iter() - .map(|p| collider.position_wrt_parent() * p) - .map(|p| na_vec_to_b2_vec(p.coords)) - .collect(); - let b2_shape = b2::PolygonShape::new_with(&points); - body.create_fixture(&b2_shape, &mut fixture_def); - } - Shape::HeightField(heightfield) => { - let mut segments = heightfield.segments(); - let seg1 = segments.next().unwrap(); - let mut vertices = vec![ - na_vec_to_b2_vec(seg1.a.coords), - na_vec_to_b2_vec(seg1.b.coords), - ]; + let shape = collider.shape(); - // TODO: this will not handle holes properly. - segments.for_each(|seg| { - vertices.push(na_vec_to_b2_vec(seg.b.coords)); - }); + if let Some(b) = shape.as_ball() { + let mut b2_shape = b2::CircleShape::new(); + b2_shape.set_radius(b.radius); + b2_shape.set_position(center); + body.create_fixture(&b2_shape, &mut fixture_def); + } else if let Some(c) = shape.as_cuboid() { + let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y); + body.create_fixture(&b2_shape, &mut fixture_def); + // } else if let Some(polygon) = shape.as_polygon() { + // let points: Vec<_> = poly + // .vertices() + // .iter() + // .map(|p| collider.position_wrt_parent() * p) + // .map(|p| na_vec_to_b2_vec(p.coords)) + // .collect(); + // let b2_shape = b2::PolygonShape::new_with(&points); + // body.create_fixture(&b2_shape, &mut fixture_def); + } else if let Some(heightfield) = shape.as_heightfield() { + let mut segments = heightfield.segments(); + let seg1 = segments.next().unwrap(); + let mut vertices = vec![ + na_vec_to_b2_vec(seg1.a.coords), + na_vec_to_b2_vec(seg1.b.coords), + ]; - let b2_shape = b2::ChainShape::new_chain(&vertices); - body.create_fixture(&b2_shape, &mut fixture_def); - } - _ => eprintln!("Creating a shape unknown to the Box2d backend."), + // TODO: this will not handle holes properly. + segments.for_each(|seg| { + vertices.push(na_vec_to_b2_vec(seg.b.coords)); + }); + + let b2_shape = b2::ChainShape::new_chain(&vertices); + body.create_fixture(&b2_shape, &mut fixture_def); + } else { + eprintln!("Creating a shape unknown to the Box2d backend."); } } diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index c2a7fb1..b449c44 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -177,28 +177,36 @@ fn nphysics_collider_from_rapier_collider( ) -> Option> { let margin = ColliderDesc::::default_margin(); let mut pos = *collider.position_wrt_parent(); + let shape = collider.shape(); - let shape = match collider.shape() { - Shape::Cuboid(cuboid) => { - ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin))) - } - Shape::Ball(ball) => ShapeHandle::new(Ball::new(ball.radius - margin)), - Shape::Capsule(capsule) => { - pos *= capsule.transform_wrt_y(); - ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius)) - } - Shape::HeightField(heightfield) => ShapeHandle::new(heightfield.clone()), + let shape = if let Some(cuboid) = shape.as_cuboid() { + ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin))) + } else if let Some(ball) = shape.as_ball() { + ShapeHandle::new(Ball::new(ball.radius - margin)) + } else if let Some(capsule) = shape.as_capsule() { + ShapeHandle::new(Capsule::new(capsule.half_height, capsule.radius)) + } else if let Some(heightfield) = shape.as_heightfield() { + ShapeHandle::new(heightfield.clone()) + } else { #[cfg(feature = "dim3")] - Shape::Trimesh(trimesh) => ShapeHandle::new(TriMesh::new( - trimesh.vertices().to_vec(), - trimesh - .indices() - .iter() - .map(|idx| na::convert(*idx)) - .collect(), - None, - )), - _ => return None, + if let Some(trimesh) = shape.as_trimesh() { + ShapeHandle::new(TriMesh::new( + trimesh.vertices().to_vec(), + trimesh + .indices() + .iter() + .map(|idx| na::convert(*idx)) + .collect(), + None, + )) + } else { + return None; + } + + #[cfg(feature = "dim2")] + { + return None; + } }; let density = if is_dynamic { collider.density() } else { 0.0 }; diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index ec2e2bf..822ad08 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -422,27 +422,22 @@ fn physx_collider_from_rapier_collider( collider: &Collider, ) -> Option<(ColliderDesc, Isometry3)> { let mut local_pose = *collider.position_wrt_parent(); - let desc = match collider.shape() { - Shape::Cuboid(cuboid) => ColliderDesc::Box( + let shape = collider.shape(); + + let desc = if let Some(cuboid) = shape.as_cuboid() { + ColliderDesc::Box( cuboid.half_extents.x, cuboid.half_extents.y, cuboid.half_extents.z, - ), - Shape::Ball(ball) => ColliderDesc::Sphere(ball.radius), - Shape::Capsule(capsule) => { - let center = capsule.center(); - let mut dir = capsule.b - capsule.a; - - if dir.x < 0.0 { - dir = -dir; - } - - let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir); - local_pose *= - Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity()); - ColliderDesc::Capsule(capsule.radius, capsule.height()) - } - Shape::Trimesh(trimesh) => ColliderDesc::TriMesh { + ) + } else if let Some(ball) = shape.as_ball() { + ColliderDesc::Sphere(ball.radius) + } else if let Some(capsule) = shape.as_capsule() { + let rot = UnitQuaternion::rotation_between(&Vector3::x(), &Vector3::y()); + local_pose *= rot.unwrap_or(UnitQuaternion::identity()); + ColliderDesc::Capsule(capsule.radius, capsule.height()) + } else if let Some(trimesh) = shape.as_trimesh() { + ColliderDesc::TriMesh { vertices: trimesh .vertices() .iter() @@ -450,11 +445,10 @@ fn physx_collider_from_rapier_collider( .collect(), indices: trimesh.flat_indices().to_vec(), mesh_scale: Vector3::repeat(1.0).into_glam(), - }, - _ => { - eprintln!("Creating a shape unknown to the PhysX backend."); - return None; } + } else { + eprintln!("Creating a shape unknown to the PhysX backend."); + return None; }; Some((desc, local_pose)) From 2b628f9580a826722346983ef42672d4e8dd8053 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 15:58:30 +0100 Subject: [PATCH 16/41] Redefine capsules as a segment with a radius, allowing us to reuse the pfm_pfm_contact generator for it. --- examples3d/heightfield3.rs | 5 +- examples3d/primitives3.rs | 5 +- examples3d/trimesh3.rs | 5 +- src/dynamics/mass_properties_capsule.rs | 13 +-- src/geometry/capsule.rs | 86 ++++++++++++------- src/geometry/collider.rs | 35 ++++---- .../capsule_capsule_contact_generator.rs | 4 +- .../contact_generator/contact_dispatcher.rs | 4 +- .../cuboid_capsule_contact_generator.rs | 2 +- src/geometry/mod.rs | 6 +- src/geometry/polygonal_feature_map.rs | 5 +- src/geometry/polyhedron_feature3d.rs | 2 + src/geometry/shape.rs | 38 +++++++- src_testbed/nphysics_backend.rs | 3 +- src_testbed/objects/capsule.rs | 2 +- src_testbed/physx_backend.rs | 11 ++- 16 files changed, 150 insertions(+), 76 deletions(-) diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index 1213020..6af93c7 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -54,13 +54,14 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - let collider = match j % 4 { + let collider = match j % 5 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), - _ => ColliderBuilder::cone(rad, rad).build(), + 3 => ColliderBuilder::cone(rad, rad).build(), + _ => ColliderBuilder::capsule_y(rad, rad).build(), }; colliders.insert(collider, handle, &mut bodies); diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index 96636a4..6a98077 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -50,13 +50,14 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - let collider = match j % 4 { + let collider = match j % 5 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), - _ => ColliderBuilder::cone(rad, rad).build(), + 3 => ColliderBuilder::cone(rad, rad).build(), + _ => ColliderBuilder::capsule_y(rad, rad).build(), }; colliders.insert(collider, handle, &mut bodies); diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index 5ada899..2a96bda 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -64,13 +64,14 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - let collider = match j % 4 { + let collider = match j % 5 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), - _ => ColliderBuilder::cone(rad, rad).build(), + 3 => ColliderBuilder::cone(rad, rad).build(), + _ => ColliderBuilder::capsule_y(rad, rad).build(), }; colliders.insert(collider, handle, &mut bodies); diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index cfd4345..dae6cfb 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -1,22 +1,24 @@ use crate::dynamics::MassProperties; use crate::math::Point; #[cfg(feature = "dim3")] -use crate::math::Rotation; +use crate::{geometry::Capsule, math::Rotation}; impl MassProperties { - pub(crate) fn from_capsule(density: f32, half_height: f32, radius: f32) -> Self { + 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); let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); let cap_vol = cyl_vol + ball_vol; let cap_mass = cap_vol * density; let mut cap_unit_i = cyl_unit_i + ball_unit_i; + let local_com = na::center(&a, &b); #[cfg(feature = "dim2")] { let h = half_height * 2.0; let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; cap_unit_i += extra; - Self::new(Point::origin(), cap_mass, cap_unit_i * cap_mass) + Self::new(local_com, cap_mass, cap_unit_i * cap_mass) } #[cfg(feature = "dim3")] @@ -25,11 +27,12 @@ impl MassProperties { let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; cap_unit_i.x += extra; cap_unit_i.z += extra; + let local_frame = Capsule::new(a, b, radius).rotation_wrt_y(); Self::with_principal_inertia_frame( - Point::origin(), + local_com, cap_mass, cap_unit_i * cap_mass, - Rotation::identity(), + local_frame, ) } } diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs index 0d754af..54736cc 100644 --- a/src/geometry/capsule.rs +++ b/src/geometry/capsule.rs @@ -1,18 +1,16 @@ -use crate::geometry::AABB; +use crate::geometry::{Ray, RayIntersection, AABB}; use crate::math::{Isometry, Point, Rotation, Vector}; use approx::AbsDiffEq; use na::Unit; -use ncollide::query::{PointProjection, PointQuery}; -use ncollide::shape::{FeatureId, Segment}; +use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast}; +use ncollide::shape::{FeatureId, Segment, SupportMap}; #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A capsule shape defined as a segment with a radius. +/// A capsule shape defined as a round segment. pub struct Capsule { - /// The first endpoint of the capsule. - pub a: Point, - /// The second enpdoint of the capsule. - pub b: Point, + /// The axis and endpoint of the capsule. + pub segment: Segment, /// The radius of the capsule. pub radius: f32, } @@ -39,13 +37,14 @@ impl Capsule { /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. pub fn new(a: Point, b: Point, radius: f32) -> Self { - Self { a, b, radius } + let segment = Segment::new(a, b); + Self { segment, radius } } /// The axis-aligned bounding box of this capsule. pub fn aabb(&self, pos: &Isometry) -> AABB { - let a = pos * self.a; - let b = pos * self.b; + let a = pos * self.segment.a; + let b = pos * self.segment.b; let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius); let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius); AABB::new(mins.into(), maxs.into()) @@ -53,7 +52,7 @@ impl Capsule { /// The height of this capsule. pub fn height(&self) -> f32 { - (self.b - self.a).norm() + (self.segment.b - self.segment.a).norm() } /// The half-height of this capsule. @@ -63,17 +62,17 @@ impl Capsule { /// The center of this capsule. pub fn center(&self) -> Point { - na::center(&self.a, &self.b) + na::center(&self.segment.a, &self.segment.b) } /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. pub fn transform_by(&self, pos: &Isometry) -> Self { - Self::new(pos * self.a, pos * self.b, self.radius) + Self::new(pos * self.segment.a, pos * self.segment.b, self.radius) } /// The rotation `r` such that `r * Y` is collinear with `b - a`. pub fn rotation_wrt_y(&self) -> Rotation { - let mut dir = self.b - self.a; + let mut dir = self.segment.b - self.segment.a; if dir.y < 0.0 { dir = -dir; } @@ -96,24 +95,49 @@ impl Capsule { } } -// impl SupportMap for Capsule { -// fn local_support_point(&self, dir: &Vector) -> Point { -// let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis()); -// self.local_support_point_toward(&dir) -// } -// -// fn local_support_point_toward(&self, dir: &Unit) -> Point { -// if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) { -// self.a + **dir * self.radius -// } else { -// self.b + **dir * self.radius -// } -// } -// } +impl SupportMap for Capsule { + fn local_support_point(&self, dir: &Vector) -> Point { + let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis()); + self.local_support_point_toward(&dir) + } + + fn local_support_point_toward(&self, dir: &Unit>) -> Point { + if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) { + self.segment.a + **dir * self.radius + } else { + self.segment.b + **dir * self.radius + } + } +} + +impl RayCast for Capsule { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option { + let ls_ray = ray.inverse_transform_by(m); + + ncollide::query::ray_intersection_with_support_map_with_params( + &Isometry::identity(), + self, + &mut VoronoiSimplex::new(), + &ls_ray, + max_toi, + solid, + ) + .map(|mut res| { + res.normal = m * res.normal; + res + }) + } +} // TODO: this code has been extracted from ncollide and added here // so we can modify it to fit with our new definition of capsule. -// Wa should find a way to avoid this code duplication. +// We should find a way to avoid this code duplication. impl PointQuery for Capsule { #[inline] fn project_point( @@ -122,7 +146,7 @@ impl PointQuery for Capsule { pt: &Point, solid: bool, ) -> PointProjection { - let seg = Segment::new(self.a, self.b); + let seg = Segment::new(self.segment.a, self.segment.b); let proj = seg.project_point(m, pt, solid); let dproj = *pt - proj.point; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 838dda2..ae5cc73 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,7 +1,7 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Proximity, - Shape, ShapeType, Triangle, Trimesh, + Segment, Shape, ShapeType, Triangle, Trimesh, }; #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, Rounded}; @@ -58,9 +58,14 @@ impl ColliderShape { ColliderShape(Arc::new(Cuboid::new(half_extents))) } - /// Initialize a capsule shape aligned with the `y` axis. - pub fn capsule(half_height: f32, radius: f32) -> Self { - ColliderShape(Arc::new(Capsule::new(half_height, radius))) + /// Initialize a capsule shape from its endpoints and radius. + pub fn capsule(a: Point, b: Point, radius: f32) -> Self { + ColliderShape(Arc::new(Capsule::new(a, b, radius))) + } + + /// Initialize a segment shape from its endpoints. + pub fn segment(a: Point, b: Point) -> Self { + ColliderShape(Arc::new(Segment::new(a, b))) } /// Initializes a triangle shape. @@ -157,6 +162,7 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { Some(ShapeType::Cuboid) => deser::(&mut seq)?, Some(ShapeType::Capsule) => deser::(&mut seq)?, Some(ShapeType::Triangle) => deser::(&mut seq)?, + Some(ShapeType::Segment) => deser::(&mut seq)?, Some(ShapeType::Trimesh) => deser::(&mut seq)?, Some(ShapeType::HeightField) => deser::(&mut seq)?, #[cfg(feature = "dim3")] @@ -349,25 +355,21 @@ impl ColliderBuilder { /// Initialize a new collider builder with a capsule shape aligned with the `x` axis. pub fn capsule_x(half_height: f32, radius: f32) -> Self { - #[cfg(feature = "dim2")] - let rot = -std::f32::consts::FRAC_PI_2; - #[cfg(feature = "dim3")] - let rot = Vector::z() * -std::f32::consts::FRAC_PI_2; - Self::new(ColliderShape::capsule(half_height, radius)) - .position(Isometry::new(na::zero(), rot)) + let p = Point::from(Vector::x() * half_height); + Self::new(ColliderShape::capsule(-p, p, radius)) } /// Initialize a new collider builder with a capsule shape aligned with the `y` axis. pub fn capsule_y(half_height: f32, radius: f32) -> Self { - Self::new(ColliderShape::capsule(half_height, radius)) + let p = Point::from(Vector::y() * half_height); + Self::new(ColliderShape::capsule(-p, p, radius)) } /// Initialize a new collider builder with a capsule shape aligned with the `z` axis. #[cfg(feature = "dim3")] pub fn capsule_z(half_height: f32, radius: f32) -> Self { - let rot = Vector::x() * std::f32::consts::FRAC_PI_2; - Self::new(ColliderShape::capsule(half_height, radius)) - .position(Isometry::new(na::zero(), rot)) + let p = Point::from(Vector::z() * half_height); + Self::new(ColliderShape::capsule(-p, p, radius)) } /// Initialize a new collider builder with a cuboid shape defined by its half-extents. @@ -377,11 +379,8 @@ impl ColliderBuilder { } /// Initializes a collider builder with a segment shape. - /// - /// A segment shape is modeled by a capsule with a 0 radius. pub fn segment(a: Point, b: Point) -> Self { - let (pos, half_height) = crate::utils::segment_to_capsule(&a, &b); - Self::new(ColliderShape::capsule(half_height, 0.0)).position(pos) + Self::new(ColliderShape::segment(a, b)) } /// Initializes a collider builder with a triangle shape. diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index b24227a..ef18ad2 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -154,8 +154,8 @@ pub fn generate_contacts<'a>( let pos12 = pos1.inverse() * pos2; let pos21 = pos12.inverse(); - let seg1 = capsule1.segment(); - let seg2_1 = capsule2.segment().transformed(&pos12); + let seg1 = capsule1.segment; + let seg2_1 = capsule2.segment.transformed(&pos12); let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( (&seg1.a, &seg1.b), (&seg2_1.a, &seg2_1.b), diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 62a1f71..1872c7b 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -99,7 +99,9 @@ impl ContactDispatcher for DefaultContactDispatcher { | (ShapeType::Cone, _) | (_, ShapeType::Cone) | (ShapeType::RoundCylinder, _) - | (_, ShapeType::RoundCylinder) => ( + | (_, ShapeType::RoundCylinder) + | (ShapeType::Capsule, _) + | (_, ShapeType::Capsule) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index 8650a78..3fd4a17 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -54,7 +54,7 @@ pub fn generate_contacts<'a>( return; } - let segment2 = capsule2.segment(); + let segment2 = capsule2.segment; /* * diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index f73de98..a3423dd 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -1,6 +1,7 @@ //! Structures related to geometry: colliders, shapes, etc. pub use self::broad_phase_multi_sap::BroadPhase; +pub use self::capsule::Capsule; pub use self::collider::{Collider, ColliderBuilder, ColliderShape}; pub use self::collider_set::{ColliderHandle, ColliderSet}; pub use self::contact::{ @@ -22,8 +23,8 @@ pub use self::rounded::{Roundable, Rounded}; pub use self::trimesh::Trimesh; pub use ncollide::query::Proximity; -/// A capsule shape. -pub type Capsule = ncollide::shape::Capsule; +/// A segment shape. +pub type Segment = ncollide::shape::Segment; /// A cuboid shape. pub type Cuboid = ncollide::shape::Cuboid; /// A triangle shape. @@ -94,6 +95,7 @@ mod trimesh; mod waabb; mod wquadtree; //mod z_order; +mod capsule; #[cfg(feature = "dim3")] mod polygonal_feature_map; mod rounded; diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs index 2586826..2a8fc8d 100644 --- a/src/geometry/polygonal_feature_map.rs +++ b/src/geometry/polygonal_feature_map.rs @@ -1,9 +1,8 @@ use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle}; +use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Segment, Triangle}; use crate::math::{Point, Vector}; use approx::AbsDiffEq; use na::{Unit, Vector2}; -use ncollide::shape::Segment; use ncollide::shape::SupportMap; /// Trait implemented by convex shapes with features with polyhedral approximations. @@ -11,7 +10,7 @@ pub trait PolygonalFeatureMap: SupportMap { fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace); } -impl PolygonalFeatureMap for Segment { +impl PolygonalFeatureMap for Segment { fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { *out_feature = PolyhedronFace::from(*self); } diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 5a86ef2..83088fe 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -40,6 +40,8 @@ impl From for PolyhedronFace { impl From> for PolyhedronFace { fn from(seg: Segment) -> Self { + // Vertices have feature ids 0 and 2. + // The segment interior has feature id 1. Self { vertices: [seg.a, seg.b, seg.b, seg.b], vids: [0, 2, 2, 2], diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index 80c9b51..8f7997e 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,5 +1,7 @@ use crate::dynamics::MassProperties; -use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Roundable, Rounded, Triangle, Trimesh}; +use crate::geometry::{ + Ball, Capsule, Cuboid, HeightField, Roundable, Rounded, Segment, Triangle, Trimesh, +}; use crate::math::Isometry; use downcast_rs::{impl_downcast, DowncastSync}; use erased_serde::Serialize; @@ -24,6 +26,8 @@ pub enum ShapeType { Cuboid, /// A capsule shape. Capsule, + /// A segment shape. + Segment, /// A triangle shape. Triangle, /// A triangle mesh shape. @@ -205,16 +209,20 @@ impl Shape for Capsule { } fn compute_aabb(&self, position: &Isometry) -> AABB { - self.bounding_volume(position) + self.aabb(position) } fn mass_properties(&self, density: f32) -> MassProperties { - MassProperties::from_capsule(density, self.half_height, self.radius) + MassProperties::from_capsule(density, self.segment.a, self.segment.b, self.radius) } fn shape_type(&self) -> ShapeType { ShapeType::Capsule } + + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((&self.segment as &dyn PolygonalFeatureMap, self.radius)) + } } impl Shape for Triangle { @@ -241,6 +249,30 @@ impl Shape for Triangle { } } +impl Shape for Segment { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Segment + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) + } +} + impl Shape for Trimesh { #[cfg(feature = "serde-serialize")] fn as_serialize(&self) -> Option<&dyn Serialize> { diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index b449c44..a2b0331 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -184,7 +184,8 @@ fn nphysics_collider_from_rapier_collider( } else if let Some(ball) = shape.as_ball() { ShapeHandle::new(Ball::new(ball.radius - margin)) } else if let Some(capsule) = shape.as_capsule() { - ShapeHandle::new(Capsule::new(capsule.half_height, capsule.radius)) + pos *= capsule.transform_wrt_y(); + ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius)) } else if let Some(heightfield) = shape.as_heightfield() { ShapeHandle::new(heightfield.clone()) } else { diff --git a/src_testbed/objects/capsule.rs b/src_testbed/objects/capsule.rs index f285b81..23160be 100644 --- a/src_testbed/objects/capsule.rs +++ b/src_testbed/objects/capsule.rs @@ -19,7 +19,7 @@ impl Capsule { window: &mut window::Window, ) -> Capsule { let r = capsule.radius; - let h = capsule.half_height * 2.0; + let h = capsule.half_height() * 2.0; #[cfg(feature = "dim2")] let node = window.add_planar_capsule(r, h); #[cfg(feature = "dim3")] diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 822ad08..7db3c4c 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -433,8 +433,15 @@ fn physx_collider_from_rapier_collider( } else if let Some(ball) = shape.as_ball() { ColliderDesc::Sphere(ball.radius) } else if let Some(capsule) = shape.as_capsule() { - let rot = UnitQuaternion::rotation_between(&Vector3::x(), &Vector3::y()); - local_pose *= rot.unwrap_or(UnitQuaternion::identity()); + let center = capsule.center(); + let mut dir = capsule.segment.b - capsule.segment.a; + + if dir.x < 0.0 { + dir = -dir; + } + + let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir); + local_pose *= Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity()); ColliderDesc::Capsule(capsule.radius, capsule.height()) } else if let Some(trimesh) = shape.as_trimesh() { ColliderDesc::TriMesh { From 12e85e0dc26bc0c6ebc00c52549fdacd4752c9fd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 16:12:32 +0100 Subject: [PATCH 17/41] AddAdd missing feature-gate. --- src/geometry/shape.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index 8f7997e..3f1ca04 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -4,6 +4,7 @@ use crate::geometry::{ }; use crate::math::Isometry; use downcast_rs::{impl_downcast, DowncastSync}; +#[cfg(feature = "serde-serialize")] use erased_serde::Serialize; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; use ncollide::query::{PointQuery, RayCast}; From 6f3f84df622ea0b753e5529bc34c0759bbe1f4a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 16:13:43 +0100 Subject: [PATCH 18/41] Adjust the benchmark message argument. --- .github/workflows/rapier-ci-bench.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/rapier-ci-bench.yml b/.github/workflows/rapier-ci-bench.yml index 8a43818..ade6a67 100644 --- a/.github/workflows/rapier-ci-bench.yml +++ b/.github/workflows/rapier-ci-bench.yml @@ -18,17 +18,17 @@ jobs: BENCHBOT_TARGET_COMMIT: ${{ github.event.pull_request.head.sha }} BENCHBOT_SHA: ${{ github.sha }} BENCHBOT_HEAD_REF: ${{github.head_ref}} - BENCHBOT_OTHER_BACKENDS: 'true' + BENCHBOT_OTHER_BACKENDS: 0 runs-on: ubuntu-latest steps: - name: Find commit SHA if: github.ref == 'refs/heads/master' run: | echo "::set-env name=BENCHBOT_TARGET_COMMIT::$BENCHBOT_SHA" - echo "::set-env name=BENCHBOT_OTHER_BACKENDS::false" + echo "::set-env name=BENCHBOT_OTHER_BACKENDS::1" - name: Send 3D bench message shell: bash run: curl -u $BENCHBOT_AMQP_USER:$BENCHBOT_AMQP_PASS -i -H "content-type:application/json" -X POST https://$BENCHBOT_AMQP_HOST/api/exchanges/$BENCHBOT_AMQP_VHOST//publish - -d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\", \"other_backends\":\"'$BENCHBOT_OTHER_BACKENDS'\" }","payload_encoding":"string"}' \ No newline at end of file + -d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\", \"other_backends\":'$BENCHBOT_OTHER_BACKENDS' }","payload_encoding":"string"}' \ No newline at end of file From d725aa4557b4c26343ba57b294d1ae5b337703e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 16:15:22 +0100 Subject: [PATCH 19/41] Adjust the benchmark message argument, again. --- .github/workflows/rapier-ci-bench.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/rapier-ci-bench.yml b/.github/workflows/rapier-ci-bench.yml index ade6a67..6c4f007 100644 --- a/.github/workflows/rapier-ci-bench.yml +++ b/.github/workflows/rapier-ci-bench.yml @@ -18,14 +18,14 @@ jobs: BENCHBOT_TARGET_COMMIT: ${{ github.event.pull_request.head.sha }} BENCHBOT_SHA: ${{ github.sha }} BENCHBOT_HEAD_REF: ${{github.head_ref}} - BENCHBOT_OTHER_BACKENDS: 0 + BENCHBOT_OTHER_BACKENDS: false runs-on: ubuntu-latest steps: - name: Find commit SHA if: github.ref == 'refs/heads/master' run: | echo "::set-env name=BENCHBOT_TARGET_COMMIT::$BENCHBOT_SHA" - echo "::set-env name=BENCHBOT_OTHER_BACKENDS::1" + echo "::set-env name=BENCHBOT_OTHER_BACKENDS::true" - name: Send 3D bench message shell: bash run: curl -u $BENCHBOT_AMQP_USER:$BENCHBOT_AMQP_PASS From 3fd4a62a2e4d6f5d97412bf80561c23ba43e40a5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 16:18:44 +0100 Subject: [PATCH 20/41] Fix 2D compilation. --- .../capsule_capsule_contact_generator.rs | 4 ++-- .../heightfield_shape_contact_generator.rs | 18 ++++-------------- src/geometry/shape.rs | 1 + 3 files changed, 7 insertions(+), 16 deletions(-) diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index ef18ad2..3104496 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -39,8 +39,8 @@ pub fn generate_contacts<'a>( let pos12 = pos1.inverse() * pos2; let pos21 = pos12.inverse(); - let seg1 = capsule1.segment(); - let seg2_1 = capsule2.segment().transformed(&pos12); + let seg1 = capsule1.segment; + let seg2_1 = capsule2.segment.transformed(&pos12); let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( (&seg1.a, &seg1.b), (&seg2_1.a, &seg2_1.b), diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index e66bbac..d48eb1d 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -109,12 +109,9 @@ fn do_generate_contacts( let shape_type2 = collider2.shape().shape_type(); heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| { - let position1 = *collider1.position(); + let position1 = collider1.position(); #[cfg(feature = "dim2")] - let (position1, dpos1, sub_shape1) = { - let (dpos, half_height) = crate::utils::segment_to_capsule(&part1.a, &part1.b); - (position1 * dpos, dpos, Capsule::new(half_height, 0.0)) - }; + let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); #[cfg(feature = "dim3")] let sub_shape1 = *part1; @@ -154,7 +151,7 @@ fn do_generate_contacts( shape1: collider2.shape(), shape2: &sub_shape1, position1: collider2.position(), - position2: &position1, + position2: position1, manifold, workspace: sub_detector.workspace.as_deref_mut(), } @@ -165,20 +162,13 @@ fn do_generate_contacts( collider2, shape1: &sub_shape1, shape2: collider2.shape(), - position1: &position1, + position1: position1, position2: collider2.position(), manifold, workspace: sub_detector.workspace.as_deref_mut(), } }; - #[cfg(feature = "dim2")] - if coll_pair.collider1 != ctxt2.manifold.pair.collider1 { - ctxt2.manifold.delta2 = collider1.position_wrt_parent() * dpos1; - } else { - ctxt2.manifold.delta1 = collider1.position_wrt_parent() * dpos1; - } - (sub_detector.generator.generate_contacts)(&mut ctxt2) }); diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index 3f1ca04..54a156b 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -221,6 +221,7 @@ impl Shape for Capsule { ShapeType::Capsule } + #[cfg(feature = "dim3")] fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { Some((&self.segment as &dyn PolygonalFeatureMap, self.radius)) } From ba6655be8e4b781bfaac06282e1c7d53a56111f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 16:24:58 +0100 Subject: [PATCH 21/41] Fix build when serialization is not enabled. --- .../contact_generator/heightfield_shape_contact_generator.rs | 2 +- src/geometry/shape.rs | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index d48eb1d..81752e4 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -162,7 +162,7 @@ fn do_generate_contacts( collider2, shape1: &sub_shape1, shape2: collider2.shape(), - position1: position1, + position1, position2: collider2.position(), manifold, workspace: sub_detector.workspace.as_deref_mut(), diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index 54a156b..8f40fd4 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -365,6 +365,7 @@ impl Shape for Cone { #[cfg(feature = "dim3")] impl Shape for Rounded { + #[cfg(feature = "serde-serialize")] fn as_serialize(&self) -> Option<&dyn Serialize> { Some(self as &dyn Serialize) } From 08930b1238c90bec16db84c50ac4ea7c9a1e0a5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Mon, 26 Oct 2020 16:36:07 +0100 Subject: [PATCH 22/41] Fix multiple warnings. --- examples3d/debug_cylinder3.rs | 4 +-- examples3d/primitives3.rs | 2 +- src/dynamics/mass_properties_capsule.rs | 4 +-- src/dynamics/mass_properties_cone.rs | 1 - src/dynamics/mass_properties_cylinder.rs | 7 ++--- src/geometry/collider.rs | 2 +- .../heightfield_shape_contact_generator.rs | 4 +-- .../pfm_pfm_contact_generator.rs | 7 +---- .../polygon_polygon_contact_generator.rs | 2 ++ src/geometry/polygon.rs | 2 ++ .../polygon_polygon_proximity_detector.rs | 2 ++ src/geometry/sat.rs | 1 + src/utils.rs | 26 ------------------- src_testbed/box2d_backend.rs | 2 +- src_testbed/nphysics_backend.rs | 2 +- src_testbed/physx_backend.rs | 2 +- src_testbed/testbed.rs | 4 ++- 17 files changed, 23 insertions(+), 51 deletions(-) diff --git a/examples3d/debug_cylinder3.rs b/examples3d/debug_cylinder3.rs index 4e7fae1..0717c67 100644 --- a/examples3d/debug_cylinder3.rs +++ b/examples3d/debug_cylinder3.rs @@ -1,4 +1,4 @@ -use na::{Point3, Vector3}; +use na::Point3; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -40,7 +40,7 @@ pub fn init_world(testbed: &mut Testbed) { 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 offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; let x = -centerx + offset; let y = centery + 3.0; diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index 6a98077..db15341 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -1,4 +1,4 @@ -use na::{Point3, Vector3}; +use na::Point3; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index dae6cfb..3b1b214 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -1,7 +1,7 @@ use crate::dynamics::MassProperties; -use crate::math::Point; #[cfg(feature = "dim3")] -use crate::{geometry::Capsule, math::Rotation}; +use crate::geometry::Capsule; +use crate::math::Point; impl MassProperties { pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs index 0fb61b6..12f831f 100644 --- a/src/dynamics/mass_properties_cone.rs +++ b/src/dynamics/mass_properties_cone.rs @@ -1,5 +1,4 @@ use crate::dynamics::MassProperties; -use crate::geometry::Cone; use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; impl MassProperties { diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs index 8d4f254..7c8054a 100644 --- a/src/dynamics/mass_properties_cylinder.rs +++ b/src/dynamics/mass_properties_cylinder.rs @@ -1,10 +1,7 @@ use crate::dynamics::MassProperties; -use crate::math::{PrincipalAngularInertia, Vector}; #[cfg(feature = "dim3")] -use { - crate::geometry::Capsule, - crate::math::{Point, Rotation}, -}; +use crate::math::{Point, Rotation}; +use crate::math::{PrincipalAngularInertia, Vector}; impl MassProperties { pub(crate) fn cylinder_y_volume_unit_inertia( diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index ae5cc73..d4f685c 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -4,7 +4,7 @@ use crate::geometry::{ Segment, Shape, ShapeType, Triangle, Trimesh, }; #[cfg(feature = "dim3")] -use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, Rounded}; +use crate::geometry::{Cone, Cylinder, Rounded}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; use ncollide::bounding_volume::AABB; diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 81752e4..9224d4e 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -5,8 +5,6 @@ use crate::geometry::contact_generator::{ use crate::geometry::Capsule; use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType}; use crate::ncollide::bounding_volume::BoundingVolume; -#[cfg(feature = "dim3")] -use crate::{geometry::Triangle, math::Point}; use std::any::Any; use std::collections::hash_map::Entry; use std::collections::HashMap; @@ -111,7 +109,7 @@ fn do_generate_contacts( 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); + let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead. #[cfg(feature = "dim3")] let sub_shape1 = *part1; diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index 37f8629..62a38ae 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -1,9 +1,6 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{ - Contact, ContactManifold, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace, -}; +use crate::geometry::{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}; @@ -11,7 +8,6 @@ use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex}; pub struct PfmPfmContactManifoldGeneratorWorkspace { simplex: VoronoiSimplex, last_gjk_dir: Option>>, - last_optimal_dir: Option>>, feature1: PolyhedronFace, feature2: PolyhedronFace, } @@ -21,7 +17,6 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace { Self { simplex: VoronoiSimplex::new(), last_gjk_dir: None, - last_optimal_dir: None, feature1: PolyhedronFace::new(), feature2: PolyhedronFace::new(), } diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index e1e6e7e..0e7543d 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -1,3 +1,5 @@ +#![allow(dead_code)] // TODO: remove this once we support polygons. + use crate::geometry::contact_generator::PrimitiveContactGenerationContext; use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon}; use crate::math::{Isometry, Point}; diff --git a/src/geometry/polygon.rs b/src/geometry/polygon.rs index cdb1012..d07cfd9 100644 --- a/src/geometry/polygon.rs +++ b/src/geometry/polygon.rs @@ -1,3 +1,5 @@ +#![allow(dead_code)] // TODO: remove this once we support polygons. + use crate::math::{Isometry, Point, Vector}; use ncollide::bounding_volume::AABB; diff --git a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs index 30a02fa..12a8e45 100644 --- a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs +++ b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs @@ -1,3 +1,5 @@ +#![allow(dead_code)] + use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; use crate::geometry::{sat, Polygon, Proximity}; use crate::math::Isometry; diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index e2548dd..452b380 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -4,6 +4,7 @@ use crate::utils::WSign; use na::Unit; use ncollide::shape::{Segment, SupportMap}; +#[allow(dead_code)] pub fn polygon_polygon_compute_separation_features( p1: &Polygon, p2: &Polygon, diff --git a/src/utils.rs b/src/utils.rs index 0bc9e17..a398a02 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,7 +1,6 @@ //! Miscellaneous utilities. use crate::dynamics::RigidBodyHandle; -use crate::math::{Isometry, Point, Rotation, Vector}; #[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))] use indexmap::IndexMap as HashMap; use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; @@ -1334,28 +1333,3 @@ pub(crate) fn other_handle( pair.0 } } - -/// Returns the rotation that aligns the y axis to the segment direction. -pub(crate) fn rotation_wrt_y(a: &Point, b: &Point) -> Rotation { - let mut dir = b - a; - - if dir.y < 0.0 { - dir = -dir; - } - - #[cfg(feature = "dim2")] - return Rotation::rotation_between(&Vector::y(), &dir); - - #[cfg(feature = "dim3")] - return Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()); -} - -// Return the transform that aligns the y axis to the segment and move the origin to the segment middle, -// and the capsule's half-height. -pub(crate) fn segment_to_capsule(a: &Point, b: &Point) -> (Isometry, f32) { - let rot = rotation_wrt_y(a, b); - let half_height = (b - a).norm() / 2.0; - let center = na::center(a, b); - let pos = Isometry::from_parts(center.coords.into(), rot); - (pos, half_height) -} diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index 9156c2a..769b12d 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -5,7 +5,7 @@ use rapier::counters::Counters; use rapier::dynamics::{ IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{Collider, ColliderSet, Shape}; +use rapier::geometry::{Collider, ColliderSet}; use std::f32; use wrapped2d::b2; diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index a2b0331..3a0e487 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -12,7 +12,7 @@ use rapier::counters::Counters; use rapier::dynamics::{ IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{Collider, ColliderSet, Shape}; +use rapier::geometry::{Collider, ColliderSet}; use rapier::math::Vector; use std::collections::HashMap; #[cfg(feature = "dim3")] diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 7db3c4c..74d6af2 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -6,7 +6,7 @@ use rapier::counters::Counters; use rapier::dynamics::{ IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{Collider, ColliderSet, Shape}; +use rapier::geometry::{Collider, ColliderSet}; use rapier::utils::WBasis; use std::collections::HashMap; diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 3d7fd7d..2b733a3 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -21,7 +21,9 @@ use na::{self, Point2, Point3, Vector3}; use rapier::dynamics::{ ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, Ray}; +#[cfg(feature = "dim3")] +use rapier::geometry::Ray; +use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent}; use rapier::math::Vector; use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline}; #[cfg(feature = "fluids")] From 6c3c60a9deea5a1a8bf0bceba252679fe01a0c10 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 09:07:56 +0100 Subject: [PATCH 23/41] Re-export crossbeam. --- src/lib.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib.rs b/src/lib.rs index 3674717..deb9313 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -10,6 +10,7 @@ #![deny(missing_docs)] +pub extern crate crossbeam; pub extern crate nalgebra as na; #[cfg(feature = "dim2")] pub extern crate ncollide2d as ncollide; From df8e2fb9feb0d9dee0c92d117786ab74cf35ef36 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 09:08:06 +0100 Subject: [PATCH 24/41] Variable renaming. --- src/geometry/polyhedron_feature3d.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs index 83088fe..e88674e 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -64,8 +64,8 @@ impl PolyhedronFace { } pub fn transform_by(&mut self, iso: &Isometry) { - for v in &mut self.vertices[0..self.num_vertices] { - *v = iso * *v; + for p in &mut self.vertices[0..self.num_vertices] { + *p = iso * *p; } } From dbdd797d5934ee76b0358b6cf845575ce0ef29af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 09:08:23 +0100 Subject: [PATCH 25/41] Add some segment/triangle SAT functions. --- src/geometry/sat.rs | 82 ++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 78 insertions(+), 4 deletions(-) diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 452b380..44446af 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -1,8 +1,20 @@ -use crate::geometry::{cuboid, Cuboid, Polygon, Triangle}; +use crate::geometry::{cuboid, Cuboid, Polygon, Segment, Triangle}; use crate::math::{Isometry, Point, Vector, DIM}; use crate::utils::WSign; use na::Unit; -use ncollide::shape::{Segment, SupportMap}; +use ncollide::shape::SupportMap; + +#[allow(dead_code)] +pub fn support_map_support_map_compute_separation( + sm1: &impl SupportMap, + sm2: &impl SupportMap, + m12: &Isometry, + dir1: &Unit>, +) -> f32 { + let p1 = sm1.local_support_point_toward(dir1); + let p2 = sm2.support_point_toward(m12, &-*dir1); + (p2 - p1).dot(dir1) +} #[allow(dead_code)] pub fn polygon_polygon_compute_separation_features( @@ -128,7 +140,6 @@ pub fn cuboid_cuboid_find_local_separating_normal_oneway( * * */ - #[cfg(feature = "dim3")] pub fn cube_support_map_compute_separation_wrt_local_line>( cube1: &Cuboid, @@ -207,7 +218,7 @@ pub fn cube_triangle_find_local_separating_edge_twoway( #[cfg(feature = "dim3")] pub fn cube_segment_find_local_separating_edge_twoway( cube1: &Cuboid, - segment2: &Segment, + segment2: &Segment, pos12: &Isometry, pos21: &Isometry, ) -> (f32, Vector) { @@ -293,3 +304,66 @@ pub fn segment_cuboid_find_local_separating_normal_oneway( ) -> (f32, Vector) { point_cuboid_find_local_separating_normal_oneway(segment1.a, segment1.normal(), shape2, pos12) } + +/* + * Capsules + */ +#[cfg(feature = "dim3")] +pub fn triangle_segment_find_local_separating_normal_oneway( + triangle1: &Triangle, + segment2: &Segment, + m12: &Isometry, +) -> (f32, Vector) { + if let Some(dir) = triangle1.normal() { + let p2a = segment2.support_point_toward(m12, &-dir); + let p2b = segment2.support_point_toward(m12, &dir); + let sep_a = (p2a - triangle1.a).dot(&dir); + let sep_b = -(p2b - triangle1.a).dot(&dir); + + if sep_a >= sep_b { + (sep_a, *dir) + } else { + (sep_b, -*dir) + } + } else { + (-f32::MAX, Vector::zeros()) + } +} + +#[cfg(feature = "dim3")] +pub fn segment_triangle_find_local_separating_edge( + segment1: &Segment, + triangle2: &Triangle, + pos12: &Isometry, +) -> (f32, Vector) { + let x2 = pos12 * (triangle2.b - triangle2.a); + let y2 = pos12 * (triangle2.c - triangle2.b); + let z2 = pos12 * (triangle2.a - triangle2.c); + let dir1 = segment1.scaled_direction(); + + let crosses1 = [dir1.cross(&x2), dir1.cross(&y2), dir1.cross(&z2)]; + let axes1 = [ + crosses1[0], + crosses1[1], + crosses1[2], + -crosses1[0], + -crosses1[1], + -crosses1[2], + ]; + let mut max_separation = -f32::MAX; + let mut sep_dir = axes1[0]; + + for axis1 in &axes1 { + if let Some(axis1) = Unit::try_new(*axis1, 0.0) { + let sep = + support_map_support_map_compute_separation(segment1, triangle2, pos12, &axis1); + + if sep > max_separation { + max_separation = sep; + sep_dir = *axis1; + } + } + } + + (max_separation, sep_dir) +} From 8c872dc0af9ece127a87b4adb112b0e1ed6ab249 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 09:20:40 +0100 Subject: [PATCH 26/41] Replace the Rounded type by a non-generic RoundCylinder type. --- src/geometry/collider.rs | 17 +++---- .../pfm_pfm_contact_generator.rs | 18 ++++---- src/geometry/mod.rs | 4 +- .../{rounded.rs => round_cylinder.rs} | 46 ++++++++----------- src/geometry/shape.rs | 22 +++++---- src_testbed/engine.rs | 5 +- 6 files changed, 54 insertions(+), 58 deletions(-) rename src/geometry/{rounded.rs => round_cylinder.rs} (71%) diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index d4f685c..522f002 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -4,7 +4,7 @@ use crate::geometry::{ Segment, Shape, ShapeType, Triangle, Trimesh, }; #[cfg(feature = "dim3")] -use crate::geometry::{Cone, Cylinder, Rounded}; +use crate::geometry::{Cone, Cylinder, RoundCylinder}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; use ncollide::bounding_volume::AABB; @@ -39,10 +39,11 @@ impl ColliderShape { /// (along along the y axis), its radius, and its roundedness (the /// radius of the sphere used for dilating the cylinder). #[cfg(feature = "dim3")] - pub fn round_cylinder(half_height: f32, radius: f32, round_radius: f32) -> Self { - ColliderShape(Arc::new(Rounded::new( - Cylinder::new(half_height, radius), - round_radius, + pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self { + ColliderShape(Arc::new(RoundCylinder::new( + half_height, + radius, + border_radius, ))) } @@ -170,7 +171,7 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { #[cfg(feature = "dim3")] Some(ShapeType::Cone) => deser::(&mut seq)?, #[cfg(feature = "dim3")] - Some(ShapeType::RoundCylinder) => deser::>(&mut seq)?, + Some(ShapeType::RoundCylinder) => deser::(&mut seq)?, None => { return Err(serde::de::Error::custom( "found invalid shape type to deserialize", @@ -332,11 +333,11 @@ impl ColliderBuilder { /// (along along the y axis), its radius, and its roundedness (the /// radius of the sphere used for dilating the cylinder). #[cfg(feature = "dim3")] - pub fn round_cylinder(half_height: f32, radius: f32, round_radius: f32) -> Self { + pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self { Self::new(ColliderShape::round_cylinder( half_height, radius, - round_radius, + border_radius, )) } diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index 62a38ae..1dcae33 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -24,11 +24,11 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace { } pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Some((pfm1, round_radius1)), Some((pfm2, round_radius2))) = ( + 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, round_radius1, pfm2, round_radius2, ctxt); + do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt); ctxt.manifold.update_warmstart_multiplier(); ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -36,9 +36,9 @@ pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { fn do_generate_contacts( pfm1: &dyn PolygonalFeatureMap, - round_radius1: f32, + border_radius1: f32, pfm2: &dyn PolygonalFeatureMap, - round_radius2: f32, + border_radius2: f32, ctxt: &mut PrimitiveContactGenerationContext, ) { let pos12 = ctxt.position1.inverse() * ctxt.position2; @@ -61,7 +61,7 @@ fn do_generate_contacts( .downcast_mut() .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); - let total_prediction = ctxt.prediction_distance + round_radius1 + round_radius2; + let total_prediction = ctxt.prediction_distance + border_radius1 + border_radius2; let contact = query::contact_support_map_support_map_with_params( &Isometry::identity(), pfm1, @@ -93,11 +93,11 @@ fn do_generate_contacts( ctxt.manifold, ); - if round_radius1 != 0.0 || round_radius2 != 0.0 { + if border_radius1 != 0.0 || border_radius2 != 0.0 { for contact in &mut ctxt.manifold.points { - contact.local_p1 += *normal1 * round_radius1; - contact.local_p2 += *normal2 * round_radius2; - contact.dist -= round_radius1 + round_radius2; + contact.local_p1 += *normal1 * border_radius1; + contact.local_p2 += *normal2 * border_radius2; + contact.dist -= border_radius1 + border_radius2; } } diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index a3423dd..2d8fc76 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -19,7 +19,7 @@ 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::rounded::{Roundable, Rounded}; +pub use self::round_cylinder::RoundCylinder; pub use self::trimesh::Trimesh; pub use ncollide::query::Proximity; @@ -98,5 +98,5 @@ mod wquadtree; mod capsule; #[cfg(feature = "dim3")] mod polygonal_feature_map; -mod rounded; +mod round_cylinder; mod shape; diff --git a/src/geometry/rounded.rs b/src/geometry/round_cylinder.rs similarity index 71% rename from src/geometry/rounded.rs rename to src/geometry/round_cylinder.rs index bfbab3b..4e2afa7 100644 --- a/src/geometry/rounded.rs +++ b/src/geometry/round_cylinder.rs @@ -1,6 +1,5 @@ #[cfg(feature = "dim3")] use crate::geometry::Cylinder; -use crate::geometry::ShapeType; use crate::math::{Isometry, Point, Vector}; use na::Unit; use ncollide::query::{ @@ -8,44 +7,35 @@ use ncollide::query::{ }; use ncollide::shape::{FeatureId, SupportMap}; -/// A shape which corner can be rounded. -pub trait Roundable { - /// The ShapeType fo this shape after rounding its corners. - fn rounded_shape_type() -> ShapeType; -} - -#[cfg(feature = "dim3")] -impl Roundable for Cylinder { - fn rounded_shape_type() -> ShapeType { - ShapeType::RoundCylinder - } -} - -/// A rounded shape. +/// A rounded cylinder. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -pub struct Rounded { - /// The shape being rounded. - pub shape: S, +#[derive(Copy, Clone, Debug)] +pub struct RoundCylinder { + /// The cylinder being rounded. + pub cylinder: Cylinder, /// The rounding radius. - pub radius: f32, + pub border_radius: f32, } -impl Rounded { - /// Create sa new shape where all its edges and vertices are rounded by a radius of `radius`. +impl RoundCylinder { + /// Create sa new cylinder where all its edges and vertices are rounded by a radius of `radius`. /// - /// This is done by applying a dilation of the given radius to the shape. - pub fn new(shape: S, radius: f32) -> Self { - Self { shape, radius } + /// This is done by applying a dilation of the given radius to the cylinder. + pub fn new(half_height: f32, radius: f32, border_radius: f32) -> Self { + Self { + cylinder: Cylinder::new(half_height, radius), + border_radius, + } } } -impl> SupportMap for Rounded { +impl SupportMap for RoundCylinder { fn local_support_point(&self, dir: &Vector) -> Point { self.local_support_point_toward(&Unit::new_normalize(*dir)) } fn local_support_point_toward(&self, dir: &Unit>) -> Point { - self.shape.local_support_point_toward(dir) + **dir * self.radius + self.cylinder.local_support_point_toward(dir) + **dir * self.border_radius } fn support_point(&self, transform: &Isometry, dir: &Vector) -> Point { @@ -63,7 +53,7 @@ impl> SupportMap for Rounded { } } -impl> RayCast for Rounded { +impl RayCast for RoundCylinder { fn toi_and_normal_with_ray( &self, m: &Isometry, @@ -90,7 +80,7 @@ impl> RayCast for Rounded { // TODO: if PointQuery had a `project_point_with_normal` method, we could just // call this and adjust the projected point accordingly. -impl> PointQuery for Rounded { +impl PointQuery for RoundCylinder { #[inline] fn project_point( &self, diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index 8f40fd4..982e484 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,6 +1,6 @@ use crate::dynamics::MassProperties; use crate::geometry::{ - Ball, Capsule, Cuboid, HeightField, Roundable, Rounded, Segment, Triangle, Trimesh, + Ball, Capsule, Cuboid, HeightField, RoundCylinder, Segment, Triangle, Trimesh, }; use crate::math::Isometry; use downcast_rs::{impl_downcast, DowncastSync}; @@ -132,11 +132,8 @@ impl dyn Shape { } /// Converts this abstract shape to a cone, if it is one. - pub fn as_rounded(&self) -> Option<&Rounded> - where - S: Roundable, - Rounded: Shape, - { + #[cfg(feature = "dim3")] + pub fn as_round_cylinder(&self) -> Option<&RoundCylinder> { self.downcast_ref() } } @@ -364,19 +361,21 @@ impl Shape for Cone { } #[cfg(feature = "dim3")] -impl Shape for Rounded { +impl Shape for RoundCylinder { #[cfg(feature = "serde-serialize")] fn as_serialize(&self) -> Option<&dyn Serialize> { Some(self as &dyn Serialize) } fn compute_aabb(&self, position: &Isometry) -> AABB { - self.shape.compute_aabb(position).loosened(self.radius) + self.cylinder + .compute_aabb(position) + .loosened(self.border_radius) } fn mass_properties(&self, density: f32) -> MassProperties { // We ignore the margin here. - self.shape.mass_properties(density) + self.cylinder.mass_properties(density) } fn shape_type(&self) -> ShapeType { @@ -385,6 +384,9 @@ impl Shape for Rounded { #[cfg(feature = "dim3")] fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((&self.shape as &dyn PolygonalFeatureMap, self.radius)) + Some(( + &self.cylinder as &dyn PolygonalFeatureMap, + self.border_radius, + )) } } diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index d6edea5..b02f146 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -412,7 +412,10 @@ impl GraphicsManager { } #[cfg(feature = "dim3")] - if let Some(cylinder) = shape.as_cylinder().or(shape.as_rounded().map(|r| &r.shape)) { + if let Some(cylinder) = shape + .as_cylinder() + .or(shape.as_round_cylinder().map(|r| &r.cylinder)) + { out.push(Node::Cylinder(Cylinder::new( handle, cylinder.half_height, From ffbc3c02c7d328d5c48a3efb84d35f5911f1880b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 09:25:58 +0100 Subject: [PATCH 27/41] Fix 2D compilation. --- src/geometry/mod.rs | 2 ++ src/geometry/round_cylinder.rs | 1 - src/geometry/sat.rs | 2 +- src/geometry/shape.rs | 6 ++---- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 2d8fc76..abb9844 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -19,6 +19,7 @@ pub use self::narrow_phase::NarrowPhase; pub use self::polygon::Polygon; pub use self::proximity::ProximityPair; pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; +#[cfg(feature = "dim3")] pub use self::round_cylinder::RoundCylinder; pub use self::trimesh::Trimesh; pub use ncollide::query::Proximity; @@ -98,5 +99,6 @@ mod wquadtree; mod capsule; #[cfg(feature = "dim3")] mod polygonal_feature_map; +#[cfg(feature = "dim3")] mod round_cylinder; mod shape; diff --git a/src/geometry/round_cylinder.rs b/src/geometry/round_cylinder.rs index 4e2afa7..ce8b43b 100644 --- a/src/geometry/round_cylinder.rs +++ b/src/geometry/round_cylinder.rs @@ -1,4 +1,3 @@ -#[cfg(feature = "dim3")] use crate::geometry::Cylinder; use crate::math::{Isometry, Point, Vector}; use na::Unit; diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 44446af..3cd66b4 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -298,7 +298,7 @@ pub fn triangle_cuboid_find_local_separating_normal_oneway( #[cfg(feature = "dim2")] pub fn segment_cuboid_find_local_separating_normal_oneway( - segment1: &Segment, + segment1: &Segment, shape2: &Cuboid, pos12: &Isometry, ) -> (f32, Vector) { diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index 982e484..ec43bf7 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,7 +1,5 @@ use crate::dynamics::MassProperties; -use crate::geometry::{ - Ball, Capsule, Cuboid, HeightField, RoundCylinder, Segment, Triangle, Trimesh, -}; +use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh}; use crate::math::Isometry; use downcast_rs::{impl_downcast, DowncastSync}; #[cfg(feature = "serde-serialize")] @@ -12,7 +10,7 @@ use num::Zero; use num_derive::FromPrimitive; #[cfg(feature = "dim3")] use { - crate::geometry::{Cone, Cylinder, PolygonalFeatureMap}, + crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, RoundCylinder}, ncollide::bounding_volume::BoundingVolume, }; From b5a1aaa4e5267da0f8b5b5fabb61d39cdb0e7a4e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 10:36:49 +0100 Subject: [PATCH 28/41] Add a 128-bits user-data attached to colliders and rigid-bodies. --- src/dynamics/rigid_body.rs | 12 ++++++++++++ src/geometry/collider.rs | 13 ++++++++++++- 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 9fa5a8e..f4d47ce 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -54,6 +54,8 @@ pub struct RigidBody { pub(crate) active_set_timestamp: u32, /// The status of the body, governing how it is affected by external forces. pub body_status: BodyStatus, + /// User-defined associated to this rigid-body. + pub user_data: u128, } impl Clone for RigidBody { @@ -90,6 +92,7 @@ impl RigidBody { active_set_offset: 0, active_set_timestamp: 0, body_status: BodyStatus::Dynamic, + user_data: 0, } } @@ -342,6 +345,7 @@ pub struct RigidBodyBuilder { angvel: AngVector, body_status: BodyStatus, can_sleep: bool, + user_data: u128, } impl RigidBodyBuilder { @@ -353,6 +357,7 @@ impl RigidBodyBuilder { angvel: na::zero(), body_status, can_sleep: true, + user_data: 0, } } @@ -400,6 +405,12 @@ impl RigidBodyBuilder { self } + /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder. + pub fn user_data(mut self, data: u128) -> Self { + self.user_data = data; + self + } + /// Sets the initial linear velocity of the rigid-body to be created. #[cfg(feature = "dim2")] pub fn linvel(mut self, x: f32, y: f32) -> Self { @@ -434,6 +445,7 @@ impl RigidBodyBuilder { rb.linvel = self.linvel; rb.angvel = self.angvel; rb.body_status = self.body_status; + rb.user_data = self.user_data; if !self.can_sleep { rb.activation.threshold = -1.0; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 522f002..ad4f813 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -206,6 +206,7 @@ pub struct Collider { pub(crate) contact_graph_index: ColliderGraphIndex, pub(crate) proximity_graph_index: ColliderGraphIndex, pub(crate) proxy_index: usize, + pub user_data: u128, } impl Clone for Collider { @@ -296,6 +297,8 @@ pub struct ColliderBuilder { pub delta: Isometry, /// Is this collider a sensor? pub is_sensor: bool, + /// The user-data of the collider beind built. + pub user_data: u128, } impl ColliderBuilder { @@ -308,6 +311,7 @@ impl ColliderBuilder { restitution: 0.0, delta: Isometry::identity(), is_sensor: false, + user_data: 0, } } @@ -413,6 +417,12 @@ impl ColliderBuilder { 0.5 } + /// An arbitrary user-defined 128-bit integer associated to the colliders built by this builder. + pub fn user_data(mut self, data: u128) -> Self { + self.user_data = data; + self + } + /// Sets whether or not the collider built by this builder is a sensor. pub fn sensor(mut self, is_sensor: bool) -> Self { self.is_sensor = is_sensor; @@ -477,7 +487,7 @@ impl ColliderBuilder { self } - /// Buildes a new collider attached to the given rigid-body. + /// Builds a new collider attached to the given rigid-body. pub fn build(&self) -> Collider { let density = self.get_density(); @@ -494,6 +504,7 @@ impl ColliderBuilder { contact_graph_index: InteractionGraph::::invalid_graph_index(), proximity_graph_index: InteractionGraph::::invalid_graph_index(), proxy_index: crate::INVALID_USIZE, + user_data: self.user_data, } } } From 49779e6c757285e91cf5b5daf123f00959408521 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 10:54:07 +0100 Subject: [PATCH 29/41] Add missing comment. --- src/dynamics/rigid_body.rs | 2 +- src/geometry/collider.rs | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index f4d47ce..417ce34 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -54,7 +54,7 @@ pub struct RigidBody { pub(crate) active_set_timestamp: u32, /// The status of the body, governing how it is affected by external forces. pub body_status: BodyStatus, - /// User-defined associated to this rigid-body. + /// User-defined data associated to this rigid-body. pub user_data: u128, } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index ad4f813..40b59ae 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -206,6 +206,7 @@ pub struct Collider { pub(crate) contact_graph_index: ColliderGraphIndex, pub(crate) proximity_graph_index: ColliderGraphIndex, pub(crate) proxy_index: usize, + /// User-defined data associated to this rigid-body. pub user_data: u128, } From 3def91d62eba6ca2486fdaa386f78d82923c705a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 12:08:46 +0100 Subject: [PATCH 30/41] Add collision groups to filter collision pairs. --- src/geometry/collider.rs | 24 ++++- src/geometry/interaction_groups.rs | 54 ++++++++++++ src/geometry/mod.rs | 2 + src/geometry/narrow_phase.rs | 136 +++-------------------------- 4 files changed, 87 insertions(+), 129 deletions(-) create mode 100644 src/geometry/interaction_groups.rs diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 40b59ae..7147e95 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,7 +1,7 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Proximity, - Segment, Shape, ShapeType, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, + InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh, }; #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, RoundCylinder}; @@ -203,6 +203,7 @@ pub struct Collider { pub friction: f32, /// The restitution coefficient of this collider. pub restitution: f32, + pub(crate) collision_groups: InteractionGroups, pub(crate) contact_graph_index: ColliderGraphIndex, pub(crate) proximity_graph_index: ColliderGraphIndex, pub(crate) proxy_index: usize, @@ -255,6 +256,11 @@ impl Collider { &self.delta } + /// The collision groups used by this collider. + pub fn collision_groups(&self) -> InteractionGroups { + self.collision_groups + } + /// The density of this collider. pub fn density(&self) -> f32 { self.density @@ -300,6 +306,8 @@ pub struct ColliderBuilder { pub is_sensor: bool, /// The user-data of the collider beind built. pub user_data: u128, + /// The collision groups for the collider being built. + pub collision_groups: InteractionGroups, } impl ColliderBuilder { @@ -313,6 +321,7 @@ impl ColliderBuilder { delta: Isometry::identity(), is_sensor: false, user_data: 0, + collision_groups: InteractionGroups::all(), } } @@ -418,12 +427,21 @@ impl ColliderBuilder { 0.5 } - /// An arbitrary user-defined 128-bit integer associated to the colliders built by this builder. + /// Sets an arbitrary user-defined 128-bit integer associated to the colliders built by this builder. pub fn user_data(mut self, data: u128) -> Self { self.user_data = data; self } + /// Sets the collision groups used by this collider. + /// + /// Two colliders will interact iff. their collision groups are compatible. + /// See [InteractionGroups::test] for details. + pub fn collision_groups(mut self, groups: InteractionGroups) -> Self { + self.collision_groups = groups; + self + } + /// Sets whether or not the collider built by this builder is a sensor. pub fn sensor(mut self, is_sensor: bool) -> Self { self.is_sensor = is_sensor; diff --git a/src/geometry/interaction_groups.rs b/src/geometry/interaction_groups.rs new file mode 100644 index 0000000..9596345 --- /dev/null +++ b/src/geometry/interaction_groups.rs @@ -0,0 +1,54 @@ +#[repr(transparent)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)] +/// Pairwise filtering using bit masks. +/// +/// This filtering method is based on two 16-bit values: +/// - The interaction groups (the 16 left-most bits of `self.0`). +/// - The interaction mask (the 16 right-most bits of `self.0`). +/// +/// An interaction is allowed between two filters `a` and `b` two conditions +/// are met simultaneously: +/// - The interaction groups of `a` has at least one bit set to `1` in common with the interaction mask of `b`. +/// - The interaction groups of `b` has at least one bit set to `1` in common with the interaction mask of `a`. +/// In other words, interactions are allowed between two filter iff. the following condition is met: +/// ```rust +/// ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0 +/// ``` +pub struct InteractionGroups(pub u32); + +impl InteractionGroups { + /// Allow interaction with everything. + pub fn all() -> Self { + Self(u32::MAX) + } + + /// Prevent all interactions. + pub fn none() -> Self { + Self(0) + } + + /// Sets the group this filter is part of. + pub fn with_groups(self, groups: u16) -> Self { + Self((self.0 & 0x0000ffff) | ((groups as u32) << 16)) + } + + /// Sets the interaction mask of this filter. + pub fn with_mask(self, mask: u16) -> Self { + Self((self.0 & 0xffff0000) | (mask as u32)) + } + + /// Check if interactions should be allowed based on the interaction groups and mask. + /// + /// An interaction is allowed iff. the groups of `self` contain at least one bit set to 1 in common + /// with the mask of `rhs`, and vice-versa. + pub fn test(self, rhs: Self) -> bool { + ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0 + } +} + +impl Default for InteractionGroups { + fn default() -> Self { + Self::all() + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index abb9844..4055d43 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -70,6 +70,7 @@ pub(crate) use self::polyhedron_feature3d::PolyhedronFace; pub(crate) use self::waabb::{WRay, WAABB}; pub(crate) use self::wquadtree::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; +pub use self::interaction_groups::InteractionGroups; pub use self::shape::{Shape, ShapeType}; mod ball; @@ -97,6 +98,7 @@ mod waabb; mod wquadtree; //mod z_order; mod capsule; +mod interaction_groups; #[cfg(feature = "dim3")] mod polygonal_feature_map; #[cfg(feature = "dim3")] diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index e95709c..5bcdcdb 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -306,6 +306,11 @@ impl NarrowPhase { return; } + if !co1.collision_groups.test(co2.collision_groups) { + // The collision is not allowed. + return; + } + let dispatcher = DefaultProximityDispatcher; if pair.detector.is_none() { // We need a redispatch for this detector. @@ -329,69 +334,6 @@ impl NarrowPhase { .unwrap() .detect_proximity(context, events); }); - - /* - // First, group pairs. - // NOTE: the transmutes here are OK because the Vec are all cleared - // before we leave this method. - // We do this in order to avoid reallocating those vecs each time - // we compute the contacts. Unsafe is necessary because we can't just - // store a Vec<&mut ProximityPair> into the NarrowPhase struct without - // polluting the World with lifetimes. - let ball_ball_prox: &mut Vec<&mut ProximityPair> = - unsafe { std::mem::transmute(&mut self.ball_ball_prox) }; - let shape_shape_prox: &mut Vec<&mut ProximityPair> = - unsafe { std::mem::transmute(&mut self.shape_shape_prox) }; - - let bodies = &bodies.bodies; - - // FIXME: don't iterate through all the interactions. - for pair in &mut self.proximity_graph.interactions { - let co1 = &colliders[pair.pair.collider1]; - let co2 = &colliders[pair.pair.collider2]; - - // FIXME: avoid lookup into bodies. - let rb1 = &bodies[co1.parent]; - let rb2 = &bodies[co2.parent]; - - if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) - { - // No need to update this proximity because nothing moved. - continue; - } - - match (co1.shape(), co2.shape()) { - (Shape::Ball(_), Shape::Ball(_)) => ball_ball_prox.push(pair), - _ => shape_shape_prox.push(pair), - } - } - - par_chunks_mut!(ball_ball_prox, SIMD_WIDTH).for_each(|pairs| { - let context = ProximityDetectionContextSimd { - dispatcher: &DefaultProximityDispatcher, - prediction_distance, - colliders, - pairs, - }; - context.pairs[0] - .detector - .detect_proximity_simd(context, events); - }); - - par_iter_mut!(shape_shape_prox).for_each(|pair| { - let context = ProximityDetectionContext { - dispatcher: &DefaultProximityDispatcher, - prediction_distance, - colliders, - pair, - }; - - context.pair.detector.detect_proximity(context, events); - }); - - ball_ball_prox.clear(); - shape_shape_prox.clear(); - */ } pub(crate) fn compute_contacts( @@ -417,6 +359,11 @@ impl NarrowPhase { return; } + if !co1.collision_groups.test(co2.collision_groups) { + // The collision is not allowed. + return; + } + let dispatcher = DefaultContactDispatcher; if pair.generator.is_none() { // We need a redispatch for this generator. @@ -440,69 +387,6 @@ impl NarrowPhase { .unwrap() .generate_contacts(context, events); }); - - /* - // First, group pairs. - // NOTE: the transmutes here are OK because the Vec are all cleared - // before we leave this method. - // We do this in order to avoid reallocating those vecs each time - // we compute the contacts. Unsafe is necessary because we can't just - // store a Vec<&mut ContactPair> into the NarrowPhase struct without - // polluting the World with lifetimes. - let ball_ball: &mut Vec<&mut ContactPair> = - unsafe { std::mem::transmute(&mut self.ball_ball) }; - let shape_shape: &mut Vec<&mut ContactPair> = - unsafe { std::mem::transmute(&mut self.shape_shape) }; - - let bodies = &bodies.bodies; - - // FIXME: don't iterate through all the interactions. - for pair in &mut self.contact_graph.interactions { - let co1 = &colliders[pair.pair.collider1]; - let co2 = &colliders[pair.pair.collider2]; - - // FIXME: avoid lookup into bodies. - let rb1 = &bodies[co1.parent]; - let rb2 = &bodies[co2.parent]; - - if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) - { - // No need to update this contact because nothing moved. - continue; - } - - match (co1.shape(), co2.shape()) { - (Shape::Ball(_), Shape::Ball(_)) => ball_ball.push(pair), - _ => shape_shape.push(pair), - } - } - - par_chunks_mut!(ball_ball, SIMD_WIDTH).for_each(|pairs| { - let context = ContactGenerationContextSimd { - dispatcher: &DefaultContactDispatcher, - prediction_distance, - colliders, - pairs, - }; - context.pairs[0] - .generator - .generate_contacts_simd(context, events); - }); - - par_iter_mut!(shape_shape).for_each(|pair| { - let context = ContactGenerationContext { - dispatcher: &DefaultContactDispatcher, - prediction_distance, - colliders, - pair, - }; - - context.pair.generator.generate_contacts(context, events); - }); - - ball_ball.clear(); - shape_shape.clear(); - */ } /// Retrieve all the interactions with at least one contact point, happening between two active bodies. From cb6a7ff9468347735ef63db9a9e38faeb476981b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 13:36:53 +0100 Subject: [PATCH 31/41] Add solver flags for controlling whether or not some contacts should be taken into account by the constraints solver. --- build/rapier2d/Cargo.toml | 1 + build/rapier3d/Cargo.toml | 2 +- src/geometry/collider.rs | 22 +++++++++++++- src/geometry/contact.rs | 29 +++++++++++++++++-- .../contact_generator/contact_generator.rs | 14 ++++++--- .../heightfield_shape_contact_generator.rs | 11 +++++-- .../trimesh_shape_contact_generator.rs | 1 + src/geometry/mod.rs | 2 +- src/geometry/narrow_phase.rs | 9 +++++- 9 files changed, 78 insertions(+), 13 deletions(-) diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index d6e5a29..34809f2 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -49,6 +49,7 @@ erased-serde = { version = "0.3", optional = true } indexmap = { version = "1", features = [ "serde-1" ], optional = true } downcast-rs = "1.2" num-derive = "0.3" +bitflags = "1" [dev-dependencies] bincode = "1" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index e9b9808..5cc1de9 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -49,7 +49,7 @@ erased-serde = { version = "0.3", optional = true } indexmap = { version = "1", features = [ "serde-1" ], optional = true } downcast-rs = "1.2" num-derive = "0.3" - +bitflags = "1" [dev-dependencies] bincode = "1" diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 7147e95..f53d75a 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -204,6 +204,7 @@ pub struct Collider { /// The restitution coefficient of this collider. pub restitution: f32, pub(crate) collision_groups: InteractionGroups, + pub(crate) solver_groups: InteractionGroups, pub(crate) contact_graph_index: ColliderGraphIndex, pub(crate) proximity_graph_index: ColliderGraphIndex, pub(crate) proxy_index: usize, @@ -261,6 +262,11 @@ impl Collider { self.collision_groups } + /// The solver groups used by this collider. + pub fn solver_groups(&self) -> InteractionGroups { + self.solver_groups + } + /// The density of this collider. pub fn density(&self) -> f32 { self.density @@ -304,10 +310,12 @@ pub struct ColliderBuilder { pub delta: Isometry, /// Is this collider a sensor? pub is_sensor: bool, - /// The user-data of the collider beind built. + /// The user-data of the collider being built. pub user_data: u128, /// The collision groups for the collider being built. pub collision_groups: InteractionGroups, + /// The solver groups for the collider being built. + pub solver_groups: InteractionGroups, } impl ColliderBuilder { @@ -322,6 +330,7 @@ impl ColliderBuilder { is_sensor: false, user_data: 0, collision_groups: InteractionGroups::all(), + solver_groups: InteractionGroups::all(), } } @@ -442,6 +451,15 @@ impl ColliderBuilder { self } + /// Sets the solver groups used by this collider. + /// + /// Forces between two colliders in contact will be computed iff their solver groups are + /// compatible. See [InteractionGroups::test] for details. + pub fn solver_groups(mut self, groups: InteractionGroups) -> Self { + self.solver_groups = groups; + self + } + /// Sets whether or not the collider built by this builder is a sensor. pub fn sensor(mut self, is_sensor: bool) -> Self { self.is_sensor = is_sensor; @@ -523,6 +541,8 @@ impl ColliderBuilder { contact_graph_index: InteractionGraph::::invalid_graph_index(), proximity_graph_index: InteractionGraph::::invalid_graph_index(), proxy_index: crate::INVALID_USIZE, + collision_groups: self.collision_groups, + solver_groups: self.solver_groups, user_data: self.user_data, } } diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index d211cf1..1f50e43 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -9,6 +9,16 @@ use { simba::simd::SimdValue, }; +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + /// Flags affecting the behavior of the constraints solver for a given contact manifold. + pub struct SolverFlags: u32 { + /// The constraint solver will take this contact manifold into + /// account for force computation. + const COMPUTE_FORCES = 0b01; + } +} + #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// The type local linear approximation of the neighborhood of a pair contact points on two shapes @@ -206,6 +216,7 @@ impl ContactPair { pub(crate) fn single_manifold<'a, 'b>( &'a mut self, colliders: &'b ColliderSet, + flags: SolverFlags, ) -> ( &'b Collider, &'b Collider, @@ -216,7 +227,7 @@ impl ContactPair { let coll2 = &colliders[self.pair.collider2]; if self.manifolds.len() == 0 { - let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2); + let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags); self.manifolds.push(manifold); } @@ -288,6 +299,8 @@ pub struct ContactManifold { /// The relative position between the second collider and its parent at the time the /// contact points were generated. pub delta2: Isometry, + /// Flags used to control some aspects of the constraints solver for this contact manifold. + pub solver_flags: SolverFlags, } impl ContactManifold { @@ -299,6 +312,7 @@ impl ContactManifold { delta2: Isometry, friction: f32, restitution: f32, + solver_flags: SolverFlags, ) -> ContactManifold { Self { #[cfg(feature = "dim2")] @@ -319,6 +333,7 @@ impl ContactManifold { delta2, constraint_index: 0, position_constraint_index: 0, + solver_flags, } } @@ -342,11 +357,17 @@ impl ContactManifold { delta2: self.delta2, constraint_index: self.constraint_index, position_constraint_index: self.position_constraint_index, + solver_flags: self.solver_flags, } } - pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self { - Self::with_subshape_indices(pair, coll1, coll2, 0, 0) + pub(crate) fn from_colliders( + pair: ColliderPair, + coll1: &Collider, + coll2: &Collider, + flags: SolverFlags, + ) -> Self { + Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags) } pub(crate) fn with_subshape_indices( @@ -355,6 +376,7 @@ impl ContactManifold { coll2: &Collider, subshape1: usize, subshape2: usize, + solver_flags: SolverFlags, ) -> Self { Self::new( pair, @@ -364,6 +386,7 @@ impl ContactManifold { *coll2.position_wrt_parent(), (coll1.friction + coll2.friction) * 0.5, (coll1.restitution + coll2.restitution) * 0.5, + solver_flags, ) } diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index b034760..728794d 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -1,5 +1,6 @@ use crate::geometry::{ Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape, + SolverFlags, }; use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] @@ -26,8 +27,9 @@ impl ContactPhase { 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); + let (collider1, collider2, manifold, workspace) = context + .pair + .single_manifold(context.colliders, context.solver_flags); let mut context2 = PrimitiveContactGenerationContext { prediction_distance: context.prediction_distance, collider1, @@ -85,9 +87,11 @@ impl ContactPhase { [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH], > = ArrayVec::new(); - for pair in context.pairs.iter_mut() { + for (pair, solver_flags) in + context.pairs.iter_mut().zip(context.solver_flags.iter()) + { let (collider1, collider2, manifold, workspace) = - pair.single_manifold(context.colliders); + pair.single_manifold(context.colliders, *solver_flags); colliders_arr.push((collider1, collider2)); manifold_arr.push(manifold); workspace_arr.push(workspace); @@ -188,6 +192,7 @@ pub struct ContactGenerationContext<'a> { pub prediction_distance: f32, pub colliders: &'a ColliderSet, pub pair: &'a mut ContactPair, + pub solver_flags: SolverFlags, } #[cfg(feature = "simd-is-enabled")] @@ -196,6 +201,7 @@ pub struct ContactGenerationContextSimd<'a, 'b> { pub prediction_distance: f32, pub colliders: &'a ColliderSet, pub pairs: &'a mut [&'b mut ContactPair], + pub solver_flags: &'a [SolverFlags], } #[derive(Copy, Clone)] diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 9224d4e..f291fa0 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -104,6 +104,7 @@ fn do_generate_contacts( 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, _| { @@ -131,8 +132,14 @@ fn do_generate_contacts( timestamp: new_timestamp, workspace: workspace2, }; - let manifold = - ContactManifold::with_subshape_indices(coll_pair, collider1, collider2, i, 0); + let manifold = ContactManifold::with_subshape_indices( + coll_pair, + collider1, + collider2, + i, + 0, + solver_flags, + ); manifolds.push(manifold); entry.insert(sub_detector) diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 502658d..9474516 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -149,6 +149,7 @@ fn do_generate_contacts( collider2, *triangle_id, 0, + ctxt.solver_flags, ) } else { // We already have a manifold for this triangle. diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 4055d43..9da35d9 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -5,7 +5,7 @@ pub use self::capsule::Capsule; pub use self::collider::{Collider, ColliderBuilder, ColliderShape}; pub use self::collider_set::{ColliderHandle, ColliderSet}; pub use self::contact::{ - Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, + Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, SolverFlags, }; pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher}; #[cfg(feature = "dim2")] diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 5bcdcdb..290d55f 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -15,7 +15,7 @@ use crate::geometry::proximity_detector::{ //}; use crate::geometry::{ BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent, - ProximityPair, RemovedCollider, + ProximityPair, RemovedCollider, SolverFlags, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] @@ -374,11 +374,18 @@ impl NarrowPhase { pair.generator_workspace = workspace; } + let solver_flags = if co1.solver_groups.test(co2.solver_groups) { + SolverFlags::COMPUTE_FORCES + } else { + SolverFlags::empty() + }; + let context = ContactGenerationContext { dispatcher: &dispatcher, prediction_distance, colliders, pair, + solver_flags, }; context From 380c204151ce85da0c21ff420e399e7edc31bfe8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 14:23:41 +0100 Subject: [PATCH 32/41] Add collision groups demos. --- examples2d/all_examples2.rs | 2 + examples2d/collision_groups2.rs | 98 ++++++++++++++++++++++++++++++ examples3d/all_examples3.rs | 2 + examples3d/collision_groups3.rs | 102 ++++++++++++++++++++++++++++++++ src_testbed/engine.rs | 7 +++ src_testbed/testbed.rs | 8 ++- 6 files changed, 218 insertions(+), 1 deletion(-) create mode 100644 examples2d/collision_groups2.rs create mode 100644 examples3d/collision_groups3.rs diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 0ebef88..9bae719 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -11,6 +11,7 @@ use rapier_testbed2d::Testbed; use std::cmp::Ordering; mod add_remove2; +mod collision_groups2; mod debug_box_ball2; mod heightfield2; mod joints2; @@ -52,6 +53,7 @@ pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ ("Add remove", add_remove2::init_world), + ("Collision groups", collision_groups2::init_world), ("Heightfield", heightfield2::init_world), ("Joints", joints2::init_world), ("Platform", platform2::init_world), diff --git a/examples2d/collision_groups2.rs b/examples2d/collision_groups2.rs new file mode 100644 index 0000000..9fd9f0b --- /dev/null +++ b/examples2d/collision_groups2.rs @@ -0,0 +1,98 @@ +use na::{Point2, Point3}; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet, InteractionGroups}; +use rapier_testbed2d::Testbed; + +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 = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height) + .build(); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height).build(); + colliders.insert(collider, floor_handle, &mut bodies); + + /* + * Setup groups + */ + const GREEN_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01); + const BLUE_GROUP: InteractionGroups = InteractionGroups::new(0b10, 0b10); + + /* + * A green floor that will collide with the GREEN group only. + */ + let green_floor = ColliderBuilder::cuboid(1.0, 0.1) + .translation(0.0, 1.0) + .collision_groups(GREEN_GROUP) + .build(); + let green_collider_handle = colliders.insert(green_floor, floor_handle, &mut bodies); + + testbed.set_collider_initial_color(green_collider_handle, Point3::new(0.0, 1.0, 0.0)); + + /* + * A blue floor that will collide with the BLUE group only. + */ + let blue_floor = ColliderBuilder::cuboid(1.0, 0.1) + .translation(0.0, 2.0) + .collision_groups(BLUE_GROUP) + .build(); + let blue_collider_handle = colliders.insert(blue_floor, floor_handle, &mut bodies); + + testbed.set_collider_initial_color(blue_collider_handle, Point3::new(0.0, 0.0, 1.0)); + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.1; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = 2.5; + + for j in 0usize..4 { + for i in 0..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + + // Alternate between the green and blue groups. + let (group, color) = if i % 2 == 0 { + (GREEN_GROUP, Point3::new(0.0, 1.0, 0.0)) + } else { + (BLUE_GROUP, Point3::new(0.0, 0.0, 1.0)) + }; + + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad) + .collision_groups(group) + .build(); + colliders.insert(collider, handle, &mut bodies); + + testbed.set_body_color(handle, color); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 1.0), 100.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 85b2504..eeb9736 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -11,6 +11,7 @@ use rapier_testbed3d::Testbed; use std::cmp::Ordering; mod add_remove3; +mod collision_groups3; mod compound3; mod debug_boxes3; mod debug_cylinder3; @@ -65,6 +66,7 @@ pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ ("Add remove", add_remove3::init_world), ("Primitives", primitives3::init_world), + ("Collision groups", collision_groups3::init_world), ("Compound", compound3::init_world), ("Domino", domino3::init_world), ("Heightfield", heightfield3::init_world), diff --git a/examples3d/collision_groups3.rs b/examples3d/collision_groups3.rs new file mode 100644 index 0000000..625bc50 --- /dev/null +++ b/examples3d/collision_groups3.rs @@ -0,0 +1,102 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet, InteractionGroups}; +use rapier_testbed3d::Testbed; + +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 = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, floor_handle, &mut bodies); + + /* + * Setup groups + */ + const GREEN_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01); + const BLUE_GROUP: InteractionGroups = InteractionGroups::new(0b10, 0b10); + + /* + * A green floor that will collide with the GREEN group only. + */ + let green_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0) + .translation(0.0, 1.0, 0.0) + .collision_groups(GREEN_GROUP) + .build(); + let green_collider_handle = colliders.insert(green_floor, floor_handle, &mut bodies); + + testbed.set_collider_initial_color(green_collider_handle, Point3::new(0.0, 1.0, 0.0)); + + /* + * A blue floor that will collide with the BLUE group only. + */ + let blue_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0) + .translation(0.0, 2.0, 0.0) + .collision_groups(BLUE_GROUP) + .build(); + let blue_collider_handle = colliders.insert(blue_floor, floor_handle, &mut bodies); + + testbed.set_collider_initial_color(blue_collider_handle, Point3::new(0.0, 0.0, 1.0)); + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.1; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = 2.5; + let centerz = shift * (num / 2) as f32; + + for j in 0usize..4 { + for i in 0..num { + for k in 0usize..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + let z = k as f32 * shift - centerz; + + // Alternate between the green and blue groups. + let (group, color) = if k % 2 == 0 { + (GREEN_GROUP, Point3::new(0.0, 1.0, 0.0)) + } else { + (BLUE_GROUP, Point3::new(0.0, 0.0, 1.0)) + }; + + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad) + .collision_groups(group) + .build(); + colliders.insert(collider, handle, &mut bodies); + + testbed.set_body_color(handle, color); + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index b02f146..1bafdb6 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -63,6 +63,7 @@ pub struct GraphicsManager { #[cfg(feature = "fluids")] boundary2sn: HashMap, b2color: HashMap>, + c2color: HashMap>, b2wireframe: HashMap, #[cfg(feature = "fluids")] f2color: HashMap>, @@ -100,6 +101,7 @@ impl GraphicsManager { #[cfg(feature = "fluids")] boundary2sn: HashMap::new(), b2color: HashMap::new(), + c2color: HashMap::new(), #[cfg(feature = "fluids")] f2color: HashMap::new(), ground_color: Point3::new(0.5, 0.5, 0.5), @@ -186,6 +188,10 @@ impl GraphicsManager { } } + pub fn set_collider_initial_color(&mut self, c: ColliderHandle, color: Point3) { + self.c2color.insert(c, color); + } + pub fn set_body_wireframe(&mut self, b: RigidBodyHandle, enabled: bool) { self.b2wireframe.insert(b, enabled); @@ -324,6 +330,7 @@ impl GraphicsManager { let mut new_nodes = Vec::new(); for collider_handle in bodies[handle].colliders() { + let color = self.c2color.get(collider_handle).copied().unwrap_or(color); let collider = &colliders[*collider_handle]; self.add_collider(window, *collider_handle, collider, color, &mut new_nodes); } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 2b733a3..3a1b938 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -23,7 +23,9 @@ use rapier::dynamics::{ }; #[cfg(feature = "dim3")] use rapier::geometry::Ray; -use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent}; +use rapier::geometry::{ + BroadPhase, ColliderHandle, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, +}; use rapier::math::Vector; use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline}; #[cfg(feature = "fluids")] @@ -497,6 +499,10 @@ impl Testbed { self.graphics.set_body_color(body, color); } + pub fn set_collider_initial_color(&mut self, collider: ColliderHandle, color: Point3) { + self.graphics.set_collider_initial_color(collider, color); + } + #[cfg(feature = "fluids")] pub fn set_fluid_color(&mut self, fluid: FluidHandle, color: Point3) { self.graphics.set_fluid_color(fluid, color); From 2509e42d7b7e1bed2f1dca1f9eac8c6484fadf25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 14:24:25 +0100 Subject: [PATCH 33/41] Add a construstructor to InteractionGroups + make its methods const. --- src/geometry/interaction_groups.rs | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/geometry/interaction_groups.rs b/src/geometry/interaction_groups.rs index 9596345..6812fbf 100644 --- a/src/geometry/interaction_groups.rs +++ b/src/geometry/interaction_groups.rs @@ -18,23 +18,28 @@ pub struct InteractionGroups(pub u32); impl InteractionGroups { + /// Initializes with the given interaction groups and interaction mask. + pub const fn new(groups: u16, masks: u16) -> Self { + Self::none().with_groups(groups).with_mask(masks) + } + /// Allow interaction with everything. - pub fn all() -> Self { + pub const fn all() -> Self { Self(u32::MAX) } /// Prevent all interactions. - pub fn none() -> Self { + pub const fn none() -> Self { Self(0) } /// Sets the group this filter is part of. - pub fn with_groups(self, groups: u16) -> Self { + pub const fn with_groups(self, groups: u16) -> Self { Self((self.0 & 0x0000ffff) | ((groups as u32) << 16)) } /// Sets the interaction mask of this filter. - pub fn with_mask(self, mask: u16) -> Self { + pub const fn with_mask(self, mask: u16) -> Self { Self((self.0 & 0xffff0000) | (mask as u32)) } @@ -42,7 +47,8 @@ impl InteractionGroups { /// /// An interaction is allowed iff. the groups of `self` contain at least one bit set to 1 in common /// with the mask of `rhs`, and vice-versa. - pub fn test(self, rhs: Self) -> bool { + #[inline] + pub const fn test(self, rhs: Self) -> bool { ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0 } } From 5a5ba9cf5992a71f8bbdf594d17f0385bf7f86ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 14:25:19 +0100 Subject: [PATCH 34/41] Take the solver flags into account when collecting contact manifolds to solve. --- src/geometry/narrow_phase.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 290d55f..f5517ba 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -413,7 +413,8 @@ impl NarrowPhase { for manifold in &mut inter.weight.manifolds { let rb1 = &bodies[manifold.body_pair.body1]; let rb2 = &bodies[manifold.body_pair.body2]; - if manifold.num_active_contacts() != 0 + if manifold.solver_flags.contains(SolverFlags::COMPUTE_FORCES) + && manifold.num_active_contacts() != 0 && (!rb1.is_dynamic() || !rb1.is_sleeping()) && (!rb2.is_dynamic() || !rb2.is_sleeping()) { From b4d322a6ca8dc363c5085ba5539281d50be7a525 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 14:29:58 +0100 Subject: [PATCH 35/41] Take collision groups into account for ray-casting. --- src/pipeline/query_pipeline.rs | 43 +++++++++++++++++++++------------- src_testbed/testbed.rs | 13 ++++++---- 2 files changed, 35 insertions(+), 21 deletions(-) diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 03103be..00d4396 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,5 +1,7 @@ use crate::dynamics::RigidBodySet; -use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree}; +use crate::geometry::{ + Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, WQuadtree, +}; /// A pipeline for performing queries on all the colliders of a scene. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -59,6 +61,7 @@ impl QueryPipeline { colliders: &'a ColliderSet, ray: &Ray, max_toi: f32, + groups: InteractionGroups, ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { // TODO: avoid allocation? let mut inter = Vec::new(); @@ -69,14 +72,17 @@ impl QueryPipeline { for handle in inter { let collider = &colliders[handle]; - if let Some(inter) = - collider - .shape() - .toi_and_normal_with_ray(collider.position(), ray, max_toi, true) - { - if inter.toi < best { - best = inter.toi; - result = Some((handle, collider, inter)); + if collider.collision_groups.test(groups) { + if let Some(inter) = collider.shape().toi_and_normal_with_ray( + collider.position(), + ray, + max_toi, + true, + ) { + if inter.toi < best { + best = inter.toi; + result = Some((handle, collider, inter)); + } } } } @@ -99,6 +105,7 @@ impl QueryPipeline { colliders: &'a ColliderSet, ray: &Ray, max_toi: f32, + groups: InteractionGroups, mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool, ) { // TODO: avoid allocation? @@ -107,13 +114,17 @@ impl QueryPipeline { for handle in inter { let collider = &colliders[handle]; - if let Some(inter) = - collider - .shape() - .toi_and_normal_with_ray(collider.position(), ray, max_toi, true) - { - if !callback(handle, collider, inter) { - return; + + if collider.collision_groups.test(groups) { + if let Some(inter) = collider.shape().toi_and_normal_with_ray( + collider.position(), + ray, + max_toi, + true, + ) { + if !callback(handle, collider, inter) { + return; + } } } } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 3a1b938..456b894 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -24,7 +24,8 @@ use rapier::dynamics::{ #[cfg(feature = "dim3")] use rapier::geometry::Ray; use rapier::geometry::{ - BroadPhase, ColliderHandle, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, + BroadPhase, ColliderHandle, ColliderSet, ContactEvent, InteractionGroups, NarrowPhase, + ProximityEvent, }; use rapier::math::Vector; use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline}; @@ -1188,10 +1189,12 @@ impl Testbed { .camera() .unproject(&self.cursor_pos, &na::convert(size)); let ray = Ray::new(pos, dir); - let hit = self - .physics - .query_pipeline - .cast_ray(&self.physics.colliders, &ray, f32::MAX); + let hit = self.physics.query_pipeline.cast_ray( + &self.physics.colliders, + &ray, + f32::MAX, + InteractionGroups::all(), + ); if let Some((_, collider, _)) = hit { if self.physics.bodies[collider.parent()].is_dynamic() { From 7cafc5471c7fb22b4034b8fe90e848cd0912204d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 15:05:10 +0100 Subject: [PATCH 36/41] Ignore the code block in the InteractionGroups doc. --- src/geometry/interaction_groups.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/geometry/interaction_groups.rs b/src/geometry/interaction_groups.rs index 6812fbf..48808de 100644 --- a/src/geometry/interaction_groups.rs +++ b/src/geometry/interaction_groups.rs @@ -12,7 +12,7 @@ /// - The interaction groups of `a` has at least one bit set to `1` in common with the interaction mask of `b`. /// - The interaction groups of `b` has at least one bit set to `1` in common with the interaction mask of `a`. /// In other words, interactions are allowed between two filter iff. the following condition is met: -/// ```rust +/// ```ignore /// ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0 /// ``` pub struct InteractionGroups(pub u32); From cc44b65094766aab40561f22431a95877ed5ff11 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 16:11:20 +0100 Subject: [PATCH 37/41] Added user-implementable traits for collision/proximity pair filtering. --- src/geometry/mod.rs | 2 + src/geometry/narrow_phase.rs | 70 ++++++++++++++++++++++++------ src/geometry/user_callbacks.rs | 60 +++++++++++++++++++++++++ src/pipeline/collision_pipeline.rs | 23 ++++++++-- src/pipeline/physics_pipeline.rs | 7 ++- src_testbed/testbed.rs | 6 +++ 6 files changed, 150 insertions(+), 18 deletions(-) create mode 100644 src/geometry/user_callbacks.rs diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 9da35d9..c8ad28e 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -22,6 +22,7 @@ pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatch #[cfg(feature = "dim3")] pub use self::round_cylinder::RoundCylinder; pub use self::trimesh::Trimesh; +pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; pub use ncollide::query::Proximity; /// A segment shape. @@ -104,3 +105,4 @@ mod polygonal_feature_map; #[cfg(feature = "dim3")] mod round_cylinder; mod shape; +mod user_callbacks; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index f5517ba..fb5da99 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -14,8 +14,9 @@ use crate::geometry::proximity_detector::{ // proximity_detector::ProximityDetectionContextSimd, WBall, //}; use crate::geometry::{ - BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent, - ProximityPair, RemovedCollider, SolverFlags, + BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactPairFilter, + PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, RemovedCollider, + SolverFlags, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] @@ -290,6 +291,7 @@ impl NarrowPhase { prediction_distance: f32, bodies: &RigidBodySet, colliders: &ColliderSet, + pair_filter: Option<&dyn ProximityPairFilter>, events: &dyn EventHandler, ) { par_iter_mut!(&mut self.proximity_graph.graph.edges).for_each(|edge| { @@ -301,16 +303,35 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) { - // No need to update this contact because nothing moved. + if (rb1.is_sleeping() && rb2.is_static()) || (rb2.is_sleeping() && rb1.is_static()) { + // No need to update this proximity because nothing moved. return; } if !co1.collision_groups.test(co2.collision_groups) { - // The collision is not allowed. + // The proximity is not allowed. return; } + if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() { + // Default filtering rule: no proximity between two non-dynamic bodies. + return; + } + + if let Some(filter) = pair_filter { + let context = PairFilterContext { + collider1: co1, + collider2: co2, + rigid_body1: rb1, + rigid_body2: rb2, + }; + + if !filter.filter_proximity_pair(&context) { + // No proximity allowed. + return; + } + } + let dispatcher = DefaultProximityDispatcher; if pair.detector.is_none() { // We need a redispatch for this detector. @@ -341,6 +362,7 @@ impl NarrowPhase { prediction_distance: f32, bodies: &RigidBodySet, colliders: &ColliderSet, + pair_filter: Option<&dyn ContactPairFilter>, events: &dyn EventHandler, ) { par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { @@ -352,9 +374,7 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static())) - || (!rb1.is_dynamic() && !rb2.is_dynamic()) - { + if (rb1.is_sleeping() && rb2.is_static()) || (rb2.is_sleeping() && rb1.is_static()) { // No need to update this contact because nothing moved. return; } @@ -364,6 +384,33 @@ impl NarrowPhase { return; } + if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() { + // Default filtering rule: no contact between two non-dynamic bodies. + return; + } + + let solver_flags = if let Some(filter) = pair_filter { + let context = PairFilterContext { + collider1: co1, + collider2: co2, + rigid_body1: rb1, + rigid_body2: rb2, + }; + + if let Some(solver_flags) = filter.filter_contact_pair(&context) { + solver_flags + } else { + // No contact allowed. + return; + } + } else { + if co1.solver_groups.test(co2.solver_groups) { + SolverFlags::COMPUTE_FORCES + } else { + SolverFlags::empty() + } + }; + let dispatcher = DefaultContactDispatcher; if pair.generator.is_none() { // We need a redispatch for this generator. @@ -374,12 +421,6 @@ impl NarrowPhase { pair.generator_workspace = workspace; } - let solver_flags = if co1.solver_groups.test(co2.solver_groups) { - SolverFlags::COMPUTE_FORCES - } else { - SolverFlags::empty() - }; - let context = ContactGenerationContext { dispatcher: &dispatcher, prediction_distance, @@ -415,6 +456,7 @@ impl NarrowPhase { let rb2 = &bodies[manifold.body_pair.body2]; if manifold.solver_flags.contains(SolverFlags::COMPUTE_FORCES) && manifold.num_active_contacts() != 0 + && (rb1.is_dynamic() || rb2.is_dynamic()) && (!rb1.is_dynamic() || !rb1.is_sleeping()) && (!rb2.is_dynamic() || !rb2.is_sleeping()) { diff --git a/src/geometry/user_callbacks.rs b/src/geometry/user_callbacks.rs new file mode 100644 index 0000000..3602faf --- /dev/null +++ b/src/geometry/user_callbacks.rs @@ -0,0 +1,60 @@ +use crate::dynamics::RigidBody; +use crate::geometry::{Collider, SolverFlags}; + +/// Context given to custom collision filters to filter-out collisions. +pub struct PairFilterContext<'a> { + /// The first collider involved in the potential collision. + pub collider1: &'a Collider, + /// The first collider involved in the potential collision. + pub collider2: &'a Collider, + /// The first collider involved in the potential collision. + pub rigid_body1: &'a RigidBody, + /// The first collider involved in the potential collision. + pub rigid_body2: &'a RigidBody, +} + +/// User-defined filter for potential contact pairs detected by the broad-phase. +/// +/// This can be used to apply custom logic in order to decide whether two colliders +/// should have their contact computed by the narrow-phase, and if these contact +/// should be solved by the constraints solver +pub trait ContactPairFilter: Send + Sync { + /// Applies the contact pair filter. + /// + /// Note that using a contact pair filter will replace the default contact filtering + /// which consists of preventing contact computation between two non-dynamic bodies. + /// + /// Note that using a contact pair filter will replace the default determination + /// of solver flags, based on the colliders solver groups. + /// + /// This filtering method is called after taking into account the colliders collision groups. + /// + /// If this returns `None`, then the narrow-phase will ignore this contact pair and + /// not compute any contact manifolds for it. + /// If this returns `Some`, then the narrow-phase will compute contact manifolds for + /// this pair of colliders, and configure them with the returned solver flags. For + /// example, if this returns `Some(SolverFlags::COMPUTE_FORCES)` then the contacts + /// will be taken into account by the constraints solver. If this returns + /// `Some(SolverFlags::empty())` then the constraints solver will ignore these + /// contacts. + fn filter_contact_pair(&self, context: &PairFilterContext) -> Option; +} + +/// User-defined filter for potential proximity pairs detected by the broad-phase. +/// +/// This can be used to apply custom logic in order to decide whether two colliders +/// should have their proximity computed by the narrow-phase. +pub trait ProximityPairFilter: Send + Sync { + /// Applies the proximity pair filter. + /// + /// Note that using a proximity pair filter will replace the default proximity filtering + /// which consists of preventing proximity computation between two non-dynamic bodies. + /// + /// This filtering method is called after taking into account the colliders collision groups. + /// + /// If this returns `false`, then the narrow-phase will ignore this pair and + /// not compute any proximity information for it. + /// If this return `true` then the narrow-phase will compute proximity + /// information for this pair. + fn filter_proximity_pair(&self, context: &PairFilterContext) -> bool; +} diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 5a19e52..b8896e8 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,7 +1,10 @@ //! Physics pipeline structures. use crate::dynamics::{JointSet, RigidBodySet}; -use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase}; +use crate::geometry::{ + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter, NarrowPhase, + ProximityPairFilter, +}; use crate::pipeline::EventHandler; /// The collision pipeline, responsible for performing collision detection between colliders. @@ -40,6 +43,8 @@ impl CollisionPipeline { narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + contact_pair_filter: Option<&dyn ContactPairFilter>, + proximity_pair_filter: Option<&dyn ProximityPairFilter>, events: &dyn EventHandler, ) { bodies.maintain_active_set(); @@ -52,8 +57,20 @@ impl CollisionPipeline { narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); - narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events); - narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events); + narrow_phase.compute_contacts( + prediction_distance, + bodies, + colliders, + contact_pair_filter, + events, + ); + narrow_phase.compute_proximities( + prediction_distance, + bodies, + colliders, + proximity_pair_filter, + events, + ); bodies.update_active_set_with_contacts( colliders, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 47fd260..b99934b 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -7,7 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, + ContactPairFilter, NarrowPhase, ProximityPairFilter, }; use crate::math::Vector; use crate::pipeline::EventHandler; @@ -68,6 +69,8 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, joints: &mut JointSet, + contact_pair_filter: Option<&dyn ContactPairFilter>, + proximity_pair_filter: Option<&dyn ProximityPairFilter>, events: &dyn EventHandler, ) { self.counters.step_started(); @@ -112,12 +115,14 @@ impl PhysicsPipeline { integration_parameters.prediction_distance, bodies, colliders, + contact_pair_filter, events, ); narrow_phase.compute_proximities( integration_parameters.prediction_distance, bodies, colliders, + proximity_pair_filter, events, ); // println!("Compute contact time: {}", instant::now() - t); diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 456b894..3bf720a 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -681,6 +681,8 @@ impl Testbed { &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, + None, + None, &self.event_handler, ); @@ -1457,6 +1459,8 @@ impl State for Testbed { &mut physics.bodies, &mut physics.colliders, &mut physics.joints, + None, + None, event_handler, ); }); @@ -1471,6 +1475,8 @@ impl State for Testbed { &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, + None, + None, &self.event_handler, ); From 24bd97636e890195c8a72f8e265809bbae44ab13 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 16:21:33 +0100 Subject: [PATCH 38/41] Rename SolverFlags::COMPUTE_FORCES to SolverFlags::COMPUTE_IMPULSES. This is closer to what the solver actually does. --- src/geometry/contact.rs | 2 +- src/geometry/narrow_phase.rs | 6 ++++-- src/geometry/user_callbacks.rs | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 1f50e43..d8f3632 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -15,7 +15,7 @@ bitflags::bitflags! { pub struct SolverFlags: u32 { /// The constraint solver will take this contact manifold into /// account for force computation. - const COMPUTE_FORCES = 0b01; + const COMPUTE_IMPULSES = 0b01; } } diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index fb5da99..fd2652d 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -405,7 +405,7 @@ impl NarrowPhase { } } else { if co1.solver_groups.test(co2.solver_groups) { - SolverFlags::COMPUTE_FORCES + SolverFlags::COMPUTE_IMPULSES } else { SolverFlags::empty() } @@ -454,7 +454,9 @@ impl NarrowPhase { for manifold in &mut inter.weight.manifolds { let rb1 = &bodies[manifold.body_pair.body1]; let rb2 = &bodies[manifold.body_pair.body2]; - if manifold.solver_flags.contains(SolverFlags::COMPUTE_FORCES) + if manifold + .solver_flags + .contains(SolverFlags::COMPUTE_IMPULSES) && manifold.num_active_contacts() != 0 && (rb1.is_dynamic() || rb2.is_dynamic()) && (!rb1.is_dynamic() || !rb1.is_sleeping()) diff --git a/src/geometry/user_callbacks.rs b/src/geometry/user_callbacks.rs index 3602faf..9b36695 100644 --- a/src/geometry/user_callbacks.rs +++ b/src/geometry/user_callbacks.rs @@ -33,7 +33,7 @@ pub trait ContactPairFilter: Send + Sync { /// not compute any contact manifolds for it. /// If this returns `Some`, then the narrow-phase will compute contact manifolds for /// this pair of colliders, and configure them with the returned solver flags. For - /// example, if this returns `Some(SolverFlags::COMPUTE_FORCES)` then the contacts + /// example, if this returns `Some(SolverFlags::COMPUTE_IMPULSES)` then the contacts /// will be taken into account by the constraints solver. If this returns /// `Some(SolverFlags::empty())` then the constraints solver will ignore these /// contacts. From 3bfa4079999f6c886e692de256abf51e4506a2b1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 16:48:05 +0100 Subject: [PATCH 39/41] ContactPairFilter: don't overwrite the effect of the solver groups. This is more consistent with the fact that the effect of collision groups is not overwritten either. --- src/geometry/narrow_phase.rs | 20 ++++++++++---------- src/geometry/user_callbacks.rs | 11 ++++------- src/pipeline/physics_pipeline.rs | 4 ++++ 3 files changed, 18 insertions(+), 17 deletions(-) diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index fd2652d..69678cd 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -320,10 +320,10 @@ impl NarrowPhase { if let Some(filter) = pair_filter { let context = PairFilterContext { - collider1: co1, - collider2: co2, rigid_body1: rb1, rigid_body2: rb2, + collider1: co1, + collider2: co2, }; if !filter.filter_proximity_pair(&context) { @@ -389,12 +389,12 @@ impl NarrowPhase { return; } - let solver_flags = if let Some(filter) = pair_filter { + let mut solver_flags = if let Some(filter) = pair_filter { let context = PairFilterContext { - collider1: co1, - collider2: co2, rigid_body1: rb1, rigid_body2: rb2, + collider1: co1, + collider2: co2, }; if let Some(solver_flags) = filter.filter_contact_pair(&context) { @@ -404,13 +404,13 @@ impl NarrowPhase { return; } } else { - if co1.solver_groups.test(co2.solver_groups) { - SolverFlags::COMPUTE_IMPULSES - } else { - SolverFlags::empty() - } + SolverFlags::COMPUTE_IMPULSES }; + if !co1.solver_groups.test(co2.solver_groups) { + solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); + } + let dispatcher = DefaultContactDispatcher; if pair.generator.is_none() { // We need a redispatch for this generator. diff --git a/src/geometry/user_callbacks.rs b/src/geometry/user_callbacks.rs index 9b36695..ae0119f 100644 --- a/src/geometry/user_callbacks.rs +++ b/src/geometry/user_callbacks.rs @@ -3,14 +3,14 @@ use crate::geometry::{Collider, SolverFlags}; /// Context given to custom collision filters to filter-out collisions. pub struct PairFilterContext<'a> { - /// The first collider involved in the potential collision. - pub collider1: &'a Collider, - /// The first collider involved in the potential collision. - pub collider2: &'a Collider, /// The first collider involved in the potential collision. pub rigid_body1: &'a RigidBody, /// The first collider involved in the potential collision. pub rigid_body2: &'a RigidBody, + /// The first collider involved in the potential collision. + pub collider1: &'a Collider, + /// The first collider involved in the potential collision. + pub collider2: &'a Collider, } /// User-defined filter for potential contact pairs detected by the broad-phase. @@ -24,9 +24,6 @@ pub trait ContactPairFilter: Send + Sync { /// Note that using a contact pair filter will replace the default contact filtering /// which consists of preventing contact computation between two non-dynamic bodies. /// - /// Note that using a contact pair filter will replace the default determination - /// of solver flags, based on the colliders solver groups. - /// /// This filtering method is called after taking into account the colliders collision groups. /// /// If this returns `None`, then the narrow-phase will ignore this contact pair and diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index b99934b..0720ff1 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -290,6 +290,8 @@ mod test { &mut bodies, &mut colliders, &mut joints, + None, + None, &(), ); } @@ -332,6 +334,8 @@ mod test { &mut bodies, &mut colliders, &mut joints, + None, + None, &(), ); } From 74f0297221607e1929db75e79089d7cb75558dfe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 17:14:13 +0100 Subject: [PATCH 40/41] Fix performance regression due to sleeping objects pairs no longer being ignored by the narrow-phase. --- src/geometry/narrow_phase.rs | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 69678cd..c1bd411 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -303,7 +303,10 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if (rb1.is_sleeping() && rb2.is_static()) || (rb2.is_sleeping() && rb1.is_static()) { + if (rb1.is_sleeping() && rb2.is_static()) + || (rb2.is_sleeping() && rb1.is_static()) + || (rb1.is_sleeping() && rb2.is_sleeping()) + { // No need to update this proximity because nothing moved. return; } @@ -374,7 +377,10 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if (rb1.is_sleeping() && rb2.is_static()) || (rb2.is_sleeping() && rb1.is_static()) { + if (rb1.is_sleeping() && rb2.is_static()) + || (rb2.is_sleeping() && rb1.is_static()) + || (rb1.is_sleeping() && rb2.is_sleeping()) + { // No need to update this contact because nothing moved. return; } From 4b8242b9c267a9412c88793575db37f79c544ca2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 27 Oct 2020 17:48:21 +0100 Subject: [PATCH 41/41] Release v0.3.0 --- CHANGELOG | 13 +++++++++++++ build/rapier2d/Cargo.toml | 2 +- build/rapier3d/Cargo.toml | 2 +- build/rapier_testbed2d/Cargo.toml | 4 ++-- build/rapier_testbed3d/Cargo.toml | 4 ++-- 5 files changed, 19 insertions(+), 6 deletions(-) diff --git a/CHANGELOG b/CHANGELOG index 06eea7d..aba1ac2 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -1,3 +1,16 @@ +## v0.3.0 +- Collider shapes are now trait-objects instead of a `Shape` enum. +- Add a user-defined `u128` to each colliders and rigid-bodies for storing user data. +- Add the support for `Cylinder`, `RoundCylinder`, and `Cone` shapes. +- Added the support for collision filtering based on bit masks (often known as collision groups, collision masks, or + collision layers in other physics engines). Each collider has two groups. Their `collision_groups` is used for filtering + what pair of colliders should have their contacts computed by the narrow-phase. Their `solver_groups` is used for filtering + what pair of colliders should have their contact forces computed by the constraints solver. +- Collision groups can also be used to filter what collider should be hit by a ray-cast performed by the `QueryPipeline`. +- Added collision filters based on user-defined trait-objects. This adds two traits `ContactPairFilter` and + `ProximityPairFilter` that allows user-defined logic for determining if two colliders/sensors are allowed to interact. +- The `PhysicsPipeline::step` method now takes two additional arguments: the optional `&ContactPairFilter` and `&ProximityPairFilter` +for filtering contact and proximity pairs. ## v0.2.1 - Fix panic in TriMesh construction and QueryPipeline update caused by a stack overflow or a subtraction underflow. diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 34809f2..d963079 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier2d" -version = "0.2.1" +version = "0.3.0" authors = [ "Sébastien Crozet " ] description = "2-dimensional physics engine in Rust." documentation = "http://docs.rs/rapier2d" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index 5cc1de9..f5c2fe4 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier3d" -version = "0.2.1" +version = "0.3.0" authors = [ "Sébastien Crozet " ] description = "3-dimensional physics engine in Rust." documentation = "http://docs.rs/rapier3d" diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index 87610c3..08c0893 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed2d" -version = "0.2.0" +version = "0.3.0" authors = [ "Sébastien Crozet " ] description = "Testbed for the 2-dimensional physics engine in Rust." homepage = "http://rapier.org" @@ -42,5 +42,5 @@ md5 = "0.7" [dependencies.rapier2d] path = "../rapier2d" -version = "0.2" +version = "0.3" features = [ "serde-serialize" ] diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index e99d741..7fa01d7 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed3d" -version = "0.2.0" +version = "0.3.0" authors = [ "Sébastien Crozet " ] description = "Testbed for the 3-dimensional physics engine in Rust." homepage = "http://rapier.org" @@ -44,5 +44,5 @@ serde = { version = "1", features = [ "derive" ] } [dependencies.rapier3d] path = "../rapier3d" -version = "0.2" +version = "0.3" features = [ "serde-serialize" ]