From 9bf1321f8f1d2e116f44c2461a53f302c4ef4171 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 8 Dec 2020 17:31:49 +0100 Subject: Outsource the contact manifold, SAT, and some shapes. --- .../ball_ball_contact_generator.rs | 6 +- .../ball_convex_contact_generator.rs | 18 +-- .../capsule_capsule_contact_generator.rs | 33 ++-- .../cuboid_capsule_contact_generator.rs | 37 ++--- .../cuboid_cuboid_contact_generator.rs | 23 +-- .../cuboid_triangle_contact_generator.rs | 38 +++-- .../heightfield_shape_contact_generator.rs | 14 +- src/geometry/contact_generator/mod.rs | 30 ---- .../contact_generator/pfm_pfm_contact_generator.rs | 30 ++-- .../polygon_polygon_contact_generator.rs | 175 ++------------------- .../trimesh_shape_contact_generator.rs | 18 +-- 11 files changed, 123 insertions(+), 299 deletions(-) (limited to 'src/geometry/contact_generator') diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs index 96ac235..7122998 100644 --- a/src/geometry/contact_generator/ball_ball_contact_generator.rs +++ b/src/geometry/contact_generator/ball_ball_contact_generator.rs @@ -1,5 +1,5 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Contact, KinematicsCategory}; +use crate::geometry::{Contact, ContactManifoldData, KinematicsCategory}; use crate::math::{Point, Vector}; #[cfg(feature = "simd-is-enabled")] use { @@ -53,7 +53,7 @@ pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationCon manifold.kinematics.category = KinematicsCategory::PointPoint; manifold.kinematics.radius1 = radii_a.extract(i); manifold.kinematics.radius2 = radii_b.extract(i); - manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(manifold); } else { manifold.points.clear(); } @@ -94,7 +94,7 @@ pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint; ctxt.manifold.kinematics.radius1 = radius_a; ctxt.manifold.kinematics.radius2 = radius_b; - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } else { ctxt.manifold.points.clear(); } diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index 69bc5e3..88f1912 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -1,8 +1,8 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Ball, Contact, KinematicsCategory}; +use crate::geometry::{Ball, Contact, ContactManifoldData, KinematicsCategory}; use crate::math::Isometry; +use buckler::query::PointQuery; use na::Unit; -use ncollide::query::PointQuery; pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { if let Some(ball1) = ctxt.shape1.as_ball() { @@ -15,7 +15,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex ctxt.manifold.sort_contacts(ctxt.prediction_distance); } -fn do_generate_contacts>( +fn do_generate_contacts( point_query1: &P, ball2: &Ball, ctxt: &mut PrimitiveContactGenerationContext, @@ -33,12 +33,8 @@ fn do_generate_contacts>( } let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into()); - - // TODO: add a `project_local_point` to the PointQuery trait to avoid - // the identity isometry. - let proj = - point_query1.project_point(&Isometry::identity(), &local_p2_1, cfg!(feature = "dim3")); - let dpos = local_p2_1 - proj.point; + let proj = point_query1.project_local_point(&local_p2_1, cfg!(feature = "dim3")); + let dpos = local_p2_1 - proj.local_point; #[allow(unused_mut)] // Because `mut local_n1, mut dist` is needed in 2D but not in 3D. if let Some((mut local_n1, mut dist)) = Unit::try_new_and_get(dpos, 0.0) { @@ -51,8 +47,8 @@ fn do_generate_contacts>( if dist <= ball2.radius + ctxt.prediction_distance { let local_n2 = position2.inverse_transform_vector(&(position1 * -*local_n1)); let local_p2 = (local_n2 * ball2.radius).into(); + let contact_point = Contact::new(proj.local_point, local_p2, 0, 0, dist - ball2.radius); - let contact_point = Contact::new(proj.point, local_p2, 0, 0, dist - ball2.radius); if ctxt.manifold.points.len() != 1 { ctxt.manifold.points.clear(); ctxt.manifold.points.push(contact_point); @@ -66,7 +62,7 @@ fn do_generate_contacts>( ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; ctxt.manifold.kinematics.radius1 = 0.0; ctxt.manifold.kinematics.radius2 = ball2.radius; - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } else { ctxt.manifold.points.clear(); } diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index 3104496..9090b36 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}; +use crate::geometry::{Capsule, Contact, ContactManifold, ContactManifoldData, KinematicsCategory}; use crate::math::Isometry; use crate::math::Vector; use approx::AbsDiffEq; -use na::Unit; #[cfg(feature = "dim2")] -use ncollide::shape::SegmentPointLocation; +use buckler::shape::SegmentPointLocation; +use na::Unit; pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) { if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) { @@ -19,7 +19,7 @@ pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationCo ); } - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -41,7 +41,7 @@ pub fn generate_contacts<'a>( let seg1 = capsule1.segment; let seg2_1 = capsule2.segment.transformed(&pos12); - let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( + let (loc1, loc2) = buckler::query::details::closest_points_segment_segment_with_locations_nD( (&seg1.a, &seg1.b), (&seg2_1.a, &seg2_1.b), ); @@ -94,11 +94,13 @@ pub fn generate_contacts<'a>( { // 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( - (seg1.a, seg1.b), - (seg2_1.a, seg2_1.b), - *local_n1, - ) { + if let Some((clip_a, clip_b)) = + buckler::query::details::clip_segment_segment_with_normal( + (seg1.a, seg1.b), + (seg2_1.a, seg2_1.b), + *local_n1, + ) + { let contact = if (clip_a.0 - local_p1).norm_squared() > f32::default_epsilon() * 100.0 { // Use clip_a as the second contact. @@ -139,7 +141,7 @@ pub fn generate_contacts<'a>( } } - super::match_contacts(manifold, &old_manifold_points, swapped); + manifold.match_contacts(&old_manifold_points, swapped); } #[cfg(feature = "dim3")] @@ -156,10 +158,11 @@ pub fn generate_contacts<'a>( 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), - ); + let (loc1, loc2) = + buckler::query::closest_points::closest_points_segment_segment_with_locations_nD( + (&seg1.a, &seg1.b), + (&seg2_1.a, &seg2_1.b), + ); { let bcoords1 = loc1.barycentric_coordinates(); diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index 3fd4a17..9f5d856 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -1,11 +1,12 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -#[cfg(feature = "dim3")] -use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory}; -#[cfg(feature = "dim2")] -use crate::geometry::{CuboidFeature, CuboidFeatureFace}; +use crate::geometry::{Capsule, ContactManifold, ContactManifoldData, Cuboid, KinematicsCategory}; use crate::math::Isometry; use crate::math::Vector; +use buckler::query::sat; +#[cfg(feature = "dim3")] +use buckler::shape::PolyhedronFeature; +#[cfg(feature = "dim2")] +use buckler::shape::{CuboidFeature, CuboidFeatureFace}; pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) { if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) { @@ -18,7 +19,7 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon ctxt.manifold, false, ); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } else if let (Some(capsule1), Some(cube2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid()) { @@ -31,7 +32,7 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon ctxt.manifold, true, ); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -61,7 +62,8 @@ pub fn generate_contacts<'a>( * Point-Face cases. * */ - let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, &segment2, &pos12); + let sep1 = + sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, &segment2, &pos12); if sep1.0 > capsule2.radius + prediction_distance { manifold.points.clear(); return; @@ -84,8 +86,7 @@ pub fn generate_contacts<'a>( #[cfg(feature = "dim2")] let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. #[cfg(feature = "dim3")] - let sep3 = - sat::cube_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12, &pos21); + let sep3 = sat::cuboid_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12); if sep3.0 > capsule2.radius + prediction_distance { manifold.points.clear(); return; @@ -118,20 +119,20 @@ pub fn generate_contacts<'a>( { if swapped_reference { feature1 = CuboidFeatureFace::from(segment2); - feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1); + feature2 = cube1.support_face(pos21 * -best_sep.1); } else { - feature1 = cuboid::support_face(cube1, best_sep.1); + feature1 = cube1.support_face(best_sep.1); feature2 = CuboidFeatureFace::from(segment2); } } #[cfg(feature = "dim3")] { if swapped_reference { - feature1 = PolyhedronFace::from(segment2); - feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1); + feature1 = PolyhedronFeature::from(segment2); + feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1); } else { - feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1); - feature2 = PolyhedronFace::from(segment2); + feature1 = cube1.polyhedron_support_face(best_sep.1); + feature2 = PolyhedronFeature::from(segment2); } } @@ -156,7 +157,7 @@ pub fn generate_contacts<'a>( manifold, ); #[cfg(feature = "dim3")] - PolyhedronFace::contacts( + PolyhedronFeature::contacts( prediction_distance + capsule2.radius, &feature1, &best_sep.1, @@ -185,5 +186,5 @@ pub fn generate_contacts<'a>( } // Transfer impulses. - super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference); + manifold.match_contacts(&old_manifold_points, swapped ^ swapped_reference); } diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index 5be5af3..1d750ef 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -1,9 +1,10 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory}; +use crate::geometry::{ContactManifold, ContactManifoldData, KinematicsCategory}; use crate::math::Isometry; #[cfg(feature = "dim2")] use crate::math::Vector; -use ncollide::shape::Cuboid; +use buckler::query::sat; +use buckler::shape::{Cuboid, CuboidFeature}; pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) { if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { @@ -19,15 +20,15 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont unreachable!() } - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); ctxt.manifold.sort_contacts(ctxt.prediction_distance); } pub fn generate_contacts<'a>( prediction_distance: f32, - mut cube1: &'a Cuboid, + mut cube1: &'a Cuboid, mut pos1: &'a Isometry, - mut cube2: &'a Cuboid, + mut cube2: &'a Cuboid, mut pos2: &'a Isometry, manifold: &mut ContactManifold, ) { @@ -43,13 +44,13 @@ pub fn generate_contacts<'a>( * Point-Face * */ - let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21); + let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12); if sep1.0 > prediction_distance { manifold.points.clear(); return; } - let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12); + let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21); if sep2.0 > prediction_distance { manifold.points.clear(); return; @@ -63,7 +64,7 @@ pub fn generate_contacts<'a>( #[cfg(feature = "dim2")] let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. #[cfg(feature = "dim3")] - let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21); + let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12); if sep3.0 > prediction_distance { manifold.points.clear(); return; @@ -97,8 +98,8 @@ pub fn generate_contacts<'a>( // Now the reference feature is from `cube1` and the best separation is `best_sep`. // Everything must be expressed in the local-space of `cube1` for contact clipping. - let feature1 = cuboid::support_feature(cube1, best_sep.1); - let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1); + let feature1 = cube1.support_feature(best_sep.1); + let mut feature2 = cube2.support_feature(pos21 * -best_sep.1); feature2.transform_by(&pos12); match (&feature1, &feature2) { @@ -151,5 +152,5 @@ pub fn generate_contacts<'a>( manifold.kinematics.radius2 = 0.0; // Transfer impulses. - super::match_contacts(manifold, &old_manifold_points, swapped); + manifold.match_contacts(&old_manifold_points, swapped); } diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 562d7d6..06dc6f0 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -1,13 +1,11 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -#[cfg(feature = "dim3")] -use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Triangle}; +use crate::geometry::{ContactManifold, ContactManifoldData, Cuboid, KinematicsCategory, Triangle}; use crate::math::Isometry; #[cfg(feature = "dim2")] -use crate::{ - geometry::{triangle, CuboidFeature}, - math::Vector, -}; +use crate::{buckler::shape::CuboidFeature, geometry::triangle, math::Vector}; +use buckler::query::sat; +#[cfg(feature = "dim3")] +use buckler::shape::PolyhedronFeature; pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) { if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { @@ -20,7 +18,7 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo ctxt.manifold, false, ); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } else if let (Some(triangle1), Some(cube2)) = (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) { @@ -33,7 +31,7 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo ctxt.manifold, true, ); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); } ctxt.manifold.sort_contacts(ctxt.prediction_distance); } @@ -61,7 +59,8 @@ pub fn generate_contacts<'a>( * Point-Face cases. * */ - let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12); + let sep1 = + sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12); if sep1.0 > prediction_distance { manifold.points.clear(); return; @@ -81,8 +80,7 @@ pub fn generate_contacts<'a>( #[cfg(feature = "dim2")] let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. #[cfg(feature = "dim3")] - let sep3 = - sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21); + let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12); if sep3.0 > prediction_distance { manifold.points.clear(); return; @@ -115,20 +113,20 @@ pub fn generate_contacts<'a>( { if swapped_reference { feature1 = triangle::support_face(triangle2, best_sep.1); - feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1); + feature2 = cube1.support_face(pos21 * -best_sep.1); } else { - feature1 = cuboid::support_face(cube1, best_sep.1); + feature1 = cube1.support_face(best_sep.1); feature2 = triangle::support_face(triangle2, pos21 * -best_sep.1); } } #[cfg(feature = "dim3")] { if swapped_reference { - feature1 = PolyhedronFace::from(*triangle2); - feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1); + feature1 = PolyhedronFeature::from(*triangle2); + feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1); } else { - feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1); - feature2 = PolyhedronFace::from(*triangle2); + feature1 = cube1.polyhedron_support_face(best_sep.1); + feature2 = PolyhedronFeature::from(*triangle2); } } @@ -153,7 +151,7 @@ pub fn generate_contacts<'a>( manifold, ); #[cfg(feature = "dim3")] - PolyhedronFace::contacts( + PolyhedronFeature::contacts( prediction_distance, &feature1, &best_sep.1, @@ -169,5 +167,5 @@ pub fn generate_contacts<'a>( manifold.kinematics.radius2 = 0.0; // Transfer impulses. - super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference); + manifold.match_contacts(&old_manifold_points, swapped ^ swapped_reference); } diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 70360c9..358ac84 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -1,3 +1,4 @@ +use crate::buckler::bounding_volume::BoundingVolume; use crate::data::hashmap::{Entry, HashMap}; use crate::data::MaybeSerializableData; use crate::geometry::contact_generator::{ @@ -6,8 +7,7 @@ use crate::geometry::contact_generator::{ }; #[cfg(feature = "dim2")] use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, HeightField, Shape}; -use crate::ncollide::bounding_volume::BoundingVolume; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData, HeightField, Shape}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; @@ -88,7 +88,7 @@ fn do_generate_contacts( let solver_flags = ctxt.solver_flags; let shape_type2 = collider2.shape().shape_type(); - heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| { + heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1| { let position1 = collider1.position(); #[cfg(feature = "dim2")] let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead. @@ -113,15 +113,13 @@ fn do_generate_contacts( timestamp: new_timestamp, workspace: workspace2, }; - let manifold = ContactManifold::with_subshape_indices( + let manifold_data = ContactManifoldData::from_colliders( coll_pair, collider1, collider2, - i, - 0, solver_flags, ); - manifolds.push(manifold); + manifolds.push(ContactManifold::with_data((i, 0), manifold_data)); entry.insert(sub_detector) } @@ -142,7 +140,7 @@ fn do_generate_contacts( let manifold = &mut manifolds[sub_detector.manifold_id]; - let mut ctxt2 = if coll_pair.collider1 != manifold.pair.collider1 { + let mut ctxt2 = if coll_pair.collider1 != manifold.data.pair.collider1 { PrimitiveContactGenerationContext { prediction_distance, collider1: collider2, diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index 9b14711..f539d7a 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -28,10 +28,6 @@ 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_with_normal; - pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag; mod ball_ball_contact_generator; @@ -53,29 +49,3 @@ mod serializable_workspace_tag; mod trimesh_shape_contact_generator; use crate::geometry::{Contact, ContactManifold}; - -pub(crate) fn match_contacts( - manifold: &mut ContactManifold, - old_contacts: &[Contact], - swapped: bool, -) { - for contact in &mut manifold.points { - if !swapped { - for old_contact in old_contacts { - if contact.fid1 == old_contact.fid1 && contact.fid2 == old_contact.fid2 { - // Transfer impulse cache. - contact.impulse = old_contact.impulse; - contact.tangent_impulse = old_contact.tangent_impulse; - } - } - } else { - for old_contact in old_contacts { - if contact.fid1 == old_contact.fid2 && contact.fid2 == old_contact.fid1 { - // Transfer impulse cache. - contact.impulse = old_contact.impulse; - contact.tangent_impulse = old_contact.tangent_impulse; - } - } - } - } -} diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index 5bc213e..721d5dc 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -1,12 +1,15 @@ use crate::data::MaybeSerializableData; use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{KinematicsCategory, PolygonalFeatureMap, PolyhedronFace}; +use crate::geometry::{ContactManifoldData, KinematicsCategory}; use crate::math::{Isometry, Vector}; +use buckler::query::{ + self, + gjk::{GJKResult, VoronoiSimplex}, +}; +use buckler::shape::{PolygonalFeatureMap, PolyhedronFeature}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; use na::Unit; -use ncollide::query; -use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] @@ -15,12 +18,12 @@ pub struct PfmPfmContactManifoldGeneratorWorkspace { feature = "serde-serialize", serde(skip, default = "VoronoiSimplex::new") )] - simplex: VoronoiSimplex, + simplex: VoronoiSimplex, last_gjk_dir: Option>>, #[cfg_attr(feature = "serde-serialize", serde(skip))] - feature1: PolyhedronFace, + feature1: PolyhedronFeature, #[cfg_attr(feature = "serde-serialize", serde(skip))] - feature2: PolyhedronFace, + feature2: PolyhedronFeature, } impl Default for PfmPfmContactManifoldGeneratorWorkspace { @@ -28,8 +31,8 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace { Self { simplex: VoronoiSimplex::new(), last_gjk_dir: None, - feature1: PolyhedronFace::new(), - feature2: PolyhedronFace::new(), + feature1: PolyhedronFeature::new(), + feature2: PolyhedronFeature::new(), } } } @@ -40,7 +43,7 @@ pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { ctxt.shape2.as_polygonal_feature_map(), ) { do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt); - ctxt.manifold.update_warmstart_multiplier(); + ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); ctxt.manifold.sort_contacts(ctxt.prediction_distance); } } @@ -73,10 +76,9 @@ fn do_generate_contacts( .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace."); let total_prediction = ctxt.prediction_distance + border_radius1 + border_radius2; - let contact = query::contact_support_map_support_map_with_params( - &Isometry::identity(), - pfm1, + let contact = query::contact::contact_support_map_support_map_with_params( &pos12, + pfm1, pfm2, total_prediction, &mut workspace.simplex, @@ -95,7 +97,7 @@ fn do_generate_contacts( pfm2.local_support_feature(&normal2, &mut workspace.feature2); workspace.feature2.transform_by(&pos12); - PolyhedronFace::contacts( + PolyhedronFeature::contacts( total_prediction, &workspace.feature1, &normal1, @@ -126,7 +128,7 @@ fn do_generate_contacts( } // Transfer impulses. - super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false); + ctxt.manifold.match_contacts(&old_manifold_points, false); } impl MaybeSerializableData for PfmPfmContactManifoldGeneratorWorkspace { diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 0e7543d..7e80fe7 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -1,10 +1,13 @@ #![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::geometry::{ + sat, Contact, ContactData, ContactManifold, ContactManifoldData, KinematicsCategory, Polygon, +}; use crate::math::{Isometry, Point}; #[cfg(feature = "dim2")] use crate::{math::Vector, utils}; +use buckler::query; pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationContext) { unimplemented!() @@ -16,7 +19,7 @@ pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationC // &ctxt.position2, // ctxt.manifold, // ); - // ctxt.manifold.update_warmstart_multiplier(); + // ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); // } else { // unreachable!() // } @@ -75,12 +78,12 @@ fn generate_contacts<'a>( m12 * p2.vertices[support_face2], m12 * p2.vertices[(support_face2 + 1) % len2], ); - if let Some((clip_a, clip_b)) = clip_segments(seg1, seg2) { + if let Some((clip_a, clip_b)) = query::details::clip_segment_segment(seg1, seg2) { let dist_a = (clip_a.1 - clip_a.0).dot(&local_n1); let dist_b = (clip_b.1 - clip_b.0).dot(&local_n1); - let mut impulses_a = (0.0, Contact::zero_tangent_impulse()); - let mut impulses_b = (0.0, Contact::zero_tangent_impulse()); + let mut data_a = ContactData::default(); + let mut data_b = ContactData::default(); let fids_a = ( ((support_face1 * 2 + clip_a.2) % (len1 * 2)) as u8, @@ -111,26 +114,15 @@ fn generate_contacts<'a>( } if fids_a == original_fids_a { - impulses_a = ( - manifold.points[0].impulse, - manifold.points[0].tangent_impulse, - ); + data_a = manifold.points[0].data; } else if fids_a == original_fids_b { - impulses_a = ( - manifold.points[1].impulse, - manifold.points[1].tangent_impulse, - ); + data_a = manifold.points[1].data; } + if fids_b == original_fids_a { - impulses_b = ( - manifold.points[0].impulse, - manifold.points[0].tangent_impulse, - ); + data_b = manifold.points[0].data; } else if fids_b == original_fids_b { - impulses_b = ( - manifold.points[1].impulse, - manifold.points[1].tangent_impulse, - ); + data_b = manifold.points[1].data; } } @@ -138,21 +130,19 @@ fn generate_contacts<'a>( manifold.points.push(Contact { local_p1: clip_a.0, local_p2: m21 * clip_a.1, - impulse: impulses_a.0, - tangent_impulse: impulses_a.1, fid1: fids_a.0, fid2: fids_a.1, dist: dist_a, + data: data_a, }); manifold.points.push(Contact { local_p1: clip_b.0, local_p2: m21 * clip_b.1, - impulse: impulses_b.0, - tangent_impulse: impulses_b.1, fid1: fids_b.0, fid2: fids_b.1, dist: dist_b, + data: data_b, }); manifold.local_n1 = local_n1; @@ -164,138 +154,3 @@ fn generate_contacts<'a>( manifold.points.clear(); } } - -// Features in clipping points are: -// 0 = First vertex. -// 1 = On the face. -// 2 = Second vertex. -pub(crate) type ClippingPoints = (Point, Point, usize, usize); - -#[cfg(feature = "dim2")] -pub(crate) fn clip_segments_with_normal( - mut seg1: (Point, Point), - mut seg2: (Point, Point), - normal: Vector, -) -> Option<(ClippingPoints, ClippingPoints)> { - use crate::utils::WBasis; - let tangent = normal.orthonormal_basis()[0]; - - let mut range1 = [seg1.0.coords.dot(&tangent), seg1.1.coords.dot(&tangent)]; - let mut range2 = [seg2.0.coords.dot(&tangent), seg2.1.coords.dot(&tangent)]; - let mut features1 = [0, 2]; - let mut features2 = [0, 2]; - - if range1[1] < range1[0] { - range1.swap(0, 1); - features1.swap(0, 1); - std::mem::swap(&mut seg1.0, &mut seg1.1); - } - - if range2[1] < range2[0] { - range2.swap(0, 1); - features2.swap(0, 1); - std::mem::swap(&mut seg2.0, &mut seg2.1); - } - - if range2[0] > range1[1] || range1[0] > range2[1] { - // No clip point. - return None; - } - - let ca = if range2[0] > range1[0] { - let bcoord = (range2[0] - range1[0]) * utils::inv(range1[1] - range1[0]); - let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord; - let p2 = seg2.0; - - (p1, p2, 1, features2[0]) - } else { - let bcoord = (range1[0] - range2[0]) * utils::inv(range2[1] - range2[0]); - let p1 = seg1.0; - let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; - - (p1, p2, features1[0], 1) - }; - - let cb = if range2[1] < range1[1] { - let bcoord = (range2[1] - range1[0]) * utils::inv(range1[1] - range1[0]); - let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord; - let p2 = seg2.1; - - (p1, p2, 1, features2[1]) - } else { - let bcoord = (range1[1] - range2[0]) * utils::inv(range2[1] - range2[0]); - let p1 = seg1.1; - let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; - - (p1, p2, features1[1], 1) - }; - - Some((ca, cb)) -} - -pub(crate) fn clip_segments( - mut seg1: (Point, Point), - mut seg2: (Point, Point), -) -> Option<(ClippingPoints, ClippingPoints)> { - // NOTE: no need to normalize the tangent. - let tangent1 = seg1.1 - seg1.0; - let sqnorm_tangent1 = tangent1.norm_squared(); - - let mut range1 = [0.0, sqnorm_tangent1]; - let mut range2 = [ - (seg2.0 - seg1.0).dot(&tangent1), - (seg2.1 - seg1.0).dot(&tangent1), - ]; - let mut features1 = [0, 2]; - let mut features2 = [0, 2]; - - if range1[1] < range1[0] { - range1.swap(0, 1); - features1.swap(0, 1); - std::mem::swap(&mut seg1.0, &mut seg1.1); - } - - if range2[1] < range2[0] { - range2.swap(0, 1); - features2.swap(0, 1); - std::mem::swap(&mut seg2.0, &mut seg2.1); - } - - if range2[0] > range1[1] || range1[0] > range2[1] { - // No clip point. - return None; - } - - let length1 = range1[1] - range1[0]; - let length2 = range2[1] - range2[0]; - - let ca = if range2[0] > range1[0] { - let bcoord = (range2[0] - range1[0]) / length1; - let p1 = seg1.0 + tangent1 * bcoord; - let p2 = seg2.0; - - (p1, p2, 1, features2[0]) - } else { - let bcoord = (range1[0] - range2[0]) / length2; - let p1 = seg1.0; - let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; - - (p1, p2, features1[0], 1) - }; - - let cb = if range2[1] < range1[1] { - let bcoord = (range2[1] - range1[0]) / length1; - let p1 = seg1.0 + tangent1 * bcoord; - let p2 = seg2.1; - - (p1, p2, 1, features2[1]) - } else { - let bcoord = (range1[1] - range2[0]) / length2; - let p1 = seg1.1; - let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; - - (p1, p2, features1[1], 1) - }; - - Some((ca, cb)) -} diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 3f7d7cd..172069d 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -1,9 +1,9 @@ +use crate::buckler::bounding_volume::{BoundingVolume, AABB}; use crate::data::MaybeSerializableData; use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, ShapeType, Trimesh}; -use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, Trimesh}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; @@ -11,7 +11,7 @@ use erased_serde::Serialize; #[derive(Clone)] pub struct TrimeshShapeContactGeneratorWorkspace { interferences: Vec, - local_aabb2: AABB, + local_aabb2: AABB, old_interferences: Vec, #[cfg_attr(feature = "serde-serialize", serde(skip))] old_manifolds: Vec, @@ -96,7 +96,7 @@ fn do_generate_contacts( .old_manifolds .iter() .map(|manifold| { - if manifold.pair.collider1 == ctxt_collider1 { + if manifold.data.pair.collider1 == ctxt_collider1 { manifold.subshape_index_pair.0 } else { manifold.subshape_index_pair.1 @@ -153,14 +153,14 @@ fn do_generate_contacts( let manifold = if old_inter_it.peek() != Some(triangle_id) { // We don't have a manifold for this triangle yet. - ContactManifold::with_subshape_indices( + let data = ContactManifoldData::from_colliders( ctxt_pair_pair, collider1, collider2, - *triangle_id, - 0, ctxt.solver_flags, - ) + ); + + ContactManifold::with_data((*triangle_id, 0), data) } else { // We already have a manifold for this triangle. old_inter_it.next(); @@ -176,7 +176,7 @@ fn do_generate_contacts( .dispatcher .dispatch_primitives(ShapeType::Triangle, shape_type2); - let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 { + let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.data.pair.collider1 { PrimitiveContactGenerationContext { prediction_distance: ctxt.prediction_distance, collider1: collider2, -- cgit From cc6d1b973002b4d366bc81ec6bf9e8240ad7b404 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 14 Dec 2020 15:51:43 +0100 Subject: Outsource the Shape trait, wquadtree, and shape types. --- .../ball_ball_contact_generator.rs | 8 +++--- .../contact_generator/contact_dispatcher.rs | 8 +++--- .../contact_generator/contact_generator.rs | 8 +++--- .../contact_generator_workspace.rs | 6 ++-- .../heightfield_shape_contact_generator.rs | 9 +++--- src/geometry/contact_generator/mod.rs | 2 +- .../serializable_workspace_tag.rs | 2 +- .../trimesh_shape_contact_generator.rs | 32 +++++++++++----------- 8 files changed, 38 insertions(+), 37 deletions(-) (limited to 'src/geometry/contact_generator') diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs index 7122998..f2ca7af 100644 --- a/src/geometry/contact_generator/ball_ball_contact_generator.rs +++ b/src/geometry/contact_generator/ball_ball_contact_generator.rs @@ -5,12 +5,12 @@ use crate::math::{Point, Vector}; use { crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd, crate::geometry::{WBall, WContact}, - crate::math::{Isometry, SimdFloat, SIMD_WIDTH}, + crate::math::{Isometry, SimdReal, SIMD_WIDTH}, simba::simd::SimdValue, }; #[cfg(feature = "simd-is-enabled")] -fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { +fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { let dcenter = ball2.center - ball1.center; let center_dist = dcenter.magnitude(); let normal = dcenter / center_dist; @@ -30,9 +30,9 @@ fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry (ContactPhase, Option) { match (shape1, shape2) { - (ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => ( + (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_trimesh_shape, ..ContactGenerator::default() }), Some(ContactGeneratorWorkspace::from( - TrimeshShapeContactGeneratorWorkspace::new(), + TriMeshShapeContactGeneratorWorkspace::new(), )), ), (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => ( diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index b1b1be6..06ab265 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -1,11 +1,11 @@ use crate::data::MaybeSerializableData; use crate::geometry::{ Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape, - SolverFlags, + ShapeType, SolverFlags, }; use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] -use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::math::{SimdReal, SIMD_WIDTH}; use crate::pipeline::EventHandler; #[derive(Copy, Clone)] @@ -158,8 +158,8 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> { pub colliders2: [&'a Collider; 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 positions1: &'a Isometry, + pub positions2: &'a Isometry, pub manifolds: &'a mut [&'b mut ContactManifold], pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>], } diff --git a/src/geometry/contact_generator/contact_generator_workspace.rs b/src/geometry/contact_generator/contact_generator_workspace.rs index e89395f..7aac592 100644 --- a/src/geometry/contact_generator/contact_generator_workspace.rs +++ b/src/geometry/contact_generator/contact_generator_workspace.rs @@ -2,7 +2,7 @@ use crate::data::MaybeSerializableData; #[cfg(feature = "dim3")] use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace; use crate::geometry::contact_generator::{ - HeightFieldShapeContactGeneratorWorkspace, TrimeshShapeContactGeneratorWorkspace, + HeightFieldShapeContactGeneratorWorkspace, TriMeshShapeContactGeneratorWorkspace, WorkspaceSerializationTag, }; @@ -81,8 +81,8 @@ impl<'de> serde::Deserialize<'de> for ContactGeneratorWorkspace { Some(WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace) => { deser::(&mut seq)? } - Some(WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace) => { - deser::(&mut seq)? + Some(WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace) => { + deser::(&mut seq)? } #[cfg(feature = "dim3")] Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => { diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 358ac84..125ac34 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -5,9 +5,10 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, ContactGeneratorWorkspace, PrimitiveContactGenerationContext, PrimitiveContactGenerator, }; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData}; #[cfg(feature = "dim2")] -use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, ContactManifoldData, HeightField, Shape}; +use buckler::shape::Capsule; +use buckler::shape::{HeightField, Shape}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; @@ -95,7 +96,7 @@ fn do_generate_contacts( #[cfg(feature = "dim3")] let sub_shape1 = *part1; - let sub_detector = match workspace.sub_detectors.entry(i) { + let sub_detector = match workspace.sub_detectors.entry(i as usize) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); let manifold = workspace.old_manifolds[sub_detector.manifold_id].take(); @@ -119,7 +120,7 @@ fn do_generate_contacts( collider2, solver_flags, ); - manifolds.push(ContactManifold::with_data((i, 0), manifold_data)); + manifolds.push(ContactManifold::with_data((i as usize, 0), manifold_data)); entry.insert(sub_detector) } diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index f539d7a..4c0716a 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -25,7 +25,7 @@ pub use self::pfm_pfm_contact_generator::{ // pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; pub use self::contact_generator_workspace::ContactGeneratorWorkspace; pub use self::trimesh_shape_contact_generator::{ - generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, + generate_contacts_trimesh_shape, TriMeshShapeContactGeneratorWorkspace, }; pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag; diff --git a/src/geometry/contact_generator/serializable_workspace_tag.rs b/src/geometry/contact_generator/serializable_workspace_tag.rs index 2488c1e..4bdb902 100644 --- a/src/geometry/contact_generator/serializable_workspace_tag.rs +++ b/src/geometry/contact_generator/serializable_workspace_tag.rs @@ -2,7 +2,7 @@ use num_derive::FromPrimitive; #[derive(Copy, Clone, Debug, FromPrimitive)] pub(super) enum WorkspaceSerializationTag { - TrimeshShapeContactGeneratorWorkspace = 0, + TriMeshShapeContactGeneratorWorkspace = 0, #[cfg(feature = "dim3")] PfmPfmContactGeneratorWorkspace, HeightfieldShapeContactGeneratorWorkspace, diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 172069d..209ac42 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -3,21 +3,21 @@ use crate::data::MaybeSerializableData; use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, Trimesh}; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, TriMesh}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] -pub struct TrimeshShapeContactGeneratorWorkspace { - interferences: Vec, +pub struct TriMeshShapeContactGeneratorWorkspace { + interferences: Vec, local_aabb2: AABB, - old_interferences: Vec, + old_interferences: Vec, #[cfg_attr(feature = "serde-serialize", serde(skip))] old_manifolds: Vec, } -impl TrimeshShapeContactGeneratorWorkspace { +impl TriMeshShapeContactGeneratorWorkspace { pub fn new() -> Self { Self { interferences: Vec::new(), @@ -40,7 +40,7 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) { } fn do_generate_contacts( - trimesh1: &Trimesh, + trimesh1: &TriMesh, collider1: &Collider, collider2: &Collider, ctxt: &mut ContactGenerationContext, @@ -52,14 +52,14 @@ fn do_generate_contacts( ctxt.pair.pair }; - let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt + let workspace: &mut TriMeshShapeContactGeneratorWorkspace = ctxt .pair .generator_workspace .as_mut() - .expect("The TrimeshShapeContactGeneratorWorkspace is missing.") + .expect("The TriMeshShapeContactGeneratorWorkspace is missing.") .0 .downcast_mut() - .expect("Invalid workspace type, expected a TrimeshShapeContactGeneratorWorkspace."); + .expect("Invalid workspace type, expected a TriMeshShapeContactGeneratorWorkspace."); /* * Compute interferences. @@ -97,9 +97,9 @@ fn do_generate_contacts( .iter() .map(|manifold| { if manifold.data.pair.collider1 == ctxt_collider1 { - manifold.subshape_index_pair.0 + manifold.subshape_index_pair.0 as u32 } else { - manifold.subshape_index_pair.1 + manifold.subshape_index_pair.1 as u32 } }) .collect(); @@ -118,7 +118,7 @@ fn do_generate_contacts( workspace.interferences.clear(); trimesh1 - .waabbs() + .quadtree() .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } @@ -134,7 +134,7 @@ fn do_generate_contacts( // TODO: don't redispatch at each frame (we should probably do the same as // the heightfield). for (i, triangle_id) in new_interferences.iter().enumerate() { - if *triangle_id >= trimesh1.num_triangles() { + if *triangle_id >= trimesh1.num_triangles() as u32 { // Because of SIMD padding, the broad-phase may return tiangle indices greater // than the max. continue; @@ -160,7 +160,7 @@ fn do_generate_contacts( ctxt.solver_flags, ); - ContactManifold::with_data((*triangle_id, 0), data) + ContactManifold::with_data((*triangle_id as usize, 0), data) } else { // We already have a manifold for this triangle. old_inter_it.next(); @@ -206,11 +206,11 @@ fn do_generate_contacts( } } -impl MaybeSerializableData for TrimeshShapeContactGeneratorWorkspace { +impl MaybeSerializableData for TriMeshShapeContactGeneratorWorkspace { #[cfg(feature = "serde-serialize")] fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> { Some(( - super::WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace as u32, + super::WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace as u32, self, )) } -- cgit From e231bacec608fa5efd24f7a876572927dbd6c9c4 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Thu, 17 Dec 2020 10:24:36 +0100 Subject: Move all the contact manifold computations out of Rapier. --- .../ball_ball_contact_generator.rs | 103 ---------- .../ball_convex_contact_generator.rs | 70 ------- .../ball_polygon_contact_generator.rs | 1 - .../capsule_capsule_contact_generator.rs | 202 ------------------ .../contact_generator/contact_dispatcher.rs | 141 ------------- .../contact_generator/contact_generator.rs | 228 --------------------- .../contact_generator_workspace.rs | 104 ---------- .../cuboid_capsule_contact_generator.rs | 190 ----------------- .../cuboid_cuboid_contact_generator.rs | 156 -------------- .../cuboid_polygon_contact_generator.rs | 1 - .../cuboid_triangle_contact_generator.rs | 171 ---------------- .../heightfield_shape_contact_generator.rs | 190 ----------------- src/geometry/contact_generator/mod.rs | 51 ----- .../contact_generator/pfm_pfm_contact_generator.rs | 146 ------------- .../polygon_polygon_contact_generator.rs | 156 -------------- .../serializable_workspace_tag.rs | 9 - .../trimesh_shape_contact_generator.rs | 221 -------------------- .../voxels_shape_contact_generator.rs | 0 18 files changed, 2140 deletions(-) delete mode 100644 src/geometry/contact_generator/ball_ball_contact_generator.rs delete mode 100644 src/geometry/contact_generator/ball_convex_contact_generator.rs delete mode 100644 src/geometry/contact_generator/ball_polygon_contact_generator.rs delete mode 100644 src/geometry/contact_generator/capsule_capsule_contact_generator.rs delete mode 100644 src/geometry/contact_generator/contact_dispatcher.rs delete mode 100644 src/geometry/contact_generator/contact_generator.rs delete mode 100644 src/geometry/contact_generator/contact_generator_workspace.rs delete mode 100644 src/geometry/contact_generator/cuboid_capsule_contact_generator.rs delete mode 100644 src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs delete mode 100644 src/geometry/contact_generator/cuboid_polygon_contact_generator.rs delete mode 100644 src/geometry/contact_generator/cuboid_triangle_contact_generator.rs delete mode 100644 src/geometry/contact_generator/heightfield_shape_contact_generator.rs delete mode 100644 src/geometry/contact_generator/mod.rs delete mode 100644 src/geometry/contact_generator/pfm_pfm_contact_generator.rs delete mode 100644 src/geometry/contact_generator/polygon_polygon_contact_generator.rs delete mode 100644 src/geometry/contact_generator/serializable_workspace_tag.rs delete mode 100644 src/geometry/contact_generator/trimesh_shape_contact_generator.rs delete mode 100644 src/geometry/contact_generator/voxels_shape_contact_generator.rs (limited to 'src/geometry/contact_generator') diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs deleted file mode 100644 index f2ca7af..0000000 --- a/src/geometry/contact_generator/ball_ball_contact_generator.rs +++ /dev/null @@ -1,103 +0,0 @@ -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Contact, ContactManifoldData, KinematicsCategory}; -use crate::math::{Point, Vector}; -#[cfg(feature = "simd-is-enabled")] -use { - crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd, - crate::geometry::{WBall, WContact}, - crate::math::{Isometry, SimdReal, SIMD_WIDTH}, - simba::simd::SimdValue, -}; - -#[cfg(feature = "simd-is-enabled")] -fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { - let dcenter = ball2.center - ball1.center; - let center_dist = dcenter.magnitude(); - let normal = dcenter / center_dist; - - WContact { - local_p1: ball1.center + normal * ball1.radius, - local_p2: pos21 * (ball2.center - normal * ball2.radius), - local_n1: normal, - local_n2: pos21 * -normal, - fid1: [0; SIMD_WIDTH], - fid2: [0; SIMD_WIDTH], - dist: center_dist - ball1.radius - ball2.radius, - } -} - -#[cfg(feature = "simd-is-enabled")] -pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) { - let pos_ba = ctxt.positions2.inverse() * ctxt.positions1; - let radii_a = - SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); - let radii_b = - SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); - - let wball_a = WBall::new(Point::origin(), radii_a); - let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b); - let contacts = generate_contacts_simd(&wball_a, &wball_b, &pos_ba); - - for (i, manifold) in ctxt.manifolds.iter_mut().enumerate() { - // FIXME: compare the dist before extracting the contact. - let (contact, local_n1, local_n2) = contacts.extract(i); - if contact.dist <= ctxt.prediction_distance { - if manifold.points.len() != 0 { - manifold.points[0].copy_geometry_from(contact); - } else { - manifold.points.push(contact); - } - - manifold.local_n1 = local_n1; - manifold.local_n2 = local_n2; - manifold.kinematics.category = KinematicsCategory::PointPoint; - manifold.kinematics.radius1 = radii_a.extract(i); - manifold.kinematics.radius2 = radii_b.extract(i); - ContactManifoldData::update_warmstart_multiplier(manifold); - } else { - manifold.points.clear(); - } - - manifold.sort_contacts(ctxt.prediction_distance); - } -} - -pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) { - let pos_ba = ctxt.position2.inverse() * ctxt.position1; - let radius_a = ctxt.shape1.as_ball().unwrap().radius; - let radius_b = ctxt.shape2.as_ball().unwrap().radius; - - let dcenter = pos_ba.inverse_transform_point(&Point::origin()).coords; - let center_dist = dcenter.magnitude(); - let dist = center_dist - radius_a - radius_b; - - if dist < ctxt.prediction_distance { - let local_n1 = if center_dist != 0.0 { - dcenter / center_dist - } else { - Vector::y() - }; - - let local_n2 = pos_ba.inverse_transform_vector(&-local_n1); - let local_p1 = local_n1 * radius_a; - let local_p2 = local_n2 * radius_b; - let contact = Contact::new(local_p1.into(), local_p2.into(), 0, 0, dist); - - if ctxt.manifold.points.len() != 0 { - ctxt.manifold.points[0].copy_geometry_from(contact); - } else { - ctxt.manifold.points.push(contact); - } - - ctxt.manifold.local_n1 = local_n1; - ctxt.manifold.local_n2 = local_n2; - ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint; - ctxt.manifold.kinematics.radius1 = radius_a; - ctxt.manifold.kinematics.radius2 = radius_b; - ContactManifoldData::update_warmstart_multiplier(ctxt.manifold); - } else { - ctxt.manifold.points.clear(); - } - - ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs deleted file mode 100644 index 88f1912..0000000 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ /dev/null @@ -1,70 +0,0 @@ -use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Ball, Contact, ContactManifoldData, KinematicsCategory}; -use crate::math::Isometry; -use buckler::query::PointQuery; -use na::Unit; - -pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { - if let Some(ball1) = ctxt.shape1.as_ball() { - ctxt.manifold.swap_identifiers(); - do_generate_contacts(ctxt.shape2, ball1, ctxt, true); - } else if let Some(ball2) = ctxt.shape2.as_ball() { - do_generate_contacts(ctxt.shape1, ball2, ctxt, false); - } - - ctxt.manifold.sort_contacts(ctxt.prediction_distance); -} - -fn do_generate_contacts( - point_query1: &P, - ball2: &Ball, - ctxt: &mut PrimitiveContactGenerationContext, - swapped: bool, -) { - let position1; - let position2; - - if swapped { - position1 = ctxt.position2; - position2 = ctxt.position1; - } else { - position1 = ctxt.position1; - position2 = ctxt.position2; - } - - let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into()); - let proj = point_query1.project_local_point(&local_p2_1, cfg!(feature = "dim3")); - let dpos = local_p2_1 - proj.local_point; - - #[allow(unused_mut)] // Because `mut local_n1, mut dist` is needed in 2D but not in 3D. - if let Some((mut local_n1, mut dist)) = Unit::try_new_and_get(dpos, 0.0) { - #[cfg(feature = "dim2")] - if proj.is_inside { - local_n1 = -local_n1; - dist = -dist; - } - - if dist <= ball2.radius + ctxt.prediction_distance { - let local_n2 = position2.inverse_transform_vector(&(position1 * -*local_n1)); - let local_p2 = (local_n2 * ball2.radius).into(); - let contact_point = Contact::new(proj.local_point, local_p2, 0, 0, dist - ball2.radius); - - if ctxt.manifold.points.len() != 1 { - ctxt.manifold.points.clear(); - ctxt.manifold.points.push(contact_point); - } else { - // Copy only the geometry so we keep the warmstart impulses. - ctxt.manifold.points[0].copy_geometry_from(contact_point); - } - - ctxt.manifold.local_n1 = *local_n1; -