From 97157c9423f3360c5e941b4065377689221014ae Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Fri, 26 Mar 2021 18:16:27 +0100 Subject: First working version of non-linear CCD based on single-substep motion-clamping. --- Cargo.toml | 8 +- benchmarks3d/all_benchmarks3.rs | 2 + benchmarks3d/ccd3.rs | 85 ++++++++++++ examples2d/all_examples2.rs | 2 + examples3d/all_examples3.rs | 2 + examples3d/ccd3.rs | 145 ++++++++++++++++++++ src/data/coarena.rs | 14 ++ src/dynamics/ccd/toi_entry.rs | 147 +++++++++++++++++++++ src/dynamics/mod.rs | 2 + src/dynamics/rigid_body.rs | 110 +++++++++++---- src/dynamics/solver/island_solver.rs | 4 +- .../joint_constraint/ball_position_constraint.rs | 4 +- .../ball_position_constraint_wide.rs | 2 +- .../joint_constraint/fixed_position_constraint.rs | 4 +- .../prismatic_position_constraint.rs | 8 +- .../revolute_position_constraint.rs | 16 +-- src/dynamics/solver/parallel_island_solver.rs | 4 +- src/dynamics/solver/position_solver.rs | 4 +- src/geometry/collider.rs | 17 +-- src/geometry/collider_set.rs | 2 +- src/geometry/narrow_phase.rs | 8 ++ src/pipeline/collision_pipeline.rs | 11 +- src/pipeline/physics_pipeline.rs | 25 +++- src/pipeline/query_pipeline.rs | 74 ++++++++--- src_testbed/box2d_backend.rs | 11 +- src_testbed/harness/mod.rs | 6 +- src_testbed/physics/mod.rs | 4 +- src_testbed/physx_backend.rs | 38 +++++- src_testbed/testbed.rs | 44 +++++- 29 files changed, 695 insertions(+), 108 deletions(-) create mode 100644 benchmarks3d/ccd3.rs create mode 100644 examples3d/ccd3.rs create mode 100644 src/dynamics/ccd/toi_entry.rs diff --git a/Cargo.toml b/Cargo.toml index b6ae4da..cfdb3ba 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,10 +19,10 @@ members = [ "build/rapier2d", "build/rapier2d-f64", "build/rapier_testbed2d", "e #kiss3d = { git = "https://github.com/sebcrozet/kiss3d" } #nalgebra = { git = "https://github.com/dimforge/nalgebra", branch = "dev" } -#parry2d = { git = "https://github.com/dimforge/parry" } -#parry3d = { git = "https://github.com/dimforge/parry" } -#parry2d-f64 = { git = "https://github.com/dimforge/parry" } -#parry3d-f64 = { git = "https://github.com/dimforge/parry" } +parry2d = { git = "https://github.com/dimforge/parry", branch = "special_cases" } +parry3d = { git = "https://github.com/dimforge/parry", branch = "special_cases" } +parry2d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" } +parry3d-f64 = { git = "https://github.com/dimforge/parry", branch = "special_cases" } #ncollide2d = { git = "https://github.com/dimforge/ncollide" } #ncollide3d = { git = "https://github.com/dimforge/ncollide" } #nphysics2d = { git = "https://github.com/dimforge/nphysics" } diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs index f35cb8e..38d71cd 100644 --- a/benchmarks3d/all_benchmarks3.rs +++ b/benchmarks3d/all_benchmarks3.rs @@ -13,6 +13,7 @@ use std::cmp::Ordering; mod balls3; mod boxes3; mod capsules3; +mod ccd3; mod compound3; mod convex_polyhedron3; mod heightfield3; @@ -52,6 +53,7 @@ pub fn main() { ("Balls", balls3::init_world), ("Boxes", boxes3::init_world), ("Capsules", capsules3::init_world), + ("CCD", ccd3::init_world), ("Compound", compound3::init_world), ("Convex polyhedron", convex_polyhedron3::init_world), ("Heightfield", heightfield3::init_world), diff --git a/benchmarks3d/ccd3.rs b/benchmarks3d/ccd3.rs new file mode 100644 index 0000000..f1831e7 --- /dev/null +++ b/benchmarks3d/ccd3.rs @@ -0,0 +1,85 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +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 = 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 = 4; + let numj = 20; + 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 mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; + + for j in 0usize..numj { + for i in 0..num { + for k in 0usize..num { + 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) + .linvel(0.0, -1000.0, 0.0) + .ccd_enabled(true) + .build(); + let handle = bodies.insert(rigid_body); + + let collider = match j % 5 { + 0 => ColliderBuilder::cuboid(rad, rad, rad), + 1 => ColliderBuilder::ball(rad), + // Rounded cylinders are much more efficient that cylinder, even if the + // rounding margin is small. + // 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0), + // 3 => ColliderBuilder::cone(rad, rad), + _ => ColliderBuilder::capsule_y(rad, rad), + }; + + colliders.insert(collider.build(), handle, &mut bodies); + } + } + + offset -= 0.05 * rad * (num as f32 - 1.0); + } + + /* + * 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/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index f4430aa..8f38ced 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 ccd2; mod collision_groups2; mod convex_polygons2; mod damping2; @@ -60,6 +61,7 @@ pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ ("Add remove", add_remove2::init_world), + ("CCD", ccd2::init_world), ("Collision groups", collision_groups2::init_world), ("Convex polygons", convex_polygons2::init_world), ("Damping", damping2::init_world), diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index a8c38c6..8122583 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -10,6 +10,7 @@ use inflector::Inflector; use rapier_testbed3d::Testbed; use std::cmp::Ordering; +mod ccd3; mod collision_groups3; mod compound3; mod convex_decomposition3; @@ -77,6 +78,7 @@ pub fn main() { let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ ("Fountain", fountain3::init_world), ("Primitives", primitives3::init_world), + ("CCD", ccd3::init_world), ("Collision groups", collision_groups3::init_world), ("Compound", compound3::init_world), ("Convex decomposition", convex_decomposition3::init_world), diff --git a/examples3d/ccd3.rs b/examples3d/ccd3.rs new file mode 100644 index 0000000..b62427f --- /dev/null +++ b/examples3d/ccd3.rs @@ -0,0 +1,145 @@ +use na::{Isometry3, Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +fn create_wall( + testbed: &mut Testbed, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + offset: Vector3, + stack_height: usize, + half_extents: Vector3, +) { + let shift = half_extents * 2.0; + for i in 0usize..stack_height { + for j in i..stack_height { + let fj = j as f32; + let fi = i as f32; + let x = offset.x; + let y = fi * shift.y + offset.y; + let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z + - stack_height as f32 * half_extents.z; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = + ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build(); + colliders.insert(collider, handle, bodies); + testbed.set_body_color(handle, Point3::new(218. / 255., 201. / 255., 1.0)); + } + } +} + +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 = 50.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, ground_handle, &mut bodies); + + /* + * Create the pyramids. + */ + let num_z = 8; + let num_x = 5; + let shift_y = ground_height + 0.5; + let shift_z = (num_z as f32 + 2.0) * 2.0; + + for i in 0..num_x { + let x = i as f32 * 6.0; + create_wall( + testbed, + &mut bodies, + &mut colliders, + Vector3::new(x, shift_y, 0.0), + num_z, + Vector3::new(0.5, 0.5, 1.0), + ); + + create_wall( + testbed, + &mut bodies, + &mut colliders, + Vector3::new(x, shift_y, shift_z), + num_z, + Vector3::new(0.5, 0.5, 1.0), + ); + } + + /* + * Create two very fast rigid-bodies. + * The first one has CCD enabled and a sensor collider attached to it. + * The second one has CCD enabled and a collider attached to it. + */ + let collider = ColliderBuilder::ball(1.0) + .density(10.0) + .sensor(true) + .build(); + let mut rigid_body = RigidBodyBuilder::new_dynamic() + .linvel(1000.0, 0.0, 0.0) + .translation(-20.0, shift_y + 2.0, 0.0) + .ccd_enabled(true) + .build(); + let sensor_handle = bodies.insert(rigid_body); + colliders.insert(collider, sensor_handle, &mut bodies); + + // Second rigid-body with CCD enabled. + let collider = ColliderBuilder::ball(1.0).density(10.0).build(); + let mut rigid_body = RigidBodyBuilder::new_dynamic() + .linvel(1000.0, 0.0, 0.0) + .translation(-20.0, shift_y + 2.0, shift_z) + .ccd_enabled(true) + .build(); + let handle = bodies.insert(rigid_body); + colliders.insert(collider.clone(), handle, &mut bodies); + + // Callback that will be executed on the main loop to handle proximities. + testbed.add_callback(move |_, mut graphics, physics, events, _| { + while let Ok(prox) = events.intersection_events.try_recv() { + let color = if prox.intersecting { + Point3::new(1.0, 1.0, 0.0) + } else { + Point3::new(0.5, 0.5, 1.0) + }; + + let parent_handle1 = physics.colliders.get(prox.collider1).unwrap().parent(); + let parent_handle2 = physics.colliders.get(prox.collider2).unwrap().parent(); + + if let Some(graphics) = &mut graphics { + if parent_handle1 != ground_handle && parent_handle1 != sensor_handle { + graphics.set_body_color(parent_handle1, color); + } + if parent_handle2 != ground_handle && parent_handle2 != sensor_handle { + graphics.set_body_color(parent_handle2, color); + } + } + } + }); + + /* + * 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/src/data/coarena.rs b/src/data/coarena.rs index 78cbfa7..c25cc55 100644 --- a/src/data/coarena.rs +++ b/src/data/coarena.rs @@ -29,6 +29,20 @@ impl Coarena { .and_then(|(gg, t)| if g == *gg { Some(t) } else { None }) } + /// Inserts an element into this coarena. + pub fn insert(&mut self, a: Index, value: T) + where + T: Clone + Default, + { + let (i1, g1) = a.into_raw_parts(); + + if self.data.len() <= i1 { + self.data.resize(i1 + 1, (u32::MAX as u64, T::default())); + } + + self.data[i1] = (g1, value); + } + /// Ensure that elements at the two given indices exist in this coarena, and return their reference. /// /// Missing elements are created automatically and initialized with the `default` value. diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs new file mode 100644 index 0000000..20f5268 --- /dev/null +++ b/src/dynamics/ccd/toi_entry.rs @@ -0,0 +1,147 @@ +use crate::data::Coarena; +use crate::dynamics::ccd::ccd_solver::CCDContact; +use crate::dynamics::ccd::CCDData; +use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle}; +use crate::geometry::{Collider, ColliderHandle}; +use crate::math::{Isometry, Real}; +use crate::parry::query::PersistentQueryDispatcher; +use crate::utils::WCross; +use na::{RealField, Unit}; +use parry::query::{NonlinearRigidMotion, QueryDispatcher, TOI}; + +#[derive(Copy, Clone, Debug)] +pub struct TOIEntry { + pub toi: Real, + pub c1: ColliderHandle, + pub b1: RigidBodyHandle, + pub c2: ColliderHandle, + pub b2: RigidBodyHandle, + pub is_intersection_test: bool, + pub timestamp: usize, +} + +impl TOIEntry { + fn new( + toi: Real, + c1: ColliderHandle, + b1: RigidBodyHandle, + c2: ColliderHandle, + b2: RigidBodyHandle, + is_intersection_test: bool, + timestamp: usize, + ) -> Self { + Self { + toi, + c1, + b1, + c2, + b2, + is_intersection_test, + timestamp, + } + } + + pub fn try_from_colliders>( + params: &IntegrationParameters, + query_dispatcher: &QD, + ch1: ColliderHandle, + ch2: ColliderHandle, + c1: &Collider, + c2: &Collider, + b1: &RigidBody, + b2: &RigidBody, + frozen1: Option, + frozen2: Option, + start_time: Real, + end_time: Real, + body_params: &Coarena, + ) -> Option { + assert!(start_time <= end_time); + + let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel; + let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel; + + let vel12 = linvel2 - linvel1; + let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness()); + + if params.dt * vel12.norm() < thickness { + return None; + } + + let is_intersection_test = c1.is_sensor() || c2.is_sensor(); + + let body_params1 = body_params.get(c1.parent.0)?; + let body_params2 = body_params.get(c2.parent.0)?; + + // Compute the TOI. + let mut motion1 = body_params1.motion(params.dt, b1, 0.0); + let mut motion2 = body_params2.motion(params.dt, b2, 0.0); + + if let Some(t) = frozen1 { + motion1.freeze(t); + } + + if let Some(t) = frozen2 { + motion2.freeze(t); + } + + let mut toi; + let motion_c1 = motion1.prepend(*c1.position_wrt_parent()); + let motion_c2 = motion2.prepend(*c2.position_wrt_parent()); + + // println!("start_time: {}", start_time); + + // If this is just an intersection test (i.e. with sensors) + // then we can stop the TOI search immediately if it starts with + // a penetration because we don't care about the whether the velocity + // at the impact is a separating velocity or not. + // If the TOI search involves two non-sensor colliders then + // we don't want to stop the TOI search at the first penetration + // because the colliders may be in a separating trajectory. + let stop_at_penetration = is_intersection_test; + + let res_toi = query_dispatcher + .nonlinear_time_of_impact( + &motion_c1, + c1.shape(), + &motion_c2, + c2.shape(), + start_time, + end_time, + stop_at_penetration, + ) + .ok(); + + toi = res_toi??; + + Some(Self::new( + toi.toi, + ch1, + c1.parent(), + ch2, + c2.parent(), + is_intersection_test, + 0, + )) + } +} + +impl PartialOrd for TOIEntry { + fn partial_cmp(&self, other: &Self) -> Option { + (-self.toi).partial_cmp(&(-other.toi)) + } +} + +impl Ord for TOIEntry { + fn cmp(&self, other: &Self) -> std::cmp::Ordering { + self.partial_cmp(other).unwrap() + } +} + +impl PartialEq for TOIEntry { + fn eq(&self, other: &Self) -> bool { + self.toi == other.toi + } +} + +impl Eq for TOIEntry {} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index eab6916..751f6f9 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -18,6 +18,7 @@ pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBui pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; pub use parry::mass_properties::MassProperties; // #[cfg(not(feature = "parallel"))] +pub use self::ccd::CCDSolver; pub use self::coefficient_combine_rule::CoefficientCombineRule; pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::rigid_body::RigidBodyChanges; @@ -26,6 +27,7 @@ pub(crate) use self::solver::IslandSolver; #[cfg(feature = "parallel")] pub(crate) use self::solver::ParallelIslandSolver; +mod ccd; mod coefficient_combine_rule; mod integration_parameters; mod joint; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 7cc7a99..9ac4763 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -36,6 +36,7 @@ bitflags::bitflags! { const ROTATION_LOCKED_X = 1 << 1; const ROTATION_LOCKED_Y = 1 << 2; const ROTATION_LOCKED_Z = 1 << 3; + const CCD_ENABLED = 1 << 4; } } @@ -58,7 +59,16 @@ bitflags::bitflags! { pub struct RigidBody { /// The world-space position of the rigid-body. pub(crate) position: Isometry, - pub(crate) predicted_position: Isometry, + /// The next position of the rigid-body. + /// + /// At the beginning of the timestep, and when the + /// timestep is complete we must have position == next_position + /// except for kinematic bodies. + /// + /// The next_position is updated after the velocity and position + /// resolution. Then it is either validated (ie. we set position := set_position) + /// or clamped by CCD. + pub(crate) next_position: Isometry, /// The local mass properties of the rigid-body. pub(crate) mass_properties: MassProperties, /// The world-space center of mass of the rigid-body. @@ -76,6 +86,10 @@ pub struct RigidBody { pub linear_damping: Real, /// Damping factor for gradually slowing down the angular motion of the rigid-body. pub angular_damping: Real, + /// The maximum linear velocity this rigid-body can reach. + pub max_linear_velocity: Real, + /// The maximum angular velocity this rigid-body can reach. + pub max_angular_velocity: Real, /// Accumulation of external forces (only for dynamic bodies). pub(crate) force: Vector, /// Accumulation of external torques (only for dynamic bodies). @@ -97,13 +111,14 @@ pub struct RigidBody { dominance_group: i8, /// User-defined data associated to this rigid-body. pub user_data: u128, + pub(crate) ccd_thickness: Real, } impl RigidBody { fn new() -> Self { Self { position: Isometry::identity(), - predicted_position: Isometry::identity(), + next_position: Isometry::identity(), mass_properties: MassProperties::zero(), world_com: Point::origin(), effective_inv_mass: 0.0, @@ -115,6 +130,8 @@ impl RigidBody { gravity_scale: 1.0, linear_damping: 0.0, angular_damping: 0.0, + max_linear_velocity: Real::MAX, + max_angular_velocity: 100.0, colliders: Vec::new(), activation: ActivationStatus::new_active(), joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(), @@ -127,6 +144,7 @@ impl RigidBody { body_status: BodyStatus::Dynamic, dominance_group: 0, user_data: 0, + ccd_thickness: Real::MAX, } } @@ -176,6 +194,20 @@ impl RigidBody { } } + /// Enables of disable CCD (continuous collision-detection) for this rigid-body. + pub fn enable_ccd(&mut self, enabled: bool) { + self.flags.set(RigidBodyFlags::CCD_ENABLED, enabled) + } + + /// Is CCD (continous collision-detection) enabled for this rigid-body? + pub fn is_ccd_enabled(&self) -> bool { + self.flags.contains(RigidBodyFlags::CCD_ENABLED) + } + + pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool { + self.is_ccd_enabled() && self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness + } + /// Sets the rigid-body's mass properties. /// /// If `wake_up` is `true` then the rigid-body will be woken up if it was @@ -228,8 +260,8 @@ impl RigidBody { /// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position` /// method and is used for estimating the kinematic body velocity at the next timestep. /// For non-kinematic bodies, this value is currently unspecified. - pub fn predicted_position(&self) -> &Isometry { - &self.predicted_position + pub fn next_position(&self) -> &Isometry { + &self.next_position } /// The scale factor applied to the gravity affecting this rigid-body. @@ -254,6 +286,8 @@ impl RigidBody { true, ); + self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness()); + let mass_properties = coll .mass_properties() .transform_by(coll.position_wrt_parent()); @@ -265,8 +299,8 @@ impl RigidBody { pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) { for handle in &self.colliders { let collider = &mut colliders[*handle]; + collider.prev_position = self.position; collider.position = self.position * collider.delta; - collider.predicted_position = self.predicted_position * collider.delta; } } @@ -331,18 +365,39 @@ impl RigidBody { !self.linvel.is_zero() || !self.angvel.is_zero() } - fn integrate_velocity(&self, dt: Real) -> Isometry { + pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry { let com = self.position * self.mass_properties.local_com; let shift = Translation::from(com.coords); shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() } - pub(crate) fn integrate(&mut self, dt: Real) { + pub(crate) fn position_at_time(&self, dt: Real) -> Isometry { + self.integrate_velocity(dt) * self.position + } + + pub(crate) fn integrate_next_position(&mut self, dt: Real, apply_damping: bool) { // TODO: do we want to apply damping before or after the velocity integration? - self.linvel *= 1.0 / (1.0 + dt * self.linear_damping); - self.angvel *= 1.0 / (1.0 + dt * self.angular_damping); + if apply_damping { + self.linvel *= 1.0 / (1.0 + dt * self.linear_damping); + self.angvel *= 1.0 / (1.0 + dt * self.angular_damping); + + // self.linvel = self.linvel.cap_magnitude(self.max_linear_velocity); + // #[cfg(feature = "dim2")] + // { + // self.angvel = na::clamp( + // self.angvel, + // -self.max_angular_velocity, + // self.max_angular_velocity, + // ); + // } + // #[cfg(feature = "dim3")] + // { + // self.angvel = self.angvel.cap_magnitude(self.max_angular_velocity); + // } + } - self.position = self.integrate_velocity(dt) * self.position; + self.next_position = self.integrate_velocity(dt) * self.position; + let _ = self.next_position.rotation.renormalize(); } /// The linear velocity of this rigid-body. @@ -416,7 +471,8 @@ impl RigidBody { /// put to sleep because it did not move for a while. pub fn set_position(&mut self, pos: Isometry, wake_up: bool) { self.changes.insert(RigidBodyChanges::POSITION); - self.set_position_internal(pos); + self.position = pos; + self.next_position = pos; // TODO: Do we really need to check that the body isn't dynamic? if wake_up && self.is_dynamic() { @@ -424,24 +480,19 @@ impl RigidBody { } } - pub(crate) fn set_position_internal(&mut self, pos: Isometry) { - self.position = pos; - - // TODO: update the predicted position for dynamic bodies too? - if self.is_static() || self.is_kinematic() { - self.predicted_position = pos; - } + pub(crate) fn set_next_position(&mut self, pos: Isometry) { + self.next_position = pos; } /// If this rigid body is kinematic, sets its future position after the next timestep integration. pub fn set_next_kinematic_position(&mut self, pos: Isometry) { if self.is_kinematic() { - self.predicted_position = pos; + self.next_position = pos; } } - pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: Real) { - let dpos = self.predicted_position * self.position.inverse(); + pub(crate) fn compute_velocity_from_next_position(&mut self, inv_dt: Real) { + let dpos = self.next_position * self.position.inverse(); #[cfg(feature = "dim2")] { self.angvel = dpos.rotation.angle() * inv_dt; @@ -453,8 +504,8 @@ impl RigidBody { self.linvel = dpos.translation.vector * inv_dt; } - pub(crate) fn update_predicted_position(&mut self, dt: Real) { - self.predicted_position = self.integrate_velocity(dt) * self.position; + pub(crate) fn update_next_position(&mut self, dt: Real) { + self.next_position = self.integrate_velocity(dt) * self.position; } pub(crate) fn update_world_mass_properties(&mut self) { @@ -666,6 +717,7 @@ pub struct RigidBodyBuilder { mass_properties: MassProperties, can_sleep: bool, sleeping: bool, + ccd_enabled: bool, dominance_group: i8, user_data: u128, } @@ -685,6 +737,7 @@ impl RigidBodyBuilder { mass_properties: MassProperties::zero(), can_sleep: true, sleeping: false, + ccd_enabled: false, dominance_group: 0, user_data: 0, } @@ -888,6 +941,12 @@ impl RigidBodyBuilder { self } + /// Enabled continuous collision-detection for this rigid-body. + pub fn ccd_enabled(mut self, enabled: bool) -> Self { + self.ccd_enabled = enabled; + self + } + /// Sets whether or not the rigid-body is to be created asleep. pub fn sleeping(mut self, sleeping: bool) -> Self { self.sleeping = sleeping; @@ -897,8 +956,8 @@ impl RigidBodyBuilder { /// Build a new rigid-body with the parameters configured with this builder. pub fn build(&self) -> RigidBody { let mut rb = RigidBody::new(); - rb.predicted_position = self.position; // FIXME: compute the correct value? - rb.set_position_internal(self.position); + rb.next_position = self.position; // FIXME: compute the correct value? + rb.position = self.position; rb.linvel = self.linvel; rb.angvel = self.angvel; rb.body_status = self.body_status; @@ -909,6 +968,7 @@ impl RigidBodyBuilder { rb.gravity_scale = self.gravity_scale; rb.flags = self.flags; rb.dominance_group = self.dominance_group; + rb.enable_ccd(self.ccd_enabled); if self.can_sleep && self.sleeping { rb.sleep(); diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index d0866bf..6ebf402 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -59,7 +59,7 @@ impl IslandSolver { counters.solver.velocity_update_time.resume(); bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.integrate(params.dt) + rb.integrate_next_position(params.dt, true) }); counters.solver.velocity_update_time.pause(); @@ -77,7 +77,7 @@ impl IslandSolver { bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { // Since we didn't run the velocity solver we need to integrate the accelerations here rb.integrate_accelerations(params.dt); - rb.integrate(params.dt); + rb.integrate_next_position(params.dt, true); }); counters.solver.velocity_update_time.pause(); } diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs index 744c00d..93996f4 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -114,7 +114,7 @@ impl BallPositionGroundConstraint { // are the local_anchors. The rb1 and rb2 have // already been flipped by the caller. Self { - anchor1: rb1.predicted_position * cparams.local_anchor2, + anchor1: rb1.next_position * cparams.local_anchor2, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor1, @@ -123,7 +123,7 @@ impl BallPositionGroundConstraint { } } else { Self { - anchor1: rb1.predicted_position * cparams.local_anchor1, + anchor1: rb1.next_position * cparams.local_anchor1, im2: rb2.effective_inv_mass, ii2: rb2.effective_world_inv_inertia_sqrt.squared(), local_anchor2: cparams.local_anchor2, diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs index 043eea7..e9162a4 100644 --- a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -134,7 +134,7 @@ impl WBallPositionGroundConstraint { cparams: [&BallJoint; SIMD_WIDTH], flipped: [bool; SIMD_WIDTH], ) -> Self { - let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]); let anchor1 = position1 * Point::from(array![|ii| if flipped[ii] { cparams[ii].local_anchor2 diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs index 7e8fc97..c98660f 100644 --- a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -100,10 +100,10 @@ impl FixedPositionGroundConstraint { let local_anchor2; if flipped { - anchor1 = rb1.predicted_position * cparams.local_anchor2; + anchor1 = rb1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; } else { - anchor1 = rb1.predicted_position * cparams.local_anchor1; + anchor1 = rb1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs index ea7e927..ed52a52 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -119,14 +119,14 @@ impl PrismaticPositionGroundConstraint { let local_axis2; if flipped { - frame1 = rb1.predicted_position * cparams.local_frame2(); + frame1 = rb1.next_position * cparams.local_frame2(); local_frame2 = cparams.local_frame1(); - axis1 = rb1.predicted_position * cparams.local_axis2; + axis1 = rb1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; } else { - frame1 = rb1.predicted_position * cparams.local_frame1(); + frame1 = rb1.next_position * cparams.local_frame1(); local_frame2 = cparams.local_frame2(); - axis1 = rb1.predicted_position * cparams.local_axis1; + axis1 = rb1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; }; diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs index 2da49e6..96391a2 100644 --- a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -145,23 +145,23 @@ impl RevolutePositionGroundConstraint { let local_basis2; if flipped { - anchor1 = rb1.predicted_position * cparams.local_anchor2; + anchor1 = rb1.next_position * cparams.local_anchor2; local_anchor2 = cparams.local_anchor1; - axis1 = rb1.predicted_position * cparams.local_axis2; + axis1 = rb1.next_position * cparams.local_axis2; local_axis2 = cparams.local_axis1; basis1 = [ - rb1.predicted_position * cparams.basis2[0], - rb1.predicted_position * cparams.basis2[1], + rb1.next_position * cparams.basis2[0], + rb1.next_position * cparams.basis2[1], ]; local_basis2 = cparams.basis1; } else { - anchor1 = rb1.predicted_position * cparams.local_anchor1; + anchor1 = rb1.next_position * cparams.local_anchor1; local_anchor2 = cparams.local_anchor2; - axis1 = rb1.predicted_position * cparams.local_axis1; + axis1 = rb1.next_position * cparams.local_axis1; local_axis2 = cparams.local_axis2; basis1 = [ - rb1.predicted_position * cparams.basis1[0], - rb1.predicted_position * cparams.basis1[1], + rb1.next_position * cparams.basis1[0], + rb1.next_position * cparams.basis1[1], ]; local_basis2 = cparams.basis2; }; diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs index 99c1ec5..f32f49f 100644 --- a/src/dynamics/solver/parallel_island_solver.rs +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -277,7 +277,7 @@ impl ParallelIslandSolver { rb.linvel += dvel.linear; rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular); rb.integrate(params.dt); - positions[rb.active_set_offset] = rb.position; + positions[rb.active_set_offset] = rb.next_position; } } @@ -298,7 +298,7 @@ impl ParallelIslandSolver { let batch_size = thread.batch_size; for handle in active_bodies[thread.position_writeback_index] { let rb = &mut bodies[handle.0]; - rb.set_position_internal(positions[rb.active_set_offset]); + rb.set_next_position(positions[rb.active_set_offset]); } } }) diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs index df0e3fc..ea92c59 100644 --- a/src/dynamics/solver/position_solver.rs +++ b/src/dynamics/solver/position_solver.rs @@ -25,7 +25,7 @@ impl PositionSolver { self.positions.extend( bodies .iter_active_island(island_id) - .map(|(_, b)| b.position), + .map(|(_, b)| b.next_position), ); for _ in 0..params.max_position_iterations { @@ -39,7 +39,7 @@ impl PositionSolver { } bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { - rb.set_position_internal(self.positions[rb.active_set_offset]) + rb.set_next_position(self.positions[rb.active_set_offset]) }); } } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 236fd5a..43f8294 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -2,7 +2,7 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle}; use crate::geometry::{InteractionGroups, SAPProxyIndex, SharedShape, SolverFlags}; use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM}; use crate::parry::transformation::vhacd::VHACDParameters; -use parry::bounding_volume::AABB; +use parry::bounding_volume::{BoundingVolume, AABB}; use parry::shape::Shape; bitflags::bitflags! { @@ -62,7 +62,7 @@ pub struct Collider { pub(crate) parent: RigidBodyHandle, pub(crate) delta: Isometry, pub(crate) position: Isometry, - pub(crate) predicted_position: Isometry, + pub(crate) prev_position: Isometry, /// The friction coefficient of this collider. pub friction: Real, /// The restitution coefficient of this collider. @@ -139,11 +139,12 @@ impl Collider { self.shape.compute_aabb(&self.position) } - // 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) - // } + /// Compute the axis-aligned bounding box of this collider. + pub fn compute_swept_aabb(&self, next_position: &Isometry) -> AABB { + let aabb1 = self.shape.compute_aabb(&self.position); + let aabb2 = self.shape.compute_aabb(next_position); + aabb1.merged(&aabb2) + } /// Compute the local-space mass properties of this collider. pub fn mass_properties(&self) -> MassProperties { @@ -595,8 +596,8 @@ impl ColliderBuilder { flags, solver_flags, parent: RigidBodyHandle::invalid(), + prev_position: Isometry::identity(), position: Isometry::identity(), - predicted_position: Isometry::identity(), proxy_index: crate::INVALID_U32, collision_groups: self.collision_groups, solver_groups: self.solver_groups, diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 76b3f6d..d84639c 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -108,8 +108,8 @@ impl ColliderSet { let parent = bodies .get_mut(parent_handle) .expect("Parent rigid body not found."); + coll.prev_position = parent.position * coll.delta; coll.position = parent.position * coll.delta; - coll.predicted_position = parent.predicted_position * coll.delta; let handle = ColliderHandle(self.colliders.insert(coll)); let coll = self.colliders.get(handle.0).unwrap(); parent.add_collider(handle, &coll); diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 9c635dc..92cf57d 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -71,6 +71,14 @@ impl NarrowPhase { } } + /// The query dispatcher used by this narrow-phase to select the right collision-detection + /// algorithms depending of the shape types. + pub fn query_dispatcher( + &self, + ) -> &dyn PersistentQueryDispatcher { + &*self.query_dispatcher + } + /// The contact graph containing all contact pairs and their contact information. pub fn contact_graph(&self) -> &InteractionGraph { &self.contact_graph diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index 6255d60..5df3f6a 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -69,21 +69,18 @@ impl CollisionPipeline { // // Update kinematic bodies velocities. // bodies.foreach_active_kinematic_body_mut_internal(|_, body| { - // body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); + // body.compute_velocity_from_next_position(integration_parameters.inv_dt()); // }); // Update colliders positions and kinematic bodies positions. bodies.foreach_active_body_mut_internal(|_, rb| { - if rb.is_kinematic() { - rb.position = rb.predicted_position; - } else { - rb.update_predicted_position(0.0); - } + rb.position = rb.next_position; + rb.update_colliders_positions(colliders); for handle in &rb.colliders { let collider = &mut colliders[*handle]; + collider.prev_position = collider.position; collider.position = rb.position * collider.delta; - collider.predicted_position = rb.predicted_position * collider.delta; } }); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 1908fad..7abe634 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -3,7 +3,7 @@ use crate::counters::Counters; #[cfg(not(feature = "parallel"))] use crate::dynamics::IslandSolver; -use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; +use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet}; #[cfg(feature = "parallel")] use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; use crate::geometry::{ @@ -68,6 +68,7 @@ impl PhysicsPipeline { bodies: &mut RigidBodySet, colliders: &mut ColliderSet, joints: &mut JointSet, + ccd_solver: Option<&mut CCDSolver>, hooks: &dyn PhysicsHooks, events: &dyn EventHandler, ) { @@ -81,7 +82,7 @@ impl PhysicsPipeline { // there to determine if this kinematic body should wake-up dynamic // bodies it is touching. bodies.foreach_active_kinematic_body_mut_internal(|_, body| { - body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); + body.compute_velocity_from_next_position(integration_parameters.inv_dt()); }); self.counters.stages.collision_detection_time.start(); @@ -218,23 +219,33 @@ impl PhysicsPipeline { }); } - // Update colliders positions and kinematic bodies positions. - // FIXME: do this in the solver? + // Handle CCD + if let Some(ccd_solver) = ccd_solver { + let impacts = ccd_solver.predict_next_impacts( + integration_parameters, + bodies, + colliders, + integration_parameters.dt, + events, + ); + ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts); + } + + // Set the rigid-bodies and kinematic bodies to their final position. bodies.foreach_active_body_mut_internal(|_, rb| { if rb.is_kinematic() { - rb.position = rb.predicted_position; rb.linvel = na::zero(); rb.angvel = na::zero(); - } else { - rb.update_predicted_position(integration_parameters.dt); } + rb.position = rb.next_position; rb.update_colliders_positions(colliders); }); self.counters.stages.solver_time.pause(); bodies.modified_inactive_set.clear(); + self.counters.step_completed(); } } diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 8cc6a60..e89a03e 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -1,10 +1,9 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray, - RayIntersection, SimdQuadTree, + RayIntersection, SimdQuadTree, AABB, }; use crate::math::{Isometry, Point, Real, Vector}; -use crate::parry::motion::RigidMotion; use parry::query::details::{ IntersectionCompositeShapeShapeBestFirstVisitor, NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor, @@ -15,7 +14,7 @@ use parry::query::details::{ use parry::query::visitors::{ BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor, }; -use parry::query::{DefaultQueryDispatcher, QueryDispatcher, TOI}; +use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, TOI}; use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape}; use std::sync::Arc; @@ -95,7 +94,7 @@ impl QueryPipeline { /// Initializes an empty query pipeline with a custom `QueryDispatcher`. /// /// Use this constructor in order to use a custom `QueryDispatcher` that is - /// awary of your own user-defined shapes. + /// aware of your own user-defined shapes. pub fn with_query_dispatcher(d: D) -> Self where D: 'static + QueryDispatcher, @@ -108,11 +107,26 @@ impl QueryPipeline { } } + /// The query dispatcher used by this query pipeline for running scene queries. + pub fn query_dispatcher(&self) -> &dyn QueryDispatcher { + &*self.query_dispatcher + } + /// Update the acceleration structure on the query pipeline. - pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) { + pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) { if !self.tree_built { - let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); - self.quadtree.clear_and_rebuild(data, self.dilation_factor); + if !use_swept_aabb { + let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb())); + self.quadtree.clear_and_rebuild(data, self.dilation_factor); + } else { + let data = colliders.iter().map(|(h, co)| { + let next_position = + bodies[co.parent()].next_position * co.position_wrt_parent(); + (h, co.compute_swept_aabb(&next_position)) + }); + self.quadtree.clear_and_rebuild(data, self.dilation_factor); + } + // FIXME: uncomment this once we handle insertion/removals properly. // self.tree_built = true; return; @@ -127,10 +141,22 @@ impl QueryPipeline { } } - self.quadtree.update( - |handle| colliders[*handle].compute_aabb(), - self.dilation_factor, - ); + if !use_swept_aabb { + self.quadtree.update( + |handle| colliders[*handle].compute_aabb(), + self.dilation_factor, + ); + } else { + self.quadtree.update( + |handle| { + let co = &colliders[*handle]; + let next_position = + bodies[co.parent()].next_position * co.position_wrt_parent(); + co.compute_swept_aabb(&next_position) + }, + self.dilation_factor, + ); + } } /// Find the closest intersection between a ray and a set of collider. @@ -336,6 +362,16 @@ impl QueryPipeline { .map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1)) } + /// Finds all handles of all the colliders with an AABB intersecting the given AABB. + pub fn colliders_with_aabb_intersecting_aabb( + &self, + aabb: &AABB, + mut callback: impl FnMut(&ColliderHandle) -> bool, + ) { + let mut visitor = BoundingVolumeIntersectionsVisitor::new(aabb, &mut callback); + self.quadtree.traverse_depth_first(&mut visitor); + } + /// Casts a shape at a constant linear velocity and retrieve the first collider it hits. /// /// This is similar to ray-casting except that we are casting a whole shape instead of @@ -386,20 +422,24 @@ impl QueryPipeline { pub fn nonlinear_cast_shape( &self, colliders: &ColliderSet, - shape_motion: &dyn RigidMotion, + shape_motion: &NonlinearRigidMotion, shape: &dyn Shape, - max_toi: Real, - target_distance: Real, + start_time: Real, + end_time: Real, + stop_at_penetration: bool, groups: InteractionGroups, ) -> Option<(ColliderHandle, TOI)> { let pipeline_shape = self.as_composite_shape(colliders, groups); + let pipeline_motion = NonlinearRigidMotion::identity(); let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new( &*self.query_dispatcher, - shape_motion, + &pipeline_motion, &pipeline_shape, + shape_motion, shape, - max_toi, - target_distance, + start_time, + end_time, + stop_at_penetration, ); self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1) } diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index 29fd4fa..df31c95 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -37,7 +37,7 @@ impl Box2dWorld { joints: &JointSet, ) -> Self { let mut world = b2::World::new(&na_vec_to_b2_vec(gravity)); - world.set_continuous_physics(false); + world.set_continuous_physics(bodies.iter().any(|b| b.1.is_ccd_enabled())); let mut res = Box2dWorld { world, @@ -77,14 +77,11 @@ impl Box2dWorld { angular_velocity: body.angvel(), linear_damping, angular_damping, + bullet: body.is_ccd_enabled(), ..b2::BodyDef::new() }; let b2_handle = self.world.create_body(&def); self.rapier2box2d.insert(handle, b2_handle); - - // Collider. - let mut b2_body = self.world.body_mut(b2_handle); - b2_body.set_bullet(false /* collider.is_ccd_enabled() */); } } @@ -163,7 +160,7 @@ impl Box2dWorld { fixture_def.restitution = collider.restitution; fixture_def.friction = collider.friction; - fixture_def.density = collider.density(); + fixture_def.density = collider.density().unwrap_or(1.0); fixture_def.is_sensor = collider.is_sensor(); fixture_def.filter = b2::Filter::new(); @@ -215,8 +212,6 @@ impl Box2dWorld { } pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) { - // self.world.set_continuous_physics(world.integration_parameters.max_ccd_substeps != 0); - counters.step_started(); self.world.step( params.dt, diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index 5e75d85..2d0c806 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -4,7 +4,7 @@ use crate::{ }; use kiss3d::window::Window; use plugin::HarnessPlugin; -use rapier::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; +use rapier::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet}; use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase}; use rapier::math::Vector; use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline}; @@ -133,6 +133,7 @@ impl Harness { self.physics.broad_phase = BroadPhase::new(); self.physics.narrow_phase = NarrowPhase::new(); self.state.timestep_id = 0; + self.physics.ccd_solver = CCDSolver::new(); self.physics.query_pipeline = QueryPipeline::new(); self.physics.pipeline = PhysicsPipeline::new(); self.physics.pipeline.counters.enable(); @@ -194,13 +195,14 @@ impl Harness { &mut self.physics.bodies, &mut self.physics.colliders, &mut self.physics.joints, + Some(&mut self.physics.ccd_solver), &*self.physics.hooks, &self.event_handler, ); self.physics .query_pipeline - .update(&self.physics.bodies, &self.physics.colliders); + .update(&self.physics.bodies, &self.physics.colliders, false); for plugin in &mut self.plugins { plugin.step(&mut self.physics, &self.state) diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs index 0987e32..b89f9c8 100644 --- a/src_testbed/physics/mod.rs +++ b/src_testbed/physics/mod.rs @@ -1,5 +1,5 @@ use crossbeam::channel::Receiver; -use rapier::dynamics::{IntegrationParameters, JointSet, RigidBodySet}; +use rapier::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet}; use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase}; use rapier::math::Vector; use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline}; @@ -73,6 +73,7 @@ pub struct PhysicsState { pub bodies: RigidBodySet, pub colliders: ColliderSet, pub joints: JointSet, + pub ccd_solver: CCDSolver, pub pipeline: PhysicsPipeline, pub query_pipeline: QueryPipeline, pub integration_parameters: IntegrationParameters, @@ -88,6 +89,7 @@ impl PhysicsState { bodies: RigidBodySet::new(), colliders: ColliderSet::new(), joints: JointSet::new(), + ccd_solver: CCDSolver::new(), pipeline: PhysicsPipeline::new(), query_pipeline: QueryPipeline::new(), integration_parameters: IntegrationParameters::default(), diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index dab4aec..42449ea 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -13,8 +13,8 @@ use physx::prelude::*; use physx::scene::FrictionType; use physx::traits::Class; use physx_sys::{ - PxBitAndByte, PxConvexFlags, PxConvexMeshGeometryFlags, PxHeightFieldSample, - PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor, + FilterShaderCallbackInfo, PxBitAndByte, PxConvexFlags, PxConvexMeshGeometryFlags, + PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor, }; use rapier::counters::Counters; use rapier::dynamics::{ @@ -160,7 +160,7 @@ impl PhysxWorld { FrictionType::Patch }; - let scene_desc = SceneDescriptor { + let mut scene_desc = SceneDescriptor { gravity: gravity.into_physx(), thread_count: num_threads as u32, broad_phase_type: BroadPhaseType::AutomaticBoxPruning, @@ -169,6 +169,14 @@ impl PhysxWorld { ..SceneDescriptor::new(()) }; + let ccd_enabled = bodies.iter().any(|(_, rb)| rb.is_ccd_enabled()); + + if ccd_enabled { + scene_desc.simulation_filter_shader = + FilterShaderDescriptor::CallDefaultFirst(ccd_filter_shader); + scene_desc.flags.insert(SceneFlag::EnableCcd); + } + let mut scene: Owner = physics.create(scene_desc).unwrap(); let mut rapier2dynamic = HashMap::new(); let mut rapier2static = HashMap::new(); @@ -231,7 +239,7 @@ impl PhysxWorld { } } - // Update mass properties. + // Update mass properties and CCD flags. for (rapier_handle, actor) in rapier2dynamic.iter_mut() { let rb = &bodies[*rapier_handle]; let densities: Vec<_> = rb @@ -248,6 +256,23 @@ impl PhysxWorld { std::ptr::null(), false, ); + + if rb.is_ccd_enabled() { + physx_sys::PxRigidBody_setRigidBodyFlag_mut( + std::mem::transmute(actor.as_mut()), + RigidBodyFlag::EnableCCD as u32, + true, + ); + // physx_sys::PxRigidBody_setMinCCDAdvanceCoefficient_mut( + // std::mem::transmute(actor.as_mut()), + // 0.0, + // ); + // physx_sys::PxRigidBody_setRigidBodyFlag_mut( + // std::mem::transmute(actor.as_mut()), + // RigidBodyFlag::EnableCCDFriction as u32, + // true, + // ); + } } } @@ -699,3 +724,8 @@ impl AdvanceCallback for OnAdvance { ) { } } + +unsafe extern "C" fn ccd_filter_shader(data: *mut FilterShaderCallbackInfo) -> u16 { + (*(*data).pairFlags).mBits |= physx_sys::PxPairFlag::eDETECT_CCD_CONTACT as u16; + 0 +} diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index bc5cd6c..0463da8 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -769,11 +769,38 @@ impl Testbed { } #[cfg(feature = "dim3")] - fn handle_special_event(&mut self, window: &mut Window, _event: Event) { + fn handle_special_event(&mut self, window: &mut Window, event: Event) { + use rapier::dynamics::RigidBodyBuilder; + use rapier::geometry::ColliderBuilder; + if window.is_conrod_ui_capturing_mouse() { return; } + match event.value { + WindowEvent::Key(Key::Space, Action::Release, _) => { + let cam_pos = self.graphics.camera().view_transform().inverse(); + let forward = cam_pos * -Vector::z(); + let vel = forward * 1000.0; + + let bodies = &mut self.harness.physics.bodies; + let colliders = &mut self.harness.physics.colliders; + + let collider = ColliderBuilder::cuboid(4.0, 2.0, 0.4).density(20.0).build(); + // let collider = ColliderBuilder::ball(2.0).density(1.0).build(); + let body = RigidBodyBuilder::new_dynamic() + .position(cam_pos) + .linvel(vel.x, vel.y, vel.z) + .ccd_enabled(true) + .build(); + + let body_handle = bodies.insert(body); + colliders.insert(collider, body_handle, bodies); + self.graphics.add(window, body_handle, bodies, colliders); + } + _ => {} + } + /* match event.value { WindowEvent::MouseButton(MouseButton::Button1, Action::Press, modifier) => { @@ -1454,6 +1481,20 @@ Hashes at frame: {} fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) { for pair in nf.contact_pairs() { for manifold in &pair.manifolds { + for contact in &manifold.data.solver_contacts { + let color = if contact.dist > 0.0 { + Point3::new(0.0, 0.0, 1.0) + } else { + Point3::new(1.0, 0.0, 0.0) + }; + + let p = contact.point; + let n = manifold.data.normal; + + use crate::engine::GraphicsWindow; + window.draw_graphics_line(&p, &(p + n * 0.4), &Point3::new(0.5, 1.0, 0.5)); + } + /* for pt in manifold.contacts() { let color = if pt.dist > 0.0 { Point3::new(0.0, 0.0, 1.0) @@ -1474,6 +1515,7 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) window.draw_graphics_line(&start, &(start + n * 0.4), &Point3::new(0.5, 1.0, 0.5)); window.draw_graphics_line(&start, &end, &color); } + */ } } } -- cgit