diff --git a/.github/workflows/rapier-ci-bench.yml b/.github/workflows/rapier-ci-bench.yml index 9cce1b2..6c4f007 100644 --- a/.github/workflows/rapier-ci-bench.yml +++ b/.github/workflows/rapier-ci-bench.yml @@ -18,15 +18,17 @@ jobs: BENCHBOT_TARGET_COMMIT: ${{ github.event.pull_request.head.sha }} BENCHBOT_SHA: ${{ github.sha }} BENCHBOT_HEAD_REF: ${{github.head_ref}} + BENCHBOT_OTHER_BACKENDS: false runs-on: ubuntu-latest steps: - name: Find commit SHA if: github.ref == 'refs/heads/master' run: | echo "::set-env name=BENCHBOT_TARGET_COMMIT::$BENCHBOT_SHA" + echo "::set-env name=BENCHBOT_OTHER_BACKENDS::true" - name: Send 3D bench message shell: bash run: curl -u $BENCHBOT_AMQP_USER:$BENCHBOT_AMQP_PASS -i -H "content-type:application/json" -X POST https://$BENCHBOT_AMQP_HOST/api/exchanges/$BENCHBOT_AMQP_VHOST//publish - -d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\" }","payload_encoding":"string"}' \ No newline at end of file + -d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\", \"other_backends\":'$BENCHBOT_OTHER_BACKENDS' }","payload_encoding":"string"}' \ No newline at end of file diff --git a/CHANGELOG b/CHANGELOG index 70b20e5..aba1ac2 100644 --- a/CHANGELOG +++ b/CHANGELOG @@ -1,4 +1,21 @@ -## v0.2.0 - WIP +## v0.3.0 +- Collider shapes are now trait-objects instead of a `Shape` enum. +- Add a user-defined `u128` to each colliders and rigid-bodies for storing user data. +- Add the support for `Cylinder`, `RoundCylinder`, and `Cone` shapes. +- Added the support for collision filtering based on bit masks (often known as collision groups, collision masks, or + collision layers in other physics engines). Each collider has two groups. Their `collision_groups` is used for filtering + what pair of colliders should have their contacts computed by the narrow-phase. Their `solver_groups` is used for filtering + what pair of colliders should have their contact forces computed by the constraints solver. +- Collision groups can also be used to filter what collider should be hit by a ray-cast performed by the `QueryPipeline`. +- Added collision filters based on user-defined trait-objects. This adds two traits `ContactPairFilter` and + `ProximityPairFilter` that allows user-defined logic for determining if two colliders/sensors are allowed to interact. +- The `PhysicsPipeline::step` method now takes two additional arguments: the optional `&ContactPairFilter` and `&ProximityPairFilter` +for filtering contact and proximity pairs. + +## v0.2.1 +- Fix panic in TriMesh construction and QueryPipeline update caused by a stack overflow or a subtraction underflow. + +## v0.2.0 The most significant change on this version is the addition of the `QueryPipeline` responsible for performing scene-wide queries. So far only ray-casting has been implemented. diff --git a/Cargo.toml b/Cargo.toml index 944fa40..cfb166a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -5,6 +5,11 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark [patch.crates-io] #wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" } #simba = { path = "../simba" } +#ncollide2d = { path = "../ncollide/build/ncollide2d" } +#ncollide3d = { path = "../ncollide/build/ncollide3d" } +#nphysics2d = { path = "../nphysics/build/nphysics2d" } +#nphysics3d = { path = "../nphysics/build/nphysics3d" } +#kiss3d = { path = "../kiss3d" } [profile.release] #debug = true diff --git a/benchmarks2d/Cargo.toml b/benchmarks2d/Cargo.toml index b1ee8db..90a6326 100644 --- a/benchmarks2d/Cargo.toml +++ b/benchmarks2d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.22" +nalgebra = "0.23" [dependencies.rapier_testbed2d] path = "../build/rapier_testbed2d" diff --git a/benchmarks3d/Cargo.toml b/benchmarks3d/Cargo.toml index 81c843a..03194df 100644 --- a/benchmarks3d/Cargo.toml +++ b/benchmarks3d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.22" +nalgebra = "0.23" [dependencies.rapier_testbed3d] path = "../build/rapier_testbed3d" diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index ff5253b..d963079 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -1,7 +1,6 @@ -# Name idea: bident for 2D and trident for 3D [package] name = "rapier2d" -version = "0.2.0" +version = "0.3.0" authors = [ "Sébastien Crozet " ] description = "2-dimensional physics engine in Rust." documentation = "http://docs.rs/rapier2d" @@ -22,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 = [ "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -35,18 +34,22 @@ required-features = [ "dim2" ] vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" -nalgebra = "0.22" -ncollide2d = "0.24" -simba = "^0.2.1" -approx = "0.3" +nalgebra = "0.23" +ncollide2d = "0.26" +simba = "0.3" +approx = "0.4" rayon = { version = "1", optional = true } -crossbeam = "0.7" +crossbeam = "0.8" generational-arena = "0.2" arrayvec = "0.5" bit-vec = "0.6" rustc-hash = "1" serde = { version = "1", features = [ "derive" ], optional = true } +erased-serde = { version = "0.3", optional = true } indexmap = { version = "1", features = [ "serde-1" ], optional = true } +downcast-rs = "1.2" +num-derive = "0.3" +bitflags = "1" [dev-dependencies] bincode = "1" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index 130e5dd..f5c2fe4 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -1,7 +1,6 @@ -# Name idea: bident for 2D and trident for 3D [package] name = "rapier3d" -version = "0.2.0" +version = "0.3.0" authors = [ "Sébastien Crozet " ] description = "3-dimensional physics engine in Rust." documentation = "http://docs.rs/rapier3d" @@ -22,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 = [ "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -35,18 +34,22 @@ required-features = [ "dim3" ] vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" -nalgebra = "0.22" -ncollide3d = "0.24" -simba = "^0.2.1" -approx = "0.3" +nalgebra = "0.23" +ncollide3d = "0.26" +simba = "0.3" +approx = "0.4" rayon = { version = "1", optional = true } -crossbeam = "0.7" +crossbeam = "0.8" generational-arena = "0.2" arrayvec = "0.5" bit-vec = "0.6" rustc-hash = "1" serde = { version = "1", features = [ "derive" ], optional = true } +erased-serde = { version = "0.3", optional = true } indexmap = { version = "1", features = [ "serde-1" ], optional = true } +downcast-rs = "1.2" +num-derive = "0.3" +bitflags = "1" [dev-dependencies] bincode = "1" diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index eeecb2a..08c0893 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed2d" -version = "0.2.0" +version = "0.3.0" authors = [ "Sébastien Crozet " ] description = "Testbed for the 2-dimensional physics engine in Rust." homepage = "http://rapier.org" @@ -23,17 +23,17 @@ other-backends = [ "wrapped2d", "nphysics2d" ] [dependencies] -nalgebra = "0.22" -kiss3d = { version = "0.25", features = [ "conrod" ] } +nalgebra = "0.23" +kiss3d = { version = "0.27", features = [ "conrod" ] } rand = "0.7" rand_pcg = "0.2" instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" num_cpus = { version = "1", optional = true } wrapped2d = { version = "0.4", optional = true } -ncollide2d = "0.24" -nphysics2d = { version = "0.17", optional = true } -crossbeam = "0.7" +ncollide2d = "0.26" +nphysics2d = { version = "0.18", optional = true } +crossbeam = "0.8" bincode = "1" flexbuffers = "0.1" Inflector = "0.11" @@ -42,5 +42,5 @@ md5 = "0.7" [dependencies.rapier2d] path = "../rapier2d" -version = "0.2" +version = "0.3" features = [ "serde-serialize" ] diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index 7675701..7fa01d7 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "rapier_testbed3d" -version = "0.2.0" +version = "0.3.0" authors = [ "Sébastien Crozet " ] description = "Testbed for the 3-dimensional physics engine in Rust." homepage = "http://rapier.org" @@ -22,19 +22,19 @@ parallel = [ "rapier3d/parallel", "num_cpus" ] other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ] [dependencies] -nalgebra = "0.22" -kiss3d = { version = "0.25", features = [ "conrod" ] } +nalgebra = "0.23" +kiss3d = { version = "0.27", features = [ "conrod" ] } rand = "0.7" rand_pcg = "0.2" instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" -glam = { version = "0.8", optional = true } +glam = { version = "0.9", optional = true } num_cpus = { version = "1", optional = true } -ncollide3d = "0.24" -nphysics3d = { version = "0.17", optional = true } -physx = { version = "0.6", optional = true } +ncollide3d = "0.26" +nphysics3d = { version = "0.18", optional = true } +physx = { version = "0.7", optional = true } physx-sys = { version = "0.4", optional = true } -crossbeam = "0.7" +crossbeam = "0.8" bincode = "1" flexbuffers = "0.1" serde_cbor = "0.11" @@ -44,5 +44,5 @@ serde = { version = "1", features = [ "derive" ] } [dependencies.rapier3d] path = "../rapier3d" -version = "0.2" +version = "0.3" features = [ "serde-serialize" ] diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml index ad63958..f1ed728 100644 --- a/examples2d/Cargo.toml +++ b/examples2d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.22" +nalgebra = "0.23" [dependencies.rapier_testbed2d] path = "../build/rapier_testbed2d" diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 7ee924e..a7ddc75 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -11,6 +11,7 @@ use rapier_testbed2d::Testbed; use std::cmp::Ordering; mod add_remove2; +mod collision_groups2; mod debug_box_ball2; mod debug_infinite_fall; mod heightfield2; @@ -53,6 +54,7 @@ pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ ("Add remove", add_remove2::init_world), + ("Collision groups", collision_groups2::init_world), ("Heightfield", heightfield2::init_world), ("Joints", joints2::init_world), ("Platform", platform2::init_world), diff --git a/examples2d/collision_groups2.rs b/examples2d/collision_groups2.rs new file mode 100644 index 0000000..9fd9f0b --- /dev/null +++ b/examples2d/collision_groups2.rs @@ -0,0 +1,98 @@ +use na::{Point2, Point3}; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet, InteractionGroups}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height) + .build(); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height).build(); + colliders.insert(collider, floor_handle, &mut bodies); + + /* + * Setup groups + */ + const GREEN_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01); + const BLUE_GROUP: InteractionGroups = InteractionGroups::new(0b10, 0b10); + + /* + * A green floor that will collide with the GREEN group only. + */ + let green_floor = ColliderBuilder::cuboid(1.0, 0.1) + .translation(0.0, 1.0) + .collision_groups(GREEN_GROUP) + .build(); + let green_collider_handle = colliders.insert(green_floor, floor_handle, &mut bodies); + + testbed.set_collider_initial_color(green_collider_handle, Point3::new(0.0, 1.0, 0.0)); + + /* + * A blue floor that will collide with the BLUE group only. + */ + let blue_floor = ColliderBuilder::cuboid(1.0, 0.1) + .translation(0.0, 2.0) + .collision_groups(BLUE_GROUP) + .build(); + let blue_collider_handle = colliders.insert(blue_floor, floor_handle, &mut bodies); + + testbed.set_collider_initial_color(blue_collider_handle, Point3::new(0.0, 0.0, 1.0)); + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.1; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = 2.5; + + for j in 0usize..4 { + for i in 0..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + + // Alternate between the green and blue groups. + let (group, color) = if i % 2 == 0 { + (GREEN_GROUP, Point3::new(0.0, 1.0, 0.0)) + } else { + (BLUE_GROUP, Point3::new(0.0, 0.0, 1.0)) + }; + + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad) + .collision_groups(group) + .build(); + colliders.insert(collider, handle, &mut bodies); + + testbed.set_body_color(handle, color); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 1.0), 100.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index efc3cce..5362554 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ] [dependencies] rand = "0.7" Inflector = "0.11" -nalgebra = "0.22" +nalgebra = "0.23" [dependencies.rapier_testbed3d] path = "../build/rapier_testbed3d" diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 64eb5b6..eeb9736 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -11,8 +11,10 @@ use rapier_testbed3d::Testbed; use std::cmp::Ordering; mod add_remove3; +mod collision_groups3; mod compound3; mod debug_boxes3; +mod debug_cylinder3; mod debug_triangle3; mod debug_trimesh3; mod domino3; @@ -64,6 +66,7 @@ pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ ("Add remove", add_remove3::init_world), ("Primitives", primitives3::init_world), + ("Collision groups", collision_groups3::init_world), ("Compound", compound3::init_world), ("Domino", domino3::init_world), ("Heightfield", heightfield3::init_world), @@ -76,6 +79,7 @@ pub fn main() { ("(Debug) boxes", debug_boxes3::init_world), ("(Debug) triangle", debug_triangle3::init_world), ("(Debug) trimesh", debug_trimesh3::init_world), + ("(Debug) cylinder", debug_cylinder3::init_world), ]; // Lexicographic sort, with stress tests moved at the end of the list. diff --git a/examples3d/collision_groups3.rs b/examples3d/collision_groups3.rs new file mode 100644 index 0000000..625bc50 --- /dev/null +++ b/examples3d/collision_groups3.rs @@ -0,0 +1,102 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet, InteractionGroups}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let floor_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, floor_handle, &mut bodies); + + /* + * Setup groups + */ + const GREEN_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01); + const BLUE_GROUP: InteractionGroups = InteractionGroups::new(0b10, 0b10); + + /* + * A green floor that will collide with the GREEN group only. + */ + let green_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0) + .translation(0.0, 1.0, 0.0) + .collision_groups(GREEN_GROUP) + .build(); + let green_collider_handle = colliders.insert(green_floor, floor_handle, &mut bodies); + + testbed.set_collider_initial_color(green_collider_handle, Point3::new(0.0, 1.0, 0.0)); + + /* + * A blue floor that will collide with the BLUE group only. + */ + let blue_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0) + .translation(0.0, 2.0, 0.0) + .collision_groups(BLUE_GROUP) + .build(); + let blue_collider_handle = colliders.insert(blue_floor, floor_handle, &mut bodies); + + testbed.set_collider_initial_color(blue_collider_handle, Point3::new(0.0, 0.0, 1.0)); + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.1; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = 2.5; + let centerz = shift * (num / 2) as f32; + + for j in 0usize..4 { + for i in 0..num { + for k in 0usize..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + let z = k as f32 * shift - centerz; + + // Alternate between the green and blue groups. + let (group, color) = if k % 2 == 0 { + (GREEN_GROUP, Point3::new(0.0, 1.0, 0.0)) + } else { + (BLUE_GROUP, Point3::new(0.0, 0.0, 1.0)) + }; + + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad) + .collision_groups(group) + .build(); + colliders.insert(collider, handle, &mut bodies); + + testbed.set_body_color(handle, color); + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/debug_boxes3.rs b/examples3d/debug_boxes3.rs index 7237fd9..919cdd6 100644 --- a/examples3d/debug_boxes3.rs +++ b/examples3d/debug_boxes3.rs @@ -17,22 +17,26 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 100.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(0.0, -ground_height, 0.0) - .build(); - let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); - colliders.insert(collider, handle, &mut bodies); + for _ in 0..6 { + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + } // Build the dynamic box rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(1.1, 0.0, 0.0) - .rotation(Vector3::new(0.8, 0.2, 0.1)) - .can_sleep(false) - .build(); - let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build(); - colliders.insert(collider, handle, &mut bodies); + for _ in 0..6 { + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(1.1, 0.0, 0.0) + .rotation(Vector3::new(0.8, 0.2, 0.1)) + .can_sleep(false) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } /* * Set up the testbed. diff --git a/examples3d/debug_cylinder3.rs b/examples3d/debug_cylinder3.rs new file mode 100644 index 0000000..0717c67 --- /dev/null +++ b/examples3d/debug_cylinder3.rs @@ -0,0 +1,65 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +// This shows a bug when a cylinder is in contact with a very large +// but very thin cuboid. In this case the EPA returns an incorrect +// contact normal, resulting in the cylinder falling through the floor. +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 100.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 1; + let rad = 1.0; + + let shiftx = rad * 2.0 + rad; + let shifty = rad * 2.0 + rad; + let shiftz = rad * 2.0 + rad; + let centerx = shiftx * (num / 2) as f32; + let centery = shifty / 2.0; + let centerz = shiftz * (num / 2) as f32; + + let offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; + + let x = -centerx + offset; + let y = centery + 3.0; + let z = -centerz + offset; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cylinder(rad, rad).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index 09df815..6af93c7 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -54,13 +54,17 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); - colliders.insert(collider, handle, &mut bodies); - } else { - let collider = ColliderBuilder::ball(rad).build(); - colliders.insert(collider, handle, &mut bodies); - } + let collider = match j % 5 { + 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), + 1 => ColliderBuilder::ball(rad).build(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), + 3 => ColliderBuilder::cone(rad, rad).build(), + _ => ColliderBuilder::capsule_y(rad, rad).build(), + }; + + colliders.insert(collider, handle, &mut bodies); } } } diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index cf678b9..db15341 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -15,7 +15,7 @@ pub fn init_world(testbed: &mut Testbed) { * Ground */ let ground_size = 100.1; - let ground_height = 0.1; + let ground_height = 2.1; let rigid_body = RigidBodyBuilder::new_static() .translation(0.0, -ground_height, 0.0) @@ -30,29 +30,34 @@ pub fn init_world(testbed: &mut Testbed) { let num = 8; let rad = 1.0; - let shift = rad * 2.0 + rad; - let centerx = shift * (num / 2) as f32; - let centery = shift / 2.0; - let centerz = shift * (num / 2) as f32; + let shiftx = rad * 2.0 + rad; + let shifty = rad * 2.0 + rad; + let shiftz = rad * 2.0 + rad; + let centerx = shiftx * (num / 2) as f32; + let centery = shifty / 2.0; + let centerz = shiftz * (num / 2) as f32; let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; for j in 0usize..20 { for i in 0..num { for k in 0usize..num { - let x = i as f32 * shift - centerx + offset; - let y = j as f32 * shift + centery + 3.0; - let z = k as f32 * shift - centerz + offset; + let x = i as f32 * shiftx - centerx + offset; + let y = j as f32 * shifty + centery + 3.0; + let z = k as f32 * shiftz - centerz + offset; // Build the rigid body. let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - let collider = match j % 2 { + let collider = match j % 5 { 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 1 => ColliderBuilder::ball(rad).build(), - // 2 => ColliderBuilder::capsule_y(rad, rad).build(), - _ => unreachable!(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), + 3 => ColliderBuilder::cone(rad, rad).build(), + _ => ColliderBuilder::capsule_y(rad, rad).build(), }; colliders.insert(collider, handle, &mut bodies); diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index d022a1f..2a96bda 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -64,13 +64,17 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); let handle = bodies.insert(rigid_body); - if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); - colliders.insert(collider, handle, &mut bodies); - } else { - let collider = ColliderBuilder::ball(rad).build(); - colliders.insert(collider, handle, &mut bodies); - } + let collider = match j % 5 { + 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), + 1 => ColliderBuilder::ball(rad).build(), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), + 3 => ColliderBuilder::cone(rad, rad).build(), + _ => ColliderBuilder::capsule_y(rad, rad).build(), + }; + + colliders.insert(collider, handle, &mut bodies); } } } diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs index 22e7da5..d64839c 100644 --- a/src/dynamics/mass_properties.rs +++ b/src/dynamics/mass_properties.rs @@ -91,7 +91,7 @@ impl MassProperties { } #[cfg(feature = "dim3")] - /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii. + /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes. pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3 { let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e); self.principal_inertia_local_frame.to_rotation_matrix() @@ -103,7 +103,7 @@ impl MassProperties { } #[cfg(feature = "dim3")] - /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. + /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes. pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); self.principal_inertia_local_frame.to_rotation_matrix() diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index 5f08958..3b1b214 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -1,30 +1,9 @@ use crate::dynamics::MassProperties; #[cfg(feature = "dim3")] use crate::geometry::Capsule; -use crate::math::{Point, PrincipalAngularInertia, Vector}; +use crate::math::Point; impl MassProperties { - fn cylinder_y_volume_unit_inertia( - half_height: f32, - radius: f32, - ) -> (f32, PrincipalAngularInertia) { - #[cfg(feature = "dim2")] - { - Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) - } - - #[cfg(feature = "dim3")] - { - let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; - let sq_radius = radius * radius; - let sq_height = half_height * half_height * 4.0; - let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; - - let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); - (volume, inertia) - } - } - pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { let half_height = (b - a).norm() / 2.0; let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs new file mode 100644 index 0000000..12f831f --- /dev/null +++ b/src/dynamics/mass_properties_cone.rs @@ -0,0 +1,29 @@ +use crate::dynamics::MassProperties; +use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; + +impl MassProperties { + pub(crate) fn cone_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia) { + let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0; + let principal = sq_radius * 3.0 / 10.0; + + (volume, Vector::new(off_principal, principal, off_principal)) + } + + pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self { + let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius); + let cyl_mass = cyl_vol * density; + + Self::with_principal_inertia_frame( + Point::new(0.0, -half_height / 2.0, 0.0), + cyl_mass, + cyl_unit_i * cyl_mass, + Rotation::identity(), + ) + } +} diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs new file mode 100644 index 0000000..7c8054a --- /dev/null +++ b/src/dynamics/mass_properties_cylinder.rs @@ -0,0 +1,40 @@ +use crate::dynamics::MassProperties; +#[cfg(feature = "dim3")] +use crate::math::{Point, Rotation}; +use crate::math::{PrincipalAngularInertia, Vector}; + +impl MassProperties { + pub(crate) fn cylinder_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia) { + #[cfg(feature = "dim2")] + { + Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) + } + + #[cfg(feature = "dim3")] + { + let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; + + let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); + (volume, inertia) + } + } + + #[cfg(feature = "dim3")] + pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self { + let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); + let cyl_mass = cyl_vol * density; + + Self::with_principal_inertia_frame( + Point::origin(), + cyl_mass, + cyl_unit_i * cyl_mass, + Rotation::identity(), + ) + } +} diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs index c87e888..8b0b811 100644 --- a/src/dynamics/mass_properties_polygon.rs +++ b/src/dynamics/mass_properties_polygon.rs @@ -1,3 +1,5 @@ +#![allow(dead_code)] // TODO: remove this + use crate::dynamics::MassProperties; use crate::math::Point; diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 4499d95..10cdd29 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -22,7 +22,10 @@ mod joint; mod mass_properties; mod mass_properties_ball; mod mass_properties_capsule; +#[cfg(feature = "dim3")] +mod mass_properties_cone; mod mass_properties_cuboid; +mod mass_properties_cylinder; #[cfg(feature = "dim2")] mod mass_properties_polygon; mod rigid_body; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index af1fb4a..417ce34 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -54,6 +54,8 @@ pub struct RigidBody { pub(crate) active_set_timestamp: u32, /// The status of the body, governing how it is affected by external forces. pub body_status: BodyStatus, + /// User-defined data associated to this rigid-body. + pub user_data: u128, } impl Clone for RigidBody { @@ -90,6 +92,7 @@ impl RigidBody { active_set_offset: 0, active_set_timestamp: 0, body_status: BodyStatus::Dynamic, + user_data: 0, } } @@ -218,6 +221,7 @@ impl RigidBody { let shift = Translation::from(com.coords); shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } + pub(crate) fn integrate(&mut self, dt: f32) { self.position = self.integrate_velocity(dt) * self.position; } @@ -334,13 +338,14 @@ impl RigidBody { } } -/// A builder for rigid-bodies. +/// A builder for rigid-bodies. pub struct RigidBodyBuilder { position: Isometry, linvel: Vector, angvel: AngVector, body_status: BodyStatus, can_sleep: bool, + user_data: u128, } impl RigidBodyBuilder { @@ -352,6 +357,7 @@ impl RigidBodyBuilder { angvel: na::zero(), body_status, can_sleep: true, + user_data: 0, } } @@ -399,6 +405,12 @@ impl RigidBodyBuilder { self } + /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder. + pub fn user_data(mut self, data: u128) -> Self { + self.user_data = data; + self + } + /// Sets the initial linear velocity of the rigid-body to be created. #[cfg(feature = "dim2")] pub fn linvel(mut self, x: f32, y: f32) -> Self { @@ -433,6 +445,7 @@ impl RigidBodyBuilder { rb.linvel = self.linvel; rb.angvel = self.angvel; rb.body_status = self.body_status; + rb.user_data = self.user_data; if !self.can_sleep { rb.activation.threshold = -1.0; diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index db83fa3..d8a46ee 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -337,7 +337,7 @@ impl SAPAxis { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct SAPRegion { - axii: [SAPAxis; DIM], + axes: [SAPAxis; DIM], existing_proxies: BitVec, #[cfg_attr(feature = "serde-serialize", serde(skip))] to_insert: Vec, // Workspace @@ -347,14 +347,14 @@ struct SAPRegion { impl SAPRegion { pub fn new(bounds: AABB) -> Self { - let axii = [ + let axes = [ SAPAxis::new(bounds.mins.x, bounds.maxs.x), SAPAxis::new(bounds.mins.y, bounds.maxs.y), #[cfg(feature = "dim3")] SAPAxis::new(bounds.mins.z, bounds.maxs.z), ]; SAPRegion { - axii, + axes, existing_proxies: BitVec::new(), to_insert: Vec::new(), update_count: 0, @@ -394,14 +394,14 @@ impl SAPRegion { // Update endpoints. let mut deleted = 0; for dim in 0..DIM { - self.axii[dim].update_endpoints(dim, proxies, reporting); - deleted += self.axii[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies); + self.axes[dim].update_endpoints(dim, proxies, reporting); + deleted += self.axes[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies); } if deleted > 0 { self.proxy_count -= deleted; for dim in 0..DIM { - self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); + self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); } } @@ -411,9 +411,9 @@ impl SAPRegion { if !self.to_insert.is_empty() { // Insert new proxies. for dim in 1..DIM { - self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None); + self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None); } - self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); + self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); self.to_insert.clear(); // In the rare event that all proxies leave this region in the next step, we need an diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs index 0d754af..54736cc 100644 --- a/src/geometry/capsule.rs +++ b/src/geometry/capsule.rs @@ -1,18 +1,16 @@ -use crate::geometry::AABB; +use crate::geometry::{Ray, RayIntersection, AABB}; use crate::math::{Isometry, Point, Rotation, Vector}; use approx::AbsDiffEq; use na::Unit; -use ncollide::query::{PointProjection, PointQuery}; -use ncollide::shape::{FeatureId, Segment}; +use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast}; +use ncollide::shape::{FeatureId, Segment, SupportMap}; #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A capsule shape defined as a segment with a radius. +/// A capsule shape defined as a round segment. pub struct Capsule { - /// The first endpoint of the capsule. - pub a: Point, - /// The second enpdoint of the capsule. - pub b: Point, + /// The axis and endpoint of the capsule. + pub segment: Segment, /// The radius of the capsule. pub radius: f32, } @@ -39,13 +37,14 @@ impl Capsule { /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. pub fn new(a: Point, b: Point, radius: f32) -> Self { - Self { a, b, radius } + let segment = Segment::new(a, b); + Self { segment, radius } } /// The axis-aligned bounding box of this capsule. pub fn aabb(&self, pos: &Isometry) -> AABB { - let a = pos * self.a; - let b = pos * self.b; + let a = pos * self.segment.a; + let b = pos * self.segment.b; let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius); let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius); AABB::new(mins.into(), maxs.into()) @@ -53,7 +52,7 @@ impl Capsule { /// The height of this capsule. pub fn height(&self) -> f32 { - (self.b - self.a).norm() + (self.segment.b - self.segment.a).norm() } /// The half-height of this capsule. @@ -63,17 +62,17 @@ impl Capsule { /// The center of this capsule. pub fn center(&self) -> Point { - na::center(&self.a, &self.b) + na::center(&self.segment.a, &self.segment.b) } /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. pub fn transform_by(&self, pos: &Isometry) -> Self { - Self::new(pos * self.a, pos * self.b, self.radius) + Self::new(pos * self.segment.a, pos * self.segment.b, self.radius) } /// The rotation `r` such that `r * Y` is collinear with `b - a`. pub fn rotation_wrt_y(&self) -> Rotation { - let mut dir = self.b - self.a; + let mut dir = self.segment.b - self.segment.a; if dir.y < 0.0 { dir = -dir; } @@ -96,24 +95,49 @@ impl Capsule { } } -// impl SupportMap for Capsule { -// fn local_support_point(&self, dir: &Vector) -> Point { -// let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis()); -// self.local_support_point_toward(&dir) -// } -// -// fn local_support_point_toward(&self, dir: &Unit) -> Point { -// if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) { -// self.a + **dir * self.radius -// } else { -// self.b + **dir * self.radius -// } -// } -// } +impl SupportMap for Capsule { + fn local_support_point(&self, dir: &Vector) -> Point { + let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis()); + self.local_support_point_toward(&dir) + } + + fn local_support_point_toward(&self, dir: &Unit>) -> Point { + if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) { + self.segment.a + **dir * self.radius + } else { + self.segment.b + **dir * self.radius + } + } +} + +impl RayCast for Capsule { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option { + let ls_ray = ray.inverse_transform_by(m); + + ncollide::query::ray_intersection_with_support_map_with_params( + &Isometry::identity(), + self, + &mut VoronoiSimplex::new(), + &ls_ray, + max_toi, + solid, + ) + .map(|mut res| { + res.normal = m * res.normal; + res + }) + } +} // TODO: this code has been extracted from ncollide and added here // so we can modify it to fit with our new definition of capsule. -// Wa should find a way to avoid this code duplication. +// We should find a way to avoid this code duplication. impl PointQuery for Capsule { #[inline] fn project_point( @@ -122,7 +146,7 @@ impl PointQuery for Capsule { pt: &Point, solid: bool, ) -> PointProjection { - let seg = Segment::new(self.a, self.b); + let seg = Segment::new(self.segment.a, self.segment.b); let proj = seg.project_point(m, pt, solid); let dproj = *pt - proj.point; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 7c293b6..f53d75a 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,145 +1,189 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ - Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, - Proximity, Ray, RayIntersection, Triangle, Trimesh, + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, + InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh, }; +#[cfg(feature = "dim3")] +use crate::geometry::{Cone, Cylinder, RoundCylinder}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use na::Point3; -use ncollide::bounding_volume::{HasBoundingVolume, AABB}; -use ncollide::query::RayCast; -use num::Zero; +use ncollide::bounding_volume::AABB; +use std::ops::Deref; +use std::sync::Arc; +/// The shape of a collider. #[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// An enum grouping all the possible shape of a collider. -pub enum Shape { - /// A ball shape. - Ball(Ball), - /// A convex polygon shape. - Polygon(Polygon), - /// A cuboid shape. - Cuboid(Cuboid), - /// A capsule shape. - Capsule(Capsule), - /// A triangle shape. - Triangle(Triangle), - /// A triangle mesh shape. - Trimesh(Trimesh), - /// A heightfield shape. - HeightField(HeightField), +pub struct ColliderShape(pub Arc); + +impl Deref for ColliderShape { + type Target = dyn Shape; + fn deref(&self) -> &dyn Shape { + &*self.0 + } } -impl Shape { - /// Gets a reference to the underlying ball shape, if `self` is one. - pub fn as_ball(&self) -> Option<&Ball> { - match self { - Shape::Ball(b) => Some(b), - _ => None, - } +impl ColliderShape { + /// Initialize a ball shape defined by its radius. + pub fn ball(radius: f32) -> Self { + ColliderShape(Arc::new(Ball::new(radius))) } - /// Gets a reference to the underlying polygon shape, if `self` is one. - pub fn as_polygon(&self) -> Option<&Polygon> { - match self { - Shape::Polygon(p) => Some(p), - _ => None, - } + /// Initialize a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. + #[cfg(feature = "dim3")] + pub fn cylinder(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } - /// Gets a reference to the underlying cuboid shape, if `self` is one. - pub fn as_cuboid(&self) -> Option<&Cuboid> { - match self { - Shape::Cuboid(c) => Some(c), - _ => None, - } + /// Initialize a rounded cylindrical shape defined by its half-height + /// (along along the y axis), its radius, and its roundedness (the + /// radius of the sphere used for dilating the cylinder). + #[cfg(feature = "dim3")] + pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self { + ColliderShape(Arc::new(RoundCylinder::new( + half_height, + radius, + border_radius, + ))) } - /// Gets a reference to the underlying capsule shape, if `self` is one. - pub fn as_capsule(&self) -> Option<&Capsule> { - match self { - Shape::Capsule(c) => Some(c), - _ => None, - } + /// Initialize a cone shape defined by its half-height + /// (along along the y axis) and its basis radius. + #[cfg(feature = "dim3")] + pub fn cone(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cone::new(half_height, radius))) } - /// Gets a reference to the underlying triangle mesh shape, if `self` is one. - pub fn as_trimesh(&self) -> Option<&Trimesh> { - match self { - Shape::Trimesh(c) => Some(c), - _ => None, - } + /// Initialize a cuboid shape defined by its half-extents. + pub fn cuboid(half_extents: Vector) -> Self { + ColliderShape(Arc::new(Cuboid::new(half_extents))) } - /// Gets a reference to the underlying heightfield shape, if `self` is one. - pub fn as_heightfield(&self) -> Option<&HeightField> { - match self { - Shape::HeightField(h) => Some(h), - _ => None, - } + /// Initialize a capsule shape from its endpoints and radius. + pub fn capsule(a: Point, b: Point, radius: f32) -> Self { + ColliderShape(Arc::new(Capsule::new(a, b, radius))) } - /// Gets a reference to the underlying triangle shape, if `self` is one. - pub fn as_triangle(&self) -> Option<&Triangle> { - match self { - Shape::Triangle(c) => Some(c), - _ => None, - } + /// Initialize a segment shape from its endpoints. + pub fn segment(a: Point, b: Point) -> Self { + ColliderShape(Arc::new(Segment::new(a, b))) } - /// Computes the axis-aligned bounding box of this shape. - pub fn compute_aabb(&self, position: &Isometry) -> AABB { - match self { - Shape::Ball(ball) => ball.bounding_volume(position), - Shape::Polygon(poly) => poly.aabb(position), - Shape::Capsule(caps) => caps.aabb(position), - Shape::Cuboid(cuboid) => cuboid.bounding_volume(position), - Shape::Triangle(triangle) => triangle.bounding_volume(position), - Shape::Trimesh(trimesh) => trimesh.aabb(position), - Shape::HeightField(heightfield) => heightfield.bounding_volume(position), - } + /// Initializes a triangle shape. + pub fn triangle(a: Point, b: Point, c: Point) -> Self { + ColliderShape(Arc::new(Triangle::new(a, b, c))) } - /// Computes the first intersection point between a ray in this collider. - /// - /// Some shapes are not supported yet and will always return `None`. - /// - /// # Parameters - /// - `position`: the position of this shape. - /// - `ray`: the ray to cast. - /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. - pub fn cast_ray( - &self, - position: &Isometry, - ray: &Ray, - max_toi: f32, - ) -> Option { - match self { - Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true), - Shape::Polygon(_poly) => None, - Shape::Capsule(caps) => { - let pos = position * caps.transform_wrt_y(); - let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius); - caps.toi_and_normal_with_ray(&pos, ray, max_toi, true) + /// Initializes a triangle mesh shape defined by its vertex and index buffers. + pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { + ColliderShape(Arc::new(Trimesh::new(vertices, indices))) + } + + /// Initializes an heightfield shape defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim2")] + pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) + } + + /// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim3")] + pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) + } +} + +#[cfg(feature = "serde-serialize")] +impl serde::Serialize for ColliderShape { + fn serialize(&self, serializer: S) -> Result + where + S: serde::Serializer, + { + use crate::serde::ser::SerializeStruct; + + if let Some(ser) = self.0.as_serialize() { + let typ = self.0.shape_type(); + let mut state = serializer.serialize_struct("ColliderShape", 2)?; + state.serialize_field("tag", &(typ as i32))?; + state.serialize_field("inner", ser)?; + state.end() + } else { + Err(serde::ser::Error::custom( + "Found a non-serializable custom shape.", + )) + } + } +} + +#[cfg(feature = "serde-serialize")] +impl<'de> serde::Deserialize<'de> for ColliderShape { + fn deserialize(deserializer: D) -> Result + where + D: serde::Deserializer<'de>, + { + struct Visitor {}; + impl<'de> serde::de::Visitor<'de> for Visitor { + type Value = ColliderShape; + fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { + write!(formatter, "one shape type tag and the inner shape data") } - Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true), - #[cfg(feature = "dim2")] - Shape::Triangle(_) | Shape::Trimesh(_) => { - // This is not implemented yet in 2D. - None - } - #[cfg(feature = "dim3")] - Shape::Triangle(triangle) => { - triangle.toi_and_normal_with_ray(position, ray, max_toi, true) - } - #[cfg(feature = "dim3")] - Shape::Trimesh(trimesh) => { - trimesh.toi_and_normal_with_ray(position, ray, max_toi, true) - } - Shape::HeightField(heightfield) => { - heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) + + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + use num::cast::FromPrimitive; + + let tag: i32 = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + + fn deser<'de, A, S: Shape + serde::Deserialize<'de>>( + seq: &mut A, + ) -> Result, A::Error> + where + A: serde::de::SeqAccess<'de>, + { + let shape: S = seq.next_element()?.ok_or_else(|| { + serde::de::Error::custom("Failed to deserialize builtin shape.") + })?; + Ok(Arc::new(shape) as Arc) + } + + let shape = match ShapeType::from_i32(tag) { + Some(ShapeType::Ball) => deser::(&mut seq)?, + Some(ShapeType::Polygon) => { + unimplemented!() + // let shape: Polygon = seq + // .next_element()? + // .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + // Arc::new(shape) as Arc + } + Some(ShapeType::Cuboid) => deser::(&mut seq)?, + Some(ShapeType::Capsule) => deser::(&mut seq)?, + Some(ShapeType::Triangle) => deser::(&mut seq)?, + Some(ShapeType::Segment) => deser::(&mut seq)?, + Some(ShapeType::Trimesh) => deser::(&mut seq)?, + Some(ShapeType::HeightField) => deser::(&mut seq)?, + #[cfg(feature = "dim3")] + Some(ShapeType::Cylinder) => deser::(&mut seq)?, + #[cfg(feature = "dim3")] + Some(ShapeType::Cone) => deser::(&mut seq)?, + #[cfg(feature = "dim3")] + Some(ShapeType::RoundCylinder) => deser::(&mut seq)?, + None => { + return Err(serde::de::Error::custom( + "found invalid shape type to deserialize", + )) + } + }; + + Ok(ColliderShape(shape)) } } + + deserializer.deserialize_struct("ColliderShape", &["tag", "inner"], Visitor {}) } } @@ -148,7 +192,7 @@ impl Shape { /// /// To build a new collider, use the `ColliderBuilder` structure. pub struct Collider { - shape: Shape, + shape: ColliderShape, density: f32, is_sensor: bool, pub(crate) parent: RigidBodyHandle, @@ -159,9 +203,13 @@ pub struct Collider { pub friction: f32, /// The restitution coefficient of this collider. pub restitution: f32, + pub(crate) collision_groups: InteractionGroups, + pub(crate) solver_groups: InteractionGroups, pub(crate) contact_graph_index: ColliderGraphIndex, pub(crate) proximity_graph_index: ColliderGraphIndex, pub(crate) proxy_index: usize, + /// User-defined data associated to this rigid-body. + pub user_data: u128, } impl Clone for Collider { @@ -209,14 +257,24 @@ impl Collider { &self.delta } + /// The collision groups used by this collider. + pub fn collision_groups(&self) -> InteractionGroups { + self.collision_groups + } + + /// The solver groups used by this collider. + pub fn solver_groups(&self) -> InteractionGroups { + self.solver_groups + } + /// The density of this collider. pub fn density(&self) -> f32 { self.density } /// The geometric shape of this collider. - pub fn shape(&self) -> &Shape { - &self.shape + pub fn shape(&self) -> &dyn Shape { + &*self.shape.0 } /// Compute the axis-aligned bounding box of this collider. @@ -232,20 +290,7 @@ impl Collider { /// Compute the local-space mass properties of this collider. pub fn mass_properties(&self) -> MassProperties { - match &self.shape { - Shape::Ball(ball) => MassProperties::from_ball(self.density, ball.radius), - #[cfg(feature = "dim2")] - Shape::Polygon(p) => MassProperties::from_polygon(self.density, p.vertices()), - #[cfg(feature = "dim3")] - Shape::Polygon(_p) => unimplemented!(), - Shape::Cuboid(c) => MassProperties::from_cuboid(self.density, c.half_extents), - Shape::Capsule(caps) => { - MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius) - } - Shape::Triangle(_) => MassProperties::zero(), - Shape::Trimesh(_) => MassProperties::zero(), - Shape::HeightField(_) => MassProperties::zero(), - } + self.shape.mass_properties(self.density) } } @@ -254,7 +299,7 @@ impl Collider { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct ColliderBuilder { /// The shape of the collider to be built. - pub shape: Shape, + pub shape: ColliderShape, /// The density of the collider to be built. density: Option, /// The friction coefficient of the collider to be built. @@ -265,11 +310,17 @@ pub struct ColliderBuilder { pub delta: Isometry, /// Is this collider a sensor? pub is_sensor: bool, + /// The user-data of the collider being built. + pub user_data: u128, + /// The collision groups for the collider being built. + pub collision_groups: InteractionGroups, + /// The solver groups for the collider being built. + pub solver_groups: InteractionGroups, } impl ColliderBuilder { /// Initialize a new collider builder with the given shape. - pub fn new(shape: Shape) -> Self { + pub fn new(shape: ColliderShape) -> Self { Self { shape, density: None, @@ -277,6 +328,9 @@ impl ColliderBuilder { restitution: 0.0, delta: Isometry::identity(), is_sensor: false, + user_data: 0, + collision_groups: InteractionGroups::all(), + solver_groups: InteractionGroups::all(), } } @@ -288,96 +342,93 @@ impl ColliderBuilder { /// Initialize a new collider builder with a ball shape defined by its radius. pub fn ball(radius: f32) -> Self { - Self::new(Shape::Ball(Ball::new(radius))) + Self::new(ColliderShape::ball(radius)) + } + + /// Initialize a new collider builder with a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. + #[cfg(feature = "dim3")] + pub fn cylinder(half_height: f32, radius: f32) -> Self { + Self::new(ColliderShape::cylinder(half_height, radius)) + } + + /// Initialize a new collider builder with a rounded cylindrical shape defined by its half-height + /// (along along the y axis), its radius, and its roundedness (the + /// radius of the sphere used for dilating the cylinder). + #[cfg(feature = "dim3")] + pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self { + Self::new(ColliderShape::round_cylinder( + half_height, + radius, + border_radius, + )) + } + + /// Initialize a new collider builder with a cone shape defined by its half-height + /// (along along the y axis) and its basis radius. + #[cfg(feature = "dim3")] + pub fn cone(half_height: f32, radius: f32) -> Self { + Self::new(ColliderShape::cone(half_height, radius)) } /// Initialize a new collider builder with a cuboid shape defined by its half-extents. #[cfg(feature = "dim2")] pub fn cuboid(hx: f32, hy: f32) -> Self { - let cuboid = Cuboid { - half_extents: Vector::new(hx, hy), - }; - - Self::new(Shape::Cuboid(cuboid)) - - /* - use crate::math::Point; - let vertices = vec![ - Point::new(hx, -hy), - Point::new(hx, hy), - Point::new(-hx, hy), - Point::new(-hx, -hy), - ]; - let normals = vec![Vector::x(), Vector::y(), -Vector::x(), -Vector::y()]; - let polygon = Polygon::new(vertices, normals); - - Self::new(Shape::Polygon(polygon)) - */ + Self::new(ColliderShape::cuboid(Vector::new(hx, hy))) } /// Initialize a new collider builder with a capsule shape aligned with the `x` axis. pub fn capsule_x(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_x(half_height, radius); - Self::new(Shape::Capsule(capsule)) + let p = Point::from(Vector::x() * half_height); + Self::new(ColliderShape::capsule(-p, p, radius)) } /// Initialize a new collider builder with a capsule shape aligned with the `y` axis. pub fn capsule_y(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_y(half_height, radius); - Self::new(Shape::Capsule(capsule)) + let p = Point::from(Vector::y() * half_height); + Self::new(ColliderShape::capsule(-p, p, radius)) } /// Initialize a new collider builder with a capsule shape aligned with the `z` axis. #[cfg(feature = "dim3")] pub fn capsule_z(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_z(half_height, radius); - Self::new(Shape::Capsule(capsule)) + let p = Point::from(Vector::z() * half_height); + Self::new(ColliderShape::capsule(-p, p, radius)) } /// Initialize a new collider builder with a cuboid shape defined by its half-extents. #[cfg(feature = "dim3")] pub fn cuboid(hx: f32, hy: f32, hz: f32) -> Self { - let cuboid = Cuboid { - half_extents: Vector::new(hx, hy, hz), - }; - - Self::new(Shape::Cuboid(cuboid)) + Self::new(ColliderShape::cuboid(Vector::new(hx, hy, hz))) } /// Initializes a collider builder with a segment shape. - /// - /// A segment shape is modeled by a capsule with a 0 radius. pub fn segment(a: Point, b: Point) -> Self { - let capsule = Capsule::new(a, b, 0.0); - Self::new(Shape::Capsule(capsule)) + Self::new(ColliderShape::segment(a, b)) } /// Initializes a collider builder with a triangle shape. pub fn triangle(a: Point, b: Point, c: Point) -> Self { - let triangle = Triangle::new(a, b, c); - Self::new(Shape::Triangle(triangle)) + Self::new(ColliderShape::triangle(a, b, c)) } /// Initializes a collider builder with a triangle mesh shape defined by its vertex and index buffers. pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { - let trimesh = Trimesh::new(vertices, indices); - Self::new(Shape::Trimesh(trimesh)) + Self::new(ColliderShape::trimesh(vertices, indices)) } /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim2")] pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { - let heightfield = HeightField::new(heights, scale); - Self::new(Shape::HeightField(heightfield)) + Self::new(ColliderShape::heightfield(heights, scale)) } /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim3")] pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { - let heightfield = HeightField::new(heights, scale); - Self::new(Shape::HeightField(heightfield)) + Self::new(ColliderShape::heightfield(heights, scale)) } /// The default friction coefficient used by the collider builder. @@ -385,6 +436,30 @@ impl ColliderBuilder { 0.5 } + /// Sets an arbitrary user-defined 128-bit integer associated to the colliders built by this builder. + pub fn user_data(mut self, data: u128) -> Self { + self.user_data = data; + self + } + + /// Sets the collision groups used by this collider. + /// + /// Two colliders will interact iff. their collision groups are compatible. + /// See [InteractionGroups::test] for details. + pub fn collision_groups(mut self, groups: InteractionGroups) -> Self { + self.collision_groups = groups; + self + } + + /// Sets the solver groups used by this collider. + /// + /// Forces between two colliders in contact will be computed iff their solver groups are + /// compatible. See [InteractionGroups::test] for details. + pub fn solver_groups(mut self, groups: InteractionGroups) -> Self { + self.solver_groups = groups; + self + } + /// Sets whether or not the collider built by this builder is a sensor. pub fn sensor(mut self, is_sensor: bool) -> Self { self.is_sensor = is_sensor; @@ -449,7 +524,7 @@ impl ColliderBuilder { self } - /// Buildes a new collider attached to the given rigid-body. + /// Builds a new collider attached to the given rigid-body. pub fn build(&self) -> Collider { let density = self.get_density(); @@ -466,6 +541,9 @@ impl ColliderBuilder { contact_graph_index: InteractionGraph::::invalid_graph_index(), proximity_graph_index: InteractionGraph::::invalid_graph_index(), proxy_index: crate::INVALID_USIZE, + collision_groups: self.collision_groups, + solver_groups: self.solver_groups, + user_data: self.user_data, } } } diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 7e235c2..d8f3632 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -9,6 +9,16 @@ use { simba::simd::SimdValue, }; +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + /// Flags affecting the behavior of the constraints solver for a given contact manifold. + pub struct SolverFlags: u32 { + /// The constraint solver will take this contact manifold into + /// account for force computation. + const COMPUTE_IMPULSES = 0b01; + } +} + #[derive(Copy, Clone, Debug, PartialEq, Eq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// The type local linear approximation of the neighborhood of a pair contact points on two shapes @@ -206,6 +216,7 @@ impl ContactPair { pub(crate) fn single_manifold<'a, 'b>( &'a mut self, colliders: &'b ColliderSet, + flags: SolverFlags, ) -> ( &'b Collider, &'b Collider, @@ -216,7 +227,7 @@ impl ContactPair { let coll2 = &colliders[self.pair.collider2]; if self.manifolds.len() == 0 { - let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2); + let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags); self.manifolds.push(manifold); } @@ -288,6 +299,8 @@ pub struct ContactManifold { /// The relative position between the second collider and its parent at the time the /// contact points were generated. pub delta2: Isometry, + /// Flags used to control some aspects of the constraints solver for this contact manifold. + pub solver_flags: SolverFlags, } impl ContactManifold { @@ -299,6 +312,7 @@ impl ContactManifold { delta2: Isometry, friction: f32, restitution: f32, + solver_flags: SolverFlags, ) -> ContactManifold { Self { #[cfg(feature = "dim2")] @@ -319,6 +333,7 @@ impl ContactManifold { delta2, constraint_index: 0, position_constraint_index: 0, + solver_flags, } } @@ -342,11 +357,17 @@ impl ContactManifold { delta2: self.delta2, constraint_index: self.constraint_index, position_constraint_index: self.position_constraint_index, + solver_flags: self.solver_flags, } } - pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self { - Self::with_subshape_indices(pair, coll1, coll2, 0, 0) + pub(crate) fn from_colliders( + pair: ColliderPair, + coll1: &Collider, + coll2: &Collider, + flags: SolverFlags, + ) -> Self { + Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags) } pub(crate) fn with_subshape_indices( @@ -355,6 +376,7 @@ impl ContactManifold { coll2: &Collider, subshape1: usize, subshape2: usize, + solver_flags: SolverFlags, ) -> Self { Self::new( pair, @@ -364,6 +386,7 @@ impl ContactManifold { *coll2.position_wrt_parent(), (coll1.friction + coll2.friction) * 0.5, (coll1.restitution + coll2.restitution) * 0.5, + solver_flags, ) } @@ -430,16 +453,26 @@ impl ContactManifold { #[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; } - // const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES; - const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES; - let local_n2 = pos12 * self.local_n2; - if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD { + if -self.local_n1.dot(&local_n2) < angle_dot_threshold { return false; } @@ -455,8 +488,7 @@ impl ContactManifold { } let new_local_p1 = local_p2 - self.local_n1 * dist; - let dist_threshold = 0.001; // FIXME: this should not be hard-coded. - if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold { + if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold { return false; } diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index a187832..69bc5e3 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -1,32 +1,21 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Ball, Contact, KinematicsCategory, Shape}; +use crate::geometry::{Ball, Contact, KinematicsCategory}; use crate::math::Isometry; use na::Unit; use ncollide::query::PointQuery; pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { - if let Shape::Ball(ball1) = ctxt.shape1 { + if let Some(ball1) = ctxt.shape1.as_ball() { ctxt.manifold.swap_identifiers(); - - match ctxt.shape2 { - Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true), - Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true), - Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true), - _ => unimplemented!(), - } - } else if let Shape::Ball(ball2) = ctxt.shape2 { - match ctxt.shape1 { - Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false), - Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false), - Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false), - _ => unimplemented!(), - } + do_generate_contacts(ctxt.shape2, ball1, ctxt, true); + } else if let Some(ball2) = ctxt.shape2.as_ball() { + do_generate_contacts(ctxt.shape1, ball2, ctxt, false); } ctxt.manifold.sort_contacts(ctxt.prediction_distance); } -fn do_generate_contacts>( +fn do_generate_contacts>( point_query1: &P, ball2: &Ball, ctxt: &mut PrimitiveContactGenerationContext, diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index 3800ce6..3104496 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -1,14 +1,14 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory, Shape}; +use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory}; use crate::math::Isometry; use crate::math::Vector; use approx::AbsDiffEq; use na::Unit; #[cfg(feature = "dim2")] -use ncollide::shape::{Segment, SegmentPointLocation}; +use ncollide::shape::SegmentPointLocation; pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Capsule(capsule1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) { generate_contacts( ctxt.prediction_distance, capsule1, @@ -39,10 +39,11 @@ pub fn generate_contacts<'a>( let pos12 = pos1.inverse() * pos2; let pos21 = pos12.inverse(); - let capsule2_1 = capsule2.transform_by(&pos12); + let seg1 = capsule1.segment; + let seg2_1 = capsule2.segment.transformed(&pos12); let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( - (&capsule1.a, &capsule1.b), - (&capsule2_1.a, &capsule2_1.b), + (&seg1.a, &seg1.b), + (&seg2_1.a, &seg2_1.b), ); // We do this clone to perform contact tracking and transfer impulses. @@ -65,8 +66,8 @@ pub fn generate_contacts<'a>( let bcoords1 = loc1.barycentric_coordinates(); let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1]; - let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1]; + let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1]; + let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1]; let local_n1 = Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis()); @@ -87,18 +88,15 @@ pub fn generate_contacts<'a>( return; } - let seg1 = Segment::new(capsule1.a, capsule1.b); - let seg2 = Segment::new(capsule2_1.a, capsule2_1.b); - - if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2.direction()) { + if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.direction()) { if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8 && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8 { - // Capsules axii are almost parallel and are almost perpendicular to the normal. + // Capsules axes are almost parallel and are almost perpendicular to the normal. // Find a second contact point. if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal( - (capsule1.a, capsule1.b), - (capsule2_1.a, capsule2_1.b), + (seg1.a, seg1.b), + (seg2_1.a, seg2_1.b), *local_n1, ) { let contact = @@ -156,17 +154,18 @@ pub fn generate_contacts<'a>( let pos12 = pos1.inverse() * pos2; let pos21 = pos12.inverse(); - let capsule2_1 = capsule1.transform_by(&pos12); + let seg1 = capsule1.segment; + let seg2_1 = capsule2.segment.transformed(&pos12); let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( - (&capsule1.a, &capsule1.b), - (&capsule2_1.a, &capsule2_1.b), + (&seg1.a, &seg1.b), + (&seg2_1.a, &seg2_1.b), ); { let bcoords1 = loc1.barycentric_coordinates(); let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1]; - let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1]; + let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1]; + let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1]; let local_n1 = Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis()); diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index 8c846e0..1872c7b 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -1,8 +1,10 @@ +#[cfg(feature = "dim3")] +use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace; use crate::geometry::contact_generator::{ ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace, PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace, }; -use crate::geometry::Shape; +use crate::geometry::ShapeType; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm @@ -11,8 +13,8 @@ pub trait ContactDispatcher { /// Select the collision-detection algorithm for the given pair of primitive shapes. fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveContactGenerator, Option>, @@ -20,8 +22,8 @@ pub trait ContactDispatcher { /// Select the collision-detection algorithm for the given pair of non-primitive shapes. fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ContactPhase, Option>); } @@ -31,14 +33,14 @@ pub struct DefaultContactDispatcher; impl ContactDispatcher for DefaultContactDispatcher { fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveContactGenerator, Option>, ) { match (shape1, shape2) { - (Shape::Ball(_), Shape::Ball(_)) => ( + (ShapeType::Ball, ShapeType::Ball) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_ball, #[cfg(feature = "simd-is-enabled")] @@ -47,52 +49,64 @@ impl ContactDispatcher for DefaultContactDispatcher { }, None, ), - (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + (ShapeType::Cuboid, ShapeType::Cuboid) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_cuboid_cuboid, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Polygon(_), Shape::Polygon(_)) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_polygon_polygon, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (Shape::Capsule(_), Shape::Capsule(_)) => ( + // (ShapeType::Polygon, ShapeType::Polygon) => ( + // PrimitiveContactGenerator { + // generate_contacts: super::generate_contacts_polygon_polygon, + // ..PrimitiveContactGenerator::default() + // }, + // None, + // ), + (ShapeType::Capsule, ShapeType::Capsule) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_capsule_capsule, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Cuboid(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Cuboid(_)) - | (Shape::Triangle(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Triangle(_)) - | (Shape::Capsule(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Capsule(_)) => ( + (_, ShapeType::Ball) | (ShapeType::Ball, _) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => ( + (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_cuboid_capsule, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => ( + (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => { + ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_cuboid_triangle, + ..PrimitiveContactGenerator::default() + }, + None, + ) + } + #[cfg(feature = "dim3")] + (ShapeType::Cylinder, _) + | (_, ShapeType::Cylinder) + | (ShapeType::Cone, _) + | (_, ShapeType::Cone) + | (ShapeType::RoundCylinder, _) + | (_, ShapeType::RoundCylinder) + | (ShapeType::Capsule, _) + | (_, ShapeType::Capsule) => ( PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_cuboid_triangle, + generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() }, - None, + Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())), ), _ => (PrimitiveContactGenerator::default(), None), } @@ -100,18 +114,18 @@ impl ContactDispatcher for DefaultContactDispatcher { fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ContactPhase, Option>) { match (shape1, shape2) { - (Shape::Trimesh(_), _) | (_, Shape::Trimesh(_)) => ( + (ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_trimesh_shape, ..ContactGenerator::default() }), Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())), ), - (Shape::HeightField(_), _) | (_, Shape::HeightField(_)) => ( + (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_heightfield_shape, ..ContactGenerator::default() diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index 9dd0050..728794d 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -1,5 +1,6 @@ use crate::geometry::{ Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape, + SolverFlags, }; use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] @@ -26,8 +27,9 @@ impl ContactPhase { Self::NearPhase(gen) => (gen.generate_contacts)(&mut context), Self::ExactPhase(gen) => { // Build the primitive context from the non-primitive context and dispatch. - let (collider1, collider2, manifold, workspace) = - context.pair.single_manifold(context.colliders); + let (collider1, collider2, manifold, workspace) = context + .pair + .single_manifold(context.colliders, context.solver_flags); let mut context2 = PrimitiveContactGenerationContext { prediction_distance: context.prediction_distance, collider1, @@ -85,9 +87,11 @@ impl ContactPhase { [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH], > = ArrayVec::new(); - for pair in context.pairs.iter_mut() { + for (pair, solver_flags) in + context.pairs.iter_mut().zip(context.solver_flags.iter()) + { let (collider1, collider2, manifold, workspace) = - pair.single_manifold(context.colliders); + pair.single_manifold(context.colliders, *solver_flags); colliders_arr.push((collider1, collider2)); manifold_arr.push(manifold); workspace_arr.push(workspace); @@ -139,8 +143,8 @@ pub struct PrimitiveContactGenerationContext<'a> { pub prediction_distance: f32, pub collider1: &'a Collider, pub collider2: &'a Collider, - pub shape1: &'a Shape, - pub shape2: &'a Shape, + pub shape1: &'a dyn Shape, + pub shape2: &'a dyn Shape, pub position1: &'a Isometry, pub position2: &'a Isometry, pub manifold: &'a mut ContactManifold, @@ -152,8 +156,8 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> { pub prediction_distance: f32, pub colliders1: [&'a Collider; SIMD_WIDTH], pub colliders2: [&'a Collider; SIMD_WIDTH], - pub shapes1: [&'a Shape; SIMD_WIDTH], - pub shapes2: [&'a Shape; SIMD_WIDTH], + pub shapes1: [&'a dyn Shape; SIMD_WIDTH], + pub shapes2: [&'a dyn Shape; SIMD_WIDTH], pub positions1: &'a Isometry, pub positions2: &'a Isometry, pub manifolds: &'a mut [&'b mut ContactManifold], @@ -188,6 +192,7 @@ pub struct ContactGenerationContext<'a> { pub prediction_distance: f32, pub colliders: &'a ColliderSet, pub pair: &'a mut ContactPair, + pub solver_flags: SolverFlags, } #[cfg(feature = "simd-is-enabled")] @@ -196,6 +201,7 @@ pub struct ContactGenerationContextSimd<'a, 'b> { pub prediction_distance: f32, pub colliders: &'a ColliderSet, pub pairs: &'a mut [&'b mut ContactPair], + pub solver_flags: &'a [SolverFlags], } #[derive(Copy, Clone)] diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index a7857a1..3fd4a17 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -1,15 +1,14 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; #[cfg(feature = "dim3")] use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory, Shape}; +use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory}; #[cfg(feature = "dim2")] use crate::geometry::{CuboidFeature, CuboidFeatureFace}; use crate::math::Isometry; use crate::math::Vector; -use ncollide::shape::Segment; pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) { generate_contacts( ctxt.prediction_distance, cube1, @@ -20,7 +19,9 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon false, ); ctxt.manifold.update_warmstart_multiplier(); - } else if let (Shape::Capsule(capsule1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(capsule1), Some(cube2)) = + (ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid()) + { generate_contacts( ctxt.prediction_distance, cube2, @@ -53,7 +54,7 @@ pub fn generate_contacts<'a>( return; } - let segment2 = Segment::new(capsule2.a, capsule2.b); + let segment2 = capsule2.segment; /* * diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index d879a22..5be5af3 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -1,12 +1,12 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory, Shape}; +use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory}; use crate::math::Isometry; #[cfg(feature = "dim2")] use crate::math::Vector; use ncollide::shape::Cuboid; pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { generate_contacts( ctxt.prediction_distance, cube1, diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 1a0358d..562d7d6 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -1,7 +1,7 @@ use crate::geometry::contact_generator::PrimitiveContactGenerationContext; #[cfg(feature = "dim3")] use crate::geometry::PolyhedronFace; -use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Shape, Triangle}; +use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Triangle}; use crate::math::Isometry; #[cfg(feature = "dim2")] use crate::{ @@ -10,7 +10,7 @@ use crate::{ }; pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { generate_contacts( ctxt.prediction_distance, cube1, @@ -21,7 +21,9 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo false, ); ctxt.manifold.update_warmstart_multiplier(); - } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(triangle1), Some(cube2)) = + (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) + { generate_contacts( ctxt.prediction_distance, cube2, diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 04afc65..f291fa0 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -3,10 +3,8 @@ use crate::geometry::contact_generator::{ }; #[cfg(feature = "dim2")] use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, HeightField, Shape}; +use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType}; use crate::ncollide::bounding_volume::BoundingVolume; -#[cfg(feature = "dim3")] -use crate::{geometry::Triangle, math::Point}; use std::any::Any; use std::collections::hash_map::Entry; use std::collections::HashMap; @@ -38,9 +36,9 @@ pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext) let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::HeightField(heightfield1) = collider1.shape() { + if let Some(heightfield1) = collider1.shape().as_heightfield() { do_generate_contacts(heightfield1, collider1, collider2, ctxt, false) - } else if let Shape::HeightField(heightfield2) = collider2.shape() { + } else if let Some(heightfield2) = collider2.shape().as_heightfield() { do_generate_contacts(heightfield2, collider2, collider1, ctxt, true) } } @@ -59,6 +57,7 @@ fn do_generate_contacts( .expect("The HeightFieldShapeContactGeneratorWorkspace is missing.") .downcast_mut() .expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace."); + let shape_type2 = collider2.shape().shape_type(); /* * Detect if the detector context has been reset. @@ -71,24 +70,9 @@ fn do_generate_contacts( } else { manifold.subshape_index_pair.1 }; - // println!( - // "Restoring for {} [chosen with {:?}]", - // subshape_id, manifold.subshape_index_pair - // ); - - // Use dummy shapes for the dispatch. - #[cfg(feature = "dim2")] - let sub_shape1 = - Shape::Capsule(Capsule::new(na::Point::origin(), na::Point::origin(), 0.0)); - #[cfg(feature = "dim3")] - let sub_shape1 = Shape::Triangle(Triangle::new( - Point::origin(), - Point::origin(), - Point::origin(), - )); let (generator, workspace2) = ctxt .dispatcher - .dispatch_primitives(&sub_shape1, collider2.shape()); + .dispatch_primitives(ShapeType::Capsule, shape_type2); let sub_detector = SubDetector { generator, @@ -120,12 +104,16 @@ fn do_generate_contacts( let manifolds = &mut ctxt.pair.manifolds; let prediction_distance = ctxt.prediction_distance; let dispatcher = ctxt.dispatcher; + let solver_flags = ctxt.solver_flags; + let shape_type2 = collider2.shape().shape_type(); heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| { + let position1 = collider1.position(); #[cfg(feature = "dim2")] - let sub_shape1 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0)); + let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead. #[cfg(feature = "dim3")] - let sub_shape1 = Shape::Triangle(*part1); + let sub_shape1 = *part1; + let sub_detector = match workspace.sub_detectors.entry(i) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); @@ -137,15 +125,21 @@ fn do_generate_contacts( } Entry::Vacant(entry) => { let (generator, workspace2) = - dispatcher.dispatch_primitives(&sub_shape1, collider2.shape()); + dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2); let sub_detector = SubDetector { generator, manifold_id: manifolds.len(), timestamp: new_timestamp, workspace: workspace2, }; - let manifold = - ContactManifold::with_subshape_indices(coll_pair, collider1, collider2, i, 0); + let manifold = ContactManifold::with_subshape_indices( + coll_pair, + collider1, + collider2, + i, + 0, + solver_flags, + ); manifolds.push(manifold); entry.insert(sub_detector) @@ -162,7 +156,7 @@ fn do_generate_contacts( shape1: collider2.shape(), shape2: &sub_shape1, position1: collider2.position(), - position2: collider1.position(), + position2: position1, manifold, workspace: sub_detector.workspace.as_deref_mut(), } @@ -173,7 +167,7 @@ fn do_generate_contacts( collider2, shape1: &sub_shape1, shape2: collider2.shape(), - position1: collider1.position(), + position1, position2: collider2.position(), manifold, workspace: sub_detector.workspace.as_deref_mut(), diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index ecd2540..0549420 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -18,15 +18,18 @@ pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triang pub use self::heightfield_shape_contact_generator::{ generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace, }; -pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; +#[cfg(feature = "dim3")] +pub use self::pfm_pfm_contact_generator::{ + generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace, +}; +// pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; pub use self::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, }; +pub(crate) use self::polygon_polygon_contact_generator::clip_segments; #[cfg(feature = "dim2")] -pub(crate) use self::polygon_polygon_contact_generator::{ - clip_segments, clip_segments_with_normal, -}; +pub(crate) use self::polygon_polygon_contact_generator::clip_segments_with_normal; mod ball_ball_contact_generator; mod ball_convex_contact_generator; @@ -39,6 +42,8 @@ mod cuboid_cuboid_contact_generator; mod cuboid_polygon_contact_generator; mod cuboid_triangle_contact_generator; mod heightfield_shape_contact_generator; +#[cfg(feature = "dim3")] +mod pfm_pfm_contact_generator; mod polygon_polygon_contact_generator; mod trimesh_shape_contact_generator; diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs new file mode 100644 index 0000000..1dcae33 --- /dev/null +++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs @@ -0,0 +1,119 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +use crate::geometry::{KinematicsCategory, PolygonalFeatureMap, PolyhedronFace}; +use crate::math::{Isometry, Vector}; +use na::Unit; +use ncollide::query; +use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex}; + +pub struct PfmPfmContactManifoldGeneratorWorkspace { + simplex: VoronoiSimplex, + last_gjk_dir: Option>>, + feature1: PolyhedronFace, + feature2: PolyhedronFace, +} + +impl Default for PfmPfmContactManifoldGeneratorWorkspace { + fn default() -> Self { + Self { + simplex: VoronoiSimplex::new(), + last_gjk_dir: None, + feature1: PolyhedronFace::new(), + feature2: PolyhedronFace::new(), + } + } +} + +pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) { + if let (Some((pfm1, border_radius1)), Some((pfm2, border_radius2))) = ( + ctxt.shape1.as_polygonal_feature_map(), + ctxt.shape2.as_polygonal_feature_map(), + ) { + do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt); + ctxt.manifold.update_warmstart_multiplier(); + ctxt.manifold.sort_contacts(ctxt.prediction_distance); + } +} + +fn do_generate_contacts( + pfm1: &dyn PolygonalFeatureMap, + border_radius1: f32, + pfm2: &dyn PolygonalFeatureMap, + border_radius2: f32, + ctxt: &mut PrimitiveContactGenerationContext, +) { + let pos12 = ctxt.position1.inverse() * ctxt.position2; + let pos21 = pos12.inverse(); + + // We use very small thresholds for the manifold update because something to high would + // cause numerical drifts with the effect of introducing bumps in + // what should have been smooth rolling motions. + if ctxt + .manifold + .try_update_contacts_eps(&pos12, crate::utils::COS_1_DEGREES, 1.0e-6) + { + return; + } + + let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt + .workspace + .as_mut() + .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.") + .downcast_mut() + .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, + &pos12, + pfm2, + total_prediction, + &mut workspace.simplex, + workspace.last_gjk_dir, + ); + + let old_manifold_points = ctxt.manifold.points.clone(); + ctxt.manifold.points.clear(); + + match contact { + GJKResult::ClosestPoints(_, _, dir) => { + workspace.last_gjk_dir = Some(dir); + let normal1 = dir; + let normal2 = pos21 * -dir; + pfm1.local_support_feature(&normal1, &mut workspace.feature1); + pfm2.local_support_feature(&normal2, &mut workspace.feature2); + workspace.feature2.transform_by(&pos12); + + PolyhedronFace::contacts( + total_prediction, + &workspace.feature1, + &normal1, + &workspace.feature2, + &pos21, + ctxt.manifold, + ); + + if border_radius1 != 0.0 || border_radius2 != 0.0 { + for contact in &mut ctxt.manifold.points { + contact.local_p1 += *normal1 * border_radius1; + contact.local_p2 += *normal2 * border_radius2; + contact.dist -= border_radius1 + border_radius2; + } + } + + // Adjust points to take the radius into account. + ctxt.manifold.local_n1 = *normal1; + ctxt.manifold.local_n2 = *normal2; + ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // TODO: is this the more appropriate? + ctxt.manifold.kinematics.radius1 = 0.0; + ctxt.manifold.kinematics.radius2 = 0.0; + } + GJKResult::NoIntersection(dir) => { + workspace.last_gjk_dir = Some(dir); + } + _ => {} + } + + // Transfer impulses. + super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false); +} diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 33b54e4..0e7543d 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -1,24 +1,27 @@ +#![allow(dead_code)] // TODO: remove this once we support polygons. + use crate::geometry::contact_generator::PrimitiveContactGenerationContext; -use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon, Shape}; +use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon}; use crate::math::{Isometry, Point}; #[cfg(feature = "dim2")] use crate::{math::Vector, utils}; -pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { - generate_contacts( - polygon1, - &ctxt.position1, - polygon2, - &ctxt.position2, - ctxt.manifold, - ); - ctxt.manifold.update_warmstart_multiplier(); - } else { - unreachable!() - } - - ctxt.manifold.sort_contacts(ctxt.prediction_distance); +pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationContext) { + unimplemented!() + // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { + // generate_contacts( + // polygon1, + // &ctxt.position1, + // polygon2, + // &ctxt.position2, + // ctxt.manifold, + // ); + // ctxt.manifold.update_warmstart_multiplier(); + // } else { + // unreachable!() + // } + // + // ctxt.manifold.sort_contacts(ctxt.prediction_distance); } fn generate_contacts<'a>( diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 52ba9b7..9474516 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -1,7 +1,7 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, Shape, Trimesh}; +use crate::geometry::{Collider, ContactManifold, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeContactGeneratorWorkspace { @@ -26,9 +26,9 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) { let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::Trimesh(trimesh1) = collider1.shape() { + if let Some(trimesh1) = collider1.shape().as_trimesh() { do_generate_contacts(trimesh1, collider1, collider2, ctxt, false) - } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + } else if let Some(trimesh2) = collider2.shape().as_trimesh() { do_generate_contacts(trimesh2, collider2, collider1, ctxt, true) } } @@ -121,6 +121,7 @@ fn do_generate_contacts( let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut old_manifolds_it = workspace.old_manifolds.drain(..); + let shape_type2 = collider2.shape().shape_type(); for (i, triangle_id) in new_interferences.iter().enumerate() { if *triangle_id >= trimesh1.num_triangles() { @@ -148,6 +149,7 @@ fn do_generate_contacts( collider2, *triangle_id, 0, + ctxt.solver_flags, ) } else { // We already have a manifold for this triangle. @@ -159,10 +161,10 @@ fn do_generate_contacts( } let manifold = &mut ctxt.pair.manifolds[i]; - let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let triangle1 = trimesh1.triangle(*triangle_id); let (generator, mut workspace2) = ctxt .dispatcher - .dispatch_primitives(&triangle1, collider2.shape()); + .dispatch_primitives(ShapeType::Triangle, shape_type2); let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 { PrimitiveContactGenerationContext { diff --git a/src/geometry/interaction_groups.rs b/src/geometry/interaction_groups.rs new file mode 100644 index 0000000..48808de --- /dev/null +++ b/src/geometry/interaction_groups.rs @@ -0,0 +1,60 @@ +#[repr(transparent)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)] +/// Pairwise filtering using bit masks. +/// +/// This filtering method is based on two 16-bit values: +/// - The interaction groups (the 16 left-most bits of `self.0`). +/// - The interaction mask (the 16 right-most bits of `self.0`). +/// +/// An interaction is allowed between two filters `a` and `b` two conditions +/// are met simultaneously: +/// - The interaction groups of `a` has at least one bit set to `1` in common with the interaction mask of `b`. +/// - The interaction groups of `b` has at least one bit set to `1` in common with the interaction mask of `a`. +/// In other words, interactions are allowed between two filter iff. the following condition is met: +/// ```ignore +/// ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0 +/// ``` +pub struct InteractionGroups(pub u32); + +impl InteractionGroups { + /// Initializes with the given interaction groups and interaction mask. + pub const fn new(groups: u16, masks: u16) -> Self { + Self::none().with_groups(groups).with_mask(masks) + } + + /// Allow interaction with everything. + pub const fn all() -> Self { + Self(u32::MAX) + } + + /// Prevent all interactions. + pub const fn none() -> Self { + Self(0) + } + + /// Sets the group this filter is part of. + pub const fn with_groups(self, groups: u16) -> Self { + Self((self.0 & 0x0000ffff) | ((groups as u32) << 16)) + } + + /// Sets the interaction mask of this filter. + pub const fn with_mask(self, mask: u16) -> Self { + Self((self.0 & 0xffff0000) | (mask as u32)) + } + + /// Check if interactions should be allowed based on the interaction groups and mask. + /// + /// An interaction is allowed iff. the groups of `self` contain at least one bit set to 1 in common + /// with the mask of `rhs`, and vice-versa. + #[inline] + pub const fn test(self, rhs: Self) -> bool { + ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0 + } +} + +impl Default for InteractionGroups { + fn default() -> Self { + Self::all() + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index 562f962..c8ad28e 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -2,10 +2,10 @@ pub use self::broad_phase_multi_sap::BroadPhase; pub use self::capsule::Capsule; -pub use self::collider::{Collider, ColliderBuilder, Shape}; +pub use self::collider::{Collider, ColliderBuilder, ColliderShape}; pub use self::collider_set::{ColliderHandle, ColliderSet}; pub use self::contact::{ - Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, + Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, SolverFlags, }; pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher}; #[cfg(feature = "dim2")] @@ -19,9 +19,14 @@ pub use self::narrow_phase::NarrowPhase; pub use self::polygon::Polygon; pub use self::proximity::ProximityPair; pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; +#[cfg(feature = "dim3")] +pub use self::round_cylinder::RoundCylinder; pub use self::trimesh::Trimesh; +pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; pub use ncollide::query::Proximity; +/// A segment shape. +pub type Segment = ncollide::shape::Segment; /// A cuboid shape. pub type Cuboid = ncollide::shape::Cuboid; /// A triangle shape. @@ -30,6 +35,12 @@ pub type Triangle = ncollide::shape::Triangle; pub type Ball = ncollide::shape::Ball; /// A heightfield shape. pub type HeightField = ncollide::shape::HeightField; +/// A cylindrical shape. +#[cfg(feature = "dim3")] +pub type Cylinder = ncollide::shape::Cylinder; +/// A cone shape. +#[cfg(feature = "dim3")] +pub type Cone = ncollide::shape::Cone; /// An axis-aligned bounding box. pub type AABB = ncollide::bounding_volume::AABB; /// Event triggered when two non-sensor colliders start or stop being in contact. @@ -40,6 +51,8 @@ pub type ProximityEvent = ncollide::pipeline::ProximityEvent; pub type Ray = ncollide::query::Ray; /// The intersection between a ray and a collider. pub type RayIntersection = ncollide::query::RayIntersection; +/// The the projection of a point on a collider. +pub type PointProjection = ncollide::query::PointProjection; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::ball::WBall; @@ -47,18 +60,22 @@ pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; +pub(crate) use self::contact_generator::clip_segments; #[cfg(feature = "dim2")] -pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal}; +pub(crate) use self::contact_generator::clip_segments_with_normal; pub(crate) use self::narrow_phase::ContactManifoldIndex; #[cfg(feature = "dim3")] +pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap; +#[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; +pub use self::interaction_groups::InteractionGroups; +pub use self::shape::{Shape, ShapeType}; mod ball; mod broad_phase_multi_sap; -mod capsule; mod collider; mod collider_set; mod contact; @@ -81,3 +98,11 @@ 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 ebe0a79..c1bd411 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -14,8 +14,9 @@ use crate::geometry::proximity_detector::{ // proximity_detector::ProximityDetectionContextSimd, WBall, //}; use crate::geometry::{ - BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent, - ProximityPair, RemovedCollider, + BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactPairFilter, + PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, RemovedCollider, + SolverFlags, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] @@ -197,7 +198,8 @@ impl NarrowPhase { if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() { let dispatcher = DefaultProximityDispatcher; - let generator = dispatcher.dispatch(co1.shape(), co2.shape()); + let generator = dispatcher + .dispatch(co1.shape().shape_type(), co2.shape().shape_type()); let interaction = ProximityPair::new(*pair, generator.0, generator.1); let _ = self.proximity_graph.add_edge( @@ -226,7 +228,8 @@ impl NarrowPhase { if self.contact_graph.graph.find_edge(gid1, gid2).is_none() { let dispatcher = DefaultContactDispatcher; - let generator = dispatcher.dispatch(co1.shape(), co2.shape()); + let generator = dispatcher + .dispatch(co1.shape().shape_type(), co2.shape().shape_type()); let interaction = ContactPair::new(*pair, generator.0, generator.1); let _ = self.contact_graph.add_edge( co1.contact_graph_index, @@ -288,6 +291,7 @@ impl NarrowPhase { prediction_distance: f32, bodies: &RigidBodySet, colliders: &ColliderSet, + pair_filter: Option<&dyn ProximityPairFilter>, events: &dyn EventHandler, ) { par_iter_mut!(&mut self.proximity_graph.graph.edges).for_each(|edge| { @@ -299,16 +303,44 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) { - // No need to update this contact because nothing moved. + if (rb1.is_sleeping() && rb2.is_static()) + || (rb2.is_sleeping() && rb1.is_static()) + || (rb1.is_sleeping() && rb2.is_sleeping()) + { + // No need to update this proximity because nothing moved. return; } + if !co1.collision_groups.test(co2.collision_groups) { + // The proximity is not allowed. + return; + } + + if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() { + // Default filtering rule: no proximity between two non-dynamic bodies. + return; + } + + if let Some(filter) = pair_filter { + let context = PairFilterContext { + rigid_body1: rb1, + rigid_body2: rb2, + collider1: co1, + collider2: co2, + }; + + if !filter.filter_proximity_pair(&context) { + // No proximity allowed. + return; + } + } + let dispatcher = DefaultProximityDispatcher; if pair.detector.is_none() { // We need a redispatch for this detector. // This can happen, e.g., after restoring a snapshot of the narrow-phase. - let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape()); + let (detector, workspace) = + dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type()); pair.detector = Some(detector); pair.detector_workspace = workspace; } @@ -326,69 +358,6 @@ impl NarrowPhase { .unwrap() .detect_proximity(context, events); }); - - /* - // First, group pairs. - // NOTE: the transmutes here are OK because the Vec are all cleared - // before we leave this method. - // We do this in order to avoid reallocating those vecs each time - // we compute the contacts. Unsafe is necessary because we can't just - // store a Vec<&mut ProximityPair> into the NarrowPhase struct without - // polluting the World with lifetimes. - let ball_ball_prox: &mut Vec<&mut ProximityPair> = - unsafe { std::mem::transmute(&mut self.ball_ball_prox) }; - let shape_shape_prox: &mut Vec<&mut ProximityPair> = - unsafe { std::mem::transmute(&mut self.shape_shape_prox) }; - - let bodies = &bodies.bodies; - - // FIXME: don't iterate through all the interactions. - for pair in &mut self.proximity_graph.interactions { - let co1 = &colliders[pair.pair.collider1]; - let co2 = &colliders[pair.pair.collider2]; - - // FIXME: avoid lookup into bodies. - let rb1 = &bodies[co1.parent]; - let rb2 = &bodies[co2.parent]; - - if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) - { - // No need to update this proximity because nothing moved. - continue; - } - - match (co1.shape(), co2.shape()) { - (Shape::Ball(_), Shape::Ball(_)) => ball_ball_prox.push(pair), - _ => shape_shape_prox.push(pair), - } - } - - par_chunks_mut!(ball_ball_prox, SIMD_WIDTH).for_each(|pairs| { - let context = ProximityDetectionContextSimd { - dispatcher: &DefaultProximityDispatcher, - prediction_distance, - colliders, - pairs, - }; - context.pairs[0] - .detector - .detect_proximity_simd(context, events); - }); - - par_iter_mut!(shape_shape_prox).for_each(|pair| { - let context = ProximityDetectionContext { - dispatcher: &DefaultProximityDispatcher, - prediction_distance, - colliders, - pair, - }; - - context.pair.detector.detect_proximity(context, events); - }); - - ball_ball_prox.clear(); - shape_shape_prox.clear(); - */ } pub(crate) fn compute_contacts( @@ -396,6 +365,7 @@ impl NarrowPhase { prediction_distance: f32, bodies: &RigidBodySet, colliders: &ColliderSet, + pair_filter: Option<&dyn ContactPairFilter>, events: &dyn EventHandler, ) { par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { @@ -407,18 +377,52 @@ impl NarrowPhase { let rb1 = &bodies[co1.parent]; let rb2 = &bodies[co2.parent]; - if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static())) - || (!rb1.is_dynamic() && !rb2.is_dynamic()) + if (rb1.is_sleeping() && rb2.is_static()) + || (rb2.is_sleeping() && rb1.is_static()) + || (rb1.is_sleeping() && rb2.is_sleeping()) { // No need to update this contact because nothing moved. return; } + if !co1.collision_groups.test(co2.collision_groups) { + // The collision is not allowed. + return; + } + + if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() { + // Default filtering rule: no contact between two non-dynamic bodies. + return; + } + + let mut solver_flags = if let Some(filter) = pair_filter { + let context = PairFilterContext { + rigid_body1: rb1, + rigid_body2: rb2, + collider1: co1, + collider2: co2, + }; + + if let Some(solver_flags) = filter.filter_contact_pair(&context) { + solver_flags + } else { + // No contact allowed. + return; + } + } else { + SolverFlags::COMPUTE_IMPULSES + }; + + if !co1.solver_groups.test(co2.solver_groups) { + solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); + } + let dispatcher = DefaultContactDispatcher; if pair.generator.is_none() { // We need a redispatch for this generator. // This can happen, e.g., after restoring a snapshot of the narrow-phase. - let (generator, workspace) = dispatcher.dispatch(co1.shape(), co2.shape()); + let (generator, workspace) = + dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type()); pair.generator = Some(generator); pair.generator_workspace = workspace; } @@ -428,6 +432,7 @@ impl NarrowPhase { prediction_distance, colliders, pair, + solver_flags, }; context @@ -436,69 +441,6 @@ impl NarrowPhase { .unwrap() .generate_contacts(context, events); }); - - /* - // First, group pairs. - // NOTE: the transmutes here are OK because the Vec are all cleared - // before we leave this method. - // We do this in order to avoid reallocating those vecs each time - // we compute the contacts. Unsafe is necessary because we can't just - // store a Vec<&mut ContactPair> into the NarrowPhase struct without - // polluting the World with lifetimes. - let ball_ball: &mut Vec<&mut ContactPair> = - unsafe { std::mem::transmute(&mut self.ball_ball) }; - let shape_shape: &mut Vec<&mut ContactPair> = - unsafe { std::mem::transmute(&mut self.shape_shape) }; - - let bodies = &bodies.bodies; - - // FIXME: don't iterate through all the interactions. - for pair in &mut self.contact_graph.interactions { - let co1 = &colliders[pair.pair.collider1]; - let co2 = &colliders[pair.pair.collider2]; - - // FIXME: avoid lookup into bodies. - let rb1 = &bodies[co1.parent]; - let rb2 = &bodies[co2.parent]; - - if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) - { - // No need to update this contact because nothing moved. - continue; - } - - match (co1.shape(), co2.shape()) { - (Shape::Ball(_), Shape::Ball(_)) => ball_ball.push(pair), - _ => shape_shape.push(pair), - } - } - - par_chunks_mut!(ball_ball, SIMD_WIDTH).for_each(|pairs| { - let context = ContactGenerationContextSimd { - dispatcher: &DefaultContactDispatcher, - prediction_distance, - colliders, - pairs, - }; - context.pairs[0] - .generator - .generate_contacts_simd(context, events); - }); - - par_iter_mut!(shape_shape).for_each(|pair| { - let context = ContactGenerationContext { - dispatcher: &DefaultContactDispatcher, - prediction_distance, - colliders, - pair, - }; - - context.pair.generator.generate_contacts(context, events); - }); - - ball_ball.clear(); - shape_shape.clear(); - */ } /// Retrieve all the interactions with at least one contact point, happening between two active bodies. @@ -518,7 +460,11 @@ impl NarrowPhase { for manifold in &mut inter.weight.manifolds { let rb1 = &bodies[manifold.body_pair.body1]; let rb2 = &bodies[manifold.body_pair.body2]; - if manifold.num_active_contacts() != 0 + if manifold + .solver_flags + .contains(SolverFlags::COMPUTE_IMPULSES) + && manifold.num_active_contacts() != 0 + && (rb1.is_dynamic() || rb2.is_dynamic()) && (!rb1.is_dynamic() || !rb1.is_sleeping()) && (!rb2.is_dynamic() || !rb2.is_sleeping()) { diff --git a/src/geometry/polygon.rs b/src/geometry/polygon.rs index cdb1012..d07cfd9 100644 --- a/src/geometry/polygon.rs +++ b/src/geometry/polygon.rs @@ -1,3 +1,5 @@ +#![allow(dead_code)] // TODO: remove this once we support polygons. + use crate::math::{Isometry, Point, Vector}; use ncollide::bounding_volume::AABB; diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs new file mode 100644 index 0000000..2a8fc8d --- /dev/null +++ b/src/geometry/polygonal_feature_map.rs @@ -0,0 +1,132 @@ +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 index dfeee29..e88674e 100644 --- a/src/geometry/polyhedron_feature3d.rs +++ b/src/geometry/polyhedron_feature3d.rs @@ -1,4 +1,5 @@ -use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle}; +use crate::approx::AbsDiffEq; +use crate::geometry::{self, Contact, ContactManifold, CuboidFeatureFace, Triangle}; use crate::math::{Isometry, Point, Vector}; use crate::utils::WBasis; use na::Point2; @@ -39,6 +40,8 @@ impl From for PolyhedronFace { impl From> for PolyhedronFace { fn from(seg: Segment) -> Self { + // Vertices have feature ids 0 and 2. + // The segment interior has feature id 1. Self { vertices: [seg.a, seg.b, seg.b, seg.b], vids: [0, 2, 2, 2], @@ -50,9 +53,19 @@ impl From> for PolyhedronFace { } impl PolyhedronFace { + pub fn new() -> Self { + Self { + vertices: [Point::origin(); 4], + vids: [0; 4], + eids: [0; 4], + fid: 0, + num_vertices: 0, + } + } + pub fn transform_by(&mut self, iso: &Isometry) { - for v in &mut self.vertices[0..self.num_vertices] { - *v = iso * *v; + for p in &mut self.vertices[0..self.num_vertices] { + *p = iso * *p; } } @@ -63,6 +76,140 @@ impl PolyhedronFace { face2: &PolyhedronFace, pos21: &Isometry, manifold: &mut ContactManifold, + ) { + match (face1.num_vertices, face2.num_vertices) { + (2, 2) => Self::contacts_edge_edge( + prediction_distance, + face1, + sep_axis1, + face2, + pos21, + manifold, + ), + _ => Self::contacts_face_face( + prediction_distance, + face1, + sep_axis1, + face2, + pos21, + manifold, + ), + } + } + + fn contacts_edge_edge( + prediction_distance: f32, + face1: &PolyhedronFace, + sep_axis1: &Vector, + face2: &PolyhedronFace, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + // Project the faces to a 2D plane for contact clipping. + // The plane they are projected onto has normal sep_axis1 + // and contains the origin (this is numerically OK because + // we are not working in world-space here). + let basis = sep_axis1.orthonormal_basis(); + let projected_edge1 = [ + Point2::new( + face1.vertices[0].coords.dot(&basis[0]), + face1.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[1].coords.dot(&basis[0]), + face1.vertices[1].coords.dot(&basis[1]), + ), + ]; + let projected_edge2 = [ + Point2::new( + face2.vertices[0].coords.dot(&basis[0]), + face2.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[1].coords.dot(&basis[0]), + face2.vertices[1].coords.dot(&basis[1]), + ), + ]; + + let tangent1 = + (projected_edge1[1] - projected_edge1[0]).try_normalize(f32::default_epsilon()); + let tangent2 = + (projected_edge2[1] - projected_edge2[0]).try_normalize(f32::default_epsilon()); + + // TODO: not sure what the best value for eps is. + // Empirically, it appears that an epsilon smaller than 1.0e-3 is too small. + if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) { + let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8; + + if !parallel { + let seg1 = (&projected_edge1[0], &projected_edge1[1]); + let seg2 = (&projected_edge2[0], &projected_edge2[1]); + let (loc1, loc2) = + ncollide::query::closest_points_segment_segment_with_locations_nD(seg1, seg2); + + // Found a contact between the two edges. + let bcoords1 = loc1.barycentric_coordinates(); + let bcoords2 = loc2.barycentric_coordinates(); + + let edge1 = (face1.vertices[0], face1.vertices[1]); + let edge2 = (face2.vertices[0], face2.vertices[1]); + let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1]; + let local_p2 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1]; + let dist = (local_p2 - local_p1).dot(&sep_axis1); + + if dist <= prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.eids[0], + fid2: face2.eids[0], + dist, + }); + } + + return; + } + } + + // The lines are parallel so we are having a conformal contact. + // Let's use a range-based clipping to extract two contact points. + // TODO: would it be better and/or more efficient to do the + //clipping in 2D? + if let Some(clips) = geometry::clip_segments( + (face1.vertices[0], face1.vertices[1]), + (face2.vertices[0], face2.vertices[1]), + ) { + manifold.points.push(Contact { + local_p1: (clips.0).0, + local_p2: pos21 * (clips.0).1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: 0, // FIXME + fid2: 0, // FIXME + dist: ((clips.0).1 - (clips.0).0).dot(&sep_axis1), + }); + + manifold.points.push(Contact { + local_p1: (clips.1).0, + local_p2: pos21 * (clips.1).1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: 0, // FIXME + fid2: 0, // FIXME + dist: ((clips.1).1 - (clips.1).0).dot(&sep_axis1), + }); + } + } + + fn contacts_face_face( + prediction_distance: f32, + face1: &PolyhedronFace, + sep_axis1: &Vector, + face2: &PolyhedronFace, + pos21: &Isometry, + manifold: &mut ContactManifold, ) { // Project the faces to a 2D plane for contact clipping. // The plane they are projected onto has normal sep_axis1 @@ -242,8 +389,6 @@ impl PolyhedronFace { /// Compute the barycentric coordinates of the intersection between the two given lines. /// Returns `None` if the lines are parallel. fn closest_points_line2d(edge1: [Point2; 2], edge2: [Point2; 2]) -> Option<(f32, f32)> { - use approx::AbsDiffEq; - // Inspired by Real-time collision detection by Christer Ericson. let dir1 = edge1[1] - edge1[0]; let dir2 = edge2[1] - edge2[0]; diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs index b00337d..eda6547 100644 --- a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs @@ -1,27 +1,19 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{Ball, Proximity, Shape}; +use crate::geometry::{Ball, Proximity}; use crate::math::Isometry; use ncollide::query::PointQuery; pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - if let Shape::Ball(ball1) = ctxt.shape1 { - match ctxt.shape2 { - Shape::Triangle(tri2) => do_detect_proximity(tri2, ball1, &ctxt), - Shape::Cuboid(cube2) => do_detect_proximity(cube2, ball1, &ctxt), - _ => unimplemented!(), - } - } else if let Shape::Ball(ball2) = ctxt.shape2 { - match ctxt.shape1 { - Shape::Triangle(tri1) => do_detect_proximity(tri1, ball2, &ctxt), - Shape::Cuboid(cube1) => do_detect_proximity(cube1, ball2, &ctxt), - _ => unimplemented!(), - } + if let Some(ball1) = ctxt.shape1.as_ball() { + do_detect_proximity(ctxt.shape2, ball1, &ctxt) + } else if let Some(ball2) = ctxt.shape2.as_ball() { + do_detect_proximity(ctxt.shape1, ball2, &ctxt) } else { panic!("Invalid shape types provide.") } } -fn do_detect_proximity>( +fn do_detect_proximity>( point_query1: &P, ball2: &Ball, ctxt: &PrimitiveProximityDetectionContext, diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs index b68ebf9..ae885b3 100644 --- a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs @@ -1,10 +1,10 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Proximity, Shape}; +use crate::geometry::{sat, Proximity}; use crate::math::Isometry; use ncollide::shape::Cuboid; pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { detect_proximity( ctxt.prediction_distance, cube1, diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs index 12f3b4a..532ab36 100644 --- a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs @@ -1,11 +1,11 @@ use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Cuboid, Proximity, Shape, Triangle}; +use crate::geometry::{sat, Cuboid, Proximity, Triangle}; use crate::math::Isometry; pub fn detect_proximity_cuboid_triangle( ctxt: &mut PrimitiveProximityDetectionContext, ) -> Proximity { - if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { detect_proximity( ctxt.prediction_distance, cube1, @@ -13,7 +13,9 @@ pub fn detect_proximity_cuboid_triangle( triangle2, ctxt.position2, ) - } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(triangle1), Some(cube2)) = + (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) + { detect_proximity( ctxt.prediction_distance, cube2, diff --git a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs index f0e049f..12a8e45 100644 --- a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs +++ b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs @@ -1,21 +1,24 @@ +#![allow(dead_code)] + use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; -use crate::geometry::{sat, Polygon, Proximity, Shape}; +use crate::geometry::{sat, Polygon, Proximity}; use crate::math::Isometry; pub fn detect_proximity_polygon_polygon( - ctxt: &mut PrimitiveProximityDetectionContext, + _ctxt: &mut PrimitiveProximityDetectionContext, ) -> Proximity { - if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { - detect_proximity( - ctxt.prediction_distance, - polygon1, - &ctxt.position1, - polygon2, - &ctxt.position2, - ) - } else { - unreachable!() - } + unimplemented!() + // if let (Some(polygon1), Some(polygon2)) = (ctxt.shape1.as_polygon(), ctxt.shape2.as_polygon()) { + // detect_proximity( + // ctxt.prediction_distance, + // polygon1, + // &ctxt.position1, + // polygon2, + // &ctxt.position2, + // ) + // } else { + // unreachable!() + // } } fn detect_proximity<'a>( diff --git a/src/geometry/proximity_detector/proximity_detector.rs b/src/geometry/proximity_detector/proximity_detector.rs index 76e8cd7..7c8ad20 100644 --- a/src/geometry/proximity_detector/proximity_detector.rs +++ b/src/geometry/proximity_detector/proximity_detector.rs @@ -120,8 +120,8 @@ pub struct PrimitiveProximityDetectionContext<'a> { pub prediction_distance: f32, pub collider1: &'a Collider, pub collider2: &'a Collider, - pub shape1: &'a Shape, - pub shape2: &'a Shape, + pub shape1: &'a dyn Shape, + pub shape2: &'a dyn Shape, pub position1: &'a Isometry, pub position2: &'a Isometry, pub workspace: Option<&'a mut (dyn Any + Send + Sync)>, @@ -132,8 +132,8 @@ pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> { pub prediction_distance: f32, pub colliders1: [&'a Collider; SIMD_WIDTH], pub colliders2: [&'a Collider; SIMD_WIDTH], - pub shapes1: [&'a Shape; SIMD_WIDTH], - pub shapes2: [&'a Shape; SIMD_WIDTH], + pub shapes1: [&'a dyn Shape; SIMD_WIDTH], + pub shapes2: [&'a dyn Shape; SIMD_WIDTH], pub positions1: &'a Isometry, pub positions2: &'a Isometry, pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>], diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs index 6d6b4c5..768aca6 100644 --- a/src/geometry/proximity_detector/proximity_dispatcher.rs +++ b/src/geometry/proximity_detector/proximity_dispatcher.rs @@ -2,7 +2,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetector, ProximityDetector, ProximityPhase, TrimeshShapeProximityDetectorWorkspace, }; -use crate::geometry::Shape; +use crate::geometry::ShapeType; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm @@ -11,8 +11,8 @@ pub trait ProximityDispatcher { /// Select the proximity detection algorithm for the given pair of primitive shapes. fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveProximityDetector, Option>, @@ -20,8 +20,8 @@ pub trait ProximityDispatcher { /// Select the proximity detection algorithm for the given pair of non-primitive shapes. fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ProximityPhase, Option>); } @@ -31,14 +31,14 @@ pub struct DefaultProximityDispatcher; impl ProximityDispatcher for DefaultProximityDispatcher { fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveProximityDetector, Option>, ) { match (shape1, shape2) { - (Shape::Ball(_), Shape::Ball(_)) => ( + (ShapeType::Ball, ShapeType::Ball) => ( PrimitiveProximityDetector { #[cfg(feature = "simd-is-enabled")] detect_proximity_simd: super::detect_proximity_ball_ball_simd, @@ -47,56 +47,56 @@ impl ProximityDispatcher for DefaultProximityDispatcher { }, None, ), - (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + (ShapeType::Cuboid, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_cuboid, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Polygon(_), Shape::Polygon(_)) => ( + (ShapeType::Polygon, ShapeType::Polygon) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_polygon_polygon, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Triangle(_), Shape::Ball(_)) => ( + (ShapeType::Triangle, ShapeType::Ball) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Ball(_), Shape::Triangle(_)) => ( + (ShapeType::Ball, ShapeType::Triangle) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Cuboid(_), Shape::Ball(_)) => ( + (ShapeType::Cuboid, ShapeType::Ball) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Ball(_), Shape::Cuboid(_)) => ( + (ShapeType::Ball, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Triangle(_), Shape::Cuboid(_)) => ( + (ShapeType::Triangle, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_triangle, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Cuboid(_), Shape::Triangle(_)) => ( + (ShapeType::Cuboid, ShapeType::Triangle) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_triangle, ..PrimitiveProximityDetector::default() @@ -109,18 +109,18 @@ impl ProximityDispatcher for DefaultProximityDispatcher { fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ProximityPhase, Option>) { match (shape1, shape2) { - (Shape::Trimesh(_), _) => ( + (ShapeType::Trimesh, _) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() }), Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), ), - (_, Shape::Trimesh(_)) => ( + (_, ShapeType::Trimesh) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index edf3085..b469637 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -1,7 +1,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; -use crate::geometry::{Collider, Proximity, Shape, Trimesh}; +use crate::geometry::{Collider, Proximity, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeProximityDetectorWorkspace { @@ -24,9 +24,9 @@ pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> P let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::Trimesh(trimesh1) = collider1.shape() { + if let Some(trimesh1) = collider1.shape().as_trimesh() { do_detect_proximity(trimesh1, collider1, collider2, ctxt) - } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + } else if let Some(trimesh2) = collider2.shape().as_trimesh() { do_detect_proximity(trimesh2, collider2, collider1, ctxt) } else { panic!("Invalid shape types provided.") @@ -83,6 +83,7 @@ fn do_detect_proximity( let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut best_proximity = Proximity::Disjoint; + let shape_type2 = collider2.shape().shape_type(); for triangle_id in new_interferences.iter() { if *triangle_id >= trimesh1.num_triangles() { @@ -107,10 +108,10 @@ fn do_detect_proximity( }; } - let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let triangle1 = trimesh1.triangle(*triangle_id); let (proximity_detector, mut workspace2) = ctxt .dispatcher - .dispatch_primitives(&triangle1, collider2.shape()); + .dispatch_primitives(ShapeType::Triangle, shape_type2); let mut ctxt2 = PrimitiveProximityDetectionContext { prediction_distance: ctxt.prediction_distance, diff --git a/src/geometry/round_cylinder.rs b/src/geometry/round_cylinder.rs new file mode 100644 index 0000000..ce8b43b --- /dev/null +++ b/src/geometry/round_cylinder.rs @@ -0,0 +1,107 @@ +use crate::geometry::Cylinder; +use crate::math::{Isometry, Point, Vector}; +use na::Unit; +use ncollide::query::{ + algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection, +}; +use ncollide::shape::{FeatureId, SupportMap}; + +/// A rounded cylinder. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug)] +pub struct RoundCylinder { + /// The cylinder being rounded. + pub cylinder: Cylinder, + /// The rounding radius. + pub border_radius: f32, +} + +impl RoundCylinder { + /// Create sa new cylinder where all its edges and vertices are rounded by a radius of `radius`. + /// + /// This is done by applying a dilation of the given radius to the cylinder. + pub fn new(half_height: f32, radius: f32, border_radius: f32) -> Self { + Self { + cylinder: Cylinder::new(half_height, radius), + border_radius, + } + } +} + +impl SupportMap for RoundCylinder { + fn local_support_point(&self, dir: &Vector) -> Point { + self.local_support_point_toward(&Unit::new_normalize(*dir)) + } + + fn local_support_point_toward(&self, dir: &Unit>) -> Point { + self.cylinder.local_support_point_toward(dir) + **dir * self.border_radius + } + + fn support_point(&self, transform: &Isometry, dir: &Vector) -> Point { + let local_dir = transform.inverse_transform_vector(dir); + transform * self.local_support_point(&local_dir) + } + + fn support_point_toward( + &self, + transform: &Isometry, + dir: &Unit>, + ) -> Point { + let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir)); + transform * self.local_support_point_toward(&local_dir) + } +} + +impl RayCast for RoundCylinder { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option> { + let ls_ray = ray.inverse_transform_by(m); + + ncollide::query::ray_intersection_with_support_map_with_params( + &Isometry::identity(), + self, + &mut VoronoiSimplex::new(), + &ls_ray, + max_toi, + solid, + ) + .map(|mut res| { + res.normal = m * res.normal; + res + }) + } +} + +// TODO: if PointQuery had a `project_point_with_normal` method, we could just +// call this and adjust the projected point accordingly. +impl PointQuery for RoundCylinder { + #[inline] + fn project_point( + &self, + m: &Isometry, + point: &Point, + solid: bool, + ) -> PointProjection { + ncollide::query::point_projection_on_support_map( + m, + self, + &mut VoronoiSimplex::new(), + point, + solid, + ) + } + + #[inline] + fn project_point_with_feature( + &self, + m: &Isometry, + point: &Point, + ) -> (PointProjection, FeatureId) { + (self.project_point(m, point, false), FeatureId::Unknown) + } +} diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 0666c04..3cd66b4 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -1,9 +1,22 @@ -use crate::geometry::{cuboid, Cuboid, Polygon, Triangle}; +use crate::geometry::{cuboid, Cuboid, Polygon, Segment, Triangle}; use crate::math::{Isometry, Point, Vector, DIM}; use crate::utils::WSign; use na::Unit; -use ncollide::shape::{Segment, SupportMap}; +use ncollide::shape::SupportMap; +#[allow(dead_code)] +pub fn support_map_support_map_compute_separation( + sm1: &impl SupportMap, + sm2: &impl SupportMap, + m12: &Isometry, + dir1: &Unit>, +) -> f32 { + let p1 = sm1.local_support_point_toward(dir1); + let p2 = sm2.support_point_toward(m12, &-*dir1); + (p2 - p1).dot(dir1) +} + +#[allow(dead_code)] pub fn polygon_polygon_compute_separation_features( p1: &Polygon, p2: &Polygon, @@ -58,8 +71,8 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( let y2 = pos12 * Vector::y(); let z2 = pos12 * Vector::z(); - // We have 3 * 3 = 9 axii to test. - let axii = [ + // We have 3 * 3 = 9 axes to test. + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), @@ -74,7 +87,7 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( Vector::new(-z2.y, z2.x, 0.0), ]; - for axis1 in &axii { + for axis1 in &axes { let norm1 = axis1.norm(); if norm1 > f32::default_epsilon() { let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line( @@ -127,7 +140,6 @@ pub fn cuboid_cuboid_find_local_separating_normal_oneway( * * */ - #[cfg(feature = "dim3")] pub fn cube_support_map_compute_separation_wrt_local_line>( cube1: &Cuboid, @@ -149,7 +161,7 @@ pub fn cube_support_map_compute_separation_wrt_local_line>( pub fn cube_support_map_find_local_separating_edge_twoway( cube1: &Cuboid, shape2: &impl SupportMap, - axii: &[Vector], + axes: &[Vector], pos12: &Isometry, pos21: &Isometry, ) -> (f32, Vector) { @@ -157,7 +169,7 @@ pub fn cube_support_map_find_local_separating_edge_twoway( let mut best_separation = -std::f32::MAX; let mut best_dir = Vector::zeros(); - for axis1 in axii { + for axis1 in axes { if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) { let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line( cube1, shape2, pos12, pos21, &axis1, @@ -184,8 +196,8 @@ pub fn cube_triangle_find_local_separating_edge_twoway( let y2 = pos12 * (triangle2.c - triangle2.b); let z2 = pos12 * (triangle2.a - triangle2.c); - // We have 3 * 3 = 3 axii to test. - let axii = [ + // We have 3 * 3 = 3 axes to test. + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), @@ -200,26 +212,26 @@ pub fn cube_triangle_find_local_separating_edge_twoway( Vector::new(-z2.y, z2.x, 0.0), ]; - cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axii, pos12, pos21) + cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axes, pos12, pos21) } #[cfg(feature = "dim3")] pub fn cube_segment_find_local_separating_edge_twoway( cube1: &Cuboid, - segment2: &Segment, + segment2: &Segment, pos12: &Isometry, pos21: &Isometry, ) -> (f32, Vector) { let x2 = pos12 * (segment2.b - segment2.a); - let axii = [ + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), Vector::new(-x2.y, x2.x, 0.0), ]; - cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axii, pos12, pos21) + cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axes, pos12, pos21) } pub fn cube_support_map_find_local_separating_normal_oneway>( @@ -286,9 +298,72 @@ pub fn triangle_cuboid_find_local_separating_normal_oneway( #[cfg(feature = "dim2")] pub fn segment_cuboid_find_local_separating_normal_oneway( - segment1: &Segment, + segment1: &Segment, shape2: &Cuboid, pos12: &Isometry, ) -> (f32, Vector) { 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 new file mode 100644 index 0000000..ec43bf7 --- /dev/null +++ b/src/geometry/shape.rs @@ -0,0 +1,390 @@ +use crate::dynamics::MassProperties; +use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh}; +use crate::math::Isometry; +use downcast_rs::{impl_downcast, DowncastSync}; +#[cfg(feature = "serde-serialize")] +use erased_serde::Serialize; +use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use ncollide::query::{PointQuery, RayCast}; +use num::Zero; +use num_derive::FromPrimitive; +#[cfg(feature = "dim3")] +use { + crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, RoundCylinder}, + ncollide::bounding_volume::BoundingVolume, +}; + +#[derive(Copy, Clone, Debug, FromPrimitive)] +/// Enum representing the type of a shape. +pub enum ShapeType { + /// A ball shape. + Ball = 1, + /// A convex polygon shape. + Polygon, + /// A cuboid shape. + Cuboid, + /// A capsule shape. + Capsule, + /// A segment shape. + Segment, + /// A triangle shape. + Triangle, + /// A triangle mesh shape. + Trimesh, + /// A heightfield shape. + HeightField, + #[cfg(feature = "dim3")] + /// A cylindrical shape. + Cylinder, + #[cfg(feature = "dim3")] + /// A cylindrical shape. + Cone, + // /// A custom shape type. + // Custom(u8), + // /// A cuboid with rounded corners. + // RoundedCuboid, + // /// A triangle with rounded corners. + // RoundedTriangle, + // /// A triangle-mesh with rounded corners. + // RoundedTrimesh, + // /// An heightfield with rounded corners. + // RoundedHeightField, + /// A cylinder with rounded corners. + #[cfg(feature = "dim3")] + RoundCylinder, + // /// A cone with rounded corners. + // RoundedCone, +} + +/// Trait implemented by shapes usable by Rapier. +pub trait Shape: RayCast + PointQuery + DowncastSync { + /// Convert this shape as a serializable entity. + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + None + } + + /// Computes the AABB of this shape. + fn compute_aabb(&self, position: &Isometry) -> AABB; + + /// Compute the mass-properties of this shape given its uniform density. + fn mass_properties(&self, density: f32) -> MassProperties; + + /// Gets the type tag of this shape. + fn shape_type(&self) -> ShapeType; + + /// Converts this shape to a polygonal feature-map, if it is one. + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + None + } + + // fn as_rounded(&self) -> Option<&Rounded>> { + // None + // } +} + +impl_downcast!(sync Shape); + +impl dyn Shape { + /// Converts this abstract shape to a ball, if it is one. + pub fn as_ball(&self) -> Option<&Ball> { + self.downcast_ref() + } + + /// Converts this abstract shape to a cuboid, if it is one. + pub fn as_cuboid(&self) -> Option<&Cuboid> { + self.downcast_ref() + } + + /// Converts this abstract shape to a capsule, if it is one. + pub fn as_capsule(&self) -> Option<&Capsule> { + self.downcast_ref() + } + + /// Converts this abstract shape to a triangle, if it is one. + pub fn as_triangle(&self) -> Option<&Triangle> { + self.downcast_ref() + } + + /// Converts this abstract shape to a triangle mesh, if it is one. + pub fn as_trimesh(&self) -> Option<&Trimesh> { + self.downcast_ref() + } + + /// Converts this abstract shape to a heightfield, if it is one. + pub fn as_heightfield(&self) -> Option<&HeightField> { + self.downcast_ref() + } + + /// Converts this abstract shape to a cylinder, if it is one. + #[cfg(feature = "dim3")] + pub fn as_cylinder(&self) -> Option<&Cylinder> { + self.downcast_ref() + } + + /// Converts this abstract shape to a cone, if it is one. + #[cfg(feature = "dim3")] + pub fn as_cone(&self) -> Option<&Cone> { + self.downcast_ref() + } + + /// Converts this abstract shape to a cone, if it is one. + #[cfg(feature = "dim3")] + pub fn as_round_cylinder(&self) -> Option<&RoundCylinder> { + self.downcast_ref() + } +} + +impl Shape for Ball { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_ball(density, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Ball + } +} + +// impl Shape for Polygon { +// #[cfg(feature = "serde-serialize")] +// fn as_serialize(&self) -> Option<&dyn Serialize> { +// Some(self as &dyn Serialize) +// } +// +// fn compute_aabb(&self, position: &Isometry) -> AABB { +// self.aabb(position) +// } +// +// fn mass_properties(&self, _density: f32) -> MassProperties { +// unimplemented!() +// } +// +// fn shape_type(&self) -> ShapeType { +// ShapeType::Polygon +// } +// } + +impl Shape for Cuboid { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cuboid(density, self.half_extents) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cuboid + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) + } +} + +impl Shape for Capsule { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_capsule(density, self.segment.a, self.segment.b, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Capsule + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((&self.segment as &dyn PolygonalFeatureMap, self.radius)) + } +} + +impl Shape for Triangle { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Triangle + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) + } +} + +impl Shape for Segment { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Segment + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) + } +} + +impl Shape for Trimesh { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Trimesh + } +} + +impl Shape for HeightField { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::HeightField + } +} + +#[cfg(feature = "dim3")] +impl Shape for Cylinder { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cylinder(density, self.half_height, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cylinder + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) + } +} + +#[cfg(feature = "dim3")] +impl Shape for Cone { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cone(density, self.half_height, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cone + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some((self as &dyn PolygonalFeatureMap, 0.0)) + } +} + +#[cfg(feature = "dim3")] +impl Shape for RoundCylinder { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.cylinder + .compute_aabb(position) + .loosened(self.border_radius) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + // We ignore the margin here. + self.cylinder.mass_properties(density) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::RoundCylinder + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { + Some(( + &self.cylinder as &dyn PolygonalFeatureMap, + self.border_radius, + )) + } +} diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index b6e23e7..b4e8518 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -1,13 +1,9 @@ -use crate::geometry::{Triangle, WQuadtree}; +use crate::geometry::{PointProjection, Ray, RayIntersection, Triangle, WQuadtree}; use crate::math::{Isometry, Point}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; - -#[cfg(feature = "dim3")] -use { - crate::geometry::{Ray, RayIntersection}, - ncollide::query::RayCast, -}; +use ncollide::query::{PointQuery, RayCast}; +use ncollide::shape::FeatureId; #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -110,6 +106,41 @@ impl Trimesh { } } +impl PointQuery for Trimesh { + fn project_point(&self, _m: &Isometry, _pt: &Point, _solid: bool) -> PointProjection { + // TODO + unimplemented!() + } + + fn project_point_with_feature( + &self, + _m: &Isometry, + _pt: &Point, + ) -> (PointProjection, FeatureId) { + // TODO + unimplemented!() + } +} + +#[cfg(feature = "dim2")] +impl RayCast for Trimesh { + fn toi_and_normal_with_ray( + &self, + _m: &Isometry, + _ray: &Ray, + _max_toi: f32, + _solid: bool, + ) -> Option { + // TODO + None + } + + fn intersects_ray(&self, _m: &Isometry, _ray: &Ray, _max_toi: f32) -> bool { + // TODO + false + } +} + #[cfg(feature = "dim3")] impl RayCast for Trimesh { fn toi_and_normal_with_ray( diff --git a/src/geometry/user_callbacks.rs b/src/geometry/user_callbacks.rs new file mode 100644 index 0000000..ae0119f --- /dev/null +++ b/src/geometry/user_callbacks.rs @@ -0,0 +1,57 @@ +use crate::dynamics::RigidBody; +use crate::geometry::{Collider, SolverFlags}; + +/// Context given to custom collision filters to filter-out collisions. +pub struct PairFilterContext<'a> { + /// The first collider involved in the potential collision. + pub rigid_body1: &'a RigidBody, + /// The first collider involved in the potential collision. + pub rigid_body2: &'a RigidBody, + /// The first collider involved in the potential collision. + pub collider1: &'a Collider, + /// The first collider involved in the potential collision. + pub collider2: &'a Collider, +} + +/// User-defined filter for potential contact pairs detected by the broad-phase. +/// +/// This can be used to apply custom logic in order to decide whether two colliders +/// should have their contact computed by the narrow-phase, and if these contact +/// should be solved by the constraints solver +pub trait ContactPairFilter: Send + Sync { + /// Applies the contact pair filter. + /// + /// Note that using a contact pair filter will replace the default contact filtering + /// which consists of preventing contact computation between two non-dynamic bodies. + /// + /// This filtering method is called after taking into account the colliders collision groups. + /// + /// If this returns `None`, then the narrow-phase will ignore this contact pair and + /// not compute any contact manifolds for it. + /// If this returns `Some`, then the narrow-phase will compute contact manifolds for + /// this pair of colliders, and configure them with the returned solver flags. For + /// example, if this returns `Some(SolverFlags::COMPUTE_IMPULSES)` then the contacts + /// will be taken into account by the constraints solver. If this returns + /// `Some(SolverFlags::empty())` then the constraints solver will ignore these + /// contacts. + fn filter_contact_pair(&self, context: &PairFilterContext) -> Option; +} + +/// User-defined filter for potential proximity pairs detected by the broad-phase. +/// +/// This can be used to apply custom logic in order to decide whether two colliders +/// should have their proximity computed by the narrow-phase. +pub trait ProximityPairFilter: Send + Sync { + /// Applies the proximity pair filter. + /// + /// Note that using a proximity pair filter will replace the default proximity filtering + /// which consists of preventing proximity computation between two non-dynamic bodies. + /// + /// This filtering method is called after taking into account the colliders collision groups. + /// + /// If this returns `false`, then the narrow-phase will ignore this pair and + /// not compute any proximity information for it. + /// If this return `true` then the narrow-phase will compute proximity + /// information for this pair. + fn filter_proximity_pair(&self, context: &PairFilterContext) -> bool; +} diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs index fce04eb..deab4a2 100644 --- a/src/geometry/wquadtree.rs +++ b/src/geometry/wquadtree.rs @@ -539,7 +539,7 @@ fn split_indices_wrt_dim<'a>( dim: usize, ) -> (&'a mut [usize], &'a mut [usize]) { let mut icurr = 0; - let mut ilast = indices.len() - 1; + let mut ilast = indices.len(); // The loop condition we can just do 0..indices.len() // instead of the test icurr < ilast because we know @@ -549,12 +549,39 @@ fn split_indices_wrt_dim<'a>( let center = aabbs[i].center(); if center[dim] > split_point[dim] { - indices.swap(icurr, ilast); ilast -= 1; + indices.swap(icurr, ilast); } else { icurr += 1; } } - indices.split_at_mut(icurr) + if icurr == 0 || icurr == indices.len() { + // We don't want to return one empty set. But + // this can happen if all the coordinates along the + // given dimension are equal. + // In this is the case, we just split in the middle. + let half = indices.len() / 2; + indices.split_at_mut(half) + } else { + indices.split_at_mut(icurr) + } +} + +#[cfg(test)] +mod test { + use crate::geometry::{WQuadtree, AABB}; + use crate::math::{Point, Vector}; + + #[test] + fn multiple_identical_AABB_stack_overflow() { + // A stack overflow was caused during the construction of the + // WAABB tree with more than four AABB with the same center. + let aabb = AABB::new(Point::origin(), Vector::repeat(1.0).into()); + + for k in 0..20 { + let mut tree = WQuadtree::new(); + tree.clear_and_rebuild((0..k).map(|i| (i, aabb)), 0.0); + } + } } diff --git a/src/lib.rs b/src/lib.rs index 3674717..deb9313 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -10,6 +10,7 @@ #![deny(missing_docs)] +pub extern crate crossbeam; pub extern crate nalgebra as na; #[cfg(feature = "dim2")] pub extern crate ncollide2d as ncollide; diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 5a19e52..b8896e8 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,7 +1,10 @@ //! Physics pipeline structures. use crate::dynamics::{JointSet, RigidBodySet}; -use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase}; +use crate::geometry::{ + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter, NarrowPhase, + ProximityPairFilter, +}; use crate::pipeline::EventHandler; /// The collision pipeline, responsible for performing collision detection between colliders. @@ -40,6 +43,8 @@ impl CollisionPipeline { narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, + contact_pair_filter: Option<&dyn ContactPairFilter>, + proximity_pair_filter: Option<&dyn ProximityPairFilter>, events: &dyn EventHandler, ) { bodies.maintain_active_set(); @@ -52,8 +57,20 @@ impl CollisionPipeline { narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events); - narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events); - narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events); + narrow_phase.compute_contacts( + prediction_distance, + bodies, + colliders, + contact_pair_filter, + events, + ); + narrow_phase.compute_proximities( + prediction_distance, + bodies, + colliders, + proximity_pair_filter, + events, + ); bodies.update_active_set_with_contacts( colliders, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 47fd260..0720ff1 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -7,7 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ - BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, + ContactPairFilter, NarrowPhase, ProximityPairFilter, }; use crate::math::Vector; use crate::pipeline::EventHandler; @@ -68,6 +69,8 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, joints: &mut JointSet, + contact_pair_filter: Option<&dyn ContactPairFilter>, + proximity_pair_filter: Option<&dyn ProximityPairFilter>, events: &dyn EventHandler, ) { self.counters.step_started(); @@ -112,12 +115,14 @@ impl PhysicsPipeline { integration_parameters.prediction_distance, bodies, colliders, + contact_pair_filter, events, ); narrow_phase.compute_proximities( integration_parameters.prediction_distance, bodies, colliders, + proximity_pair_filter, events, ); // println!("Compute contact time: {}", instant::now() - t); @@ -285,6 +290,8 @@ mod test { &mut bodies, &mut colliders, &mut joints, + None, + None, &(), ); } @@ -327,6 +334,8 @@ mod test { &mut bodies, &mut colliders, &mut joints, + None, + None, &(), ); } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 32f59fc..00d4396 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,5 +1,7 @@ use crate::dynamics::RigidBodySet; -use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree}; +use crate::geometry::{ + Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, WQuadtree, +}; /// A pipeline for performing queries on all the colliders of a scene. #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -59,6 +61,7 @@ impl QueryPipeline { colliders: &'a ColliderSet, ray: &Ray, max_toi: f32, + groups: InteractionGroups, ) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> { // TODO: avoid allocation? let mut inter = Vec::new(); @@ -69,10 +72,17 @@ impl QueryPipeline { for handle in inter { let collider = &colliders[handle]; - if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { - if inter.toi < best { - best = inter.toi; - result = Some((handle, collider, inter)); + if collider.collision_groups.test(groups) { + if let Some(inter) = collider.shape().toi_and_normal_with_ray( + collider.position(), + ray, + max_toi, + true, + ) { + if inter.toi < best { + best = inter.toi; + result = Some((handle, collider, inter)); + } } } } @@ -95,6 +105,7 @@ impl QueryPipeline { colliders: &'a ColliderSet, ray: &Ray, max_toi: f32, + groups: InteractionGroups, mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool, ) { // TODO: avoid allocation? @@ -103,9 +114,17 @@ impl QueryPipeline { for handle in inter { let collider = &colliders[handle]; - if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { - if !callback(handle, collider, inter) { - return; + + if collider.collision_groups.test(groups) { + if let Some(inter) = collider.shape().toi_and_normal_with_ray( + collider.position(), + ray, + max_toi, + true, + ) { + if !callback(handle, collider, inter) { + return; + } } } } diff --git a/src/utils.rs b/src/utils.rs index ecdd4fd..a398a02 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -19,8 +19,10 @@ use { // pub(crate) const COS_10_DEGREES: f32 = 0.98480775301; // pub(crate) const COS_45_DEGREES: f32 = 0.70710678118; // pub(crate) const SIN_45_DEGREES: f32 = COS_45_DEGREES; +#[cfg(feature = "dim3")] +pub(crate) const COS_1_DEGREES: f32 = 0.99984769515; pub(crate) const COS_5_DEGREES: f32 = 0.99619469809; -#[cfg(feature = "dim2")] +// #[cfg(feature = "dim2")] pub(crate) const COS_FRAC_PI_8: f32 = 0.92387953251; #[cfg(feature = "dim2")] pub(crate) const SIN_FRAC_PI_8: f32 = 0.38268343236; @@ -91,7 +93,7 @@ impl> WSign> for Vector3 { impl WSign for SimdFloat { fn copy_sign_to(self, to: SimdFloat) -> SimdFloat { - self.simd_copysign(to) + to.simd_copysign(self) } } diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index c25ff1f..769b12d 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -5,7 +5,7 @@ use rapier::counters::Counters; use rapier::dynamics::{ IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{Collider, ColliderSet, Shape}; +use rapier::geometry::{Collider, ColliderSet}; use std::f32; use wrapped2d::b2; @@ -167,44 +167,42 @@ impl Box2dWorld { fixture_def.is_sensor = collider.is_sensor(); fixture_def.filter = b2::Filter::new(); - match collider.shape() { - Shape::Ball(b) => { - let mut b2_shape = b2::CircleShape::new(); - b2_shape.set_radius(b.radius); - b2_shape.set_position(center); - body.create_fixture(&b2_shape, &mut fixture_def); - } - Shape::Cuboid(c) => { - let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y); - body.create_fixture(&b2_shape, &mut fixture_def); - } - Shape::Polygon(poly) => { - let points: Vec<_> = poly - .vertices() - .iter() - .map(|p| collider.position_wrt_parent() * p) - .map(|p| na_vec_to_b2_vec(p.coords)) - .collect(); - let b2_shape = b2::PolygonShape::new_with(&points); - body.create_fixture(&b2_shape, &mut fixture_def); - } - Shape::HeightField(heightfield) => { - let mut segments = heightfield.segments(); - let seg1 = segments.next().unwrap(); - let mut vertices = vec![ - na_vec_to_b2_vec(seg1.a.coords), - na_vec_to_b2_vec(seg1.b.coords), - ]; + let shape = collider.shape(); - // TODO: this will not handle holes properly. - segments.for_each(|seg| { - vertices.push(na_vec_to_b2_vec(seg.b.coords)); - }); + if let Some(b) = shape.as_ball() { + let mut b2_shape = b2::CircleShape::new(); + b2_shape.set_radius(b.radius); + b2_shape.set_position(center); + body.create_fixture(&b2_shape, &mut fixture_def); + } else if let Some(c) = shape.as_cuboid() { + let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y); + body.create_fixture(&b2_shape, &mut fixture_def); + // } else if let Some(polygon) = shape.as_polygon() { + // let points: Vec<_> = poly + // .vertices() + // .iter() + // .map(|p| collider.position_wrt_parent() * p) + // .map(|p| na_vec_to_b2_vec(p.coords)) + // .collect(); + // let b2_shape = b2::PolygonShape::new_with(&points); + // body.create_fixture(&b2_shape, &mut fixture_def); + } else if let Some(heightfield) = shape.as_heightfield() { + let mut segments = heightfield.segments(); + let seg1 = segments.next().unwrap(); + let mut vertices = vec![ + na_vec_to_b2_vec(seg1.a.coords), + na_vec_to_b2_vec(seg1.b.coords), + ]; - let b2_shape = b2::ChainShape::new_chain(&vertices); - body.create_fixture(&b2_shape, &mut fixture_def); - } - _ => eprintln!("Creating a shape unknown to the Box2d backend."), + // TODO: this will not handle holes properly. + segments.for_each(|seg| { + vertices.push(na_vec_to_b2_vec(seg.b.coords)); + }); + + let b2_shape = b2::ChainShape::new_chain(&vertices); + body.create_fixture(&b2_shape, &mut fixture_def); + } else { + eprintln!("Creating a shape unknown to the Box2d backend."); } } diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index 217da48..1bafdb6 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -9,11 +9,10 @@ use na::Point3; use crate::math::Point; use crate::objects::ball::Ball; use crate::objects::box_node::Box as BoxNode; -use crate::objects::convex::Convex; use crate::objects::heightfield::HeightField; use crate::objects::node::{GraphicsNode, Node}; use rapier::dynamics::{RigidBodyHandle, RigidBodySet}; -use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape}; +use rapier::geometry::{Collider, ColliderHandle, ColliderSet}; //use crate::objects::capsule::Capsule; //use crate::objects::convex::Convex; //#[cfg(feature = "fluids")] @@ -26,6 +25,10 @@ use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape}; //#[cfg(feature = "fluids")] //use crate::objects::FluidRenderingMode; use crate::objects::capsule::Capsule; +#[cfg(feature = "dim3")] +use crate::objects::cone::Cone; +#[cfg(feature = "dim3")] +use crate::objects::cylinder::Cylinder; use crate::objects::mesh::Mesh; use rand::{Rng, SeedableRng}; use rand_pcg::Pcg32; @@ -60,6 +63,7 @@ pub struct GraphicsManager { #[cfg(feature = "fluids")] boundary2sn: HashMap, b2color: HashMap>, + c2color: HashMap>, b2wireframe: HashMap, #[cfg(feature = "fluids")] f2color: HashMap>, @@ -97,6 +101,7 @@ impl GraphicsManager { #[cfg(feature = "fluids")] boundary2sn: HashMap::new(), b2color: HashMap::new(), + c2color: HashMap::new(), #[cfg(feature = "fluids")] f2color: HashMap::new(), ground_color: Point3::new(0.5, 0.5, 0.5), @@ -183,6 +188,10 @@ impl GraphicsManager { } } + pub fn set_collider_initial_color(&mut self, c: ColliderHandle, color: Point3) { + self.c2color.insert(c, color); + } + pub fn set_body_wireframe(&mut self, b: RigidBodyHandle, enabled: bool) { self.b2wireframe.insert(b, enabled); @@ -321,6 +330,7 @@ impl GraphicsManager { let mut new_nodes = Vec::new(); for collider_handle in bodies[handle].colliders() { + let color = self.c2color.get(collider_handle).copied().unwrap_or(color); let collider = &colliders[*collider_handle]; self.add_collider(window, *collider_handle, collider, color, &mut new_nodes); } @@ -349,33 +359,44 @@ impl GraphicsManager { color: Point3, out: &mut Vec, ) { - match collider.shape() { - Shape::Ball(ball) => { - out.push(Node::Ball(Ball::new(handle, ball.radius, color, window))) - } - Shape::Polygon(poly) => out.push(Node::Convex(Convex::new( - handle, - poly.vertices().to_vec(), - color, - window, - ))), - Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new( + let shape = collider.shape(); + + if let Some(ball) = shape.as_ball() { + out.push(Node::Ball(Ball::new(handle, ball.radius, color, window))) + } + + // Shape::Polygon(poly) => out.push(Node::Convex(Convex::new( + // handle, + // poly.vertices().to_vec(), + // color, + // window, + // ))), + + if let Some(cuboid) = shape.as_cuboid() { + out.push(Node::Box(BoxNode::new( handle, cuboid.half_extents, color, window, - ))), - Shape::Capsule(capsule) => { - out.push(Node::Capsule(Capsule::new(handle, capsule, color, window))) - } - Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new( + ))) + } + + if let Some(capsule) = shape.as_capsule() { + out.push(Node::Capsule(Capsule::new(handle, capsule, color, window))) + } + + if let Some(triangle) = shape.as_triangle() { + out.push(Node::Mesh(Mesh::new( handle, vec![triangle.a, triangle.b, triangle.c], vec![Point3::new(0, 1, 2)], color, window, - ))), - Shape::Trimesh(trimesh) => out.push(Node::Mesh(Mesh::new( + ))) + } + + if let Some(trimesh) = shape.as_trimesh() { + out.push(Node::Mesh(Mesh::new( handle, trimesh.vertices().to_vec(), trimesh @@ -385,13 +406,41 @@ impl GraphicsManager { .collect(), color, window, - ))), - Shape::HeightField(heightfield) => out.push(Node::HeightField(HeightField::new( + ))) + } + + if let Some(heightfield) = shape.as_heightfield() { + out.push(Node::HeightField(HeightField::new( handle, heightfield, color, window, - ))), + ))) + } + + #[cfg(feature = "dim3")] + if let Some(cylinder) = shape + .as_cylinder() + .or(shape.as_round_cylinder().map(|r| &r.cylinder)) + { + out.push(Node::Cylinder(Cylinder::new( + handle, + cylinder.half_height, + cylinder.radius, + color, + window, + ))) + } + + #[cfg(feature = "dim3")] + if let Some(cone) = shape.as_cone() { + out.push(Node::Cone(Cone::new( + handle, + cone.half_height, + cone.radius, + color, + window, + ))) } } diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs index c2a7fb1..3a0e487 100644 --- a/src_testbed/nphysics_backend.rs +++ b/src_testbed/nphysics_backend.rs @@ -12,7 +12,7 @@ use rapier::counters::Counters; use rapier::dynamics::{ IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{Collider, ColliderSet, Shape}; +use rapier::geometry::{Collider, ColliderSet}; use rapier::math::Vector; use std::collections::HashMap; #[cfg(feature = "dim3")] @@ -177,28 +177,37 @@ fn nphysics_collider_from_rapier_collider( ) -> Option> { let margin = ColliderDesc::::default_margin(); let mut pos = *collider.position_wrt_parent(); + let shape = collider.shape(); - let shape = match collider.shape() { - Shape::Cuboid(cuboid) => { - ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin))) - } - Shape::Ball(ball) => ShapeHandle::new(Ball::new(ball.radius - margin)), - Shape::Capsule(capsule) => { - pos *= capsule.transform_wrt_y(); - ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius)) - } - Shape::HeightField(heightfield) => ShapeHandle::new(heightfield.clone()), + let shape = if let Some(cuboid) = shape.as_cuboid() { + ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin))) + } else if let Some(ball) = shape.as_ball() { + ShapeHandle::new(Ball::new(ball.radius - margin)) + } else if let Some(capsule) = shape.as_capsule() { + pos *= capsule.transform_wrt_y(); + ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius)) + } else if let Some(heightfield) = shape.as_heightfield() { + ShapeHandle::new(heightfield.clone()) + } else { #[cfg(feature = "dim3")] - Shape::Trimesh(trimesh) => ShapeHandle::new(TriMesh::new( - trimesh.vertices().to_vec(), - trimesh - .indices() - .iter() - .map(|idx| na::convert(*idx)) - .collect(), - None, - )), - _ => return None, + if let Some(trimesh) = shape.as_trimesh() { + ShapeHandle::new(TriMesh::new( + trimesh.vertices().to_vec(), + trimesh + .indices() + .iter() + .map(|idx| na::convert(*idx)) + .collect(), + None, + )) + } else { + return None; + } + + #[cfg(feature = "dim2")] + { + return None; + } }; let density = if is_dynamic { collider.density() } else { 0.0 }; diff --git a/src_testbed/objects/cone.rs b/src_testbed/objects/cone.rs new file mode 100644 index 0000000..58b014f --- /dev/null +++ b/src_testbed/objects/cone.rs @@ -0,0 +1,74 @@ +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window::Window; +use na::Point3; +use rapier::geometry::{ColliderHandle, ColliderSet}; +use rapier::math::Isometry; + +pub struct Cone { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl Cone { + pub fn new( + collider: ColliderHandle, + half_height: f32, + radius: f32, + color: Point3, + window: &mut Window, + ) -> Cone { + #[cfg(feature = "dim2")] + let node = window.add_rectangle(radius, half_height); + #[cfg(feature = "dim3")] + let node = window.add_cone(radius, half_height * 2.0); + + let mut res = Cone { + color, + base_color: color, + gfx: node, + collider, + }; + + // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten"); + res.gfx.set_color(color.x, color.y, color.z); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + self.color = color; + self.base_color = color; + } + + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.collider, + &self.color, + &Isometry::identity(), + ); + } + + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.collider + } +} diff --git a/src_testbed/objects/cylinder.rs b/src_testbed/objects/cylinder.rs new file mode 100644 index 0000000..01a6737 --- /dev/null +++ b/src_testbed/objects/cylinder.rs @@ -0,0 +1,74 @@ +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window::Window; +use na::Point3; +use rapier::geometry::{ColliderHandle, ColliderSet}; +use rapier::math::Isometry; + +pub struct Cylinder { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl Cylinder { + pub fn new( + collider: ColliderHandle, + half_height: f32, + radius: f32, + color: Point3, + window: &mut Window, + ) -> Cylinder { + #[cfg(feature = "dim2")] + let node = window.add_rectangle(radius, half_height); + #[cfg(feature = "dim3")] + let node = window.add_cylinder(radius, half_height * 2.0); + + let mut res = Cylinder { + color, + base_color: color, + gfx: node, + collider, + }; + + // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten"); + res.gfx.set_color(color.x, color.y, color.z); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + self.color = color; + self.base_color = color; + } + + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.collider, + &self.color, + &Isometry::identity(), + ); + } + + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.collider + } +} diff --git a/src_testbed/objects/mod.rs b/src_testbed/objects/mod.rs index 82895b3..e4d7bc4 100644 --- a/src_testbed/objects/mod.rs +++ b/src_testbed/objects/mod.rs @@ -1,7 +1,9 @@ pub mod ball; pub mod box_node; pub mod capsule; +pub mod cone; pub mod convex; +pub mod cylinder; pub mod heightfield; pub mod mesh; pub mod node; diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs index 93b5eac..d1de799 100644 --- a/src_testbed/objects/node.rs +++ b/src_testbed/objects/node.rs @@ -10,6 +10,8 @@ use crate::objects::mesh::Mesh; use kiss3d::window::Window; use na::Point3; +use crate::objects::cone::Cone; +use crate::objects::cylinder::Cylinder; use rapier::geometry::{ColliderHandle, ColliderSet}; use rapier::math::Isometry; @@ -28,6 +30,8 @@ pub enum Node { // Polyline(Polyline), Mesh(Mesh), Convex(Convex), + Cylinder(Cylinder), + Cone(Cone), } impl Node { @@ -42,6 +46,8 @@ impl Node { // Node::Polyline(ref mut n) => n.select(), Node::Mesh(ref mut n) => n.select(), Node::Convex(ref mut n) => n.select(), + Node::Cylinder(ref mut n) => n.select(), + Node::Cone(ref mut n) => n.select(), } } @@ -56,6 +62,8 @@ impl Node { // Node::Polyline(ref mut n) => n.unselect(), Node::Mesh(ref mut n) => n.unselect(), Node::Convex(ref mut n) => n.unselect(), + Node::Cylinder(ref mut n) => n.unselect(), + Node::Cone(ref mut n) => n.unselect(), } } @@ -70,6 +78,8 @@ impl Node { // Node::Polyline(ref mut n) => n.update(colliders), Node::Mesh(ref mut n) => n.update(colliders), Node::Convex(ref mut n) => n.update(colliders), + Node::Cylinder(ref mut n) => n.update(colliders), + Node::Cone(ref mut n) => n.update(colliders), } } @@ -97,6 +107,8 @@ impl Node { Node::HeightField(ref n) => Some(n.scene_node()), Node::Mesh(ref n) => Some(n.scene_node()), Node::Convex(ref n) => Some(n.scene_node()), + Node::Cylinder(ref n) => Some(n.scene_node()), + Node::Cone(ref n) => Some(n.scene_node()), #[cfg(feature = "dim2")] _ => None, } @@ -113,6 +125,8 @@ impl Node { Node::HeightField(ref mut n) => Some(n.scene_node_mut()), Node::Mesh(ref mut n) => Some(n.scene_node_mut()), Node::Convex(ref mut n) => Some(n.scene_node_mut()), + Node::Cylinder(ref mut n) => Some(n.scene_node_mut()), + Node::Cone(ref mut n) => Some(n.scene_node_mut()), #[cfg(feature = "dim2")] _ => None, } @@ -129,6 +143,8 @@ impl Node { // Node::Polyline(ref n) => n.object(), Node::Mesh(ref n) => n.object(), Node::Convex(ref n) => n.object(), + Node::Cylinder(ref n) => n.object(), + Node::Cone(ref n) => n.object(), } } @@ -143,6 +159,8 @@ impl Node { // Node::Polyline(ref mut n) => n.set_color(color), Node::Mesh(ref mut n) => n.set_color(color), Node::Convex(ref mut n) => n.set_color(color), + Node::Cylinder(ref mut n) => n.set_color(color), + Node::Cone(ref mut n) => n.set_color(color), } } } diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index ec2e2bf..74d6af2 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -6,7 +6,7 @@ use rapier::counters::Counters; use rapier::dynamics::{ IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{Collider, ColliderSet, Shape}; +use rapier::geometry::{Collider, ColliderSet}; use rapier::utils::WBasis; use std::collections::HashMap; @@ -422,27 +422,29 @@ fn physx_collider_from_rapier_collider( collider: &Collider, ) -> Option<(ColliderDesc, Isometry3)> { let mut local_pose = *collider.position_wrt_parent(); - let desc = match collider.shape() { - Shape::Cuboid(cuboid) => ColliderDesc::Box( + let shape = collider.shape(); + + let desc = if let Some(cuboid) = shape.as_cuboid() { + ColliderDesc::Box( cuboid.half_extents.x, cuboid.half_extents.y, cuboid.half_extents.z, - ), - Shape::Ball(ball) => ColliderDesc::Sphere(ball.radius), - Shape::Capsule(capsule) => { - let center = capsule.center(); - let mut dir = capsule.b - capsule.a; + ) + } else if let Some(ball) = shape.as_ball() { + ColliderDesc::Sphere(ball.radius) + } else if let Some(capsule) = shape.as_capsule() { + let center = capsule.center(); + let mut dir = capsule.segment.b - capsule.segment.a; - if dir.x < 0.0 { - dir = -dir; - } - - let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir); - local_pose *= - Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity()); - ColliderDesc::Capsule(capsule.radius, capsule.height()) + if dir.x < 0.0 { + dir = -dir; } - Shape::Trimesh(trimesh) => ColliderDesc::TriMesh { + + let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir); + local_pose *= Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity()); + ColliderDesc::Capsule(capsule.radius, capsule.height()) + } else if let Some(trimesh) = shape.as_trimesh() { + ColliderDesc::TriMesh { vertices: trimesh .vertices() .iter() @@ -450,11 +452,10 @@ fn physx_collider_from_rapier_collider( .collect(), indices: trimesh.flat_indices().to_vec(), mesh_scale: Vector3::repeat(1.0).into_glam(), - }, - _ => { - eprintln!("Creating a shape unknown to the PhysX backend."); - return None; } + } else { + eprintln!("Creating a shape unknown to the PhysX backend."); + return None; }; Some((desc, local_pose)) diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 3d7fd7d..3bf720a 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -21,7 +21,12 @@ use na::{self, Point2, Point3, Vector3}; use rapier::dynamics::{ ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet, }; -use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, Ray}; +#[cfg(feature = "dim3")] +use rapier::geometry::Ray; +use rapier::geometry::{ + BroadPhase, ColliderHandle, ColliderSet, ContactEvent, InteractionGroups, NarrowPhase, + ProximityEvent, +}; use rapier::math::Vector; use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline}; #[cfg(feature = "fluids")] @@ -495,6 +500,10 @@ impl Testbed { self.graphics.set_body_color(body, color); } + pub fn set_collider_initial_color(&mut self, collider: ColliderHandle, color: Point3) { + self.graphics.set_collider_initial_color(collider, color); + } + #[cfg(feature = "fluids")] pub fn set_fluid_color(&mut self, fluid: FluidHandle, color: Point3) { self.graphics.set_fluid_color(fluid, color); @@ -672,6 +681,8 @@ impl Testbed { &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, + None, + None, &self.event_handler, ); @@ -1180,10 +1191,12 @@ impl Testbed { .camera() .unproject(&self.cursor_pos, &na::convert(size)); let ray = Ray::new(pos, dir); - let hit = self - .physics - .query_pipeline - .cast_ray(&self.physics.colliders, &ray, f32::MAX); + let hit = self.physics.query_pipeline.cast_ray( + &self.physics.colliders, + &ray, + f32::MAX, + InteractionGroups::all(), + ); if let Some((_, collider, _)) = hit { if self.physics.bodies[collider.parent()].is_dynamic() { @@ -1446,6 +1459,8 @@ impl State for Testbed { &mut physics.bodies, &mut physics.colliders, &mut physics.joints, + None, + None, event_handler, ); }); @@ -1460,6 +1475,8 @@ impl State for Testbed { &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, + None, + None, &self.event_handler, );