nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #334 from dimforge/fixes

Browse Source 
Some CCD and debug-render improvements
This commit is contained in:
Sébastien Crozet
2022-05-31 10:22:28 +02:00
committed by GitHub
parent c630635e57 0640f5e660
commit fb1bfc762c
35 changed files with 419 additions and 208 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
CHANGELOG.md
View File

@@ -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

4
benchmarks2d/joint_ball2.rs
View File

@@ -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);

4
benchmarks2d/joint_fixed2.rs
View File

@@ -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);

2
benchmarks2d/joint_prismatic2.rs
View File

@@ -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;
}

4
benchmarks3d/joint_ball3.rs
View File

@@ -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);

4
benchmarks3d/joint_fixed3.rs
View File

@@ -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);

2
benchmarks3d/joint_prismatic3.rs
View File

@@ -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;
}

8
benchmarks3d/joint_revolute3.rs
View File

@@ -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];
}

4
examples2d/joints2.rs
View File

@@ -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);

4
examples3d/debug_articulations3.rs
View File

@@ -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);

44
examples3d/debug_excentric_boxes3.rs Normal file
View File

@@ -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());
}

2
examples3d/debug_prismatic3.rs
View File

@@ -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

64
examples3d/joints3.rs
View File

@@ -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);
}
}

34
src/dynamics/ccd/ccd_solver.rs
View File

@@ -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 {

34
src/dynamics/ccd/toi_entry.rs
View File

@@ -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)

31
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -42,6 +42,7 @@ pub struct ImpulseJointSet {
rb_graph_ids: Coarena<RigidBodyGraphIndex>,
joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>,
pub(crate) to_wake_up: Vec<RigidBodyHandle>, // 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<GenericJoint>,
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<ImpulseJoint> {
pub fn remove(&mut self, handle: ImpulseJointHandle, wake_up: bool) -> Option<ImpulseJoint> {
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<ImpulseJointHandle> {
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) {

47
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -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<RigidBodyHandle, ()>,
pub(crate) to_wake_up: Vec<RigidBodyHandle>,
}
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<GenericJoint>,
wake_up: bool,
) -> Option<MultibodyJointHandle> {
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);
}
}
}

5
src/dynamics/rigid_body.rs
View File

@@ -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(),

2
src/dynamics/rigid_body_components.rs
View File

@@ -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,

4
src/dynamics/rigid_body_set.rs
View File

@@ -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)
}

18
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -34,6 +34,7 @@ impl GenericVelocityConstraint {
jacobian_id: &mut usize,
insert_at: Option<usize>,
) {
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<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
@@ -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();
}
}

22
src/dynamics/solver/generic_velocity_ground_constraint.rs
View File

@@ -32,6 +32,7 @@ impl GenericVelocityGroundConstraint {
jacobian_id: &mut usize,
insert_at: Option<usize>,
) {
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<Real>,
generic_mj_lambdas: &mut DVector<Real>,
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();
}
}

7
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -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,

44
src/dynamics/solver/velocity_constraint.rs
View File

@@ -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<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
@@ -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<Real>, // One of the friction force directions.
pub im1: Vector<Real>,
pub im2: Vector<Real>,
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<Real>],
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;
}

22
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -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<SimdReal>,
pub im2: Vector<SimdReal>,
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<SimdReal> =
@@ -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<Real>],
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;
}

24
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -17,6 +17,7 @@ pub(crate) struct VelocityGroundConstraint {
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS],
@@ -34,6 +35,7 @@ impl VelocityGroundConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
insert_at: Option<usize>,
) {
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<Real>],
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;
}

31
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -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<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im2: Vector<SimdReal>,
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<AnyVelocityConstraint>,
insert_at: Option<usize>,
) {
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<Real>],
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;
}

7
src/dynamics/solver/velocity_solver.rs
View File

@@ -35,7 +35,6 @@ impl VelocitySolver {
joint_constraints: &mut [AnyJointVelocityConstraint],
generic_joint_jacobians: &DVector<Real>,
) {
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,

74
src/pipeline/debug_render_pipeline/debug_render_pipeline.rs
View File

@@ -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")]

12
src/pipeline/debug_render_pipeline/debug_render_style.rs
View File

@@ -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],
}
}
}

3
src_testbed/debug_render.rs
View File

@@ -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,
);
}

8
src_testbed/lines/debuglines.wgsl
View File

@@ -25,9 +25,13 @@ struct FragmentOutput {
fn vertex(vertex: Vertex) -> VertexOutput {
var out: VertexOutput;
out.clip_position = view.view_proj * vec4<f32>(vertex.pos, 1.0);
//out.color = vertex.color;
// https://github.com/bevyengine/bevy/blob/328c26d02c50de0bc77f0d24a376f43ba89517b1/examples/2d/mesh2d_manual.rs#L234
out.color = vec4<f32>((vec4<u32>(vertex.color) >> vec4<u32>(8u, 8u, 16u, 24u)) & vec4<u32>(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<f32>(r, g, b, a);
return out;
}

9
src_testbed/lines/debuglines2d.wgsl
View File

@@ -17,10 +17,11 @@ struct VertexOutput {
fn vertex(vertex: Vertex) -> VertexOutput {
var out: VertexOutput;
out.clip_position = view.view_proj * vec4<f32>(vertex.place, 1.0);
// What is this craziness?
out.color = vec4<f32>((vec4<u32>(vertex.color) >> vec4<u32>(0u, 8u, 16u, 24u)) & vec4<u32>(255u)) / 255.0;
//out.color = vertex.color;
//out.color = vec4<f32>(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<f32>(r, g, b, a);
return out;
}

11
src_testbed/lines/mod.rs
View File

@@ -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<Assets<Mesh>>) {
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<Entity, With<DebugLinesMesh>>) {
}
#[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;
}

24
src_testbed/testbed.rs
View File

@@ -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);
}
}
_ => {}