diff options
12 files changed, 54 insertions, 88 deletions
diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index bf976b5..0304338 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -236,8 +236,8 @@ impl ContactPair { // (This order can be modified by the contact determination algorithm). let manifold = &mut self.manifolds[0]; if manifold.pair.collider1 == self.pair.collider1 { - manifold.position1 = *coll1.position(); - manifold.position2 = *coll2.position(); + manifold.position1 = coll1.position; + manifold.position2 = coll2.position; ( coll1, coll2, @@ -245,8 +245,8 @@ impl ContactPair { self.generator_workspace.as_mut().map(|w| &mut *w.0), ) } else { - manifold.position1 = *coll2.position(); - manifold.position2 = *coll1.position(); + manifold.position1 = coll2.position; + manifold.position2 = coll1.position; ( coll2, coll1, diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs index 96ac235..b949842 100644 --- a/src/geometry/contact_generator/ball_ball_contact_generator.rs +++ b/src/geometry/contact_generator/ball_ball_contact_generator.rs @@ -28,7 +28,9 @@ fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry<SimdFlo #[cfg(feature = "simd-is-enabled")] pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) { - let pos_ba = ctxt.positions2.inverse() * ctxt.positions1; + let positions1 = Isometry::from(array![|ii| ctxt.manifolds[ii].position1; SIMD_WIDTH]); + let positions2 = Isometry::from(array![|ii| ctxt.manifolds[ii].position2; SIMD_WIDTH]); + let pos_ba = positions2.inverse() * positions1; let radii_a = SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); let radii_b = @@ -63,7 +65,7 @@ pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationCon } pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) { - let pos_ba = ctxt.position2.inverse() * ctxt.position1; + let pos_ba = ctxt.manifold.position2.inverse() * ctxt.manifold.position1; let radius_a = ctxt.shape1.as_ball().unwrap().radius; let radius_b = ctxt.shape2.as_ball().unwrap().radius; diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index 69bc5e3..55df093 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -25,11 +25,11 @@ fn do_generate_contacts<P: ?Sized + PointQuery<f32>>( let position2; if swapped { - position1 = ctxt.position2; - position2 = ctxt.position1; + position1 = ctxt.manifold.position2; + position2 = ctxt.manifold.position1; } else { - position1 = ctxt.position1; - position2 = ctxt.position2; + position1 = ctxt.manifold.position1; + position2 = ctxt.manifold.position2; } let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into()); diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index 3104496..4a30188 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -9,14 +9,7 @@ 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()) { - generate_contacts( - ctxt.prediction_distance, - capsule1, - ctxt.position1, - capsule2, - ctxt.position2, - ctxt.manifold, - ); + generate_contacts(ctxt.prediction_distance, capsule1, capsule2, ctxt.manifold); } ctxt.manifold.update_warmstart_multiplier(); @@ -27,16 +20,14 @@ pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationCo pub fn generate_contacts<'a>( prediction_distance: f32, capsule1: &'a Capsule, - pos1: &'a Isometry<f32>, capsule2: &'a Capsule, - pos2: &'a Isometry<f32>, manifold: &mut ContactManifold, ) { // FIXME: the contact kinematics is not correctly set here. // We use the common "Point-Plane" kinematics with zero radius everytime. // Instead we should select point/point ore point-plane (with non-zero // radius for the point) depending on the features involved in the contact. - let pos12 = pos1.inverse() * pos2; + let pos12 = manifold.position1.inverse() * manifold.position2; let pos21 = pos12.inverse(); let seg1 = capsule1.segment; @@ -146,12 +137,10 @@ pub fn generate_contacts<'a>( pub fn generate_contacts<'a>( prediction_distance: f32, capsule1: &'a Capsule, - pos1: &'a Isometry<f32>, capsule2: &'a Capsule, - pos2: &'a Isometry<f32>, manifold: &mut ContactManifold, ) { - let pos12 = pos1.inverse() * pos2; + let pos12 = manifold.position1.inverse() * manifold.position2; let pos21 = pos12.inverse(); let seg1 = capsule1.segment; diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index b1b1be6..6b53e0a 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -36,8 +36,6 @@ impl ContactPhase { collider2, shape1: collider1.shape(), shape2: collider2.shape(), - position1: collider1.position(), - position2: collider2.position(), manifold, workspace, }; @@ -107,12 +105,6 @@ impl ContactPhase { colliders2, shapes1: array![|ii| colliders1[ii].shape(); SIMD_WIDTH], shapes2: array![|ii| colliders2[ii].shape(); SIMD_WIDTH], - positions1: &Isometry::from( - array![|ii| *colliders1[ii].position(); SIMD_WIDTH], - ), - positions2: &Isometry::from( - array![|ii| *colliders2[ii].position(); SIMD_WIDTH], - ), manifolds: manifold_arr.as_mut_slice(), workspaces: workspace_arr.as_mut_slice(), }; @@ -145,8 +137,6 @@ pub struct PrimitiveContactGenerationContext<'a> { pub collider2: &'a Collider, pub shape1: &'a dyn Shape, pub shape2: &'a dyn Shape, - pub position1: &'a Isometry<f32>, - pub position2: &'a Isometry<f32>, pub manifold: &'a mut ContactManifold, pub workspace: Option<&'a mut (dyn MaybeSerializableData)>, } @@ -158,8 +148,6 @@ 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<SimdFloat>, - pub positions2: &'a Isometry<SimdFloat>, pub manifolds: &'a mut [&'b mut ContactManifold], pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>], } diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index 3fd4a17..95dc52b 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -12,9 +12,7 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon generate_contacts( ctxt.prediction_distance, cube1, - ctxt.position1, capsule2, - ctxt.position2, ctxt.manifold, false, ); @@ -25,9 +23,7 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon generate_contacts( ctxt.prediction_distance, cube2, - ctxt.position2, capsule1, - ctxt.position1, ctxt.manifold, true, ); @@ -39,19 +35,27 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon pub fn generate_contacts<'a>( prediction_distance: f32, cube1: &'a Cuboid, - mut pos1: &'a Isometry<f32>, capsule2: &'a Capsule, - mut pos2: &'a Isometry<f32>, manifold: &mut ContactManifold, swapped: bool, ) { - let mut pos12 = pos1.inverse() * pos2; - let mut pos21 = pos12.inverse(); + let mut pos12; + let mut pos21; - if (!swapped && manifold.try_update_contacts(&pos12)) - || (swapped && manifold.try_update_contacts(&pos21)) - { - return; + if !swapped { + pos12 = manifold.position1.inverse() * manifold.position2; + pos21 = pos12.inverse(); + + if manifold.try_update_contacts(&pos12) { + return; + } + } else { + pos12 = manifold.position2.inverse() * manifold.position1; + pos21 = pos12.inverse(); + + if manifold.try_update_contacts(&pos21) { + return; + } } let segment2 = capsule2.segment; @@ -97,16 +101,14 @@ pub fn generate_contacts<'a>( * and get the polygons to clip. * */ - let mut swapped_reference = false; + let mut swapped_reference = sep2.0 > sep1.0 && sep2.0 > sep3.0; let mut best_sep = sep1; - if sep2.0 > sep1.0 && sep2.0 > sep3.0 { + if swapped_reference { // The reference shape will be the second shape. // std::mem::swap(&mut cube1, &mut capsule2); - std::mem::swap(&mut pos1, &mut pos2); std::mem::swap(&mut pos12, &mut pos21); best_sep = sep2; - swapped_reference = true; } else if sep3.0 > sep1.0 { best_sep = sep3; } diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index 5be5af3..fe00a02 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -7,14 +7,7 @@ use ncollide::shape::Cuboid; pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) { if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { - generate_contacts( - ctxt.prediction_distance, - cube1, - ctxt.position1, - cube2, - ctxt.position2, - ctxt.manifold, - ); + generate_contacts(ctxt.prediction_distance, cube1, cube2, ctxt.manifold); } else { unreachable!() } @@ -26,12 +19,10 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont pub fn generate_contacts<'a>( prediction_distance: f32, mut cube1: &'a Cuboid<f32>, - mut pos1: &'a Isometry<f32>, mut cube2: &'a Cuboid<f32>, - mut pos2: &'a Isometry<f32>, manifold: &mut ContactManifold, ) { - let mut pos12 = pos1.inverse() * pos2; + let mut pos12 = manifold.position1.inverse() * manifold.position2; let mut pos21 = pos12.inverse(); if manifold.try_update_contacts(&pos12) { @@ -81,7 +72,6 @@ pub fn generate_contacts<'a>( if sep2.0 > sep1.0 && sep2.0 > sep3.0 { // The reference shape will be the second shape. std::mem::swap(&mut cube1, &mut cube2); - std::mem::swap(&mut pos1, &mut pos2); std::mem::swap(&mut pos12, &mut pos21); manifold.swap_identifiers(); best_sep = sep2; diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 562d7d6..44589e5 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -14,9 +14,7 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo generate_contacts( ctxt.prediction_distance, cube1, - ctxt.position1, triangle2, - ctxt.position2, ctxt.manifold, false, ); @@ -27,9 +25,7 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo generate_contacts( ctxt.prediction_distance, cube2, - ctxt.position2, triangle1, - ctxt.position1, ctxt.manifold, true, ); @@ -41,19 +37,27 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo pub fn generate_contacts<'a>( prediction_distance: f32, cube1: &'a Cuboid, - mut pos1: &'a Isometry<f32>, triangle2: &'a Triangle, - mut pos2: &'a Isometry<f32>, manifold: &mut ContactManifold, swapped: bool, ) { - let mut pos12 = pos1.inverse() * pos2; - let mut pos21 = pos12.inverse(); + let mut pos12; + let mut pos21; - if (!swapped && manifold.try_update_contacts(&pos12)) - || (swapped && manifold.try_update_contacts(&pos21)) - { - return; + if !swapped { + pos12 = manifold.position1.inverse() * manifold.position2; + pos21 = pos12.inverse(); + + if manifold.try_update_contacts(&pos12) { + return; + } + } else { + pos12 = manifold.position2.inverse() * manifold.position1; + pos21 = pos12.inverse(); + + if manifold.try_update_contacts(&pos21) { + return; + } } /* @@ -100,7 +104,6 @@ pub fn generate_contacts<'a>( if sep2.0 > sep1.0 && sep2.0 > sep3.0 { // The reference shape will be the second shape. // std::mem::swap(&mut cube1, &mut triangle2); - std::mem::swap(&mut pos1, &mut pos2); std::mem::swap(&mut pos12, &mut pos21); best_sep = sep2; swapped_reference = true; diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index f3790a1..b747ce2 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -151,8 +151,6 @@ fn do_generate_contacts( collider2: collider1, shape1: collider2.shape(), shape2: &sub_shape1, - position1: collider2.position(), - position2: position1, manifold, workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0), } @@ -165,8 +163,6 @@ fn do_generate_contacts( collider2, shape1: &sub_shape1, shape2: collider2.shape(), - position1, - position2: collider2.position(), manifold, workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0), } diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs index 5bc213e..e3ee1af 100644 --- a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -52,7 +52,7 @@ fn do_generate_contacts( border_radius2: f32, ctxt: &mut PrimitiveContactGenerationContext, ) { - let pos12 = ctxt.position1.inverse() * ctxt.position2; + let pos12 = ctxt.manifold.position1.inverse() * ctxt.manifold.position2; let pos21 = pos12.inverse(); // We use very small thresholds for the manifold update because something to high would diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 0e7543d..f9ab915 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -11,9 +11,9 @@ pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationC // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { // generate_contacts( // polygon1, - // &ctxt.position1, + // &ctxt.manifold.position1, // polygon2, - // &ctxt.position2, + // &ctxt.manifold.position2, // ctxt.manifold, // ); // ctxt.manifold.update_warmstart_multiplier(); diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 238c84d..7b1e825 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -185,8 +185,6 @@ fn do_generate_contacts( collider2: collider1, shape1: collider2.shape(), shape2: &triangle1, - position1: collider2.position(), - position2: collider1.position(), manifold, workspace: workspace2.as_mut().map(|w| &mut *w.0), } @@ -199,8 +197,6 @@ fn do_generate_contacts( collider2, shape1: &triangle1, shape2: collider2.shape(), - position1: collider1.position(), - position2: collider2.position(), manifold, workspace: workspace2.as_mut().map(|w| &mut *w.0), } |