diff --git a/CHANGELOG.md b/CHANGELOG.md index c4516ba..e46ad40 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,11 @@ +## Unreleased +### Modified +- Add a `wake_up: bool` argument to the `ImpulseJointSet::insert` and `MultibodyJointSet::insert` to + automatically wake-up the rigid-bodies attached to the inserted joint. +- The methods `ImpulseJointSet::remove/remove_joints_attached_to_rigid_body`, + `MultibodyJointSet::remove/remove_joints_attached_to_rigid_body` and + `MultibodyjointSet::remove_multibody_articulations` no longer require the `bodies` + and `islands` arguments. ## v0.12.0 (30 Apr. 2022) ### Fixed diff --git a/benchmarks2d/joint_ball2.rs b/benchmarks2d/joint_ball2.rs index 9fd61ff..1988274 100644 --- a/benchmarks2d/joint_ball2.rs +++ b/benchmarks2d/joint_ball2.rs @@ -42,7 +42,7 @@ pub fn init_world(testbed: &mut Testbed) { if i > 0 { let parent_handle = *body_handles.last().unwrap(); let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } // Horizontal joint. @@ -50,7 +50,7 @@ pub fn init_world(testbed: &mut Testbed) { let parent_index = body_handles.len() - numi; let parent_handle = body_handles[parent_index]; let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } body_handles.push(child_handle); diff --git a/benchmarks2d/joint_fixed2.rs b/benchmarks2d/joint_fixed2.rs index 36ff62e..c4cabcb 100644 --- a/benchmarks2d/joint_fixed2.rs +++ b/benchmarks2d/joint_fixed2.rs @@ -48,7 +48,7 @@ pub fn init_world(testbed: &mut Testbed) { let parent_handle = *body_handles.last().unwrap(); let joint = FixedJointBuilder::new() .local_frame2(Isometry::translation(0.0, shift)); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } // Horizontal joint. @@ -57,7 +57,7 @@ pub fn init_world(testbed: &mut Testbed) { let parent_handle = body_handles[parent_index]; let joint = FixedJointBuilder::new() .local_frame2(Isometry::translation(-shift, 0.0)); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } body_handles.push(child_handle); diff --git a/benchmarks2d/joint_prismatic2.rs b/benchmarks2d/joint_prismatic2.rs index d4d328b..676a433 100644 --- a/benchmarks2d/joint_prismatic2.rs +++ b/benchmarks2d/joint_prismatic2.rs @@ -46,7 +46,7 @@ pub fn init_world(testbed: &mut Testbed) { let prism = PrismaticJointBuilder::new(axis) .local_anchor2(point![0.0, shift]) .limits([-1.5, 1.5]); - impulse_joints.insert(curr_parent, curr_child, prism); + impulse_joints.insert(curr_parent, curr_child, prism, true); curr_parent = curr_child; } diff --git a/benchmarks3d/joint_ball3.rs b/benchmarks3d/joint_ball3.rs index f137c69..a968138 100644 --- a/benchmarks3d/joint_ball3.rs +++ b/benchmarks3d/joint_ball3.rs @@ -37,7 +37,7 @@ pub fn init_world(testbed: &mut Testbed) { if i > 0 { let parent_handle = *body_handles.last().unwrap(); let joint = SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } // Horizontal joint. @@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } body_handles.push(child_handle); diff --git a/benchmarks3d/joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs index b15515c..c949857 100644 --- a/benchmarks3d/joint_fixed3.rs +++ b/benchmarks3d/joint_fixed3.rs @@ -54,7 +54,7 @@ pub fn init_world(testbed: &mut Testbed) { let parent_handle = *body_handles.last().unwrap(); let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } // Horizontal joint. @@ -63,7 +63,7 @@ pub fn init_world(testbed: &mut Testbed) { let parent_handle = body_handles[parent_index]; let joint = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } body_handles.push(child_handle); diff --git a/benchmarks3d/joint_prismatic3.rs b/benchmarks3d/joint_prismatic3.rs index a97dc56..78953e4 100644 --- a/benchmarks3d/joint_prismatic3.rs +++ b/benchmarks3d/joint_prismatic3.rs @@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) { let prism = PrismaticJointBuilder::new(axis) .local_anchor2(point![0.0, 0.0, -shift]) .limits([-2.0, 0.0]); - impulse_joints.insert(curr_parent, curr_child, prism); + impulse_joints.insert(curr_parent, curr_child, prism, true); curr_parent = curr_child; } diff --git a/benchmarks3d/joint_revolute3.rs b/benchmarks3d/joint_revolute3.rs index 779248a..d434515 100644 --- a/benchmarks3d/joint_revolute3.rs +++ b/benchmarks3d/joint_revolute3.rs @@ -55,10 +55,10 @@ pub fn init_world(testbed: &mut Testbed) { RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]), ]; - impulse_joints.insert(curr_parent, handles[0], revs[0]); - impulse_joints.insert(handles[0], handles[1], revs[1]); - impulse_joints.insert(handles[1], handles[2], revs[2]); - impulse_joints.insert(handles[2], handles[3], revs[3]); + impulse_joints.insert(curr_parent, handles[0], revs[0], true); + impulse_joints.insert(handles[0], handles[1], revs[1], true); + impulse_joints.insert(handles[1], handles[2], revs[2], true); + impulse_joints.insert(handles[2], handles[3], revs[3], true); curr_parent = handles[3]; } diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs index 31fda33..6162589 100644 --- a/examples2d/joints2.rs +++ b/examples2d/joints2.rs @@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) { if i > 0 { let parent_handle = *body_handles.last().unwrap(); let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } // Horizontal joint. @@ -53,7 +53,7 @@ pub fn init_world(testbed: &mut Testbed) { let parent_index = body_handles.len() - numi; let parent_handle = body_handles[parent_index]; let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } body_handles.push(child_handle); diff --git a/examples3d/debug_articulations3.rs b/examples3d/debug_articulations3.rs index e844630..c7f6cd3 100644 --- a/examples3d/debug_articulations3.rs +++ b/examples3d/debug_articulations3.rs @@ -39,7 +39,7 @@ fn create_ball_articulations( let parent_handle = *body_handles.last().unwrap(); let joint = SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); - multibody_joints.insert(parent_handle, child_handle, joint); + multibody_joints.insert(parent_handle, child_handle, joint, true); } // Horizontal multibody_joint. @@ -52,7 +52,7 @@ fn create_ball_articulations( // let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); // let joint = // RevoluteJoint::new(Vector::x_axis()).local_anchor2(point![-shift, 0.0, 0.0]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } body_handles.push(child_handle); diff --git a/examples3d/debug_excentric_boxes3.rs b/examples3d/debug_excentric_boxes3.rs new file mode 100644 index 0000000..938de91 --- /dev/null +++ b/examples3d/debug_excentric_boxes3.rs @@ -0,0 +1,44 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground_size = 100.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); + colliders.insert_with_parent(collider, handle, &mut bodies); + + // Build the dynamic box rigid body. + let (mut vtx, idx) = Cuboid::new(vector![1.0, 1.0, 1.0]).to_trimesh(); + vtx.iter_mut() + .for_each(|pt| *pt += vector![100.0, 100.0, 100.0]); + let shape = SharedShape::convex_mesh(vtx, &idx).unwrap(); + + for _ in 0..2 { + let rigid_body = RigidBodyBuilder::dynamic() + .translation(vector![-100.0, -100.0 + 10.0, -100.0]) + .can_sleep(false); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::new(shape.clone()); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![10.0, 10.0, 10.0], Point::origin()); +} diff --git a/examples3d/debug_prismatic3.rs b/examples3d/debug_prismatic3.rs index 815f4e5..776c50c 100644 --- a/examples3d/debug_prismatic3.rs +++ b/examples3d/debug_prismatic3.rs @@ -36,7 +36,7 @@ fn prismatic_repro( let prismatic = PrismaticJointBuilder::new(Vector::y_axis()) .local_anchor1(point![pos.x, pos.y, pos.z]) .motor_position(0.0, stiffness, damping); - impulse_joints.insert(box_rb, wheel_rb, prismatic); + impulse_joints.insert(box_rb, wheel_rb, prismatic, true); } // put a small box under one of the wheels diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index f7c6cb3..dc4deed 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -24,9 +24,9 @@ fn create_coupled_joints( .coupled_axes(JointAxesMask::Y | JointAxesMask::Z); if use_articulations { - multibody_joints.insert(ground, body1, joint1); + multibody_joints.insert(ground, body1, joint1, true); } else { - impulse_joints.insert(ground, body1, joint1); + impulse_joints.insert(ground, body1, joint1, true); } } @@ -66,9 +66,9 @@ fn create_prismatic_joints( .limits([-2.0, 2.0]); if use_articulations { - multibody_joints.insert(curr_parent, curr_child, prism); + multibody_joints.insert(curr_parent, curr_child, prism, true); } else { - impulse_joints.insert(curr_parent, curr_child, prism); + impulse_joints.insert(curr_parent, curr_child, prism, true); } curr_parent = curr_child; } @@ -130,9 +130,9 @@ fn create_actuated_prismatic_joints( } if use_articulations { - multibody_joints.insert(curr_parent, curr_child, prism); + multibody_joints.insert(curr_parent, curr_child, prism, true); } else { - impulse_joints.insert(curr_parent, curr_child, prism); + impulse_joints.insert(curr_parent, curr_child, prism, true); } curr_parent = curr_child; @@ -185,15 +185,15 @@ fn create_revolute_joints( ]; if use_articulations { - multibody_joints.insert(curr_parent, handles[0], revs[0]); - multibody_joints.insert(handles[0], handles[1], revs[1]); - multibody_joints.insert(handles[1], handles[2], revs[2]); - multibody_joints.insert(handles[2], handles[3], revs[3]); + multibody_joints.insert(curr_parent, handles[0], revs[0], true); + multibody_joints.insert(handles[0], handles[1], revs[1], true); + multibody_joints.insert(handles[1], handles[2], revs[2], true); + multibody_joints.insert(handles[2], handles[3], revs[3], true); } else { - impulse_joints.insert(curr_parent, handles[0], revs[0]); - impulse_joints.insert(handles[0], handles[1], revs[1]); - impulse_joints.insert(handles[1], handles[2], revs[2]); - impulse_joints.insert(handles[2], handles[3], revs[3]); + impulse_joints.insert(curr_parent, handles[0], revs[0], true); + impulse_joints.insert(handles[0], handles[1], revs[1], true); + impulse_joints.insert(handles[1], handles[2], revs[2], true); + impulse_joints.insert(handles[2], handles[3], revs[3], true); } curr_parent = handles[3]; @@ -228,9 +228,9 @@ fn create_revolute_joints_with_limits( // .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z); if use_articulations { - multibody_joints.insert(ground, platform1, joint1); + multibody_joints.insert(ground, platform1, joint1, true); } else { - impulse_joints.insert(ground, platform1, joint1); + impulse_joints.insert(ground, platform1, joint1, true); } let joint2 = RevoluteJointBuilder::new(z) @@ -247,9 +247,9 @@ fn create_revolute_joints_with_limits( // .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z); if use_articulations { - multibody_joints.insert(platform1, platform2, joint2); + multibody_joints.insert(platform1, platform2, joint2, true); } else { - impulse_joints.insert(platform1, platform2, joint2); + impulse_joints.insert(platform1, platform2, joint2, true); } // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits. @@ -315,9 +315,9 @@ fn create_fixed_joints( let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); if use_articulations { - multibody_joints.insert(parent_handle, child_handle, joint); + multibody_joints.insert(parent_handle, child_handle, joint, true); } else { - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } } @@ -326,7 +326,7 @@ fn create_fixed_joints( let parent_index = body_handles.len() - 1; let parent_handle = body_handles[parent_index]; let joint = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } body_handles.push(child_handle); @@ -374,9 +374,9 @@ fn create_spherical_joints( SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); if use_articulations { - multibody_joints.insert(parent_handle, child_handle, joint); + multibody_joints.insert(parent_handle, child_handle, joint, true); } else { - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } } @@ -385,7 +385,7 @@ fn create_spherical_joints( let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } body_handles.push(child_handle); @@ -426,11 +426,11 @@ fn create_spherical_joints_with_limits( .limits(JointAxis::Y, [-0.3, 0.3]); if use_articulations { - multibody_joints.insert(ground, ball1, joint1); - multibody_joints.insert(ball1, ball2, joint2); + multibody_joints.insert(ground, ball1, joint1, true); + multibody_joints.insert(ball1, ball2, joint2, true); } else { - impulse_joints.insert(ground, ball1, joint1); - impulse_joints.insert(ball1, ball2, joint2); + impulse_joints.insert(ground, ball1, joint1, true); + impulse_joints.insert(ball1, ball2, joint2, true); } } @@ -493,9 +493,9 @@ fn create_actuated_revolute_joints( } if use_articulations { - multibody_joints.insert(parent_handle, child_handle, joint); + multibody_joints.insert(parent_handle, child_handle, joint, true); } else { - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } } @@ -559,9 +559,9 @@ fn create_actuated_spherical_joints( } if use_articulations { - multibody_joints.insert(parent_handle, child_handle, joint); + multibody_joints.insert(parent_handle, child_handle, joint, true); } else { - impulse_joints.insert(parent_handle, child_handle, joint); + impulse_joints.insert(parent_handle, child_handle, joint, true); } } diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index bdde135..9e0ab8e 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -60,12 +60,20 @@ impl CCDSolver { let min_toi = (rb.ccd.ccd_thickness * 0.15 - * crate::utils::inv(rb.ccd.max_point_velocity(&rb.vels))) + * crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels))) .min(dt); - // println!("Min toi: {}, Toi: {}", min_toi, toi); - let new_pos = rb - .vels - .integrate(toi.max(min_toi), &rb.pos.position, &local_com); + // println!( + // "Min toi: {}, Toi: {}, thick: {}, max_vel: {}", + // min_toi, + // toi, + // rb.ccd.ccd_thickness, + // rb.ccd.max_point_velocity(&rb.integrated_vels) + // ); + let new_pos = rb.integrated_vels.integrate( + toi.max(min_toi), + &rb.pos.position, + &local_com, + ); rb.pos.next_position = new_pos; } } @@ -95,7 +103,7 @@ impl CCDSolver { } else { None }; - let moving_fast = rb.ccd.is_moving_fast(dt, &rb.vels, forces); + let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces); rb.ccd.ccd_active = moving_fast; ccd_active = ccd_active || moving_fast; } @@ -131,7 +139,7 @@ impl CCDSolver { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( dt, &rb1.forces, - &rb1.vels, + &rb1.integrated_vels, &rb1.mprops, ); @@ -256,7 +264,7 @@ impl CCDSolver { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( dt, &rb1.forces, - &rb1.vels, + &rb1.integrated_vels, &rb1.mprops, ); @@ -491,7 +499,10 @@ impl CCDSolver { let local_com1 = &rb1.mprops.local_mprops.local_com; let frozen1 = frozen.get(&b1); let pos1 = frozen1 - .map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1)) + .map(|t| { + rb1.integrated_vels + .integrate(*t, &rb1.pos.position, local_com1) + }) .unwrap_or(rb1.pos.next_position); pos1 * co_parent1.pos_wrt_parent } else { @@ -504,7 +515,10 @@ impl CCDSolver { let local_com2 = &rb2.mprops.local_mprops.local_com; let frozen2 = frozen.get(&b2); let pos2 = frozen2 - .map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2)) + .map(|t| { + rb2.integrated_vels + .integrate(*t, &rb2.pos.position, local_com2) + }) .unwrap_or(rb2.pos.next_position); pos2 * co_parent2.pos_wrt_parent } else { diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs index 4591825..6f5a47d 100644 --- a/src/dynamics/ccd/toi_entry.rs +++ b/src/dynamics/ccd/toi_entry.rs @@ -56,14 +56,14 @@ impl TOIEntry { return None; } - let linvel1 = - frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.linvel).unwrap_or(na::zero()); - let linvel2 = - frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.linvel).unwrap_or(na::zero()); - let angvel1 = - frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.angvel).unwrap_or(na::zero()); - let angvel2 = - frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.angvel).unwrap_or(na::zero()); + let linvel1 = frozen1.is_none() as u32 as Real + * rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero()); + let linvel2 = frozen2.is_none() as u32 as Real + * rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero()); + let angvel1 = frozen1.is_none() as u32 as Real + * rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero()); + let angvel2 = frozen2.is_none() as u32 as Real + * rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero()); #[cfg(feature = "dim2")] let vel12 = (linvel2 - linvel1).norm() @@ -114,6 +114,20 @@ impl TOIEntry { // because the colliders may be in a separating trajectory. let stop_at_penetration = is_pseudo_intersection_test; + // let pos12 = motion_c1 + // .position_at_time(start_time) + // .inv_mul(&motion_c2.position_at_time(start_time)); + // let vel12 = linvel2 - linvel1; + // let res_toi = query_dispatcher + // .time_of_impact( + // &pos12, + // &vel12, + // co1.shape.as_ref(), + // co2.shape.as_ref(), + // end_time - start_time, + // ) + // .ok(); + let res_toi = query_dispatcher .nonlinear_time_of_impact( &motion_c1, @@ -144,8 +158,8 @@ impl TOIEntry { NonlinearRigidMotion::new( rb.pos.position, rb.mprops.local_mprops.local_com, - rb.vels.linvel, - rb.vels.angvel, + rb.integrated_vels.linvel, + rb.integrated_vels.angvel, ) } else { NonlinearRigidMotion::constant_position(rb.pos.next_position) diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 0309cff..c4ec734 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -42,6 +42,7 @@ pub struct ImpulseJointSet { rb_graph_ids: Coarena, joint_ids: Arena, // Map joint handles to edge ids on the graph. joint_graph: InteractionGraph, + pub(crate) to_wake_up: Vec, // A set of rigid-body handles to wake-up during the next timestep. } impl ImpulseJointSet { @@ -51,6 +52,7 @@ impl ImpulseJointSet { rb_graph_ids: Coarena::new(), joint_ids: Arena::new(), joint_graph: InteractionGraph::new(), + to_wake_up: vec![], } } @@ -180,11 +182,15 @@ impl ImpulseJointSet { } /// Inserts a new joint into this set and retrieve its handle. + /// + /// If `wake_up` is set to `true`, then the bodies attached to this joint will be + /// automatically woken up during the next timestep. pub fn insert( &mut self, body1: RigidBodyHandle, body2: RigidBodyHandle, data: impl Into, + wake_up: bool, ) -> ImpulseJointHandle { let data = data.into(); let handle = self.joint_ids.insert(0.into()); @@ -217,6 +223,12 @@ impl ImpulseJointSet { } self.joint_ids[handle] = self.joint_graph.add_edge(graph_index1, graph_index2, joint); + + if wake_up { + self.to_wake_up.push(body1); + self.to_wake_up.push(body2); + } + ImpulseJointHandle(handle) } @@ -257,23 +269,16 @@ impl ImpulseJointSet { /// /// If `wake_up` is set to `true`, then the bodies attached to this joint will be /// automatically woken up. - pub fn remove( - &mut self, - handle: ImpulseJointHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, - wake_up: bool, - ) -> Option { + pub fn remove(&mut self, handle: ImpulseJointHandle, wake_up: bool) -> Option { let id = self.joint_ids.remove(handle.0)?; let endpoints = self.joint_graph.graph.edge_endpoints(id)?; if wake_up { - // Wake-up the bodies attached to this joint. if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.0) { - islands.wake_up(bodies, *rb_handle, true); + self.to_wake_up.push(*rb_handle); } if let Some(rb_handle) = self.joint_graph.graph.node_weight(endpoints.1) { - islands.wake_up(bodies, *rb_handle, true); + self.to_wake_up.push(*rb_handle); } } @@ -294,8 +299,6 @@ impl ImpulseJointSet { pub fn remove_joints_attached_to_rigid_body( &mut self, handle: RigidBodyHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, ) -> Vec { let mut deleted = vec![]; @@ -324,8 +327,8 @@ impl ImpulseJointSet { } // Wake up the attached bodies. - islands.wake_up(bodies, h1, true); - islands.wake_up(bodies, h2, true); + self.to_wake_up.push(h1); + self.to_wake_up.push(h2); } if let Some(other) = self.joint_graph.remove_node(deleted_id) { diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 50a9438..f1b2ffe 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -1,8 +1,6 @@ use crate::data::{Arena, Coarena, Index}; use crate::dynamics::joint::MultibodyLink; -use crate::dynamics::{ - GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyHandle, RigidBodySet, -}; +use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle}; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; use crate::parry::partitioning::IndexedData; @@ -84,6 +82,7 @@ pub struct MultibodyJointSet { // NOTE: this is mostly for the island extraction. So perhaps we won’t need // that any more in the future when we improve our island builder. pub(crate) connectivity_graph: InteractionGraph, + pub(crate) to_wake_up: Vec, } impl MultibodyJointSet { @@ -93,6 +92,7 @@ impl MultibodyJointSet { multibodies: Arena::new(), rb2mb: Coarena::new(), connectivity_graph: InteractionGraph::new(), + to_wake_up: vec![], } } @@ -113,6 +113,7 @@ impl MultibodyJointSet { body1: RigidBodyHandle, body2: RigidBodyHandle, data: impl Into, + wake_up: bool, ) -> Option { let data = data.into(); let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { @@ -155,6 +156,11 @@ impl MultibodyJointSet { multibody1.append(mb2, link1.id, MultibodyJoint::new(data)); + if wake_up { + self.to_wake_up.push(body1); + self.to_wake_up.push(body2); + } + // Because each rigid-body can only have one parent link, // we can use the second rigid-body’s handle as the multibody_joint’s // handle. @@ -162,13 +168,7 @@ impl MultibodyJointSet { } /// Removes an multibody_joint from this set. - pub fn remove( - &mut self, - handle: MultibodyJointHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, - wake_up: bool, - ) { + pub fn remove(&mut self, handle: MultibodyJointHandle, wake_up: bool) { if let Some(removed) = self.rb2mb.get(handle.0).copied() { let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); @@ -181,8 +181,8 @@ impl MultibodyJointSet { ); if wake_up { - islands.wake_up(bodies, RigidBodyHandle(handle.0), true); - islands.wake_up(bodies, parent_rb, true); + self.to_wake_up.push(RigidBodyHandle(handle.0)); + self.to_wake_up.push(parent_rb); } // TODO: remove the node if it no longer has any attached edges? @@ -211,13 +211,7 @@ impl MultibodyJointSet { } /// Removes all the multibody_joints from the multibody the given rigid-body is part of. - pub fn remove_multibody_articulations( - &mut self, - handle: RigidBodyHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, - wake_up: bool, - ) { + pub fn remove_multibody_articulations(&mut self, handle: RigidBodyHandle, wake_up: bool) { if let Some(removed) = self.rb2mb.get(handle.0).copied() { // Remove the multibody. let multibody = self.multibodies.remove(removed.multibody.0).unwrap(); @@ -225,7 +219,7 @@ impl MultibodyJointSet { let rb_handle = link.rigid_body; if wake_up { - islands.wake_up(bodies, rb_handle, true); + self.to_wake_up.push(rb_handle); } // Remove the rigid-body <-> multibody mapping for this link. @@ -239,12 +233,7 @@ impl MultibodyJointSet { } /// Removes all the multibody joints attached to a rigid-body. - pub fn remove_joints_attached_to_rigid_body( - &mut self, - rb_to_remove: RigidBodyHandle, - islands: &mut IslandManager, - bodies: &mut RigidBodySet, - ) { + pub fn remove_joints_attached_to_rigid_body(&mut self, rb_to_remove: RigidBodyHandle) { // TODO: optimize this. if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() { let mut articulations_to_remove = vec![]; @@ -255,12 +244,12 @@ impl MultibodyJointSet { // There is a multibody_joint handle is equal to the second rigid-body’s handle. articulations_to_remove.push(MultibodyJointHandle(rb2.0)); - islands.wake_up(bodies, rb1, true); - islands.wake_up(bodies, rb2, true); + self.to_wake_up.push(rb1); + self.to_wake_up.push(rb2); } for articulation_handle in articulations_to_remove { - self.remove(articulation_handle, islands, bodies, true); + self.remove(articulation_handle, true); } } } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 24e9754..5eca5a2 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -19,6 +19,10 @@ use num::Zero; pub struct RigidBody { pub(crate) pos: RigidBodyPosition, pub(crate) mprops: RigidBodyMassProps, + // NOTE: we need this so that the CCD can use the actual velocities obtained + // by the velocity solver with bias. If we switch to intepolation, we + // should remove this field. + pub(crate) integrated_vels: RigidBodyVelocity, pub(crate) vels: RigidBodyVelocity, pub(crate) damping: RigidBodyDamping, pub(crate) forces: RigidBodyForces, @@ -47,6 +51,7 @@ impl RigidBody { Self { pos: RigidBodyPosition::default(), mprops: RigidBodyMassProps::default(), + integrated_vels: RigidBodyVelocity::default(), vels: RigidBodyVelocity::default(), damping: RigidBodyDamping::default(), forces: RigidBodyForces::default(), diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index b818ce2..3d35d17 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -752,7 +752,7 @@ pub struct RigidBodyCcd { impl Default for RigidBodyCcd { fn default() -> Self { Self { - ccd_thickness: 0.0, + ccd_thickness: Real::MAX, ccd_max_dist: 0.0, ccd_active: false, ccd_enabled: false, diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 62a6a54..cd8f30a 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -109,8 +109,8 @@ impl RigidBodySet { /* * Remove impulse_joints attached to this rigid-body. */ - impulse_joints.remove_joints_attached_to_rigid_body(handle, islands, self); - multibody_joints.remove_joints_attached_to_rigid_body(handle, islands, self); + impulse_joints.remove_joints_attached_to_rigid_body(handle); + multibody_joints.remove_joints_attached_to_rigid_body(handle); Some(rb) } diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 318727e..ed8c569 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -34,6 +34,7 @@ impl GenericVelocityConstraint { jacobian_id: &mut usize, insert_at: Option, ) { + let cfm_factor = params.cfm_factor(); let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); @@ -45,6 +46,7 @@ impl GenericVelocityConstraint { let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type); let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type); + let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; let multibody1 = multibodies .rigid_body_link(handle1) @@ -92,6 +94,7 @@ impl GenericVelocityConstraint { .enumerate() { let chunk_j_id = *jacobian_id; + let mut is_fast_contact = false; let mut constraint = VelocityConstraint { dir1: force_dir1, #[cfg(feature = "dim3")] @@ -107,6 +110,7 @@ impl GenericVelocityConstraint { } else { na::zero() }, + cfm_factor, limit: 0.0, mj_lambda1, mj_lambda2, @@ -196,10 +200,13 @@ impl GenericVelocityConstraint { let rhs_bias = /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0); + let rhs = rhs_wo_bias + rhs_bias; + is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5); + constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, - rhs: rhs_wo_bias + rhs_bias, + rhs, rhs_wo_bias, impulse: na::zero(), r, @@ -283,6 +290,8 @@ impl GenericVelocityConstraint { } } + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0); // NOTE: we use the generic constraint for non-dynamic bodies because this will @@ -310,7 +319,6 @@ impl GenericVelocityConstraint { pub fn solve( &mut self, - cfm_factor: Real, jacobians: &DVector, mj_lambdas: &mut [DeltaVel], generic_mj_lambdas: &mut DVector, @@ -332,7 +340,7 @@ impl GenericVelocityConstraint { let elements = &mut self.velocity_constraint.elements [..self.velocity_constraint.num_contacts as usize]; VelocityConstraintElement::generic_solve_group( - cfm_factor, + self.velocity_constraint.cfm_factor, elements, jacobians, &self.velocity_constraint.dir1, @@ -364,7 +372,7 @@ impl GenericVelocityConstraint { self.velocity_constraint.writeback_impulses(manifolds_all); } - pub fn remove_bias_from_rhs(&mut self) { - self.velocity_constraint.remove_bias_from_rhs(); + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.velocity_constraint.remove_cfm_and_bias_from_rhs(); } } diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index 16648c4..91bb9fb 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -32,6 +32,7 @@ impl GenericVelocityGroundConstraint { jacobian_id: &mut usize, insert_at: Option, ) { + let cfm_factor = params.cfm_factor(); let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); @@ -85,12 +86,14 @@ impl GenericVelocityGroundConstraint { .enumerate() { let chunk_j_id = *jacobian_id; + let mut is_fast_contact = false; let mut constraint = VelocityGroundConstraint { dir1: force_dir1, #[cfg(feature = "dim3")] tangent1: tangents1[0], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: mprops2.effective_inv_mass, + cfm_factor, limit: 0.0, mj_lambda2, manifold_id, @@ -130,16 +133,20 @@ impl GenericVelocityGroundConstraint { let is_bouncy = manifold_point.is_bouncy() as u32 as Real; let is_resting = 1.0 - is_bouncy; - let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) - * (vel1 - vel2).dot(&force_dir1); + let dvel = (vel1 - vel2).dot(&force_dir1); + let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel; rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0); + let rhs = rhs_wo_bias + rhs_bias; + is_fast_contact = + is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5); + constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2: na::zero(), // Unused for generic constraints. - rhs: rhs_wo_bias + rhs_bias, + rhs, rhs_wo_bias, impulse: na::zero(), r, @@ -181,6 +188,8 @@ impl GenericVelocityGroundConstraint { } } + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + let constraint = GenericVelocityGroundConstraint { velocity_constraint: constraint, j_id: chunk_j_id, @@ -198,7 +207,6 @@ impl GenericVelocityGroundConstraint { pub fn solve( &mut self, - cfm_factor: Real, jacobians: &DVector, generic_mj_lambdas: &mut DVector, solve_restitution: bool, @@ -209,7 +217,7 @@ impl GenericVelocityGroundConstraint { let elements = &mut self.velocity_constraint.elements [..self.velocity_constraint.num_contacts as usize]; VelocityGroundConstraintElement::generic_solve_group( - cfm_factor, + self.velocity_constraint.cfm_factor, elements, jacobians, self.velocity_constraint.limit, @@ -226,7 +234,7 @@ impl GenericVelocityGroundConstraint { self.velocity_constraint.writeback_impulses(manifolds_all); } - pub fn remove_bias_from_rhs(&mut self) { - self.velocity_constraint.remove_bias_from_rhs(); + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.velocity_constraint.remove_cfm_and_bias_from_rhs(); } } diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs index ab34a42..3db1cdc 100644 --- a/src/dynamics/solver/parallel_velocity_solver.rs +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -45,7 +45,6 @@ impl ParallelVelocitySolver { let joint_descs = &joint_constraints.constraint_descs[..]; let mut target_num_desc = 0; let mut shift = 0; - let cfm_factor = params.cfm_factor(); // Each thread will concurrently grab thread.batch_size constraint desc to // solve. If the batch size is large enough to cross the boundary of @@ -122,7 +121,6 @@ impl ParallelVelocitySolver { // Solve rigid-body contacts. solve!( contact_constraints, - cfm_factor, &contact_constraints.generic_jacobians, &mut self.mj_lambdas, &mut self.generic_mj_lambdas, @@ -135,7 +133,6 @@ impl ParallelVelocitySolver { // Solve generic rigid-body contacts. solve!( contact_constraints, - cfm_factor, &contact_constraints.generic_jacobians, &mut self.mj_lambdas, &mut self.generic_mj_lambdas, @@ -148,7 +145,6 @@ impl ParallelVelocitySolver { if solve_friction { solve!( contact_constraints, - cfm_factor, &contact_constraints.generic_jacobians, &mut self.mj_lambdas, &mut self.generic_mj_lambdas, @@ -173,7 +169,6 @@ impl ParallelVelocitySolver { for _ in 0..remaining_friction_iterations { solve!( contact_constraints, - cfm_factor, &contact_constraints.generic_jacobians, &mut self.mj_lambdas, &mut self.generic_mj_lambdas, @@ -265,7 +260,6 @@ impl ParallelVelocitySolver { solve!( contact_constraints, - cfm_factor, &contact_constraints.generic_jacobians, &mut self.mj_lambdas, &mut self.generic_mj_lambdas, @@ -277,7 +271,6 @@ impl ParallelVelocitySolver { solve!( contact_constraints, - cfm_factor, &contact_constraints.generic_jacobians, &mut self.mj_lambdas, &mut self.generic_mj_lambdas, diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 23d4b97..7d2294e 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -47,21 +47,20 @@ impl AnyVelocityConstraint { pub fn remove_bias_from_rhs(&mut self) { match self { - AnyVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(), + AnyVelocityConstraint::Nongrouped(c) => c.remove_cfm_and_bias_from_rhs(), + AnyVelocityConstraint::NongroupedGround(c) => c.remove_cfm_and_bias_from_rhs(), #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::Grouped(c) => c.remove_bias_from_rhs(), + AnyVelocityConstraint::Grouped(c) => c.remove_cfm_and_bias_from_rhs(), #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::GroupedGround(c) => c.remove_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_bias_from_rhs(), + AnyVelocityConstraint::GroupedGround(c) => c.remove_cfm_and_bias_from_rhs(), + AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_cfm_and_bias_from_rhs(), + AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_cfm_and_bias_from_rhs(), AnyVelocityConstraint::Empty => unreachable!(), } } pub fn solve( &mut self, - cfm_factor: Real, jacobians: &DVector, mj_lambdas: &mut [DeltaVel], generic_mj_lambdas: &mut DVector, @@ -70,21 +69,20 @@ impl AnyVelocityConstraint { ) { match self { AnyVelocityConstraint::NongroupedGround(c) => { - c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction) + c.solve(mj_lambdas, solve_restitution, solve_friction) } AnyVelocityConstraint::Nongrouped(c) => { - c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction) + c.solve(mj_lambdas, solve_restitution, solve_friction) } #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::GroupedGround(c) => { - c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction) + c.solve(mj_lambdas, solve_restitution, solve_friction) } #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::Grouped(c) => { - c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction) + c.solve(mj_lambdas, solve_restitution, solve_friction) } AnyVelocityConstraint::NongroupedGeneric(c) => c.solve( - cfm_factor, jacobians, mj_lambdas, generic_mj_lambdas, @@ -92,7 +90,6 @@ impl AnyVelocityConstraint { solve_friction, ), AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve( - cfm_factor, jacobians, generic_mj_lambdas, solve_restitution, @@ -124,6 +121,7 @@ pub(crate) struct VelocityConstraint { pub tangent1: Vector, // One of the friction force directions. pub im1: Vector, pub im2: Vector, + pub cfm_factor: Real, pub limit: Real, pub mj_lambda1: usize, pub mj_lambda2: usize, @@ -153,6 +151,7 @@ impl VelocityConstraint { ) { assert_eq!(manifold.data.relative_dominance, 0); + let cfm_factor = params.cfm_factor(); let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); @@ -163,6 +162,7 @@ impl VelocityConstraint { let (vels1, mprops1) = (&rb1.vels, &rb1.mprops); let rb2 = &bodies[handle2]; let (vels2, mprops2) = (&rb2.vels, &rb2.mprops); + let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; let mj_lambda1 = rb1.ids.active_set_offset; let mj_lambda2 = rb2.ids.active_set_offset; @@ -180,6 +180,8 @@ impl VelocityConstraint { .chunks(MAX_MANIFOLD_POINTS) .enumerate() { + let mut is_fast_contact = false; + #[cfg(not(target_arch = "wasm32"))] let mut constraint = VelocityConstraint { dir1: force_dir1, @@ -188,6 +190,7 @@ impl VelocityConstraint { elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1: mprops1.effective_inv_mass, im2: mprops2.effective_inv_mass, + cfm_factor, limit: 0.0, mj_lambda1, mj_lambda2, @@ -235,6 +238,7 @@ impl VelocityConstraint { } constraint.im1 = mprops1.effective_inv_mass; constraint.im2 = mprops2.effective_inv_mass; + constraint.cfm_factor = cfm_factor; constraint.limit = 0.0; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; @@ -280,10 +284,14 @@ impl VelocityConstraint { let rhs_bias = /* is_resting * */ erp_inv_dt * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0); + + let rhs = rhs_wo_bias + rhs_bias; + is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5); + constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, - rhs: rhs_wo_bias + rhs_bias, + rhs, rhs_wo_bias, impulse: na::zero(), r: projected_mass, @@ -329,6 +337,8 @@ impl VelocityConstraint { } } + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + #[cfg(not(target_arch = "wasm32"))] if let Some(at) = insert_at { out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint); @@ -340,7 +350,6 @@ impl VelocityConstraint { pub fn solve( &mut self, - cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -349,7 +358,7 @@ impl VelocityConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityConstraintElement::solve_group( - cfm_factor, + self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] @@ -386,7 +395,8 @@ impl VelocityConstraint { } } - pub fn remove_bias_from_rhs(&mut self) { + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = 1.0; for elt in &mut self.elements { elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; } diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 68246da..b8e0a7e 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -12,6 +12,7 @@ use crate::math::{ use crate::utils::WBasis; use crate::utils::{self, WAngularInertia, WCross, WDot}; use num::Zero; +use parry::math::SimdBool; use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] @@ -23,6 +24,7 @@ pub(crate) struct WVelocityConstraint { pub num_contacts: u8, pub im1: Vector, pub im2: Vector, + pub cfm_factor: SimdReal, pub limit: SimdReal, pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH], @@ -43,6 +45,8 @@ impl WVelocityConstraint { assert_eq!(manifolds[ii].data.relative_dominance, 0); } + let cfm_factor = SimdReal::splat(params.cfm_factor()); + let dt = SimdReal::splat(params.dt); let inv_dt = SimdReal::splat(params.inv_dt()); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); @@ -58,6 +62,10 @@ impl WVelocityConstraint { let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops]; let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops]; + let ccd_thickness1 = SimdReal::from(gather![|ii| bodies[handles1[ii]].ccd.ccd_thickness]); + let ccd_thickness2 = SimdReal::from(gather![|ii| bodies[handles2[ii]].ccd.ccd_thickness]); + let ccd_thickness = ccd_thickness1 + ccd_thickness2; + let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]); let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]); let ii1: AngularInertia = @@ -91,6 +99,7 @@ impl WVelocityConstraint { gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + let mut is_fast_contact = SimdBool::splat(false); let mut constraint = WVelocityConstraint { dir1: force_dir1, #[cfg(feature = "dim3")] @@ -98,6 +107,7 @@ impl WVelocityConstraint { elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1, im2, + cfm_factor, limit: SimdReal::splat(0.0), mj_lambda1, mj_lambda2, @@ -147,6 +157,10 @@ impl WVelocityConstraint { .simd_clamp(-max_penetration_correction, SimdReal::zero()) * (erp_inv_dt/* * is_resting */); + let rhs = rhs_wo_bias + rhs_bias; + is_fast_contact = + is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5)); + constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, gcross2, @@ -189,6 +203,8 @@ impl WVelocityConstraint { } } + constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor); + if let Some(at) = insert_at { out_constraints[at + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::Grouped(constraint); @@ -200,7 +216,6 @@ impl WVelocityConstraint { pub fn solve( &mut self, - cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -220,7 +235,7 @@ impl WVelocityConstraint { }; VelocityConstraintElement::solve_group( - SimdReal::splat(cfm_factor), + self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] @@ -270,7 +285,8 @@ impl WVelocityConstraint { } } - pub fn remove_bias_from_rhs(&mut self) { + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = SimdReal::splat(1.0); for elt in &mut self.elements { elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 39a2a19..19bf7e6 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -17,6 +17,7 @@ pub(crate) struct VelocityGroundConstraint { #[cfg(feature = "dim3")] pub tangent1: Vector, // One of the friction force directions. pub im2: Vector, + pub cfm_factor: Real, pub limit: Real, pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], @@ -34,6 +35,7 @@ impl VelocityGroundConstraint { out_constraints: &mut Vec, insert_at: Option, ) { + let cfm_factor = params.cfm_factor(); let inv_dt = params.inv_dt(); let erp_inv_dt = params.erp_inv_dt(); @@ -80,6 +82,7 @@ impl VelocityGroundConstraint { tangent1: tangents1[0], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: mprops2.effective_inv_mass, + cfm_factor, limit: 0.0, mj_lambda2, manifold_id, @@ -125,6 +128,7 @@ impl VelocityGroundConstraint { constraint.tangent1 = tangents1[0]; } constraint.im2 = mprops2.effective_inv_mass; + constraint.cfm_factor = cfm_factor; constraint.limit = 0.0; constraint.mj_lambda2 = mj_lambda2; constraint.manifold_id = manifold_id; @@ -132,6 +136,8 @@ impl VelocityGroundConstraint { constraint.num_contacts = manifold_points.len() as u8; } + let mut is_fast_contact = false; + for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; let dp2 = manifold_point.point - mprops2.world_com; @@ -156,17 +162,21 @@ impl VelocityGroundConstraint { let is_bouncy = manifold_point.is_bouncy() as u32 as Real; let is_resting = 1.0 - is_bouncy; - let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) - * (vel1 - vel2).dot(&force_dir1); + let dvel = (vel1 - vel2).dot(&force_dir1); + let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel; rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = /* is_resting * */ erp_inv_dt * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0); + let rhs = rhs_wo_bias + rhs_bias; + is_fast_contact = + is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5); + constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, - rhs: rhs_wo_bias + rhs_bias, + rhs, rhs_wo_bias, impulse: na::zero(), r: projected_mass, @@ -206,6 +216,8 @@ impl VelocityGroundConstraint { } } + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + #[cfg(not(target_arch = "wasm32"))] if let Some(at) = insert_at { out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint); @@ -217,7 +229,6 @@ impl VelocityGroundConstraint { pub fn solve( &mut self, - cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -225,7 +236,7 @@ impl VelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityGroundConstraintElement::solve_group( - cfm_factor, + self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] @@ -260,7 +271,8 @@ impl VelocityGroundConstraint { } } - pub fn remove_bias_from_rhs(&mut self) { + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = 1.0; for elt in &mut self.elements { elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; } diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 37f4688..25bb30d 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -13,6 +13,7 @@ use crate::math::{ use crate::utils::WBasis; use crate::utils::{self, WAngularInertia, WCross, WDot}; use num::Zero; +use parry::math::SimdBool; use simba::simd::{SimdPartialOrd, SimdValue}; #[derive(Copy, Clone, Debug)] @@ -23,6 +24,7 @@ pub(crate) struct WVelocityGroundConstraint { pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], pub num_contacts: u8, pub im2: Vector, + pub cfm_factor: SimdReal, pub limit: SimdReal, pub mj_lambda2: [usize; SIMD_WIDTH], pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], @@ -38,6 +40,8 @@ impl WVelocityGroundConstraint { out_constraints: &mut Vec, insert_at: Option, ) { + let cfm_factor = SimdReal::splat(params.cfm_factor()); + let dt = SimdReal::splat(params.dt); let inv_dt = SimdReal::splat(params.inv_dt()); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); @@ -65,11 +69,12 @@ impl WVelocityGroundConstraint { .unwrap_or_else(Point::origin) }]); - let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = - gather![|ii| &bodies[handles2[ii].unwrap()].vels]; - let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii].unwrap()].ids]; - let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = - gather![|ii| &bodies[handles2[ii].unwrap()].mprops]; + let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]]; + + let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels]; + let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids]; + let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops]; + let ccd_thickness = SimdReal::from(gather![|ii| bodies2[ii].ccd.ccd_thickness]); let flipped_sign = SimdReal::from(flipped); @@ -101,12 +106,14 @@ impl WVelocityGroundConstraint { let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + let mut is_fast_contact = SimdBool::splat(false); let mut constraint = WVelocityGroundConstraint { dir1: force_dir1, #[cfg(feature = "dim3")] tangent1: tangents1[0], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2, + cfm_factor, limit: SimdReal::splat(0.0), mj_lambda2, manifold_id, @@ -152,9 +159,13 @@ impl WVelocityGroundConstraint { .simd_clamp(-max_penetration_correction, SimdReal::zero()) * (erp_inv_dt/* * is_resting */); + let rhs = rhs_wo_bias + rhs_bias; + is_fast_contact = + is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5)); + constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, - rhs: rhs_wo_bias + rhs_bias, + rhs, rhs_wo_bias, impulse: na::zero(), r: projected_mass, @@ -187,6 +198,8 @@ impl WVelocityGroundConstraint { } } + constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor); + if let Some(at) = insert_at { out_constraints[at + l / MAX_MANIFOLD_POINTS] = AnyVelocityConstraint::GroupedGround(constraint); @@ -198,7 +211,6 @@ impl WVelocityGroundConstraint { pub fn solve( &mut self, - cfm_factor: Real, mj_lambdas: &mut [DeltaVel], solve_normal: bool, solve_friction: bool, @@ -211,7 +223,7 @@ impl WVelocityGroundConstraint { }; VelocityGroundConstraintElement::solve_group( - SimdReal::splat(cfm_factor), + self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] @@ -256,7 +268,8 @@ impl WVelocityGroundConstraint { } } - pub fn remove_bias_from_rhs(&mut self) { + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = SimdReal::splat(1.0); for elt in &mut self.elements { elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; } diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index d5dc77d..d15ea68 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -35,7 +35,6 @@ impl VelocitySolver { joint_constraints: &mut [AnyJointVelocityConstraint], generic_joint_jacobians: &DVector, ) { - let cfm_factor = params.cfm_factor(); self.mj_lambdas.clear(); self.mj_lambdas .resize(islands.active_island(island_id).len(), DeltaVel::zero()); @@ -86,7 +85,6 @@ impl VelocitySolver { for constraint in &mut *contact_constraints { constraint.solve( - cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -98,7 +96,6 @@ impl VelocitySolver { if solve_friction { for constraint in &mut *contact_constraints { constraint.solve( - cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -121,7 +118,6 @@ impl VelocitySolver { for _ in 0..remaining_friction_iterations { for constraint in &mut *contact_constraints { constraint.solve( - cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -168,6 +164,7 @@ impl VelocitySolver { &rb.pos.position, &rb.mprops.local_mprops.local_com, ); + rb.integrated_vels = new_vels; rb.pos = new_pos; } } @@ -190,7 +187,6 @@ impl VelocitySolver { for constraint in &mut *contact_constraints { constraint.solve( - 1.0, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -201,7 +197,6 @@ impl VelocitySolver { for constraint in &mut *contact_constraints { constraint.solve( - 1.0, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, diff --git a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs index 14da40d..bcd19e8 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_pipeline.rs @@ -2,7 +2,7 @@ use super::{outlines, DebugRenderBackend}; use crate::dynamics::{ GenericJoint, ImpulseJointSet, MultibodyJointSet, RigidBodySet, RigidBodyType, }; -use crate::geometry::{Ball, ColliderSet, Cuboid, Shape, TypedShape}; +use crate::geometry::{Ball, ColliderSet, Cuboid, NarrowPhase, Shape, TypedShape}; #[cfg(feature = "dim3")] use crate::geometry::{Cone, Cylinder}; use crate::math::{Isometry, Point, Real, Vector, DIM}; @@ -24,6 +24,12 @@ bitflags::bitflags! { const MULTIBODY_JOINTS = 1 << 2; /// If this flag is set, the impulse joints will be rendered. const IMPULSE_JOINTS = 1 << 3; + /// If this flag is set, the solver contacts will be rendered. + const SOLVER_CONTACTS = 1 << 4; + /// If this flag is set, the geometric contacts will be rendered. + const CONTACTS = 1 << 5; + /// If this flag is set, the AABBs of colliders will be rendered. + const COLLIDER_AABBS = 1 << 6; } } @@ -71,10 +77,62 @@ impl DebugRenderPipeline { colliders: &ColliderSet, impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, ) { self.render_rigid_bodies(backend, bodies); self.render_colliders(backend, bodies, colliders); self.render_joints(backend, bodies, impulse_joints, multibody_joints); + self.render_contacts(backend, colliders, narrow_phase); + } + + /// Render contact. + pub fn render_contacts( + &mut self, + backend: &mut impl DebugRenderBackend, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + ) { + if self.mode.contains(DebugRenderMode::CONTACTS) { + for pair in narrow_phase.contact_pairs() { + if let (Some(co1), Some(co2)) = + (colliders.get(pair.collider1), colliders.get(pair.collider2)) + { + for manifold in &pair.manifolds { + for contact in manifold.contacts() { + backend.draw_line( + DebugRenderObject::Other, + co1.position() * contact.local_p1, + co2.position() * contact.local_p2, + self.style.contact_depth_color, + ); + backend.draw_line( + DebugRenderObject::Other, + co1.position() * contact.local_p1, + co1.position() + * (contact.local_p1 + + manifold.local_n1 * self.style.contact_normal_length), + self.style.contact_normal_color, + ); + } + } + } + } + } + + if self.mode.contains(DebugRenderMode::SOLVER_CONTACTS) { + for pair in narrow_phase.contact_pairs() { + for manifold in &pair.manifolds { + for contact in &manifold.data.solver_contacts { + backend.draw_line( + DebugRenderObject::Other, + contact.point, + contact.point + manifold.data.normal * self.style.contact_normal_length, + self.style.contact_normal_color, + ); + } + } + } + } } /// Render only the joints from the scene. @@ -224,6 +282,20 @@ impl DebugRenderPipeline { self.render_shape(object, backend, co.shape(), co.position(), color) } } + + if self.mode.contains(DebugRenderMode::COLLIDER_AABBS) { + for (_, co) in colliders.iter() { + let aabb = co.compute_aabb(); + let cuboid = Cuboid::new(aabb.half_extents()); + self.render_shape( + DebugRenderObject::Other, + backend, + &cuboid, + &aabb.center().into(), + self.style.collider_aabb_color, + ); + } + } } #[cfg(feature = "dim2")] diff --git a/src/pipeline/debug_render_pipeline/debug_render_style.rs b/src/pipeline/debug_render_pipeline/debug_render_style.rs index ac2b3a3..987d95d 100644 --- a/src/pipeline/debug_render_pipeline/debug_render_style.rs +++ b/src/pipeline/debug_render_pipeline/debug_render_style.rs @@ -38,6 +38,14 @@ pub struct DebugRenderStyle { pub sleep_color_multiplier: [f32; 4], /// The length of the local coordinate axes rendered for a rigid-body. pub rigid_body_axes_length: Real, + /// The collor for the segments joining the two contact points. + pub contact_depth_color: DebugColor, + /// The color of the contact normals. + pub contact_normal_color: DebugColor, + /// The length of the contact normals. + pub contact_normal_length: Real, + /// The color of the colliders AABBs. + pub collider_aabb_color: DebugColor, } impl Default for DebugRenderStyle { @@ -55,6 +63,10 @@ impl Default for DebugRenderStyle { multibody_joint_separation_color: [0.0, 1.0, 0.4, 1.0], sleep_color_multiplier: [1.0, 1.0, 0.2, 1.0], rigid_body_axes_length: 0.5, + contact_depth_color: [120.0, 1.0, 0.4, 1.0], + contact_normal_color: [0.0, 1.0, 1.0, 1.0], + contact_normal_length: 0.3, + collider_aabb_color: [124.0, 1.0, 0.4, 1.0], } } } diff --git a/src_testbed/debug_render.rs b/src_testbed/debug_render.rs index 913663e..047793d 100644 --- a/src_testbed/debug_render.rs +++ b/src_testbed/debug_render.rs @@ -25,7 +25,7 @@ impl Plugin for RapierDebugRenderPlugin { )) .insert_resource(DebugRenderPipeline::new( Default::default(), - !DebugRenderMode::RIGID_BODY_AXES, + !DebugRenderMode::RIGID_BODY_AXES & !DebugRenderMode::COLLIDER_AABBS, )) .add_system_to_stage(CoreStage::Update, debug_render_scene); } @@ -68,5 +68,6 @@ fn debug_render_scene( &harness.physics.colliders, &harness.physics.impulse_joints, &harness.physics.multibody_joints, + &harness.physics.narrow_phase, ); } diff --git a/src_testbed/lines/debuglines.wgsl b/src_testbed/lines/debuglines.wgsl index 0987839..364d9ac 100644 --- a/src_testbed/lines/debuglines.wgsl +++ b/src_testbed/lines/debuglines.wgsl @@ -25,9 +25,13 @@ struct FragmentOutput { fn vertex(vertex: Vertex) -> VertexOutput { var out: VertexOutput; out.clip_position = view.view_proj * vec4(vertex.pos, 1.0); - //out.color = vertex.color; // https://github.com/bevyengine/bevy/blob/328c26d02c50de0bc77f0d24a376f43ba89517b1/examples/2d/mesh2d_manual.rs#L234 - out.color = vec4((vec4(vertex.color) >> vec4(8u, 8u, 16u, 24u)) & vec4(255u)) / 255.0; + // ... except the above doesn't seem to work in 3d. Not sure what's going on there. + var r = f32(vertex.color & 255u) / 255.0; + var g = f32(vertex.color >> 8u & 255u) / 255.0; + var b = f32(vertex.color >> 16u & 255u) / 255.0; + var a = f32(vertex.color >> 24u & 255u) / 255.0; + out.color = vec4(r, g, b, a); return out; } diff --git a/src_testbed/lines/debuglines2d.wgsl b/src_testbed/lines/debuglines2d.wgsl index 9316531..b722d8a 100644 --- a/src_testbed/lines/debuglines2d.wgsl +++ b/src_testbed/lines/debuglines2d.wgsl @@ -17,10 +17,11 @@ struct VertexOutput { fn vertex(vertex: Vertex) -> VertexOutput { var out: VertexOutput; out.clip_position = view.view_proj * vec4(vertex.place, 1.0); - // What is this craziness? - out.color = vec4((vec4(vertex.color) >> vec4(0u, 8u, 16u, 24u)) & vec4(255u)) / 255.0; - //out.color = vertex.color; - //out.color = vec4(1.0, 0.0, 0.0, 1.0); + var r = f32(vertex.color & 255u) / 255.0; + var g = f32(vertex.color >> 8u & 255u) / 255.0; + var b = f32(vertex.color >> 16u & 255u) / 255.0; + var a = f32(vertex.color >> 24u & 255u) / 255.0; + out.color = vec4(r, g, b, a); return out; } diff --git a/src_testbed/lines/mod.rs b/src_testbed/lines/mod.rs index 7e78ee9..a7dd7bb 100644 --- a/src_testbed/lines/mod.rs +++ b/src_testbed/lines/mod.rs @@ -1,4 +1,5 @@ #![allow(warnings)] +use bevy::render::view::NoFrustumCulling; /** * * NOTE: this module and its submodules are only temporary. It is a copy-paste of the bevy-debug-lines @@ -33,7 +34,7 @@ mod render_dim; // gates-specific code. #[cfg(feature = "dim3")] mod dim { - pub(crate) use crate::lines::render_dim::r3d::{queue, DebugLinePipeline, DrawDebugLines}; + pub(crate) use super::render_dim::r3d::{queue, DebugLinePipeline, DrawDebugLines}; pub(crate) use bevy::core_pipeline::Opaque3d as Phase; use bevy::{asset::Handle, render::mesh::Mesh}; @@ -49,7 +50,7 @@ mod dim { } #[cfg(feature = "dim2")] mod dim { - pub(crate) use crate::lines::render_dim::r2d::{queue, DebugLinePipeline, DrawDebugLines}; + pub(crate) use super::render_dim::r2d::{queue, DebugLinePipeline, DrawDebugLines}; pub(crate) use bevy::core_pipeline::Transparent2d as Phase; use bevy::{asset::Handle, render::mesh::Mesh, sprite::Mesh2dHandle}; @@ -172,6 +173,7 @@ fn setup(mut cmds: Commands, mut meshes: ResMut>) { dim::into_handle(meshes.add(mesh)), NotShadowCaster, NotShadowReceiver, + NoFrustumCulling, Transform::default(), GlobalTransform::default(), Visibility::default(), @@ -190,7 +192,7 @@ fn update( // For each debug line mesh, fill its buffers with the relevant positions/colors chunks. for (mesh_handle, debug_lines_idx) in debug_line_meshes.iter() { let mesh = meshes.get_mut(dim::from_handle(mesh_handle)).unwrap(); - use VertexAttributeValues::{Float32x3, Float32x4, Uint32}; + use VertexAttributeValues::{Float32x3, Uint32}; if let Some(Float32x3(vbuffer)) = mesh.attribute_mut(Mesh::ATTRIBUTE_POSITION) { vbuffer.clear(); if let Some(new_content) = lines @@ -238,7 +240,7 @@ fn extract(mut commands: Commands, query: Query>) { } #[derive(Component)] -struct DebugLinesMesh(usize); +pub(crate) struct DebugLinesMesh(usize); #[derive(Component)] pub(crate) struct RenderDebugLinesMesh; @@ -320,7 +322,6 @@ impl DebugLines { end_color: Color, ) { if self.positions.len() >= MAX_POINTS { - warn!("Tried to add a new line when existing number of lines was already at maximum, ignoring."); return; } diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index c17e5ac..629d4cc 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -691,12 +691,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { .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, - ); + self.harness.physics.impulse_joints.remove(*to_delete, true); } } KeyCode::A => { @@ -710,12 +705,10 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { .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, - ); + self.harness + .physics + .multibody_joints + .remove(*to_delete, true); } } KeyCode::M => { @@ -731,12 +724,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { self.harness .physics .multibody_joints - .remove_multibody_articulations( - to_delete, - &mut self.harness.physics.islands, - &mut self.harness.physics.bodies, - true, - ); + .remove_multibody_articulations(to_delete, true); } } _ => {}