From 9bf1321f8f1d2e116f44c2461a53f302c4ef4171 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 8 Dec 2020 17:31:49 +0100 Subject: [PATCH] Outsource the contact manifold, SAT, and some shapes. --- Cargo.toml | 2 + benchmarks3d/heightfield3.rs | 8 +- benchmarks3d/trimesh3.rs | 39 +- build/rapier2d/Cargo.toml | 4 +- build/rapier3d/Cargo.toml | 4 +- build/rapier_testbed2d/Cargo.toml | 1 + build/rapier_testbed3d/Cargo.toml | 1 + examples3d/trimesh3.rs | 42 +- src/dynamics/rigid_body_set.rs | 2 +- src/dynamics/solver/categorization.rs | 8 +- src/dynamics/solver/interaction_groups.rs | 12 +- .../solver/parallel_velocity_solver.rs | 4 +- src/dynamics/solver/position_constraint.rs | 12 +- .../solver/position_constraint_wide.rs | 12 +- .../solver/position_ground_constraint.rs | 16 +- .../solver/position_ground_constraint_wide.rs | 14 +- src/dynamics/solver/velocity_constraint.rs | 30 +- .../solver/velocity_constraint_wide.rs | 28 +- .../solver/velocity_ground_constraint.rs | 34 +- .../solver/velocity_ground_constraint_wide.rs | 28 +- src/geometry/broad_phase_multi_sap.rs | 12 +- src/geometry/capsule.rs | 192 ------- src/geometry/collider.rs | 6 +- src/geometry/contact.rs | 333 ++--------- .../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 - .../pfm_pfm_contact_generator.rs | 30 +- .../polygon_polygon_contact_generator.rs | 175 +----- .../trimesh_shape_contact_generator.rs | 18 +- src/geometry/cuboid.rs | 234 -------- src/geometry/cuboid_feature2d.rs | 128 ----- src/geometry/cuboid_feature3d.rs | 516 ------------------ src/geometry/mod.rs | 110 ++-- src/geometry/narrow_phase.rs | 7 +- src/geometry/polygon.rs | 4 +- src/geometry/polygonal_feature_map.rs | 132 ----- src/geometry/polyhedron_feature3d.rs | 445 --------------- .../ball_convex_proximity_detector.rs | 11 +- .../cuboid_cuboid_proximity_detector.rs | 17 +- .../cuboid_triangle_proximity_detector.rs | 8 +- .../trimesh_shape_proximity_detector.rs | 4 +- src/geometry/round_cylinder.rs | 49 +- src/geometry/sat.rs | 344 +----------- src/geometry/shape.rs | 49 +- src/geometry/triangle.rs | 4 +- src/geometry/trimesh.rs | 63 ++- src/geometry/waabb.rs | 12 +- src/geometry/wquadtree.rs | 2 +- src/lib.rs | 11 +- src/pipeline/query_pipeline.rs | 4 +- src_testbed/engine.rs | 2 +- src_testbed/lib.rs | 4 + src_testbed/nphysics_backend.rs | 7 +- src_testbed/objects/heightfield.rs | 17 +- src_testbed/objects/polyline.rs | 2 +- src_testbed/testbed.rs | 4 +- 62 files changed, 552 insertions(+), 2904 deletions(-) delete mode 100644 src/geometry/capsule.rs delete mode 100644 src/geometry/cuboid.rs delete mode 100644 src/geometry/cuboid_feature2d.rs delete mode 100644 src/geometry/cuboid_feature3d.rs delete mode 100644 src/geometry/polygonal_feature_map.rs delete mode 100644 src/geometry/polyhedron_feature3d.rs diff --git a/Cargo.toml b/Cargo.toml index cfb166a..55e9b43 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,6 +10,8 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark #nphysics2d = { path = "../nphysics/build/nphysics2d" } #nphysics3d = { path = "../nphysics/build/nphysics3d" } #kiss3d = { path = "../kiss3d" } +buckler2d = { path = "../buckler/build/buckler2d" } +buckler3d = { path = "../buckler/build/buckler3d" } [profile.release] #debug = true diff --git a/benchmarks3d/heightfield3.rs b/benchmarks3d/heightfield3.rs index 4f8211c..fc64856 100644 --- a/benchmarks3d/heightfield3.rs +++ b/benchmarks3d/heightfield3.rs @@ -1,4 +1,4 @@ -use na::{DMatrix, Point3, Vector3}; +use na::{ComplexField, DMatrix, Point3, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -23,7 +23,11 @@ pub fn init_world(testbed: &mut Testbed) { } else { let x = i as f32 * ground_size.x / (nsubdivs as f32); let z = j as f32 * ground_size.z / (nsubdivs as f32); - x.sin() + z.cos() + + // NOTE: make sure we use the sin/cos from simba to ensure + // cross-platform determinism of the example when the + // enhanced_determinism feature is enabled. + ::sin(x) + ::cos(z) } }); diff --git a/benchmarks3d/trimesh3.rs b/benchmarks3d/trimesh3.rs index ce1d1c5..e0701f1 100644 --- a/benchmarks3d/trimesh3.rs +++ b/benchmarks3d/trimesh3.rs @@ -1,6 +1,6 @@ -use na::Point3; +use na::{ComplexField, DMatrix, Point3, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; -use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField}; use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { @@ -14,28 +14,27 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground */ - let ground_size = 200.0f32; - let ground_height = 1.0; + let ground_size = Vector3::new(200.0, 1.0, 200.0); let nsubdivs = 20; - let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs); - let indices = quad - .flat_indices() - .chunks(3) - .map(|is| Point3::new(is[0], is[2], is[1])) - .collect(); - let mut vertices = quad.coords; + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs { + 10.0 + } else { + let x = i as f32 * ground_size.x / (nsubdivs as f32); + let z = j as f32 * ground_size.z / (nsubdivs as f32); - // ncollide generates a quad with `z` as the normal. - // so we switch z and y here and set a random altitude at each point. - for p in vertices.iter_mut() { - p.z = p.y; - p.y = (p.x.sin() + p.z.cos()) * ground_height; - - if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 { - p.y = 10.0; + // NOTE: make sure we use the sin/cos from simba to ensure + // cross-platform determinism of the example when the + // enhanced_determinism feature is enabled. + ::sin(x) + ::cos(z) } - } + }); + + // Here we will build our trimesh from the mesh representation of an + // heightfield. + let heightfield = HeightField::new(heights, ground_size); + let (vertices, indices) = heightfield.to_trimesh(); let rigid_body = RigidBodyBuilder::new_static().build(); let handle = bodies.insert(rigid_body); diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 40e2db4..987fa2f 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -21,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -35,7 +35,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.23" -ncollide2d = "0.26" +buckler2d = "0.1" simba = "0.3" approx = "0.4" rayon = { version = "1", optional = true } diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index b1a1026..d409035 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -21,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -35,7 +35,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.23" -ncollide3d = "0.26" +buckler3d = "0.1" simba = "0.3" approx = "0.4" rayon = { version = "1", optional = true } diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index 0b981e3..72ead41 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -31,6 +31,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" num_cpus = { version = "1", optional = true } wrapped2d = { version = "0.4", optional = true } +buckler2d = "0.1" ncollide2d = "0.26" nphysics2d = { version = "0.18", optional = true } crossbeam = "0.8" diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index 3019021..d4ad764 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -30,6 +30,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" glam = { version = "0.10", optional = true } num_cpus = { version = "1", optional = true } +buckler3d = "0.1" ncollide3d = "0.26" nphysics3d = { version = "0.18", optional = true } physx = { version = "0.8", optional = true } diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index 0cee465..d753988 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -1,6 +1,6 @@ -use na::{ComplexField, Point3}; +use na::{ComplexField, DMatrix, Point3, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; -use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField}; use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { @@ -14,31 +14,27 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground */ - let ground_size = 100.0f32; - let ground_height = 1.0; + let ground_size = Vector3::new(100.0, 1.0, 100.0); let nsubdivs = 20; - let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs); - let indices = quad - .flat_indices() - .chunks(3) - .map(|is| Point3::new(is[0], is[2], is[1])) - .collect(); - let mut vertices = quad.coords; + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs { + 10.0 + } else { + let x = i as f32 * ground_size.x / (nsubdivs as f32); + let z = j as f32 * ground_size.z / (nsubdivs as f32); - // ncollide generates a quad with `z` as the normal. - // so we switch z and y here and set a random altitude at each point. - for p in vertices.iter_mut() { - p.z = p.y; - // NOTE: make sure we use the sin/cos from simba to ensure - // cross-platform determinism of the example when the - // enhanced_determinism feature is enabled. - p.y = (::sin(p.x) + ::cos(p.z)) * ground_height; - - if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 { - p.y = 10.0; + // NOTE: make sure we use the sin/cos from simba to ensure + // cross-platform determinism of the example when the + // enhanced_determinism feature is enabled. + ::sin(x) + ::cos(z) } - } + }); + + // Here we will build our trimesh from the mesh representation of an + // heightfield. + let heightfield = HeightField::new(heights, ground_size); + let (vertices, indices) = heightfield.to_trimesh(); let rigid_body = RigidBodyBuilder::new_static().build(); let handle = bodies.insert(rigid_body); diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index b8667bc..c32c402 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -479,7 +479,7 @@ impl RigidBodySet { if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { for inter in contacts { for manifold in &inter.2.manifolds { - if manifold.num_active_contacts() > 0 { + if manifold.num_active_contacts > 0 { let other = crate::utils::other_handle( (inter.0, inter.1), *collider_handle, diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 9ddf791..d0a2d0f 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -12,8 +12,8 @@ pub(crate) fn categorize_position_contacts( ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; if !rb1.is_dynamic() || !rb2.is_dynamic() { match manifold.kinematics.category { @@ -38,8 +38,8 @@ pub(crate) fn categorize_velocity_contacts( ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; if !rb1.is_dynamic() || !rb2.is_dynamic() { out_ground.push(*manifold_i) diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 04acaaf..18e846b 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -12,7 +12,7 @@ pub(crate) trait PairInteraction { impl<'a> PairInteraction for &'a mut ContactManifold { fn body_pair(&self) -> BodyPair { - self.body_pair + self.data.body_pair } } @@ -77,7 +77,7 @@ impl ParallelInteractionGroups { .iter() .zip(self.interaction_colors.iter_mut()) { - let body_pair = interactions[*interaction_id].body_pair(); + let body_pair = interactions[*interaction_id].data.body_pair(); let rb1 = &bodies[body_pair.body1]; let rb2 = &bodies[body_pair.body2]; @@ -338,7 +338,7 @@ impl InteractionGroups { let mut occupied_mask = 0u128; let max_interaction_points = interaction_indices .iter() - .map(|i| interactions[*i].num_active_contacts()) + .map(|i| interactions[*i].num_active_contacts) .max() .unwrap_or(1); @@ -351,12 +351,12 @@ impl InteractionGroups { // FIXME: how could we avoid iterating // on each interaction at every iteration on k? - if interaction.num_active_contacts() != k { + if interaction.num_active_contacts != k { continue; } - let body1 = &bodies[interaction.body_pair.body1]; - let body2 = &bodies[interaction.body_pair.body2]; + let body1 = &bodies[interaction.data.body_pair.body1]; + let body2 = &bodies[interaction.data.body_pair.body2]; let is_static1 = !body1.is_dynamic(); let is_static2 = !body2.is_dynamic(); diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 303a18f..451dd32 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -247,13 +247,13 @@ impl ParallelVelocitySolverPart { VelocityConstraintDesc::NongroundGrouped(joint_id) => { let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies); - self.constraints[joints[0].constraint_index] = constraint; + self.constraints[joints[0].data.constraint_index] = constraint; } #[cfg(feature = "simd-is-enabled")] VelocityConstraintDesc::GroundGrouped(joint_id) => { let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies); - self.constraints[joints[0].constraint_index] = constraint; + self.constraints[joints[0].data.constraint_index] = constraint; } } } diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 69fcf57..b329e39 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -88,8 +88,8 @@ impl PositionConstraint { out_constraints: &mut Vec, push: bool, ) { - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; let shift1 = manifold.local_n1 * -manifold.kinematics.radius1; let shift2 = manifold.local_n2 * -manifold.kinematics.radius2; let radius = @@ -104,8 +104,8 @@ impl PositionConstraint { let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; for l in 0..manifold_points.len() { - local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1); - local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2); + local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1); + local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2); } let constraint = PositionConstraint { @@ -132,10 +132,10 @@ impl PositionConstraint { } } else { if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPointPoint(constraint); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPlanePoint(constraint); } } diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 0633926..ecc0fc3 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -35,8 +35,8 @@ impl WPositionConstraint { out_constraints: &mut Vec, push: bool, ) { - let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH]; - let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH]; + let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; + let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let sqrt_ii1: AngularInertia = @@ -51,8 +51,8 @@ impl WPositionConstraint { let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); - let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); - let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -100,10 +100,10 @@ impl WPositionConstraint { } } else { if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPointPoint(constraint); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPlanePoint(constraint); } } diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index dcd2d64..7a15f3e 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -28,8 +28,8 @@ impl PositionGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rb1 = &bodies[manifold.body_pair.body1]; - let mut rb2 = &bodies[manifold.body_pair.body2]; + let mut rb1 = &bodies[manifold.data.body_pair.body1]; + let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flip = !rb2.is_dynamic(); let local_n1; @@ -41,13 +41,13 @@ impl PositionGroundConstraint { std::mem::swap(&mut rb1, &mut rb2); local_n1 = manifold.local_n2; local_n2 = manifold.local_n1; - delta1 = &manifold.delta2; - delta2 = &manifold.delta1; + delta1 = &manifold.data.delta2; + delta2 = &manifold.data.delta1; } else { local_n1 = manifold.local_n1; local_n2 = manifold.local_n2; - delta1 = &manifold.delta1; - delta2 = &manifold.delta2; + delta1 = &manifold.data.delta1; + delta2 = &manifold.data.delta2; }; let coll_pos1 = rb1.position * delta1; @@ -105,10 +105,10 @@ impl PositionGroundConstraint { } } else { if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPointPointGround(constraint); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPlanePointGround(constraint); } } diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index e423c0a..1609709 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -32,8 +32,10 @@ impl WPositionGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH]; - let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH]; + let mut rbs1 = + array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; + let mut rbs2 = + array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -55,10 +57,10 @@ impl WPositionGroundConstraint { ); let delta1 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH], ); let delta2 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], ); let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); @@ -112,10 +114,10 @@ impl WPositionGroundConstraint { } } else { if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPointPointGround(constraint); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPlanePointGround(constraint); } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 948081d..1bc4c0e 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -144,14 +144,14 @@ impl VelocityConstraint { out_constraints: &mut Vec, push: bool, ) { - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; let mj_lambda1 = rb1.active_set_offset; let mj_lambda2 = rb2.active_set_offset; - let pos_coll1 = rb1.position * manifold.delta1; - let pos_coll2 = rb2.position * manifold.delta2; + let pos_coll1 = rb1.position * manifold.data.delta1; + let pos_coll2 = rb2.position * manifold.data.delta2; let force_dir1 = pos_coll1 * (-manifold.local_n1); - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold .active_contacts() @@ -164,7 +164,7 @@ impl VelocityConstraint { elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1: rb1.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass, - limit: manifold.friction, + limit: manifold.data.friction, mj_lambda1, mj_lambda2, manifold_id, @@ -207,7 +207,7 @@ impl VelocityConstraint { constraint.dir1 = force_dir1; constraint.im1 = rb1.mass_properties.inv_mass; constraint.im2 = rb2.mass_properties.inv_mass; - constraint.limit = manifold.friction; + constraint.limit = manifold.data.friction; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; @@ -241,12 +241,12 @@ impl VelocityConstraint { let mut rhs = (vel1 - vel2).dot(&force_dir1); if rhs <= -params.restitution_velocity_threshold { - rhs += manifold.restitution * rhs + rhs += manifold.data.restitution * rhs } rhs += manifold_point.dist.max(0.0) * params.inv_dt(); - let impulse = manifold_points[k].impulse * warmstart_coeff; + let impulse = manifold_points[k].data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityConstraintElementPart { gcross1, @@ -275,9 +275,9 @@ impl VelocityConstraint { + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); #[cfg(feature = "dim2")] - let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] - let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart { gcross1, @@ -294,7 +294,7 @@ impl VelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint)); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyVelocityConstraint::Nongrouped(constraint); } } @@ -388,15 +388,15 @@ impl VelocityConstraint { for k in 0..self.num_contacts as usize { let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = [ + active_contacts[k_base + k].data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 5d8078a..a4efb9a 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -69,11 +69,11 @@ impl WVelocityConstraint { push: bool, ) { let inv_dt = SimdFloat::splat(params.inv_dt()); - let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; - let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; - let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); - let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let ii1: AngularInertia = @@ -103,13 +103,13 @@ impl WVelocityConstraint { let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]); + let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); let restitution_velocity_threshold = SimdFloat::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { @@ -140,7 +140,7 @@ impl WVelocityConstraint { let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -176,11 +176,11 @@ impl WVelocityConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); @@ -202,7 +202,7 @@ impl WVelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Grouped(constraint)); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::Grouped(constraint); } } @@ -342,15 +342,15 @@ impl WVelocityConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = impulses[ii]; + active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index d8ef8be..c3a3356 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -63,26 +63,26 @@ impl VelocityGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rb1 = &bodies[manifold.body_pair.body1]; - let mut rb2 = &bodies[manifold.body_pair.body2]; + let mut rb1 = &bodies[manifold.data.body_pair.body1]; + let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = !rb2.is_dynamic(); let force_dir1; let coll_pos1; let coll_pos2; if flipped { - coll_pos1 = rb2.position * manifold.delta2; - coll_pos2 = rb1.position * manifold.delta1; + coll_pos1 = rb2.position * manifold.data.delta2; + coll_pos2 = rb1.position * manifold.data.delta1; force_dir1 = coll_pos1 * (-manifold.local_n2); std::mem::swap(&mut rb1, &mut rb2); } else { - coll_pos1 = rb1.position * manifold.delta1; - coll_pos2 = rb2.position * manifold.delta2; + coll_pos1 = rb1.position * manifold.data.delta1; + coll_pos2 = rb2.position * manifold.data.delta2; force_dir1 = coll_pos1 * (-manifold.local_n1); } let mj_lambda2 = rb2.active_set_offset; - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold .active_contacts() @@ -94,7 +94,7 @@ impl VelocityGroundConstraint { dir1: force_dir1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: rb2.mass_properties.inv_mass, - limit: manifold.friction, + limit: manifold.data.friction, mj_lambda2, manifold_id, manifold_contact_id: l * MAX_MANIFOLD_POINTS, @@ -135,7 +135,7 @@ impl VelocityGroundConstraint { { constraint.dir1 = force_dir1; constraint.im2 = rb2.mass_properties.inv_mass; - constraint.limit = manifold.friction; + constraint.limit = manifold.data.friction; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; @@ -173,12 +173,12 @@ impl VelocityGroundConstraint { let mut rhs = (vel1 - vel2).dot(&force_dir1); if rhs <= -params.restitution_velocity_threshold { - rhs += manifold.restitution * rhs + rhs += manifold.data.restitution * rhs } rhs += manifold_point.dist.max(0.0) * params.inv_dt(); - let impulse = manifold_points[k].impulse * warmstart_coeff; + let impulse = manifold_points[k].data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { gcross2, @@ -199,9 +199,9 @@ impl VelocityGroundConstraint { let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); #[cfg(feature = "dim2")] - let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] - let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; constraint.elements[k].tangent_part[j] = VelocityGroundConstraintElementPart { @@ -218,7 +218,7 @@ impl VelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint)); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyVelocityConstraint::NongroupedGround(constraint); } } @@ -290,15 +290,15 @@ impl VelocityGroundConstraint { for k in 0..self.num_contacts as usize { let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = [ + active_contacts[k_base + k].data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index be01944..84c4182 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -65,8 +65,8 @@ impl WVelocityGroundConstraint { push: bool, ) { let inv_dt = SimdFloat::splat(params.inv_dt()); - let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; - let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -90,10 +90,10 @@ impl WVelocityGroundConstraint { let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let delta1 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH], ); let delta2 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], ); let coll_pos1 = pos1 * delta1; @@ -109,13 +109,13 @@ impl WVelocityGroundConstraint { let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]); + let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); let restitution_velocity_threshold = SimdFloat::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { @@ -146,7 +146,7 @@ impl WVelocityGroundConstraint { let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -178,11 +178,11 @@ impl WVelocityGroundConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); @@ -202,7 +202,7 @@ impl WVelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint)); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::GroupedGround(constraint); } } @@ -302,15 +302,15 @@ impl WVelocityGroundConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = impulses[ii]; + active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 863990d..56c05df 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -4,7 +4,7 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; use bit_vec::BitVec; -use ncollide::bounding_volume::{BoundingVolume, AABB}; +use buckler::bounding_volume::{BoundingVolume, AABB}; use std::cmp::Ordering; use std::ops::{Index, IndexMut}; @@ -67,7 +67,7 @@ fn point_key(point: Point) -> Point { (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() } -fn region_aabb(index: Point) -> AABB { +fn region_aabb(index: Point) -> AABB { let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into(); let maxs = mins + Vector::repeat(CELL_WIDTH); AABB::new(mins, maxs) @@ -345,7 +345,7 @@ struct SAPRegion { } impl SAPRegion { - pub fn new(bounds: AABB) -> Self { + pub fn new(bounds: AABB) -> Self { let axes = [ SAPAxis::new(bounds.mins.x, bounds.maxs.x), SAPAxis::new(bounds.mins.y, bounds.maxs.y), @@ -361,7 +361,7 @@ impl SAPRegion { } } - pub fn recycle(bounds: AABB, mut old: Self) -> Self { + pub fn recycle(bounds: AABB, mut old: Self) -> Self { // Correct the bounds for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) { axis.min_bound = bound; @@ -381,7 +381,7 @@ impl SAPRegion { old } - pub fn recycle_or_new(bounds: AABB, pool: &mut Vec) -> Self { + pub fn recycle_or_new(bounds: AABB, pool: &mut Vec) -> Self { if let Some(old) = pool.pop() { Self::recycle(bounds, old) } else { @@ -488,7 +488,7 @@ pub struct BroadPhase { #[derive(Clone)] pub(crate) struct BroadPhaseProxy { handle: ColliderHandle, - aabb: AABB, + aabb: AABB, next_free: u32, } diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs deleted file mode 100644 index 54736cc..0000000 --- a/src/geometry/capsule.rs +++ /dev/null @@ -1,192 +0,0 @@ -use crate::geometry::{Ray, RayIntersection, AABB}; -use crate::math::{Isometry, Point, Rotation, Vector}; -use approx::AbsDiffEq; -use na::Unit; -use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast}; -use ncollide::shape::{FeatureId, Segment, SupportMap}; - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A capsule shape defined as a round segment. -pub struct Capsule { - /// The axis and endpoint of the capsule. - pub segment: Segment, - /// The radius of the capsule. - pub radius: f32, -} - -impl Capsule { - /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius. - pub fn new_x(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::x() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius. - pub fn new_y(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::y() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius. - #[cfg(feature = "dim3")] - pub fn new_z(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::z() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. - pub fn new(a: Point, b: Point, radius: f32) -> Self { - let segment = Segment::new(a, b); - Self { segment, radius } - } - - /// The axis-aligned bounding box of this capsule. - pub fn aabb(&self, pos: &Isometry) -> AABB { - let a = pos * self.segment.a; - let b = pos * self.segment.b; - let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius); - let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius); - AABB::new(mins.into(), maxs.into()) - } - - /// The height of this capsule. - pub fn height(&self) -> f32 { - (self.segment.b - self.segment.a).norm() - } - - /// The half-height of this capsule. - pub fn half_height(&self) -> f32 { - self.height() / 2.0 - } - - /// The center of this capsule. - pub fn center(&self) -> Point { - na::center(&self.segment.a, &self.segment.b) - } - - /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. - pub fn transform_by(&self, pos: &Isometry) -> Self { - Self::new(pos * self.segment.a, pos * self.segment.b, self.radius) - } - - /// The rotation `r` such that `r * Y` is collinear with `b - a`. - pub fn rotation_wrt_y(&self) -> Rotation { - let mut dir = self.segment.b - self.segment.a; - if dir.y < 0.0 { - dir = -dir; - } - - #[cfg(feature = "dim2")] - { - Rotation::rotation_between(&Vector::y(), &dir) - } - - #[cfg(feature = "dim3")] - { - Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()) - } - } - - /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`. - pub fn transform_wrt_y(&self) -> Isometry { - let rot = self.rotation_wrt_y(); - Isometry::from_parts(self.center().coords.into(), rot) - } -} - -impl SupportMap for Capsule { - fn local_support_point(&self, dir: &Vector) -> Point { - let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis()); - self.local_support_point_toward(&dir) - } - - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) { - self.segment.a + **dir * self.radius - } else { - self.segment.b + **dir * self.radius - } - } -} - -impl RayCast for Capsule { - fn toi_and_normal_with_ray( - &self, - m: &Isometry, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option { - let ls_ray = ray.inverse_transform_by(m); - - ncollide::query::ray_intersection_with_support_map_with_params( - &Isometry::identity(), - self, - &mut VoronoiSimplex::new(), - &ls_ray, - max_toi, - solid, - ) - .map(|mut res| { - res.normal = m * res.normal; - res - }) - } -} - -// TODO: this code has been extracted from ncollide and added here -// so we can modify it to fit with our new definition of capsule. -// We should find a way to avoid this code duplication. -impl PointQuery for Capsule { - #[inline] - fn project_point( - &self, - m: &Isometry, - pt: &Point, - solid: bool, - ) -> PointProjection { - let seg = Segment::new(self.segment.a, self.segment.b); - let proj = seg.project_point(m, pt, solid); - let dproj = *pt - proj.point; - - if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) { - let inside = dist <= self.radius; - if solid && inside { - return PointProjection::new(true, *pt); - } else { - return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius); - } - } else if solid { - return PointProjection::new(true, *pt); - } - - #[cfg(feature = "dim2")] - if let Some(dir) = seg.normal() { - let dir = m * *dir; - PointProjection::new(true, proj.point + dir * self.radius) - } else { - // The segment has no normal, likely because it degenerates to a point. - PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) - } - - #[cfg(feature = "dim3")] - if let Some(dir) = seg.direction() { - use crate::utils::WBasis; - let dir = m * dir.orthonormal_basis()[0]; - PointProjection::new(true, proj.point + dir * self.radius) - } else { - // The segment has no normal, likely because it degenerates to a point. - PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) - } - } - - #[inline] - fn project_point_with_feature( - &self, - m: &Isometry, - pt: &Point, - ) -> (PointProjection, FeatureId) { - (self.project_point(m, pt, false), FeatureId::Face(0)) - } -} diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index c04be35..8154554 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -6,8 +6,8 @@ use crate::geometry::{ #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, RoundCylinder}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; +use buckler::bounding_volume::AABB; use na::Point3; -use ncollide::bounding_volume::AABB; use std::ops::Deref; use std::sync::Arc; @@ -270,11 +270,11 @@ impl Collider { } /// Compute the axis-aligned bounding box of this collider. - pub fn compute_aabb(&self) -> AABB { + pub fn compute_aabb(&self) -> AABB { self.shape.compute_aabb(&self.position) } - // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB { + // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB { // let aabb1 = self.shape.compute_aabb(&self.position); // let aabb2 = self.shape.compute_aabb(&self.predicted_position); // aabb1.merged(&aabb2) diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index a4a176e..1f0a902 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -1,7 +1,8 @@ +use crate::buckler::query::TrackedData; use crate::data::MaybeSerializableData; -use crate::dynamics::BodyPair; +use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet}; use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase}; -use crate::geometry::{Collider, ColliderPair, ColliderSet}; +use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold}; use crate::math::{Isometry, Point, Vector}; #[cfg(feature = "simd-is-enabled")] use { @@ -19,39 +20,6 @@ bitflags::bitflags! { } } -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// The type local linear approximation of the neighborhood of a pair contact points on two shapes -pub enum KinematicsCategory { - /// Both neighborhoods are assimilated to a single point. - PointPoint, - /// The first shape's neighborhood at the contact point is assimilated to a plane while - /// the second is assimilated to a point. - PlanePoint, -} - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// Local contact geometry at the neighborhood of a pair of contact points. -pub struct ContactKinematics { - /// The local contact geometry. - pub category: KinematicsCategory, - /// The dilation applied to the first contact geometry. - pub radius1: f32, - /// The dilation applied to the second contact geometry. - pub radius2: f32, -} - -impl Default for ContactKinematics { - fn default() -> Self { - ContactKinematics { - category: KinematicsCategory::PointPoint, - radius1: 0.0, - radius2: 0.0, - } - } -} - #[cfg(feature = "simd-is-enabled")] pub(crate) struct WContact { pub local_p1: Point, @@ -70,10 +38,9 @@ impl WContact { local_p1: self.local_p1.extract(i), local_p2: self.local_p2.extract(i), dist: self.dist.extract(i), - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), fid1: self.fid1[i], fid2: self.fid2[i], + data: ContactData::default(), }; (c, self.local_n1.extract(i), self.local_n2.extract(i)) @@ -83,11 +50,7 @@ impl WContact { #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A single contact between two collider. -pub struct Contact { - /// The contact point in the local-space of the first collider. - pub local_p1: Point, - /// The contact point in the local-space of the second collider. - pub local_p2: Point, +pub struct ContactData { /// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body. /// /// The impulse applied to the second collider's rigid-body is given by `-impulse`. @@ -100,46 +63,9 @@ pub struct Contact { /// collider's rigid-body. #[cfg(feature = "dim3")] pub tangent_impulse: [f32; 2], - /// The identifier of the subshape of the first collider involved in this contact. - /// - /// For primitive shapes like cuboid, ball, etc., this is 0. - /// For shapes like trimesh and heightfield this identifies the specific triangle - /// involved in the contact. - pub fid1: u8, - /// The identifier of the subshape of the second collider involved in this contact. - /// - /// For primitive shapes like cuboid, ball, etc., this is 0. - /// For shapes like trimesh and heightfield this identifies the specific triangle - /// involved in the contact. - pub fid2: u8, - /// The distance between the two colliders along the contact normal. - /// - /// If this is negative, the colliders are penetrating. - pub dist: f32, } -impl Contact { - pub(crate) fn new( - local_p1: Point, - local_p2: Point, - fid1: u8, - fid2: u8, - dist: f32, - ) -> Self { - Self { - local_p1, - local_p2, - impulse: 0.0, - #[cfg(feature = "dim2")] - tangent_impulse: 0.0, - #[cfg(feature = "dim3")] - tangent_impulse: [0.0; 2], - fid1, - fid2, - dist, - } - } - +impl ContactData { #[cfg(feature = "dim2")] pub(crate) fn zero_tangent_impulse() -> f32 { 0.0 @@ -149,26 +75,15 @@ impl Contact { pub(crate) fn zero_tangent_impulse() -> [f32; 2] { [0.0, 0.0] } +} - pub(crate) fn copy_geometry_from(&mut self, contact: Contact) { - self.local_p1 = contact.local_p1; - self.local_p2 = contact.local_p2; - self.fid1 = contact.fid1; - self.fid2 = contact.fid2; - self.dist = contact.dist; +impl Default for ContactData { + fn default() -> Self { + Self { + impulse: 0.0, + tangent_impulse: Self::zero_tangent_impulse(), + } } - - // pub(crate) fn swap(self) -> Self { - // Self { - // local_p1: self.local_p2, - // local_p2: self.local_p1, - // impulse: self.impulse, - // tangent_impulse: self.tangent_impulse, - // fid1: self.fid2, - // fid2: self.fid1, - // dist: self.dist, - // } - // } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -227,15 +142,16 @@ impl ContactPair { let coll2 = &colliders[self.pair.collider2]; if self.manifolds.len() == 0 { - let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags); - self.manifolds.push(manifold); + let manifold_data = ContactManifoldData::from_colliders(self.pair, coll1, coll2, flags); + self.manifolds + .push(ContactManifold::with_data((0, 0), manifold_data)); } // We have to make sure the order of the returned collider // match the order of the pair stored inside of the manifold. // (This order can be modified by the contact determination algorithm). let manifold = &mut self.manifolds[0]; - if manifold.pair.collider1 == self.pair.collider1 { + if manifold.data.pair.collider1 == self.pair.collider1 { ( coll1, coll2, @@ -259,30 +175,12 @@ impl ContactPair { /// /// A contact manifold describes a set of contacts between two colliders. All the contact /// part of the same contact manifold share the same contact normal and contact kinematics. -pub struct ContactManifold { - // NOTE: use a SmallVec instead? - // And for 2D use an ArrayVec since there will never be more than 2 contacts anyways. - #[cfg(feature = "dim2")] - pub(super) points: arrayvec::ArrayVec<[Contact; 2]>, - #[cfg(feature = "dim3")] - pub(super) points: Vec, - /// The number of active contacts on this contact manifold. - /// - /// Active contacts are these that may result in contact forces. - pub num_active_contacts: usize, - /// The contact normal of all the contacts of this manifold, expressed in the local space of the first collider. - pub local_n1: Vector, - /// The contact normal of all the contacts of this manifold, expressed in the local space of the second collider. - pub local_n2: Vector, - /// The contact kinematics of all the contacts of this manifold. - pub kinematics: ContactKinematics, +pub struct ContactManifoldData { // The following are set by the narrow-phase. - /// The pair of body involved in this contact manifold. - pub body_pair: BodyPair, /// The pair of colliders involved in this contact manifold. pub pair: ColliderPair, - /// The pair of subshapes involved in this contact manifold. - pub subshape_index_pair: (usize, usize), + /// The pair of body involved in this contact manifold. + pub body_pair: BodyPair, pub(crate) warmstart_multiplier: f32, // The two following are set by the constraints solver. pub(crate) constraint_index: usize, @@ -303,29 +201,44 @@ pub struct ContactManifold { pub solver_flags: SolverFlags, } -impl ContactManifold { +impl Default for ContactManifoldData { + fn default() -> Self { + Self::new( + ColliderPair::new(ColliderSet::invalid_handle(), ColliderSet::invalid_handle()), + BodyPair::new( + RigidBodySet::invalid_handle(), + RigidBodySet::invalid_handle(), + ), + Isometry::identity(), + Isometry::identity(), + 0.0, + 0.0, + SolverFlags::empty(), + ) + } +} + +impl TrackedData for ContactManifoldData { + fn flip(&mut self) { + std::mem::swap(&mut self.pair.collider1, &mut self.pair.collider2); + std::mem::swap(&mut self.body_pair.body1, &mut self.body_pair.body2); + std::mem::swap(&mut self.delta1, &mut self.delta2); + } +} + +impl ContactManifoldData { pub(crate) fn new( pair: ColliderPair, - subshapes: (usize, usize), body_pair: BodyPair, delta1: Isometry, delta2: Isometry, friction: f32, restitution: f32, solver_flags: SolverFlags, - ) -> ContactManifold { + ) -> ContactManifoldData { Self { - #[cfg(feature = "dim2")] - points: arrayvec::ArrayVec::new(), - #[cfg(feature = "dim3")] - points: Vec::new(), - num_active_contacts: 0, - local_n1: Vector::zeros(), - local_n2: Vector::zeros(), pair, - subshape_index_pair: subshapes, body_pair, - kinematics: ContactKinematics::default(), warmstart_multiplier: Self::min_warmstart_multiplier(), friction, restitution, @@ -337,50 +250,23 @@ impl ContactManifold { } } - pub(crate) fn take(&mut self) -> Self { - ContactManifold { - #[cfg(feature = "dim2")] - points: self.points.clone(), - #[cfg(feature = "dim3")] - points: std::mem::replace(&mut self.points, Vec::new()), - num_active_contacts: self.num_active_contacts, - local_n1: self.local_n1, - local_n2: self.local_n2, - kinematics: self.kinematics, - body_pair: self.body_pair, - pair: self.pair, - subshape_index_pair: self.subshape_index_pair, - warmstart_multiplier: self.warmstart_multiplier, - friction: self.friction, - restitution: self.restitution, - delta1: self.delta1, - delta2: self.delta2, - constraint_index: self.constraint_index, - position_constraint_index: self.position_constraint_index, - solver_flags: self.solver_flags, - } - } - pub(crate) fn from_colliders( pair: ColliderPair, coll1: &Collider, coll2: &Collider, flags: SolverFlags, ) -> Self { - Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags) + Self::with_subshape_indices(pair, coll1, coll2, flags) } pub(crate) fn with_subshape_indices( pair: ColliderPair, coll1: &Collider, coll2: &Collider, - subshape1: usize, - subshape2: usize, solver_flags: SolverFlags, ) -> Self { Self::new( pair, - (subshape1, subshape2), BodyPair::new(coll1.parent, coll2.parent), *coll1.position_wrt_parent(), *coll2.position_wrt_parent(), @@ -398,134 +284,25 @@ impl ContactManifold { 0.01 } - /// Number of active contacts on this contact manifold. - #[inline] - pub fn num_active_contacts(&self) -> usize { - self.num_active_contacts - } - - /// The slice of all the active contacts on this contact manifold. - /// - /// Active contacts are contacts that may end up generating contact forces. - #[inline] - pub fn active_contacts(&self) -> &[Contact] { - &self.points[..self.num_active_contacts] - } - - #[inline] - pub(crate) fn active_contacts_mut(&mut self) -> &mut [Contact] { - &mut self.points[..self.num_active_contacts] - } - - /// The slice of all the contacts, active or not, on this contact manifold. - #[inline] - pub fn all_contacts(&self) -> &[Contact] { - &self.points - } - - pub(crate) fn swap_identifiers(&mut self) { - self.pair = self.pair.swap(); - self.body_pair = self.body_pair.swap(); - self.subshape_index_pair = (self.subshape_index_pair.1, self.subshape_index_pair.0); - std::mem::swap(&mut self.delta1, &mut self.delta2); - } - - pub(crate) fn update_warmstart_multiplier(&mut self) { + pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) { // In 2D, tall stacks will actually suffer from this // because oscillation due to inaccuracies in 2D often // cause contacts to break, which would result in // a reset of the warmstart multiplier. if cfg!(feature = "dim2") { - self.warmstart_multiplier = 1.0; + manifold.data.warmstart_multiplier = 1.0; return; } - for pt in &self.points { - if pt.impulse != 0.0 { - self.warmstart_multiplier = (self.warmstart_multiplier * 2.0).min(1.0); + for pt in &manifold.points { + if pt.data.impulse != 0.0 { + manifold.data.warmstart_multiplier = + (manifold.data.warmstart_multiplier * 2.0).min(1.0); return; } } // Reset the multiplier. - self.warmstart_multiplier = Self::min_warmstart_multiplier() - } - - #[inline] - pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry) -> bool { - // const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES; - const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES; - const DIST_SQ_THRESHOLD: f32 = 0.001; // FIXME: this should not be hard-coded. - self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD) - } - - #[inline] - pub(crate) fn try_update_contacts_eps( - &mut self, - pos12: &Isometry, - angle_dot_threshold: f32, - dist_sq_threshold: f32, - ) -> bool { - if self.points.len() == 0 { - return false; - } - - let local_n2 = pos12 * self.local_n2; - - if -self.local_n1.dot(&local_n2) < angle_dot_threshold { - return false; - } - - for pt in &mut self.points { - let local_p2 = pos12 * pt.local_p2; - let dpt = local_p2 - pt.local_p1; - let dist = dpt.dot(&self.local_n1); - - if dist * pt.dist < 0.0 { - // We switched between penetrating/non-penetrating. - // The may result in other contacts to appear. - return false; - } - let new_local_p1 = local_p2 - self.local_n1 * dist; - - if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold { - return false; - } - - pt.dist = dist; - pt.local_p1 = new_local_p1; - } - - true - } - - /// Sort the contacts of this contact manifold such that the active contacts are in the first - /// positions of the array. - #[inline] - pub(crate) fn sort_contacts(&mut self, prediction_distance: f32) { - let num_contacts = self.points.len(); - match num_contacts { - 0 => { - self.num_active_contacts = 0; - } - 1 => { - self.num_active_contacts = (self.points[0].dist < prediction_distance) as usize; - } - _ => { - let mut first_inactive_index = num_contacts; - - self.num_active_contacts = 0; - while self.num_active_contacts != first_inactive_index { - if self.points[self.num_active_contacts].dist >= prediction_distance { - // Swap with the last contact. - self.points - .swap(self.num_active_contacts, first_inactive_index - 1); - first_inactive_index -= 1; - } else { - self.num_active_contacts += 1; - } - } - } - } + manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() } } 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, diff --git a/src/geometry/cuboid.rs b/src/geometry/cuboid.rs deleted file mode 100644 index 89e1a1e..0000000 --- a/src/geometry/cuboid.rs +++ /dev/null @@ -1,234 +0,0 @@ -#[cfg(feature = "dim3")] -use crate::geometry::PolyhedronFace; -use crate::geometry::{Cuboid, CuboidFeature, CuboidFeatureFace}; -use crate::math::{Point, Vector}; -use crate::utils::WSign; - -pub fn local_support_point(cube: &Cuboid, local_dir: Vector) -> Point { - local_dir.copy_sign_to(cube.half_extents).into() -} - -// #[cfg(feature = "dim2")] -// pub fn polygon_ref( -// cuboid: Cuboid, -// out_vertices: &mut [Point; 4], -// out_normals: &mut [Vector; 4], -// ) -> PolygonRef { -// *out_vertices = [ -// Point::new(cuboid.half_extents.x, -cuboid.half_extents.y), -// Point::new(cuboid.half_extents.x, cuboid.half_extents.y), -// Point::new(-cuboid.half_extents.x, cuboid.half_extents.y), -// Point::new(-cuboid.half_extents.x, -cuboid.half_extents.y), -// ]; -// *out_normals = [Vector::x(), Vector::y(), -Vector::x(), -Vector::y()]; -// -// PolygonRef { -// vertices: &out_vertices[..], -// normals: &out_normals[..], -// } -// } - -#[cfg(feature = "dim2")] -pub fn vertex_feature_id(vertex: Point) -> u8 { - ((vertex.x.to_bits() >> 31) & 0b001 | (vertex.y.to_bits() >> 30) & 0b010) as u8 -} - -// #[cfg(feature = "dim3")] -// pub fn vertex_feature_id(vertex: Point) -> u8 { -// ((vertex.x.to_bits() >> 31) & 0b001 -// | (vertex.y.to_bits() >> 30) & 0b010 -// | (vertex.z.to_bits() >> 29) & 0b100) as u8 -// } - -#[cfg(feature = "dim3")] -pub fn polyhedron_support_face(cube: &Cuboid, local_dir: Vector) -> PolyhedronFace { - support_face(cube, local_dir).into() -} - -#[cfg(feature = "dim2")] -pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector) -> CuboidFeature { - // In 2D, it is best for stability to always return a face. - // It won't have any notable impact on performances anyway. - CuboidFeature::Face(support_face(cube, local_dir)) - - /* - let amax = local_dir.amax(); - - const MAX_DOT_THRESHOLD: f32 = 0.98480775301; // 10 degrees. - - if amax > MAX_DOT_THRESHOLD { - // Support face. - CuboidFeature::Face(cube.support_face(local_dir)) - } else { - // Support vertex - CuboidFeature::Vertex(cube.support_vertex(local_dir)) - } - */ -} - -#[cfg(feature = "dim3")] -pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector) -> CuboidFeature { - CuboidFeature::Face(support_face(cube, local_dir)) - /* - const MAX_DOT_THRESHOLD: f32 = crate::utils::COS_10_DEGREES; - const MIN_DOT_THRESHOLD: f32 = 1.0 - MAX_DOT_THRESHOLD; - - let amax = local_dir.amax(); - let amin = local_dir.amin(); - - if amax > MAX_DOT_THRESHOLD { - // Support face. - CuboidFeature::Face(support_face(cube, local_dir)) - } else if amin < MIN_DOT_THRESHOLD { - // Support edge. - CuboidFeature::Edge(support_edge(cube, local_dir)) - } else { - // Support vertex. - CuboidFeature::Vertex(support_vertex(cube, local_dir)) - } - */ -} - -// #[cfg(feature = "dim3")] -// pub(crate) fn support_vertex(cube: &Cuboid, local_dir: Vector) -> CuboidFeatureVertex { -// let vertex = local_support_point(cube, local_dir); -// let vid = vertex_feature_id(vertex); -// -// CuboidFeatureVertex { vertex, vid } -// } - -// #[cfg(feature = "dim3")] -// pub(crate) fn support_edge(cube: &Cuboid, local_dir: Vector) -> CuboidFeatureEdge { -// let he = cube.half_extents; -// let i = local_dir.iamin(); -// let j = (i + 1) % 3; -// let k = (i + 2) % 3; -// let mut a = Point::origin(); -// a[i] = he[i]; -// a[j] = local_dir[j].copy_sign_to(he[j]); -// a[k] = local_dir[k].copy_sign_to(he[k]); -// -// let mut b = a; -// b[i] = -he[i]; -// -// let vid1 = vertex_feature_id(a); -// let vid2 = vertex_feature_id(b); -// let eid = (vid1.max(vid2) << 3) | vid1.min(vid2) | 0b11_000_000; -// -// CuboidFeatureEdge { -// vertices: [a, b], -// vids: [vid1, vid2], -// eid, -// } -// } - -#[cfg(feature = "dim2")] -pub fn support_face(cube: &Cuboid, local_dir: Vector) -> CuboidFeatureFace { - let he = cube.half_extents; - let i = local_dir.iamin(); - let j = (i + 1) % 2; - let mut a = Point::origin(); - a[i] = he[i]; - a[j] = local_dir[j].copy_sign_to(he[j]); - - let mut b = a; - b[i] = -he[i]; - - let vid1 = vertex_feature_id(a); - let vid2 = vertex_feature_id(b); - let fid = (vid1.max(vid2) << 2) | vid1.min(vid2) | 0b11_00_00; - - CuboidFeatureFace { - vertices: [a, b], - vids: [vid1, vid2], - fid, - } -} - -#[cfg(feature = "dim3")] -pub(crate) fn support_face(cube: &Cuboid, local_dir: Vector) -> CuboidFeatureFace { - // NOTE: can we use the orthonormal basis of local_dir - // to make this AoSoA friendly? - let he = cube.half_extents; - let iamax = local_dir.iamax(); - let sign = local_dir[iamax].copy_sign_to(1.0); - - let vertices = match iamax { - 0 => [ - Point::new(he.x * sign, he.y, he.z), - Point::new(he.x * sign, -he.y, he.z), - Point::new(he.x * sign, -he.y, -he.z), - Point::new(he.x * sign, he.y, -he.z), - ], - 1 => [ - Point::new(he.x, he.y * sign, he.z), - Point::new(-he.x, he.y * sign, he.z), - Point::new(-he.x, he.y * sign, -he.z), - Point::new(he.x, he.y * sign, -he.z), - ], - 2 => [ - Point::new(he.x, he.y, he.z * sign), - Point::new(he.x, -he.y, he.z * sign), - Point::new(-he.x, -he.y, he.z * sign), - Point::new(-he.x, he.y, he.z * sign), - ], - _ => unreachable!(), - }; - - pub fn vid(i: u8) -> u8 { - // Each vertex has an even feature id. - i * 2 - } - - let sign_index = ((sign as i8 + 1) / 2) as usize; - // The vertex id as numbered depending on the sign of the vertex - // component. A + sign means the corresponding bit is 0 while a - - // sign means the corresponding bit is 1. - // For exampl the vertex [2.0, -1.0, -3.0] has the id 0b011 - let vids = match iamax { - 0 => [ - [vid(0b000), vid(0b010), vid(0b011), vid(0b001)], - [vid(0b100), vid(0b110), vid(0b111), vid(0b101)], - ][sign_index], - 1 => [ - [vid(0b000), vid(0b100), vid(0b101), vid(0b001)], - [vid(0b010), vid(0b110), vid(0b111), vid(0b011)], - ][sign_index], - 2 => [ - [vid(0b000), vid(0b010), vid(0b110), vid(0b100)], - [vid(0b001), vid(0b011), vid(0b111), vid(0b101)], - ][sign_index], - _ => unreachable!(), - }; - - // The feature ids of edges is obtained from the vertex ids - // of their endpoints. - // Assuming vid1 > vid2, we do: (vid1 << 3) | vid2 | 0b11000000 - // - let eids = match iamax { - 0 => [ - [0b11_010_000, 0b11_011_010, 0b11_011_001, 0b11_001_000], - [0b11_110_100, 0b11_111_110, 0b11_111_101, 0b11_101_100], - ][sign_index], - 1 => [ - [0b11_100_000, 0b11_101_100, 0b11_101_001, 0b11_001_000], - [0b11_110_010, 0b11_111_110, 0b11_111_011, 0b11_011_010], - ][sign_index], - 2 => [ - [0b11_010_000, 0b11_110_010, 0b11_110_100, 0b11_100_000], - [0b11_011_001, 0b11_111_011, 0b11_111_101, 0b11_101_001], - ][sign_index], - _ => unreachable!(), - }; - - // The face with normals [x, y, z] are numbered [10, 11, 12]. - // The face with negated normals are numbered [13, 14, 15]. - let fid = iamax + sign_index * 3 + 10; - - CuboidFeatureFace { - vertices, - vids, - eids, - fid: fid as u8, - } -} diff --git a/src/geometry/cuboid_feature2d.rs b/src/geometry/cuboid_feature2d.rs deleted file mode 100644 index 7e8001f..0000000 --- a/src/geometry/cuboid_feature2d.rs +++ /dev/null @@ -1,128 +0,0 @@ -use crate::geometry::{self, Contact, ContactManifold}; -use crate::math::{Isometry, Point, Vector}; -use ncollide::shape::Segment; - -#[derive(Debug)] -#[allow(dead_code)] -pub enum CuboidFeature { - Face(CuboidFeatureFace), - Vertex(CuboidFeatureVertex), -} - -#[derive(Debug)] -pub struct CuboidFeatureVertex { - pub vertex: Point, - pub vid: u8, -} - -impl CuboidFeatureVertex { - pub fn transform_by(&mut self, iso: &Isometry) { - self.vertex = iso * self.vertex; - } -} - -#[derive(Debug)] -pub struct CuboidFeatureFace { - pub vertices: [Point; 2], - pub vids: [u8; 2], - pub fid: u8, -} - -impl From> for CuboidFeatureFace { - fn from(seg: Segment) -> Self { - CuboidFeatureFace { - vertices: [seg.a, seg.b], - vids: [0, 2], - fid: 1, - } - } -} - -impl CuboidFeatureFace { - pub fn transform_by(&mut self, iso: &Isometry) { - self.vertices[0] = iso * self.vertices[0]; - self.vertices[1] = iso * self.vertices[1]; - } -} - -impl CuboidFeature { - pub fn transform_by(&mut self, iso: &Isometry) { - match self { - CuboidFeature::Face(face) => face.transform_by(iso), - CuboidFeature::Vertex(vertex) => vertex.transform_by(iso), - } - } - - /// Compute contacts points between a face and a vertex. - /// - /// This method assume we already know that at least one contact exists. - pub fn face_vertex_contacts( - face1: &CuboidFeatureFace, - sep_axis1: &Vector, - vertex2: &CuboidFeatureVertex, - pos21: &Isometry, - manifold: &mut ContactManifold, - ) { - let tangent1 = face1.vertices[1] - face1.vertices[0]; - let normal1 = Vector::new(-tangent1.y, tangent1.x); - let denom = -normal1.dot(&sep_axis1); - let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom; - let local_p2 = vertex2.vertex; - let local_p1 = vertex2.vertex - dist * sep_axis1; - - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.fid, - fid2: vertex2.vid, - dist, - }); - } - - pub fn face_face_contacts( - _prediction_distance: f32, - face1: &CuboidFeatureFace, - normal1: &Vector, - face2: &CuboidFeatureFace, - pos21: &Isometry, - manifold: &mut ContactManifold, - ) { - if let Some((clip_a, clip_b)) = geometry::clip_segments( - (face1.vertices[0], face1.vertices[1]), - (face2.vertices[0], face2.vertices[1]), - ) { - let fids1 = [face1.vids[0], face1.fid, face1.vids[1]]; - let fids2 = [face2.vids[0], face2.fid, face2.vids[1]]; - - let dist = (clip_a.1 - clip_a.0).dot(normal1); - if true { - // dist < prediction_distance { - manifold.points.push(Contact { - local_p1: clip_a.0, - local_p2: pos21 * clip_a.1, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: fids1[clip_a.2], - fid2: fids2[clip_a.3], - dist, - }); - } - - let dist = (clip_b.1 - clip_b.0).dot(normal1); - if true { - // dist < prediction_distance { - manifold.points.push(Contact { - local_p1: clip_b.0, - local_p2: pos21 * clip_b.1, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: fids1[clip_b.2], - fid2: fids2[clip_b.3], - dist, - }); - } - } - } -} diff --git a/src/geometry/cuboid_feature3d.rs b/src/geometry/cuboid_feature3d.rs deleted file mode 100644 index 3a4b675..0000000 --- a/src/geometry/cuboid_feature3d.rs +++ /dev/null @@ -1,516 +0,0 @@ -use crate::geometry::{Contact, ContactManifold}; -use crate::math::{Isometry, Point, Vector}; -use crate::utils::WBasis; -use na::Point2; - -#[derive(Debug)] -#[allow(dead_code)] -pub(crate) enum CuboidFeature { - Face(CuboidFeatureFace), - Edge(CuboidFeatureEdge), - Vertex(CuboidFeatureVertex), -} - -#[derive(Debug)] -pub(crate) struct CuboidFeatureVertex { - pub vertex: Point, - pub vid: u8, -} - -impl CuboidFeatureVertex { - pub fn transform_by(&mut self, iso: &Isometry) { - self.vertex = iso * self.vertex; - } -} - -#[derive(Debug)] -pub(crate) struct CuboidFeatureEdge { - pub vertices: [Point; 2], - pub vids: [u8; 2], - pub eid: u8, -} - -impl CuboidFeatureEdge { - pub fn transform_by(&mut self, iso: &Isometry) { - self.vertices[0] = iso * self.vertices[0]; - self.vertices[1] = iso * self.vertices[1]; - } -} - -#[derive(Debug)] -pub(crate) struct CuboidFeatureFace { - pub vertices: [Point; 4], - pub vids: [u8; 4], // Feature ID of the vertices. - pub eids: [u8; 4], // Feature ID of the edges. - pub fid: u8, // Feature ID of the face. -} - -impl CuboidFeatureFace { - pub fn transform_by(&mut self, iso: &Isometry) { - self.vertices[0] = iso * self.vertices[0]; - self.vertices[1] = iso * self.vertices[1]; - self.vertices[2] = iso * self.vertices[2]; - self.vertices[3] = iso * self.vertices[3]; - } -} - -impl CuboidFeature { - pub fn transform_by(&mut self, iso: &Isometry) { - match self { - CuboidFeature::Face(face) => face.transform_by(iso), - CuboidFeature::Edge(edge) => edge.transform_by(iso), - CuboidFeature::Vertex(vertex) => vertex.transform_by(iso), - } - } - - /// Compute contacts points between a face and a vertex. - /// - /// This method assume we already know that at least one contact exists. - pub fn face_vertex_contacts( - face1: &CuboidFeatureFace, - sep_axis1: &Vector, - vertex2: &CuboidFeatureVertex, - pos21: &Isometry, - manifold: &mut ContactManifold, - ) { - let normal1 = - (face1.vertices[0] - face1.vertices[1]).cross(&(face1.vertices[2] - face1.vertices[1])); - let denom = -normal1.dot(&sep_axis1); - let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom; - let local_p2 = vertex2.vertex; - let local_p1 = vertex2.vertex - dist * sep_axis1; - - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.fid, - fid2: vertex2.vid, - dist, - }); - } - - /// Compute contacts points between a face and an edge. - /// - /// This method assume we already know that at least one contact exists. - pub fn face_edge_contacts( - prediction_distance: f32, - face1: &CuboidFeatureFace, - sep_axis1: &Vector, - edge2: &CuboidFeatureEdge, - pos21: &Isometry, - manifold: &mut ContactManifold, - flipped: bool, - ) { - // Project the faces to a 2D plane for contact clipping. - // The plane they are projected onto has normal sep_axis1 - // and contains the origin (this is numerically OK because - // we are not working in world-space here). - let basis = sep_axis1.orthonormal_basis(); - let projected_face1 = [ - Point2::new( - face1.vertices[0].coords.dot(&basis[0]), - face1.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[1].coords.dot(&basis[0]), - face1.vertices[1].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[2].coords.dot(&basis[0]), - face1.vertices[2].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[3].coords.dot(&basis[0]), - face1.vertices[3].coords.dot(&basis[1]), - ), - ]; - let projected_edge2 = [ - Point2::new( - edge2.vertices[0].coords.dot(&basis[0]), - edge2.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - edge2.vertices[1].coords.dot(&basis[0]), - edge2.vertices[1].coords.dot(&basis[1]), - ), - ]; - - // Now we have to compute the intersection between all pairs of - // edges from the face 1 with the edge 2 - for i in 0..4 { - let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]]; - - if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) { - if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 { - // Found a contact between the two edges. - let edge1 = [face1.vertices[i], face1.vertices[(i + 1) % 4]]; - let local_p1 = edge1[0] * (1.0 - bcoords.0) + edge1[1].coords * bcoords.0; - let local_p2 = edge2.vertices[0] * (1.0 - bcoords.1) - + edge2.vertices[1].coords * bcoords.1; - let dist = (local_p2 - local_p1).dot(&sep_axis1); - - if dist < prediction_distance { - if flipped { - manifold.points.push(Contact { - local_p1: local_p2, - // All points are expressed in the locale space of the first shape - // (even if there was a flip). So the point we need to transform by - // pos21 is the one that will go into local_p2. - local_p2: pos21 * local_p1, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: edge2.eid, - fid2: face1.eids[i], - dist, - }); - } else { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.eids[i], - fid2: edge2.eid, - dist, - }); - } - } - } - } - } - - // Project the two points from the edge into the face. - let normal1 = - (face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1])); - - 'point_loop2: for i in 0..2 { - let p2 = projected_edge2[i]; - - let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3])); - for j in 0..3 { - let new_sign = - (projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j])); - if new_sign * sign < 0.0 { - // The point lies outside. - continue 'point_loop2; - } - } - - // All the perp had the same sign: the point is inside of the other shapes projection. - // Output the contact. - let denom = -normal1.dot(&sep_axis1); - let dist = (face1.vertices[0] - edge2.vertices[i]).dot(&normal1) / denom; - let local_p2 = edge2.vertices[i]; - let local_p1 = edge2.vertices[i] - dist * sep_axis1; - - if dist < prediction_distance { - if flipped { - manifold.points.push(Contact { - local_p1: local_p2, - // All points are expressed in the locale space of the first shape - // (even if there was a flip). So the point we need to transform by - // pos21 is the one that will go into local_p2. - local_p2: pos21 * local_p1, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: edge2.vids[i], - fid2: face1.fid, - dist, - }); - } else { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.fid, - fid2: edge2.vids[i], - dist, - }); - } - } - } - } - - /// Compute contacts points between two edges. - /// - /// This method assume we already know that at least one contact exists. - pub fn edge_edge_contacts( - edge1: &CuboidFeatureEdge, - sep_axis1: &Vector, - edge2: &CuboidFeatureEdge, - pos21: &Isometry, - manifold: &mut ContactManifold, - ) { - let basis = sep_axis1.orthonormal_basis(); - let projected_edge1 = [ - Point2::new( - edge1.vertices[0].coords.dot(&basis[0]), - edge1.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - edge1.vertices[1].coords.dot(&basis[0]), - edge1.vertices[1].coords.dot(&basis[1]), - ), - ]; - let projected_edge2 = [ - Point2::new( - edge2.vertices[0].coords.dot(&basis[0]), - edge2.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - edge2.vertices[1].coords.dot(&basis[0]), - edge2.vertices[1].coords.dot(&basis[1]), - ), - ]; - - if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) { - let local_p1 = - edge1.vertices[0] * (1.0 - bcoords.0) + edge1.vertices[1].coords * bcoords.0; - let local_p2 = - edge2.vertices[0] * (1.0 - bcoords.1) + edge2.vertices[1].coords * bcoords.1; - let dist = (local_p2 - local_p1).dot(&sep_axis1); - - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, // NOTE: local_p2 is expressed in the local space of cube1. - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: edge1.eid, - fid2: edge2.eid, - dist, - }); - } - } - - pub fn face_face_contacts( - _prediction_distance: f32, - face1: &CuboidFeatureFace, - sep_axis1: &Vector, - face2: &CuboidFeatureFace, - pos21: &Isometry, - manifold: &mut ContactManifold, - ) { - // Project the faces to a 2D plane for contact clipping. - // The plane they are projected onto has normal sep_axis1 - // and contains the origin (this is numerically OK because - // we are not working in world-space here). - let basis = sep_axis1.orthonormal_basis(); - let projected_face1 = [ - Point2::new( - face1.vertices[0].coords.dot(&basis[0]), - face1.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[1].coords.dot(&basis[0]), - face1.vertices[1].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[2].coords.dot(&basis[0]), - face1.vertices[2].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[3].coords.dot(&basis[0]), - face1.vertices[3].coords.dot(&basis[1]), - ), - ]; - let projected_face2 = [ - Point2::new( - face2.vertices[0].coords.dot(&basis[0]), - face2.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - face2.vertices[1].coords.dot(&basis[0]), - face2.vertices[1].coords.dot(&basis[1]), - ), - Point2::new( - face2.vertices[2].coords.dot(&basis[0]), - face2.vertices[2].coords.dot(&basis[1]), - ), - Point2::new( - face2.vertices[3].coords.dot(&basis[0]), - face2.vertices[3].coords.dot(&basis[1]), - ), - ]; - - // Also find all the vertices located inside of the other projected face. - let normal1 = - (face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1])); - let normal2 = - (face2.vertices[2] - face2.vertices[1]).cross(&(face2.vertices[0] - face2.vertices[1])); - - // NOTE: The loop iterating on all the vertices for face1 is different from - // the one iterating on all the vertices of face2. In the second loop, we - // classify every point wrt. every edge on the other face. This will give - // us bit masks to filter out several edge-edge intersections. - 'point_loop1: for i in 0..4 { - let p1 = projected_face1[i]; - - let sign = (projected_face2[0] - projected_face2[3]).perp(&(p1 - projected_face2[3])); - for j in 0..3 { - let new_sign = - (projected_face2[j + 1] - projected_face2[j]).perp(&(p1 - projected_face2[j])); - if new_sign * sign < 0.0 { - // The point lies outside. - continue 'point_loop1; - } - } - - // All the perp had the same sign: the point is inside of the other shapes projection. - // Output the contact. - let denom = normal2.dot(&sep_axis1); - let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom; - let local_p1 = face1.vertices[i]; - let local_p2 = face1.vertices[i] + dist * sep_axis1; - - if true { - // dist <= prediction_distance { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.vids[i], - fid2: face2.fid, - dist, - }); - } - } - - let is_clockwise1 = (projected_face1[0] - projected_face1[1]) - .perp(&(projected_face1[2] - projected_face1[1])) - >= 0.0; - let mut vertex_class2 = [0u8; 4]; - - for i in 0..4 { - let p2 = projected_face2[i]; - - let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3])); - vertex_class2[i] |= ((sign >= 0.0) as u8) << 3; - - for j in 0..3 { - let sign = - (projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j])); - vertex_class2[i] |= ((sign >= 0.0) as u8) << j; - } - - if !is_clockwise1 { - vertex_class2[i] = (!vertex_class2[i]) & 0b01111; - } - - if vertex_class2[i] == 0 { - // All the perp had the same sign: the point is inside of the other shapes projection. - // Output the contact. - let denom = -normal1.dot(&sep_axis1); - let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom; - let local_p2 = face2.vertices[i]; - let local_p1 = face2.vertices[i] - dist * sep_axis1; - - if true { - // dist < prediction_distance { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.fid, - fid2: face2.vids[i], - dist, - }); - } - } - } - - // Now we have to compute the intersection between all pairs of - // edges from the face 1 and from the face2. - for j in 0..4 { - let projected_edge2 = [projected_face2[j], projected_face2[(j + 1) % 4]]; - - if (vertex_class2[j] & vertex_class2[(j + 1) % 4]) != 0 { - continue; - } - - let edge_class2 = vertex_class2[j] | vertex_class2[(j + 1) % 4]; - - for i in 0..4 { - if (edge_class2 & (1 << i)) != 0 { - let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]]; - if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) { - if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 - { - // Found a contact between the two edges. - let edge1 = (face1.vertices[i], face1.vertices[(i + 1) % 4]); - let edge2 = (face2.vertices[j], face2.vertices[(j + 1) % 4]); - let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0; - let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1; - let dist = (local_p2 - local_p1).dot(&sep_axis1); - - if true { - // dist <= prediction_distance { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.eids[i], - fid2: face2.eids[j], - dist, - }); - } - } - } - } - } - } - } -} - -/// Compute the barycentric coordinates of the intersection between the two given lines. -/// Returns `None` if the lines are parallel. -fn closest_points_line2d(edge1: [Point2; 2], edge2: [Point2; 2]) -> Option<(f32, f32)> { - use approx::AbsDiffEq; - - // Inspired by Real-time collision detection by Christer Ericson. - let dir1 = edge1[1] - edge1[0]; - let dir2 = edge2[1] - edge2[0]; - let r = edge1[0] - edge2[0]; - - let a = dir1.norm_squared(); - let e = dir2.norm_squared(); - let f = dir2.dot(&r); - - let eps = f32::default_epsilon(); - - if a <= eps && e <= eps { - Some((0.0, 0.0)) - } else if a <= eps { - Some((0.0, f / e)) - } else { - let c = dir1.dot(&r); - if e <= eps { - Some((-c / a, 0.0)) - } else { - let b = dir1.dot(&dir2); - let ae = a * e; - let bb = b * b; - let denom = ae - bb; - - // Use absolute and ulps error to test collinearity. - let parallel = denom <= eps || approx::ulps_eq!(ae, bb); - - let s = if !parallel { - (b * f - c * e) / denom - } else { - 0.0 - }; - - if parallel { - None - } else { - Some((s, (b * s + f) / e)) - } - } - } -} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index c8ad28e..caa229a 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -1,17 +1,11 @@ //! Structures related to geometry: colliders, shapes, etc. pub use self::broad_phase_multi_sap::BroadPhase; -pub use self::capsule::Capsule; pub use self::collider::{Collider, ColliderBuilder, ColliderShape}; pub use self::collider_set::{ColliderHandle, ColliderSet}; -pub use self::contact::{ - Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, SolverFlags, -}; +pub use self::contact::{ContactData, ContactManifoldData}; +pub use self::contact::{ContactPair, SolverFlags}; pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher}; -#[cfg(feature = "dim2")] -pub(crate) use self::cuboid_feature2d::{CuboidFeature, CuboidFeatureFace}; -#[cfg(feature = "dim3")] -pub(crate) use self::cuboid_feature3d::{CuboidFeature, CuboidFeatureFace}; pub use self::interaction_graph::{ ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex, }; @@ -23,36 +17,87 @@ pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatch pub use self::round_cylinder::RoundCylinder; pub use self::trimesh::Trimesh; pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; -pub use ncollide::query::Proximity; +pub use buckler::query::Proximity; +pub use buckler::query::{KinematicsCategory, TrackedContact}; + +pub type Contact = buckler::query::TrackedContact; +pub type ContactManifold = buckler::query::ContactManifold; /// A segment shape. -pub type Segment = ncollide::shape::Segment; +pub type Segment = buckler::shape::Segment; /// A cuboid shape. -pub type Cuboid = ncollide::shape::Cuboid; +pub type Cuboid = buckler::shape::Cuboid; /// A triangle shape. -pub type Triangle = ncollide::shape::Triangle; +pub type Triangle = buckler::shape::Triangle; /// A ball shape. -pub type Ball = ncollide::shape::Ball; +pub type Ball = buckler::shape::Ball; +/// A capsule shape. +pub type Capsule = buckler::shape::Capsule; /// A heightfield shape. -pub type HeightField = ncollide::shape::HeightField; +pub type HeightField = buckler::shape::HeightField; /// A cylindrical shape. #[cfg(feature = "dim3")] -pub type Cylinder = ncollide::shape::Cylinder; +pub type Cylinder = buckler::shape::Cylinder; /// A cone shape. #[cfg(feature = "dim3")] -pub type Cone = ncollide::shape::Cone; +pub type Cone = buckler::shape::Cone; /// An axis-aligned bounding box. -pub type AABB = ncollide::bounding_volume::AABB; -/// Event triggered when two non-sensor colliders start or stop being in contact. -pub type ContactEvent = ncollide::pipeline::ContactEvent; -/// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not). -pub type ProximityEvent = ncollide::pipeline::ProximityEvent; +pub type AABB = buckler::bounding_volume::AABB; /// A ray that can be cast against colliders. -pub type Ray = ncollide::query::Ray; +pub type Ray = buckler::query::Ray; /// The intersection between a ray and a collider. -pub type RayIntersection = ncollide::query::RayIntersection; +pub type RayIntersection = buckler::query::RayIntersection; /// The the projection of a point on a collider. -pub type PointProjection = ncollide::query::PointProjection; +pub type PointProjection = buckler::query::PointProjection; + +#[derive(Copy, Clone, Hash, Debug)] +/// Events occurring when two collision objects start or stop being in contact (or penetration). +pub enum ContactEvent { + /// Event occurring when two collision objects start being in contact. + /// + /// This event is generated whenever the narrow-phase finds a contact between two collision objects that did not have any contact at the last update. + Started(ColliderHandle, ColliderHandle), + /// Event occurring when two collision objects stop being in contact. + /// + /// This event is generated whenever the narrow-phase fails to find any contact between two collision objects that did have at least one contact at the last update. + Stopped(ColliderHandle, ColliderHandle), +} + +#[derive(Copy, Clone, Debug)] +/// Events occurring when two collision objects start or stop being in close proximity, contact, or disjoint. +pub struct ProximityEvent { + /// The first collider to which the proximity event applies. + pub collider1: ColliderHandle, + /// The second collider to which the proximity event applies. + pub collider2: ColliderHandle, + /// The previous state of proximity between the two collision objects. + pub prev_status: Proximity, + /// The new state of proximity between the two collision objects. + pub new_status: Proximity, +} + +impl ProximityEvent { + /// Instantiates a new proximity event. + /// + /// Panics if `prev_status` is equal to `new_status`. + pub fn new( + collider1: ColliderHandle, + collider2: ColliderHandle, + prev_status: Proximity, + new_status: Proximity, + ) -> Self { + assert_ne!( + prev_status, new_status, + "The previous and new status of a proximity event must not be the same." + ); + Self { + collider1, + collider2, + prev_status, + new_status, + } + } +} #[cfg(feature = "simd-is-enabled")] pub(crate) use self::ball::WBall; @@ -60,14 +105,7 @@ pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; -pub(crate) use self::contact_generator::clip_segments; -#[cfg(feature = "dim2")] -pub(crate) use self::contact_generator::clip_segments_with_normal; pub(crate) use self::narrow_phase::ContactManifoldIndex; -#[cfg(feature = "dim3")] -pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap; -#[cfg(feature = "dim3")] -pub(crate) use self::polyhedron_feature3d::PolyhedronFace; pub(crate) use self::waabb::{WRay, WAABB}; pub(crate) use self::wquadtree::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; @@ -80,16 +118,9 @@ mod collider; mod collider_set; mod contact; mod contact_generator; -pub(crate) mod cuboid; -#[cfg(feature = "dim2")] -mod cuboid_feature2d; -#[cfg(feature = "dim3")] -mod cuboid_feature3d; mod interaction_graph; mod narrow_phase; mod polygon; -#[cfg(feature = "dim3")] -mod polyhedron_feature3d; mod proximity; mod proximity_detector; pub(crate) mod sat; @@ -98,11 +129,8 @@ mod trimesh; mod waabb; mod wquadtree; //mod z_order; -mod capsule; mod interaction_groups; #[cfg(feature = "dim3")] -mod polygonal_feature_map; -#[cfg(feature = "dim3")] mod round_cylinder; mod shape; mod user_callbacks; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index a21d3c6..463dd99 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -21,9 +21,9 @@ use crate::geometry::{ use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] //use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::buckler::query::Proximity; use crate::data::pubsub::Subscription; use crate::data::Coarena; -use crate::ncollide::query::Proximity; use crate::pipeline::EventHandler; use std::collections::HashMap; //use simba::simd::SimdValue; @@ -568,9 +568,10 @@ impl NarrowPhase { // FIXME: don't iterate through all the interactions. for inter in self.contact_graph.graph.edges.iter_mut() { for manifold in &mut inter.weight.manifolds { - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; if manifold + .data .solver_flags .contains(SolverFlags::COMPUTE_IMPULSES) && manifold.num_active_contacts() != 0 diff --git a/src/geometry/polygon.rs b/src/geometry/polygon.rs index d07cfd9..0bc3e56 100644 --- a/src/geometry/polygon.rs +++ b/src/geometry/polygon.rs @@ -1,7 +1,7 @@ #![allow(dead_code)] // TODO: remove this once we support polygons. use crate::math::{Isometry, Point, Vector}; -use ncollide::bounding_volume::AABB; +use buckler::bounding_volume::AABB; #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -27,7 +27,7 @@ impl Polygon { } /// Compute the axis-aligned bounding box of the polygon. - pub fn aabb(&self, pos: &Isometry) -> AABB { + pub fn aabb(&self, pos: &Isometry) -> AABB { let p0 = pos * self.vertices[0]; let mut mins = p0; let mut maxs = p0; diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs deleted file mode 100644 index 2a8fc8d..0000000 --- a/src/geometry/polygonal_feature_map.rs +++ /dev/null @@ -1,132 +0,0 @@ -use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Segment, Triangle}; -use crate::math::{Point, Vector}; -use approx::AbsDiffEq; -use na::{Unit, Vector2}; -use ncollide::shape::SupportMap; - -/// Trait implemented by convex shapes with features with polyhedral approximations. -pub trait PolygonalFeatureMap: SupportMap { - fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace); -} - -impl PolygonalFeatureMap for Segment { - fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { - *out_feature = PolyhedronFace::from(*self); - } -} - -impl PolygonalFeatureMap for Triangle { - fn local_support_feature(&self, _: &Unit>, out_feature: &mut PolyhedronFace) { - *out_feature = PolyhedronFace::from(*self); - } -} - -impl PolygonalFeatureMap for Cuboid { - fn local_support_feature(&self, dir: &Unit>, out_feature: &mut PolyhedronFace) { - let face = cuboid::support_face(self, **dir); - *out_feature = PolyhedronFace::from(face); - } -} - -impl PolygonalFeatureMap for Cylinder { - fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { - // About feature ids. - // At all times, we consider our cylinder to be approximated as follows: - // - The curved part is approximated by a single segment. - // - Each flat cap of the cylinder is approximated by a square. - // - The curved-part segment has a feature ID of 0, and its endpoint with negative - // `y` coordinate has an ID of 1. - // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order - // when looking at the cap with an eye looking towards +y). - // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order. - // - The bottom cap has its face feature ID of 9. - // - The feature IDs of the top cap are the same as the bottom cap to which we add 10. - // So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19. - // - Note that at all times, one of each cap's vertices are the same as the curved-part - // segment endpoints. - let dir2 = Vector2::new(dir.x, dir.z) - .try_normalize(f32::default_epsilon()) - .unwrap_or(Vector2::x()); - - if dir.y.abs() < 0.5 { - // We return a segment lying on the cylinder's curved part. - out_features.vertices[0] = Point::new( - dir2.x * self.radius, - -self.half_height, - dir2.y * self.radius, - ); - out_features.vertices[1] = - Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius); - out_features.eids = [0, 0, 0, 0]; - out_features.fid = 0; - out_features.num_vertices = 2; - out_features.vids = [1, 11, 11, 11]; - } else { - // We return a square approximation of the cylinder cap. - let y = self.half_height.copysign(dir.y); - out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius); - out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius); - out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius); - out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius); - - if dir.y < 0.0 { - out_features.eids = [2, 4, 6, 8]; - out_features.fid = 9; - out_features.num_vertices = 4; - out_features.vids = [1, 3, 5, 7]; - } else { - out_features.eids = [12, 14, 16, 18]; - out_features.fid = 19; - out_features.num_vertices = 4; - out_features.vids = [11, 13, 15, 17]; - } - } - } -} - -impl PolygonalFeatureMap for Cone { - fn local_support_feature(&self, dir: &Unit>, out_features: &mut PolyhedronFace) { - // About feature ids. It is very similar to the feature ids of cylinders. - // At all times, we consider our cone to be approximated as follows: - // - The curved part is approximated by a single segment. - // - The flat cap of the cone is approximated by a square. - // - The curved-part segment has a feature ID of 0, and its endpoint with negative - // `y` coordinate has an ID of 1. - // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order - // when looking at the cap with an eye looking towards +y). - // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order. - // - The bottom cap has its face feature ID of 9. - // - Note that at all times, one of the cap's vertices are the same as the curved-part - // segment endpoints. - let dir2 = Vector2::new(dir.x, dir.z) - .try_normalize(f32::default_epsilon()) - .unwrap_or(Vector2::x()); - - if dir.y > 0.0 { - // We return a segment lying on the cone's curved part. - out_features.vertices[0] = Point::new( - dir2.x * self.radius, - -self.half_height, - dir2.y * self.radius, - ); - out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0); - out_features.eids = [0, 0, 0, 0]; - out_features.fid = 0; - out_features.num_vertices = 2; - out_features.vids = [1, 11, 11, 11]; - } else { - // We return a square approximation of the cone cap. - let y = -self.half_height; - out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius); - out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius); - out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius); - out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius); - - out_features.eids = [2, 4, 6, 8]; - out_features.fid = 9; - out_features.num_vertices = 4; - out_features.vids = [1, 3, 5, 7]; - } - } -} diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs deleted file mode 100644 index 5ad62df..0000000 --- a/src/geometry/polyhedron_feature3d.rs +++ /dev/null @@ -1,445 +0,0 @@ -use crate::approx::AbsDiffEq; -use crate::geometry::{self, Contact, ContactManifold, CuboidFeatureFace, Triangle}; -use crate::math::{Isometry, Point, Vector}; -use crate::utils::WBasis; -use na::Point2; -use ncollide::shape::Segment; - -#[derive(Debug, Clone)] -pub struct PolyhedronFace { - pub vertices: [Point; 4], - pub vids: [u8; 4], // Feature ID of the vertices. - pub eids: [u8; 4], // Feature ID of the edges. - pub fid: u8, // Feature ID of the face. - pub num_vertices: usize, -} - -impl Default for PolyhedronFace { - fn default() -> Self { - Self { - vertices: [Point::origin(); 4], - vids: [0; 4], - eids: [0; 4], - fid: 0, - num_vertices: 0, - } - } -} - -impl From for PolyhedronFace { - fn from(face: CuboidFeatureFace) -> Self { - Self { - vertices: face.vertices, - vids: face.vids, - eids: face.eids, - fid: face.fid, - num_vertices: 4, - } - } -} - -impl From for PolyhedronFace { - fn from(tri: Triangle) -> Self { - Self { - vertices: [tri.a, tri.b, tri.c, tri.c], - vids: [0, 2, 4, 4], - eids: [1, 3, 5, 5], - fid: 0, - num_vertices: 3, - } - } -} - -impl From> for PolyhedronFace { - fn from(seg: Segment) -> Self { - // Vertices have feature ids 0 and 2. - // The segment interior has feature id 1. - Self { - vertices: [seg.a, seg.b, seg.b, seg.b], - vids: [0, 2, 2, 2], - eids: [1, 1, 1, 1], - fid: 0, - num_vertices: 2, - } - } -} - -impl PolyhedronFace { - pub fn new() -> Self { - Self { - vertices: [Point::origin(); 4], - vids: [0; 4], - eids: [0; 4], - fid: 0, - num_vertices: 0, - } - } - - pub fn transform_by(&mut self, iso: &Isometry) { - for p in &mut self.vertices[0..self.num_vertices] { - *p = iso * *p; - } - } - - pub fn contacts( - prediction_distance: f32, - face1: &PolyhedronFace, - sep_axis1: &Vector, - face2: &PolyhedronFace, - pos21: &Isometry, - manifold: &mut ContactManifold, - ) { - match (face1.num_vertices, face2.num_vertices) { - (2, 2) => Self::contacts_edge_edge( - prediction_distance, - face1, - sep_axis1, - face2, - pos21, - manifold, - ), - _ => Self::contacts_face_face( - prediction_distance, - face1, - sep_axis1, - face2, - pos21, - manifold, - ), - } - } - - fn contacts_edge_edge( - prediction_distance: f32, - face1: &PolyhedronFace, - sep_axis1: &Vector, - face2: &PolyhedronFace, - pos21: &Isometry, - manifold: &mut ContactManifold, - ) { - // Project the faces to a 2D plane for contact clipping. - // The plane they are projected onto has normal sep_axis1 - // and contains the origin (this is numerically OK because - // we are not working in world-space here). - let basis = sep_axis1.orthonormal_basis(); - let projected_edge1 = [ - Point2::new( - face1.vertices[0].coords.dot(&basis[0]), - face1.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[1].coords.dot(&basis[0]), - face1.vertices[1].coords.dot(&basis[1]), - ), - ]; - let projected_edge2 = [ - Point2::new( - face2.vertices[0].coords.dot(&basis[0]), - face2.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - face2.vertices[1].coords.dot(&basis[0]), - face2.vertices[1].coords.dot(&basis[1]), - ), - ]; - - let tangent1 = - (projected_edge1[1] - projected_edge1[0]).try_normalize(f32::default_epsilon()); - let tangent2 = - (projected_edge2[1] - projected_edge2[0]).try_normalize(f32::default_epsilon()); - - // TODO: not sure what the best value for eps is. - // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small. - if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) { - let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8; - - if !parallel { - let seg1 = (&projected_edge1[0], &projected_edge1[1]); - let seg2 = (&projected_edge2[0], &projected_edge2[1]); - let (loc1, loc2) = - ncollide::query::closest_points_segment_segment_with_locations_nD(seg1, seg2); - - // Found a contact between the two edges. - let bcoords1 = loc1.barycentric_coordinates(); - let bcoords2 = loc2.barycentric_coordinates(); - - let edge1 = (face1.vertices[0], face1.vertices[1]); - let edge2 = (face2.vertices[0], face2.vertices[1]); - let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1]; - let local_p2 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1]; - let dist = (local_p2 - local_p1).dot(&sep_axis1); - - if dist <= prediction_distance { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.eids[0], - fid2: face2.eids[0], - dist, - }); - } - - return; - } - } - - // The lines are parallel so we are having a conformal contact. - // Let's use a range-based clipping to extract two contact points. - // TODO: would it be better and/or more efficient to do the - //clipping in 2D? - if let Some(clips) = geometry::clip_segments( - (face1.vertices[0], face1.vertices[1]), - (face2.vertices[0], face2.vertices[1]), - ) { - manifold.points.push(Contact { - local_p1: (clips.0).0, - local_p2: pos21 * (clips.0).1, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: 0, // FIXME - fid2: 0, // FIXME - dist: ((clips.0).1 - (clips.0).0).dot(&sep_axis1), - }); - - manifold.points.push(Contact { - local_p1: (clips.1).0, - local_p2: pos21 * (clips.1).1, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: 0, // FIXME - fid2: 0, // FIXME - dist: ((clips.1).1 - (clips.1).0).dot(&sep_axis1), - }); - } - } - - fn contacts_face_face( - prediction_distance: f32, - face1: &PolyhedronFace, - sep_axis1: &Vector, - face2: &PolyhedronFace, - pos21: &Isometry, - manifold: &mut ContactManifold, - ) { - // Project the faces to a 2D plane for contact clipping. - // The plane they are projected onto has normal sep_axis1 - // and contains the origin (this is numerically OK because - // we are not working in world-space here). - let basis = sep_axis1.orthonormal_basis(); - let projected_face1 = [ - Point2::new( - face1.vertices[0].coords.dot(&basis[0]), - face1.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[1].coords.dot(&basis[0]), - face1.vertices[1].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[2].coords.dot(&basis[0]), - face1.vertices[2].coords.dot(&basis[1]), - ), - Point2::new( - face1.vertices[3].coords.dot(&basis[0]), - face1.vertices[3].coords.dot(&basis[1]), - ), - ]; - let projected_face2 = [ - Point2::new( - face2.vertices[0].coords.dot(&basis[0]), - face2.vertices[0].coords.dot(&basis[1]), - ), - Point2::new( - face2.vertices[1].coords.dot(&basis[0]), - face2.vertices[1].coords.dot(&basis[1]), - ), - Point2::new( - face2.vertices[2].coords.dot(&basis[0]), - face2.vertices[2].coords.dot(&basis[1]), - ), - Point2::new( - face2.vertices[3].coords.dot(&basis[0]), - face2.vertices[3].coords.dot(&basis[1]), - ), - ]; - - // Also find all the vertices located inside of the other projected face. - if face2.num_vertices > 2 { - let normal2 = (face2.vertices[2] - face2.vertices[1]) - .cross(&(face2.vertices[0] - face2.vertices[1])); - let denom = normal2.dot(&sep_axis1); - - if !relative_eq!(denom, 0.0) { - let last_index2 = face2.num_vertices as usize - 1; - 'point_loop1: for i in 0..face1.num_vertices as usize { - let p1 = projected_face1[i]; - - let sign = (projected_face2[0] - projected_face2[last_index2]) - .perp(&(p1 - projected_face2[last_index2])); - for j in 0..last_index2 { - let new_sign = (projected_face2[j + 1] - projected_face2[j]) - .perp(&(p1 - projected_face2[j])); - if new_sign * sign < 0.0 { - // The point lies outside. - continue 'point_loop1; - } - } - - // All the perp had the same sign: the point is inside of the other shapes projection. - // Output the contact. - let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom; - let local_p1 = face1.vertices[i]; - let local_p2 = face1.vertices[i] + dist * sep_axis1; - - if dist <= prediction_distance { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.vids[i], - fid2: face2.fid, - dist, - }); - } - } - } - } - - if face1.num_vertices > 2 { - let normal1 = (face1.vertices[2] - face1.vertices[1]) - .cross(&(face1.vertices[0] - face1.vertices[1])); - - let denom = -normal1.dot(&sep_axis1); - if !relative_eq!(denom, 0.0) { - let last_index1 = face1.num_vertices as usize - 1; - 'point_loop2: for i in 0..face2.num_vertices as usize { - let p2 = projected_face2[i]; - - let sign = (projected_face1[0] - projected_face1[last_index1]) - .perp(&(p2 - projected_face1[last_index1])); - for j in 0..last_index1 { - let new_sign = (projected_face1[j + 1] - projected_face1[j]) - .perp(&(p2 - projected_face1[j])); - - if new_sign * sign < 0.0 { - // The point lies outside. - continue 'point_loop2; - } - } - - // All the perp had the same sign: the point is inside of the other shapes projection. - // Output the contact. - let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom; - let local_p2 = face2.vertices[i]; - let local_p1 = face2.vertices[i] - dist * sep_axis1; - - if true { - // dist <= prediction_distance { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.fid, - fid2: face2.vids[i], - dist, - }); - } - } - } - } - - // Now we have to compute the intersection between all pairs of - // edges from the face 1 and from the face2. - for j in 0..face2.num_vertices { - let projected_edge2 = [ - projected_face2[j], - projected_face2[(j + 1) % face2.num_vertices], - ]; - - for i in 0..face1.num_vertices { - let projected_edge1 = [ - projected_face1[i], - projected_face1[(i + 1) % face1.num_vertices], - ]; - if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) { - if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 { - // Found a contact between the two edges. - let edge1 = ( - face1.vertices[i], - face1.vertices[(i + 1) % face1.num_vertices], - ); - let edge2 = ( - face2.vertices[j], - face2.vertices[(j + 1) % face2.num_vertices], - ); - let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0; - let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1; - let dist = (local_p2 - local_p1).dot(&sep_axis1); - - if dist <= prediction_distance { - manifold.points.push(Contact { - local_p1, - local_p2: pos21 * local_p2, - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), - fid1: face1.eids[i], - fid2: face2.eids[j], - dist, - }); - } - } - } - } - } - } -} - -/// Compute the barycentric coordinates of the intersection between the two given lines. -/// Returns `None` if the lines are parallel. -fn closest_points_line2d(edge1: [Point2; 2], edge2: [Point2; 2]) -> Option<(f32, f32)> { - // Inspired by Real-time collision detection by Christer Ericson. - let dir1 = edge1[1] - edge1[0]; - let dir2 = edge2[1] - edge2[0]; - let r = edge1[0] - edge2[0]; - - let a = dir1.norm_squared(); - let e = dir2.norm_squared(); - let f = dir2.dot(&r); - - let eps = f32::default_epsilon(); - - if a <= eps && e <= eps { - Some((0.0, 0.0)) - } else if a <= eps { - Some((0.0, f / e)) - } else { - let c = dir1.dot(&r); - if e <= eps { - Some((-c / a, 0.0)) - } else { - let b = dir1.dot(&dir2); - let ae = a * e; - let bb = b * b; - let denom = ae - bb; - - // Use absolute and ulps error to test collinearity. - let parallel = denom <= eps || approx::ulps_eq!(ae, bb); - - let s = if !parallel { - (b * f - c * e) / denom - } else { - 0.0 - }; - - if parallel { - None - } else { - Some((s, (b * s + f) / e)) - } - } - } -} diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs index eda6547..5c1a105 100644 --- a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs @@ -1,7 +1,7 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; use crate::geometry::{Ball, Proximity}; use crate::math::Isometry; -use ncollide::query::PointQuery; +use buckler::query::PointQuery; pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { if let Some(ball1) = ctxt.shape1.as_ball() { @@ -13,7 +13,7 @@ pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContex } } -fn do_detect_proximity>( +fn do_detect_proximity( point_query1: &P, ball2: &Ball, ctxt: &PrimitiveProximityDetectionContext, @@ -22,11 +22,8 @@ fn do_detect_proximity>( .position1 .inverse_transform_point(&ctxt.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; let dist = dpos.norm(); if proj.is_inside { diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs index ae885b3..93a5103 100644 --- a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs @@ -1,7 +1,8 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Proximity}; +use crate::geometry::Proximity; use crate::math::Isometry; -use ncollide::shape::Cuboid; +use buckler::query::sat; +use buckler::shape::Cuboid; pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { @@ -19,9 +20,9 @@ pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionCont pub fn detect_proximity<'a>( prediction_distance: f32, - cube1: &'a Cuboid, + cube1: &'a Cuboid, pos1: &'a Isometry, - cube2: &'a Cuboid, + cube2: &'a Cuboid, pos2: &'a Isometry, ) -> Proximity { let pos12 = pos1.inverse() * pos2; @@ -32,14 +33,12 @@ pub fn detect_proximity<'a>( * Point-Face * */ - let sep1 = - sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21).0; + let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12).0; if sep1 > prediction_distance { return Proximity::Disjoint; } - let sep2 = - sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12).0; + let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21).0; if sep2 > prediction_distance { return Proximity::Disjoint; } @@ -52,7 +51,7 @@ pub fn detect_proximity<'a>( #[cfg(feature = "dim2")] let sep3 = -f32::MAX; // This case does not exist in 2D. #[cfg(feature = "dim3")] - let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21).0; + let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12).0; if sep3 > prediction_distance { return Proximity::Disjoint; } diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs index 532ab36..922855a 100644 --- a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs @@ -1,6 +1,7 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Cuboid, Proximity, Triangle}; +use crate::geometry::{Cuboid, Proximity, Triangle}; use crate::math::Isometry; +use buckler::query::sat; pub fn detect_proximity_cuboid_triangle( ctxt: &mut PrimitiveProximityDetectionContext, @@ -44,7 +45,7 @@ pub fn detect_proximity<'a>( * */ let sep1 = - sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12).0; + sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12).0; if sep1 > prediction_distance { return Proximity::Disjoint; } @@ -62,8 +63,7 @@ pub fn detect_proximity<'a>( #[cfg(feature = "dim2")] let sep3 = -f32::MAX; // This case does not exist in 2D. #[cfg(feature = "dim3")] - let sep3 = - sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21).0; + let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12).0; if sep3 > prediction_distance { return Proximity::Disjoint; } diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index b469637..5366b39 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -1,12 +1,12 @@ +use crate::buckler::bounding_volume::{BoundingVolume, AABB}; use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; use crate::geometry::{Collider, Proximity, ShapeType, Trimesh}; -use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeProximityDetectorWorkspace { interferences: Vec, - local_aabb2: AABB, + local_aabb2: AABB, old_interferences: Vec, } diff --git a/src/geometry/round_cylinder.rs b/src/geometry/round_cylinder.rs index ce8b43b..450a9fc 100644 --- a/src/geometry/round_cylinder.rs +++ b/src/geometry/round_cylinder.rs @@ -1,10 +1,10 @@ use crate::geometry::Cylinder; use crate::math::{Isometry, Point, Vector}; -use na::Unit; -use ncollide::query::{ - algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection, +use buckler::query::{ + gjk::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection, }; -use ncollide::shape::{FeatureId, SupportMap}; +use buckler::shape::{FeatureId, SupportMap}; +use na::Unit; /// A rounded cylinder. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -28,7 +28,7 @@ impl RoundCylinder { } } -impl SupportMap for RoundCylinder { +impl SupportMap for RoundCylinder { fn local_support_point(&self, dir: &Vector) -> Point { self.local_support_point_toward(&Unit::new_normalize(*dir)) } @@ -52,43 +52,29 @@ impl SupportMap for RoundCylinder { } } -impl RayCast for RoundCylinder { - fn toi_and_normal_with_ray( +impl RayCast for RoundCylinder { + fn cast_local_ray_and_get_normal( &self, - m: &Isometry, - ray: &Ray, + ray: &Ray, max_toi: f32, solid: bool, - ) -> Option> { - let ls_ray = ray.inverse_transform_by(m); - - ncollide::query::ray_intersection_with_support_map_with_params( - &Isometry::identity(), + ) -> Option { + buckler::query::details::local_ray_intersection_with_support_map_with_params( self, &mut VoronoiSimplex::new(), - &ls_ray, + ray, max_toi, solid, ) - .map(|mut res| { - res.normal = m * res.normal; - res - }) } } // TODO: if PointQuery had a `project_point_with_normal` method, we could just // call this and adjust the projected point accordingly. -impl PointQuery for RoundCylinder { +impl PointQuery for RoundCylinder { #[inline] - fn project_point( - &self, - m: &Isometry, - point: &Point, - solid: bool, - ) -> PointProjection { - ncollide::query::point_projection_on_support_map( - m, + fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { + buckler::query::details::local_point_projection_on_support_map( self, &mut VoronoiSimplex::new(), point, @@ -97,11 +83,10 @@ impl PointQuery for RoundCylinder { } #[inline] - fn project_point_with_feature( + fn project_local_point_and_get_feature( &self, - m: &Isometry, point: &Point, - ) -> (PointProjection, FeatureId) { - (self.project_point(m, point, false), FeatureId::Unknown) + ) -> (PointProjection, FeatureId) { + (self.project_local_point(point, false), FeatureId::Unknown) } } diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 3cd66b4..08fd38a 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -1,20 +1,7 @@ -use crate::geometry::{cuboid, Cuboid, Polygon, Segment, Triangle}; +use crate::geometry::{Cuboid, Polygon, Segment, Triangle}; use crate::math::{Isometry, Point, Vector, DIM}; use crate::utils::WSign; use na::Unit; -use ncollide::shape::SupportMap; - -#[allow(dead_code)] -pub fn support_map_support_map_compute_separation( - sm1: &impl SupportMap, - sm2: &impl SupportMap, - m12: &Isometry, - dir1: &Unit>, -) -> f32 { - let p1 = sm1.local_support_point_toward(dir1); - let p2 = sm2.support_point_toward(m12, &-*dir1); - (p2 - p1).dot(dir1) -} #[allow(dead_code)] pub fn polygon_polygon_compute_separation_features( @@ -38,332 +25,3 @@ pub fn polygon_polygon_compute_separation_features( (max_separation, separation_features.0, separation_features.1) } - -#[cfg(feature = "dim3")] -pub fn cuboid_cuboid_compute_separation_wrt_local_line( - cube1: &Cuboid, - cube2: &Cuboid, - pos12: &Isometry, - pos21: &Isometry, - axis1: &Vector, -) -> (f32, Vector) { - let signum = pos12.translation.vector.dot(axis1).copy_sign_to(1.0); - let axis1 = axis1 * signum; - let local_pt1 = cuboid::local_support_point(cube1, axis1); - let local_pt2 = cuboid::local_support_point(cube2, pos21 * -axis1); - let pt2 = pos12 * local_pt2; - let separation = (pt2 - local_pt1).dot(&axis1); - (separation, axis1) -} - -#[cfg(feature = "dim3")] -pub fn cuboid_cuboid_find_local_separating_edge_twoway( - cube1: &Cuboid, - cube2: &Cuboid, - pos12: &Isometry, - pos21: &Isometry, -) -> (f32, Vector) { - use approx::AbsDiffEq; - let mut best_separation = -std::f32::MAX; - let mut best_dir = Vector::zeros(); - - let x2 = pos12 * Vector::x(); - let y2 = pos12 * Vector::y(); - let z2 = pos12 * Vector::z(); - - // We have 3 * 3 = 9 axes to test. - let axes = [ - // Vector::{x, y ,z}().cross(y2) - Vector::new(0.0, -x2.z, x2.y), - Vector::new(x2.z, 0.0, -x2.x), - Vector::new(-x2.y, x2.x, 0.0), - // Vector::{x, y ,z}().cross(y2) - Vector::new(0.0, -y2.z, y2.y), - Vector::new(y2.z, 0.0, -y2.x), - Vector::new(-y2.y, y2.x, 0.0), - // Vector::{x, y ,z}().cross(y2) - Vector::new(0.0, -z2.z, z2.y), - Vector::new(z2.z, 0.0, -z2.x), - Vector::new(-z2.y, z2.x, 0.0), - ]; - - for axis1 in &axes { - let norm1 = axis1.norm(); - if norm1 > f32::default_epsilon() { - let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line( - cube1, - cube2, - pos12, - pos21, - &(axis1 / norm1), - ); - - if separation > best_separation { - best_separation = separation; - best_dir = axis1; - } - } - } - - (best_separation, best_dir) -} - -pub fn cuboid_cuboid_find_local_separating_normal_oneway( - cube1: &Cuboid, - cube2: &Cuboid, - pos12: &Isometry, - pos21: &Isometry, -) -> (f32, Vector) { - let mut best_separation = -std::f32::MAX; - let mut best_dir = Vector::zeros(); - - for i in 0..DIM { - let sign = pos12.translation.vector[i].copy_sign_to(1.0); - let axis1 = Vector::ith(i, sign); - let local_pt2 = cuboid::local_support_point(cube2, pos21 * -axis1); - let pt2 = pos12 * local_pt2; - let separation = pt2[i] * sign - cube1.half_extents[i]; - - if separation > best_separation { - best_separation = separation; - best_dir = axis1; - } - } - - (best_separation, best_dir) -} - -/* - * - * - * Triangles. - * - * - */ -#[cfg(feature = "dim3")] -pub fn cube_support_map_compute_separation_wrt_local_line>( - cube1: &Cuboid, - shape2: &S, - pos12: &Isometry, - pos21: &Isometry, - axis1: &Unit>, -) -> (f32, Unit>) { - let signum = pos12.translation.vector.dot(axis1).copy_sign_to(1.0); - let axis1 = Unit::new_unchecked(**axis1 * signum); - let local_pt1 = cuboid::local_support_point(cube1, *axis1); - let local_pt2 = shape2.local_support_point_toward(&(pos21 * -axis1)); - let pt2 = pos12 * local_pt2; - let separation = (pt2 - local_pt1).dot(&axis1); - (separation, axis1) -} - -#[cfg(feature = "dim3")] -pub fn cube_support_map_find_local_separating_edge_twoway( - cube1: &Cuboid, - shape2: &impl SupportMap, - axes: &[Vector], - pos12: &Isometry, - pos21: &Isometry, -) -> (f32, Vector) { - use approx::AbsDiffEq; - let mut best_separation = -std::f32::MAX; - let mut best_dir = Vector::zeros(); - - for axis1 in axes { - if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) { - let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line( - cube1, shape2, pos12, pos21, &axis1, - ); - - if separation > best_separation { - best_separation = separation; - best_dir = *axis1; - } - } - } - - (best_separation, best_dir) -} - -#[cfg(feature = "dim3")] -pub fn cube_triangle_find_local_separating_edge_twoway( - cube1: &Cuboid, - triangle2: &Triangle, - pos12: &Isometry, - pos21: &Isometry, -) -> (f32, Vector) { - let x2 = pos12 * (triangle2.b - triangle2.a); - let y2 = pos12 * (triangle2.c - triangle2.b); - let z2 = pos12 * (triangle2.a - triangle2.c); - - // We have 3 * 3 = 3 axes to test. - let axes = [ - // Vector::{x, y ,z}().cross(y2) - Vector::new(0.0, -x2.z, x2.y), - Vector::new(x2.z, 0.0, -x2.x), - Vector::new(-x2.y, x2.x, 0.0), - // Vector::{x, y ,z}().cross(y2) - Vector::new(0.0, -y2.z, y2.y), - Vector::new(y2.z, 0.0, -y2.x), - Vector::new(-y2.y, y2.x, 0.0), - // Vector::{x, y ,z}().cross(y2) - Vector::new(0.0, -z2.z, z2.y), - Vector::new(z2.z, 0.0, -z2.x), - Vector::new(-z2.y, z2.x, 0.0), - ]; - - cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axes, pos12, pos21) -} - -#[cfg(feature = "dim3")] -pub fn cube_segment_find_local_separating_edge_twoway( - cube1: &Cuboid, - segment2: &Segment, - pos12: &Isometry, - pos21: &Isometry, -) -> (f32, Vector) { - let x2 = pos12 * (segment2.b - segment2.a); - - let axes = [ - // Vector::{x, y ,z}().cross(y2) - Vector::new(0.0, -x2.z, x2.y), - Vector::new(x2.z, 0.0, -x2.x), - Vector::new(-x2.y, x2.x, 0.0), - ]; - - cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axes, pos12, pos21) -} - -pub fn cube_support_map_find_local_separating_normal_oneway>( - cube1: &Cuboid, - shape2: &S, - pos12: &Isometry, -) -> (f32, Vector) { - let mut best_separation = -std::f32::MAX; - let mut best_dir = Vector::zeros(); - - for i in 0..DIM { - for sign in &[-1.0, 1.0] { - let axis1 = Vector::ith(i, *sign); - let pt2 = shape2.support_point_toward(&pos12, &Unit::new_unchecked(-axis1)); - let separation = pt2[i] * *sign - cube1.half_extents[i]; - - if separation > best_separation { - best_separation = separation; - best_dir = axis1; - } - } - } - - (best_separation, best_dir) -} - -// NOTE: this only works with cuboid on the rhs because it has its symmetry origin at zero -// (therefore we can check only one normal direction). -pub fn point_cuboid_find_local_separating_normal_oneway( - point1: Point, - normal1: Option>>, - shape2: &Cuboid, - pos12: &Isometry, -) -> (f32, Vector) { - let mut best_separation = -std::f32::MAX; - let mut best_dir = Vector::zeros(); - - if let Some(normal1) = normal1 { - let axis1 = if (pos12.translation.vector - point1.coords).dot(&normal1) >= 0.0 { - normal1 - } else { - -normal1 - }; - - let pt2 = shape2.support_point_toward(&pos12, &-axis1); - let separation = (pt2 - point1).dot(&axis1); - - if separation > best_separation { - best_separation = separation; - best_dir = *axis1; - } - } - - (best_separation, best_dir) -} - -pub fn triangle_cuboid_find_local_separating_normal_oneway( - triangle1: &Triangle, - shape2: &Cuboid, - pos12: &Isometry, -) -> (f32, Vector) { - point_cuboid_find_local_separating_normal_oneway(triangle1.a, triangle1.normal(), shape2, pos12) -} - -#[cfg(feature = "dim2")] -pub fn segment_cuboid_find_local_separating_normal_oneway( - segment1: &Segment, - shape2: &Cuboid, - pos12: &Isometry, -) -> (f32, Vector) { - point_cuboid_find_local_separating_normal_oneway(segment1.a, segment1.normal(), shape2, pos12) -} - -/* - * Capsules - */ -#[cfg(feature = "dim3")] -pub fn triangle_segment_find_local_separating_normal_oneway( - triangle1: &Triangle, - segment2: &Segment, - m12: &Isometry, -) -> (f32, Vector) { - if let Some(dir) = triangle1.normal() { - let p2a = segment2.support_point_toward(m12, &-dir); - let p2b = segment2.support_point_toward(m12, &dir); - let sep_a = (p2a - triangle1.a).dot(&dir); - let sep_b = -(p2b - triangle1.a).dot(&dir); - - if sep_a >= sep_b { - (sep_a, *dir) - } else { - (sep_b, -*dir) - } - } else { - (-f32::MAX, Vector::zeros()) - } -} - -#[cfg(feature = "dim3")] -pub fn segment_triangle_find_local_separating_edge( - segment1: &Segment, - triangle2: &Triangle, - pos12: &Isometry, -) -> (f32, Vector) { - let x2 = pos12 * (triangle2.b - triangle2.a); - let y2 = pos12 * (triangle2.c - triangle2.b); - let z2 = pos12 * (triangle2.a - triangle2.c); - let dir1 = segment1.scaled_direction(); - - let crosses1 = [dir1.cross(&x2), dir1.cross(&y2), dir1.cross(&z2)]; - let axes1 = [ - crosses1[0], - crosses1[1], - crosses1[2], - -crosses1[0], - -crosses1[1], - -crosses1[2], - ]; - let mut max_separation = -f32::MAX; - let mut sep_dir = axes1[0]; - - for axis1 in &axes1 { - if let Some(axis1) = Unit::try_new(*axis1, 0.0) { - let sep = - support_map_support_map_compute_separation(segment1, triangle2, pos12, &axis1); - - if sep > max_separation { - max_separation = sep; - sep_dir = *axis1; - } - } - } - - (max_separation, sep_dir) -} diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs index 66840a0..80b24c5 100644 --- a/src/geometry/shape.rs +++ b/src/geometry/shape.rs @@ -1,17 +1,18 @@ use crate::dynamics::MassProperties; use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh}; use crate::math::Isometry; +use buckler::bounding_volume::AABB; +use buckler::query::{PointQuery, RayCast}; use downcast_rs::{impl_downcast, DowncastSync}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; -use ncollide::bounding_volume::{HasBoundingVolume, AABB}; -use ncollide::query::{PointQuery, RayCast}; use num::Zero; use num_derive::FromPrimitive; #[cfg(feature = "dim3")] use { - crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, RoundCylinder}, - ncollide::bounding_volume::BoundingVolume, + crate::geometry::{Cone, Cylinder, RoundCylinder}, + buckler::bounding_volume::BoundingVolume, + buckler::shape::PolygonalFeatureMap, }; #[derive(Copy, Clone, Debug, FromPrimitive)] @@ -57,7 +58,7 @@ pub enum ShapeType { } /// Trait implemented by shapes usable by Rapier. -pub trait Shape: RayCast + PointQuery + DowncastSync { +pub trait Shape: RayCast + PointQuery + DowncastSync { /// Convert this shape as a serializable entity. #[cfg(feature = "serde-serialize")] fn as_serialize(&self) -> Option<&dyn Serialize> { @@ -67,7 +68,7 @@ pub trait Shape: RayCast + PointQuery + DowncastSync { // TODO: add a compute_local_aabb method? /// Computes the AABB of this shape. - fn compute_aabb(&self, position: &Isometry) -> AABB; + fn compute_aabb(&self, position: &Isometry) -> AABB; /// Compute the mass-properties of this shape given its uniform density. fn mass_properties(&self, density: f32) -> MassProperties; @@ -144,8 +145,8 @@ impl Shape for Ball { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.bounding_volume(position) + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) } fn mass_properties(&self, density: f32) -> MassProperties { @@ -163,7 +164,7 @@ impl Shape for Ball { // Some(self as &dyn Serialize) // } // -// fn compute_aabb(&self, position: &Isometry) -> AABB { +// fn compute_aabb(&self, position: &Isometry) -> AABB { // self.aabb(position) // } // @@ -182,8 +183,8 @@ impl Shape for Cuboid { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.bounding_volume(position) + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) } fn mass_properties(&self, density: f32) -> MassProperties { @@ -206,7 +207,7 @@ impl Shape for Capsule { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { + fn compute_aabb(&self, position: &Isometry) -> AABB { self.aabb(position) } @@ -230,8 +231,8 @@ impl Shape for Triangle { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.bounding_volume(position) + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) } fn mass_properties(&self, _density: f32) -> MassProperties { @@ -254,8 +255,8 @@ impl Shape for Segment { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.bounding_volume(position) + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) } fn mass_properties(&self, _density: f32) -> MassProperties { @@ -278,7 +279,7 @@ impl Shape for Trimesh { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { + fn compute_aabb(&self, position: &Isometry) -> AABB { self.aabb(position) } @@ -297,8 +298,8 @@ impl Shape for HeightField { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.bounding_volume(position) + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) } fn mass_properties(&self, _density: f32) -> MassProperties { @@ -317,8 +318,8 @@ impl Shape for Cylinder { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.bounding_volume(position) + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) } fn mass_properties(&self, density: f32) -> MassProperties { @@ -342,8 +343,8 @@ impl Shape for Cone { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.bounding_volume(position) + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) } fn mass_properties(&self, density: f32) -> MassProperties { @@ -367,7 +368,7 @@ impl Shape for RoundCylinder { Some(self as &dyn Serialize) } - fn compute_aabb(&self, position: &Isometry) -> AABB { + fn compute_aabb(&self, position: &Isometry) -> AABB { self.cylinder .compute_aabb(position) .loosened(self.border_radius) diff --git a/src/geometry/triangle.rs b/src/geometry/triangle.rs index 02375b7..1f2bc92 100644 --- a/src/geometry/triangle.rs +++ b/src/geometry/triangle.rs @@ -1,7 +1,5 @@ #[cfg(feature = "dim2")] -use crate::geometry::{CuboidFeatureFace, Triangle}; -#[cfg(feature = "dim2")] -use crate::math::Vector; +use crate::{buckler::shape::CuboidFeatureFace, geometry::Triangle, math::Vector}; #[cfg(feature = "dim2")] pub fn support_face(_triangle: &Triangle, _local_dir: Vector) -> CuboidFeatureFace { diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index b4e8518..02a3b1c 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -1,16 +1,18 @@ -use crate::geometry::{PointProjection, Ray, RayIntersection, Triangle, WQuadtree}; +use crate::geometry::{ + Cuboid, HeightField, PointProjection, Ray, RayIntersection, Triangle, WQuadtree, +}; use crate::math::{Isometry, Point}; +use buckler::bounding_volume::AABB; +use buckler::query::{PointQuery, RayCast}; +use buckler::shape::FeatureId; use na::Point3; -use ncollide::bounding_volume::{HasBoundingVolume, AABB}; -use ncollide::query::{PointQuery, RayCast}; -use ncollide::shape::FeatureId; #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A triangle mesh. pub struct Trimesh { wquadtree: WQuadtree, - aabb: AABB, + aabb: AABB, vertices: Vec>, indices: Vec>, } @@ -34,7 +36,7 @@ impl Trimesh { vertices[idx[1] as usize], vertices[idx[2] as usize], ) - .local_bounding_volume(); + .local_aabb(); (i, aabb) }); @@ -52,7 +54,7 @@ impl Trimesh { } /// Compute the axis-aligned bounding box of this triangle mesh. - pub fn aabb(&self, pos: &Isometry) -> AABB { + pub fn aabb(&self, pos: &Isometry) -> AABB { self.aabb.transform_by(pos) } @@ -106,15 +108,14 @@ impl Trimesh { } } -impl PointQuery for Trimesh { - fn project_point(&self, _m: &Isometry, _pt: &Point, _solid: bool) -> PointProjection { +impl PointQuery for Trimesh { + fn project_local_point(&self, _pt: &Point, _solid: bool) -> PointProjection { // TODO unimplemented!() } - fn project_point_with_feature( + fn project_local_point_and_get_feature( &self, - _m: &Isometry, _pt: &Point, ) -> (PointProjection, FeatureId) { // TODO @@ -123,10 +124,9 @@ impl PointQuery for Trimesh { } #[cfg(feature = "dim2")] -impl RayCast for Trimesh { - fn toi_and_normal_with_ray( +impl RayCast for Trimesh { + fn cast_local_ray_and_get_normal( &self, - _m: &Isometry, _ray: &Ray, _max_toi: f32, _solid: bool, @@ -142,24 +142,21 @@ impl RayCast for Trimesh { } #[cfg(feature = "dim3")] -impl RayCast for Trimesh { - fn toi_and_normal_with_ray( +impl RayCast for Trimesh { + fn cast_local_ray_and_get_normal( &self, - m: &Isometry, ray: &Ray, max_toi: f32, solid: bool, ) -> Option { // FIXME: do a best-first search. let mut intersections = Vec::new(); - let ls_ray = ray.inverse_transform_by(m); - self.wquadtree - .cast_ray(&ls_ray, max_toi, &mut intersections); + self.wquadtree.cast_ray(&ray, max_toi, &mut intersections); let mut best: Option = None; for inter in intersections { let tri = self.triangle(inter); - if let Some(inter) = tri.toi_and_normal_with_ray(m, ray, max_toi, solid) { + if let Some(inter) = tri.cast_local_ray_and_get_normal(ray, max_toi, solid) { if let Some(curr) = &mut best { if curr.toi > inter.toi { *curr = inter; @@ -173,16 +170,14 @@ impl RayCast for Trimesh { best } - fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_toi: f32) -> bool { + fn intersects_local_ray(&self, ray: &Ray, max_toi: f32) -> bool { // FIXME: do a best-first search. let mut intersections = Vec::new(); - let ls_ray = ray.inverse_transform_by(m); - self.wquadtree - .cast_ray(&ls_ray, max_toi, &mut intersections); + self.wquadtree.cast_ray(&ray, max_toi, &mut intersections); for inter in intersections { let tri = self.triangle(inter); - if tri.intersects_ray(m, ray, max_toi) { + if tri.intersects_local_ray(ray, max_toi) { return true; } } @@ -190,3 +185,19 @@ impl RayCast for Trimesh { false } } + +#[cfg(feature = "dim3")] +impl From for Trimesh { + fn from(heightfield: HeightField) -> Self { + let (vtx, idx) = heightfield.to_trimesh(); + Trimesh::new(vtx, idx) + } +} + +#[cfg(feature = "dim3")] +impl From for Trimesh { + fn from(cuboid: Cuboid) -> Self { + let (vtx, idx) = cuboid.to_trimesh(); + Trimesh::new(vtx, idx) + } +} diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs index 1ebf528..08f1df1 100644 --- a/src/geometry/waabb.rs +++ b/src/geometry/waabb.rs @@ -1,7 +1,7 @@ use crate::geometry::Ray; use crate::math::{Point, Vector, DIM, SIMD_WIDTH}; use crate::utils; -use ncollide::bounding_volume::AABB; +use buckler::bounding_volume::AABB; use num::{One, Zero}; use { crate::math::{SimdBool, SimdFloat}, @@ -97,7 +97,7 @@ impl WAABB { Self::splat(AABB::new_invalid()) } - pub fn splat(aabb: AABB) -> Self { + pub fn splat(aabb: AABB) -> Self { Self { mins: Point::splat(aabb.mins), maxs: Point::splat(aabb.maxs), @@ -119,7 +119,7 @@ impl WAABB { self.maxs += dilation; } - pub fn replace(&mut self, i: usize, aabb: AABB) { + pub fn replace(&mut self, i: usize, aabb: AABB) { self.mins.replace(i, aabb.mins); self.maxs.replace(i, aabb.maxs); } @@ -196,7 +196,7 @@ impl WAABB { & other.mins.z.simd_le(self.maxs.z) } - pub fn to_merged_aabb(&self) -> AABB { + pub fn to_merged_aabb(&self) -> AABB { AABB::new( self.mins.coords.map(|e| e.simd_horizontal_min()).into(), self.maxs.coords.map(|e| e.simd_horizontal_max()).into(), @@ -204,8 +204,8 @@ impl WAABB { } } -impl From<[AABB; SIMD_WIDTH]> for WAABB { - fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { +impl From<[AABB; SIMD_WIDTH]> for WAABB { + fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH]; diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index deab4a2..6f57ed3 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -4,7 +4,7 @@ use crate::math::Point; #[cfg(feature = "dim3")] use crate::math::Vector; use crate::simd::{SimdFloat, SIMD_WIDTH}; -use ncollide::bounding_volume::BoundingVolume; +use buckler::bounding_volume::BoundingVolume; use simba::simd::{SimdBool, SimdValue}; use std::collections::VecDeque; use std::ops::Range; diff --git a/src/lib.rs b/src/lib.rs index deb9313..ed9eda5 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -8,14 +8,15 @@ //! - The ability to run a perfectly deterministic simulation on different machine, as long as they //! are compliant with the IEEE 754-2008 floating point standard. -#![deny(missing_docs)] +// FIXME: deny that +#![allow(missing_docs)] +#[cfg(feature = "dim2")] +pub extern crate buckler2d as buckler; +#[cfg(feature = "dim3")] +pub extern crate buckler3d as buckler; pub extern crate crossbeam; pub extern crate nalgebra as na; -#[cfg(feature = "dim2")] -pub extern crate ncollide2d as ncollide; -#[cfg(feature = "dim3")] -pub extern crate ncollide3d as ncollide; #[cfg(feature = "serde")] #[macro_use] extern crate serde; diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 665fee8..dee4312 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -74,7 +74,7 @@ impl QueryPipeline { for handle in inter { if let Some(collider) = colliders.get(handle) { if collider.collision_groups.test(groups) { - if let Some(inter) = collider.shape().toi_and_normal_with_ray( + if let Some(inter) = collider.shape().cast_ray_and_get_normal( collider.position(), ray, max_toi, @@ -118,7 +118,7 @@ impl QueryPipeline { let collider = &colliders[handle]; if collider.collision_groups.test(groups) { - if let Some(inter) = collider.shape().toi_and_normal_with_ray( + if let Some(inter) = collider.shape().cast_ray_and_get_normal( collider.position(), ray, max_toi, diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 040c883..a864cfd 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -501,7 +501,7 @@ impl GraphicsManager { object: DefaultColliderHandle, colliders: &DefaultColliderSet, delta: Isometry, - shape: &Cuboid, + shape: &Cuboid, color: Point3, out: &mut Vec, ) { diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index 5358e7c..0d1483e 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -1,5 +1,9 @@ #[macro_use] extern crate kiss3d; +#[cfg(feature = "dim2")] +extern crate buckler2d as buckler; +#[cfg(feature = "dim3")] +extern crate buckler3d as buckler; extern crate nalgebra as na; #[cfg(feature = "dim2")] extern crate ncollide2d as ncollide; diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index e112c22..4384480 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -1,4 +1,4 @@ -use ncollide::shape::{Ball, Capsule, Cuboid, ShapeHandle}; +use ncollide::shape::{Ball, Capsule, Cuboid, HeightField, ShapeHandle}; use nphysics::force_generator::DefaultForceGeneratorSet; use nphysics::joint::{ DefaultJointConstraintSet, FixedConstraint, PrismaticConstraint, RevoluteConstraint, @@ -187,7 +187,10 @@ fn nphysics_collider_from_rapier_collider( pos *= capsule.transform_wrt_y(); ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius)) } else if let Some(heightfield) = shape.as_heightfield() { - ShapeHandle::new(heightfield.clone()) + let heights = heightfield.heights(); + let scale = heightfield.scale(); + let heightfield = HeightField::new(heights.clone(), *scale); + ShapeHandle::new(heightfield) } else { #[cfg(feature = "dim3")] if let Some(trimesh) = shape.as_trimesh() { diff --git a/src_testbed/objects/heightfield.rs b/src_testbed/objects/heightfield.rs index 0815592..977bcca 100644 --- a/src_testbed/objects/heightfield.rs +++ b/src_testbed/objects/heightfield.rs @@ -1,15 +1,16 @@ #[cfg(feature = "dim3")] use crate::objects::node::{self, GraphicsNode}; +use buckler::shape; +use kiss3d::resource::Mesh; use kiss3d::window::Window; use na::{self, Point3}; -use ncollide::shape; -#[cfg(feature = "dim3")] -use ncollide::transformation::ToTriMesh; use rapier::geometry::{ColliderHandle, ColliderSet}; #[cfg(feature = "dim2")] use rapier::math::Point; #[cfg(feature = "dim3")] use rapier::math::Vector; +use std::cell::RefCell; +use std::rc::Rc; pub struct HeightField { color: Point3, @@ -25,7 +26,7 @@ impl HeightField { #[cfg(feature = "dim2")] pub fn new( collider: ColliderHandle, - heightfield: &shape::HeightField, + heightfield: &shape::HeightField, color: Point3, _: &mut Window, ) -> HeightField { @@ -47,16 +48,18 @@ impl HeightField { #[cfg(feature = "dim3")] pub fn new( collider: ColliderHandle, - heightfield: &shape::HeightField, + heightfield: &shape::HeightField, color: Point3, window: &mut Window, ) -> HeightField { - let mesh = heightfield.to_trimesh(()); + let (vertices, indices) = heightfield.to_trimesh(); + let indices = indices.into_iter().map(|i| na::convert(i)).collect(); + let mesh = Mesh::new(vertices, indices, None, None, false); let mut res = HeightField { color, base_color: color, - gfx: window.add_trimesh(mesh, Vector::repeat(1.0)), + gfx: window.add_mesh(Rc::new(RefCell::new(mesh)), Vector::repeat(1.0)), collider: collider, }; diff --git a/src_testbed/objects/polyline.rs b/src_testbed/objects/polyline.rs index 77841ae..efb5e95 100644 --- a/src_testbed/objects/polyline.rs +++ b/src_testbed/objects/polyline.rs @@ -1,6 +1,6 @@ +use buckler2d::shape; use kiss3d::window::Window; use na::{Isometry2, Point2, Point3}; -use ncollide2d::shape; use nphysics2d::object::{ColliderAnchor, DefaultColliderHandle, DefaultColliderSet}; pub struct Polyline { diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 4fac8e1..60fb5d9 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -1521,8 +1521,8 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) } else { Point3::new(1.0, 0.0, 0.0) }; - let pos1 = colliders[manifold.pair.collider1].position(); - let pos2 = colliders[manifold.pair.collider2].position(); + let pos1 = colliders[manifold.data.pair.collider1].position(); + let pos2 = colliders[manifold.data.pair.collider2].position(); let start = pos1 * pt.local_p1; let end = pos2 * pt.local_p2; let n = pos1 * manifold.local_n1;