From f74b8401ad9ef50b8cdbf1f43a2b21f6c42b0ebc Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 2 Jan 2022 14:47:40 +0100 Subject: Implement multibody joints and the new solver --- src_testbed/box2d_backend.rs | 19 +- src_testbed/harness/mod.rs | 45 +++-- src_testbed/lib.rs | 10 -- src_testbed/nphysics_backend.rs | 248 -------------------------- src_testbed/physics/mod.rs | 25 +-- src_testbed/physx_backend.rs | 383 ++++++++++++++++++++++------------------ src_testbed/testbed.rs | 382 ++++++++++++++++++++------------------- src_testbed/ui.rs | 84 +++++---- 8 files changed, 513 insertions(+), 683 deletions(-) delete mode 100644 src_testbed/nphysics_backend.rs (limited to 'src_testbed') diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs index f0bffa3..2a143fb 100644 --- a/src_testbed/box2d_backend.rs +++ b/src_testbed/box2d_backend.rs @@ -2,14 +2,12 @@ use std::collections::HashMap; use na::{Isometry2, Vector2}; use rapier::counters::Counters; -use rapier::dynamics::{ - IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, -}; +use rapier::dynamics::{ImpulseJointSet, IntegrationParameters, RigidBodyHandle, RigidBodySet}; use rapier::geometry::{Collider, ColliderSet}; use std::f32; use wrapped2d::b2; -use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef}; +// use wrapped2d::dynamics::joints::{PrismaticJointDef, RevoluteJointDef, WeldJointDef}; use wrapped2d::user_data::NoUserData; fn na_vec_to_b2_vec(v: Vector2) -> b2::Vec2 { @@ -34,7 +32,7 @@ impl Box2dWorld { gravity: Vector2, bodies: &RigidBodySet, colliders: &ColliderSet, - joints: &JointSet, + impulse_joints: &ImpulseJointSet, ) -> Self { let mut world = b2::World::new(&na_vec_to_b2_vec(gravity)); world.set_continuous_physics(bodies.iter().any(|b| b.1.is_ccd_enabled())); @@ -46,7 +44,7 @@ impl Box2dWorld { res.insert_bodies(bodies); res.insert_colliders(colliders); - res.insert_joints(joints); + res.insert_joints(impulse_joints); res } @@ -95,8 +93,9 @@ impl Box2dWorld { } } - fn insert_joints(&mut self, joints: &JointSet) { - for joint in joints.iter() { + fn insert_joints(&mut self, _impulse_joints: &ImpulseJointSet) { + /* + for joint in impulse_joints.iter() { let body_a = self.rapier2box2d[&joint.1.body1]; let body_b = self.rapier2box2d[&joint.1.body2]; @@ -154,6 +153,8 @@ impl Box2dWorld { } } } + + */ } fn create_fixture(collider: &Collider, body: &mut b2::MetaBody) { @@ -225,7 +226,7 @@ impl Box2dWorld { self.world.step( params.dt, params.max_velocity_iterations as i32, - params.max_position_iterations as i32, + params.max_stabilization_iterations as i32, ); counters.step_completed(); } diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index 0bc08d0..3358a70 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -3,7 +3,10 @@ use crate::{ TestbedGraphics, }; use plugin::HarnessPlugin; -use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet}; +use rapier::dynamics::{ + CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, + RigidBodySet, +}; use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase}; use rapier::math::Vector; use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline}; @@ -78,9 +81,14 @@ impl Harness { } } - pub fn new(bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) -> Self { + pub fn new( + bodies: RigidBodySet, + colliders: ColliderSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, + ) -> Self { let mut res = Self::new_empty(); - res.set_world(bodies, colliders, joints); + res.set_world(bodies, colliders, impulse_joints, multibody_joints); res } @@ -100,24 +108,39 @@ impl Harness { &mut self.physics } - pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) { - self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ()) + pub fn set_world( + &mut self, + bodies: RigidBodySet, + colliders: ColliderSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, + ) { + self.set_world_with_params( + bodies, + colliders, + impulse_joints, + multibody_joints, + Vector::y() * -9.81, + (), + ) } pub fn set_world_with_params( &mut self, bodies: RigidBodySet, colliders: ColliderSet, - joints: JointSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, gravity: Vector, hooks: impl PhysicsHooks + 'static, ) { // println!("Num bodies: {}", bodies.len()); - // println!("Num joints: {}", joints.len()); + // println!("Num impulse_joints: {}", impulse_joints.len()); self.physics.gravity = gravity; self.physics.bodies = bodies; self.physics.colliders = colliders; - self.physics.joints = joints; + self.physics.impulse_joints = impulse_joints; + self.physics.multibody_joints = multibody_joints; self.physics.hooks = Box::new(hooks); self.physics.islands = IslandManager::new(); @@ -162,7 +185,8 @@ impl Harness { &mut physics.narrow_phase, &mut physics.bodies, &mut physics.colliders, - &mut physics.joints, + &mut physics.impulse_joints, + &mut physics.multibody_joints, &mut physics.ccd_solver, &*physics.hooks, event_handler, @@ -179,7 +203,8 @@ impl Harness { &mut self.physics.narrow_phase, &mut self.physics.bodies, &mut self.physics.colliders, - &mut self.physics.joints, + &mut self.physics.impulse_joints, + &mut self.physics.multibody_joints, &mut self.physics.ccd_solver, &*self.physics.hooks, &self.event_handler, diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs index a3e3a00..b9a8822 100644 --- a/src_testbed/lib.rs +++ b/src_testbed/lib.rs @@ -1,12 +1,4 @@ extern crate nalgebra as na; -#[cfg(all(feature = "dim2", feature = "other-backends"))] -extern crate ncollide2d as ncollide; -#[cfg(all(feature = "dim3", feature = "other-backends"))] -extern crate ncollide3d as ncollide; -#[cfg(all(feature = "dim2", feature = "other-backends"))] -extern crate nphysics2d as nphysics; -#[cfg(all(feature = "dim3", feature = "other-backends"))] -extern crate nphysics3d as nphysics; #[cfg(feature = "dim2")] extern crate parry2d as parry; #[cfg(feature = "dim3")] @@ -37,8 +29,6 @@ mod camera2d; mod camera3d; mod graphics; pub mod harness; -#[cfg(feature = "other-backends")] -mod nphysics_backend; pub mod objects; pub mod physics; #[cfg(all(feature = "dim3", feature = "other-backends"))] diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs deleted file mode 100644 index c7d6c0e..0000000 --- a/src_testbed/nphysics_backend.rs +++ /dev/null @@ -1,248 +0,0 @@ -#[cfg(feature = "dim2")] -use ncollide::shape::ConvexPolygon; -use ncollide::shape::{Ball, Capsule, Cuboid, HeightField, ShapeHandle}; -use nphysics::force_generator::DefaultForceGeneratorSet; -use nphysics::joint::{ - DefaultJointConstraintSet, FixedConstraint, PrismaticConstraint, RevoluteConstraint, -}; -use nphysics::object::{ - BodyPartHandle, ColliderDesc, DefaultBodyHandle, DefaultBodySet, DefaultColliderSet, - RigidBodyDesc, -}; -use nphysics::world::{DefaultGeometricalWorld, DefaultMechanicalWorld}; -use rapier::counters::Counters; -use rapier::dynamics::{ - IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, -}; -use rapier::geometry::{Collider, ColliderSet}; -use rapier::math::Vector; -use std::collections::HashMap; -#[cfg(feature = "dim3")] -use {ncollide::shape::TriMesh, nphysics::joint::BallConstraint}; - -pub struct NPhysicsWorld { - rapier2nphysics: HashMap, - mechanical_world: DefaultMechanicalWorld, - geometrical_world: DefaultGeometricalWorld, - bodies: DefaultBodySet, - colliders: DefaultColliderSet, - joints: DefaultJointConstraintSet, - force_generators: DefaultForceGeneratorSet, -} - -impl NPhysicsWorld { - pub fn from_rapier( - gravity: Vector, - bodies: &RigidBodySet, - colliders: &ColliderSet, - joints: &JointSet, - ) -> Self { - let mut rapier2nphysics = HashMap::new(); - - let mechanical_world = DefaultMechanicalWorld::new(gravity); - let geometrical_world = DefaultGeometricalWorld::new(); - let mut nphysics_bodies = DefaultBodySet::new(); - let mut nphysics_colliders = DefaultColliderSet::new(); - let mut nphysics_joints = DefaultJointConstraintSet::new(); - let force_generators = DefaultForceGeneratorSet::new(); - - for (rapier_handle, rb) in bodies.iter() { - // let material = physics.create_material(rb.collider.friction, rb.collider.friction, 0.0); - let nphysics_rb = RigidBodyDesc::new().position(*rb.position()).build(); - let nphysics_rb_handle = nphysics_bodies.insert(nphysics_rb); - - rapier2nphysics.insert(rapier_handle, nphysics_rb_handle); - } - - for (_, collider) in colliders.iter() { - if let Some(parent_handle) = collider.parent() { - let parent = &bodies[parent_handle]; - let nphysics_rb_handle = rapier2nphysics[&parent_handle]; - if let Some(collider) = - nphysics_collider_from_rapier_collider(&collider, parent.is_dynamic()) - { - let nphysics_collider = collider.build(BodyPartHandle(nphysics_rb_handle, 0)); - nphysics_colliders.insert(nphysics_collider); - } else { - eprintln!("Creating shape unknown to the nphysics backend.") - } - } - } - - for joint in joints.iter() { - let b1 = BodyPartHandle(rapier2nphysics[&joint.1.body1], 0); - let b2 = BodyPartHandle(rapier2nphysics[&joint.1.body2], 0); - - match &joint.1.params { - JointParams::FixedJoint(params) => { - let c = FixedConstraint::new( - b1, - b2, - params.local_frame1.translation.vector.into(), - params.local_frame1.rotation, - params.local_frame2.translation.vector.into(), - params.local_frame2.rotation, - ); - nphysics_joints.insert(c); - } - #[cfg(feature = "dim3")] - JointParams::BallJoint(params) => { - let c = BallConstraint::new(b1, b2, params.local_anchor1, params.local_anchor2); - nphysics_joints.insert(c); - } - #[cfg(feature = "dim2")] - JointParams::BallJoint(params) => { - let c = - RevoluteConstraint::new(b1, b2, params.local_anchor1, params.local_anchor2); - nphysics_joints.insert(c); - } - #[cfg(feature = "dim3")] - JointParams::RevoluteJoint(params) => { - let c = RevoluteConstraint::new( - b1, - b2, - params.local_anchor1, - params.local_axis1, - params.local_anchor2, - params.local_axis2, - ); - nphysics_joints.insert(c); - } - JointParams::PrismaticJoint(params) => { - let mut c = PrismaticConstraint::new( - b1, - b2, - params.local_anchor1, - params.local_axis1(), - params.local_anchor2, - ); - - if params.limits_enabled { - c.enable_min_offset(params.limits[0]); - c.enable_max_offset(params.limits[1]); - } - - nphysics_joints.insert(c); - } // JointParams::GenericJoint(_) => { - // eprintln!( - // "Joint type currently unsupported by the nphysics backend: GenericJoint." - // ) - // } - } - } - - Self { - rapier2nphysics, - mechanical_world, - geometrical_world, - bodies: nphysics_bodies, - colliders: nphysics_colliders, - joints: nphysics_joints, - force_generators, - } - } - - pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) { - self.mechanical_world - .integration_parameters - .max_position_iterations = params.max_position_iterations; - self.mechanical_world - .integration_parameters - .max_velocity_iterations = params.max_velocity_iterations; - self.mechanical_world - .integration_parameters - .set_dt(params.dt); - self.mechanical_world.integration_parameters.warmstart_coeff = params.warmstart_coeff; - - counters.step_started(); - self.mechanical_world.step( - &mut self.geometrical_world, - &mut self.bodies, - &mut self.colliders, - &mut self.joints, - &mut self.force_generators, - ); - counters.step_completed(); - } - - pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) { - for (rapier_handle, nphysics_handle) in self.rapier2nphysics.iter() { - let rb = bodies.get_mut(*rapier_handle).unwrap(); - let ra = self.bodies.rigid_body(*nphysics_handle).unwrap(); - let pos = *ra.position(); - rb.set_position(pos, false); - - for coll_handle in rb.colliders() { - let collider = &mut colliders[*coll_handle]; - collider.set_position( - pos * collider.position_wrt_parent().copied().unwrap_or(na::one()), - ); - } - } - } -} - -fn nphysics_collider_from_rapier_collider( - collider: &Collider, - is_dynamic: bool, -) -> Option> { - let mut margin = ColliderDesc::::default_margin(); - let mut pos = collider.position_wrt_parent().copied().unwrap_or(na::one()); - let shape = collider.shape(); - - let shape = if let Some(cuboid) = shape.as_cuboid() { - ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin))) - } else if let Some(cuboid) = shape.as_round_cuboid() { - margin = cuboid.border_radius; - ShapeHandle::new(Cuboid::new(cuboid.base_shape.half_extents)) - } else if let Some(ball) = shape.as_ball() { - ShapeHandle::new(Ball::new(ball.radius - margin)) - } else if let Some(capsule) = shape.as_capsule() { - pos *= capsule.transform_wrt_y(); - ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius)) - } else if let Some(heightfield) = shape.as_heightfield() { - let heights = heightfield.heights(); - let scale = heightfield.scale(); - let heightfield = HeightField::new(heights.clone(), *scale); - ShapeHandle::new(heightfield) - } else { - #[cfg(feature = "dim3")] - if let Some(trimesh) = shape.as_trimesh() { - ShapeHandle::new(TriMesh::new( - trimesh.vertices().to_vec(), - trimesh - .indices() - .iter() - .map(|idx| na::point![idx[0] as usize, idx[1] as usize, idx[2] as usize]) - .collect(), - None, - )) - } else { - return None; - } - - #[cfg(feature = "dim2")] - if let Some(polygon) = shape.as_round_convex_polygon() { - margin = polygon.border_radius; - ShapeHandle::new(ConvexPolygon::try_from_points(polygon.base_shape.points()).unwrap()) - } else if let Some(polygon) = shape.as_convex_polygon() { - ShapeHandle::new(ConvexPolygon::try_from_points(polygon.points()).unwrap()) - } else { - return None; - } - }; - - let density = if is_dynamic { - collider.density().unwrap_or(0.0) - } else { - 0.0 - }; - - Some( - ColliderDesc::new(shape) - .position(pos) - .density(density) - .sensor(collider.is_sensor()) - .margin(margin), - ) -} diff --git a/src_testbed/physics/mod.rs b/src_testbed/physics/mod.rs index 29e9a46..13bf915 100644 --- a/src_testbed/physics/mod.rs +++ b/src_testbed/physics/mod.rs @@ -1,5 +1,8 @@ use crossbeam::channel::Receiver; -use rapier::dynamics::{CCDSolver, IntegrationParameters, IslandManager, JointSet, RigidBodySet}; +use rapier::dynamics::{ + CCDSolver, ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet, + RigidBodySet, +}; use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase}; use rapier::math::Vector; use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline}; @@ -10,7 +13,7 @@ pub struct PhysicsSnapshot { narrow_phase: Vec, bodies: Vec, colliders: Vec, - joints: Vec, + impulse_joints: Vec, } impl PhysicsSnapshot { @@ -20,7 +23,7 @@ impl PhysicsSnapshot { narrow_phase: &NarrowPhase, bodies: &RigidBodySet, colliders: &ColliderSet, - joints: &JointSet, + impulse_joints: &ImpulseJointSet, ) -> bincode::Result { Ok(Self { timestep_id, @@ -28,7 +31,7 @@ impl PhysicsSnapshot { narrow_phase: bincode::serialize(narrow_phase)?, bodies: bincode::serialize(bodies)?, colliders: bincode::serialize(colliders)?, - joints: bincode::serialize(joints)?, + impulse_joints: bincode::serialize(impulse_joints)?, }) } @@ -40,7 +43,7 @@ impl PhysicsSnapshot { NarrowPhase, RigidBodySet, ColliderSet, - JointSet, + ImpulseJointSet, )> { Ok(( self.timestep_id, @@ -48,7 +51,7 @@ impl PhysicsSnapshot { bincode::deserialize(&self.narrow_phase)?, bincode::deserialize(&self.bodies)?, bincode::deserialize(&self.colliders)?, - bincode::deserialize(&self.joints)?, + bincode::deserialize(&self.impulse_joints)?, )) } @@ -57,13 +60,13 @@ impl PhysicsSnapshot { + self.narrow_phase.len() + self.bodies.len() + self.colliders.len() - + self.joints.len(); + + self.impulse_joints.len(); println!("Snapshot length: {}B", total); println!("|_ broad_phase: {}B", self.broad_phase.len()); println!("|_ narrow_phase: {}B", self.narrow_phase.len()); println!("|_ bodies: {}B", self.bodies.len()); println!("|_ colliders: {}B", self.colliders.len()); - println!("|_ joints: {}B", self.joints.len()); + println!("|_ impulse_joints: {}B", self.impulse_joints.len()); } } @@ -73,7 +76,8 @@ pub struct PhysicsState { pub narrow_phase: NarrowPhase, pub bodies: RigidBodySet, pub colliders: ColliderSet, - pub joints: JointSet, + pub impulse_joints: ImpulseJointSet, + pub multibody_joints: MultibodyJointSet, pub ccd_solver: CCDSolver, pub pipeline: PhysicsPipeline, pub query_pipeline: QueryPipeline, @@ -90,7 +94,8 @@ impl PhysicsState { narrow_phase: NarrowPhase::new(), bodies: RigidBodySet::new(), colliders: ColliderSet::new(), - joints: JointSet::new(), + impulse_joints: ImpulseJointSet::new(), + multibody_joints: MultibodyJointSet::new(), ccd_solver: CCDSolver::new(), pipeline: PhysicsPipeline::new(), query_pipeline: QueryPipeline::new(), diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 6c3155f..9eec3ff 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -1,9 +1,7 @@ #![allow(dead_code)] -use na::{ - Isometry3, Matrix3, Matrix4, Point3, Quaternion, Rotation3, Translation3, Unit, UnitQuaternion, - Vector3, -}; +use na::{Isometry3, Matrix4, Point3, Quaternion, Translation3, Unit, UnitQuaternion, Vector3}; +use physx::articulation_joint_base::JointMap; use physx::cooking::{ ConvexMeshCookingResult, PxConvexMeshDesc, PxCooking, PxCookingParams, PxHeightFieldDesc, PxTriangleMeshDesc, TriangleMeshCookingResult, @@ -13,15 +11,16 @@ use physx::prelude::*; use physx::scene::FrictionType; use physx::traits::Class; use physx_sys::{ - FilterShaderCallbackInfo, PxBitAndByte, PxConvexFlags, PxConvexMeshGeometryFlags, - PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new, PxRigidActor, + FilterShaderCallbackInfo, PxArticulationLink_getInboundJoint, PxBitAndByte, PxConvexFlags, + PxConvexMeshGeometryFlags, PxHeightFieldSample, PxMeshGeometryFlags, PxMeshScale_new, + PxRigidActor, }; use rapier::counters::Counters; use rapier::dynamics::{ - IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, + ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyHandle, RigidBodySet, }; use rapier::geometry::{Collider, ColliderSet}; -use rapier::utils::WBasis; +use rapier::prelude::JointAxesMask; use std::collections::HashMap; trait IntoNa { @@ -145,7 +144,8 @@ impl PhysxWorld { integration_parameters: &IntegrationParameters, bodies: &RigidBodySet, colliders: &ColliderSet, - joints: &JointSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, use_two_friction_directions: bool, num_threads: usize, ) -> Self { @@ -181,6 +181,7 @@ impl PhysxWorld { let mut scene: Owner = physics.create(scene_desc).unwrap(); let mut rapier2dynamic = HashMap::new(); let mut rapier2static = HashMap::new(); + let mut rapier2link = HashMap::new(); let cooking_params = PxCookingParams::new(&*physics).expect("Failed to init PhysX cooking."); let mut cooking = PxCooking::new(physics.foundation_mut(), &cooking_params) @@ -192,6 +193,10 @@ impl PhysxWorld { * */ for (rapier_handle, rb) in bodies.iter() { + if multibody_joints.rigid_body_link(rapier_handle).is_some() { + continue; + }; + let pos = rb.position().into_physx(); if rb.is_dynamic() { let mut actor = physics.create_dynamic(&pos, rapier_handle).unwrap(); @@ -200,8 +205,10 @@ impl PhysxWorld { actor.set_linear_velocity(&linvel, true); actor.set_angular_velocity(&angvel, true); actor.set_solver_iteration_counts( - integration_parameters.max_position_iterations as u32, - integration_parameters.max_velocity_iterations as u32, + // Use our number of velocity iterations as their number of position iterations. + integration_parameters.max_velocity_iterations.max(1) as u32, + // Use our number of velocity stabilization iterations as their number of velocity iterations. + integration_parameters.max_stabilization_iterations.max(1) as u32, ); rapier2dynamic.insert(rapier_handle, actor); @@ -211,6 +218,79 @@ impl PhysxWorld { } } + /* + * Articulations. + */ + for multibody in multibody_joints.multibodies() { + let mut articulation: Owner = + physics.create_articulation_reduced_coordinate(()).unwrap(); + let mut parent = None; + + for link in multibody.links() { + let is_root = parent.is_none(); + let rb_handle = link.rigid_body_handle(); + let rb = bodies.get(rb_handle).unwrap(); + + if is_root && rb.is_static() { + articulation.set_articulation_flag(ArticulationFlag::FixBase, true); + } + + let link_pose = rb.position().into_physx(); + let px_link = articulation + .create_link(parent.take(), &link_pose, rb_handle) + .unwrap(); + + // TODO: there is no get_inbound_joint_mut? + if let Some(px_inbound_joint) = unsafe { + (PxArticulationLink_getInboundJoint(px_link.as_ptr()) + as *mut physx_sys::PxArticulationJointBase + as *mut JointMap) + .as_mut() + } { + let frame1 = link.joint().data.local_frame1.into_physx(); + let frame2 = link.joint().data.local_frame2.into_physx(); + + px_inbound_joint.set_parent_pose(&frame1); + px_inbound_joint.set_child_pose(&frame2); + + /* + + let px_joint = px_inbound_joint + .as_articulation_joint_reduced_coordinate() + .unwrap(); + + if let Some(_) = link + .articulation() + .downcast_ref::() + { + px_joint.set_joint_type(ArticulationJointType::Spherical); + px_joint.set_motion(ArticulationAxis::Swing1, ArticulationMotion::Free); + px_joint.set_motion(ArticulationAxis::Swing2, ArticulationMotion::Free); + px_joint.set_motion(ArticulationAxis::Twist, ArticulationMotion::Free); + } else if let Some(_) = + link.articulation().downcast_ref::() + { + px_joint.set_joint_type(ArticulationJointType::Revolute); + px_joint.set_motion(ArticulationAxis::Swing1, ArticulationMotion::Free); + px_joint.set_motion(ArticulationAxis::Swing2, ArticulationMotion::Free); + px_joint.set_motion(ArticulationAxis::Twist, ArticulationMotion::Free); + } + + */ + } + + // FIXME: we are using transmute here in order to erase the lifetime of + // the &mut ref behind px_link (which is tied to the lifetime of the + // multibody_joint). This looks necessary because we need + // that mutable ref to create the next link. Yet, the link creation + // methods also requires a mutable ref to the multibody_joint. + rapier2link.insert(rb_handle, px_link as *mut PxArticulationLink); + parent = Some(unsafe { std::mem::transmute(px_link as *mut _) }); + } + + scene.add_articulation(articulation); + } + /* * * Colliders @@ -223,7 +303,15 @@ impl PhysxWorld { if let Some(parent_handle) = collider.parent() { let parent_body = &bodies[parent_handle]; - if !parent_body.is_dynamic() { + if let Some(link) = rapier2link.get_mut(&parent_handle) { + unsafe { + physx_sys::PxRigidActor_attachShape_mut( + *link as *mut PxRigidActor, + px_shape.as_mut_ptr(), + ); + } + } else if !parent_body.is_dynamic() { + println!("Ground collider"); let actor = rapier2static.get_mut(&parent_handle).unwrap(); actor.attach_shape(&mut px_shape); } else { @@ -246,8 +334,8 @@ impl PhysxWorld { } // Update mass properties and CCD flags. - for (rapier_handle, actor) in rapier2dynamic.iter_mut() { - let rb = &bodies[*rapier_handle]; + for (rapier_handle, _rb) in bodies.iter() { + let rb = &bodies[rapier_handle]; let densities: Vec<_> = rb .colliders() .iter() @@ -255,8 +343,16 @@ impl PhysxWorld { .collect(); unsafe { + let actor = if let Some(actor) = rapier2dynamic.get_mut(&rapier_handle) { + std::mem::transmute(actor.as_mut()) + } else if let Some(actor) = rapier2link.get_mut(&rapier_handle) { + *actor as *mut _ + } else { + continue; + }; + physx_sys::PxRigidBodyExt_updateMassAndInertia_mut( - std::mem::transmute(actor.as_mut()), + actor, densities.as_ptr(), densities.len() as u32, std::ptr::null(), @@ -265,19 +361,10 @@ impl PhysxWorld { if rb.is_ccd_enabled() { physx_sys::PxRigidBody_setRigidBodyFlag_mut( - std::mem::transmute(actor.as_mut()), + actor, 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, - // ); } } } @@ -289,9 +376,10 @@ impl PhysxWorld { */ Self::setup_joints( &mut physics, - joints, + impulse_joints, &mut rapier2static, &mut rapier2dynamic, + &mut rapier2link, ); for (_, actor) in rapier2static { @@ -312,18 +400,22 @@ impl PhysxWorld { fn setup_joints( physics: &mut PxPhysicsFoundation, - joints: &JointSet, + impulse_joints: &ImpulseJointSet, rapier2static: &mut HashMap>, rapier2dynamic: &mut HashMap>, + rapier2link: &mut HashMap, ) { unsafe { - for joint in joints.iter() { + for joint in impulse_joints.iter() { let actor1 = rapier2static .get_mut(&joint.1.body1) .map(|act| &mut **act as *mut PxRigidStatic as *mut PxRigidActor) .or(rapier2dynamic .get_mut(&joint.1.body1) .map(|act| &mut **act as *mut PxRigidDynamic as *mut PxRigidActor)) + .or(rapier2link + .get_mut(&joint.1.body1) + .map(|lnk| *lnk as *mut PxRigidActor)) .unwrap(); let actor2 = rapier2static .get_mut(&joint.1.body2) @@ -331,150 +423,83 @@ impl PhysxWorld { .or(rapier2dynamic .get_mut(&joint.1.body2) .map(|act| &mut **act as *mut PxRigidDynamic as *mut PxRigidActor)) + .or(rapier2link + .get_mut(&joint.1.body2) + .map(|lnk| *lnk as *mut PxRigidActor)) .unwrap(); - match &joint.1.params { - JointParams::BallJoint(params) => { - let frame1 = Isometry3::new(params.local_anchor1.coords, na::zero()) - .into_physx() - .into(); - let frame2 = Isometry3::new(params.local_anchor2.coords, na::zero()) - .into_physx() - .into(); - - physx_sys::phys_PxSphericalJointCreate( - physics.as_mut_ptr(), - actor1, - &frame1 as *const _, - actor2, - &frame2 as *const _, - ); - } - JointParams::RevoluteJoint(params) => { - // NOTE: orthonormal_basis() returns the two basis vectors. - // However we only use one and recompute the other just to - // make sure our basis is right-handed. - let basis1a = params.local_axis1.orthonormal_basis()[0]; - let basis2a = params.local_axis2.orthonormal_basis()[0]; - let basis1b = params.local_axis1.cross(&basis1a); - let basis2b = params.local_axis2.cross(&basis2a); - - let rotmat1 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[ - params.local_axis1.into_inner(), - basis1a, - basis1b, - ])); - let rotmat2 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[ - params.local_axis2.into_inner(), - basis2a, - basis2b, - ])); - let axisangle1 = rotmat1.scaled_axis(); - let axisangle2 = rotmat2.scaled_axis(); - - let frame1 = Isometry3::new(params.local_anchor1.coords, axisangle1) - .into_physx() - .into(); - let frame2 = Isometry3::new(params.local_anchor2.coords, axisangle2) - .into_physx() - .into(); - - let revolute_joint = physx_sys::phys_PxRevoluteJointCreate( - physics.as_mut_ptr(), - actor1, - &frame1 as *const _, - actor2, - &frame2 as *const _, - ); - - physx_sys::PxRevoluteJoint_setDriveVelocity_mut( - revolute_joint, - params.motor_target_vel, - true, - ); + let px_frame1 = joint.1.data.local_frame1.into_physx(); + let px_frame2 = joint.1.data.local_frame2.into_physx(); - if params.motor_damping != 0.0 { - physx_sys::PxRevoluteJoint_setRevoluteJointFlag_mut( - revolute_joint, - physx_sys::PxRevoluteJointFlag::eDRIVE_ENABLED, - true, - ); - } - } - - JointParams::PrismaticJoint(params) => { - // NOTE: orthonormal_basis() returns the two basis vectors. - // However we only use one and recompute the other just to - // make sure our basis is right-handed. - let basis1a = params.local_axis1().orthonormal_basis()[0]; - let basis2a = params.local_axis2().orthonormal_basis()[0]; - let basis1b = params.local_axis1().cross(&basis1a); - let basis2b = params.local_axis2().cross(&basis2a); - - let rotmat1 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[ - params.local_axis1().into_inner(), - basis1a, - basis1b, - ])); - let rotmat2 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[ - params.local_axis2().into_inner(), - basis2a, - basis2b, - ])); - - let axisangle1 = rotmat1.scaled_axis(); - let axisangle2 = rotmat2.scaled_axis(); - - let frame1 = Isometry3::new(params.local_anchor1.coords, axisangle1) - .into_physx() - .into(); - let frame2 = Isometry3::new(params.local_anchor2.coords, axisangle2) - .into_physx() - .into(); - - let joint = physx_sys::phys_PxPrismaticJointCreate( - physics.as_mut_ptr(), - actor1, - &frame1 as *const _, - actor2, - &frame2 as *const _, - ); + let px_joint = physx_sys::phys_PxD6JointCreate( + physics.as_mut_ptr(), + actor1, + px_frame1.as_ptr(), + actor2, + px_frame2.as_ptr(), + ); - if params.limits_enabled { - let limits = physx_sys::PxJointLinearLimitPair { - restitution: 0.0, - bounceThreshold: 0.0, - stiffness: 0.0, - damping: 0.0, - contactDistance: 0.01, - lower: params.limits[0], - upper: params.limits[1], - }; - physx_sys::PxPrismaticJoint_setLimit_mut(joint, &limits); - physx_sys::PxPrismaticJoint_setPrismaticJointFlag_mut( - joint, - physx_sys::PxPrismaticJointFlag::eLIMIT_ENABLED, - true, - ); - } - } - JointParams::FixedJoint(params) => { - let frame1 = params.local_frame1.into_physx().into(); - let frame2 = params.local_frame2.into_physx().into(); - - physx_sys::phys_PxFixedJointCreate( - physics.as_mut_ptr(), - actor1, - &frame1 as *const _, - actor2, - &frame2 as *const _, - ); - } // JointParams::GenericJoint(_) => { - // eprintln!( - // "Joint type currently unsupported by the PhysX backend: GenericJoint." - // ) - // } - } + let motion_x = if joint.1.data.limit_axes.contains(JointAxesMask::X) { + physx_sys::PxD6Motion::eLIMITED + } else if !joint.1.data.locked_axes.contains(JointAxesMask::X) { + physx_sys::PxD6Motion::eFREE + } else { + physx_sys::PxD6Motion::eLOCKED + }; + let motion_y = if joint.1.data.limit_axes.contains(JointAxesMask::Y) { + physx_sys::PxD6Motion::eLIMITED + } else if !joint.1.data.locked_axes.contains(JointAxesMask::Y) { + physx_sys::PxD6Motion::eFREE + } else { + physx_sys::PxD6Motion::eLOCKED + }; + let motion_z = if joint.1.data.limit_axes.contains(JointAxesMask::Z) { + physx_sys::PxD6Motion::eLIMITED + } else if !joint.1.data.locked_axes.contains(JointAxesMask::Z) { + physx_sys::PxD6Motion::eFREE + } else { + physx_sys::PxD6Motion::eLOCKED + }; + let motion_ax = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_X) { + physx_sys::PxD6Motion::eLIMITED + } else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_X) { + physx_sys::PxD6Motion::eFREE + } else { + physx_sys::PxD6Motion::eLOCKED + }; + let motion_ay = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_Y) { + physx_sys::PxD6Motion::eLIMITED + } else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_Y) { + physx_sys::PxD6Motion::eFREE + } else { + physx_sys::PxD6Motion::eLOCKED + }; + let motion_az = if joint.1.data.limit_axes.contains(JointAxesMask::ANG_Z) { + physx_sys::PxD6Motion::eLIMITED + } else if !joint.1.data.locked_axes.contains(JointAxesMask::ANG_Z) { + physx_sys::PxD6Motion::eFREE + } else { + physx_sys::PxD6Motion::eLOCKED + }; + + physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eX, motion_x); + physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eY, motion_y); + physx_sys::PxD6Joint_setMotion_mut(px_joint, physx_sys::PxD6Axis::eZ, motion_z); + physx_sys::PxD6Joint_setMotion_mut( + px_joint, + physx_sys::PxD6Axis::eTWIST, + motion_ax, + ); + physx_sys::PxD6Joint_setMotion_mut( + px_joint, + physx_sys::PxD6Axis::eSWING1, + motion_ay, + ); + physx_sys::PxD6Joint_setMotion_mut( + px_joint, + physx_sys::PxD6Axis::eSWING2, + motion_az, + ); } } } @@ -497,9 +522,7 @@ impl PhysxWorld { } pub fn sync(&mut self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) { - for actor in self.scene.as_mut().unwrap().get_dynamic_actors() { - let handle = actor.get_user_data(); - let pos = actor.get_global_pose().into_na(); + let mut sync_pos = |handle: &RigidBodyHandle, pos: Isometry3| { let rb = &mut bodies[*handle]; rb.set_position(pos, false); @@ -509,6 +532,22 @@ impl PhysxWorld { pos * collider.position_wrt_parent().copied().unwrap_or(na::one()), ); } + }; + + for actor in self.scene.as_mut().unwrap().get_dynamic_actors() { + let handle = actor.get_user_data(); + let pos = actor.get_global_pose().into_na(); + sync_pos(handle, pos); + } + + for articulation in self.scene.as_mut().unwrap().get_articulations() { + if let Some(articulation) = articulation.as_articulation_reduced_coordinate() { + for link in articulation.get_links() { + let handle = link.get_user_data(); + let pos = link.get_global_pose().into_na(); + sync_pos(handle, pos); + } + } } } } @@ -673,7 +712,7 @@ fn physx_collider_from_rapier_collider( type PxPhysicsFoundation = PhysicsFoundation; type PxMaterial = physx::material::PxMaterial<()>; type PxShape = physx::shape::PxShape<(), PxMaterial>; -type PxArticulationLink = physx::articulation_link::PxArticulationLink<(), PxShape>; +type PxArticulationLink = physx::articulation_link::PxArticulationLink; type PxRigidStatic = physx::rigid_static::PxRigidStatic<(), PxShape>; type PxRigidDynamic = physx::rigid_dynamic::PxRigidDynamic; type PxArticulation = physx::articulation::PxArticulation<(), PxArticulationLink>; diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 706a40a..ac53733 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -11,7 +11,8 @@ use crate::{graphics::GraphicsManager, harness::RunState}; use na::{self, Point2, Point3, Vector3}; use rapier::dynamics::{ - IntegrationParameters, JointSet, RigidBodyActivation, RigidBodyHandle, RigidBodySet, + ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation, + RigidBodyHandle, RigidBodySet, }; use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase}; #[cfg(feature = "dim3")] @@ -22,8 +23,6 @@ use rapier::pipeline::PhysicsHooks; #[cfg(all(feature = "dim2", feature = "other-backends"))] use crate::box2d_backend::Box2dWorld; use crate::harness::Harness; -#[cfg(feature = "other-backends")] -use crate::nphysics_backend::NPhysicsWorld; #[cfg(all(feature = "dim3", feature = "other-backends"))] use crate::physx_backend::PhysxWorld; use bevy::render::camera::Camera; @@ -38,12 +37,10 @@ use crate::camera3d::{OrbitCamera, OrbitCameraPlugin}; use bevy::render::pipeline::PipelineDescriptor; const RAPIER_BACKEND: usize = 0; -#[cfg(feature = "other-backends")] -const NPHYSICS_BACKEND: usize = 1; #[cfg(all(feature = "dim2", feature = "other-backends"))] -const BOX2D_BACKEND: usize = 2; -pub(crate) const PHYSX_BACKEND_PATCH_FRICTION: usize = 2; -pub(crate) const PHYSX_BACKEND_TWO_FRICTION_DIR: usize = 3; +const BOX2D_BACKEND: usize = 1; +pub(crate) const PHYSX_BACKEND_PATCH_FRICTION: usize = 1; +pub(crate) const PHYSX_BACKEND_TWO_FRICTION_DIR: usize = 2; #[derive(PartialEq)] pub enum RunMode { @@ -128,7 +125,6 @@ struct OtherBackends { box2d: Option, #[cfg(feature = "dim3")] physx: Option, - nphysics: Option, } struct Plugins(Vec>); @@ -169,8 +165,6 @@ impl TestbedApp { #[allow(unused_mut)] let mut backend_names = vec!["rapier"]; - #[cfg(feature = "other-backends")] - backend_names.push("nphysics"); #[cfg(all(feature = "dim2", feature = "other-backends"))] backend_names.push("box2d"); #[cfg(all(feature = "dim3", feature = "other-backends"))] @@ -207,7 +201,6 @@ impl TestbedApp { box2d: None, #[cfg(feature = "dim3")] physx: None, - nphysics: None, }; TestbedApp { @@ -271,28 +264,15 @@ impl TestbedApp { for (backend_id, backend) in backend_names.iter().enumerate() { println!("|_ using backend {}", backend); self.state.selected_backend = backend_id; - if cfg!(feature = "dim3") - && (backend_id == PHYSX_BACKEND_PATCH_FRICTION - || backend_id == PHYSX_BACKEND_TWO_FRICTION_DIR) - { - self.harness - .physics - .integration_parameters - .max_velocity_iterations = 1; - self.harness - .physics - .integration_parameters - .max_position_iterations = 4; - } else { - self.harness - .physics - .integration_parameters - .max_velocity_iterations = 4; - self.harness - .physics - .integration_parameters - .max_position_iterations = 1; - } + self.harness + .physics + .integration_parameters + .max_velocity_iterations = 4; + self.harness + .physics + .integration_parameters + .max_stabilization_iterations = 1; + // Init world. let mut testbed = Testbed { graphics: None, @@ -341,20 +321,6 @@ impl TestbedApp { ); } } - - #[cfg(feature = "other-backends")] - { - if self.state.selected_backend == NPHYSICS_BACKEND { - self.other_backends.nphysics.as_mut().unwrap().step( - &mut self.harness.physics.pipeline.counters, - &self.harness.physics.integration_parameters, - ); - self.other_backends.nphysics.as_mut().unwrap().sync( - &mut self.harness.physics.bodies, - &mut self.harness.physics.colliders, - ); - } - } } // Skip the first update. @@ -498,20 +464,40 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> { &mut self.harness } - pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) { - self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ()) + pub fn set_world( + &mut self, + bodies: RigidBodySet, + colliders: ColliderSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, + ) { + self.set_world_with_params( + bodies, + colliders, + impulse_joints, + multibody_joints, + Vector::y() * -9.81, + (), + ) } pub fn set_world_with_params( &mut self, bodies: RigidBodySet, colliders: ColliderSet, - joints: JointSet, + impulse_joints: ImpulseJointSet, + multibody_joints: MultibodyJointSet, gravity: Vector, hooks: impl PhysicsHooks + 'static, ) { - self.harness - .set_world_with_params(bodies, colliders, joints, gravity, hooks); + self.harness.set_world_with_params( + bodies, + colliders, + impulse_joints, + multibody_joints, + gravity, + hooks, + ); self.state .action_flags @@ -526,7 +512,7 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> { self.harness.physics.gravity, &self.harness.physics.bodies, &self.harness.physics.colliders, - &self.harness.physics.joints, + &self.harness.physics.impulse_joints, )); } } @@ -541,24 +527,13 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> { &self.harness.physics.integration_parameters, &self.harness.physics.bodies, &self.harness.physics.colliders, - &self.harness.physics.joints, + &self.harness.physics.impulse_joints, + &self.harness.physics.multibody_joints, self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR, self.harness.state.num_threads, )); } } - - #[cfg(feature = "other-backends")] - { - if self.state.selected_backend == NPHYSICS_BACKEND { - self.other_backends.nphysics = Some(NPhysicsWorld::from_rapier( - self.harness.physics.gravity, - &self.harness.physics.bodies, - &self.harness.physics.colliders, - &self.harness.physics.joints, - )); - } - } } #[cfg(feature = "dim2")] @@ -634,85 +609,134 @@ impl<'a, 'b, 'c, 'd> Testbed<'a, 'b, 'c, 'd> { self.plugins.0.push(Box::new(plugin)); } - // fn handle_common_event<'b>(&mut self, event: Event<'b>) -> Event<'b> { - // match event.value { - // WindowEvent::Key(Key::T, Action::Release, _) => { - // if self.state.running == RunMode::Stop { - // self.state.running = RunMode::Running; - // } else { - // self.state.running = RunMode::Stop; - // } - // } - // WindowEvent::Key(Key::S, Action::Release, _) => self.state.running = RunMode::Step, - // WindowEvent::Key(Key::R, Action::Release, _) => self - // .state - // .action_flags - // .set(TestbedActionFlags::EXAMPLE_CHANGED, true), - // WindowEvent::Key(Key::C, Action::Release, _) => { - // // Delete 1 collider of 10% of the remaining dynamic bodies. - // let mut colliders: Vec<_> = self - // .harness - // .physics - // .bodies - // .iter() - // .filter(|e| e.1.is_dynamic()) - // .filter(|e| !e.1.colliders().is_empty()) - // .map(|e| e.1.colliders().to_vec()) - // .collect(); - // colliders.sort_by_key(|co| -(co.len() as isize)); - // - // let num_to_delete = (colliders.len() / 10).max(1); - // for to_delete in &colliders[..num_to_delete] { - // self.harness.physics.colliders.remove( - // to_delete[0], - // &mut self.harness.physics.islands, - // &mut self.harness.physics.bodies, - // true, - // ); - // } - // } - // WindowEvent::Key(Key::D, Action::Release, _) => { - // // Delete 10% of the remaining dynamic bodies. - // let dynamic_bodies: Vec<_> = self - // .harness - // .physics - // .bodies - // .iter() - // .filter(|e| !e.1.is_static()) - // .map(|e| e.0) - // .collect(); - // let num_to_delete = (dynamic_bodies.len() / 10).max(1); - // for to_delete in &dynamic_bodies[..num_to_delete] { - // self.harness.physics.bodies.remove( - // *to_delete, - // &mut self.harness.physics.islands, - // &mut self.harness.physics.colliders, - // &mut self.harness.physics.joints, - // ); - // } - // } - // WindowEvent::Key(Key::J, Action::Release, _) => { - // // Delete 10% of the remaining joints. - // let joints: Vec<_> = self.harness.physics.joints.iter().map(|e| e.0).collect(); - // let num_to_delete = (joints.len() / 10).max(1); - // for to_delete in &joints[..num_to_delete] { - // self.harness.physics.joints.remove( - // *to_delete, - // &mut self.harness.physics.islands, - // &mut self.harness.physics.bodies, - // true, - // ); - // } - // } - // WindowEvent::CursorPos(x, y, _) => { - // self.state.cursor_pos.x = x as f32; - // self.state.cursor_pos.y = y as f32; - // } - // _ => {} - // } - // - // event - // } + fn handle_common_events(&mut self, events: &Input) { + for key in events.get_just_released() { + match *key { + KeyCode::T => { + if self.state.running == RunMode::Stop { + self.state.running = RunMode::Running; + } else { + self.state.running = RunMode::Stop; + } + } + KeyCode::S => self.state.running = RunMode::Step, + KeyCode::R => self + .state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, true), + KeyCode::C => { + // Delete 1 collider of 10% of the remaining dynamic bodies. + let mut colliders: Vec<_> = self + .harness + .physics + .bodies + .iter() + .filter(|e| e.1.is_dynamic()) + .filter(|e| !e.1.colliders().is_empty()) + .map(|e| e.1.colliders().to_vec()) + .collect(); + colliders.sort_by_key(|co| -(co.len() as isize)); + + let num_to_delete = (colliders.len() / 10).max(1); + for to_delete in &colliders[..num_to_delete] { + if let Some(graphics) = self.graphics.as_mut() { + graphics.remove_collider(to_delete[0], &self.harness.physics.colliders); + } + self.harness.physics.colliders.remove( + to_delete[0], + &mut self.harness.physics.islands, + &mut self.harness.physics.bodies, + true, + ); + } + } + KeyCode::D => { + // Delete 10% of the remaining dynamic bodies. + let dynamic_bodies: Vec<_> = self + .harness + .physics + .bodies + .iter() + .filter(|e| !e.1.is_static()) + .map(|e| e.0) + .collect(); + let num_to_delete = (dynamic_bodies.len() / 10).max(1); + for to_delete in &dynamic_bodies[..num_to_delete] { + if let Some(graphics) = self.graphics.as_mut() { + graphics.remove_body(*to_delete); + } + self.harness.physics.bodies.remove( + *to_delete, + &mut self.harness.physics.islands, + &mut self.harness.physics.colliders, + &mut self.harness.physics.impulse_joints, + &mut self.harness.physics.multibody_joints, + ); + } + } + KeyCode::J => { + // Delete 10% of the remaining impulse_joints. + let impulse_joints: Vec<_> = self + .harness + .physics + .impulse_joints + .iter() + .map(|e| e.0) + .collect(); + let num_to_delete = (impulse_joints.len() / 10).max(1); + for to_delete in &impulse_joints[..num_to_delete] { + self.harness.physics.impulse_joints.remove( + *to_delete, + &mut self.harness.physics.islands, + &mut self.harness.physics.bodies, + true, + ); + } + } + KeyCode::A => { + // Delete 10% of the remaining multibody_joints. + let multibody_joints: Vec<_> = self + .harness + .physics + .multibody_joints + .iter() + .map(|e| e.0) + .collect(); + let num_to_delete = (multibody_joints.len() / 10).max(1); + for to_delete in &multibody_joints[..num_to_delete] { + self.harness.physics.multibody_joints.remove( + *to_delete, + &mut self.harness.physics.islands, + &mut self.harness.physics.bodies, + true, + ); + } + } + KeyCode::M => { + // Delete one remaining multibody. + let to_delete = self + .harness + .physics + .multibody_joints + .iter() + .next() + .map(|a| a.2.rigid_body_handle()); + if let Some(to_delete) = to_delete { + self.harness + .physics + .multibody_joints + .remove_multibody_articulations( + to_delete, + &mut self.harness.physics.islands, + &mut self.harness.physics.bodies, + true, + ); + } + } + _ => {} + } + } + } // #[cfg(feature = "dim2")] // fn handle_special_event(&mut self) {} @@ -878,11 +902,37 @@ fn update_testbed( ui_context: Res, mut gfx_components: Query<(&mut Transform,)>, mut cameras: Query<(&Camera, &GlobalTransform, &mut OrbitCamera)>, + keys: Res>, ) { let meshes = &mut *meshes; let materials = &mut *materials; let prev_example = state.selected_example; + // Handle inputs + { + let graphics_context = TestbedGraphics { + pipelines: &mut *pipelines, + shaders: &mut *shaders, + graphics: &mut *graphics, + commands: &mut commands, + meshes: &mut *meshes, + materials: &mut *materials,