diff options
Diffstat (limited to 'src/geometry/contact_generator')
11 files changed, 123 insertions, 299 deletions
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<P: ?Sized + PointQuery<f32>>( +fn do_generate_contacts<P: ?Sized + PointQuery>( point_query1: &P, ball2: &Ball, ctxt: &mut PrimitiveContactGenerationContext, @@ -33,12 +33,8 @@ fn do_generate_contacts<P: ?Sized + PointQuery<f32>>( } 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<P: ?Sized + PointQuery<f32>>( 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<P: ?Sized + PointQuery<f32>>( 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<f32>, + mut cube1: &'a Cuboid, mut pos1: &'a Isometry<f32>, - mut cube2: &'a Cuboid<f32>, + mut cube2: &'a Cuboid, mut pos2: &'a Isometry<f32>, 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<f32>, + simplex: VoronoiSimplex, last_gjk_dir: Option<Unit<Vector<f32>>>, #[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<f32>, Point<f32>, usize, usize); - -#[cfg(feature = "dim2")] -pub(crate) fn clip_segments_with_normal( - mut seg1: (Point<f32>, Point<f32>), - mut seg2: (Point<f32>, Point<f32>), - normal: Vector<f32>, -) -> 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; - } |