From 9bf1321f8f1d2e116f44c2461a53f302c4ef4171 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 8 Dec 2020 17:31:49 +0100 Subject: Outsource the contact manifold, SAT, and some shapes. --- Cargo.toml | 2 + benchmarks3d/heightfield3.rs | 8 +- benchmarks3d/trimesh3.rs | 39 +- build/rapier2d/Cargo.toml | 4 +- build/rapier3d/Cargo.toml | 4 +- build/rapier_testbed2d/Cargo.toml | 1 + build/rapier_testbed3d/Cargo.toml | 1 + examples3d/trimesh3.rs | 42 +- src/dynamics/rigid_body_set.rs | 2 +- src/dynamics/solver/categorization.rs | 8 +- src/dynamics/solver/interaction_groups.rs | 12 +- src/dynamics/solver/parallel_velocity_solver.rs | 4 +- src/dynamics/solver/position_constraint.rs | 12 +- src/dynamics/solver/position_constraint_wide.rs | 12 +- src/dynamics/solver/position_ground_constraint.rs | 16 +- .../solver/position_ground_constraint_wide.rs | 14 +- src/dynamics/solver/velocity_constraint.rs | 30 +- src/dynamics/solver/velocity_constraint_wide.rs | 28 +- src/dynamics/solver/velocity_ground_constraint.rs | 34 +- .../solver/velocity_ground_constraint_wide.rs | 28 +- src/geometry/broad_phase_multi_sap.rs | 12 +- src/geometry/capsule.rs | 192 -------- src/geometry/collider.rs | 6 +- src/geometry/contact.rs | 333 +++---------- .../ball_ball_contact_generator.rs | 6 +- .../ball_convex_contact_generator.rs | 18 +- .../capsule_capsule_contact_generator.rs | 33 +- .../cuboid_capsule_contact_generator.rs | 37 +- .../cuboid_cuboid_contact_generator.rs | 23 +- .../cuboid_triangle_contact_generator.rs | 38 +- .../heightfield_shape_contact_generator.rs | 14 +- src/geometry/contact_generator/mod.rs | 30 -- .../contact_generator/pfm_pfm_contact_generator.rs | 30 +- .../polygon_polygon_contact_generator.rs | 175 +------ .../trimesh_shape_contact_generator.rs | 18 +- src/geometry/cuboid.rs | 234 ---------- src/geometry/cuboid_feature2d.rs | 128 ----- src/geometry/cuboid_feature3d.rs | 516 --------------------- src/geometry/mod.rs | 110 +++-- src/geometry/narrow_phase.rs | 7 +- src/geometry/polygon.rs | 4 +- src/geometry/polygonal_feature_map.rs | 132 ------ src/geometry/polyhedron_feature3d.rs | 445 ------------------ .../ball_convex_proximity_detector.rs | 11 +- .../cuboid_cuboid_proximity_detector.rs | 17 +- .../cuboid_triangle_proximity_detector.rs | 8 +- .../trimesh_shape_proximity_detector.rs | 4 +- src/geometry/round_cylinder.rs | 49 +- src/geometry/sat.rs | 344 +------------- src/geometry/shape.rs | 49 +- src/geometry/triangle.rs | 4 +- src/geometry/trimesh.rs | 63 +-- src/geometry/waabb.rs | 12 +- src/geometry/wquadtree.rs | 2 +- src/lib.rs | 11 +- src/pipeline/query_pipeline.rs | 4 +- src_testbed/engine.rs | 2 +- src_testbed/lib.rs | 4 + src_testbed/nphysics_backend.rs | 7 +- src_testbed/objects/heightfield.rs | 17 +- src_testbed/objects/polyline.rs | 2 +- src_testbed/testbed.rs | 4 +- 62 files changed, 552 insertions(+), 2904 deletions(-) delete mode 100644 src/geometry/capsule.rs delete mode 100644 src/geometry/cuboid.rs delete mode 100644 src/geometry/cuboid_feature2d.rs delete mode 100644 src/geometry/cuboid_feature3d.rs delete mode 100644 src/geometry/polygonal_feature_map.rs delete mode 100644 src/geometry/polyhedron_feature3d.rs diff --git a/Cargo.toml b/Cargo.toml index cfb166a..55e9b43 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,6 +10,8 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark #nphysics2d = { path = "../nphysics/build/nphysics2d" } #nphysics3d = { path = "../nphysics/build/nphysics3d" } #kiss3d = { path = "../kiss3d" } +buckler2d = { path = "../buckler/build/buckler2d" } +buckler3d = { path = "../buckler/build/buckler3d" } [profile.release] #debug = true diff --git a/benchmarks3d/heightfield3.rs b/benchmarks3d/heightfield3.rs index 4f8211c..fc64856 100644 --- a/benchmarks3d/heightfield3.rs +++ b/benchmarks3d/heightfield3.rs @@ -1,4 +1,4 @@ -use na::{DMatrix, Point3, Vector3}; +use na::{ComplexField, DMatrix, Point3, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; use rapier3d::geometry::{ColliderBuilder, ColliderSet}; use rapier_testbed3d::Testbed; @@ -23,7 +23,11 @@ pub fn init_world(testbed: &mut Testbed) { } else { let x = i as f32 * ground_size.x / (nsubdivs as f32); let z = j as f32 * ground_size.z / (nsubdivs as f32); - x.sin() + z.cos() + + // NOTE: make sure we use the sin/cos from simba to ensure + // cross-platform determinism of the example when the + // enhanced_determinism feature is enabled. + ::sin(x) + ::cos(z) } }); diff --git a/benchmarks3d/trimesh3.rs b/benchmarks3d/trimesh3.rs index ce1d1c5..e0701f1 100644 --- a/benchmarks3d/trimesh3.rs +++ b/benchmarks3d/trimesh3.rs @@ -1,6 +1,6 @@ -use na::Point3; +use na::{ComplexField, DMatrix, Point3, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; -use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField}; use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { @@ -14,28 +14,27 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground */ - let ground_size = 200.0f32; - let ground_height = 1.0; + let ground_size = Vector3::new(200.0, 1.0, 200.0); let nsubdivs = 20; - let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs); - let indices = quad - .flat_indices() - .chunks(3) - .map(|is| Point3::new(is[0], is[2], is[1])) - .collect(); - let mut vertices = quad.coords; + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs { + 10.0 + } else { + let x = i as f32 * ground_size.x / (nsubdivs as f32); + let z = j as f32 * ground_size.z / (nsubdivs as f32); - // ncollide generates a quad with `z` as the normal. - // so we switch z and y here and set a random altitude at each point. - for p in vertices.iter_mut() { - p.z = p.y; - p.y = (p.x.sin() + p.z.cos()) * ground_height; - - if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 { - p.y = 10.0; + // NOTE: make sure we use the sin/cos from simba to ensure + // cross-platform determinism of the example when the + // enhanced_determinism feature is enabled. + ::sin(x) + ::cos(z) } - } + }); + + // Here we will build our trimesh from the mesh representation of an + // heightfield. + let heightfield = HeightField::new(heights, ground_size); + let (vertices, indices) = heightfield.to_trimesh(); let rigid_body = RigidBodyBuilder::new_static().build(); let handle = bodies.insert(rigid_body); diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 40e2db4..987fa2f 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -21,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -35,7 +35,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.23" -ncollide2d = "0.26" +buckler2d = "0.1" simba = "0.3" approx = "0.4" rayon = { version = "1", optional = true } diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index b1a1026..d409035 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -21,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -35,7 +35,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.23" -ncollide3d = "0.26" +buckler3d = "0.1" simba = "0.3" approx = "0.4" rayon = { version = "1", optional = true } diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index 0b981e3..72ead41 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -31,6 +31,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" num_cpus = { version = "1", optional = true } wrapped2d = { version = "0.4", optional = true } +buckler2d = "0.1" ncollide2d = "0.26" nphysics2d = { version = "0.18", optional = true } crossbeam = "0.8" diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index 3019021..d4ad764 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -30,6 +30,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" glam = { version = "0.10", optional = true } num_cpus = { version = "1", optional = true } +buckler3d = "0.1" ncollide3d = "0.26" nphysics3d = { version = "0.18", optional = true } physx = { version = "0.8", optional = true } diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index 0cee465..d753988 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -1,6 +1,6 @@ -use na::{ComplexField, Point3}; +use na::{ComplexField, DMatrix, Point3, Vector3}; use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; -use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField}; use rapier_testbed3d::Testbed; pub fn init_world(testbed: &mut Testbed) { @@ -14,31 +14,27 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground */ - let ground_size = 100.0f32; - let ground_height = 1.0; + let ground_size = Vector3::new(100.0, 1.0, 100.0); let nsubdivs = 20; - let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs); - let indices = quad - .flat_indices() - .chunks(3) - .map(|is| Point3::new(is[0], is[2], is[1])) - .collect(); - let mut vertices = quad.coords; + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs { + 10.0 + } else { + let x = i as f32 * ground_size.x / (nsubdivs as f32); + let z = j as f32 * ground_size.z / (nsubdivs as f32); - // ncollide generates a quad with `z` as the normal. - // so we switch z and y here and set a random altitude at each point. - for p in vertices.iter_mut() { - p.z = p.y; - // NOTE: make sure we use the sin/cos from simba to ensure - // cross-platform determinism of the example when the - // enhanced_determinism feature is enabled. - p.y = (::sin(p.x) + ::cos(p.z)) * ground_height; - - if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 { - p.y = 10.0; + // NOTE: make sure we use the sin/cos from simba to ensure + // cross-platform determinism of the example when the + // enhanced_determinism feature is enabled. + ::sin(x) + ::cos(z) } - } + }); + + // Here we will build our trimesh from the mesh representation of an + // heightfield. + let heightfield = HeightField::new(heights, ground_size); + let (vertices, indices) = heightfield.to_trimesh(); let rigid_body = RigidBodyBuilder::new_static().build(); let handle = bodies.insert(rigid_body); diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index b8667bc..c32c402 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -479,7 +479,7 @@ impl RigidBodySet { if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) { for inter in contacts { for manifold in &inter.2.manifolds { - if manifold.num_active_contacts() > 0 { + if manifold.num_active_contacts > 0 { let other = crate::utils::other_handle( (inter.0, inter.1), *collider_handle, diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs index 9ddf791..d0a2d0f 100644 --- a/src/dynamics/solver/categorization.rs +++ b/src/dynamics/solver/categorization.rs @@ -12,8 +12,8 @@ pub(crate) fn categorize_position_contacts( ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; if !rb1.is_dynamic() || !rb2.is_dynamic() { match manifold.kinematics.category { @@ -38,8 +38,8 @@ pub(crate) fn categorize_velocity_contacts( ) { for manifold_i in manifold_indices { let manifold = &manifolds[*manifold_i]; - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; if !rb1.is_dynamic() || !rb2.is_dynamic() { out_ground.push(*manifold_i) diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 04acaaf..18e846b 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -12,7 +12,7 @@ pub(crate) trait PairInteraction { impl<'a> PairInteraction for &'a mut ContactManifold { fn body_pair(&self) -> BodyPair { - self.body_pair + self.data.body_pair } } @@ -77,7 +77,7 @@ impl ParallelInteractionGroups { .iter() .zip(self.interaction_colors.iter_mut()) { - let body_pair = interactions[*interaction_id].body_pair(); + let body_pair = interactions[*interaction_id].data.body_pair(); let rb1 = &bodies[body_pair.body1]; let rb2 = &bodies[body_pair.body2]; @@ -338,7 +338,7 @@ impl InteractionGroups { let mut occupied_mask = 0u128; let max_interaction_points = interaction_indices .iter() - .map(|i| interactions[*i].num_active_contacts()) + .map(|i| interactions[*i].num_active_contacts) .max() .unwrap_or(1); @@ -351,12 +351,12 @@ impl InteractionGroups { // FIXME: how could we avoid iterating // on each interaction at every iteration on k? - if interaction.num_active_contacts() != k { + if interaction.num_active_contacts != k { continue; } - let body1 = &bodies[interaction.body_pair.body1]; - let body2 = &bodies[interaction.body_pair.body2]; + let body1 = &bodies[interaction.data.body_pair.body1]; + let body2 = &bodies[interaction.data.body_pair.body2]; let is_static1 = !body1.is_dynamic(); let is_static2 = !body2.is_dynamic(); diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index 303a18f..451dd32 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -247,13 +247,13 @@ impl ParallelVelocitySolverPart { VelocityConstraintDesc::NongroundGrouped(joint_id) => { let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies); - self.constraints[joints[0].constraint_index] = constraint; + self.constraints[joints[0].data.constraint_index] = constraint; } #[cfg(feature = "simd-is-enabled")] VelocityConstraintDesc::GroundGrouped(joint_id) => { let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies); - self.constraints[joints[0].constraint_index] = constraint; + self.constraints[joints[0].data.constraint_index] = constraint; } } } diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs index 69fcf57..b329e39 100644 --- a/src/dynamics/solver/position_constraint.rs +++ b/src/dynamics/solver/position_constraint.rs @@ -88,8 +88,8 @@ impl PositionConstraint { out_constraints: &mut Vec, push: bool, ) { - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; let shift1 = manifold.local_n1 * -manifold.kinematics.radius1; let shift2 = manifold.local_n2 * -manifold.kinematics.radius2; let radius = @@ -104,8 +104,8 @@ impl PositionConstraint { let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; for l in 0..manifold_points.len() { - local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1); - local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2); + local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1); + local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2); } let constraint = PositionConstraint { @@ -132,10 +132,10 @@ impl PositionConstraint { } } else { if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPointPoint(constraint); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPlanePoint(constraint); } } diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs index 0633926..ecc0fc3 100644 --- a/src/dynamics/solver/position_constraint_wide.rs +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -35,8 +35,8 @@ impl WPositionConstraint { out_constraints: &mut Vec, push: bool, ) { - let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH]; - let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH]; + let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; + let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let sqrt_ii1: AngularInertia = @@ -51,8 +51,8 @@ impl WPositionConstraint { let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); - let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); - let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; @@ -100,10 +100,10 @@ impl WPositionConstraint { } } else { if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPointPoint(constraint); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPlanePoint(constraint); } } diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs index dcd2d64..7a15f3e 100644 --- a/src/dynamics/solver/position_ground_constraint.rs +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -28,8 +28,8 @@ impl PositionGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rb1 = &bodies[manifold.body_pair.body1]; - let mut rb2 = &bodies[manifold.body_pair.body2]; + let mut rb1 = &bodies[manifold.data.body_pair.body1]; + let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flip = !rb2.is_dynamic(); let local_n1; @@ -41,13 +41,13 @@ impl PositionGroundConstraint { std::mem::swap(&mut rb1, &mut rb2); local_n1 = manifold.local_n2; local_n2 = manifold.local_n1; - delta1 = &manifold.delta2; - delta2 = &manifold.delta1; + delta1 = &manifold.data.delta2; + delta2 = &manifold.data.delta1; } else { local_n1 = manifold.local_n1; local_n2 = manifold.local_n2; - delta1 = &manifold.delta1; - delta2 = &manifold.delta2; + delta1 = &manifold.data.delta1; + delta2 = &manifold.data.delta2; }; let coll_pos1 = rb1.position * delta1; @@ -105,10 +105,10 @@ impl PositionGroundConstraint { } } else { if manifold.kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPointPointGround(constraint); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyPositionConstraint::NongroupedPlanePointGround(constraint); } } diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs index e423c0a..1609709 100644 --- a/src/dynamics/solver/position_ground_constraint_wide.rs +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -32,8 +32,10 @@ impl WPositionGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH]; - let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH]; + let mut rbs1 = + array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH]; + let mut rbs2 = + array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -55,10 +57,10 @@ impl WPositionGroundConstraint { ); let delta1 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH], ); let delta2 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], ); let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); @@ -112,10 +114,10 @@ impl WPositionGroundConstraint { } } else { if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPointPointGround(constraint); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyPositionConstraint::GroupedPlanePointGround(constraint); } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 948081d..1bc4c0e 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -144,14 +144,14 @@ impl VelocityConstraint { out_constraints: &mut Vec, push: bool, ) { - let rb1 = &bodies[manifold.body_pair.body1]; - let rb2 = &bodies[manifold.body_pair.body2]; + let rb1 = &bodies[manifold.data.body_pair.body1]; + let rb2 = &bodies[manifold.data.body_pair.body2]; let mj_lambda1 = rb1.active_set_offset; let mj_lambda2 = rb2.active_set_offset; - let pos_coll1 = rb1.position * manifold.delta1; - let pos_coll2 = rb2.position * manifold.delta2; + let pos_coll1 = rb1.position * manifold.data.delta1; + let pos_coll2 = rb2.position * manifold.data.delta2; let force_dir1 = pos_coll1 * (-manifold.local_n1); - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold .active_contacts() @@ -164,7 +164,7 @@ impl VelocityConstraint { elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1: rb1.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass, - limit: manifold.friction, + limit: manifold.data.friction, mj_lambda1, mj_lambda2, manifold_id, @@ -207,7 +207,7 @@ impl VelocityConstraint { constraint.dir1 = force_dir1; constraint.im1 = rb1.mass_properties.inv_mass; constraint.im2 = rb2.mass_properties.inv_mass; - constraint.limit = manifold.friction; + constraint.limit = manifold.data.friction; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; @@ -241,12 +241,12 @@ impl VelocityConstraint { let mut rhs = (vel1 - vel2).dot(&force_dir1); if rhs <= -params.restitution_velocity_threshold { - rhs += manifold.restitution * rhs + rhs += manifold.data.restitution * rhs } rhs += manifold_point.dist.max(0.0) * params.inv_dt(); - let impulse = manifold_points[k].impulse * warmstart_coeff; + let impulse = manifold_points[k].data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityConstraintElementPart { gcross1, @@ -275,9 +275,9 @@ impl VelocityConstraint { + gcross2.gdot(gcross2)); let rhs = (vel1 - vel2).dot(&tangents1[j]); #[cfg(feature = "dim2")] - let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] - let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart { gcross1, @@ -294,7 +294,7 @@ impl VelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint)); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyVelocityConstraint::Nongrouped(constraint); } } @@ -388,15 +388,15 @@ impl VelocityConstraint { for k in 0..self.num_contacts as usize { let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = [ + active_contacts[k_base + k].data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 5d8078a..a4efb9a 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -69,11 +69,11 @@ impl WVelocityConstraint { push: bool, ) { let inv_dt = SimdFloat::splat(params.inv_dt()); - let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; - let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; - let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]); - let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]); + let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]); + let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]); let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let ii1: AngularInertia = @@ -103,13 +103,13 @@ impl WVelocityConstraint { let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]); + let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); let restitution_velocity_threshold = SimdFloat::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { @@ -140,7 +140,7 @@ impl WVelocityConstraint { let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -176,11 +176,11 @@ impl WVelocityConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); @@ -202,7 +202,7 @@ impl WVelocityConstraint { if push { out_constraints.push(AnyVelocityConstraint::Grouped(constraint)); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::Grouped(constraint); } } @@ -342,15 +342,15 @@ impl WVelocityConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = impulses[ii]; + active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index d8ef8be..c3a3356 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -63,26 +63,26 @@ impl VelocityGroundConstraint { out_constraints: &mut Vec, push: bool, ) { - let mut rb1 = &bodies[manifold.body_pair.body1]; - let mut rb2 = &bodies[manifold.body_pair.body2]; + let mut rb1 = &bodies[manifold.data.body_pair.body1]; + let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = !rb2.is_dynamic(); let force_dir1; let coll_pos1; let coll_pos2; if flipped { - coll_pos1 = rb2.position * manifold.delta2; - coll_pos2 = rb1.position * manifold.delta1; + coll_pos1 = rb2.position * manifold.data.delta2; + coll_pos2 = rb1.position * manifold.data.delta1; force_dir1 = coll_pos1 * (-manifold.local_n2); std::mem::swap(&mut rb1, &mut rb2); } else { - coll_pos1 = rb1.position * manifold.delta1; - coll_pos2 = rb2.position * manifold.delta2; + coll_pos1 = rb1.position * manifold.data.delta1; + coll_pos2 = rb2.position * manifold.data.delta2; force_dir1 = coll_pos1 * (-manifold.local_n1); } let mj_lambda2 = rb2.active_set_offset; - let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; for (l, manifold_points) in manifold .active_contacts() @@ -94,7 +94,7 @@ impl VelocityGroundConstraint { dir1: force_dir1, elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: rb2.mass_properties.inv_mass, - limit: manifold.friction, + limit: manifold.data.friction, mj_lambda2, manifold_id, manifold_contact_id: l * MAX_MANIFOLD_POINTS, @@ -135,7 +135,7 @@ impl VelocityGroundConstraint { { constraint.dir1 = force_dir1; constraint.im2 = rb2.mass_properties.inv_mass; - constraint.limit = manifold.friction; + constraint.limit = manifold.data.friction; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; @@ -173,12 +173,12 @@ impl VelocityGroundConstraint { let mut rhs = (vel1 - vel2).dot(&force_dir1); if rhs <= -params.restitution_velocity_threshold { - rhs += manifold.restitution * rhs + rhs += manifold.data.restitution * rhs } rhs += manifold_point.dist.max(0.0) * params.inv_dt(); - let impulse = manifold_points[k].impulse * warmstart_coeff; + let impulse = manifold_points[k].data.impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { gcross2, @@ -199,9 +199,9 @@ impl VelocityGroundConstraint { let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); #[cfg(feature = "dim2")] - let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] - let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; constraint.elements[k].tangent_part[j] = VelocityGroundConstraintElementPart { @@ -218,7 +218,7 @@ impl VelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint)); } else { - out_constraints[manifold.constraint_index + l] = + out_constraints[manifold.data.constraint_index + l] = AnyVelocityConstraint::NongroupedGround(constraint); } } @@ -290,15 +290,15 @@ impl VelocityGroundConstraint { for k in 0..self.num_contacts as usize { let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = self.elements[k].tangent_part[0].impulse; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = [ + active_contacts[k_base + k].data.tangent_impulse = [ self.elements[k].tangent_part[0].impulse, self.elements[k].tangent_part[1].impulse, ]; diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index be01944..84c4182 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -65,8 +65,8 @@ impl WVelocityGroundConstraint { push: bool, ) { let inv_dt = SimdFloat::splat(params.inv_dt()); - let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; - let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH]; + let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH]; let mut flipped = [false; SIMD_WIDTH]; for ii in 0..SIMD_WIDTH { @@ -90,10 +90,10 @@ impl WVelocityGroundConstraint { let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let delta1 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH], ); let delta2 = Isometry::from( - array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH], + array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH], ); let coll_pos1 = pos1 * delta1; @@ -109,13 +109,13 @@ impl WVelocityGroundConstraint { let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; - let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); - let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]); + let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]); + let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]); let restitution_velocity_threshold = SimdFloat::splat(params.restitution_velocity_threshold); let warmstart_multiplier = - SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { @@ -146,7 +146,7 @@ impl WVelocityGroundConstraint { let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let impulse = - SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); let dp1 = p1 - world_com1; let dp2 = p2 - world_com2; @@ -178,11 +178,11 @@ impl WVelocityGroundConstraint { for j in 0..DIM - 1 { #[cfg(feature = "dim2")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH], ); #[cfg(feature = "dim3")] let impulse = SimdFloat::from( - array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH], ); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); @@ -202,7 +202,7 @@ impl WVelocityGroundConstraint { if push { out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint)); } else { - out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::GroupedGround(constraint); } } @@ -302,15 +302,15 @@ impl WVelocityGroundConstraint { let manifold = &mut manifolds_all[self.manifold_id[ii]]; let k_base = self.manifold_contact_id; let active_contacts = manifold.active_contacts_mut(); - active_contacts[k_base + k].impulse = impulses[ii]; + active_contacts[k_base + k].data.impulse = impulses[ii]; #[cfg(feature = "dim2")] { - active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii]; } #[cfg(feature = "dim3")] { - active_contacts[k_base + k].tangent_impulse = + active_contacts[k_base + k].data.tangent_impulse = [tangent_impulses[ii], bitangent_impulses[ii]]; } } diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 863990d..56c05df 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -4,7 +4,7 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; use bit_vec::BitVec; -use ncollide::bounding_volume::{BoundingVolume, AABB}; +use buckler::bounding_volume::{BoundingVolume, AABB}; use std::cmp::Ordering; use std::ops::{Index, IndexMut}; @@ -67,7 +67,7 @@ fn point_key(point: Point) -> Point { (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() } -fn region_aabb(index: Point) -> AABB { +fn region_aabb(index: Point) -> AABB { let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into(); let maxs = mins + Vector::repeat(CELL_WIDTH); AABB::new(mins, maxs) @@ -345,7 +345,7 @@ struct SAPRegion { } impl SAPRegion { - pub fn new(bounds: AABB) -> Self { + pub fn new(bounds: AABB) -> Self { let axes = [ SAPAxis::new(bounds.mins.x, bounds.maxs.x), SAPAxis::new(bounds.mins.y, bounds.maxs.y), @@ -361,7 +361,7 @@ impl SAPRegion { } } - pub fn recycle(bounds: AABB, mut old: Self) -> Self { + pub fn recycle(bounds: AABB, mut old: Self) -> Self { // Correct the bounds for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) { axis.min_bound = bound; @@ -381,7 +381,7 @@ impl SAPRegion { old } - pub fn recycle_or_new(bounds: AABB, pool: &mut Vec) -> Self { + pub fn recycle_or_new(bounds: AABB, pool: &mut Vec) -> Self { if let Some(old) = pool.pop() { Self::recycle(bounds, old) } else { @@ -488,7 +488,7 @@ pub struct BroadPhase { #[derive(Clone)] pub(crate) struct BroadPhaseProxy { handle: ColliderHandle, - aabb: AABB, + aabb: AABB, next_free: u32, } diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs deleted file mode 100644 index 54736cc..0000000 --- a/src/geometry/capsule.rs +++ /dev/null @@ -1,192 +0,0 @@ -use crate::geometry::{Ray, RayIntersection, AABB}; -use crate::math::{Isometry, Point, Rotation, Vector}; -use approx::AbsDiffEq; -use na::Unit; -use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast}; -use ncollide::shape::{FeatureId, Segment, SupportMap}; - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A capsule shape defined as a round segment. -pub struct Capsule { - /// The axis and endpoint of the capsule. - pub segment: Segment, - /// The radius of the capsule. - pub radius: f32, -} - -impl Capsule { - /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius. - pub fn new_x(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::x() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius. - pub fn new_y(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::y() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius. - #[cfg(feature = "dim3")] - pub fn new_z(half_height: f32, radius: f32) -> Self { - let b = Point::from(Vector::z() * half_height); - Self::new(-b, b, radius) - } - - /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. - pub fn new(a: Point, b: Point, radius: f32) -> Self { - let segment = Segment::new(a, b); - Self { segment, radius } - } - - /// The axis-aligned bounding box of this capsule. - pub fn aabb(&self, pos: &Isometry) -> AABB { - let a = pos * self.segment.a; - let b = pos * self.segment.b; - let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius); - let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius); - AABB::new(mins.into(), maxs.into()) - } - - /// The height of this capsule. - pub fn height(&self) -> f32 { - (self.segment.b - self.segment.a).norm() - } - - /// The half-height of this capsule. - pub fn half_height(&self) -> f32 { - self.height() / 2.0 - } - - /// The center of this capsule. - pub fn center(&self) -> Point { - na::center(&self.segment.a, &self.segment.b) - } - - /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. - pub fn transform_by(&self, pos: &Isometry) -> Self { - Self::new(pos * self.segment.a, pos * self.segment.b, self.radius) - } - - /// The rotation `r` such that `r * Y` is collinear with `b - a`. - pub fn rotation_wrt_y(&self) -> Rotation { - let mut dir = self.segment.b - self.segment.a; - if dir.y < 0.0 { - dir = -dir; - } - - #[cfg(feature = "dim2")] - { - Rotation::rotation_between(&Vector::y(), &dir) - } - - #[cfg(feature = "dim3")] - { - Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()) - } - } - - /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`. - pub fn transform_wrt_y(&self) -> Isometry { - let rot = self.rotation_wrt_y(); - Isometry::from_parts(self.center().coords.into(), rot) - } -} - -impl SupportMap for Capsule { - fn local_support_point(&self, dir: &Vector) -> Point { - let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis()); - self.local_support_point_toward(&dir) - } - - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) { - self.segment.a + **dir * self.radius - } else { - self.segment.b + **dir * self.radius - } - } -} - -impl RayCast for Capsule { - fn toi_and_normal_with_ray( - &self, - m: &Isometry, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option { - let ls_ray = ray.inverse_transform_by(m); - - ncollide::query::ray_intersection_with_support_map_with_params( - &Isometry::identity(), - self, - &mut VoronoiSimplex::new(), - &ls_ray, - max_toi, - solid, - ) - .map(|mut res| { - res.normal = m * res.normal; - res - }) - } -} - -// TODO: this code has been extracted from ncollide and added here -// so we can modify it to fit with our new definition of capsule. -// We should find a way to avoid this code duplication. -impl PointQuery for Capsule { - #[inline] - fn project_point( - &self, - m: &Isometry, - pt: &Point, - solid: bool, - ) -> PointProjection { - let seg = Segment::new(self.segment.a, self.segment.b); - let proj = seg.project_point(m, pt, solid); - let dproj = *pt - proj.point; - - if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) { - let inside = dist <= self.radius; - if solid && inside { - return PointProjection::new(true, *pt); - } else { - return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius); - } - } else if solid { - return PointProjection::new(true, *pt); - } - - #[cfg(feature = "dim2")] - if let Some(dir) = seg.normal() { - let dir = m * *dir; - PointProjection::new(true, proj.point + dir * self.radius) - } else { - // The segment has no normal, likely because it degenerates to a point. - PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) - } - - #[cfg(feature = "dim3")] - if let Some(dir) = seg.direction() { - use crate::utils::WBasis; - let dir = m * dir.orthonormal_basis()[0]; - PointProjection::new(true, proj.point + dir * self.radius) - } else { - // The segment has no normal, likely because it degenerates to a point. - PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) - } - } - - #[inline] - fn project_point_with_feature( - &self, - m: &Isometry, - pt: &Point, - ) -> (PointProjection, FeatureId) { - (self.project_point(m, pt, false), FeatureId::Face(0)) - } -} diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index c04be35..8154554 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -6,8 +6,8 @@ use crate::geometry::{ #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder, RoundCylinder}; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; +use buckler::bounding_volume::AABB; use na::Point3; -use ncollide::bounding_volume::AABB; use std::ops::Deref; use std::sync::Arc; @@ -270,11 +270,11 @@ impl Collider { } /// Compute the axis-aligned bounding box of this collider. - pub fn compute_aabb(&self) -> AABB { + pub fn compute_aabb(&self) -> AABB { self.shape.compute_aabb(&self.position) } - // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB { + // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB { // let aabb1 = self.shape.compute_aabb(&self.position); // let aabb2 = self.shape.compute_aabb(&self.predicted_position); // aabb1.merged(&aabb2) diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index a4a176e..1f0a902 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -1,7 +1,8 @@ +use crate::buckler::query::TrackedData; use crate::data::MaybeSerializableData; -use crate::dynamics::BodyPair; +use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet}; use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase}; -use crate::geometry::{Collider, ColliderPair, ColliderSet}; +use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold}; use crate::math::{Isometry, Point, Vector}; #[cfg(feature = "simd-is-enabled")] use { @@ -19,39 +20,6 @@ bitflags::bitflags! { } } -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// The type local linear approximation of the neighborhood of a pair contact points on two shapes -pub enum KinematicsCategory { - /// Both neighborhoods are assimilated to a single point. - PointPoint, - /// The first shape's neighborhood at the contact point is assimilated to a plane while - /// the second is assimilated to a point. - PlanePoint, -} - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// Local contact geometry at the neighborhood of a pair of contact points. -pub struct ContactKinematics { - /// The local contact geometry. - pub category: KinematicsCategory, - /// The dilation applied to the first contact geometry. - pub radius1: f32, - /// The dilation applied to the second contact geometry. - pub radius2: f32, -} - -impl Default for ContactKinematics { - fn default() -> Self { - ContactKinematics { - category: KinematicsCategory::PointPoint, - radius1: 0.0, - radius2: 0.0, - } - } -} - #[cfg(feature = "simd-is-enabled")] pub(crate) struct WContact { pub local_p1: Point, @@ -70,10 +38,9 @@ impl WContact { local_p1: self.local_p1.extract(i), local_p2: self.local_p2.extract(i), dist: self.dist.extract(i), - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), fid1: self.fid1[i], fid2: self.fid2[i], + data: ContactData::default(), }; (c, self.local_n1.extract(i), self.local_n2.extract(i)) @@ -83,11 +50,7 @@ impl WContact { #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A single contact between two collider. -pub struct Contact { - /// The contact point in the local-space of the first collider. - pub local_p1: Point, - /// The contact point in the local-space of the second collider. - pub local_p2: Point, +pub struct ContactData { /// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body. /// /// The impulse applied to the second collider's rigid-body is given by `-impulse`. @@ -100,46 +63,9 @@ pub struct Contact { /// collider's rigid-body. #[cfg(feature = "dim3")] pub tangent_impulse: [f32; 2], - /// The identifier of the subshape of the first collider involved in this contact. - /// - /// For primitive shapes like cuboid, ball, etc., this is 0. - /// For shapes like trimesh and heightfield this identifies the specific triangle - /// involved in the contact. - pub fid1: u8, - /// The identifier of the subshape of the second collider involved in this contact. - /// - /// For primitive shapes like cuboid, ball, etc., this is 0. - /// For shapes like trimesh and heightfield this identifies the specific triangle - /// involved in the contact. - pub fid2: u8, - /// The distance between the two colliders along the contact normal. - /// - /// If this is negative, the colliders are penetrating. - pub dist: f32, } -impl Contact { - pub(crate) fn new( - local_p1: Point, - local_p2: Point, - fid1: u8, - fid2: u8, - dist: f32, - ) -> Self { - Self { - local_p1, - local_p2, - impulse: 0.0, - #[cfg(feature = "dim2")] - tangent_impulse: 0.0, - #[cfg(feature = "dim3")] - tangent_impulse: [0.0; 2], - fid1, - fid2, - dist, - } - } - +impl ContactData { #[cfg(feature = "dim2")] pub(crate) fn zero_tangent_impulse() -> f32 { 0.0 @@ -149,26 +75,15 @@ impl Contact { pub(crate) fn zero_tangent_impulse() -> [f32; 2] { [0.0, 0.0] } +} - pub(crate) fn copy_geometry_from(&mut self, contact: Contact) { - self.local_p1 = contact.local_p1; - self.local_p2 = contact.local_p2; - self.fid1 = contact.fid1; - self.fid2 = contact.fid2; - self.dist = contact.dist; +impl Default for ContactData { + fn default() -> Self { + Self { + impulse: 0.0, + tangent_impulse: Self::zero_tangent_impulse(), + } } - - // pub(crate) fn swap(self) -> Self { - // Self { - // local_p1: self.local_p2, - // local_p2: self.local_p1, - // impulse: self.impulse, - // tangent_impulse: self.tangent_impulse, - // fid1: self.fid2, - // fid2: self.fid1, - // dist: self.dist, - // } - // } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -227,15 +142,16 @@ impl ContactPair { let coll2 = &colliders[self.pair.collider2]; if self.manifolds.len() == 0 { - let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags); - self.manifolds.push(manifold); + let manifold_data = ContactManifoldData::from_colliders(self.pair, coll1, coll2, flags