From c3f39f5164600cf2d1ce379e37328562080abc1c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 24 Oct 2025 10:04:02 +0200 Subject: [PATCH 01/12] feat: initial implementation of contact manifold reduction --- src/geometry/narrow_phase.rs | 117 +++++++++++++++++++++++++++-------- 1 file changed, 92 insertions(+), 25 deletions(-) diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 64805a7a7..757046317 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -10,10 +10,10 @@ use crate::dynamics::{ use crate::geometry::{ BoundingVolume, BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, - ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags, + ContactPair, InteractionGraph, IntersectionPair, Segment, SolverContact, SolverFlags, TemporaryInteractionIndex, }; -use crate::math::{Real, Vector}; +use crate::math::{MAX_MANIFOLD_POINTS, Real, Vector}; use crate::pipeline::{ ActiveEvents, ActiveHooks, ContactModificationContext, EventHandler, PairFilterContext, PhysicsHooks, @@ -831,7 +831,8 @@ impl NarrowPhase { // No update needed for these colliders. return; } - if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) && co1.parent.is_some() + if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) + && co1.parent.is_some() { // Same parents. Ignore collisions. pair.clear(); @@ -858,7 +859,7 @@ impl NarrowPhase { let link1 = multibody_joints.rigid_body_link(co_parent1.handle); let link2 = multibody_joints.rigid_body_link(co_parent2.handle); - if let (Some(link1),Some(link2)) = (link1, link2) { + if let (Some(link1), Some(link2)) = (link1, link2) { // If both bodies belong to the same multibody, apply some additional built-in // contact filtering rules. if link1.multibody == link2.multibody { @@ -936,27 +937,30 @@ impl NarrowPhase { let contact_skin_sum = co1.contact_skin() + co2.contact_skin(); let soft_ccd_prediction1 = rb1.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0); let soft_ccd_prediction2 = rb2.map(|rb| rb.soft_ccd_prediction()).unwrap_or(0.0); - let effective_prediction_distance = if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 { + let effective_prediction_distance = + if soft_ccd_prediction1 > 0.0 || soft_ccd_prediction2 > 0.0 { let aabb1 = co1.compute_collision_aabb(0.0); let aabb2 = co2.compute_collision_aabb(0.0); let inv_dt = crate::utils::inv(dt); - let linvel1 = rb1.map(|rb| rb.linvel() - .cap_magnitude(soft_ccd_prediction1 * inv_dt)).unwrap_or_default(); - let linvel2 = rb2.map(|rb| rb.linvel() - .cap_magnitude(soft_ccd_prediction2 * inv_dt)).unwrap_or_default(); + let linvel1 = rb1 + .map(|rb| rb.linvel().cap_magnitude(soft_ccd_prediction1 * inv_dt)) + .unwrap_or_default(); + let linvel2 = rb2 + .map(|rb| rb.linvel().cap_magnitude(soft_ccd_prediction2 * inv_dt)) + .unwrap_or_default(); - if !aabb1.intersects(&aabb2) && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) { + if !aabb1.intersects(&aabb2) + && !aabb1.intersects_moving_aabb(&aabb2, linvel2 - linvel1) + { pair.clear(); break 'emit_events; } - - prediction_distance.max( - dt * (linvel1 - linvel2).norm()) + contact_skin_sum - } else { - prediction_distance + contact_skin_sum - }; + prediction_distance.max(dt * (linvel1 - linvel2).norm()) + contact_skin_sum + } else { + prediction_distance + contact_skin_sum + }; let _ = query_dispatcher.contact_manifolds( &pos12, @@ -998,20 +1002,83 @@ impl NarrowPhase { manifold.data.normal = world_pos1 * manifold.local_n1; // Generate solver contacts. - for (contact_id, contact) in manifold.points.iter().enumerate() { - if contact_id > u8::MAX as usize { - log::warn!("A contact manifold cannot contain more than 255 contacts currently, dropping contact in excess."); - break; + let mut selected = [0, 1, 2, 3]; + + #[cfg(feature = "dim3")] + { + let mut solver_contacts_len = 0; + + if manifold.points.len() > 4 { + // Run contact reduction so we only have up to four solver contacts. + // We follow a logic similar to Bepu’s approach. + // 1. Find the deepest contact. + let mut deepest_dist = manifold.points[0].dist; + for (i, pt) in manifold.points.iter().enumerate() { + if pt.dist < deepest_dist { + deepest_dist = pt.dist; + selected[0] = i; + } + } + + // 2. Find the point that is the furthest from the deepest one. + let selected_a = &manifold.points[selected[0]].local_p1; + let mut furthest_dist = -f32::MAX; + for (i, pt) in manifold.points.iter().enumerate() { + let dist = na::distance_squared(&pt.local_p1, &selected_a); + if i != selected[0] + && pt.dist < prediction_distance + && dist > furthest_dist + { + furthest_dist = dist; + selected[1] = i; + } + } + + // 3. Now find the two points furthest from the segment we built so far. + let selected_b = &manifold.points[selected[1]].local_p1; + let selected_ab = selected_b - selected_a; + let tangent = selected_ab.cross(&manifold.local_n1); + + // Find the points that minimize and maximize the dot product with the tangent. + let mut min_dot = Real::MAX; + let mut max_dot = -Real::MAX; + for (i, pt) in manifold.points.iter().enumerate() { + if i == selected[0] || i == selected[1] { + continue; + } + + let dot = (pt.local_p1 - selected_a).dot(&tangent); + if dot < min_dot { + min_dot = dot; + selected[2] = i; + } + + if dot > max_dot { + max_dot = dot; + selected[3] = i; + } + } } + } - let effective_contact_dist = contact.dist - co1.contact_skin() - co2.contact_skin(); + let mut solver_contacts_len = 0; + for contact_id in &selected[..MAX_MANIFOLD_POINTS.min(manifold.points.len())] { + // manifold.points.iter().enumerate() { + let contact = &manifold.points[*contact_id]; + let effective_contact_dist = + contact.dist - co1.contact_skin() - co2.contact_skin(); let keep_solver_contact = effective_contact_dist < prediction_distance || { let world_pt1 = world_pos1 * contact.local_p1; let world_pt2 = world_pos2 * contact.local_p2; - let vel1 = rb1.map(|rb| rb.velocity_at_point(&world_pt1)).unwrap_or_default(); - let vel2 = rb2.map(|rb| rb.velocity_at_point(&world_pt2)).unwrap_or_default(); - effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt < prediction_distance + let vel1 = rb1 + .map(|rb| rb.velocity_at_point(&world_pt1)) + .unwrap_or_default(); + let vel2 = rb2 + .map(|rb| rb.velocity_at_point(&world_pt2)) + .unwrap_or_default(); + effective_contact_dist + (vel2 - vel1).dot(&manifold.data.normal) * dt + < prediction_distance }; if keep_solver_contact { @@ -1021,7 +1088,7 @@ impl NarrowPhase { let effective_point = na::center(&world_pt1, &world_pt2); let solver_contact = SolverContact { - contact_id: [contact_id as u32], + contact_id: [*contact_id as u32], point: effective_point, dist: effective_contact_dist, friction, From 4a888dba86bc2102798c9a2e3af4063b065aedbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 24 Oct 2025 10:14:41 +0200 Subject: [PATCH 02/12] feat: try bepu-like manifold reduction --- src/geometry/narrow_phase.rs | 142 +++++++++++++++++++++++++---------- 1 file changed, 104 insertions(+), 38 deletions(-) diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 757046317..f05059875 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -1003,67 +1003,133 @@ impl NarrowPhase { // Generate solver contacts. let mut selected = [0, 1, 2, 3]; + let mut num_selected = MAX_MANIFOLD_POINTS.min(manifold.points.len()); #[cfg(feature = "dim3")] { - let mut solver_contacts_len = 0; - if manifold.points.len() > 4 { - // Run contact reduction so we only have up to four solver contacts. - // We follow a logic similar to Bepu’s approach. - // 1. Find the deepest contact. - let mut deepest_dist = manifold.points[0].dist; + // Run contact reduction using Bepu's InternalReduce algorithm. + // This reduces the manifold to at most 4 contacts using: + // 1. Deepest contact (with extremity bias for stability) + // 2. Most distant contact from the first + // 3. Two contacts that maximize positive and negative signed area with the first edge + + // Step 1: Find the deepest contact, biased by extremity for frame stability. + // The extremity heuristic helps maintain consistent contact selection across frames + // when multiple contacts have similar depths. + let mut best_score = -Real::MAX; + const EXTREMITY_SCALE: Real = 1e-2; + // Use an arbitrary direction (roughly 38 degrees from X axis) to break ties + const EXTREMITY_DIR_X: Real = 0.7946897654; + const EXTREMITY_DIR_Y: Real = 0.60701579614; + for (i, pt) in manifold.points.iter().enumerate() { - if pt.dist < deepest_dist { - deepest_dist = pt.dist; + // Extremity measures how far the contact is from the origin in the tangent plane + let extremity = (pt.local_p1.x * EXTREMITY_DIR_X + + pt.local_p1.y * EXTREMITY_DIR_Y) + .abs(); + + // Score = depth + small extremity bias (only for non-speculative contacts) + // Negative dist = deeper penetration = higher score + let score = if pt.dist >= 0.0 { + -pt.dist // Speculative contact, no extremity bias + } else { + -pt.dist + extremity * EXTREMITY_SCALE + }; + + if score > best_score { + best_score = score; selected[0] = i; } } - // 2. Find the point that is the furthest from the deepest one. - let selected_a = &manifold.points[selected[0]].local_p1; - let mut furthest_dist = -f32::MAX; + // Step 2: Find the point most distant from the first contact. + // This establishes a baseline "edge" for the manifold. + let contact0_pos = manifold.points[selected[0]].local_p1; + let mut max_distance_squared = 0.0; + for (i, pt) in manifold.points.iter().enumerate() { - let dist = na::distance_squared(&pt.local_p1, &selected_a); - if i != selected[0] - && pt.dist < prediction_distance - && dist > furthest_dist - { - furthest_dist = dist; + let offset = pt.local_p1 - contact0_pos; + let distance_squared = offset.x * offset.x + offset.y * offset.y; + + if distance_squared > max_distance_squared { + max_distance_squared = distance_squared; selected[1] = i; } } - // 3. Now find the two points furthest from the segment we built so far. - let selected_b = &manifold.points[selected[1]].local_p1; - let selected_ab = selected_b - selected_a; - let tangent = selected_ab.cross(&manifold.local_n1); - - // Find the points that minimize and maximize the dot product with the tangent. - let mut min_dot = Real::MAX; - let mut max_dot = -Real::MAX; - for (i, pt) in manifold.points.iter().enumerate() { - if i == selected[0] || i == selected[1] { - continue; + // Early out if the contacts are too close together + let epsilon = 1e-6; + if max_distance_squared <= epsilon { + // Only one meaningful contact + num_selected = 1; + } else { + // Step 3: Find two more contacts that maximize positive and negative signed area. + // Using the first two contacts as an edge, we look for contacts that form triangles + // with the largest magnitude negative and positive areas. This maximizes the + // spatial extent of the contact manifold. + + num_selected = 2; + selected[2] = usize::MAX; + selected[3] = usize::MAX; + + let contact1_pos = manifold.points[selected[1]].local_p1; + let edge_offset_x = contact1_pos.x - contact0_pos.x; + let edge_offset_y = contact1_pos.y - contact0_pos.y; + + let mut min_signed_area = 0.0; + let mut max_signed_area = 0.0; + + for (i, pt) in manifold.points.iter().enumerate() { + let candidate_offset_x = pt.local_p1.x - contact0_pos.x; + let candidate_offset_y = pt.local_p1.y - contact0_pos.y; + + // Signed area of the triangle formed by (contact0, contact1, candidate) + // This is a 2D cross product: (candidate - contact0) × (contact1 - contact0) + let mut signed_area = candidate_offset_x * edge_offset_y + - candidate_offset_y * edge_offset_x; + + // Penalize speculative contacts (they're less important) + if pt.dist >= 0.0 { + signed_area *= 0.25; + } + + if signed_area < min_signed_area { + min_signed_area = signed_area; + selected[2] = i; + } + if signed_area > max_signed_area { + max_signed_area = signed_area; + selected[3] = i; + } } - let dot = (pt.local_p1 - selected_a).dot(&tangent); - if dot < min_dot { - min_dot = dot; - selected[2] = i; + // Check if the signed areas are significant enough + // Epsilon based on the edge length squared + let area_epsilon = + max_distance_squared * max_distance_squared * 1e-6; + + // If the areas are too small, don't add those contacts + if min_signed_area * min_signed_area <= area_epsilon { + selected[2] = usize::MAX; } + if max_signed_area * max_signed_area <= area_epsilon { + selected[3] = usize::MAX; + } + + let keep2 = selected[2] != usize::MAX; + let keep3 = selected[3] != usize::MAX; + num_selected += keep2 as usize + keep3 as usize; - if dot > max_dot { - max_dot = dot; - selected[3] = i; + if !keep2 { + selected[2] = selected[3]; } } } } - let mut solver_contacts_len = 0; - for contact_id in &selected[..MAX_MANIFOLD_POINTS.min(manifold.points.len())] { - // manifold.points.iter().enumerate() { + for contact_id in &selected[..num_selected] { + // // manifold.points.iter().enumerate() { let contact = &manifold.points[*contact_id]; let effective_contact_dist = contact.dist - co1.contact_skin() - co2.contact_skin(); From 9d63781777c1ededfa6574af7c3eb7d5932d4afe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 24 Oct 2025 16:08:47 +0200 Subject: [PATCH 03/12] feat: simplification of the constraints counting and indexing logic --- .../contact_constraints_set.rs | 98 ++--- .../contact_with_coulomb_friction.rs | 288 ++++++++------- .../contact_with_twist_friction.rs | 273 +++++++------- .../generic_contact_constraint.rs | 340 +++++++++--------- 4 files changed, 471 insertions(+), 528 deletions(-) diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index 57523e2c0..e9a55321a 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -31,14 +31,8 @@ pub struct ConstraintsCounts { } impl ConstraintsCounts { - pub fn from_contacts(manifold: &ContactManifold) -> Self { - let rest = manifold.data.solver_contacts.len() % MAX_MANIFOLD_POINTS != 0; - Self { - num_constraints: manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS - + rest as usize, - num_jacobian_lines: manifold.data.solver_contacts.len() * DIM, - } - } + // NOTE: constraints count from contacts is always 1 since the max number of solver contacts + // matches the max number of contact per constraint. pub fn from_joint(joint: &ImpulseJoint) -> Self { let joint = &joint.data; @@ -334,18 +328,8 @@ impl ContactConstraintsSet { solver_bodies: &SolverBodies, manifolds_all: &[&mut ContactManifold], ) { - let total_num_constraints = self - .interaction_groups - .simd_interactions - .chunks_exact(SIMD_WIDTH) - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) - .sum::() - + self - .interaction_groups - .nongrouped_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum::(); + let total_num_constraints = (self.interaction_groups.simd_interactions.len() / SIMD_WIDTH) + + self.interaction_groups.nongrouped_interactions.len(); unsafe { reset_buffer( @@ -358,15 +342,14 @@ impl ContactConstraintsSet { ); } - let mut curr_start = 0; + // TODO PERF: could avoid this index using zip. + let mut curr_id = 0; for manifolds_i in self .interaction_groups .simd_interactions .chunks_exact(SIMD_WIDTH) { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints; let manifold_id = array![|ii| manifolds_i[ii]]; let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]]; @@ -375,16 +358,14 @@ impl ContactConstraintsSet { manifolds, bodies, solver_bodies, - &mut self.simd_velocity_twist_constraints_builder[curr_start..], - &mut self.simd_velocity_twist_constraints[curr_start..], + &mut self.simd_velocity_twist_constraints_builder[curr_id], + &mut self.simd_velocity_twist_constraints[curr_id], ); - curr_start += num_to_add; + curr_id += 1; } for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints; let mut manifold_id = [usize::MAX; SIMD_WIDTH]; manifold_id[0] = *manifolds_i; let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH]; @@ -394,14 +375,14 @@ impl ContactConstraintsSet { manifolds, bodies, solver_bodies, - &mut self.simd_velocity_twist_constraints_builder[curr_start..], - &mut self.simd_velocity_twist_constraints[curr_start..], + &mut self.simd_velocity_twist_constraints_builder[curr_id], + &mut self.simd_velocity_twist_constraints[curr_id], ); - curr_start += num_to_add; + curr_id += 1; } - assert_eq!(curr_start, total_num_constraints); + assert_eq!(curr_id, total_num_constraints); } fn simd_compute_coulomb_constraints( @@ -410,18 +391,8 @@ impl ContactConstraintsSet { solver_bodies: &SolverBodies, manifolds_all: &[&mut ContactManifold], ) { - let total_num_constraints = self - .interaction_groups - .simd_interactions - .chunks_exact(SIMD_WIDTH) - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints) - .sum::() - + self - .interaction_groups - .nongrouped_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum::(); + let total_num_constraints = self.interaction_groups.simd_interactions.len() / SIMD_WIDTH + + self.interaction_groups.nongrouped_interactions.len(); unsafe { reset_buffer( @@ -434,15 +405,14 @@ impl ContactConstraintsSet { ); } - let mut curr_start = 0; + // TODO PERF: could avoid this index using zip. + let mut curr_id = 0; for manifolds_i in self .interaction_groups .simd_interactions .chunks_exact(SIMD_WIDTH) { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints; let manifold_id = array![|ii| manifolds_i[ii]]; let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]]; @@ -451,16 +421,14 @@ impl ContactConstraintsSet { manifolds, bodies, solver_bodies, - &mut self.simd_velocity_coulomb_constraints_builder[curr_start..], - &mut self.simd_velocity_coulomb_constraints[curr_start..], + &mut self.simd_velocity_coulomb_constraints_builder[curr_id], + &mut self.simd_velocity_coulomb_constraints[curr_id], ); - curr_start += num_to_add; + curr_id += 1; } for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() { - let num_to_add = - ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints; let mut manifold_id = [usize::MAX; SIMD_WIDTH]; manifold_id[0] = *manifolds_i; let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH]; @@ -470,14 +438,14 @@ impl ContactConstraintsSet { manifolds, bodies, solver_bodies, - &mut self.simd_velocity_coulomb_constraints_builder[curr_start..], - &mut self.simd_velocity_coulomb_constraints[curr_start..], + &mut self.simd_velocity_coulomb_constraints_builder[curr_id], + &mut self.simd_velocity_coulomb_constraints[curr_id], ); - curr_start += num_to_add; + curr_id += 1; } - assert_eq!(curr_start, total_num_constraints); + assert_eq!(curr_id, total_num_constraints); } fn compute_generic_constraints( @@ -487,11 +455,7 @@ impl ContactConstraintsSet { manifolds_all: &[&mut ContactManifold], jacobian_id: &mut usize, ) { - let total_num_constraints = self - .generic_two_body_interactions - .iter() - .map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints) - .sum::(); + let total_num_constraints = self.generic_two_body_interactions.len(); self.generic_velocity_constraints_builder.resize( total_num_constraints, @@ -500,27 +464,27 @@ impl ContactConstraintsSet { self.generic_velocity_constraints .resize(total_num_constraints, GenericContactConstraint::invalid()); - let mut curr_start = 0; + // TODO PERF: could avoid this index using zip. + let mut curr_id = 0; for manifold_i in &self.generic_two_body_interactions { let manifold = &manifolds_all[*manifold_i]; - let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints; GenericContactConstraintBuilder::generate( *manifold_i, manifold, bodies, multibody_joints, - &mut self.generic_velocity_constraints_builder[curr_start..], - &mut self.generic_velocity_constraints[curr_start..], + &mut self.generic_velocity_constraints_builder[curr_id], + &mut self.generic_velocity_constraints[curr_id], &mut self.generic_jacobians, jacobian_id, ); - curr_start += num_to_add; + curr_id += 1; } - assert_eq!(curr_start, total_num_constraints); + assert_eq!(curr_id, total_num_constraints); } pub fn warmstart( diff --git a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs index 50c0bc057..09dd92908 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs @@ -43,8 +43,8 @@ impl ContactWithCoulombFrictionBuilder { manifolds: [&ContactManifold; SIMD_WIDTH], bodies: &RigidBodySet, solver_bodies: &SolverBodies, - out_builders: &mut [ContactWithCoulombFrictionBuilder], - out_constraints: &mut [ContactWithCoulombFriction], + out_builder: &mut ContactWithCoulombFrictionBuilder, + out_constraint: &mut ContactWithCoulombFriction, ) { // TODO: could we avoid having to fetch the ids here? It’s the only thing we // read from the original rigid-bodies. @@ -83,165 +83,159 @@ impl ContactWithCoulombFrictionBuilder { let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear); - for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = - array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; - let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); - - let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; - let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; - - constraint.dir1 = force_dir1; - constraint.im1 = poses1.im; - constraint.im2 = poses2.im; - constraint.solver_vel1 = ids1; - constraint.solver_vel2 = ids2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = num_points as u8; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - - for k in 0..num_points { - // SAFETY: we already know that the `manifold_points` has `num_points` elements - // so `k` isn’t out of bounds. - let solver_contact = - unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; - - let is_bouncy = solver_contact.is_bouncy(); - - let dp1 = solver_contact.point - world_com1; - let dp2 = solver_contact.point - world_com2; - - let vel1 = vels1.linear + vels1.angular.gcross(dp1); - let vel2 = vels2.linear + vels2.angular.gcross(dp2); - - constraint.limit = solver_contact.friction; - constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); + let manifold_points = + array![|ii| &manifolds[ii].data.solver_contacts[..num_active_contacts]]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + out_constraint.dir1 = force_dir1; + out_constraint.im1 = poses1.im; + out_constraint.im2 = poses2.im; + out_constraint.solver_vel1 = ids1; + out_constraint.solver_vel2 = ids2; + out_constraint.manifold_id = manifold_id; + out_constraint.num_contacts = num_points as u8; + #[cfg(feature = "dim3")] + { + out_constraint.tangent1 = tangents1[0]; + } - // Normal part. - let normal_rhs_wo_bias; - { - let torque_dir1 = dp1.gcross(force_dir1); - let torque_dir2 = dp2.gcross(-force_dir1); - let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); - let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + for k in 0..num_points { + // SAFETY: we already know that the `manifold_points` has `num_points` elements + // so `k` isn’t out of bounds. + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; - let imsum = poses1.im + poses2.im; - let projected_mass = utils::simd_inv( - force_dir1.dot(&imsum.component_mul(&force_dir1)) - + ii_torque_dir1.gdot(torque_dir1) - + ii_torque_dir2.gdot(torque_dir2), - ); + let is_bouncy = solver_contact.is_bouncy(); - let projected_velocity = (vel1 - vel2).dot(&force_dir1); - normal_rhs_wo_bias = - is_bouncy * solver_contact.restitution * projected_velocity; + let dp1 = solver_contact.point - world_com1; + let dp2 = solver_contact.point - world_com2; - constraint.normal_part[k].torque_dir1 = torque_dir1; - constraint.normal_part[k].torque_dir2 = torque_dir2; - constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; - constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; - constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; - constraint.normal_part[k].r = projected_mass; - } + let vel1 = vels1.linear + vels1.angular.gcross(dp1); + let vel2 = vels2.linear + vels2.angular.gcross(dp2); - // tangent parts. - constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse; + out_constraint.limit = solver_contact.friction; + out_constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); - for j in 0..DIM - 1 { - let torque_dir1 = dp1.gcross(tangents1[j]); - let torque_dir2 = dp2.gcross(-tangents1[j]); - let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); - let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + let projected_mass = utils::simd_inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2), + ); - let imsum = poses1.im + poses2.im; + let projected_velocity = (vel1 - vel2).dot(&force_dir1); + normal_rhs_wo_bias = is_bouncy * solver_contact.restitution * projected_velocity; - let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + ii_torque_dir1.gdot(torque_dir1) - + ii_torque_dir2.gdot(torque_dir2); - let rhs_wo_bias = solver_contact.tangent_velocity.dot(&tangents1[j]); - - constraint.tangent_part[k].torque_dir1[j] = torque_dir1; - constraint.tangent_part[k].torque_dir2[j] = torque_dir2; - constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; - constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; - constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; - constraint.tangent_part[k].rhs[j] = rhs_wo_bias; - constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") { - utils::simd_inv(r) - } else { - r - }; - } + out_constraint.normal_part[k].torque_dir1 = torque_dir1; + out_constraint.normal_part[k].torque_dir2 = torque_dir2; + out_constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; + out_constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; + out_constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; + out_constraint.normal_part[k].r = projected_mass; + } - #[cfg(feature = "dim3")] - { - // TODO PERF: we already applied the inverse inertia to the torque - // dire before. Could we reuse the value instead of retransforming? - constraint.tangent_part[k].r[2] = SimdReal::splat(2.0) - * (constraint.tangent_part[k].ii_torque_dir1[0] - .gdot(constraint.tangent_part[k].torque_dir1[1]) - + constraint.tangent_part[k].ii_torque_dir2[0] - .gdot(constraint.tangent_part[k].torque_dir2[1])); - } + // tangent parts. + out_constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse; + + for j in 0..DIM - 1 { + let torque_dir1 = dp1.gcross(tangents1[j]); + let torque_dir2 = dp2.gcross(-tangents1[j]); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2); + let rhs_wo_bias = solver_contact.tangent_velocity.dot(&tangents1[j]); + + out_constraint.tangent_part[k].torque_dir1[j] = torque_dir1; + out_constraint.tangent_part[k].torque_dir2[j] = torque_dir2; + out_constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; + out_constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; + out_constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; + out_constraint.tangent_part[k].rhs[j] = rhs_wo_bias; + out_constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") { + utils::simd_inv(r) + } else { + r + }; + } - // Builder. - builder.infos[k].local_p1 = - poses1.pose.inverse_transform_point(&solver_contact.point); - builder.infos[k].local_p2 = - poses2.pose.inverse_transform_point(&solver_contact.point); - builder.infos[k].tangent_vel = solver_contact.tangent_velocity; - builder.infos[k].dist = solver_contact.dist; - builder.infos[k].normal_vel = normal_rhs_wo_bias; + #[cfg(feature = "dim3")] + { + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + out_constraint.tangent_part[k].r[2] = SimdReal::splat(2.0) + * (out_constraint.tangent_part[k].ii_torque_dir1[0] + .gdot(out_constraint.tangent_part[k].torque_dir1[1]) + + out_constraint.tangent_part[k].ii_torque_dir2[0] + .gdot(out_constraint.tangent_part[k].torque_dir2[1])); } - if BLOCK_SOLVER_ENABLED { - // Coupling between consecutive pairs. - for k in 0..num_points / 2 { - let k0 = k * 2; - let k1 = k * 2 + 1; - - let imsum = poses1.im + poses2.im; - let r0 = constraint.normal_part[k0].r; - let r1 = constraint.normal_part[k1].r; - - let mut r_mat = SdpMatrix2::zero(); - - // TODO PERF: we already applied the inverse inertia to the torque - // dire before. Could we reuse the value instead of retransforming? - r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) - + constraint.normal_part[k0] - .ii_torque_dir1 - .gdot(constraint.normal_part[k1].torque_dir1) - + constraint.normal_part[k0] - .ii_torque_dir2 - .gdot(constraint.normal_part[k1].torque_dir2); - r_mat.m11 = utils::simd_inv(r0); - r_mat.m22 = utils::simd_inv(r1); - - let (inv, det) = { - let _disable_fe_except = + // Builder. + out_builder.infos[k].local_p1 = + poses1.pose.inverse_transform_point(&solver_contact.point); + out_builder.infos[k].local_p2 = + poses2.pose.inverse_transform_point(&solver_contact.point); + out_builder.infos[k].tangent_vel = solver_contact.tangent_velocity; + out_builder.infos[k].dist = solver_contact.dist; + out_builder.infos[k].normal_vel = normal_rhs_wo_bias; + } + + if BLOCK_SOLVER_ENABLED { + // Coupling between consecutive pairs. + for k in 0..num_points / 2 { + let k0 = k * 2; + let k1 = k * 2 + 1; + + let imsum = poses1.im + poses2.im; + let r0 = out_constraint.normal_part[k0].r; + let r1 = out_constraint.normal_part[k1].r; + + let mut r_mat = SdpMatrix2::zero(); + + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1)) + + out_constraint.normal_part[k0] + .ii_torque_dir1 + .gdot(out_constraint.normal_part[k1].torque_dir1) + + out_constraint.normal_part[k0] + .ii_torque_dir2 + .gdot(out_constraint.normal_part[k1].torque_dir2); + r_mat.m11 = utils::simd_inv(r0); + r_mat.m22 = utils::simd_inv(r1); + + let (inv, det) = { + let _disable_fe_except = crate::utils::DisableFloatingPointExceptionsFlags:: disable_floating_point_exceptions(); - r_mat.inverse_and_get_determinant_unchecked() - }; - let is_invertible = det.simd_gt(SimdReal::zero()); - - // If inversion failed, the contacts are redundant. - // Ignore the one with the smallest depth (it is too late to - // have the constraint removed from the constraint set, so just - // set the mass (r) matrix elements to 0. - constraint.normal_part[k0].r_mat_elts = [ - inv.m11.select(is_invertible, r0), - inv.m22.select(is_invertible, SimdReal::zero()), - ]; - constraint.normal_part[k1].r_mat_elts = [ - inv.m12.select(is_invertible, SimdReal::zero()), - r_mat.m12.select(is_invertible, SimdReal::zero()), - ]; - } + r_mat.inverse_and_get_determinant_unchecked() + }; + let is_invertible = det.simd_gt(SimdReal::zero()); + + // If inversion failed, the contacts are redundant. + // Ignore the one with the smallest depth (it is too late to + // have the constraint removed from the constraint set, so just + // set the mass (r) matrix elements to 0. + out_constraint.normal_part[k0].r_mat_elts = [ + inv.m11.select(is_invertible, r0), + inv.m22.select(is_invertible, SimdReal::zero()), + ]; + out_constraint.normal_part[k1].r_mat_elts = [ + inv.m12.select(is_invertible, SimdReal::zero()), + r_mat.m12.select(is_invertible, SimdReal::zero()), + ]; } } } diff --git a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs index fc50997f2..1fce11fb3 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs @@ -50,8 +50,8 @@ impl ContactWithTwistFrictionBuilder { manifolds: [&ContactManifold; SIMD_WIDTH], bodies: &RigidBodySet, solver_bodies: &SolverBodies, - out_builders: &mut [ContactWithTwistFrictionBuilder], - out_constraints: &mut [ContactWithTwistFriction], + out_builder: &mut ContactWithTwistFrictionBuilder, + out_constraint: &mut ContactWithTwistFriction, ) { // TODO: could we avoid having to fetch the ids here? It’s the only thing we // read from the original rigid-bodies. @@ -90,166 +90,159 @@ impl ContactWithTwistFrictionBuilder { let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear); - for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) { - let manifold_points = - array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; - let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + let manifold_points = + array![|ii| &manifolds[ii].data.solver_contacts[..num_active_contacts]]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); - let inv_num_points = SimdReal::splat(1.0 / num_points as Real); + let inv_num_points = SimdReal::splat(1.0 / num_points as Real); - let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS]; - let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS]; - - constraint.dir1 = force_dir1; - constraint.im1 = poses1.im; - constraint.im2 = poses2.im; - constraint.solver_vel1 = ids1; - constraint.solver_vel2 = ids2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = num_points as u8; - #[cfg(feature = "dim3")] - { - constraint.tangent1 = tangents1[0]; - } - - let mut friction_center = Point::origin(); - let mut twist_warmstart = na::zero(); - let mut tangent_warmstart = na::zero(); - let mut tangent_vel: Vector<_> = na::zero(); + out_constraint.dir1 = force_dir1; + out_constraint.im1 = poses1.im; + out_constraint.im2 = poses2.im; + out_constraint.solver_vel1 = ids1; + out_constraint.solver_vel2 = ids2; + out_constraint.manifold_id = manifold_id; + out_constraint.num_contacts = num_points as u8; + #[cfg(feature = "dim3")] + { + out_constraint.tangent1 = tangents1[0]; + } - for k in 0..num_points { - // SAFETY: we already know that the `manifold_points` has `num_points` elements - // so `k` isn’t out of bounds. - let solver_contact = - unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; + let mut friction_center = Point::origin(); + let mut twist_warmstart = na::zero(); + let mut tangent_warmstart = na::zero(); + let mut tangent_vel: Vector<_> = na::zero(); - let is_bouncy = solver_contact.is_bouncy(); + for k in 0..num_points { + // SAFETY: we already know that the `manifold_points` has `num_points` elements + // so `k` isn’t out of bounds. + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; - friction_center += solver_contact.point.coords * inv_num_points; + let is_bouncy = solver_contact.is_bouncy(); - let dp1 = solver_contact.point - world_com1; - let dp2 = solver_contact.point - world_com2; + friction_center += solver_contact.point.coords * inv_num_points; - let vel1 = vels1.linear + vels1.angular.gcross(dp1); - let vel2 = vels2.linear + vels2.angular.gcross(dp2); + let dp1 = solver_contact.point - world_com1; + let dp2 = solver_contact.point - world_com2; - twist_warmstart += solver_contact.warmstart_twist_impulse * inv_num_points; - tangent_warmstart += solver_contact.warmstart_tangent_impulse * inv_num_points; - tangent_vel += solver_contact.tangent_velocity * inv_num_points; + let vel1 = vels1.linear + vels1.angular.gcross(dp1); + let vel2 = vels2.linear + vels2.angular.gcross(dp2); - constraint.limit = solver_contact.friction; - constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); + twist_warmstart += solver_contact.warmstart_twist_impulse * inv_num_points; + tangent_warmstart += solver_contact.warmstart_tangent_impulse * inv_num_points; + tangent_vel += solver_contact.tangent_velocity * inv_num_points; - // Normal part. - let normal_rhs_wo_bias; - { - let torque_dir1 = dp1.gcross(force_dir1); - let torque_dir2 = dp2.gcross(-force_dir1); - let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); - let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + out_constraint.limit = solver_contact.friction; + out_constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8); - let imsum = poses1.im + poses2.im; - let projected_mass = utils::simd_inv( - force_dir1.dot(&imsum.component_mul(&force_dir1)) - + ii_torque_dir1.gdot(torque_dir1) - + ii_torque_dir2.gdot(torque_dir2), - ); + // Normal part. + let normal_rhs_wo_bias; + { + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); - let projected_velocity = (vel1 - vel2).dot(&force_dir1); - normal_rhs_wo_bias = - is_bouncy * solver_contact.restitution * projected_velocity; + let imsum = poses1.im + poses2.im; + let projected_mass = utils::simd_inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2), + ); - constraint.normal_part[k].torque_dir1 = torque_dir1; - constraint.normal_part[k].torque_dir2 = torque_dir2; - constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; - constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; - constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; - constraint.normal_part[k].r = projected_mass; - } + let projected_velocity = (vel1 - vel2).dot(&force_dir1); + normal_rhs_wo_bias = is_bouncy * solver_contact.restitution * projected_velocity; - // Builder. - builder.infos[k].local_p1 = - poses1.pose.inverse_transform_point(&solver_contact.point); - builder.infos[k].local_p2 = - poses2.pose.inverse_transform_point(&solver_contact.point); - builder.infos[k].dist = solver_contact.dist; - builder.infos[k].normal_vel = normal_rhs_wo_bias; + out_constraint.normal_part[k].torque_dir1 = torque_dir1; + out_constraint.normal_part[k].torque_dir2 = torque_dir2; + out_constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1; + out_constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2; + out_constraint.normal_part[k].impulse = solver_contact.warmstart_impulse; + out_constraint.normal_part[k].r = projected_mass; } - /* - * Tangent/twist part - */ - constraint.tangent_part.impulse = tangent_warmstart; - constraint.twist_part.impulse = twist_warmstart; - - builder.local_friction_center1 = poses1.pose.inverse_transform_point(&friction_center); - builder.local_friction_center2 = poses2.pose.inverse_transform_point(&friction_center); - - let dp1 = friction_center - world_com1; - let dp2 = friction_center - world_com2; - - // Twist part. It has no effect when there is only one point. - if num_points > 1 { - let mut twist_dists = [SimdReal::zero(); MAX_MANIFOLD_POINTS]; - for k in 0..num_points { - // FIXME PERF: we don’t want to re-fetch here just to get the solver contact point! - let solver_contact = - unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; - twist_dists[k] = nalgebra::distance(&friction_center, &solver_contact.point); - } + // Builder. + out_builder.infos[k].local_p1 = + poses1.pose.inverse_transform_point(&solver_contact.point); + out_builder.infos[k].local_p2 = + poses2.pose.inverse_transform_point(&solver_contact.point); + out_builder.infos[k].dist = solver_contact.dist; + out_builder.infos[k].normal_vel = normal_rhs_wo_bias; + } - let ii_twist_dir1 = poses1.ii.transform_vector(force_dir1); - let ii_twist_dir2 = poses2.ii.transform_vector(-force_dir1); - constraint.twist_part.rhs = SimdReal::zero(); - constraint.twist_part.ii_twist_dir1 = ii_twist_dir1; - constraint.twist_part.ii_twist_dir2 = ii_twist_dir2; - constraint.twist_part.r = utils::simd_inv( - ii_twist_dir1.gdot(force_dir1) + ii_twist_dir2.gdot(-force_dir1), - ); - constraint.twist_dists = twist_dists; - } + /* + * Tangent/twist part + */ + out_constraint.tangent_part.impulse = tangent_warmstart; + out_constraint.twist_part.impulse = twist_warmstart; - // Tangent part. - for j in 0..2 { - let torque_dir1 = dp1.gcross(tangents1[j]); - let torque_dir2 = dp2.gcross(-tangents1[j]); - let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); - let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + out_builder.local_friction_center1 = poses1.pose.inverse_transform_point(&friction_center); + out_builder.local_friction_center2 = poses2.pose.inverse_transform_point(&friction_center); - let imsum = poses1.im + poses2.im; + let dp1 = friction_center - world_com1; + let dp2 = friction_center - world_com2; - let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + ii_torque_dir1.gdot(torque_dir1) - + ii_torque_dir2.gdot(torque_dir2); - - // TODO: add something similar to tangent velocity to the twist - // constraint for the case where the different points don’t - // have the same tangent vel? - let rhs_wo_bias = tangent_vel.dot(&tangents1[j]); - - constraint.tangent_part.torque_dir1[j] = torque_dir1; - constraint.tangent_part.torque_dir2[j] = torque_dir2; - constraint.tangent_part.ii_torque_dir1[j] = ii_torque_dir1; - constraint.tangent_part.ii_torque_dir2[j] = ii_torque_dir2; - constraint.tangent_part.rhs_wo_bias[j] = rhs_wo_bias; - constraint.tangent_part.rhs[j] = rhs_wo_bias; - constraint.tangent_part.r[j] = if cfg!(feature = "dim2") { - utils::simd_inv(r) - } else { - r - }; + // Twist part. It has no effect when there is only one point. + if num_points > 1 { + let mut twist_dists = [SimdReal::zero(); MAX_MANIFOLD_POINTS]; + for k in 0..num_points { + // FIXME PERF: we don’t want to re-fetch here just to get the solver contact point! + let solver_contact = + unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) }; + twist_dists[k] = nalgebra::distance(&friction_center, &solver_contact.point); } - #[cfg(feature = "dim3")] - { - // TODO PERF: we already applied the inverse inertia to the torque - // dire before. Could we reuse the value instead of retransforming? - constraint.tangent_part.r[2] = SimdReal::splat(2.0) - * (constraint.tangent_part.ii_torque_dir1[0] - .gdot(constraint.tangent_part.torque_dir1[1]) - + constraint.tangent_part.ii_torque_dir2[0] - .gdot(constraint.tangent_part.torque_dir2[1])); - } + let ii_twist_dir1 = poses1.ii.transform_vector(force_dir1); + let ii_twist_dir2 = poses2.ii.transform_vector(-force_dir1); + out_constraint.twist_part.rhs = SimdReal::zero(); + out_constraint.twist_part.ii_twist_dir1 = ii_twist_dir1; + out_constraint.twist_part.ii_twist_dir2 = ii_twist_dir2; + out_constraint.twist_part.r = + utils::simd_inv(ii_twist_dir1.gdot(force_dir1) + ii_twist_dir2.gdot(-force_dir1)); + out_constraint.twist_dists = twist_dists; + } + + // Tangent part. + for j in 0..2 { + let torque_dir1 = dp1.gcross(tangents1[j]); + let torque_dir2 = dp2.gcross(-tangents1[j]); + let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1); + let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2); + + let imsum = poses1.im + poses2.im; + + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + ii_torque_dir1.gdot(torque_dir1) + + ii_torque_dir2.gdot(torque_dir2); + + // TODO: add something similar to tangent velocity to the twist + // constraint for the case where the different points don’t + // have the same tangent vel? + let rhs_wo_bias = tangent_vel.dot(&tangents1[j]); + + out_constraint.tangent_part.torque_dir1[j] = torque_dir1; + out_constraint.tangent_part.torque_dir2[j] = torque_dir2; + out_constraint.tangent_part.ii_torque_dir1[j] = ii_torque_dir1; + out_constraint.tangent_part.ii_torque_dir2[j] = ii_torque_dir2; + out_constraint.tangent_part.rhs_wo_bias[j] = rhs_wo_bias; + out_constraint.tangent_part.rhs[j] = rhs_wo_bias; + out_constraint.tangent_part.r[j] = if cfg!(feature = "dim2") { + utils::simd_inv(r) + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + // TODO PERF: we already applied the inverse inertia to the torque + // dire before. Could we reuse the value instead of retransforming? + out_constraint.tangent_part.r[2] = SimdReal::splat(2.0) + * (out_constraint.tangent_part.ii_torque_dir1[0] + .gdot(out_constraint.tangent_part.torque_dir1[1]) + + out_constraint.tangent_part.ii_torque_dir2[0] + .gdot(out_constraint.tangent_part.torque_dir2[1])); } } diff --git a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs index c8294bc82..eed4bacf6 100644 --- a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs @@ -36,8 +36,8 @@ impl GenericContactConstraintBuilder { manifold: &ContactManifold, bodies: &RigidBodySet, multibodies: &MultibodyJointSet, - out_builders: &mut [GenericContactConstraintBuilder], - out_constraints: &mut [GenericContactConstraint], + out_builder: &mut GenericContactConstraintBuilder, + out_constraint: &mut GenericContactConstraint, jacobians: &mut DVector, jacobian_id: &mut usize, ) { @@ -99,54 +99,127 @@ impl GenericContactConstraintBuilder { jacobians.resize_vertically_mut(required_jacobian_len, 0.0); } - for (l, manifold_points) in manifold - .data - .solver_contacts - .chunks(MAX_MANIFOLD_POINTS) - .enumerate() + let chunk_j_id = *jacobian_id; + + let manifold_points = &manifold.data.solver_contacts; + out_constraint.dir1 = force_dir1; + out_constraint.im1 = if type1.is_dynamic_or_kinematic() { + mprops1.effective_inv_mass + } else { + na::zero() + }; + out_constraint.im2 = if type2.is_dynamic_or_kinematic() { + mprops2.effective_inv_mass + } else { + na::zero() + }; + out_constraint.solver_vel1 = solver_vel1; + out_constraint.solver_vel2 = solver_vel2; + out_constraint.manifold_id = manifold_id; + out_constraint.num_contacts = manifold_points.len() as u8; + #[cfg(feature = "dim3")] { - let chunk_j_id = *jacobian_id; - - let builder = &mut out_builders[l]; - let constraint = &mut out_constraints[l]; - constraint.dir1 = force_dir1; - constraint.im1 = if type1.is_dynamic_or_kinematic() { - mprops1.effective_inv_mass - } else { - na::zero() - }; - constraint.im2 = if type2.is_dynamic_or_kinematic() { - mprops2.effective_inv_mass - } else { - na::zero() - }; - constraint.solver_vel1 = solver_vel1; - constraint.solver_vel2 = solver_vel2; - constraint.manifold_id = manifold_id; - constraint.num_contacts = manifold_points.len() as u8; - #[cfg(feature = "dim3")] + out_constraint.tangent1 = tangents1[0]; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let point = manifold_point.point; + let dp1 = point - mprops1.world_com; + let dp2 = point - mprops2.world_com; + + let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); + let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + + out_constraint.limit = manifold_point.friction; + out_constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; + + // Normal part. + let normal_rhs_wo_bias; { - constraint.tangent1 = tangents1[0]; - } + let torque_dir1 = dp1.gcross(force_dir1); + let torque_dir2 = dp2.gcross(-force_dir1); + + let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() { + mprops1 + .effective_world_inv_inertia + .transform_vector(torque_dir1) + } else { + na::zero() + }; + let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() { + mprops2 + .effective_world_inv_inertia + .transform_vector(torque_dir2) + } else { + na::zero() + }; + + let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { + mb1.fill_jacobians( + *link_id1, + force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir1), + #[cfg(feature = "dim3")] + torque_dir1, + jacobian_id, + jacobians, + ) + .0 + } else if type1.is_dynamic_or_kinematic() { + force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) + + ii_torque_dir1.gdot(torque_dir1) + } else { + 0.0 + }; + + let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { + mb2.fill_jacobians( + *link_id2, + -force_dir1, + #[cfg(feature = "dim2")] + na::vector!(torque_dir2), + #[cfg(feature = "dim3")] + torque_dir2, + jacobian_id, + jacobians, + ) + .0 + } else if type2.is_dynamic_or_kinematic() { + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + ii_torque_dir2.gdot(torque_dir2) + } else { + 0.0 + }; - for k in 0..manifold_points.len() { - let manifold_point = &manifold_points[k]; - let point = manifold_point.point; - let dp1 = point - mprops1.world_com; - let dp2 = point - mprops2.world_com; + let r = crate::utils::inv(inv_r1 + inv_r2); - let vel1 = vels1.linvel + vels1.angvel.gcross(dp1); - let vel2 = vels2.linvel + vels2.angvel.gcross(dp2); + let is_bouncy = manifold_point.is_bouncy() as u32 as Real; - constraint.limit = manifold_point.friction; - constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; + normal_rhs_wo_bias = + (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - // Normal part. - let normal_rhs_wo_bias; - { - let torque_dir1 = dp1.gcross(force_dir1); - let torque_dir2 = dp2.gcross(-force_dir1); + out_constraint.normal_part[k] = ContactConstraintNormalPart { + torque_dir1, + torque_dir2, + ii_torque_dir1, + ii_torque_dir2, + rhs: na::zero(), + rhs_wo_bias: na::zero(), + impulse_accumulator: na::zero(), + impulse: manifold_point.warmstart_impulse, + r, + r_mat_elts: [0.0; 2], + }; + } + + // Tangent parts. + { + out_constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_impulse; + for j in 0..DIM - 1 { + let torque_dir1 = dp1.gcross(tangents1[j]); let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() { mprops1 .effective_world_inv_inertia @@ -154,6 +227,10 @@ impl GenericContactConstraintBuilder { } else { na::zero() }; + out_constraint.tangent_part[k].torque_dir1[j] = torque_dir1; + out_constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; + + let torque_dir2 = dp2.gcross(-tangents1[j]); let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() { mprops2 .effective_world_inv_inertia @@ -161,13 +238,15 @@ impl GenericContactConstraintBuilder { } else { na::zero() }; + out_constraint.tangent_part[k].torque_dir2[j] = torque_dir2; + out_constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { mb1.fill_jacobians( *link_id1, - force_dir1, + tangents1[j], #[cfg(feature = "dim2")] - na::vector!(torque_dir1), + na::vector![torque_dir1], #[cfg(feature = "dim3")] torque_dir1, jacobian_id, @@ -184,9 +263,9 @@ impl GenericContactConstraintBuilder { let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { mb2.fill_jacobians( *link_id2, - -force_dir1, + -tangents1[j], #[cfg(feature = "dim2")] - na::vector!(torque_dir2), + na::vector![torque_dir2], #[cfg(feature = "dim3")] torque_dir2, jacobian_id, @@ -201,141 +280,54 @@ impl GenericContactConstraintBuilder { }; let r = crate::utils::inv(inv_r1 + inv_r2); + let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); - let is_bouncy = manifold_point.is_bouncy() as u32 as Real; + out_constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; + out_constraint.tangent_part[k].rhs[j] = rhs_wo_bias; - normal_rhs_wo_bias = - (is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); - - constraint.normal_part[k] = ContactConstraintNormalPart { - torque_dir1, - torque_dir2, - ii_torque_dir1, - ii_torque_dir2, - rhs: na::zero(), - rhs_wo_bias: na::zero(), - impulse_accumulator: na::zero(), - impulse: manifold_point.warmstart_impulse, - r, - r_mat_elts: [0.0; 2], - }; - } - - // Tangent parts. - { - constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_impulse; - - for j in 0..DIM - 1 { - let torque_dir1 = dp1.gcross(tangents1[j]); - let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() { - mprops1 - .effective_world_inv_inertia - .transform_vector(torque_dir1) - } else { - na::zero() - }; - constraint.tangent_part[k].torque_dir1[j] = torque_dir1; - constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1; - - let torque_dir2 = dp2.gcross(-tangents1[j]); - let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() { - mprops2 - .effective_world_inv_inertia - .transform_vector(torque_dir2) - } else { - na::zero() - }; - constraint.tangent_part[k].torque_dir2[j] = torque_dir2; - constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2; - - let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() { - mb1.fill_jacobians( - *link_id1, - tangents1[j], - #[cfg(feature = "dim2")] - na::vector![torque_dir1], - #[cfg(feature = "dim3")] - torque_dir1, - jacobian_id, - jacobians, - ) - .0 - } else if type1.is_dynamic_or_kinematic() { - force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1)) - + ii_torque_dir1.gdot(torque_dir1) - } else { - 0.0 - }; - - let inv_r2 = if let Some((mb2, link_id2)) = multibody2.as_ref() { - mb2.fill_jacobians( - *link_id2, - -tangents1[j], - #[cfg(feature = "dim2")] - na::vector![torque_dir2], - #[cfg(feature = "dim3")] - torque_dir2, - jacobian_id, - jacobians, - ) - .0 - } else if type2.is_dynamic_or_kinematic() { - force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) - + ii_torque_dir2.gdot(torque_dir2) - } else { - 0.0 - }; - - let r = crate::utils::inv(inv_r1 + inv_r2); - let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]); - - constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias; - constraint.tangent_part[k].rhs[j] = rhs_wo_bias; - - // TODO: in 3D, we should take into account gcross[0].dot(gcross[1]) - // in lhs. See the corresponding code on the `velocity_constraint.rs` - // file. - constraint.tangent_part[k].r[j] = r; - } + // TODO: in 3D, we should take into account gcross[0].dot(gcross[1]) + // in lhs. See the corresponding code on the `velocity_constraint.rs` + // file. + out_constraint.tangent_part[k].r[j] = r; } + } - // Builder. - let infos = CoulombContactPointInfos { - local_p1: rb1 - .pos - .position - .inverse_transform_point(&manifold_point.point), - local_p2: rb2 - .pos - .position - .inverse_transform_point(&manifold_point.point), - tangent_vel: manifold_point.tangent_velocity, - dist: manifold_point.dist, - normal_vel: normal_rhs_wo_bias, - }; + // Builder. + let infos = CoulombContactPointInfos { + local_p1: rb1 + .pos + .position + .inverse_transform_point(&manifold_point.point), + local_p2: rb2 + .pos + .position + .inverse_transform_point(&manifold_point.point), + tangent_vel: manifold_point.tangent_velocity, + dist: manifold_point.dist, + normal_vel: normal_rhs_wo_bias, + }; - builder.handle1 = handle1; - builder.handle2 = handle2; - builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; - builder.infos[k] = infos; - constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; - } + out_builder.handle1 = handle1; + out_builder.handle2 = handle2; + out_builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness; + out_builder.infos[k] = infos; + out_constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8; + } - let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); - let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0); + 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 - // reduce all ops to nothing because its ndofs will be zero. - let generic_constraint_mask = (multibody1.is_some() as u8) - | ((multibody2.is_some() as u8) << 1) - | (!type1.is_dynamic_or_kinematic() as u8) - | ((!type2.is_dynamic_or_kinematic() as u8) << 1); + // NOTE: we use the generic constraint for non-dynamic bodies because this will + // reduce all ops to nothing because its ndofs will be zero. + let generic_constraint_mask = (multibody1.is_some() as u8) + | ((multibody2.is_some() as u8) << 1) + | (!type1.is_dynamic_or_kinematic() as u8) + | ((!type2.is_dynamic_or_kinematic() as u8) << 1); - constraint.j_id = chunk_j_id; - constraint.ndofs1 = ndofs1; - constraint.ndofs2 = ndofs2; - constraint.generic_constraint_mask = generic_constraint_mask; - } + out_constraint.j_id = chunk_j_id; + out_constraint.ndofs1 = ndofs1; + out_constraint.ndofs2 = ndofs2; + out_constraint.generic_constraint_mask = generic_constraint_mask; } pub fn update( From ce651d7bc2285af040858e1ae18e2542df7df2a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 30 Oct 2025 11:55:40 +0100 Subject: [PATCH 04/12] feat: add concept of incremental islands with a single awake island More islands manager fixes --- crates/rapier2d-f64/Cargo.toml | 1 + crates/rapier2d/Cargo.toml | 1 + crates/rapier3d-f64/Cargo.toml | 1 + crates/rapier3d/Cargo.toml | 1 + src/dynamics/island_manager2.rs | 468 ++++++++++++++++++ .../joint/impulse_joint/impulse_joint_set.rs | 2 +- src/dynamics/mod.rs | 1 + src/dynamics/rigid_body_components.rs | 41 ++ src/dynamics/solver/velocity_solver.rs | 7 +- src/geometry/narrow_phase.rs | 35 +- src/pipeline/collision_pipeline.rs | 6 +- src/pipeline/physics_pipeline.rs | 20 +- src/pipeline/user_changes.rs | 25 +- src_testbed/physx_backend.rs | 42 +- 14 files changed, 598 insertions(+), 53 deletions(-) create mode 100644 src/dynamics/island_manager2.rs diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index ce94a1381..4539e8d5b 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -43,6 +43,7 @@ serde-serialize = [ "dep:serde", "bit-vec/serde", "arrayvec/serde", + "vec_map/serde" ] enhanced-determinism = ["simba/libm_force", "parry2d-f64/enhanced-determinism"] debug-render = [] diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index a1bd0320f..6ecb5d3e0 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -44,6 +44,7 @@ serde-serialize = [ "dep:serde", "bit-vec/serde", "arrayvec/serde", + "vec_map/serde" ] enhanced-determinism = ["simba/libm_force", "parry2d/enhanced-determinism"] debug-render = [] diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index 0600a8c51..7128fe510 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -46,6 +46,7 @@ serde-serialize = [ "parry3d-f64/serde-serialize", "dep:serde", "bit-vec/serde", + "vec_map/serde" ] enhanced-determinism = ["simba/libm_force", "parry3d-f64/enhanced-determinism"] debug-render = [] diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index 22bfb46e6..db4ad303e 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -48,6 +48,7 @@ serde-serialize = [ "parry3d/serde-serialize", "dep:serde", "bit-vec/serde", + "vec_map/serde" ] enhanced-determinism = ["simba/libm_force", "parry3d/enhanced-determinism"] debug-render = [] diff --git a/src/dynamics/island_manager2.rs b/src/dynamics/island_manager2.rs new file mode 100644 index 000000000..e666da306 --- /dev/null +++ b/src/dynamics/island_manager2.rs @@ -0,0 +1,468 @@ +use crate::dynamics::{ + ImpulseJointSet, MultibodyJointSet, RigidBody, RigidBodyActivation, RigidBodyChanges, + RigidBodyColliders, RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, + RigidBodyVelocity, +}; +use crate::geometry::{ColliderSet, ContactPair, NarrowPhase}; +use crate::math::Real; +use crate::prelude::{ColliderHandle, ContactManifoldData}; +use crate::utils::SimdDot; +use std::collections::VecDeque; +use std::ops::IndexMut; +use vec_map::VecMap; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Default)] +struct Island { + bodies: Vec, + additional_solver_iterations: usize, +} + +/// System that manages which bodies are active (awake) vs sleeping to optimize performance. +/// +/// ## Sleeping Optimization +/// +/// Bodies at rest automatically "sleep" - they're excluded from simulation until something +/// disturbs them (collision, joint connection to moving body, manual wake-up). This can +/// dramatically improve performance in scenes with many static/resting objects. +/// +/// ## Islands +/// +/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together. +/// This allows parallel solving and better organization. +/// +/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline). +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone)] +pub struct IslandManager { + // Island 0 is active, all the others are sleeping. + pub(crate) islands: VecMap, + pub(crate) free_islands: Vec, + /// Potential candidate roots for graph traversal to identify a sleeping + /// connected component. + traversal_candidates: VecDeque, + /// Same as `traversal_candidates` but these roots **must** all be traversed as the next + /// timestep to properly handle bodies that are sleeping at creation. + priority_traversal_candidates: Vec, + timestamp: u32, // TODO: the physics pipeline (or something else) should expose a step_id? + #[cfg_attr(feature = "serde-serialize", serde(skip))] + stack: Vec, // Workspace. +} + +impl Default for IslandManager { + fn default() -> Self { + let mut islands = VecMap::new(); + islands.insert(0, Island::default()); + Self { + islands, + free_islands: Default::default(), + traversal_candidates: Default::default(), + priority_traversal_candidates: Default::default(), + timestamp: Default::default(), + stack: Default::default(), + } + } +} + +impl IslandManager { + /// Creates a new empty island manager. + pub fn new() -> Self { + Self::default() + } + + // TODO: rename this to `num_active_islands`? + pub(crate) fn num_active_islands(&self) -> usize { + 1 + } + + pub(crate) fn rigid_body_removed( + &mut self, + removed_handle: RigidBodyHandle, + removed_ids: &RigidBodyIds, + bodies: &mut RigidBodySet, + ) { + let Some(island) = self.islands.get_mut(removed_ids.active_island_id) else { + // The island already doesn’t exist. + return; + }; + + let swapped_handle = island.bodies.last().copied().unwrap_or(removed_handle); + island + .bodies + .swap_remove(removed_ids.active_set_offset as usize); + + if swapped_handle != removed_handle { + let swapped_body = bodies + .get_mut(swapped_handle) + .expect("Internal error: bodies must be removed from islands on at a times"); + swapped_body.ids.active_set_offset = removed_ids.active_set_offset; + swapped_body.ids.active_set_id = removed_ids.active_set_id; + } + + if removed_ids.active_island_id != 0 && island.bodies.is_empty() { + // We removed the last body from this island. Remove the island (but never remove + // island 0 since it’s the special active island). + self.islands.remove(removed_ids.active_island_id); + self.free_islands.push(removed_ids.active_island_id); + } + } + + pub(crate) fn contact_started_or_stopped( + &mut self, + bodies: &mut RigidBodySet, + handle1: Option, + handle2: Option, + started: bool, + ) { + if started { + if let Some(handle1) = handle1 { + self.wake_up(bodies, handle1, false); + } + if let Some(handle2) = handle2 { + self.wake_up(bodies, handle2, false); + } + } + } + + /// Wakes up a sleeping body, forcing it back into the active simulation. + /// + /// Use this when you want to ensure a body is active (useful after manually moving + /// a sleeping body, or to prevent it from sleeping in the next few frames). + /// + /// # Parameters + /// * `strong` - If `true`, the body is guaranteed to stay awake for multiple frames. + /// If `false`, it might sleep again immediately if conditions are met. + /// + /// # Example + /// ``` + /// # use rapier3d::prelude::*; + /// # let mut bodies = RigidBodySet::new(); + /// # let mut islands = IslandManager::new(); + /// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic()); + /// islands.wake_up(&mut bodies, body_handle, true); + /// let body = bodies.get_mut(body_handle).unwrap(); + /// // Wake up a body before applying force to it + /// body.add_force(vector![100.0, 0.0, 0.0], false); + /// ``` + /// + /// Only affects dynamic bodies (kinematic and fixed bodies don't sleep). + pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) { + // NOTE: the use an Option here because there are many legitimate cases (like when + // deleting a joint attached to an already-removed body) where we could be + // attempting to wake-up a rigid-body that has already been deleted. + if bodies.get(handle).map(|rb| rb.is_dynamic_or_kinematic()) == Some(true) { + let rb = bodies.index_mut_internal(handle); + + // TODO: not sure if this is still relevant: + // // Check that the user didn’t change the sleeping state explicitly, in which + // // case we don’t overwrite it. + // if rb.changes.contains(RigidBodyChanges::SLEEP) { + // return; + // } + + rb.activation.wake_up(strong); + if rb.is_enabled() && rb.ids.active_island_id != 0 { + // Wake up the sleeping island this rigid-body is part of. + let Some(removed_island) = self.islands.remove(rb.ids.active_island_id) else { + // TODO: the island doesn’t exist is that an internal error? + return; + }; + + self.free_islands.push(rb.ids.active_island_id); + + // TODO: if we switched to linked list, we could avoid moving around all this memory. + let active_island = &mut self.islands[0]; + for handle in &removed_island.bodies { + let Some(rb) = bodies.get_mut_internal(*handle) else { + // This body no longer exists. + continue; + }; + rb.wake_up(false); + rb.ids.active_island_id = 0; + rb.ids.active_set_id = active_island.bodies.len(); + rb.ids.active_set_offset = (active_island.bodies.len()) as u32; + active_island.bodies.push(*handle); + } + + active_island.additional_solver_iterations = active_island + .additional_solver_iterations + .max(removed_island.additional_solver_iterations); + } + } + } + + // TODO: remove the island_id argument? + pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { + assert_eq!(island_id, 0); + &self.islands[0].bodies + } + + pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize { + self.islands[0].additional_solver_iterations + } + + /// Handles of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping). + #[inline] + pub fn active_bodies(&self) -> &[RigidBodyHandle] { + &self.islands[0].bodies + } + + // TODO: what if the body is sleeping when we add it? + pub(crate) fn update_body(&mut self, handle: RigidBodyHandle, bodies: &mut RigidBodySet) { + let Some(rb) = bodies.get_mut(handle) else { + return; + }; + + if rb.is_fixed() { + return; + } + + let id = self.islands[0].bodies.len(); + + if rb.ids.active_set_id == usize::MAX { + // This body is new. + rb.ids.active_island_id = 0; + rb.ids.active_set_id = id; + rb.ids.active_set_offset = id as u32; + self.islands[0].bodies.push(handle); + + if rb.is_sleeping() { + // The rigid-body is sleeping. We need to move it into a sleeping island during + // the next step. + self.priority_traversal_candidates.push(handle); + } + } + + // Push the body to the active set if it is not inside the active set yet, and + // is not longer sleeping or became dynamic. + if (rb.changes.contains(RigidBodyChanges::SLEEP) + || rb.changes.contains(RigidBodyChanges::TYPE)) + && rb.is_enabled() + // Don’t wake up if the user put it to sleep manually. + && !rb.activation.sleeping + { + self.wake_up(bodies, handle, false); + } + } + + pub(crate) fn update_active_set_with_contacts( + &mut self, + dt: Real, + length_unit: Real, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + min_island_size: usize, + ) { + self.timestamp += 1; + // 1. Update active rigid-bodies energy. + // let t0 = std::time::Instant::now(); + for handle in &self.islands[0].bodies { + let Some(rb) = bodies.get_mut_internal(*handle) else { + // This branch happens if the rigid-body no longer exists. + continue; + }; + let sq_linvel = rb.vels.linvel.norm_squared(); + let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); + let can_sleep_before = rb.activation.is_eligible_for_sleep(); + + rb.activation + .update_energy(rb.body_type, length_unit, sq_linvel, sq_angvel, dt); + + let can_sleep_now = rb.activation.is_eligible_for_sleep(); + + // 2. Identify active rigid-bodies that transition from "awake" to "can_sleep" + // and push the sleep root candidate if applicable. + if !can_sleep_before && can_sleep_now { + // This is a new candidate for island extraction. + if !rb.activation.is_sleep_root_candidate { + self.traversal_candidates.push_back(*handle); + } + } + } + // println!("Update energy: {}", t0.elapsed().as_secs_f32() * 1000.0); + + // 3. Perform sleeping islands extraction on **all** the prioritized sleep candidates. + // let t0 = std::time::Instant::now(); + let frame_base_timestamp = self.timestamp; + let mut niters = 0; + for sleep_root in std::mem::take(&mut self.priority_traversal_candidates) { + niters += self.extract_sleeping_island( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + sleep_root, + frame_base_timestamp, + ); + self.timestamp += 1; + } + + // 3. Perform one, or multiple, sleeping islands extraction (graph traversal). + // Limit the traversal cost by not traversing all the known sleeping roots if + // there are too many. + while let Some(sleep_root) = self.traversal_candidates.pop_front() { + if niters > 1000 { + break; // Limit cost per frame. + } + + niters += self.extract_sleeping_island( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + sleep_root, + frame_base_timestamp, + ); + + // TODO PERF: early-break if we consider we have done enough island extraction work. + self.timestamp += 1; + } + // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0); + } + + /// Returns the number of iterations run by the graph traversal so we can balance load across + /// frames. + fn extract_sleeping_island( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + sleep_root: RigidBodyHandle, + frame_base_timestamp: u32, + ) -> usize { + let Some(rb) = bodies.get_mut_internal(sleep_root) else { + // This branch happens if the rigid-body no longer exists. + return 0; + }; + + rb.activation.is_sleep_root_candidate = false; + if rb.ids.active_island_id != 0 || rb.ids.active_set_timestamp == self.timestamp { + return 0; // This rigid-body is already in a sleeping island or was already traversed. + } + + // TODO: implement recycling islands to avoid repeated allocations? + let mut new_island = Island::default(); + self.stack.push(sleep_root); + + let mut can_sleep = true; + let mut niter = 0; + while let Some(handle) = self.stack.pop() { + let rb = bodies.index_mut_internal(handle); + + if rb.is_fixed() { + // Don’t propagate islands through fixed bodies. + continue; + } + + if rb.ids.active_set_timestamp == self.timestamp { + // We already visited this body and its neighbors. + continue; + } + + if rb.ids.active_set_timestamp >= frame_base_timestamp { + // We already visited this body and its neighbors during this frame. + // So we already know this islands cannot sleep (otherwise the bodies + // currently being traversed would already have been marked as sleeping). + return niter; + } + + niter += 1; + rb.ids.active_set_timestamp = self.timestamp; + + assert!(!rb.activation.sleeping); + + // TODO PERF: early-exit as soon as we reach a body not eligible to sleep. + can_sleep = can_sleep && rb.activation().is_eligible_for_sleep(); + if !rb.activation.is_eligible_for_sleep() { + // If this body cannot sleep, abort the traversal, we are not traversing + // yet an island that can sleep. + self.stack.clear(); + return niter; + } + + // Traverse bodies that are interacting with the current one either through + // contacts or a joint. + push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); + push_linked_bodies(impulse_joints, multibody_joints, handle, &mut self.stack); + new_island.bodies.push(handle); + } + + assert!(can_sleep); + + // If we reached this line, we completed a sleeping island traversal. + // - Put its bodies to sleep. + // - Remove them from the active set. + // - Push the sleeping island. + let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len()); + let active_island = &mut self.islands[0]; + for (id, handle) in new_island.bodies.iter().enumerate() { + let rb = bodies.index_mut_internal(*handle); + rb.sleep(); + + let id_to_remove = rb.ids.active_set_id; + rb.ids.active_island_id = new_island_id; + rb.ids.active_set_id = id; + rb.ids.active_set_offset = id as u32; + + active_island.bodies.swap_remove(id_to_remove); + if let Some(moved_handle) = active_island.bodies.get(id_to_remove).copied() { + let moved_rb = bodies.index_mut_internal(moved_handle); + moved_rb.ids.active_set_id = id_to_remove; + moved_rb.ids.active_set_offset = id_to_remove as u32; + } + niter += 1; // This contributes to the cost of the operation. + } + + self.islands.insert(new_island_id, new_island); + niter + } +} + +// Read all the contacts and push objects touching this rigid-body. +#[inline] +fn push_contacting_bodies( + rb_colliders: &RigidBodyColliders, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + stack: &mut Vec, +) { + for collider_handle in &rb_colliders.0 { + for inter in narrow_phase.contact_pairs_with(*collider_handle) { + for manifold in &inter.manifolds { + if !manifold.data.solver_contacts.is_empty() { + let other = crate::utils::select_other( + (inter.collider1, inter.collider2), + *collider_handle, + ); + if let Some(other_body) = colliders[other].parent { + stack.push(other_body.handle); + } + break; + } + } + } + } +} + +fn push_linked_bodies( + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + handle: RigidBodyHandle, + stack: &mut Vec, +) { + for inter in impulse_joints.attached_enabled_joints(handle) { + let other = crate::utils::select_other((inter.0, inter.1), handle); + stack.push(other); + } + + for other in multibody_joints.bodies_attached_with_enabled_joint(handle) { + stack.push(other); + } +} diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 08c668bb0..216b649c6 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -377,7 +377,7 @@ impl ImpulseJointSet { bodies: &RigidBodySet, out: &mut [Vec], ) { - for out_island in &mut out[..islands.num_islands()] { + for out_island in &mut out[..islands.num_active_islands()] { out_island.clear(); } diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 41e91dadb..bf818d95d 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -26,6 +26,7 @@ mod ccd; mod coefficient_combine_rule; mod integration_parameters; mod island_manager; +mod island_manager2; mod joint; mod rigid_body_components; mod solver; diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index d8cce7a5e..7e2e0efee 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1183,6 +1183,10 @@ pub struct RigidBodyActivation { /// Is this body currently sleeping? pub sleeping: bool, + + /// If this is `true` then the island manager has already registered that this rigid-body + /// can be used as a root for graph traversal to potentially extract a sleeping island. + pub is_sleep_root_candidate: bool, } impl Default for RigidBodyActivation { @@ -1216,6 +1220,7 @@ impl RigidBodyActivation { time_until_sleep: Self::default_time_until_sleep(), time_since_can_sleep: 0.0, sleeping: false, + is_sleep_root_candidate: false, } } @@ -1227,6 +1232,7 @@ impl RigidBodyActivation { time_until_sleep: Self::default_time_until_sleep(), time_since_can_sleep: Self::default_time_until_sleep(), sleeping: true, + is_sleep_root_candidate: false, } } @@ -1260,6 +1266,41 @@ impl RigidBodyActivation { self.sleeping = true; self.time_since_can_sleep = self.time_until_sleep; } + + /// Does this body have a sufficiently low kinetic energy for a long enough + /// duration to be eligible for sleeping? + pub fn is_eligible_for_sleep(&self) -> bool { + self.time_since_can_sleep >= self.time_until_sleep + } + + pub(crate) fn update_energy( + &mut self, + body_type: RigidBodyType, + length_unit: Real, + sq_linvel: Real, + sq_angvel: Real, + dt: Real, + ) { + let can_sleep = match body_type { + RigidBodyType::Dynamic => { + let linear_threshold = self.normalized_linear_threshold * length_unit; + sq_linvel < linear_threshold * linear_threshold.abs() + && sq_angvel < self.angular_threshold * self.angular_threshold.abs() + } + RigidBodyType::KinematicPositionBased | RigidBodyType::KinematicVelocityBased => { + // Platforms only sleep if both velocities are exactly zero. If it’s not exactly + // zero, then the user really wants them to move. + sq_linvel == 0.0 && sq_angvel == 0.0 + } + RigidBodyType::Fixed => true, + }; + + if can_sleep { + self.time_since_can_sleep += dt; + } else { + self.time_since_can_sleep = 0.0; + } + } } #[cfg(test)] diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index ef2d186e7..e186883db 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -110,6 +110,11 @@ impl VelocitySolver { } } else { let rb = &bodies[*handle]; + if rb.ids.active_set_offset == u32::MAX { + println!("Invalid: {:?} -> {}", *handle, rb.ids.active_set_offset); + println!("ty: {:?}", rb.body_type); + } + let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_offset as usize]; self.solver_bodies @@ -181,7 +186,7 @@ impl VelocitySolver { if params.warmstart_coefficient != 0.0 { // TODO PERF: we could probably figure out a way to avoid this warmstart when // step_id > 0? Maybe for that to happen `solver_vels` needs to - // represent velocity changes instead of total rigid-boody velocities. + // represent velocity changes instead of total rigid-body velocities. // Need to be careful wrt. multibody and joints too. contact_constraints .warmstart(&mut self.solver_bodies, &mut self.generic_solver_vels); diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index f05059875..197a49d40 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -808,7 +808,8 @@ impl NarrowPhase { &mut self, prediction_distance: Real, dt: Real, - bodies: &RigidBodySet, + islands: &mut IslandManager, + bodies: &mut RigidBodySet, colliders: &ColliderSet, impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, @@ -1234,16 +1235,28 @@ impl NarrowPhase { } } - let active_events = co1.flags.active_events | co2.flags.active_events; - - if pair.has_any_active_contact != had_any_active_contact - && active_events.contains(ActiveEvents::COLLISION_EVENTS) - { - if pair.has_any_active_contact { - pair.emit_start_event(bodies, colliders, events); - } else { - pair.emit_stop_event(bodies, colliders, events); + /* + * Handle actions on contact start/stop: + * - Emit event (if applicable). + * - Notify the island manager to potentially wake up the bodies. + */ + if pair.has_any_active_contact != had_any_active_contact { + let active_events = co1.flags.active_events | co2.flags.active_events; + if active_events.contains(ActiveEvents::COLLISION_EVENTS) { + if pair.has_any_active_contact { + pair.emit_start_event(bodies, colliders, events); + } else { + pair.emit_stop_event(bodies, colliders, events); + } } + + // FIXME: this won’t work with the parallel feature enabled. + islands.contact_started_or_stopped( + bodies, + co1.parent.map(|p| p.handle), + co2.parent.map(|p| p.handle), + pair.has_any_active_contact, + ); } }); } @@ -1258,7 +1271,7 @@ impl NarrowPhase { out_manifolds: &mut Vec<&'a mut ContactManifold>, out: &mut [Vec], ) { - for out_island in &mut out[..islands.num_islands()] { + for out_island in &mut out[..islands.num_active_islands()] { out_island.clear(); } diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs index a93f5554f..5b4457d6b 100644 --- a/src/pipeline/collision_pipeline.rs +++ b/src/pipeline/collision_pipeline.rs @@ -1,6 +1,6 @@ //! Physics pipeline structures. -use crate::dynamics::{ImpulseJointSet, IntegrationParameters, MultibodyJointSet}; +use crate::dynamics::{ImpulseJointSet, IntegrationParameters, IslandManager, MultibodyJointSet}; use crate::geometry::{ BroadPhaseBvh, BroadPhasePairEvent, ColliderChanges, ColliderHandle, ColliderPair, ModifiedColliders, NarrowPhase, @@ -53,6 +53,7 @@ impl CollisionPipeline { fn detect_collisions( &mut self, prediction_distance: Real, + islands: &mut IslandManager, broad_phase: &mut BroadPhaseBvh, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, @@ -98,6 +99,7 @@ impl CollisionPipeline { narrow_phase.compute_contacts( prediction_distance, 0.0, + islands, bodies, colliders, &ImpulseJointSet::new(), @@ -126,6 +128,7 @@ impl CollisionPipeline { pub fn step( &mut self, prediction_distance: Real, + islands: &mut IslandManager, broad_phase: &mut BroadPhaseBvh, narrow_phase: &mut NarrowPhase, bodies: &mut RigidBodySet, @@ -162,6 +165,7 @@ impl CollisionPipeline { self.detect_collisions( prediction_distance, + islands, broad_phase, narrow_phase, bodies, diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 1838893aa..ed0f72763 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -169,6 +169,7 @@ impl PhysicsPipeline { narrow_phase.compute_contacts( integration_parameters.prediction_distance(), integration_parameters.dt, + islands, bodies, colliders, impulse_joints, @@ -206,16 +207,17 @@ impl PhysicsPipeline { integration_parameters.min_island_size, ); - if self.manifold_indices.len() < islands.num_islands() { + if self.manifold_indices.len() < islands.num_active_islands() { self.manifold_indices - .resize(islands.num_islands(), Vec::new()); + .resize(islands.num_active_islands(), Vec::new()); } - if self.joint_constraint_indices.len() < islands.num_islands() { + if self.joint_constraint_indices.len() < islands.num_active_islands() { self.joint_constraint_indices - .resize(islands.num_islands(), Vec::new()); + .resize(islands.num_active_islands(), Vec::new()); } + // let t0 = std::time::Instant::now(); let mut manifolds = Vec::new(); narrow_phase.select_active_contacts( islands, @@ -229,6 +231,10 @@ impl PhysicsPipeline { bodies, &mut self.joint_constraint_indices, ); + // println!( + // "Manifolds extraction: {:?}", + // t0.elapsed().as_secs_f32() * 1000.0 + // ); self.counters.stages.island_construction_time.pause(); self.counters.stages.update_time.resume(); @@ -245,16 +251,16 @@ impl PhysicsPipeline { self.counters.stages.update_time.pause(); self.counters.stages.solver_time.resume(); - if self.solvers.len() < islands.num_islands() { + if self.solvers.len() < islands.num_active_islands() { self.solvers - .resize_with(islands.num_islands(), IslandSolver::new); + .resize_with(islands.num_active_islands(), IslandSolver::new); } #[cfg(not(feature = "parallel"))] { enable_flush_to_zero!(); - for island_id in 0..islands.num_islands() { + for island_id in 0..islands.num_active_islands() { self.solvers[island_id].init_and_solve( island_id, &mut self.counters, diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index e39c06676..e396dfe88 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -63,30 +63,21 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( continue; } - let rb = bodies.index_mut_internal(*handle); - let mut ids = rb.ids; - let changes = rb.changes; - let activation = rb.activation; - { + let rb = bodies.index_mut_internal(*handle); + let changes = rb.changes; + let activation = rb.activation; + if rb.is_enabled() { // The body's status changed. We need to make sure // it is on the correct active set. if let Some(islands) = islands.as_deref_mut() { - // Push the body to the active set if it is not inside the active set yet, and - // is not longer sleeping or became dynamic. - if (changes.contains(RigidBodyChanges::SLEEP) || changes.contains(RigidBodyChanges::TYPE)) - && rb.is_enabled() - && !rb.activation.sleeping // May happen if the body was put to sleep manually. - && rb.is_dynamic_or_kinematic() // Only dynamic bodies are in the active dynamic set. - && islands.active_set.get(ids.active_set_id) != Some(handle) - { - ids.active_set_id = islands.active_set.len(); // This will handle the case where the activation_channel contains duplicates. - islands.active_set.push(*handle); - } + islands.update_body(*handle, bodies); } } + let rb = bodies.index_mut_internal(*handle); + // Update the colliders' positions. if changes.contains(RigidBodyChanges::POSITION) || changes.contains(RigidBodyChanges::COLLIDERS) @@ -157,7 +148,6 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( ); } - rb.ids = ids; rb.activation = activation; } @@ -166,6 +156,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( if let Some(action) = final_action { match action { FinalAction::RemoveFromIsland => { + let rb = bodies.index_mut_internal(*handle); let ids = rb.ids; islands.rigid_body_removed(*handle, &ids, bodies); } diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 91e0ae2c8..3c7e1a0d4 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -1,5 +1,6 @@ #![allow(dead_code)] +use crate::ui::egui::emath::OrderedFloat; use na::{Isometry3, Matrix4, Point3, Quaternion, Translation3, Unit, UnitQuaternion, Vector3}; use physx::cooking::{ ConvexMeshCookingResult, PxConvexMeshDesc, PxCookingParams, PxHeightFieldDesc, @@ -141,7 +142,7 @@ pub struct PhysxWorld { impl Drop for PhysxWorld { fn drop(&mut self) { let scene = self.scene.take(); - // FIXME: we get a segfault if we don't forget the scene. + // FIXME: we get a segfault if we don't leak the scene. std::mem::forget(scene); } } @@ -300,10 +301,13 @@ impl PhysxWorld { * Colliders * */ + let mut materials_cache = HashMap::new(); for (_, collider) in colliders.iter() { - if let Some((mut px_shape, px_material, collider_pos)) = - physx_collider_from_rapier_collider(&mut *physics, &collider) - { + if let Some((mut px_shape, collider_pos)) = physx_collider_from_rapier_collider( + &mut *physics, + &collider, + &mut materials_cache, + ) { if let Some(parent_handle) = collider.parent() { let parent_body = &bodies[parent_handle]; @@ -331,7 +335,6 @@ impl PhysxWorld { } shapes.push(px_shape); - materials.push(px_material); } } } @@ -557,7 +560,8 @@ impl PhysxWorld { fn physx_collider_from_rapier_collider( physics: &mut PxPhysicsFoundation, collider: &Collider, -) -> Option<(Owner, Owner, Isometry3)> { + materials_cache: &mut HashMap<[OrderedFloat; 2], Owner>, +) -> Option<(Owner, Isometry3)> { let mut local_pose = collider.position_wrt_parent().copied().unwrap_or(na::one()); let cooking_params = PxCookingParams::new(physics).unwrap(); let shape = collider.shape(); @@ -566,14 +570,22 @@ fn physx_collider_from_rapier_collider( } else { ShapeFlags::SimulationShape }; - let mut material = physics - .create_material( - collider.material().friction, - collider.material().friction, - collider.material().restitution, - (), - ) - .unwrap(); + + let material = materials_cache + .entry([ + OrderedFloat(collider.material().friction), + OrderedFloat(collider.material().restitution), + ]) + .or_insert_with(|| { + physics + .create_material( + collider.material().friction, + collider.material().friction, + collider.material().restitution, + (), + ) + .unwrap() + }); let materials = &mut [material.as_mut()][..]; let shape = if let Some(cuboid) = shape.as_cuboid() { @@ -705,7 +717,7 @@ fn physx_collider_from_rapier_collider( return None; }; - shape.map(|s| (s, material, local_pose)) + shape.map(|s| (s, local_pose)) } type PxPhysicsFoundation = PhysicsFoundation; From cee2d16859d0048c278d05aa14387f54e1bc4acb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 31 Oct 2025 22:14:17 +0100 Subject: [PATCH 05/12] =?UTF-8?q?feat:=E2=80=AFstart=20adding=20support=20?= =?UTF-8?q?for=20multiple=20awake=20islands?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/dynamics/ccd/ccd_solver.rs | 6 +- src/dynamics/island_manager2.rs | 267 ++++++++++++------ .../joint/impulse_joint/impulse_joint_set.rs | 2 +- src/dynamics/solver/interaction_groups.rs | 4 +- src/dynamics/solver/island_solver.rs | 2 +- .../joint_constraint/joint_constraints_set.rs | 5 +- src/dynamics/solver/velocity_solver.rs | 6 +- src/geometry/narrow_phase.rs | 14 +- src/pipeline/physics_pipeline.rs | 42 +-- 9 files changed, 220 insertions(+), 128 deletions(-) diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs index 376373023..52b8e9376 100644 --- a/src/dynamics/ccd/ccd_solver.rs +++ b/src/dynamics/ccd/ccd_solver.rs @@ -88,7 +88,7 @@ impl CCDSolver { // println!("Checking CCD activation"); for handle in islands.active_bodies() { - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); if rb.ccd.ccd_enabled { let forces = if include_forces { @@ -143,7 +143,7 @@ impl CCDSolver { let mut min_toi = dt; for handle in islands.active_bodies() { - let rb1 = &bodies[*handle]; + let rb1 = &bodies[handle]; if rb1.ccd.ccd_active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( @@ -276,7 +276,7 @@ impl CCDSolver { */ // TODO: don't iterate through all the colliders. for handle in islands.active_bodies() { - let rb1 = &bodies[*handle]; + let rb1 = &bodies[handle]; if rb1.ccd.ccd_active { let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities( diff --git a/src/dynamics/island_manager2.rs b/src/dynamics/island_manager2.rs index e666da306..9db68af07 100644 --- a/src/dynamics/island_manager2.rs +++ b/src/dynamics/island_manager2.rs @@ -13,9 +13,45 @@ use vec_map::VecMap; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone, Default)] -struct Island { +pub(crate) struct Island { + /// The rigid-bodies part of this island. bodies: Vec, + /// The additional solver iterations needed by this island. additional_solver_iterations: usize, + /// Index of this island in `IslandManager::awake_islands`. + /// + /// If `None`, the island is sleeping. + id_in_awake_list: Option, +} + +impl Island { + pub fn singleton(handle: RigidBodyHandle, rb: &RigidBody) -> Self { + Self { + bodies: vec![handle], + additional_solver_iterations: rb.additional_solver_iterations, + id_in_awake_list: None, + } + } + + pub fn bodies(&self) -> &[RigidBodyHandle] { + &self.bodies + } + + pub fn additional_solver_iterations(&self) -> usize { + self.additional_solver_iterations + } + + pub fn is_sleeping(&self) -> bool { + self.id_in_awake_list.is_none() + } + + pub fn len(&self) -> usize { + self.bodies.len() + } + + pub(crate) fn id_in_awake_list(&self) -> Option { + self.id_in_awake_list + } } /// System that manages which bodies are active (awake) vs sleeping to optimize performance. @@ -33,46 +69,28 @@ struct Island { /// /// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline). #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone)] +#[derive(Clone, Default)] pub struct IslandManager { - // Island 0 is active, all the others are sleeping. pub(crate) islands: VecMap, + pub(crate) awake_islands: Vec, + // TODO PERF: should this be `Vec<(usize, Island)>` to reuse the allocation? pub(crate) free_islands: Vec, /// Potential candidate roots for graph traversal to identify a sleeping /// connected component. traversal_candidates: VecDeque, - /// Same as `traversal_candidates` but these roots **must** all be traversed as the next - /// timestep to properly handle bodies that are sleeping at creation. - priority_traversal_candidates: Vec, timestamp: u32, // TODO: the physics pipeline (or something else) should expose a step_id? #[cfg_attr(feature = "serde-serialize", serde(skip))] stack: Vec, // Workspace. } -impl Default for IslandManager { - fn default() -> Self { - let mut islands = VecMap::new(); - islands.insert(0, Island::default()); - Self { - islands, - free_islands: Default::default(), - traversal_candidates: Default::default(), - priority_traversal_candidates: Default::default(), - timestamp: Default::default(), - stack: Default::default(), - } - } -} - impl IslandManager { /// Creates a new empty island manager. pub fn new() -> Self { Self::default() } - // TODO: rename this to `num_active_islands`? - pub(crate) fn num_active_islands(&self) -> usize { - 1 + pub(crate) fn active_islands(&self) -> &[usize] { + &self.awake_islands } pub(crate) fn rigid_body_removed( @@ -91,6 +109,7 @@ impl IslandManager { .bodies .swap_remove(removed_ids.active_set_offset as usize); + // Remap the active_set_id of the body we moved with the `swap_remove`. if swapped_handle != removed_handle { let swapped_body = bodies .get_mut(swapped_handle) @@ -99,9 +118,16 @@ impl IslandManager { swapped_body.ids.active_set_id = removed_ids.active_set_id; } - if removed_ids.active_island_id != 0 && island.bodies.is_empty() { - // We removed the last body from this island. Remove the island (but never remove - // island 0 since it’s the special active island). + // If we deleted the last body from this island, delete the island. + if island.bodies.is_empty() { + if let Some(awake_id) = island.id_in_awake_list { + // Remove it from the free island list. + self.awake_islands.swap_remove(awake_id); + // Update the awake list index of the awake island id we moved. + if let Some(moved_id) = self.awake_islands.get(awake_id) { + self.islands[*moved_id].id_in_awake_list = Some(awake_id); + } + } self.islands.remove(removed_ids.active_island_id); self.free_islands.push(removed_ids.active_island_id); } @@ -115,11 +141,32 @@ impl IslandManager { started: bool, ) { if started { - if let Some(handle1) = handle1 { - self.wake_up(bodies, handle1, false); - } - if let Some(handle2) = handle2 { - self.wake_up(bodies, handle2, false); + match (handle1, handle2) { + (Some(handle1), Some(handle2)) => { + self.wake_up(bodies, handle1, false); + self.wake_up(bodies, handle2, false); + + if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) { + // If both bodies are not part of the same island, merge the islands. + if !rb1.is_fixed() + && !rb2.is_fixed() + && rb1.ids.active_island_id != rb2.ids.active_island_id + { + self.merge_islands( + bodies, + rb1.ids.active_island_id, + rb2.ids.active_island_id, + ); + } + } + } + (Some(handle1), None) => { + self.wake_up(bodies, handle1, false); + } + (None, Some(handle2)) => { + self.wake_up(bodies, handle2, false); + } + (None, None) => { /* Nothing to do. */ } } } } @@ -150,7 +197,7 @@ impl IslandManager { // NOTE: the use an Option here because there are many legitimate cases (like when // deleting a joint attached to an already-removed body) where we could be // attempting to wake-up a rigid-body that has already been deleted. - if bodies.get(handle).map(|rb| rb.is_dynamic_or_kinematic()) == Some(true) { + if bodies.get(handle).map(|rb| !rb.is_fixed()) == Some(true) { let rb = bodies.index_mut_internal(handle); // TODO: not sure if this is still relevant: @@ -161,53 +208,99 @@ impl IslandManager { // } rb.activation.wake_up(strong); - if rb.is_enabled() && rb.ids.active_island_id != 0 { - // Wake up the sleeping island this rigid-body is part of. - let Some(removed_island) = self.islands.remove(rb.ids.active_island_id) else { - // TODO: the island doesn’t exist is that an internal error? - return; - }; - - self.free_islands.push(rb.ids.active_island_id); - - // TODO: if we switched to linked list, we could avoid moving around all this memory. - let active_island = &mut self.islands[0]; - for handle in &removed_island.bodies { - let Some(rb) = bodies.get_mut_internal(*handle) else { - // This body no longer exists. - continue; - }; + let island_to_wake_up = rb.ids.active_island_id; + self.wake_up_island(bodies, island_to_wake_up); + } + } + + fn wake_up_island(&mut self, bodies: &mut RigidBodySet, island_id: usize) { + let Some(island) = self.islands.get_mut(island_id) else { + return; + }; + + if island.is_sleeping() { + island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(island_id); + + // Wake up all the bodies from this island. + for handle in &island.bodies { + if let Some(rb) = bodies.get_mut(*handle) { rb.wake_up(false); - rb.ids.active_island_id = 0; - rb.ids.active_set_id = active_island.bodies.len(); - rb.ids.active_set_offset = (active_island.bodies.len()) as u32; - active_island.bodies.push(*handle); } - - active_island.additional_solver_iterations = active_island - .additional_solver_iterations - .max(removed_island.additional_solver_iterations); } } } - // TODO: remove the island_id argument? - pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { - assert_eq!(island_id, 0); - &self.islands[0].bodies + fn merge_islands(&mut self, bodies: &mut RigidBodySet, island_id1: usize, island_id2: usize) { + if island_id1 == island_id2 { + return; + } + + println!("Merging: {} - {}", island_id1, island_id2); + let island1 = &self.islands[island_id1]; + let island2 = &self.islands[island_id2]; + + assert_eq!( + island1.id_in_awake_list.is_some(), + island2.id_in_awake_list.is_some(), + "Internal error: cannot merge two island with different sleeping statuses." + ); + + // Prefer removing the smallest island to reduce the amount of memory to move. + let (to_keep, to_remove) = if island1.bodies.len() < island2.bodies.len() { + (island_id2, island_id1) + } else { + (island_id1, island_id2) + }; + + println!("Removing: {}", to_remove); + + let Some(removed_island) = self.islands.remove(to_remove) else { + // TODO: the island doesn’t exist is that an internal error? + return; + }; + + self.free_islands.push(to_remove); + + // TODO: if we switched to linked list, we could avoid moving around all this memory. + let target_island = &mut self.islands[to_keep]; + for handle in &removed_island.bodies { + let Some(rb) = bodies.get_mut_internal(*handle) else { + // This body no longer exists. + continue; + }; + rb.wake_up(false); + rb.ids.active_island_id = to_keep; + rb.ids.active_set_id = target_island.bodies.len(); + rb.ids.active_set_offset = (target_island.bodies.len()) as u32; + target_island.bodies.push(*handle); + target_island.additional_solver_iterations = target_island + .additional_solver_iterations + .max(rb.additional_solver_iterations); + } + + // Update the awake_islands list. + if let Some(awake_id_to_remove) = removed_island.id_in_awake_list { + self.awake_islands.swap_remove(awake_id_to_remove); + // Update the awake list index of the awake island id we moved. + if let Some(moved_id) = self.awake_islands.get(awake_id_to_remove) { + self.islands[*moved_id].id_in_awake_list = Some(awake_id_to_remove); + } + } } - pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize { - self.islands[0].additional_solver_iterations + pub(crate) fn island(&self, island_id: usize) -> &Island { + &self.islands[island_id] } /// Handles of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping). #[inline] - pub fn active_bodies(&self) -> &[RigidBodyHandle] { - &self.islands[0].bodies + pub fn active_bodies(&self) -> impl Iterator + '_ { + self.awake_islands + .iter() + .flat_map(|i| self.islands[*i].bodies.iter().copied()) } - // TODO: what if the body is sleeping when we add it? pub(crate) fn update_body(&mut self, handle: RigidBodyHandle, bodies: &mut RigidBodySet) { let Some(rb) = bodies.get_mut(handle) else { return; @@ -217,20 +310,21 @@ impl IslandManager { return; } - let id = self.islands[0].bodies.len(); - - if rb.ids.active_set_id == usize::MAX { - // This body is new. - rb.ids.active_island_id = 0; - rb.ids.active_set_id = id; - rb.ids.active_set_offset = id as u32; - self.islands[0].bodies.push(handle); + if rb.ids.active_island_id == usize::MAX { + // We’ve never seen this rigid-body before. + let mut new_island = Island::singleton(handle, rb); + let id = self.free_islands.pop().unwrap_or(self.islands.len()); - if rb.is_sleeping() { - // The rigid-body is sleeping. We need to move it into a sleeping island during - // the next step. - self.priority_traversal_candidates.push(handle); + if !rb.is_sleeping() { + new_island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(id); } + + rb.ids.active_island_id = id; + rb.ids.active_set_id = 0; + rb.ids.active_set_offset = 0; + self.islands.insert(id, new_island); + println!("Created islands: {}", self.islands.len()); } // Push the body to the active set if it is not inside the active set yet, and @@ -256,6 +350,7 @@ impl IslandManager { multibody_joints: &MultibodyJointSet, min_island_size: usize, ) { + /* self.timestamp += 1; // 1. Update active rigid-bodies energy. // let t0 = std::time::Instant::now(); @@ -288,18 +383,6 @@ impl IslandManager { // let t0 = std::time::Instant::now(); let frame_base_timestamp = self.timestamp; let mut niters = 0; - for sleep_root in std::mem::take(&mut self.priority_traversal_candidates) { - niters += self.extract_sleeping_island( - bodies, - colliders, - impulse_joints, - multibody_joints, - narrow_phase, - sleep_root, - frame_base_timestamp, - ); - self.timestamp += 1; - } // 3. Perform one, or multiple, sleeping islands extraction (graph traversal). // Limit the traversal cost by not traversing all the known sleeping roots if @@ -323,6 +406,7 @@ impl IslandManager { self.timestamp += 1; } // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0); + */ } /// Returns the number of iterations run by the graph traversal so we can balance load across @@ -337,6 +421,8 @@ impl IslandManager { sleep_root: RigidBodyHandle, frame_base_timestamp: u32, ) -> usize { + 0 + /* let Some(rb) = bodies.get_mut_internal(sleep_root) else { // This branch happens if the rigid-body no longer exists. return 0; @@ -422,6 +508,7 @@ impl IslandManager { self.islands.insert(new_island_id, new_island); niter + */ } } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 216b649c6..be0da4362 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -377,7 +377,7 @@ impl ImpulseJointSet { bodies: &RigidBodySet, out: &mut [Vec], ) { - for out_island in &mut out[..islands.num_active_islands()] { + for out_island in &mut out[..islands.active_islands().len()] { out_island.clear(); } diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index dcc20b451..78af07122 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -244,7 +244,7 @@ impl InteractionGroups { // is full, we don't clear the corresponding body mask bit. This may result // in less grouped constraints. self.body_masks - .resize(islands.active_island(island_id).len(), 0u128); + .resize(islands.island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. @@ -396,7 +396,7 @@ impl InteractionGroups { // in less grouped contacts. // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop. self.body_masks - .resize(islands.active_island(island_id).len(), 0u128); + .resize(islands.island(island_id).len(), 0u128); // NOTE: each bit of the occupied mask indicates what bucket already // contains at least one constraint. diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 80f2d4b6d..9939c7190 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -44,7 +44,7 @@ impl IslandSolver { ) { counters.solver.velocity_assembly_time.resume(); let num_solver_iterations = base_params.num_solver_iterations - + islands.active_island_additional_solver_iterations(island_id); + + islands.island(island_id).additional_solver_iterations(); let mut params = *base_params; params.dt /= num_solver_iterations as Real; diff --git a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs index c7abe9af2..10f28b934 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraints_set.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraints_set.rs @@ -183,6 +183,7 @@ impl JointConstraintsSet { fn compute_generic_joint_constraints( &mut self, + // TODO: pass around the &Island directly. island_id: usize, islands: &IslandManager, bodies: &RigidBodySet, @@ -193,7 +194,7 @@ impl JointConstraintsSet { // Count the internal and external constraints builder. let num_external_constraint_builders = self.generic_two_body_interactions.len(); let mut num_internal_constraint_builders = 0; - for handle in islands.active_island(island_id) { + for handle in islands.island(island_id).bodies() { if let Some(link_id) = multibodies.rigid_body_link(*handle) { if JointGenericInternalConstraintBuilder::num_constraints(multibodies, link_id) > 0 { @@ -230,7 +231,7 @@ impl JointConstraintsSet { // Generate internal constraints builder. They are indexed after the let mut curr_builder = self.generic_two_body_interactions.len(); - for handle in islands.active_island(island_id) { + for handle in islands.island(island_id).bodies() { if curr_builder >= self.generic_velocity_constraints_builder.len() { break; // No more builder need to be generated. } diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index e186883db..517e84483 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -82,7 +82,7 @@ impl VelocitySolver { self.solver_bodies.clear(); let aligned_solver_bodies_len = - islands.active_island(island_id).len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH; + islands.island(island_id).len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH; self.solver_bodies.resize(aligned_solver_bodies_len); self.solver_vels_increment.clear(); @@ -97,7 +97,7 @@ impl VelocitySolver { // Assign solver ids to multibodies, and collect the relevant roots. // And init solver_vels for rigid-bodies. let mut multibody_solver_id = 0; - for handle in islands.active_island(island_id) { + for handle in islands.island(island_id).bodies() { if let Some(link) = multibodies.rigid_body_link(*handle).copied() { let multibody = multibodies .get_multibody_mut_internal(link.multibody) @@ -295,7 +295,7 @@ impl VelocitySolver { bodies: &mut RigidBodySet, multibodies: &mut MultibodyJointSet, ) { - for handle in islands.active_island(island_id) { + for handle in islands.island(island_id).bodies() { let link = if self.multibody_roots.is_empty() { None } else { diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 197a49d40..b8cdc8882 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -1271,7 +1271,7 @@ impl NarrowPhase { out_manifolds: &mut Vec<&'a mut ContactManifold>, out: &mut [Vec], ) { - for out_island in &mut out[..islands.num_active_islands()] { + for out_island in &mut out[..islands.active_islands().len()] { out_island.clear(); } @@ -1314,13 +1314,17 @@ impl NarrowPhase { && (!rb_type1.is_dynamic() || !sleeping1) && (!rb_type2.is_dynamic() || !sleeping2) { - let island_index = if !rb_type1.is_dynamic() { - active_island_id2 + let island_awake_index = if !rb_type1.is_dynamic() { + islands.islands[active_island_id2] + .id_in_awake_list() + .expect("Internal error: island should be awake.") } else { - active_island_id1 + islands.islands[active_island_id1] + .id_in_awake_list() + .expect("Internal error: island should be awake.") }; - out[island_index].push(out_manifolds.len()); + out[island_awake_index].push(out_manifolds.len()); out_manifolds.push(manifold); push_pair = true; } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index ed0f72763..328157a3d 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -207,14 +207,14 @@ impl PhysicsPipeline { integration_parameters.min_island_size, ); - if self.manifold_indices.len() < islands.num_active_islands() { - self.manifold_indices - .resize(islands.num_active_islands(), Vec::new()); + let num_active_islands = islands.active_islands().len(); + if self.manifold_indices.len() < num_active_islands { + self.manifold_indices.resize(num_active_islands, Vec::new()); } - if self.joint_constraint_indices.len() < islands.num_active_islands() { + if self.joint_constraint_indices.len() < num_active_islands { self.joint_constraint_indices - .resize(islands.num_active_islands(), Vec::new()); + .resize(num_active_islands, Vec::new()); } // let t0 = std::time::Instant::now(); @@ -241,7 +241,7 @@ impl PhysicsPipeline { for handle in islands.active_bodies() { // TODO: should that be moved to the solver (just like we moved // the multibody dynamics update) since it depends on dt? - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); rb.mprops .update_world_mass_properties(rb.body_type, &rb.pos.position); let effective_mass = rb.mprops.effective_mass(); @@ -251,26 +251,26 @@ impl PhysicsPipeline { self.counters.stages.update_time.pause(); self.counters.stages.solver_time.resume(); - if self.solvers.len() < islands.num_active_islands() { + if self.solvers.len() < num_active_islands { self.solvers - .resize_with(islands.num_active_islands(), IslandSolver::new); + .resize_with(num_active_islands, IslandSolver::new); } #[cfg(not(feature = "parallel"))] { enable_flush_to_zero!(); - for island_id in 0..islands.num_active_islands() { - self.solvers[island_id].init_and_solve( - island_id, + for (island_awake_id, island_id) in islands.active_islands().iter().enumerate() { + self.solvers[island_awake_id].init_and_solve( + *island_id, &mut self.counters, integration_parameters, islands, bodies, &mut manifolds[..], - &self.manifold_indices[island_id], + &self.manifold_indices[island_awake_id], impulse_joints.joints_mut(), - &self.joint_constraint_indices[island_id], + &self.joint_constraint_indices[island_awake_id], multibody_joints, ) } @@ -282,8 +282,7 @@ impl PhysicsPipeline { use rayon::prelude::*; use std::sync::atomic::Ordering; - let num_islands = islands.num_islands(); - let solvers = &mut self.solvers[..num_islands]; + let solvers = &mut self.solvers[..num_active_islands]; let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); let manifolds = &std::sync::atomic::AtomicPtr::new(&mut manifolds as *mut _); let impulse_joints = @@ -302,7 +301,8 @@ impl PhysicsPipeline { solvers .par_iter_mut() .enumerate() - .for_each(|(island_id, solver)| { + .for_each(|(island_awake_id, solver)| { + let island_id = islands.active_islands()[island_awake_id]; let bodies: &mut RigidBodySet = unsafe { &mut *bodies.load(Ordering::Relaxed) }; let manifolds: &mut Vec<&mut ContactManifold> = @@ -320,9 +320,9 @@ impl PhysicsPipeline { islands, bodies, &mut manifolds[..], - &manifold_indices[island_id], + &manifold_indices[island_awake_id], impulse_joints, - &joint_constraint_indices[island_id], + &joint_constraint_indices[island_awake_id], multibody_joints, ) }); @@ -395,7 +395,7 @@ impl PhysicsPipeline { ) { // Set the rigid-bodies and kinematic bodies to their final position. for handle in islands.active_bodies() { - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); rb.pos.position = rb.pos.next_position; rb.colliders .update_positions(colliders, modified_colliders, &rb.pos.position); @@ -415,7 +415,7 @@ impl PhysicsPipeline { // bodies it is touching. for handle in islands.active_bodies() { // TODO PERF: only iterate on kinematic position-based bodies - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); match rb.body_type { RigidBodyType::KinematicPositionBased => { @@ -749,7 +749,7 @@ impl PhysicsPipeline { // not modified by the user in the mean time. self.counters.stages.update_time.resume(); for handle in islands.active_bodies() { - let rb = bodies.index_mut_internal(*handle); + let rb = bodies.index_mut_internal(handle); rb.mprops .update_world_mass_properties(rb.body_type, &rb.pos.position); } From 51ed764329f377528e5408e328ec37fd1b9d1413 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sat, 1 Nov 2025 10:05:37 +0100 Subject: [PATCH 06/12] feat: add more timings --- examples2d/one_way_platforms2.rs | 2 +- src/counters/collision_detection_counters.rs | 4 ++++ src/counters/solver_counters.rs | 6 ++++++ src/dynamics/island_manager2.rs | 14 ++++++++++++-- src/dynamics/solver/interaction_groups.rs | 2 +- src/dynamics/solver/island_solver.rs | 13 ++++--------- src/dynamics/solver/velocity_solver.rs | 9 +++------ src/pipeline/physics_pipeline.rs | 6 ++++++ src_testbed/ui.rs | 14 ++++++++++++++ 9 files changed, 51 insertions(+), 19 deletions(-) diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index 0d005c488..158d1262a 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -105,7 +105,7 @@ pub fn init_world(testbed: &mut Testbed) { } for handle in physics.islands.active_bodies() { - let body = &mut physics.bodies[*handle]; + let body = &mut physics.bodies[handle]; if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); } else if body.position().translation.y < -1.0 { diff --git a/src/counters/collision_detection_counters.rs b/src/counters/collision_detection_counters.rs index 1d2d28fa8..ab5c360c7 100644 --- a/src/counters/collision_detection_counters.rs +++ b/src/counters/collision_detection_counters.rs @@ -8,6 +8,7 @@ pub struct CollisionDetectionCounters { pub ncontact_pairs: usize, /// Time spent for the broad-phase of the collision detection. pub broad_phase_time: Timer, + pub final_broad_phase_time: Timer, /// Time spent for the narrow-phase of the collision detection. pub narrow_phase_time: Timer, } @@ -18,6 +19,7 @@ impl CollisionDetectionCounters { CollisionDetectionCounters { ncontact_pairs: 0, broad_phase_time: Timer::new(), + final_broad_phase_time: Timer::new(), narrow_phase_time: Timer::new(), } } @@ -26,6 +28,7 @@ impl CollisionDetectionCounters { pub fn reset(&mut self) { self.ncontact_pairs = 0; self.broad_phase_time.reset(); + self.final_broad_phase_time.reset(); self.narrow_phase_time.reset(); } } @@ -34,6 +37,7 @@ impl Display for CollisionDetectionCounters { fn fmt(&self, f: &mut Formatter) -> Result { writeln!(f, "Number of contact pairs: {}", self.ncontact_pairs)?; writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?; + writeln!(f, "Final broad-phase time: {}", self.final_broad_phase_time)?; writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time) } } diff --git a/src/counters/solver_counters.rs b/src/counters/solver_counters.rs index aced5f396..f7193d294 100644 --- a/src/counters/solver_counters.rs +++ b/src/counters/solver_counters.rs @@ -12,6 +12,8 @@ pub struct SolverCounters { pub velocity_resolution_time: Timer, /// Time spent for the assembly of all the velocity constraints. pub velocity_assembly_time: Timer, + pub velocity_assembly_time_a: Timer, + pub velocity_assembly_time_b: Timer, /// Time spent for the update of the velocity of the bodies. pub velocity_update_time: Timer, /// Time spent to write force back to user-accessible data. @@ -25,6 +27,8 @@ impl SolverCounters { nconstraints: 0, ncontacts: 0, velocity_assembly_time: Timer::new(), + velocity_assembly_time_a: Timer::new(), + velocity_assembly_time_b: Timer::new(), velocity_resolution_time: Timer::new(), velocity_update_time: Timer::new(), velocity_writeback_time: Timer::new(), @@ -37,6 +41,8 @@ impl SolverCounters { self.ncontacts = 0; self.velocity_resolution_time.reset(); self.velocity_assembly_time.reset(); + self.velocity_assembly_time_a.reset(); + self.velocity_assembly_time_b.reset(); self.velocity_update_time.reset(); self.velocity_writeback_time.reset(); } diff --git a/src/dynamics/island_manager2.rs b/src/dynamics/island_manager2.rs index 9db68af07..0f45c4096 100644 --- a/src/dynamics/island_manager2.rs +++ b/src/dynamics/island_manager2.rs @@ -236,7 +236,6 @@ impl IslandManager { return; } - println!("Merging: {} - {}", island_id1, island_id2); let island1 = &self.islands[island_id1]; let island2 = &self.islands[island_id2]; @@ -253,7 +252,7 @@ impl IslandManager { (island_id1, island_id2) }; - println!("Removing: {}", to_remove); + println!("Merging: {} <- {}", to_keep, to_remove); let Some(removed_island) = self.islands.remove(to_remove) else { // TODO: the island doesn’t exist is that an internal error? @@ -287,6 +286,17 @@ impl IslandManager { self.islands[*moved_id].id_in_awake_list = Some(awake_id_to_remove); } } + + if self.islands[to_keep].bodies.len() > 48 { + println!( + "Completed: {:?}", + self.islands[to_keep] + .bodies + .iter() + .map(|h| h.into_raw_parts().0) + .collect::>() + ); + } } pub(crate) fn island(&self, island_id: usize) -> &Island { diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index 78af07122..d537d894c 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -72,7 +72,7 @@ impl ParallelInteractionGroups { interactions: &[Interaction], interaction_indices: &[usize], ) { - let num_island_bodies = islands.active_island(island_id).len(); + let num_island_bodies = islands.island(island_id).len(); self.bodies_color.clear(); self.interaction_indices.clear(); self.groups.clear(); diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index 9939c7190..c8c395206 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -43,6 +43,7 @@ impl IslandSolver { multibodies: &mut MultibodyJointSet, ) { counters.solver.velocity_assembly_time.resume(); + counters.solver.velocity_assembly_time_a.resume(); let num_solver_iterations = base_params.num_solver_iterations + islands.island(island_id).additional_solver_iterations(); @@ -55,7 +56,6 @@ impl IslandSolver { * */ // INIT - // let t0 = std::time::Instant::now(); self.velocity_solver .init_solver_velocities_and_solver_bodies( base_params.dt, @@ -65,8 +65,8 @@ impl IslandSolver { bodies, multibodies, ); - // let t_solver_body_init = t0.elapsed().as_secs_f32(); - // let t0 = std::time::Instant::now(); + counters.solver.velocity_assembly_time_a.pause(); + counters.solver.velocity_assembly_time_b.resume(); self.velocity_solver.init_constraints( island_id, islands, @@ -81,13 +81,8 @@ impl IslandSolver { #[cfg(feature = "dim3")] params.friction_model, ); - // let t_init_constraints = t0.elapsed().as_secs_f32(); + counters.solver.velocity_assembly_time_b.pause(); counters.solver.velocity_assembly_time.pause(); - // println!( - // "Solver body init: {}, init constraints: {}", - // t_solver_body_init * 1000.0, - // t_init_constraints * 1000.0 - // ); // SOLVE counters.solver.velocity_resolution_time.resume(); diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 517e84483..b718f2140 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -97,7 +97,8 @@ impl VelocitySolver { // Assign solver ids to multibodies, and collect the relevant roots. // And init solver_vels for rigid-bodies. let mut multibody_solver_id = 0; - for handle in islands.island(island_id).bodies() { + + for (offset, handle) in islands.island(island_id).bodies().iter().enumerate() { if let Some(link) = multibodies.rigid_body_link(*handle).copied() { let multibody = multibodies .get_multibody_mut_internal(link.multibody) @@ -110,11 +111,7 @@ impl VelocitySolver { } } else { let rb = &bodies[*handle]; - if rb.ids.active_set_offset == u32::MAX { - println!("Invalid: {:?} -> {}", *handle, rb.ids.active_set_offset); - println!("ty: {:?}", rb.body_type); - } - + assert_eq!(offset, rb.ids.active_set_offset as usize); let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_offset as usize]; self.solver_bodies diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index 328157a3d..f07bdc154 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -649,7 +649,9 @@ impl PhysicsPipeline { self.counters.ccd.num_substeps += 1; + self.counters.custom.resume(); self.interpolate_kinematic_velocities(&integration_parameters, islands, bodies); + self.counters.custom.pause(); self.build_islands_and_solve_velocity_constraints( gravity, &integration_parameters, @@ -711,6 +713,8 @@ impl PhysicsPipeline { } else { // If we ran the last substep, just update the broad-phase bvh instead // of a full collision-detection step. + self.counters.stages.collision_detection_time.resume(); + self.counters.cd.final_broad_phase_time.resume(); for handle in modified_colliders.iter() { let co = colliders.index_mut_internal(*handle); // NOTE: `advance_to_final_positions` might have added disabled colliders to @@ -738,6 +742,8 @@ impl PhysicsPipeline { // Empty the modified colliders set. See comment for `co.change.remove(..)` above. modified_colliders.clear(); + self.counters.cd.final_broad_phase_time.pause(); + self.counters.stages.collision_detection_time.pause(); } } diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 844ae39c0..3cf46dfde 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -320,6 +320,10 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { "Broad-phase: {:.2}ms", counters.broad_phase_time_ms() )); + ui.label(format!( + "Final broad-phase: {:.2}ms", + counters.cd.final_broad_phase_time.time_ms() + )); ui.label(format!( "Narrow-phase: {:.2}ms", counters.narrow_phase_time_ms() @@ -332,6 +336,14 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { "Velocity assembly: {:.2}ms", counters.solver.velocity_assembly_time.time_ms() )); + ui.label(format!( + "> Velocity assembly A: {:.2}ms", + counters.solver.velocity_assembly_time_a.time_ms() + )); + ui.label(format!( + "> Velocity assembly B: {:.2}ms", + counters.solver.velocity_assembly_time_b.time_ms() + )); ui.label(format!( "Velocity resolution: {:.2}ms", counters.velocity_resolution_time_ms() @@ -370,10 +382,12 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { "Island computation: {:.2}ms", counters.island_construction_time_ms() )); + ui.label(format!("Mprops update: {:.2}ms", counters.update_time_ms())); ui.label(format!( "User changes: {:.2}ms", counters.stages.user_changes.time_ms() )); + ui.label(format!("Debug timer: {:.2}ms", counters.custom.time_ms())); }); } From 3e88c718e384d0e78859652e8de871aa33262f46 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 6 Nov 2025 16:13:27 +0100 Subject: [PATCH 07/12] feat: implement incremental island split & merge --- examples3d/one_way_platforms3.rs | 2 +- src/counters/stages_counters.rs | 9 + src/dynamics/island_manager2.rs | 416 +++++++++++++++--- .../joint/impulse_joint/impulse_joint_set.rs | 17 +- .../multibody_joint/multibody_joint_set.rs | 5 + src/geometry/narrow_phase.rs | 3 +- src/pipeline/physics_pipeline.rs | 42 +- src_testbed/ui.rs | 4 + 8 files changed, 428 insertions(+), 70 deletions(-) diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index f9f7cb05b..5f7dc6898 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -105,7 +105,7 @@ pub fn init_world(testbed: &mut Testbed) { } for handle in physics.islands.active_bodies() { - let body = physics.bodies.get_mut(*handle).unwrap(); + let body = physics.bodies.get_mut(handle).unwrap(); if body.position().translation.y > 1.0 { body.set_gravity_scale(1.0, false); } else if body.position().translation.y < -1.0 { diff --git a/src/counters/stages_counters.rs b/src/counters/stages_counters.rs index a3a1841fd..eaaf2516a 100644 --- a/src/counters/stages_counters.rs +++ b/src/counters/stages_counters.rs @@ -10,6 +10,8 @@ pub struct StagesCounters { pub collision_detection_time: Timer, /// Time spent for the computation of collision island and body activation/deactivation (sleeping). pub island_construction_time: Timer, + /// Time spent for collecting awake constraints from islands. + pub island_constraints_collection_time: Timer, /// Total time spent for the constraints resolution and position update.t pub solver_time: Timer, /// Total time spent for CCD and CCD resolution. @@ -25,6 +27,7 @@ impl StagesCounters { update_time: Timer::new(), collision_detection_time: Timer::new(), island_construction_time: Timer::new(), + island_constraints_collection_time: Timer::new(), solver_time: Timer::new(), ccd_time: Timer::new(), user_changes: Timer::new(), @@ -36,6 +39,7 @@ impl StagesCounters { self.update_time.reset(); self.collision_detection_time.reset(); self.island_construction_time.reset(); + self.island_constraints_collection_time.reset(); self.solver_time.reset(); self.ccd_time.reset(); self.user_changes.reset(); @@ -55,6 +59,11 @@ impl Display for StagesCounters { "Island construction time: {}", self.island_construction_time )?; + writeln!( + f, + "Island construction time: {}", + self.island_constraints_collection_time + )?; writeln!(f, "Solver time: {}", self.solver_time)?; writeln!(f, "CCD time: {}", self.ccd_time)?; writeln!(f, "User changes time: {}", self.user_changes) diff --git a/src/dynamics/island_manager2.rs b/src/dynamics/island_manager2.rs index 0f45c4096..3362d7b06 100644 --- a/src/dynamics/island_manager2.rs +++ b/src/dynamics/island_manager2.rs @@ -8,7 +8,7 @@ use crate::math::Real; use crate::prelude::{ColliderHandle, ContactManifoldData}; use crate::utils::SimdDot; use std::collections::VecDeque; -use std::ops::IndexMut; +use std::ops::{Index, IndexMut}; use vec_map::VecMap; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -54,6 +54,59 @@ impl Island { } } +#[derive(Copy, Clone, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct IslandsOptimizerMergeState { + curr_awake_id: usize, +} + +#[derive(Copy, Clone, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct IslandsOptimizerSplitState { + curr_awake_id: usize, + curr_body_id: usize, +} + +/// Configuration of the awake islands optimization strategy. +/// +/// Note that this currently only affects active islands. Sleeping islands are always kept minimal. +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct IslandsOptimizer { + /// The optimizer will try to merge small islands so their size exceed this minimum. + /// + /// Note that it will never merge incompatible islands (currently define as islands with + /// different additional solver iteration counts). + min_island_size: usize, + /// The optimizer will try to split large islands so their size do not exceed this maximum. + /// + /// IMPORTANT: Must be greater than `2 * min_island_size` to avoid conflict between the splits + /// and merges. + max_island_size: usize, + /// Indicates if the optimizer is in split or merge mode. Swaps between modes every step. + mode: usize, + merge_state: IslandsOptimizerMergeState, + split_state: IslandsOptimizerSplitState, +} + +impl Default for IslandsOptimizer { + fn default() -> Self { + // TODO: figure out the best values. + Self { + min_island_size: 1024, + max_island_size: 4096, + mode: 0, + merge_state: Default::default(), + split_state: Default::default(), + } + } +} + +/// An island starting at this rigid-body might be eligible for sleeping. +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct SleepCandidate(RigidBodyHandle); + /// System that manages which bodies are active (awake) vs sleeping to optimize performance. /// /// ## Sleeping Optimization @@ -76,9 +129,10 @@ pub struct IslandManager { // TODO PERF: should this be `Vec<(usize, Island)>` to reuse the allocation? pub(crate) free_islands: Vec, /// Potential candidate roots for graph traversal to identify a sleeping - /// connected component. - traversal_candidates: VecDeque, + /// connected component or to split an island in two. + traversal_candidates: VecDeque, timestamp: u32, // TODO: the physics pipeline (or something else) should expose a step_id? + optimizer: IslandsOptimizer, #[cfg_attr(feature = "serde-serialize", serde(skip))] stack: Vec, // Workspace. } @@ -133,18 +187,21 @@ impl IslandManager { } } - pub(crate) fn contact_started_or_stopped( + pub(crate) fn interaction_started_or_stopped( &mut self, bodies: &mut RigidBodySet, handle1: Option, handle2: Option, started: bool, + wake_up: bool, ) { if started { match (handle1, handle2) { (Some(handle1), Some(handle2)) => { - self.wake_up(bodies, handle1, false); - self.wake_up(bodies, handle2, false); + if wake_up { + self.wake_up(bodies, handle1, false); + self.wake_up(bodies, handle2, false); + } if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) { // If both bodies are not part of the same island, merge the islands. @@ -152,6 +209,9 @@ impl IslandManager { && !rb2.is_fixed() && rb1.ids.active_island_id != rb2.ids.active_island_id { + assert_ne!(rb1.ids.active_island_id, usize::MAX); + assert_ne!(rb2.ids.active_island_id, usize::MAX); + self.merge_islands( bodies, rb1.ids.active_island_id, @@ -161,10 +221,14 @@ impl IslandManager { } } (Some(handle1), None) => { - self.wake_up(bodies, handle1, false); + if wake_up { + self.wake_up(bodies, handle1, false); + } } (None, Some(handle2)) => { - self.wake_up(bodies, handle2, false); + if wake_up { + self.wake_up(bodies, handle2, false); + } } (None, None) => { /* Nothing to do. */ } } @@ -252,8 +316,6 @@ impl IslandManager { (island_id1, island_id2) }; - println!("Merging: {} <- {}", to_keep, to_remove); - let Some(removed_island) = self.islands.remove(to_remove) else { // TODO: the island doesn’t exist is that an internal error? return; @@ -286,17 +348,6 @@ impl IslandManager { self.islands[*moved_id].id_in_awake_list = Some(awake_id_to_remove); } } - - if self.islands[to_keep].bodies.len() > 48 { - println!( - "Completed: {:?}", - self.islands[to_keep] - .bodies - .iter() - .map(|h| h.into_raw_parts().0) - .collect::>() - ); - } } pub(crate) fn island(&self, island_id: usize) -> &Island { @@ -320,21 +371,39 @@ impl IslandManager { return; } + // Check if this is the first time we see this rigid-body. if rb.ids.active_island_id == usize::MAX { - // We’ve never seen this rigid-body before. - let mut new_island = Island::singleton(handle, rb); - let id = self.free_islands.pop().unwrap_or(self.islands.len()); + // Check if there is room in the last awake island to add this body. + // NOTE: only checking the last is suboptimal. Perhaps we should keep vec of + // small islands ids? + let insert_in_last_island = self.awake_islands.last().map(|id| { + self.islands[*id].bodies.len() < self.optimizer.min_island_size + && self.islands[*id].is_sleeping() == rb.is_sleeping() + }); + // let insert_in_last_island = insert_in_last_island.is_some().then_some(true); + + if !rb.is_sleeping() && insert_in_last_island == Some(true) { + let id = *self.awake_islands.last().unwrap_or_else(|| unreachable!()); + let target_island = &mut self.islands[id]; + + rb.ids.active_island_id = id; + rb.ids.active_set_id = target_island.bodies.len(); + rb.ids.active_set_offset = target_island.bodies.len() as u32; + target_island.bodies.push(handle); + } else { + let mut new_island = Island::singleton(handle, rb); + let id = self.free_islands.pop().unwrap_or(self.islands.len()); + + if !rb.is_sleeping() { + new_island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(id); + } - if !rb.is_sleeping() { - new_island.id_in_awake_list = Some(self.awake_islands.len()); - self.awake_islands.push(id); + self.islands.insert(id, new_island); + rb.ids.active_island_id = id; + rb.ids.active_set_id = 0; + rb.ids.active_set_offset = 0; } - - rb.ids.active_island_id = id; - rb.ids.active_set_id = 0; - rb.ids.active_set_offset = 0; - self.islands.insert(id, new_island); - println!("Created islands: {}", self.islands.len()); } // Push the body to the active set if it is not inside the active set yet, and @@ -360,12 +429,16 @@ impl IslandManager { multibody_joints: &MultibodyJointSet, min_island_size: usize, ) { - /* self.timestamp += 1; // 1. Update active rigid-bodies energy. + // TODO PERF: should this done by the velocity solver after solving the constraints? // let t0 = std::time::Instant::now(); - for handle in &self.islands[0].bodies { - let Some(rb) = bodies.get_mut_internal(*handle) else { + for handle in self + .awake_islands + .iter() + .flat_map(|i| self.islands[*i].bodies.iter().copied()) + { + let Some(rb) = bodies.get_mut_internal(handle) else { // This branch happens if the rigid-body no longer exists. continue; }; @@ -383,40 +456,46 @@ impl IslandManager { if !can_sleep_before && can_sleep_now { // This is a new candidate for island extraction. if !rb.activation.is_sleep_root_candidate { - self.traversal_candidates.push_back(*handle); + self.traversal_candidates.push_back(SleepCandidate(handle)); } } } // println!("Update energy: {}", t0.elapsed().as_secs_f32() * 1000.0); - // 3. Perform sleeping islands extraction on **all** the prioritized sleep candidates. - // let t0 = std::time::Instant::now(); let frame_base_timestamp = self.timestamp; - let mut niters = 0; + let mut cost = 0; // 3. Perform one, or multiple, sleeping islands extraction (graph traversal). // Limit the traversal cost by not traversing all the known sleeping roots if // there are too many. + const MAX_PER_FRAME_COST: usize = 1000; // TODO: find the best value. while let Some(sleep_root) = self.traversal_candidates.pop_front() { - if niters > 1000 { - break; // Limit cost per frame. + if cost > MAX_PER_FRAME_COST { + // Early-break if we consider we have done enough island extraction work. + break; } - niters += self.extract_sleeping_island( + cost += self.extract_sleeping_island( bodies, colliders, impulse_joints, multibody_joints, narrow_phase, - sleep_root, + sleep_root.0, frame_base_timestamp, ); - // TODO PERF: early-break if we consider we have done enough island extraction work. self.timestamp += 1; } + + self.update_optimizer( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + ); // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0); - */ } /// Returns the number of iterations run by the graph traversal so we can balance load across @@ -431,16 +510,22 @@ impl IslandManager { sleep_root: RigidBodyHandle, frame_base_timestamp: u32, ) -> usize { - 0 - /* let Some(rb) = bodies.get_mut_internal(sleep_root) else { // This branch happens if the rigid-body no longer exists. return 0; }; rb.activation.is_sleep_root_candidate = false; - if rb.ids.active_island_id != 0 || rb.ids.active_set_timestamp == self.timestamp { - return 0; // This rigid-body is already in a sleeping island or was already traversed. + + if rb.ids.active_set_timestamp >= frame_base_timestamp { + return 0; // We already traversed this rigid-body this frame. + } + + let active_island_id = rb.ids.active_island_id; + let active_island = &mut self.islands[active_island_id]; + if active_island.is_sleeping() { + // This rigid-body is already part of a sleeping island. + return 0; } // TODO: implement recycling islands to avoid repeated allocations? @@ -496,29 +581,250 @@ impl IslandManager { // - Put its bodies to sleep. // - Remove them from the active set. // - Push the sleeping island. + if active_island.len() == new_island.len() { + // The whole island is asleep. No need to insert a new one. + // Put all its bodies to sleep. + for handle in &active_island.bodies { + let rb = bodies.index_mut_internal(*handle); + rb.sleep(); + } + + // Mark the existing island as sleeping (by clearing its `id_in_awake_list`) + // and remove it from the awake list. + let island_awake_id = active_island + .id_in_awake_list + .take() + .unwrap_or_else(|| unreachable!()); + self.awake_islands.swap_remove(island_awake_id); + + if let Some(moved_id) = self.awake_islands.get(island_awake_id) { + self.islands[*moved_id].id_in_awake_list = Some(island_awake_id); + } + } else { + niter += new_island.len(); // Include this part into the cost estimate for this function. + self.extract_sub_island(bodies, active_island_id, new_island, true); + } + niter + } + + fn update_optimizer( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + ) { + if self.optimizer.mode % 2 == 0 { + self.incremental_merge(bodies); + } else { + self.incremental_split( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + ); + } + + self.optimizer.mode = self.optimizer.mode.wrapping_add(1); + } + + /// Attempts to merge awake islands that are too small. + fn incremental_merge(&mut self, bodies: &mut RigidBodySet) { + struct IslandData { + id: usize, + awake_id: usize, + solver_iters: usize, + } + + // Ensure the awake id is still in bounds. + if self.optimizer.merge_state.curr_awake_id >= self.awake_islands.len() { + self.optimizer.merge_state.curr_awake_id = 0; + } + + // Find a first candidate for a merge. + let mut island1 = None; + for awake_id in self.optimizer.merge_state.curr_awake_id..self.awake_islands.len() { + let id = self.awake_islands[awake_id]; + let island = &self.islands[id]; + if island.len() < self.optimizer.min_island_size { + island1 = Some(IslandData { + awake_id, + id, + solver_iters: island.additional_solver_iterations, + }); + break; + } + } + + if let Some(island1) = island1 { + // Indicates if we found a merge candidate for the next incremental update. + let mut found_next = false; + self.optimizer.merge_state.curr_awake_id = island1.awake_id + 1; + + // Find a second candidate for a merge. + for awake_id2 in island1.awake_id + 1..self.awake_islands.len() { + let id2 = self.awake_islands[awake_id2]; + let island2 = &self.islands[id2]; + + if island1.solver_iters == island2.additional_solver_iterations + && island2.len() < self.optimizer.min_island_size + { + // Found a second candidate! Merge them. + self.merge_islands(bodies, island1.id, id2); + println!("Found a merge {}!", self.awake_islands.len()); + + // TODO: support doing more than a single merge per frame. + return; + } else if island2.len() < self.optimizer.min_island_size && !found_next { + // We found a good candidate for the next incremental merge (we can’t just + // merge it now because it’s not compatible with the current island). + self.optimizer.merge_state.curr_awake_id = awake_id2; + found_next = true; + } + } + } else { + self.optimizer.merge_state.curr_awake_id = 0; + } + } + + /// Attempts to split awake islands that are too big. + fn incremental_split( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + ) { + if self.optimizer.split_state.curr_awake_id >= self.awake_islands.len() { + self.optimizer.split_state.curr_awake_id = 0; + } + + for awake_id in self.optimizer.split_state.curr_awake_id..self.awake_islands.len() { + let id = self.awake_islands[awake_id]; + if self.islands[id].len() > self.optimizer.max_island_size { + // Try to split this island. + // Note that the traversal logic is similar to the sleeping island + // extraction, except that we have different stopping criteria. + self.stack.clear(); + + // TODO: implement islands recycling to avoid reallocating every time. + let mut new_island = Island::default(); + self.timestamp += 1; + + for root in &self.islands[id].bodies { + self.stack.push(*root); + + let len_before_traversal = new_island.len(); + while let Some(handle) = self.stack.pop() { + let rb = bodies.index_mut(handle); + if rb.is_fixed() { + // Don’t propagate islands through rigid-bodies. + continue; + } + + if rb.ids.active_set_timestamp == self.timestamp { + // We already visited this body. + continue; + } + + rb.ids.active_set_timestamp = self.timestamp; + assert!(!rb.activation.sleeping); + + // Traverse bodies that are interacting with the current one either through + // contacts or a joint. + push_contacting_bodies( + &rb.colliders, + colliders, + narrow_phase, + &mut self.stack, + ); + push_linked_bodies( + impulse_joints, + multibody_joints, + handle, + &mut self.stack, + ); + new_island.bodies.push(handle); + + // Our new island cannot grow any further. + if new_island.bodies.len() > self.optimizer.max_island_size { + new_island.bodies.truncate(len_before_traversal); + self.stack.clear(); + break; + } + } + + // Extract this island. + if new_island.len() == 0 { + // println!("Failed to split island."); + return; + } else if new_island.bodies.len() >= self.optimizer.min_island_size { + // println!( + // "Split an island: {}/{} ({} islands)", + // new_island.len(), + // self.islands[id].len(), + // self.awake_islands.len(), + // ); + self.extract_sub_island(bodies, id, new_island, false); + return; // TODO: support extracting more than one island per frame. + } + } + } else { + self.optimizer.split_state.curr_awake_id = awake_id + 1; + } + } + } + + /// Remove from the island at `source_id` all the rigid-body that are in `new_island`, and + /// insert `new_island` into the islands set. + /// + /// **All** rigid-bodies from `new_island` must currently be part of the island at `source_id`. + fn extract_sub_island( + &mut self, + bodies: &mut RigidBodySet, + source_id: usize, + mut new_island: Island, + sleep: bool, + ) { let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len()); - let active_island = &mut self.islands[0]; + let active_island = &mut self.islands[source_id]; + for (id, handle) in new_island.bodies.iter().enumerate() { let rb = bodies.index_mut_internal(*handle); - rb.sleep(); + + // If the new island is sleeping, ensure all its bodies are sleeping. + if sleep { + rb.sleep(); + } let id_to_remove = rb.ids.active_set_id; + assert_eq!(rb.ids.active_island_id, source_id); rb.ids.active_island_id = new_island_id; rb.ids.active_set_id = id; rb.ids.active_set_offset = id as u32; + new_island.additional_solver_iterations = new_island + .additional_solver_iterations + .max(rb.additional_solver_iterations); active_island.bodies.swap_remove(id_to_remove); + if let Some(moved_handle) = active_island.bodies.get(id_to_remove).copied() { let moved_rb = bodies.index_mut_internal(moved_handle); moved_rb.ids.active_set_id = id_to_remove; moved_rb.ids.active_set_offset = id_to_remove as u32; } - niter += 1; // This contributes to the cost of the operation. + } + + // If the new island is awake, add it to the awake list. + if !sleep { + new_island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(new_island_id); } self.islands.insert(new_island_id, new_island); - niter - */ } } diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index be0da4362..07104b4bd 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -73,6 +73,8 @@ pub struct ImpulseJointSet { joint_graph: InteractionGraph, /// A set of rigid-body handles to wake-up during the next timestep. pub(crate) to_wake_up: HashSet, + /// A set of rigid-body pairs to join in the island manager during the next timestep. + pub(crate) to_join: HashSet<(RigidBodyHandle, RigidBodyHandle)>, } impl ImpulseJointSet { @@ -83,6 +85,7 @@ impl ImpulseJointSet { joint_ids: Arena::new(), joint_graph: InteractionGraph::new(), to_wake_up: HashSet::default(), + to_join: HashSet::default(), } } @@ -366,6 +369,8 @@ impl ImpulseJointSet { self.to_wake_up.insert(body2); } + self.to_join.insert((body1, body2)); + ImpulseJointHandle(handle) } @@ -392,13 +397,17 @@ impl ImpulseJointSet { && (!rb1.is_dynamic_or_kinematic() || !rb1.is_sleeping()) && (!rb2.is_dynamic_or_kinematic() || !rb2.is_sleeping()) { - let island_index = if !rb1.is_dynamic_or_kinematic() { - rb2.ids.active_island_id + let island_awake_index = if !rb1.is_dynamic_or_kinematic() { + islands.islands[rb2.ids.active_island_id] + .id_in_awake_list() + .expect("Internal error: island should be awake.") } else { - rb1.ids.active_island_id + islands.islands[rb1.ids.active_island_id] + .id_in_awake_list() + .expect("Internal error: island should be awake.") }; - out[island_index].push(i); + out[island_awake_index].push(i); } } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index b3e184e57..7295fd7bd 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -95,6 +95,8 @@ pub struct MultibodyJointSet { // that any more in the future when we improve our island builder. pub(crate) connectivity_graph: InteractionGraph, pub(crate) to_wake_up: HashSet, + /// A set of rigid-body pairs to join in the island manager during the next timestep. + pub(crate) to_join: HashSet<(RigidBodyHandle, RigidBodyHandle)>, } impl MultibodyJointSet { @@ -105,6 +107,7 @@ impl MultibodyJointSet { rb2mb: Coarena::new(), connectivity_graph: InteractionGraph::new(), to_wake_up: HashSet::default(), + to_join: HashSet::default(), } } @@ -205,6 +208,8 @@ impl MultibodyJointSet { self.to_wake_up.insert(body2); } + self.to_join.insert((body1, 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. diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index b8cdc8882..9ab56c6ac 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -1251,11 +1251,12 @@ impl NarrowPhase { } // FIXME: this won’t work with the parallel feature enabled. - islands.contact_started_or_stopped( + islands.interaction_started_or_stopped( bodies, co1.parent.map(|p| p.handle), co2.parent.map(|p| p.handle), pair.has_any_active_contact, + true, ); } }); diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index f07bdc154..e76a82276 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -216,8 +216,12 @@ impl PhysicsPipeline { self.joint_constraint_indices .resize(num_active_islands, Vec::new()); } + self.counters.stages.island_construction_time.pause(); - // let t0 = std::time::Instant::now(); + self.counters + .stages + .island_constraints_collection_time + .resume(); let mut manifolds = Vec::new(); narrow_phase.select_active_contacts( islands, @@ -231,11 +235,10 @@ impl PhysicsPipeline { bodies, &mut self.joint_constraint_indices, ); - // println!( - // "Manifolds extraction: {:?}", - // t0.elapsed().as_secs_f32() * 1000.0 - // ); - self.counters.stages.island_construction_time.pause(); + self.counters + .stages + .island_constraints_collection_time + .pause(); self.counters.stages.update_time.resume(); for handle in islands.active_bodies() { @@ -501,16 +504,16 @@ impl PhysicsPipeline { // Apply some of delayed wake-ups. self.counters.stages.user_changes.start(); #[cfg(feature = "enhanced-determinism")] - let impulse_joints_iterator = impulse_joints + let to_wake_up_iterator = impulse_joints .to_wake_up .drain(..) .chain(multibody_joints.to_wake_up.drain(..)); #[cfg(not(feature = "enhanced-determinism"))] - let impulse_joints_iterator = impulse_joints + let to_wake_up_iterator = impulse_joints .to_wake_up .drain() .chain(multibody_joints.to_wake_up.drain()); - for handle in impulse_joints_iterator { + for handle in to_wake_up_iterator { islands.wake_up(bodies, handle, true); } @@ -544,6 +547,27 @@ impl PhysicsPipeline { .copied() .filter(|h| colliders.get(*h).map(|c| !c.is_enabled()).unwrap_or(false)), ); + + // Join islands based on new joints. + #[cfg(feature = "enhanced-determinism")] + let to_join_iterator = impulse_joints + .to_join + .drain(..) + .chain(multibody_joints.to_join.drain(..)); + #[cfg(not(feature = "enhanced-determinism"))] + let to_join_iterator = impulse_joints + .to_join + .drain() + .chain(multibody_joints.to_join.drain()); + for (handle1, handle2) in to_join_iterator { + islands.interaction_started_or_stopped( + bodies, + Some(handle1), + Some(handle2), + true, + false, + ); + } self.counters.stages.user_changes.pause(); // TODO: do this only on user-change. diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 3cf46dfde..be95082e7 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -382,6 +382,10 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { "Island computation: {:.2}ms", counters.island_construction_time_ms() )); + ui.label(format!( + "Active constraints collection: {:.2}ms", + counters.stages.island_constraints_collection_time.time_ms() + )); ui.label(format!("Mprops update: {:.2}ms", counters.update_time_ms())); ui.label(format!( "User changes: {:.2}ms", From 0fd8d10cb2c98162b39c44b5cf2b0f16c90d4b9a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Thu, 6 Nov 2025 17:38:00 +0100 Subject: [PATCH 08/12] chore: refactor islands manager into multiple files --- src/dynamics/island_manager.rs | 357 ---------- src/dynamics/island_manager/island.rs | 157 ++++ src/dynamics/island_manager/manager.rs | 297 ++++++++ src/dynamics/island_manager/mod.rs | 10 + src/dynamics/island_manager/optimizer.rs | 226 ++++++ src/dynamics/island_manager/sleep.rs | 185 +++++ src/dynamics/island_manager/utils.rs | 44 ++ src/dynamics/island_manager2.rs | 871 ----------------------- src/dynamics/mod.rs | 1 - src/pipeline/physics_pipeline.rs | 2 +- src/pipeline/user_changes.rs | 2 +- 11 files changed, 921 insertions(+), 1231 deletions(-) delete mode 100644 src/dynamics/island_manager.rs create mode 100644 src/dynamics/island_manager/island.rs create mode 100644 src/dynamics/island_manager/manager.rs create mode 100644 src/dynamics/island_manager/mod.rs create mode 100644 src/dynamics/island_manager/optimizer.rs create mode 100644 src/dynamics/island_manager/sleep.rs create mode 100644 src/dynamics/island_manager/utils.rs delete mode 100644 src/dynamics/island_manager2.rs diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs deleted file mode 100644 index d580ef589..000000000 --- a/src/dynamics/island_manager.rs +++ /dev/null @@ -1,357 +0,0 @@ -use crate::dynamics::{ - ImpulseJointSet, MultibodyJointSet, RigidBodyActivation, RigidBodyChanges, RigidBodyColliders, - RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, RigidBodyVelocity, -}; -use crate::geometry::{ColliderSet, NarrowPhase}; -use crate::math::Real; -use crate::utils::SimdDot; - -/// System that manages which bodies are active (awake) vs sleeping to optimize performance. -/// -/// ## Sleeping Optimization -/// -/// Bodies at rest automatically "sleep" - they're excluded from simulation until something -/// disturbs them (collision, joint connection to moving body, manual wake-up). This can -/// dramatically improve performance in scenes with many static/resting objects. -/// -/// ## Islands -/// -/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together. -/// This allows parallel solving and better organization. -/// -/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline). -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Default)] -pub struct IslandManager { - pub(crate) active_set: Vec, - pub(crate) active_islands: Vec, - pub(crate) active_islands_additional_solver_iterations: Vec, - active_set_timestamp: u32, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - can_sleep: Vec, // Workspace. - #[cfg_attr(feature = "serde-serialize", serde(skip))] - stack: Vec, // Workspace. -} - -impl IslandManager { - /// Creates a new empty island manager. - pub fn new() -> Self { - Self { - active_set: vec![], - active_islands: vec![], - active_islands_additional_solver_iterations: vec![], - active_set_timestamp: 0, - can_sleep: vec![], - stack: vec![], - } - } - - pub(crate) fn num_islands(&self) -> usize { - self.active_islands.len().saturating_sub(1) - } - - /// Update this data-structure after one or multiple rigid-bodies have been removed for `bodies`. - pub fn cleanup_removed_rigid_bodies(&mut self, bodies: &mut RigidBodySet) { - let mut i = 0; - - while i < self.active_set.len() { - let handle = self.active_set[i]; - if bodies.get(handle).is_none() { - // This rigid-body no longer exists, so we need to remove it from the active set. - self.active_set.swap_remove(i); - - if i < self.active_set.len() { - // Update the self.active_set_id for the body that has been swapped. - if let Some(swapped_rb) = bodies.get_mut_internal(self.active_set[i]) { - swapped_rb.ids.active_set_id = i; - } - } - } else { - i += 1; - } - } - } - - pub(crate) fn rigid_body_removed( - &mut self, - removed_handle: RigidBodyHandle, - removed_ids: &RigidBodyIds, - bodies: &mut RigidBodySet, - ) { - if self.active_set.get(removed_ids.active_set_id) == Some(&removed_handle) { - self.active_set.swap_remove(removed_ids.active_set_id); - - if let Some(replacement) = self - .active_set - .get(removed_ids.active_set_id) - .and_then(|h| bodies.get_mut_internal(*h)) - { - replacement.ids.active_set_id = removed_ids.active_set_id; - } - } - } - - /// Wakes up a sleeping body, forcing it back into the active simulation. - /// - /// Use this when you want to ensure a body is active (useful after manually moving - /// a sleeping body, or to prevent it from sleeping in the next few frames). - /// - /// # Parameters - /// * `strong` - If `true`, the body is guaranteed to stay awake for multiple frames. - /// If `false`, it might sleep again immediately if conditions are met. - /// - /// # Example - /// ``` - /// # use rapier3d::prelude::*; - /// # let mut bodies = RigidBodySet::new(); - /// # let mut islands = IslandManager::new(); - /// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic()); - /// islands.wake_up(&mut bodies, body_handle, true); - /// let body = bodies.get_mut(body_handle).unwrap(); - /// // Wake up a body before applying force to it - /// body.add_force(vector![100.0, 0.0, 0.0], false); - /// ``` - /// - /// Only affects dynamic bodies (kinematic and fixed bodies don't sleep). - pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) { - // NOTE: the use an Option here because there are many legitimate cases (like when - // deleting a joint attached to an already-removed body) where we could be - // attempting to wake-up a rigid-body that has already been deleted. - if bodies.get(handle).map(|rb| rb.body_type()) == Some(RigidBodyType::Dynamic) { - let rb = bodies.index_mut_internal(handle); - - // Check that the user didn’t change the sleeping state explicitly, in which - // case we don’t overwrite it. - if !rb.changes.contains(RigidBodyChanges::SLEEP) { - rb.activation.wake_up(strong); - - if rb.is_enabled() && self.active_set.get(rb.ids.active_set_id) != Some(&handle) { - rb.ids.active_set_id = self.active_set.len(); - self.active_set.push(handle); - } - } - } - } - - pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { - let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - &self.active_set[island_range] - } - - pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize { - self.active_islands_additional_solver_iterations[island_id] - } - - /// Handls of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping). - #[inline] - pub fn active_bodies(&self) -> &[RigidBodyHandle] { - &self.active_set - } - - #[cfg(feature = "parallel")] - #[allow(dead_code)] // That will likely be useful when we re-introduce intra-island parallelism. - pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range { - self.active_islands[island_id]..self.active_islands[island_id + 1] - } - - pub(crate) fn update_active_set_with_contacts( - &mut self, - dt: Real, - length_unit: Real, - bodies: &mut RigidBodySet, - colliders: &ColliderSet, - narrow_phase: &NarrowPhase, - impulse_joints: &ImpulseJointSet, - multibody_joints: &MultibodyJointSet, - min_island_size: usize, - ) { - assert!( - min_island_size > 0, - "The minimum island size must be at least 1." - ); - - // Update the energy of every rigid body and - // keep only those that may not sleep. - // let t = Instant::now(); - self.active_set_timestamp += 1; - self.stack.clear(); - self.can_sleep.clear(); - - // NOTE: the `.rev()` is here so that two successive timesteps preserve - // the order of the bodies in the `active_set` vec. This reversal - // does not seem to affect performances nor stability. However it makes - // debugging slightly nicer. - for h in self.active_set.drain(..).rev() { - let can_sleep = &mut self.can_sleep; - let stack = &mut self.stack; - - let rb = bodies.index_mut_internal(h); - let sq_linvel = rb.vels.linvel.norm_squared(); - let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); - - update_energy( - &mut rb.activation, - rb.body_type, - length_unit, - sq_linvel, - sq_angvel, - dt, - ); - - if rb.activation.time_since_can_sleep >= rb.activation.time_until_sleep { - // Mark them as sleeping for now. This will - // be set to false during the graph traversal - // if it should not be put to sleep. - rb.activation.sleeping = true; - can_sleep.push(h); - } else { - stack.push(h); - } - } - - // Read all the contacts and push objects touching touching this rigid-body. - #[inline] - fn push_contacting_bodies( - rb_colliders: &RigidBodyColliders, - colliders: &ColliderSet, - narrow_phase: &NarrowPhase, - stack: &mut Vec, - ) { - for collider_handle in &rb_colliders.0 { - for inter in narrow_phase.contact_pairs_with(*collider_handle) { - for manifold in &inter.manifolds { - if !manifold.data.solver_contacts.is_empty() { - let other = crate::utils::select_other( - (inter.collider1, inter.collider2), - *collider_handle, - ); - if let Some(other_body) = colliders[other].parent { - stack.push(other_body.handle); - } - break; - } - } - } - } - } - - // println!("Selection: {}", Instant::now() - t); - - // let t = Instant::now(); - // Propagation of awake state and awake island computation through the - // traversal of the interaction graph. - self.active_islands_additional_solver_iterations.clear(); - self.active_islands.clear(); - self.active_islands.push(0); - - // saturating_sub(1) prevents underflow when the stack is empty. - let mut island_marker = self.stack.len().saturating_sub(1); - - // NOTE: islands containing a body with non-standard number of iterations won’t - // be merged with another island, unless another island with standard - // iterations number already started before and got continued due to the - // `min_island_size`. That could be avoided by pushing bodies with non-standard - // iterations on top of the stack (and other bodies on the back). Not sure it’s - // worth it though. - let mut additional_solver_iterations = 0; - - while let Some(handle) = self.stack.pop() { - let rb = bodies.index_mut_internal(handle); - - if rb.ids.active_set_timestamp == self.active_set_timestamp - || !rb.is_dynamic_or_kinematic() - { - // We already visited this body and its neighbors. - // Also, we don't propagate awake state through fixed bodies. - continue; - } - - if self.stack.len() < island_marker { - if additional_solver_iterations != rb.additional_solver_iterations - || self.active_set.len() - *self.active_islands.last().unwrap() - >= min_island_size - { - // We are starting a new island. - self.active_islands_additional_solver_iterations - .push(additional_solver_iterations); - self.active_islands.push(self.active_set.len()); - additional_solver_iterations = 0; - } - - island_marker = self.stack.len(); - } - - additional_solver_iterations = - additional_solver_iterations.max(rb.additional_solver_iterations); - - // Transmit the active state to all the rigid-bodies with colliders - // in contact or joined with this collider. - push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); - - for inter in impulse_joints.attached_enabled_joints(handle) { - let other = crate::utils::select_other((inter.0, inter.1), handle); - self.stack.push(other); - } - - for other in multibody_joints.bodies_attached_with_enabled_joint(handle) { - self.stack.push(other); - } - - rb.activation.wake_up(false); - rb.ids.active_island_id = self.active_islands.len() - 1; - rb.ids.active_set_id = self.active_set.len(); - rb.ids.active_set_offset = - (rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id]) as u32; - rb.ids.active_set_timestamp = self.active_set_timestamp; - - self.active_set.push(handle); - } - - self.active_islands_additional_solver_iterations - .push(additional_solver_iterations); - self.active_islands.push(self.active_set.len()); - // println!( - // "Extraction: {}, num islands: {}", - // Instant::now() - t, - // self.active_islands.len() - 1 - // ); - - // Actually put to sleep bodies which have not been detected as awake. - for handle in &self.can_sleep { - let rb = bodies.index_mut_internal(*handle); - if rb.activation.sleeping { - rb.vels = RigidBodyVelocity::zero(); - rb.activation.sleep(); - } - } - } -} - -fn update_energy( - activation: &mut RigidBodyActivation, - body_type: RigidBodyType, - length_unit: Real, - sq_linvel: Real, - sq_angvel: Real, - dt: Real, -) { - let can_sleep = match body_type { - RigidBodyType::Dynamic => { - let linear_threshold = activation.normalized_linear_threshold * length_unit; - sq_linvel < linear_threshold * linear_threshold.abs() - && sq_angvel < activation.angular_threshold * activation.angular_threshold.abs() - } - RigidBodyType::KinematicPositionBased | RigidBodyType::KinematicVelocityBased => { - // Platforms only sleep if both velocities are exactly zero. If it’s not exactly - // zero, then the user really wants them to move. - sq_linvel == 0.0 && sq_angvel == 0.0 - } - RigidBodyType::Fixed => true, - }; - - if can_sleep { - activation.time_since_can_sleep += dt; - } else { - activation.time_since_can_sleep = 0.0; - } -} diff --git a/src/dynamics/island_manager/island.rs b/src/dynamics/island_manager/island.rs new file mode 100644 index 000000000..060a3c735 --- /dev/null +++ b/src/dynamics/island_manager/island.rs @@ -0,0 +1,157 @@ +use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; + +use super::IslandManager; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Default)] +pub(crate) struct Island { + /// The rigid-bodies part of this island. + pub(super) bodies: Vec, + /// The additional solver iterations needed by this island. + pub(super) additional_solver_iterations: usize, + /// Index of this island in `IslandManager::awake_islands`. + /// + /// If `None`, the island is sleeping. + pub(super) id_in_awake_list: Option, +} + +impl Island { + pub fn singleton(handle: RigidBodyHandle, rb: &RigidBody) -> Self { + Self { + bodies: vec![handle], + additional_solver_iterations: rb.additional_solver_iterations, + id_in_awake_list: None, + } + } + + pub fn bodies(&self) -> &[RigidBodyHandle] { + &self.bodies + } + + pub fn additional_solver_iterations(&self) -> usize { + self.additional_solver_iterations + } + + pub fn is_sleeping(&self) -> bool { + self.id_in_awake_list.is_none() + } + + pub fn len(&self) -> usize { + self.bodies.len() + } + + pub(crate) fn id_in_awake_list(&self) -> Option { + self.id_in_awake_list + } +} + +impl IslandManager { + /// Remove from the island at `source_id` all the rigid-body that are in `new_island`, and + /// insert `new_island` into the islands set. + /// + /// **All** rigid-bodies from `new_island` must currently be part of the island at `source_id`. + pub(super) fn extract_sub_island( + &mut self, + bodies: &mut RigidBodySet, + source_id: usize, + mut new_island: Island, + sleep: bool, + ) { + let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len()); + let active_island = &mut self.islands[source_id]; + + for (id, handle) in new_island.bodies.iter().enumerate() { + let rb = bodies.index_mut_internal(*handle); + + // If the new island is sleeping, ensure all its bodies are sleeping. + if sleep { + rb.sleep(); + } + + let id_to_remove = rb.ids.active_set_id; + assert_eq!(rb.ids.active_island_id, source_id); + rb.ids.active_island_id = new_island_id; + rb.ids.active_set_id = id; + rb.ids.active_set_offset = id as u32; + + new_island.additional_solver_iterations = new_island + .additional_solver_iterations + .max(rb.additional_solver_iterations); + active_island.bodies.swap_remove(id_to_remove); + + if let Some(moved_handle) = active_island.bodies.get(id_to_remove).copied() { + let moved_rb = bodies.index_mut_internal(moved_handle); + moved_rb.ids.active_set_id = id_to_remove; + moved_rb.ids.active_set_offset = id_to_remove as u32; + } + } + + // If the new island is awake, add it to the awake list. + if !sleep { + new_island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(new_island_id); + } + + self.islands.insert(new_island_id, new_island); + } + + pub(super) fn merge_islands( + &mut self, + bodies: &mut RigidBodySet, + island_id1: usize, + island_id2: usize, + ) { + if island_id1 == island_id2 { + return; + } + + let island1 = &self.islands[island_id1]; + let island2 = &self.islands[island_id2]; + + assert_eq!( + island1.id_in_awake_list.is_some(), + island2.id_in_awake_list.is_some(), + "Internal error: cannot merge two island with different sleeping statuses." + ); + + // Prefer removing the smallest island to reduce the amount of memory to move. + let (to_keep, to_remove) = if island1.bodies.len() < island2.bodies.len() { + (island_id2, island_id1) + } else { + (island_id1, island_id2) + }; + + let Some(removed_island) = self.islands.remove(to_remove) else { + // TODO: the island doesn’t exist is that an internal error? + return; + }; + + self.free_islands.push(to_remove); + + // TODO: if we switched to linked list, we could avoid moving around all this memory. + let target_island = &mut self.islands[to_keep]; + for handle in &removed_island.bodies { + let Some(rb) = bodies.get_mut_internal(*handle) else { + // This body no longer exists. + continue; + }; + rb.wake_up(false); + rb.ids.active_island_id = to_keep; + rb.ids.active_set_id = target_island.bodies.len(); + rb.ids.active_set_offset = (target_island.bodies.len()) as u32; + target_island.bodies.push(*handle); + target_island.additional_solver_iterations = target_island + .additional_solver_iterations + .max(rb.additional_solver_iterations); + } + + // Update the awake_islands list. + if let Some(awake_id_to_remove) = removed_island.id_in_awake_list { + self.awake_islands.swap_remove(awake_id_to_remove); + // Update the awake list index of the awake island id we moved. + if let Some(moved_id) = self.awake_islands.get(awake_id_to_remove) { + self.islands[*moved_id].id_in_awake_list = Some(awake_id_to_remove); + } + } + } +} diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs new file mode 100644 index 000000000..ddab1f069 --- /dev/null +++ b/src/dynamics/island_manager/manager.rs @@ -0,0 +1,297 @@ +use crate::dynamics::{ + ImpulseJointSet, MultibodyJointSet, RigidBodyChanges, RigidBodyHandle, RigidBodyIds, + RigidBodySet, +}; +use crate::geometry::{ColliderSet, NarrowPhase}; +use crate::math::Real; +use crate::utils::SimdDot; +use std::collections::VecDeque; +use vec_map::VecMap; + +use super::{Island, IslandsOptimizer}; + +/// An island starting at this rigid-body might be eligible for sleeping. +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct SleepCandidate(RigidBodyHandle); + +/// System that manages which bodies are active (awake) vs sleeping to optimize performance. +/// +/// ## Sleeping Optimization +/// +/// Bodies at rest automatically "sleep" - they're excluded from simulation until something +/// disturbs them (collision, joint connection to moving body, manual wake-up). This can +/// dramatically improve performance in scenes with many static/resting objects. +/// +/// ## Islands +/// +/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together. +/// This allows parallel solving and better organization. +/// +/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline). +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Clone, Default)] +pub struct IslandManager { + pub(crate) islands: VecMap, + pub(crate) awake_islands: Vec, + // TODO PERF: should this be `Vec<(usize, Island)>` to reuse the allocation? + pub(crate) free_islands: Vec, + /// Potential candidate roots for graph traversal to identify a sleeping + /// connected component or to split an island in two. + pub(super) traversal_candidates: VecDeque, + pub(super) timestamp: u32, // TODO: the physics pipeline (or something else) should expose a step_id? + pub(super) optimizer: IslandsOptimizer, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(super) stack: Vec, // Workspace. +} + +impl IslandManager { + /// Creates a new empty island manager. + pub fn new() -> Self { + Self::default() + } + + pub(crate) fn active_islands(&self) -> &[usize] { + &self.awake_islands + } + + pub(crate) fn rigid_body_removed( + &mut self, + removed_handle: RigidBodyHandle, + removed_ids: &RigidBodyIds, + bodies: &mut RigidBodySet, + ) { + let Some(island) = self.islands.get_mut(removed_ids.active_island_id) else { + // The island already doesn’t exist. + return; + }; + + let swapped_handle = island.bodies.last().copied().unwrap_or(removed_handle); + island + .bodies + .swap_remove(removed_ids.active_set_offset as usize); + + // Remap the active_set_id of the body we moved with the `swap_remove`. + if swapped_handle != removed_handle { + let swapped_body = bodies + .get_mut(swapped_handle) + .expect("Internal error: bodies must be removed from islands on at a times"); + swapped_body.ids.active_set_offset = removed_ids.active_set_offset; + swapped_body.ids.active_set_id = removed_ids.active_set_id; + } + + // If we deleted the last body from this island, delete the island. + if island.bodies.is_empty() { + if let Some(awake_id) = island.id_in_awake_list { + // Remove it from the free island list. + self.awake_islands.swap_remove(awake_id); + // Update the awake list index of the awake island id we moved. + if let Some(moved_id) = self.awake_islands.get(awake_id) { + self.islands[*moved_id].id_in_awake_list = Some(awake_id); + } + } + self.islands.remove(removed_ids.active_island_id); + self.free_islands.push(removed_ids.active_island_id); + } + } + + pub(crate) fn interaction_started_or_stopped( + &mut self, + bodies: &mut RigidBodySet, + handle1: Option, + handle2: Option, + started: bool, + wake_up: bool, + ) { + if started { + match (handle1, handle2) { + (Some(handle1), Some(handle2)) => { + if wake_up { + self.wake_up(bodies, handle1, false); + self.wake_up(bodies, handle2, false); + } + + if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) { + // If both bodies are not part of the same island, merge the islands. + if !rb1.is_fixed() + && !rb2.is_fixed() + && rb1.ids.active_island_id != rb2.ids.active_island_id + { + assert_ne!(rb1.ids.active_island_id, usize::MAX); + assert_ne!(rb2.ids.active_island_id, usize::MAX); + + self.merge_islands( + bodies, + rb1.ids.active_island_id, + rb2.ids.active_island_id, + ); + } + } + } + (Some(handle1), None) => { + if wake_up { + self.wake_up(bodies, handle1, false); + } + } + (None, Some(handle2)) => { + if wake_up { + self.wake_up(bodies, handle2, false); + } + } + (None, None) => { /* Nothing to do. */ } + } + } + } + + pub(crate) fn island(&self, island_id: usize) -> &Island { + &self.islands[island_id] + } + + /// Handles of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping). + #[inline] + pub fn active_bodies(&self) -> impl Iterator + '_ { + self.awake_islands + .iter() + .flat_map(|i| self.islands[*i].bodies.iter().copied()) + } + + pub(crate) fn rigid_body_updated( + &mut self, + handle: RigidBodyHandle, + bodies: &mut RigidBodySet, + ) { + let Some(rb) = bodies.get_mut(handle) else { + return; + }; + + if rb.is_fixed() { + return; + } + + // Check if this is the first time we see this rigid-body. + if rb.ids.active_island_id == usize::MAX { + // Check if there is room in the last awake island to add this body. + // NOTE: only checking the last is suboptimal. Perhaps we should keep vec of + // small islands ids? + let insert_in_last_island = self.awake_islands.last().map(|id| { + self.islands[*id].bodies.len() < self.optimizer.min_island_size + && self.islands[*id].is_sleeping() == rb.is_sleeping() + }); + // let insert_in_last_island = insert_in_last_island.is_some().then_some(true); + + if !rb.is_sleeping() && insert_in_last_island == Some(true) { + let id = *self.awake_islands.last().unwrap_or_else(|| unreachable!()); + let target_island = &mut self.islands[id]; + + rb.ids.active_island_id = id; + rb.ids.active_set_id = target_island.bodies.len(); + rb.ids.active_set_offset = target_island.bodies.len() as u32; + target_island.bodies.push(handle); + } else { + let mut new_island = Island::singleton(handle, rb); + let id = self.free_islands.pop().unwrap_or(self.islands.len()); + + if !rb.is_sleeping() { + new_island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(id); + } + + self.islands.insert(id, new_island); + rb.ids.active_island_id = id; + rb.ids.active_set_id = 0; + rb.ids.active_set_offset = 0; + } + } + + // Push the body to the active set if it is not inside the active set yet, and + // is not longer sleeping or became dynamic. + if (rb.changes.contains(RigidBodyChanges::SLEEP) + || rb.changes.contains(RigidBodyChanges::TYPE)) + && rb.is_enabled() + // Don’t wake up if the user put it to sleep manually. + && !rb.activation.sleeping + { + self.wake_up(bodies, handle, false); + } + } + + pub(crate) fn update_islands( + &mut self, + dt: Real, + length_unit: Real, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + min_island_size: usize, + ) { + self.timestamp += 1; + // 1. Update active rigid-bodies energy. + // TODO PERF: should this done by the velocity solver after solving the constraints? + // let t0 = std::time::Instant::now(); + for handle in self + .awake_islands + .iter() + .flat_map(|i| self.islands[*i].bodies.iter().copied()) + { + let Some(rb) = bodies.get_mut_internal(handle) else { + // This branch happens if the rigid-body no longer exists. + continue; + }; + let sq_linvel = rb.vels.linvel.norm_squared(); + let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); + let can_sleep_before = rb.activation.is_eligible_for_sleep(); + + rb.activation + .update_energy(rb.body_type, length_unit, sq_linvel, sq_angvel, dt); + + let can_sleep_now = rb.activation.is_eligible_for_sleep(); + + // 2. Identify active rigid-bodies that transition from "awake" to "can_sleep" + // and push the sleep root candidate if applicable. + if !can_sleep_before && can_sleep_now { + // This is a new candidate for island extraction. + if !rb.activation.is_sleep_root_candidate { + self.traversal_candidates.push_back(SleepCandidate(handle)); + } + } + } + // println!("Update energy: {}", t0.elapsed().as_secs_f32() * 1000.0); + + let frame_base_timestamp = self.timestamp; + let mut cost = 0; + + // 3. Perform one, or multiple, sleeping islands extraction (graph traversal). + // Limit the traversal cost by not traversing all the known sleeping roots if + // there are too many. + const MAX_PER_FRAME_COST: usize = 1000; // TODO: find the best value. + while let Some(sleep_root) = self.traversal_candidates.pop_front() { + if cost > MAX_PER_FRAME_COST { + // Early-break if we consider we have done enough island extraction work. + break; + } + + cost += self.extract_sleeping_island( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + sleep_root.0, + frame_base_timestamp, + ); + + self.timestamp += 1; + } + + self.update_optimizer( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + ); + // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0); + } +} diff --git a/src/dynamics/island_manager/mod.rs b/src/dynamics/island_manager/mod.rs new file mode 100644 index 000000000..8a54427c2 --- /dev/null +++ b/src/dynamics/island_manager/mod.rs @@ -0,0 +1,10 @@ +pub use manager::IslandManager; + +pub(crate) use island::Island; +pub(self) use optimizer::IslandsOptimizer; + +mod island; +mod manager; +mod optimizer; +mod sleep; +mod utils; diff --git a/src/dynamics/island_manager/optimizer.rs b/src/dynamics/island_manager/optimizer.rs new file mode 100644 index 000000000..c7bbb93fa --- /dev/null +++ b/src/dynamics/island_manager/optimizer.rs @@ -0,0 +1,226 @@ +use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodySet}; +use crate::geometry::{ColliderSet, NarrowPhase}; +use std::ops::IndexMut; + +use super::{Island, IslandManager}; + +#[derive(Copy, Clone, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(super) struct IslandsOptimizerMergeState { + curr_awake_id: usize, +} + +#[derive(Copy, Clone, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(super) struct IslandsOptimizerSplitState { + curr_awake_id: usize, + curr_body_id: usize, +} + +/// Configuration of the awake islands optimization strategy. +/// +/// Note that this currently only affects active islands. Sleeping islands are always kept minimal. +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(super) struct IslandsOptimizer { + /// The optimizer will try to merge small islands so their size exceed this minimum. + /// + /// Note that it will never merge incompatible islands (currently define as islands with + /// different additional solver iteration counts). + pub(super) min_island_size: usize, + /// The optimizer will try to split large islands so their size do not exceed this maximum. + /// + /// IMPORTANT: Must be greater than `2 * min_island_size` to avoid conflict between the splits + /// and merges. + pub(super) max_island_size: usize, + /// Indicates if the optimizer is in split or merge mode. Swaps between modes every step. + pub(super) mode: usize, + pub(super) merge_state: IslandsOptimizerMergeState, + pub(super) split_state: IslandsOptimizerSplitState, +} + +impl Default for IslandsOptimizer { + fn default() -> Self { + // TODO: figure out the best values. + Self { + min_island_size: 1024, + max_island_size: 4096, + mode: 0, + merge_state: Default::default(), + split_state: Default::default(), + } + } +} + +impl IslandManager { + pub(super) fn update_optimizer( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + ) { + if self.optimizer.mode % 2 == 0 { + self.incremental_merge(bodies); + } else { + self.incremental_split( + bodies, + colliders, + impulse_joints, + multibody_joints, + narrow_phase, + ); + } + + self.optimizer.mode = self.optimizer.mode.wrapping_add(1); + } + + /// Attempts to merge awake islands that are too small. + fn incremental_merge(&mut self, bodies: &mut RigidBodySet) { + struct IslandData { + id: usize, + awake_id: usize, + solver_iters: usize, + } + + // Ensure the awake id is still in bounds. + if self.optimizer.merge_state.curr_awake_id >= self.awake_islands.len() { + self.optimizer.merge_state.curr_awake_id = 0; + } + + // Find a first candidate for a merge. + let mut island1 = None; + for awake_id in self.optimizer.merge_state.curr_awake_id..self.awake_islands.len() { + let id = self.awake_islands[awake_id]; + let island = &self.islands[id]; + if island.len() < self.optimizer.min_island_size { + island1 = Some(IslandData { + awake_id, + id, + solver_iters: island.additional_solver_iterations, + }); + break; + } + } + + if let Some(island1) = island1 { + // Indicates if we found a merge candidate for the next incremental update. + let mut found_next = false; + self.optimizer.merge_state.curr_awake_id = island1.awake_id + 1; + + // Find a second candidate for a merge. + for awake_id2 in island1.awake_id + 1..self.awake_islands.len() { + let id2 = self.awake_islands[awake_id2]; + let island2 = &self.islands[id2]; + + if island1.solver_iters == island2.additional_solver_iterations + && island2.len() < self.optimizer.min_island_size + { + // Found a second candidate! Merge them. + self.merge_islands(bodies, island1.id, id2); + println!("Found a merge {}!", self.awake_islands.len()); + + // TODO: support doing more than a single merge per frame. + return; + } else if island2.len() < self.optimizer.min_island_size && !found_next { + // We found a good candidate for the next incremental merge (we can’t just + // merge it now because it’s not compatible with the current island). + self.optimizer.merge_state.curr_awake_id = awake_id2; + found_next = true; + } + } + } else { + self.optimizer.merge_state.curr_awake_id = 0; + } + } + + /// Attempts to split awake islands that are too big. + fn incremental_split( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + ) { + if self.optimizer.split_state.curr_awake_id >= self.awake_islands.len() { + self.optimizer.split_state.curr_awake_id = 0; + } + + for awake_id in self.optimizer.split_state.curr_awake_id..self.awake_islands.len() { + let id = self.awake_islands[awake_id]; + if self.islands[id].len() > self.optimizer.max_island_size { + // Try to split this island. + // Note that the traversal logic is similar to the sleeping island + // extraction, except that we have different stopping criteria. + self.stack.clear(); + + // TODO: implement islands recycling to avoid reallocating every time. + let mut new_island = Island::default(); + self.timestamp += 1; + + for root in &self.islands[id].bodies { + self.stack.push(*root); + + let len_before_traversal = new_island.len(); + while let Some(handle) = self.stack.pop() { + let rb = bodies.index_mut(handle); + if rb.is_fixed() { + // Don’t propagate islands through rigid-bodies. + continue; + } + + if rb.ids.active_set_timestamp == self.timestamp { + // We already visited this body. + continue; + } + + rb.ids.active_set_timestamp = self.timestamp; + assert!(!rb.activation.sleeping); + + // Traverse bodies that are interacting with the current one either through + // contacts or a joint. + super::utils::push_contacting_bodies( + &rb.colliders, + colliders, + narrow_phase, + &mut self.stack, + ); + super::utils::push_linked_bodies( + impulse_joints, + multibody_joints, + handle, + &mut self.stack, + ); + new_island.bodies.push(handle); + + // Our new island cannot grow any further. + if new_island.bodies.len() > self.optimizer.max_island_size { + new_island.bodies.truncate(len_before_traversal); + self.stack.clear(); + break; + } + } + + // Extract this island. + if new_island.len() == 0 { + // println!("Failed to split island."); + return; + } else if new_island.bodies.len() >= self.optimizer.min_island_size { + // println!( + // "Split an island: {}/{} ({} islands)", + // new_island.len(), + // self.islands[id].len(), + // self.awake_islands.len(), + // ); + self.extract_sub_island(bodies, id, new_island, false); + return; // TODO: support extracting more than one island per frame. + } + } + } else { + self.optimizer.split_state.curr_awake_id = awake_id + 1; + } + } + } +} diff --git a/src/dynamics/island_manager/sleep.rs b/src/dynamics/island_manager/sleep.rs new file mode 100644 index 000000000..565050d2f --- /dev/null +++ b/src/dynamics/island_manager/sleep.rs @@ -0,0 +1,185 @@ +use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderSet, NarrowPhase}; + +use super::{Island, IslandManager}; + +impl IslandManager { + /// Wakes up a sleeping body, forcing it back into the active simulation. + /// + /// Use this when you want to ensure a body is active (useful after manually moving + /// a sleeping body, or to prevent it from sleeping in the next few frames). + /// + /// # Parameters + /// * `strong` - If `true`, the body is guaranteed to stay awake for multiple frames. + /// If `false`, it might sleep again immediately if conditions are met. + /// + /// # Example + /// ``` + /// # use rapier3d::prelude::*; + /// # let mut bodies = RigidBodySet::new(); + /// # let mut islands = IslandManager::new(); + /// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic()); + /// islands.wake_up(&mut bodies, body_handle, true); + /// let body = bodies.get_mut(body_handle).unwrap(); + /// // Wake up a body before applying force to it + /// body.add_force(vector![100.0, 0.0, 0.0], false); + /// ``` + /// + /// Only affects dynamic bodies (kinematic and fixed bodies don't sleep). + pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) { + // NOTE: the use an Option here because there are many legitimate cases (like when + // deleting a joint attached to an already-removed body) where we could be + // attempting to wake-up a rigid-body that has already been deleted. + if bodies.get(handle).map(|rb| !rb.is_fixed()) == Some(true) { + let rb = bodies.index_mut_internal(handle); + + // TODO: not sure if this is still relevant: + // // Check that the user didn’t change the sleeping state explicitly, in which + // // case we don’t overwrite it. + // if rb.changes.contains(RigidBodyChanges::SLEEP) { + // return; + // } + + rb.activation.wake_up(strong); + let island_to_wake_up = rb.ids.active_island_id; + self.wake_up_island(bodies, island_to_wake_up); + } + } + + /// Returns the number of iterations run by the graph traversal so we can balance load across + /// frames. + pub(super) fn extract_sleeping_island( + &mut self, + bodies: &mut RigidBodySet, + colliders: &ColliderSet, + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + narrow_phase: &NarrowPhase, + sleep_root: RigidBodyHandle, + frame_base_timestamp: u32, + ) -> usize { + let Some(rb) = bodies.get_mut_internal(sleep_root) else { + // This branch happens if the rigid-body no longer exists. + return 0; + }; + + rb.activation.is_sleep_root_candidate = false; + + if rb.ids.active_set_timestamp >= frame_base_timestamp { + return 0; // We already traversed this rigid-body this frame. + } + + let active_island_id = rb.ids.active_island_id; + let active_island = &mut self.islands[active_island_id]; + if active_island.is_sleeping() { + // This rigid-body is already part of a sleeping island. + return 0; + } + + // TODO: implement recycling islands to avoid repeated allocations? + let mut new_island = Island::default(); + self.stack.push(sleep_root); + + let mut can_sleep = true; + let mut niter = 0; + while let Some(handle) = self.stack.pop() { + let rb = bodies.index_mut_internal(handle); + + if rb.is_fixed() { + // Don’t propagate islands through fixed bodies. + continue; + } + + if rb.ids.active_set_timestamp == self.timestamp { + // We already visited this body and its neighbors. + continue; + } + + if rb.ids.active_set_timestamp >= frame_base_timestamp { + // We already visited this body and its neighbors during this frame. + // So we already know this islands cannot sleep (otherwise the bodies + // currently being traversed would already have been marked as sleeping). + return niter; + } + + niter += 1; + rb.ids.active_set_timestamp = self.timestamp; + + assert!(!rb.activation.sleeping); + + // TODO PERF: early-exit as soon as we reach a body not eligible to sleep. + can_sleep = can_sleep && rb.activation().is_eligible_for_sleep(); + if !rb.activation.is_eligible_for_sleep() { + // If this body cannot sleep, abort the traversal, we are not traversing + // yet an island that can sleep. + self.stack.clear(); + return niter; + } + + // Traverse bodies that are interacting with the current one either through + // contacts or a joint. + super::utils::push_contacting_bodies( + &rb.colliders, + colliders, + narrow_phase, + &mut self.stack, + ); + super::utils::push_linked_bodies( + impulse_joints, + multibody_joints, + handle, + &mut self.stack, + ); + new_island.bodies.push(handle); + } + + assert!(can_sleep); + + // If we reached this line, we completed a sleeping island traversal. + // - Put its bodies to sleep. + // - Remove them from the active set. + // - Push the sleeping island. + if active_island.len() == new_island.len() { + // The whole island is asleep. No need to insert a new one. + // Put all its bodies to sleep. + for handle in &active_island.bodies { + let rb = bodies.index_mut_internal(*handle); + rb.sleep(); + } + + // Mark the existing island as sleeping (by clearing its `id_in_awake_list`) + // and remove it from the awake list. + let island_awake_id = active_island + .id_in_awake_list + .take() + .unwrap_or_else(|| unreachable!()); + self.awake_islands.swap_remove(island_awake_id); + + if let Some(moved_id) = self.awake_islands.get(island_awake_id) { + self.islands[*moved_id].id_in_awake_list = Some(island_awake_id); + } + } else { + niter += new_island.len(); // Include this part into the cost estimate for this function. + self.extract_sub_island(bodies, active_island_id, new_island, true); + } + niter + } + + fn wake_up_island(&mut self, bodies: &mut RigidBodySet, island_id: usize) { + let Some(island) = self.islands.get_mut(island_id) else { + return; + }; + + if island.is_sleeping() { + island.id_in_awake_list = Some(self.awake_islands.len()); + self.awake_islands.push(island_id); + + // Wake up all the bodies from this island. + for handle in &island.bodies { + if let Some(rb) = bodies.get_mut(*handle) { + rb.wake_up(false); + } + } + } + } +} diff --git a/src/dynamics/island_manager/utils.rs b/src/dynamics/island_manager/utils.rs new file mode 100644 index 000000000..c596df8b4 --- /dev/null +++ b/src/dynamics/island_manager/utils.rs @@ -0,0 +1,44 @@ +use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodyColliders, RigidBodyHandle}; +use crate::geometry::{ColliderSet, NarrowPhase}; + +// Read all the contacts and push objects touching this rigid-body. +#[inline] +pub(super) fn push_contacting_bodies( + rb_colliders: &RigidBodyColliders, + colliders: &ColliderSet, + narrow_phase: &NarrowPhase, + stack: &mut Vec, +) { + for collider_handle in &rb_colliders.0 { + for inter in narrow_phase.contact_pairs_with(*collider_handle) { + for manifold in &inter.manifolds { + if !manifold.data.solver_contacts.is_empty() { + let other = crate::utils::select_other( + (inter.collider1, inter.collider2), + *collider_handle, + ); + if let Some(other_body) = colliders[other].parent { + stack.push(other_body.handle); + } + break; + } + } + } + } +} + +pub(super) fn push_linked_bodies( + impulse_joints: &ImpulseJointSet, + multibody_joints: &MultibodyJointSet, + handle: RigidBodyHandle, + stack: &mut Vec, +) { + for inter in impulse_joints.attached_enabled_joints(handle) { + let other = crate::utils::select_other((inter.0, inter.1), handle); + stack.push(other); + } + + for other in multibody_joints.bodies_attached_with_enabled_joint(handle) { + stack.push(other); + } +} diff --git a/src/dynamics/island_manager2.rs b/src/dynamics/island_manager2.rs deleted file mode 100644 index 3362d7b06..000000000 --- a/src/dynamics/island_manager2.rs +++ /dev/null @@ -1,871 +0,0 @@ -use crate::dynamics::{ - ImpulseJointSet, MultibodyJointSet, RigidBody, RigidBodyActivation, RigidBodyChanges, - RigidBodyColliders, RigidBodyHandle, RigidBodyIds, RigidBodySet, RigidBodyType, - RigidBodyVelocity, -}; -use crate::geometry::{ColliderSet, ContactPair, NarrowPhase}; -use crate::math::Real; -use crate::prelude::{ColliderHandle, ContactManifoldData}; -use crate::utils::SimdDot; -use std::collections::VecDeque; -use std::ops::{Index, IndexMut}; -use vec_map::VecMap; - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Default)] -pub(crate) struct Island { - /// The rigid-bodies part of this island. - bodies: Vec, - /// The additional solver iterations needed by this island. - additional_solver_iterations: usize, - /// Index of this island in `IslandManager::awake_islands`. - /// - /// If `None`, the island is sleeping. - id_in_awake_list: Option, -} - -impl Island { - pub fn singleton(handle: RigidBodyHandle, rb: &RigidBody) -> Self { - Self { - bodies: vec![handle], - additional_solver_iterations: rb.additional_solver_iterations, - id_in_awake_list: None, - } - } - - pub fn bodies(&self) -> &[RigidBodyHandle] { - &self.bodies - } - - pub fn additional_solver_iterations(&self) -> usize { - self.additional_solver_iterations - } - - pub fn is_sleeping(&self) -> bool { - self.id_in_awake_list.is_none() - } - - pub fn len(&self) -> usize { - self.bodies.len() - } - - pub(crate) fn id_in_awake_list(&self) -> Option { - self.id_in_awake_list - } -} - -#[derive(Copy, Clone, Default)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct IslandsOptimizerMergeState { - curr_awake_id: usize, -} - -#[derive(Copy, Clone, Default)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct IslandsOptimizerSplitState { - curr_awake_id: usize, - curr_body_id: usize, -} - -/// Configuration of the awake islands optimization strategy. -/// -/// Note that this currently only affects active islands. Sleeping islands are always kept minimal. -#[derive(Copy, Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct IslandsOptimizer { - /// The optimizer will try to merge small islands so their size exceed this minimum. - /// - /// Note that it will never merge incompatible islands (currently define as islands with - /// different additional solver iteration counts). - min_island_size: usize, - /// The optimizer will try to split large islands so their size do not exceed this maximum. - /// - /// IMPORTANT: Must be greater than `2 * min_island_size` to avoid conflict between the splits - /// and merges. - max_island_size: usize, - /// Indicates if the optimizer is in split or merge mode. Swaps between modes every step. - mode: usize, - merge_state: IslandsOptimizerMergeState, - split_state: IslandsOptimizerSplitState, -} - -impl Default for IslandsOptimizer { - fn default() -> Self { - // TODO: figure out the best values. - Self { - min_island_size: 1024, - max_island_size: 4096, - mode: 0, - merge_state: Default::default(), - split_state: Default::default(), - } - } -} - -/// An island starting at this rigid-body might be eligible for sleeping. -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct SleepCandidate(RigidBodyHandle); - -/// System that manages which bodies are active (awake) vs sleeping to optimize performance. -/// -/// ## Sleeping Optimization -/// -/// Bodies at rest automatically "sleep" - they're excluded from simulation until something -/// disturbs them (collision, joint connection to moving body, manual wake-up). This can -/// dramatically improve performance in scenes with many static/resting objects. -/// -/// ## Islands -/// -/// Connected bodies (via contacts or joints) are grouped into "islands" that are solved together. -/// This allows parallel solving and better organization. -/// -/// You rarely interact with this directly - it's automatically managed by [`PhysicsPipeline`](crate::pipeline::PhysicsPipeline). -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Default)] -pub struct IslandManager { - pub(crate) islands: VecMap, - pub(crate) awake_islands: Vec, - // TODO PERF: should this be `Vec<(usize, Island)>` to reuse the allocation? - pub(crate) free_islands: Vec, - /// Potential candidate roots for graph traversal to identify a sleeping - /// connected component or to split an island in two. - traversal_candidates: VecDeque, - timestamp: u32, // TODO: the physics pipeline (or something else) should expose a step_id? - optimizer: IslandsOptimizer, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - stack: Vec, // Workspace. -} - -impl IslandManager { - /// Creates a new empty island manager. - pub fn new() -> Self { - Self::default() - } - - pub(crate) fn active_islands(&self) -> &[usize] { - &self.awake_islands - } - - pub(crate) fn rigid_body_removed( - &mut self, - removed_handle: RigidBodyHandle, - removed_ids: &RigidBodyIds, - bodies: &mut RigidBodySet, - ) { - let Some(island) = self.islands.get_mut(removed_ids.active_island_id) else { - // The island already doesn’t exist. - return; - }; - - let swapped_handle = island.bodies.last().copied().unwrap_or(removed_handle); - island - .bodies - .swap_remove(removed_ids.active_set_offset as usize); - - // Remap the active_set_id of the body we moved with the `swap_remove`. - if swapped_handle != removed_handle { - let swapped_body = bodies - .get_mut(swapped_handle) - .expect("Internal error: bodies must be removed from islands on at a times"); - swapped_body.ids.active_set_offset = removed_ids.active_set_offset; - swapped_body.ids.active_set_id = removed_ids.active_set_id; - } - - // If we deleted the last body from this island, delete the island. - if island.bodies.is_empty() { - if let Some(awake_id) = island.id_in_awake_list { - // Remove it from the free island list. - self.awake_islands.swap_remove(awake_id); - // Update the awake list index of the awake island id we moved. - if let Some(moved_id) = self.awake_islands.get(awake_id) { - self.islands[*moved_id].id_in_awake_list = Some(awake_id); - } - } - self.islands.remove(removed_ids.active_island_id); - self.free_islands.push(removed_ids.active_island_id); - } - } - - pub(crate) fn interaction_started_or_stopped( - &mut self, - bodies: &mut RigidBodySet, - handle1: Option, - handle2: Option, - started: bool, - wake_up: bool, - ) { - if started { - match (handle1, handle2) { - (Some(handle1), Some(handle2)) => { - if wake_up { - self.wake_up(bodies, handle1, false); - self.wake_up(bodies, handle2, false); - } - - if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) { - // If both bodies are not part of the same island, merge the islands. - if !rb1.is_fixed() - && !rb2.is_fixed() - && rb1.ids.active_island_id != rb2.ids.active_island_id - { - assert_ne!(rb1.ids.active_island_id, usize::MAX); - assert_ne!(rb2.ids.active_island_id, usize::MAX); - - self.merge_islands( - bodies, - rb1.ids.active_island_id, - rb2.ids.active_island_id, - ); - } - } - } - (Some(handle1), None) => { - if wake_up { - self.wake_up(bodies, handle1, false); - } - } - (None, Some(handle2)) => { - if wake_up { - self.wake_up(bodies, handle2, false); - } - } - (None, None) => { /* Nothing to do. */ } - } - } - } - - /// Wakes up a sleeping body, forcing it back into the active simulation. - /// - /// Use this when you want to ensure a body is active (useful after manually moving - /// a sleeping body, or to prevent it from sleeping in the next few frames). - /// - /// # Parameters - /// * `strong` - If `true`, the body is guaranteed to stay awake for multiple frames. - /// If `false`, it might sleep again immediately if conditions are met. - /// - /// # Example - /// ``` - /// # use rapier3d::prelude::*; - /// # let mut bodies = RigidBodySet::new(); - /// # let mut islands = IslandManager::new(); - /// # let body_handle = bodies.insert(RigidBodyBuilder::dynamic()); - /// islands.wake_up(&mut bodies, body_handle, true); - /// let body = bodies.get_mut(body_handle).unwrap(); - /// // Wake up a body before applying force to it - /// body.add_force(vector![100.0, 0.0, 0.0], false); - /// ``` - /// - /// Only affects dynamic bodies (kinematic and fixed bodies don't sleep). - pub fn wake_up(&mut self, bodies: &mut RigidBodySet, handle: RigidBodyHandle, strong: bool) { - // NOTE: the use an Option here because there are many legitimate cases (like when - // deleting a joint attached to an already-removed body) where we could be - // attempting to wake-up a rigid-body that has already been deleted. - if bodies.get(handle).map(|rb| !rb.is_fixed()) == Some(true) { - let rb = bodies.index_mut_internal(handle); - - // TODO: not sure if this is still relevant: - // // Check that the user didn’t change the sleeping state explicitly, in which - // // case we don’t overwrite it. - // if rb.changes.contains(RigidBodyChanges::SLEEP) { - // return; - // } - - rb.activation.wake_up(strong); - let island_to_wake_up = rb.ids.active_island_id; - self.wake_up_island(bodies, island_to_wake_up); - } - } - - fn wake_up_island(&mut self, bodies: &mut RigidBodySet, island_id: usize) { - let Some(island) = self.islands.get_mut(island_id) else { - return; - }; - - if island.is_sleeping() { - island.id_in_awake_list = Some(self.awake_islands.len()); - self.awake_islands.push(island_id); - - // Wake up all the bodies from this island. - for handle in &island.bodies { - if let Some(rb) = bodies.get_mut(*handle) { - rb.wake_up(false); - } - } - } - } - - fn merge_islands(&mut self, bodies: &mut RigidBodySet, island_id1: usize, island_id2: usize) { - if island_id1 == island_id2 { - return; - } - - let island1 = &self.islands[island_id1]; - let island2 = &self.islands[island_id2]; - - assert_eq!( - island1.id_in_awake_list.is_some(), - island2.id_in_awake_list.is_some(), - "Internal error: cannot merge two island with different sleeping statuses." - ); - - // Prefer removing the smallest island to reduce the amount of memory to move. - let (to_keep, to_remove) = if island1.bodies.len() < island2.bodies.len() { - (island_id2, island_id1) - } else { - (island_id1, island_id2) - }; - - let Some(removed_island) = self.islands.remove(to_remove) else { - // TODO: the island doesn’t exist is that an internal error? - return; - }; - - self.free_islands.push(to_remove); - - // TODO: if we switched to linked list, we could avoid moving around all this memory. - let target_island = &mut self.islands[to_keep]; - for handle in &removed_island.bodies { - let Some(rb) = bodies.get_mut_internal(*handle) else { - // This body no longer exists. - continue; - }; - rb.wake_up(false); - rb.ids.active_island_id = to_keep; - rb.ids.active_set_id = target_island.bodies.len(); - rb.ids.active_set_offset = (target_island.bodies.len()) as u32; - target_island.bodies.push(*handle); - target_island.additional_solver_iterations = target_island - .additional_solver_iterations - .max(rb.additional_solver_iterations); - } - - // Update the awake_islands list. - if let Some(awake_id_to_remove) = removed_island.id_in_awake_list { - self.awake_islands.swap_remove(awake_id_to_remove); - // Update the awake list index of the awake island id we moved. - if let Some(moved_id) = self.awake_islands.get(awake_id_to_remove) { - self.islands[*moved_id].id_in_awake_list = Some(awake_id_to_remove); - } - } - } - - pub(crate) fn island(&self, island_id: usize) -> &Island { - &self.islands[island_id] - } - - /// Handles of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping). - #[inline] - pub fn active_bodies(&self) -> impl Iterator + '_ { - self.awake_islands - .iter() - .flat_map(|i| self.islands[*i].bodies.iter().copied()) - } - - pub(crate) fn update_body(&mut self, handle: RigidBodyHandle, bodies: &mut RigidBodySet) { - let Some(rb) = bodies.get_mut(handle) else { - return; - }; - - if rb.is_fixed() { - return; - } - - // Check if this is the first time we see this rigid-body. - if rb.ids.active_island_id == usize::MAX { - // Check if there is room in the last awake island to add this body. - // NOTE: only checking the last is suboptimal. Perhaps we should keep vec of - // small islands ids? - let insert_in_last_island = self.awake_islands.last().map(|id| { - self.islands[*id].bodies.len() < self.optimizer.min_island_size - && self.islands[*id].is_sleeping() == rb.is_sleeping() - }); - // let insert_in_last_island = insert_in_last_island.is_some().then_some(true); - - if !rb.is_sleeping() && insert_in_last_island == Some(true) { - let id = *self.awake_islands.last().unwrap_or_else(|| unreachable!()); - let target_island = &mut self.islands[id]; - - rb.ids.active_island_id = id; - rb.ids.active_set_id = target_island.bodies.len(); - rb.ids.active_set_offset = target_island.bodies.len() as u32; - target_island.bodies.push(handle); - } else { - let mut new_island = Island::singleton(handle, rb); - let id = self.free_islands.pop().unwrap_or(self.islands.len()); - - if !rb.is_sleeping() { - new_island.id_in_awake_list = Some(self.awake_islands.len()); - self.awake_islands.push(id); - } - - self.islands.insert(id, new_island); - rb.ids.active_island_id = id; - rb.ids.active_set_id = 0; - rb.ids.active_set_offset = 0; - } - } - - // Push the body to the active set if it is not inside the active set yet, and - // is not longer sleeping or became dynamic. - if (rb.changes.contains(RigidBodyChanges::SLEEP) - || rb.changes.contains(RigidBodyChanges::TYPE)) - && rb.is_enabled() - // Don’t wake up if the user put it to sleep manually. - && !rb.activation.sleeping - { - self.wake_up(bodies, handle, false); - } - } - - pub(crate) fn update_active_set_with_contacts( - &mut self, - dt: Real, - length_unit: Real, - bodies: &mut RigidBodySet, - colliders: &ColliderSet, - narrow_phase: &NarrowPhase, - impulse_joints: &ImpulseJointSet, - multibody_joints: &MultibodyJointSet, - min_island_size: usize, - ) { - self.timestamp += 1; - // 1. Update active rigid-bodies energy. - // TODO PERF: should this done by the velocity solver after solving the constraints? - // let t0 = std::time::Instant::now(); - for handle in self - .awake_islands - .iter() - .flat_map(|i| self.islands[*i].bodies.iter().copied()) - { - let Some(rb) = bodies.get_mut_internal(handle) else { - // This branch happens if the rigid-body no longer exists. - continue; - }; - let sq_linvel = rb.vels.linvel.norm_squared(); - let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); - let can_sleep_before = rb.activation.is_eligible_for_sleep(); - - rb.activation - .update_energy(rb.body_type, length_unit, sq_linvel, sq_angvel, dt); - - let can_sleep_now = rb.activation.is_eligible_for_sleep(); - - // 2. Identify active rigid-bodies that transition from "awake" to "can_sleep" - // and push the sleep root candidate if applicable. - if !can_sleep_before && can_sleep_now { - // This is a new candidate for island extraction. - if !rb.activation.is_sleep_root_candidate { - self.traversal_candidates.push_back(SleepCandidate(handle)); - } - } - } - // println!("Update energy: {}", t0.elapsed().as_secs_f32() * 1000.0); - - let frame_base_timestamp = self.timestamp; - let mut cost = 0; - - // 3. Perform one, or multiple, sleeping islands extraction (graph traversal). - // Limit the traversal cost by not traversing all the known sleeping roots if - // there are too many. - const MAX_PER_FRAME_COST: usize = 1000; // TODO: find the best value. - while let Some(sleep_root) = self.traversal_candidates.pop_front() { - if cost > MAX_PER_FRAME_COST { - // Early-break if we consider we have done enough island extraction work. - break; - } - - cost += self.extract_sleeping_island( - bodies, - colliders, - impulse_joints, - multibody_joints, - narrow_phase, - sleep_root.0, - frame_base_timestamp, - ); - - self.timestamp += 1; - } - - self.update_optimizer( - bodies, - colliders, - impulse_joints, - multibody_joints, - narrow_phase, - ); - // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0); - } - - /// Returns the number of iterations run by the graph traversal so we can balance load across - /// frames. - fn extract_sleeping_island( - &mut self, - bodies: &mut RigidBodySet, - colliders: &ColliderSet, - impulse_joints: &ImpulseJointSet, - multibody_joints: &MultibodyJointSet, - narrow_phase: &NarrowPhase, - sleep_root: RigidBodyHandle, - frame_base_timestamp: u32, - ) -> usize { - let Some(rb) = bodies.get_mut_internal(sleep_root) else { - // This branch happens if the rigid-body no longer exists. - return 0; - }; - - rb.activation.is_sleep_root_candidate = false; - - if rb.ids.active_set_timestamp >= frame_base_timestamp { - return 0; // We already traversed this rigid-body this frame. - } - - let active_island_id = rb.ids.active_island_id; - let active_island = &mut self.islands[active_island_id]; - if active_island.is_sleeping() { - // This rigid-body is already part of a sleeping island. - return 0; - } - - // TODO: implement recycling islands to avoid repeated allocations? - let mut new_island = Island::default(); - self.stack.push(sleep_root); - - let mut can_sleep = true; - let mut niter = 0; - while let Some(handle) = self.stack.pop() { - let rb = bodies.index_mut_internal(handle); - - if rb.is_fixed() { - // Don’t propagate islands through fixed bodies. - continue; - } - - if rb.ids.active_set_timestamp == self.timestamp { - // We already visited this body and its neighbors. - continue; - } - - if rb.ids.active_set_timestamp >= frame_base_timestamp { - // We already visited this body and its neighbors during this frame. - // So we already know this islands cannot sleep (otherwise the bodies - // currently being traversed would already have been marked as sleeping). - return niter; - } - - niter += 1; - rb.ids.active_set_timestamp = self.timestamp; - - assert!(!rb.activation.sleeping); - - // TODO PERF: early-exit as soon as we reach a body not eligible to sleep. - can_sleep = can_sleep && rb.activation().is_eligible_for_sleep(); - if !rb.activation.is_eligible_for_sleep() { - // If this body cannot sleep, abort the traversal, we are not traversing - // yet an island that can sleep. - self.stack.clear(); - return niter; - } - - // Traverse bodies that are interacting with the current one either through - // contacts or a joint. - push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack); - push_linked_bodies(impulse_joints, multibody_joints, handle, &mut self.stack); - new_island.bodies.push(handle); - } - - assert!(can_sleep); - - // If we reached this line, we completed a sleeping island traversal. - // - Put its bodies to sleep. - // - Remove them from the active set. - // - Push the sleeping island. - if active_island.len() == new_island.len() { - // The whole island is asleep. No need to insert a new one. - // Put all its bodies to sleep. - for handle in &active_island.bodies { - let rb = bodies.index_mut_internal(*handle); - rb.sleep(); - } - - // Mark the existing island as sleeping (by clearing its `id_in_awake_list`) - // and remove it from the awake list. - let island_awake_id = active_island - .id_in_awake_list - .take() - .unwrap_or_else(|| unreachable!()); - self.awake_islands.swap_remove(island_awake_id); - - if let Some(moved_id) = self.awake_islands.get(island_awake_id) { - self.islands[*moved_id].id_in_awake_list = Some(island_awake_id); - } - } else { - niter += new_island.len(); // Include this part into the cost estimate for this function. - self.extract_sub_island(bodies, active_island_id, new_island, true); - } - niter - } - - fn update_optimizer( - &mut self, - bodies: &mut RigidBodySet, - colliders: &ColliderSet, - impulse_joints: &ImpulseJointSet, - multibody_joints: &MultibodyJointSet, - narrow_phase: &NarrowPhase, - ) { - if self.optimizer.mode % 2 == 0 { - self.incremental_merge(bodies); - } else { - self.incremental_split( - bodies, - colliders, - impulse_joints, - multibody_joints, - narrow_phase, - ); - } - - self.optimizer.mode = self.optimizer.mode.wrapping_add(1); - } - - /// Attempts to merge awake islands that are too small. - fn incremental_merge(&mut self, bodies: &mut RigidBodySet) { - struct IslandData { - id: usize, - awake_id: usize, - solver_iters: usize, - } - - // Ensure the awake id is still in bounds. - if self.optimizer.merge_state.curr_awake_id >= self.awake_islands.len() { - self.optimizer.merge_state.curr_awake_id = 0; - } - - // Find a first candidate for a merge. - let mut island1 = None; - for awake_id in self.optimizer.merge_state.curr_awake_id..self.awake_islands.len() { - let id = self.awake_islands[awake_id]; - let island = &self.islands[id]; - if island.len() < self.optimizer.min_island_size { - island1 = Some(IslandData { - awake_id, - id, - solver_iters: island.additional_solver_iterations, - }); - break; - } - } - - if let Some(island1) = island1 { - // Indicates if we found a merge candidate for the next incremental update. - let mut found_next = false; - self.optimizer.merge_state.curr_awake_id = island1.awake_id + 1; - - // Find a second candidate for a merge. - for awake_id2 in island1.awake_id + 1..self.awake_islands.len() { - let id2 = self.awake_islands[awake_id2]; - let island2 = &self.islands[id2]; - - if island1.solver_iters == island2.additional_solver_iterations - && island2.len() < self.optimizer.min_island_size - { - // Found a second candidate! Merge them. - self.merge_islands(bodies, island1.id, id2); - println!("Found a merge {}!", self.awake_islands.len()); - - // TODO: support doing more than a single merge per frame. - return; - } else if island2.len() < self.optimizer.min_island_size && !found_next { - // We found a good candidate for the next incremental merge (we can’t just - // merge it now because it’s not compatible with the current island). - self.optimizer.merge_state.curr_awake_id = awake_id2; - found_next = true; - } - } - } else { - self.optimizer.merge_state.curr_awake_id = 0; - } - } - - /// Attempts to split awake islands that are too big. - fn incremental_split( - &mut self, - bodies: &mut RigidBodySet, - colliders: &ColliderSet, - impulse_joints: &ImpulseJointSet, - multibody_joints: &MultibodyJointSet, - narrow_phase: &NarrowPhase, - ) { - if self.optimizer.split_state.curr_awake_id >= self.awake_islands.len() { - self.optimizer.split_state.curr_awake_id = 0; - } - - for awake_id in self.optimizer.split_state.curr_awake_id..self.awake_islands.len() { - let id = self.awake_islands[awake_id]; - if self.islands[id].len() > self.optimizer.max_island_size { - // Try to split this island. - // Note that the traversal logic is similar to the sleeping island - // extraction, except that we have different stopping criteria. - self.stack.clear(); - - // TODO: implement islands recycling to avoid reallocating every time. - let mut new_island = Island::default(); - self.timestamp += 1; - - for root in &self.islands[id].bodies { - self.stack.push(*root); - - let len_before_traversal = new_island.len(); - while let Some(handle) = self.stack.pop() { - let rb = bodies.index_mut(handle); - if rb.is_fixed() { - // Don’t propagate islands through rigid-bodies. - continue; - } - - if rb.ids.active_set_timestamp == self.timestamp { - // We already visited this body. - continue; - } - - rb.ids.active_set_timestamp = self.timestamp; - assert!(!rb.activation.sleeping); - - // Traverse bodies that are interacting with the current one either through - // contacts or a joint. - push_contacting_bodies( - &rb.colliders, - colliders, - narrow_phase, - &mut self.stack, - ); - push_linked_bodies( - impulse_joints, - multibody_joints, - handle, - &mut self.stack, - ); - new_island.bodies.push(handle); - - // Our new island cannot grow any further. - if new_island.bodies.len() > self.optimizer.max_island_size { - new_island.bodies.truncate(len_before_traversal); - self.stack.clear(); - break; - } - } - - // Extract this island. - if new_island.len() == 0 { - // println!("Failed to split island."); - return; - } else if new_island.bodies.len() >= self.optimizer.min_island_size { - // println!( - // "Split an island: {}/{} ({} islands)", - // new_island.len(), - // self.islands[id].len(), - // self.awake_islands.len(), - // ); - self.extract_sub_island(bodies, id, new_island, false); - return; // TODO: support extracting more than one island per frame. - } - } - } else { - self.optimizer.split_state.curr_awake_id = awake_id + 1; - } - } - } - - /// Remove from the island at `source_id` all the rigid-body that are in `new_island`, and - /// insert `new_island` into the islands set. - /// - /// **All** rigid-bodies from `new_island` must currently be part of the island at `source_id`. - fn extract_sub_island( - &mut self, - bodies: &mut RigidBodySet, - source_id: usize, - mut new_island: Island, - sleep: bool, - ) { - let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len()); - let active_island = &mut self.islands[source_id]; - - for (id, handle) in new_island.bodies.iter().enumerate() { - let rb = bodies.index_mut_internal(*handle); - - // If the new island is sleeping, ensure all its bodies are sleeping. - if sleep { - rb.sleep(); - } - - let id_to_remove = rb.ids.active_set_id; - assert_eq!(rb.ids.active_island_id, source_id); - rb.ids.active_island_id = new_island_id; - rb.ids.active_set_id = id; - rb.ids.active_set_offset = id as u32; - - new_island.additional_solver_iterations = new_island - .additional_solver_iterations - .max(rb.additional_solver_iterations); - active_island.bodies.swap_remove(id_to_remove); - - if let Some(moved_handle) = active_island.bodies.get(id_to_remove).copied() { - let moved_rb = bodies.index_mut_internal(moved_handle); - moved_rb.ids.active_set_id = id_to_remove; - moved_rb.ids.active_set_offset = id_to_remove as u32; - } - } - - // If the new island is awake, add it to the awake list. - if !sleep { - new_island.id_in_awake_list = Some(self.awake_islands.len()); - self.awake_islands.push(new_island_id); - } - - self.islands.insert(new_island_id, new_island); - } -} - -// Read all the contacts and push objects touching this rigid-body. -#[inline] -fn push_contacting_bodies( - rb_colliders: &RigidBodyColliders, - colliders: &ColliderSet, - narrow_phase: &NarrowPhase, - stack: &mut Vec, -) { - for collider_handle in &rb_colliders.0 { - for inter in narrow_phase.contact_pairs_with(*collider_handle) { - for manifold in &inter.manifolds { - if !manifold.data.solver_contacts.is_empty() { - let other = crate::utils::select_other( - (inter.collider1, inter.collider2), - *collider_handle, - ); - if let Some(other_body) = colliders[other].parent { - stack.push(other_body.handle); - } - break; - } - } - } - } -} - -fn push_linked_bodies( - impulse_joints: &ImpulseJointSet, - multibody_joints: &MultibodyJointSet, - handle: RigidBodyHandle, - stack: &mut Vec, -) { - for inter in impulse_joints.attached_enabled_joints(handle) { - let other = crate::utils::select_other((inter.0, inter.1), handle); - stack.push(other); - } - - for other in multibody_joints.bodies_attached_with_enabled_joint(handle) { - stack.push(other); - } -} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index bf818d95d..41e91dadb 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -26,7 +26,6 @@ mod ccd; mod coefficient_combine_rule; mod integration_parameters; mod island_manager; -mod island_manager2; mod joint; mod rigid_body_components; mod solver; diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index e76a82276..ab2be423a 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -196,7 +196,7 @@ impl PhysicsPipeline { events: &dyn EventHandler, ) { self.counters.stages.island_construction_time.resume(); - islands.update_active_set_with_contacts( + islands.update_islands( integration_parameters.dt, integration_parameters.length_unit, bodies, diff --git a/src/pipeline/user_changes.rs b/src/pipeline/user_changes.rs index e396dfe88..1eadfb220 100644 --- a/src/pipeline/user_changes.rs +++ b/src/pipeline/user_changes.rs @@ -72,7 +72,7 @@ pub(crate) fn handle_user_changes_to_rigid_bodies( // The body's status changed. We need to make sure // it is on the correct active set. if let Some(islands) = islands.as_deref_mut() { - islands.update_body(*handle, bodies); + islands.rigid_body_updated(*handle, bodies); } } From 9ceeb34d383d5b8cfbf124dffcc15a4caf30e4d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 7 Nov 2025 13:16:41 +0100 Subject: [PATCH 09/12] chore: refactor manifold reduction to its own file + add naive reduction method --- src/geometry/manifold_reduction.rs | 220 +++++++++++++++++++++++++++++ src/geometry/mod.rs | 3 + src/geometry/narrow_phase.rs | 134 ++---------------- 3 files changed, 236 insertions(+), 121 deletions(-) create mode 100644 src/geometry/manifold_reduction.rs diff --git a/src/geometry/manifold_reduction.rs b/src/geometry/manifold_reduction.rs new file mode 100644 index 000000000..4244ba1df --- /dev/null +++ b/src/geometry/manifold_reduction.rs @@ -0,0 +1,220 @@ +use crate::geometry::ContactManifold; +use crate::math::{Point, Real}; +use crate::utils::SimdBasis; + +pub(crate) fn reduce_manifold_naive( + manifold: &ContactManifold, + selected: &mut [usize; 4], + num_selected: &mut usize, + prediction_distance: Real, +) { + if manifold.points.len() <= 4 { + return; + } + + // Run contact reduction so we only have up to four solver contacts. + // We follow a logic similar to Bepu’s approach. + // 1. Find the deepest contact. + *selected = [usize::MAX; 4]; + + let mut deepest_dist = Real::MAX; + for (i, pt) in manifold.points.iter().enumerate() { + if pt.dist < deepest_dist { + deepest_dist = pt.dist; + selected[0] = i; + } + } + + if selected[0] == usize::MAX { + *num_selected = 0; + return; + } + + // 2. Find the point that is the furthest from the deepest point. + let selected_a = &manifold.points[selected[0]].local_p1; + let mut furthest_dist = -f32::MAX; + for (i, pt) in manifold.points.iter().enumerate() { + let dist = na::distance_squared(&pt.local_p1, &selected_a); + if i != selected[0] && pt.dist <= prediction_distance && dist > furthest_dist { + furthest_dist = dist; + selected[1] = i; + } + } + + if selected[1] == usize::MAX { + *num_selected = 1; + return; + } + + // 3. Now find the two points furthest from the segment we built so far. + let selected_b = &manifold.points[selected[1]].local_p1; + + if selected_a == selected_b { + *num_selected = 1; + return; + } + + let selected_ab = selected_b - selected_a; + let tangent = selected_ab.cross(&manifold.local_n1); + + // Find the points that minimize and maximize the dot product with the tangent. + let mut min_dot = Real::MAX; + let mut max_dot = -Real::MAX; + for (i, pt) in manifold.points.iter().enumerate() { + if i == selected[0] || i == selected[1] || pt.dist > prediction_distance { + continue; + } + + let dot = (pt.local_p1 - selected_a).dot(&tangent); + if dot < min_dot { + min_dot = dot; + selected[2] = i; + } + + if dot > max_dot { + max_dot = dot; + selected[3] = i; + } + } + + if selected[2] == usize::MAX { + *num_selected = 2; + } else if selected[2] == selected[3] { + *num_selected = 3; + } else { + *num_selected = 4; + } +} + +// Run contact reduction using Bepu's InternalReduce algorithm. +// This reduces the manifold to at most 4 contacts using: +// 1. Deepest contact (with extremity bias for stability) +// 2. Most distant contact from the first +// 3. Two contacts that maximize positive and negative signed area with the first edge +pub(crate) fn reduce_manifold_bepu_like( + manifold: &ContactManifold, + selected: &mut [usize; 4], + num_selected: &mut usize, +) { + if manifold.points.len() <= 4 { + return; + } + + // Step 1: Find the deepest contact, biased by extremity for frame stability. + // The extremity heuristic helps maintain consistent contact selection across frames + // when multiple contacts have similar depths. + let mut best_score = -Real::MAX; + const EXTREMITY_SCALE: Real = 1e-2; + // Use an arbitrary direction (roughly 38 degrees from X axis) to break ties + const EXTREMITY_DIR_X: Real = 0.7946897654; + const EXTREMITY_DIR_Y: Real = 0.60701579614; + + let tangents = manifold.local_n1.orthonormal_basis(); + + for (i, pt) in manifold.points.iter().enumerate() { + // Extremity measures how far the contact is from the origin in the tangent plane + let tx1 = pt.local_p1.coords.dot(&tangents[0]); + let ty1 = pt.local_p1.coords.dot(&tangents[1]); + + let extremity = (tx1 * EXTREMITY_DIR_X + ty1 * EXTREMITY_DIR_Y).abs(); + + // Score = depth + small extremity bias (only for non-speculative contacts) + // Negative dist = deeper penetration = higher score + let score = if pt.dist >= 0.0 { + -pt.dist // Speculative contact, no extremity bias + } else { + -pt.dist + extremity * EXTREMITY_SCALE + }; + + if score > best_score { + best_score = score; + selected[0] = i; + } + } + + // Step 2: Find the point most distant from the first contact. + // This establishes a baseline "edge" for the manifold. + let contact0_pos = manifold.points[selected[0]].local_p1; + let mut max_distance_squared = 0.0; + + for (i, pt) in manifold.points.iter().enumerate() { + let offset = pt.local_p1 - contact0_pos; + let offset_x = offset.dot(&tangents[0]); + let offset_y = offset.dot(&tangents[1]); + let distance_squared = offset_x * offset_x + offset_y * offset_y; + + if distance_squared > max_distance_squared { + max_distance_squared = distance_squared; + selected[1] = i; + } + } + + // Early out if the contacts are too close together + let epsilon = 1e-6; + if max_distance_squared <= epsilon { + // Only one meaningful contact + *num_selected = 1; + } else { + // Step 3: Find two more contacts that maximize positive and negative signed area. + // Using the first two contacts as an edge, we look for contacts that form triangles + // with the largest magnitude negative and positive areas. This maximizes the + // spatial extent of the contact manifold. + + *num_selected = 2; + selected[2] = usize::MAX; + selected[3] = usize::MAX; + + let contact1_pos = manifold.points[selected[1]].local_p1; + let edge_offset = contact1_pos - contact0_pos; + let edge_offset_x = edge_offset.dot(&tangents[0]); + let edge_offset_y = edge_offset.dot(&tangents[1]); + + let mut min_signed_area = 0.0; + let mut max_signed_area = 0.0; + + for (i, pt) in manifold.points.iter().enumerate() { + let candidate_offset = pt.local_p1 - contact0_pos; + let candidate_offset_x = candidate_offset.dot(&tangents[0]); + let candidate_offset_y = candidate_offset.dot(&tangents[1]); + + // Signed area of the triangle formed by (contact0, contact1, candidate) + // This is a 2D cross product: (candidate - contact0) × (contact1 - contact0) + let mut signed_area = + candidate_offset_x * edge_offset_y - candidate_offset_y * edge_offset_x; + + // Penalize speculative contacts (they're less important) + if pt.dist >= 0.0 { + signed_area *= 0.25; + } + + if signed_area < min_signed_area { + min_signed_area = signed_area; + selected[2] = i; + } + if signed_area > max_signed_area { + max_signed_area = signed_area; + selected[3] = i; + } + } + + // Check if the signed areas are significant enough + // Epsilon based on the edge length squared + let area_epsilon = max_distance_squared * max_distance_squared * 1e-6; + + // If the areas are too small, don't add those contacts + if min_signed_area * min_signed_area <= area_epsilon { + selected[2] = usize::MAX; + } + if max_signed_area * max_signed_area <= area_epsilon { + selected[3] = usize::MAX; + } + + let keep2 = selected[2] != usize::MAX; + let keep3 = selected[3] != usize::MAX; + *num_selected += keep2 as usize + keep3 as usize; + + if !keep2 { + selected[2] = selected[3]; + } + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index e7c97bb5b..ca0e12e1d 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -231,3 +231,6 @@ mod broad_phase_pair_event; mod collider; mod collider_set; mod mesh_converter; + +#[cfg(feature = "dim3")] +mod manifold_reduction; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 9ab56c6ac..f3d5c5d80 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -7,6 +7,7 @@ use crate::dynamics::{ CoefficientCombineRule, ImpulseJointSet, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType, }; +use crate::geometry::manifold_reduction::reduce_manifold_bepu_like; use crate::geometry::{ BoundingVolume, BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, @@ -1007,127 +1008,18 @@ impl NarrowPhase { let mut num_selected = MAX_MANIFOLD_POINTS.min(manifold.points.len()); #[cfg(feature = "dim3")] - { - if manifold.points.len() > 4 { - // Run contact reduction using Bepu's InternalReduce algorithm. - // This reduces the manifold to at most 4 contacts using: - // 1. Deepest contact (with extremity bias for stability) - // 2. Most distant contact from the first - // 3. Two contacts that maximize positive and negative signed area with the first edge - - // Step 1: Find the deepest contact, biased by extremity for frame stability. - // The extremity heuristic helps maintain consistent contact selection across frames - // when multiple contacts have similar depths. - let mut best_score = -Real::MAX; - const EXTREMITY_SCALE: Real = 1e-2; - // Use an arbitrary direction (roughly 38 degrees from X axis) to break ties - const EXTREMITY_DIR_X: Real = 0.7946897654; - const EXTREMITY_DIR_Y: Real = 0.60701579614; - - for (i, pt) in manifold.points.iter().enumerate() { - // Extremity measures how far the contact is from the origin in the tangent plane - let extremity = (pt.local_p1.x * EXTREMITY_DIR_X - + pt.local_p1.y * EXTREMITY_DIR_Y) - .abs(); - - // Score = depth + small extremity bias (only for non-speculative contacts) - // Negative dist = deeper penetration = higher score - let score = if pt.dist >= 0.0 { - -pt.dist // Speculative contact, no extremity bias - } else { - -pt.dist + extremity * EXTREMITY_SCALE - }; - - if score > best_score { - best_score = score; - selected[0] = i; - } - } - - // Step 2: Find the point most distant from the first contact. - // This establishes a baseline "edge" for the manifold. - let contact0_pos = manifold.points[selected[0]].local_p1; - let mut max_distance_squared = 0.0; - - for (i, pt) in manifold.points.iter().enumerate() { - let offset = pt.local_p1 - contact0_pos; - let distance_squared = offset.x * offset.x + offset.y * offset.y; - - if distance_squared > max_distance_squared { - max_distance_squared = distance_squared; - selected[1] = i; - } - } - - // Early out if the contacts are too close together - let epsilon = 1e-6; - if max_distance_squared <= epsilon { - // Only one meaningful contact - num_selected = 1; - } else { - // Step 3: Find two more contacts that maximize positive and negative signed area. - // Using the first two contacts as an edge, we look for contacts that form triangles - // with the largest magnitude negative and positive areas. This maximizes the - // spatial extent of the contact manifold. - - num_selected = 2; - selected[2] = usize::MAX; - selected[3] = usize::MAX; - - let contact1_pos = manifold.points[selected[1]].local_p1; - let edge_offset_x = contact1_pos.x - contact0_pos.x; - let edge_offset_y = contact1_pos.y - contact0_pos.y; - - let mut min_signed_area = 0.0; - let mut max_signed_area = 0.0; - - for (i, pt) in manifold.points.iter().enumerate() { - let candidate_offset_x = pt.local_p1.x - contact0_pos.x; - let candidate_offset_y = pt.local_p1.y - contact0_pos.y; - - // Signed area of the triangle formed by (contact0, contact1, candidate) - // This is a 2D cross product: (candidate - contact0) × (contact1 - contact0) - let mut signed_area = candidate_offset_x * edge_offset_y - - candidate_offset_y * edge_offset_x; - - // Penalize speculative contacts (they're less important) - if pt.dist >= 0.0 { - signed_area *= 0.25; - } - - if signed_area < min_signed_area { - min_signed_area = signed_area; - selected[2] = i; - } - if signed_area > max_signed_area { - max_signed_area = signed_area; - selected[3] = i; - } - } - - // Check if the signed areas are significant enough - // Epsilon based on the edge length squared - let area_epsilon = - max_distance_squared * max_distance_squared * 1e-6; - - // If the areas are too small, don't add those contacts - if min_signed_area * min_signed_area <= area_epsilon { - selected[2] = usize::MAX; - } - if max_signed_area * max_signed_area <= area_epsilon { - selected[3] = usize::MAX; - } - - let keep2 = selected[2] != usize::MAX; - let keep3 = selected[3] != usize::MAX; - num_selected += keep2 as usize + keep3 as usize; - - if !keep2 { - selected[2] = selected[3]; - } - } - } - } + super::manifold_reduction::reduce_manifold_bepu_like( + manifold, + &mut selected, + &mut num_selected, + ); + // #[cfg(feature = "dim3")] + // super::manifold_reduction::reduce_manifold_naive( + // manifold, + // &mut selected, + // &mut num_selected, + // prediction_distance, + // ); for contact_id in &selected[..num_selected] { // // manifold.points.iter().enumerate() { From a05f09ca349fd2453e22fa4548793793ff9b1f4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 7 Nov 2025 13:17:03 +0100 Subject: [PATCH 10/12] =?UTF-8?q?feat:=E2=80=AFadd=20islands=20manager=20v?= =?UTF-8?q?alidation=20checks?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/dynamics/island_manager/island.rs | 9 ++++- src/dynamics/island_manager/manager.rs | 9 +++-- src/dynamics/island_manager/mod.rs | 1 + src/dynamics/island_manager/optimizer.rs | 1 - src/dynamics/island_manager/sleep.rs | 6 ++++ src/dynamics/island_manager/validation.rs | 44 +++++++++++++++++++++++ src/pipeline/physics_pipeline.rs | 1 + 7 files changed, 66 insertions(+), 5 deletions(-) create mode 100644 src/dynamics/island_manager/validation.rs diff --git a/src/dynamics/island_manager/island.rs b/src/dynamics/island_manager/island.rs index 060a3c735..7ebb9942b 100644 --- a/src/dynamics/island_manager/island.rs +++ b/src/dynamics/island_manager/island.rs @@ -69,7 +69,12 @@ impl IslandManager { } let id_to_remove = rb.ids.active_set_id; - assert_eq!(rb.ids.active_island_id, source_id); + + assert_eq!( + rb.ids.active_island_id, source_id, + "note, new id: {}", + new_island_id + ); rb.ids.active_island_id = new_island_id; rb.ids.active_set_id = id; rb.ids.active_set_offset = id as u32; @@ -121,6 +126,8 @@ impl IslandManager { (island_id1, island_id2) }; + // println!("Merging: keep {} remove {}", to_keep, to_remove); + let Some(removed_island) = self.islands.remove(to_remove) else { // TODO: the island doesn’t exist is that an internal error? return; diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs index ddab1f069..bacd335aa 100644 --- a/src/dynamics/island_manager/manager.rs +++ b/src/dynamics/island_manager/manager.rs @@ -112,14 +112,14 @@ impl IslandManager { } if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) { + assert!(rb1.is_fixed() || rb1.ids.active_island_id != usize::MAX); + assert!(rb2.is_fixed() || rb2.ids.active_island_id != usize::MAX); + // If both bodies are not part of the same island, merge the islands. if !rb1.is_fixed() && !rb2.is_fixed() && rb1.ids.active_island_id != rb2.ids.active_island_id { - assert_ne!(rb1.ids.active_island_id, usize::MAX); - assert_ne!(rb2.ids.active_island_id, usize::MAX); - self.merge_islands( bodies, rb1.ids.active_island_id, @@ -293,5 +293,8 @@ impl IslandManager { narrow_phase, ); // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0); + + #[cfg(debug_assertions)] + self.assert_state_is_valid(bodies); } } diff --git a/src/dynamics/island_manager/mod.rs b/src/dynamics/island_manager/mod.rs index 8a54427c2..4ace3cf0f 100644 --- a/src/dynamics/island_manager/mod.rs +++ b/src/dynamics/island_manager/mod.rs @@ -8,3 +8,4 @@ mod manager; mod optimizer; mod sleep; mod utils; +mod validation; diff --git a/src/dynamics/island_manager/optimizer.rs b/src/dynamics/island_manager/optimizer.rs index c7bbb93fa..71b723fb9 100644 --- a/src/dynamics/island_manager/optimizer.rs +++ b/src/dynamics/island_manager/optimizer.rs @@ -119,7 +119,6 @@ impl IslandManager { { // Found a second candidate! Merge them. self.merge_islands(bodies, island1.id, id2); - println!("Found a merge {}!", self.awake_islands.len()); // TODO: support doing more than a single merge per frame. return; diff --git a/src/dynamics/island_manager/sleep.rs b/src/dynamics/island_manager/sleep.rs index 565050d2f..4f2e74c39 100644 --- a/src/dynamics/island_manager/sleep.rs +++ b/src/dynamics/island_manager/sleep.rs @@ -78,6 +78,7 @@ impl IslandManager { // TODO: implement recycling islands to avoid repeated allocations? let mut new_island = Island::default(); + self.stack.clear(); self.stack.push(sleep_root); let mut can_sleep = true; @@ -106,6 +107,11 @@ impl IslandManager { rb.ids.active_set_timestamp = self.timestamp; assert!(!rb.activation.sleeping); + assert_eq!( + rb.ids.active_island_id, active_island_id, + "note niter: {}", + niter + ); // TODO PERF: early-exit as soon as we reach a body not eligible to sleep. can_sleep = can_sleep && rb.activation().is_eligible_for_sleep(); diff --git a/src/dynamics/island_manager/validation.rs b/src/dynamics/island_manager/validation.rs new file mode 100644 index 000000000..99568d720 --- /dev/null +++ b/src/dynamics/island_manager/validation.rs @@ -0,0 +1,44 @@ +use crate::dynamics::{IslandManager, RigidBodySet}; + +impl IslandManager { + #[cfg(debug_assertions)] + pub fn assert_state_is_valid(&self, bodies: &RigidBodySet) { + for (island_id, island) in self.islands.iter() { + // Sleeping island must not be in the awake list. + if island.is_sleeping() { + assert!(!self.awake_islands.contains(&island_id)); + } else { + // If the island is awake, the awake id must match. + let awake_id = island.id_in_awake_list.unwrap(); + self.awake_islands[awake_id] == island_id; + } + + for (body_id, handle) in island.bodies.iter().enumerate() { + if let Some(rb) = bodies.get(*handle) { + // The body’s sleeping status must match the island’s status. + assert_eq!(rb.is_sleeping(), island.is_sleeping()); + // The body’s island id must match the island id. + assert_eq!(rb.ids.active_island_id, island_id); + // The body’s offset must match its handle’s position in island.bodies. + assert_eq!(body_id, rb.ids.active_set_offset as usize); + assert_eq!(body_id, rb.ids.active_set_id); + } + } + } + + // Free island ids must actually be free. + for id in self.free_islands.iter() { + assert!(self.islands.get(*id).is_none()); + } + + // The awake islands list must not have duplicates. + let mut awake_islands_dedup = self.awake_islands.clone(); + awake_islands_dedup.sort(); + awake_islands_dedup.dedup(); + assert_eq!(self.awake_islands.len(), awake_islands_dedup.len()); + + println!( + "`IslandManager::assert_state_is_valid` validation checks passed. This is slow. Only enable for debugging." + ); + } +} diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index ab2be423a..a8c346c56 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -196,6 +196,7 @@ impl PhysicsPipeline { events: &dyn EventHandler, ) { self.counters.stages.island_construction_time.resume(); + // NOTE: this must be done after the narrow-phase. islands.update_islands( integration_parameters.dt, integration_parameters.length_unit, From 7a2454f263548a2a03a4d8ead50846ca768195e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 7 Nov 2025 18:22:24 +0100 Subject: [PATCH 11/12] fix various bugs in the new islands system --- src/counters/collision_detection_counters.rs | 2 + src/counters/solver_counters.rs | 14 ++-- src/dynamics/island_manager/island.rs | 10 +-- src/dynamics/island_manager/manager.rs | 71 +++++++++---------- src/dynamics/island_manager/mod.rs | 2 +- src/dynamics/island_manager/optimizer.rs | 6 +- src/dynamics/island_manager/sleep.rs | 58 +++++++++------ src/dynamics/island_manager/utils.rs | 17 ++--- src/dynamics/island_manager/validation.rs | 34 ++++++++- src/dynamics/rigid_body_components.rs | 27 +++++-- .../contact_constraints_set.rs | 3 +- src/dynamics/solver/island_solver.rs | 17 +++-- src/geometry/contact_pair.rs | 11 +-- src/geometry/manifold_reduction.rs | 20 +++--- src/geometry/narrow_phase.rs | 67 +++++------------ src/pipeline/physics_pipeline.rs | 3 +- src_testbed/physx_backend.rs | 4 +- src_testbed/ui.rs | 14 ++-- 18 files changed, 209 insertions(+), 171 deletions(-) diff --git a/src/counters/collision_detection_counters.rs b/src/counters/collision_detection_counters.rs index ab5c360c7..43730c570 100644 --- a/src/counters/collision_detection_counters.rs +++ b/src/counters/collision_detection_counters.rs @@ -8,6 +8,8 @@ pub struct CollisionDetectionCounters { pub ncontact_pairs: usize, /// Time spent for the broad-phase of the collision detection. pub broad_phase_time: Timer, + /// Time spent by the final broad-phase AABB update after body movement to keep + /// user scene queries valid. pub final_broad_phase_time: Timer, /// Time spent for the narrow-phase of the collision detection. pub narrow_phase_time: Timer, diff --git a/src/counters/solver_counters.rs b/src/counters/solver_counters.rs index f7193d294..19b721f78 100644 --- a/src/counters/solver_counters.rs +++ b/src/counters/solver_counters.rs @@ -12,8 +12,10 @@ pub struct SolverCounters { pub velocity_resolution_time: Timer, /// Time spent for the assembly of all the velocity constraints. pub velocity_assembly_time: Timer, - pub velocity_assembly_time_a: Timer, - pub velocity_assembly_time_b: Timer, + /// Time spent by the velocity assembly for initializing solver bodies. + pub velocity_assembly_time_solver_bodies: Timer, + /// Time spent by the velocity assemble for initializing the constraints. + pub velocity_assembly_time_constraints_init: Timer, /// Time spent for the update of the velocity of the bodies. pub velocity_update_time: Timer, /// Time spent to write force back to user-accessible data. @@ -27,8 +29,8 @@ impl SolverCounters { nconstraints: 0, ncontacts: 0, velocity_assembly_time: Timer::new(), - velocity_assembly_time_a: Timer::new(), - velocity_assembly_time_b: Timer::new(), + velocity_assembly_time_solver_bodies: Timer::new(), + velocity_assembly_time_constraints_init: Timer::new(), velocity_resolution_time: Timer::new(), velocity_update_time: Timer::new(), velocity_writeback_time: Timer::new(), @@ -41,8 +43,8 @@ impl SolverCounters { self.ncontacts = 0; self.velocity_resolution_time.reset(); self.velocity_assembly_time.reset(); - self.velocity_assembly_time_a.reset(); - self.velocity_assembly_time_b.reset(); + self.velocity_assembly_time_solver_bodies.reset(); + self.velocity_assembly_time_constraints_init.reset(); self.velocity_update_time.reset(); self.velocity_writeback_time.reset(); } diff --git a/src/dynamics/island_manager/island.rs b/src/dynamics/island_manager/island.rs index 7ebb9942b..c268b9094 100644 --- a/src/dynamics/island_manager/island.rs +++ b/src/dynamics/island_manager/island.rs @@ -58,7 +58,7 @@ impl IslandManager { sleep: bool, ) { let new_island_id = self.free_islands.pop().unwrap_or(self.islands.len()); - let active_island = &mut self.islands[source_id]; + let source_island = &mut self.islands[source_id]; for (id, handle) in new_island.bodies.iter().enumerate() { let rb = bodies.index_mut_internal(*handle); @@ -82,9 +82,9 @@ impl IslandManager { new_island.additional_solver_iterations = new_island .additional_solver_iterations .max(rb.additional_solver_iterations); - active_island.bodies.swap_remove(id_to_remove); + source_island.bodies.swap_remove(id_to_remove); - if let Some(moved_handle) = active_island.bodies.get(id_to_remove).copied() { + if let Some(moved_handle) = source_island.bodies.get(id_to_remove).copied() { let moved_rb = bodies.index_mut_internal(moved_handle); moved_rb.ids.active_set_id = id_to_remove; moved_rb.ids.active_set_offset = id_to_remove as u32; @@ -95,6 +95,8 @@ impl IslandManager { if !sleep { new_island.id_in_awake_list = Some(self.awake_islands.len()); self.awake_islands.push(new_island_id); + } else { + new_island.id_in_awake_list = None; } self.islands.insert(new_island_id, new_island); @@ -126,7 +128,7 @@ impl IslandManager { (island_id1, island_id2) }; - // println!("Merging: keep {} remove {}", to_keep, to_remove); + // println!("Merging: {} <- {}", to_keep, to_remove); let Some(removed_island) = self.islands.remove(to_remove) else { // TODO: the island doesn’t exist is that an internal error? diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs index bacd335aa..ed680678b 100644 --- a/src/dynamics/island_manager/manager.rs +++ b/src/dynamics/island_manager/manager.rs @@ -1,19 +1,19 @@ +use super::{Island, IslandsOptimizer}; use crate::dynamics::{ ImpulseJointSet, MultibodyJointSet, RigidBodyChanges, RigidBodyHandle, RigidBodyIds, RigidBodySet, }; use crate::geometry::{ColliderSet, NarrowPhase}; use crate::math::Real; +use crate::prelude::SleepRootState; use crate::utils::SimdDot; use std::collections::VecDeque; use vec_map::VecMap; -use super::{Island, IslandsOptimizer}; - /// An island starting at this rigid-body might be eligible for sleeping. #[derive(Copy, Clone, Debug, PartialEq)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct SleepCandidate(RigidBodyHandle); +pub(super) struct SleepCandidate(RigidBodyHandle); /// System that manages which bodies are active (awake) vs sleeping to optimize performance. /// @@ -39,7 +39,7 @@ pub struct IslandManager { /// Potential candidate roots for graph traversal to identify a sleeping /// connected component or to split an island in two. pub(super) traversal_candidates: VecDeque, - pub(super) timestamp: u32, // TODO: the physics pipeline (or something else) should expose a step_id? + pub(super) traversal_timestamp: u32, pub(super) optimizer: IslandsOptimizer, #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(super) stack: Vec, // Workspace. @@ -103,14 +103,14 @@ impl IslandManager { started: bool, wake_up: bool, ) { - if started { - match (handle1, handle2) { - (Some(handle1), Some(handle2)) => { - if wake_up { - self.wake_up(bodies, handle1, false); - self.wake_up(bodies, handle2, false); - } + match (handle1, handle2) { + (Some(handle1), Some(handle2)) => { + if wake_up { + self.wake_up(bodies, handle1, false); + self.wake_up(bodies, handle2, false); + } + if started { if let (Some(rb1), Some(rb2)) = (bodies.get(handle1), bodies.get(handle2)) { assert!(rb1.is_fixed() || rb1.ids.active_island_id != usize::MAX); assert!(rb2.is_fixed() || rb2.ids.active_island_id != usize::MAX); @@ -128,18 +128,20 @@ impl IslandManager { } } } - (Some(handle1), None) => { - if wake_up { - self.wake_up(bodies, handle1, false); - } + } + (Some(handle1), None) => { + if wake_up { + // NOTE: see NOTE of the Some(_), Some(_) case. + self.wake_up(bodies, handle1, false); } - (None, Some(handle2)) => { - if wake_up { - self.wake_up(bodies, handle2, false); - } + } + (None, Some(handle2)) => { + if wake_up { + // NOTE: see NOTE of the Some(_), Some(_) case. + self.wake_up(bodies, handle2, false); } - (None, None) => { /* Nothing to do. */ } } + (None, None) => { /* Nothing to do. */ } } } @@ -224,9 +226,7 @@ impl IslandManager { narrow_phase: &NarrowPhase, impulse_joints: &ImpulseJointSet, multibody_joints: &MultibodyJointSet, - min_island_size: usize, ) { - self.timestamp += 1; // 1. Update active rigid-bodies energy. // TODO PERF: should this done by the velocity solver after solving the constraints? // let t0 = std::time::Instant::now(); @@ -241,8 +241,6 @@ impl IslandManager { }; let sq_linvel = rb.vels.linvel.norm_squared(); let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); - let can_sleep_before = rb.activation.is_eligible_for_sleep(); - rb.activation .update_energy(rb.body_type, length_unit, sq_linvel, sq_angvel, dt); @@ -250,16 +248,16 @@ impl IslandManager { // 2. Identify active rigid-bodies that transition from "awake" to "can_sleep" // and push the sleep root candidate if applicable. - if !can_sleep_before && can_sleep_now { + if can_sleep_now && rb.activation.sleep_root_state == SleepRootState::Unknown { // This is a new candidate for island extraction. - if !rb.activation.is_sleep_root_candidate { - self.traversal_candidates.push_back(SleepCandidate(handle)); - } + self.traversal_candidates.push_back(SleepCandidate(handle)); + rb.activation.sleep_root_state = SleepRootState::TraversalPending; + } else if !can_sleep_now { + rb.activation.sleep_root_state = SleepRootState::Unknown; } } // println!("Update energy: {}", t0.elapsed().as_secs_f32() * 1000.0); - let frame_base_timestamp = self.timestamp; let mut cost = 0; // 3. Perform one, or multiple, sleeping islands extraction (graph traversal). @@ -267,11 +265,6 @@ impl IslandManager { // there are too many. const MAX_PER_FRAME_COST: usize = 1000; // TODO: find the best value. while let Some(sleep_root) = self.traversal_candidates.pop_front() { - if cost > MAX_PER_FRAME_COST { - // Early-break if we consider we have done enough island extraction work. - break; - } - cost += self.extract_sleeping_island( bodies, colliders, @@ -279,10 +272,12 @@ impl IslandManager { multibody_joints, narrow_phase, sleep_root.0, - frame_base_timestamp, ); - self.timestamp += 1; + if cost > MAX_PER_FRAME_COST { + // Early-break if we consider we have done enough island extraction work. + break; + } } self.update_optimizer( @@ -294,7 +289,7 @@ impl IslandManager { ); // println!("Island extraction: {}", t0.elapsed().as_secs_f32() * 1000.0); - #[cfg(debug_assertions)] - self.assert_state_is_valid(bodies); + // NOTE: uncomment for debugging. + // self.assert_state_is_valid(bodies, colliders, narrow_phase); } } diff --git a/src/dynamics/island_manager/mod.rs b/src/dynamics/island_manager/mod.rs index 4ace3cf0f..93f9c8f54 100644 --- a/src/dynamics/island_manager/mod.rs +++ b/src/dynamics/island_manager/mod.rs @@ -1,7 +1,7 @@ pub use manager::IslandManager; pub(crate) use island::Island; -pub(self) use optimizer::IslandsOptimizer; +use optimizer::IslandsOptimizer; mod island; mod manager; diff --git a/src/dynamics/island_manager/optimizer.rs b/src/dynamics/island_manager/optimizer.rs index 71b723fb9..fab096984 100644 --- a/src/dynamics/island_manager/optimizer.rs +++ b/src/dynamics/island_manager/optimizer.rs @@ -157,7 +157,7 @@ impl IslandManager { // TODO: implement islands recycling to avoid reallocating every time. let mut new_island = Island::default(); - self.timestamp += 1; + self.traversal_timestamp += 1; for root in &self.islands[id].bodies { self.stack.push(*root); @@ -170,12 +170,12 @@ impl IslandManager { continue; } - if rb.ids.active_set_timestamp == self.timestamp { + if rb.ids.active_set_timestamp == self.traversal_timestamp { // We already visited this body. continue; } - rb.ids.active_set_timestamp = self.timestamp; + rb.ids.active_set_timestamp = self.traversal_timestamp; assert!(!rb.activation.sleeping); // Traverse bodies that are interacting with the current one either through diff --git a/src/dynamics/island_manager/sleep.rs b/src/dynamics/island_manager/sleep.rs index 4f2e74c39..a15a9846d 100644 --- a/src/dynamics/island_manager/sleep.rs +++ b/src/dynamics/island_manager/sleep.rs @@ -1,4 +1,6 @@ -use crate::dynamics::{ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet}; +use crate::dynamics::{ + ImpulseJointSet, MultibodyJointSet, RigidBodyHandle, RigidBodySet, SleepRootState, +}; use crate::geometry::{ColliderSet, NarrowPhase}; use super::{Island, IslandManager}; @@ -56,19 +58,19 @@ impl IslandManager { multibody_joints: &MultibodyJointSet, narrow_phase: &NarrowPhase, sleep_root: RigidBodyHandle, - frame_base_timestamp: u32, ) -> usize { let Some(rb) = bodies.get_mut_internal(sleep_root) else { // This branch happens if the rigid-body no longer exists. return 0; }; - rb.activation.is_sleep_root_candidate = false; - - if rb.ids.active_set_timestamp >= frame_base_timestamp { - return 0; // We already traversed this rigid-body this frame. + if rb.activation.sleep_root_state != SleepRootState::TraversalPending { + // We already traversed this sleep root. + return 0; } + rb.activation.sleep_root_state = SleepRootState::Traversed; + let active_island_id = rb.ids.active_island_id; let active_island = &mut self.islands[active_island_id]; if active_island.is_sleeping() { @@ -81,8 +83,9 @@ impl IslandManager { self.stack.clear(); self.stack.push(sleep_root); - let mut can_sleep = true; let mut niter = 0; + self.traversal_timestamp += 1; + while let Some(handle) = self.stack.pop() { let rb = bodies.index_mut_internal(handle); @@ -91,30 +94,41 @@ impl IslandManager { continue; } - if rb.ids.active_set_timestamp == self.timestamp { + if rb.ids.active_set_timestamp == self.traversal_timestamp { // We already visited this body and its neighbors. continue; } - if rb.ids.active_set_timestamp >= frame_base_timestamp { - // We already visited this body and its neighbors during this frame. - // So we already know this islands cannot sleep (otherwise the bodies - // currently being traversed would already have been marked as sleeping). - return niter; - } + // if rb.ids.active_set_timestamp >= frame_base_timestamp { + // // We already visited this body and its neighbors during this frame. + // // So we already know this islands cannot sleep (otherwise the bodies + // // currently being traversed would already have been marked as sleeping). + // return niter; + // } niter += 1; - rb.ids.active_set_timestamp = self.timestamp; + rb.ids.active_set_timestamp = self.traversal_timestamp; + + if rb.activation.is_eligible_for_sleep() { + rb.activation.sleep_root_state = SleepRootState::Traversed; + } - assert!(!rb.activation.sleeping); assert_eq!( - rb.ids.active_island_id, active_island_id, - "note niter: {}", - niter + rb.ids.active_island_id, + active_island_id, + "handle: {:?}, note niter: {}, isl size: {}", + handle, + niter, + active_island.len() + ); + assert!( + !rb.activation.sleeping, + "is sleeping: {:?} note niter: {}, isl size: {}", + handle, + niter, + active_island.len() ); - // TODO PERF: early-exit as soon as we reach a body not eligible to sleep. - can_sleep = can_sleep && rb.activation().is_eligible_for_sleep(); if !rb.activation.is_eligible_for_sleep() { // If this body cannot sleep, abort the traversal, we are not traversing // yet an island that can sleep. @@ -139,8 +153,6 @@ impl IslandManager { new_island.bodies.push(handle); } - assert!(can_sleep); - // If we reached this line, we completed a sleeping island traversal. // - Put its bodies to sleep. // - Remove them from the active set. diff --git a/src/dynamics/island_manager/utils.rs b/src/dynamics/island_manager/utils.rs index c596df8b4..d9db20870 100644 --- a/src/dynamics/island_manager/utils.rs +++ b/src/dynamics/island_manager/utils.rs @@ -11,16 +11,13 @@ pub(super) fn push_contacting_bodies( ) { for collider_handle in &rb_colliders.0 { for inter in narrow_phase.contact_pairs_with(*collider_handle) { - for manifold in &inter.manifolds { - if !manifold.data.solver_contacts.is_empty() { - let other = crate::utils::select_other( - (inter.collider1, inter.collider2), - *collider_handle, - ); - if let Some(other_body) = colliders[other].parent { - stack.push(other_body.handle); - } - break; + if inter.has_any_active_contact() { + let other = crate::utils::select_other( + (inter.collider1, inter.collider2), + *collider_handle, + ); + if let Some(other_body) = colliders[other].parent { + stack.push(other_body.handle); } } } diff --git a/src/dynamics/island_manager/validation.rs b/src/dynamics/island_manager/validation.rs index 99568d720..fe912c4f1 100644 --- a/src/dynamics/island_manager/validation.rs +++ b/src/dynamics/island_manager/validation.rs @@ -1,8 +1,15 @@ use crate::dynamics::{IslandManager, RigidBodySet}; +use crate::geometry::NarrowPhase; +use crate::prelude::ColliderSet; impl IslandManager { - #[cfg(debug_assertions)] - pub fn assert_state_is_valid(&self, bodies: &RigidBodySet) { + #[allow(dead_code)] + pub(super) fn assert_state_is_valid( + &self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + nf: &NarrowPhase, + ) { for (island_id, island) in self.islands.iter() { // Sleeping island must not be in the awake list. if island.is_sleeping() { @@ -10,7 +17,7 @@ impl IslandManager { } else { // If the island is awake, the awake id must match. let awake_id = island.id_in_awake_list.unwrap(); - self.awake_islands[awake_id] == island_id; + assert_eq!(self.awake_islands[awake_id], island_id); } for (body_id, handle) in island.bodies.iter().enumerate() { @@ -37,6 +44,27 @@ impl IslandManager { awake_islands_dedup.dedup(); assert_eq!(self.awake_islands.len(), awake_islands_dedup.len()); + // If two bodies have solver contacts, they must be in the same island. + for pair in nf.contact_pairs() { + let Some(body_handle1) = colliders[pair.collider1].parent.map(|p| p.handle) else { + continue; + }; + let Some(body_handle2) = colliders[pair.collider2].parent.map(|p| p.handle) else { + continue; + }; + + let body1 = &bodies[body_handle1]; + let body2 = &bodies[body_handle2]; + + if body1.is_fixed() || body2.is_fixed() { + continue; + } + + if pair.has_any_active_contact() { + assert_eq!(body1.ids.active_island_id, body2.ids.active_island_id); + } + } + println!( "`IslandManager::assert_state_is_valid` validation checks passed. This is slow. Only enable for debugging." ); diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index 7e2e0efee..b07ee6282 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1138,6 +1138,19 @@ impl RigidBodyDominance { } } +#[derive(Copy, Clone, PartialEq, Eq, Debug, Default)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) enum SleepRootState { + /// This sleep root has already been traversed. No need to traverse + /// again until the rigid-body either gets awaken by an event. + Traversed, + /// This sleep root has not been traversed yet. + TraversalPending, + /// This body can become a sleep root once it falls asleep. + #[default] + Unknown, +} + /// Controls when a body goes to sleep (becomes inactive to save CPU). /// /// ## Sleeping System @@ -1184,9 +1197,7 @@ pub struct RigidBodyActivation { /// Is this body currently sleeping? pub sleeping: bool, - /// If this is `true` then the island manager has already registered that this rigid-body - /// can be used as a root for graph traversal to potentially extract a sleeping island. - pub is_sleep_root_candidate: bool, + pub(crate) sleep_root_state: SleepRootState, } impl Default for RigidBodyActivation { @@ -1220,7 +1231,7 @@ impl RigidBodyActivation { time_until_sleep: Self::default_time_until_sleep(), time_since_can_sleep: 0.0, sleeping: false, - is_sleep_root_candidate: false, + sleep_root_state: SleepRootState::Unknown, } } @@ -1232,7 +1243,7 @@ impl RigidBodyActivation { time_until_sleep: Self::default_time_until_sleep(), time_since_can_sleep: Self::default_time_until_sleep(), sleeping: true, - is_sleep_root_candidate: false, + sleep_root_state: SleepRootState::Unknown, } } @@ -1255,6 +1266,12 @@ impl RigidBodyActivation { #[inline] pub fn wake_up(&mut self, strong: bool) { self.sleeping = false; + + // Make this body eligible as a sleep root again. + if self.sleep_root_state != SleepRootState::TraversalPending { + self.sleep_root_state = SleepRootState::Unknown; + } + if strong { self.time_since_can_sleep = 0.0; } diff --git a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs index e9a55321a..4d46d7aae 100644 --- a/src/dynamics/solver/contact_constraint/contact_constraints_set.rs +++ b/src/dynamics/solver/contact_constraint/contact_constraints_set.rs @@ -11,10 +11,9 @@ use crate::dynamics::{ RigidBodySet, }; use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::Real; use crate::math::SIMD_WIDTH; -use crate::math::{MAX_MANIFOLD_POINTS, Real}; use na::DVector; -use parry::math::DIM; use crate::dynamics::solver::contact_constraint::any_contact_constraint::AnyContactConstraintMut; #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index c8c395206..96aa56e27 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -43,7 +43,10 @@ impl IslandSolver { multibodies: &mut MultibodyJointSet, ) { counters.solver.velocity_assembly_time.resume(); - counters.solver.velocity_assembly_time_a.resume(); + counters + .solver + .velocity_assembly_time_solver_bodies + .resume(); let num_solver_iterations = base_params.num_solver_iterations + islands.island(island_id).additional_solver_iterations(); @@ -65,8 +68,11 @@ impl IslandSolver { bodies, multibodies, ); - counters.solver.velocity_assembly_time_a.pause(); - counters.solver.velocity_assembly_time_b.resume(); + counters.solver.velocity_assembly_time_solver_bodies.pause(); + counters + .solver + .velocity_assembly_time_constraints_init + .resume(); self.velocity_solver.init_constraints( island_id, islands, @@ -81,7 +87,10 @@ impl IslandSolver { #[cfg(feature = "dim3")] params.friction_model, ); - counters.solver.velocity_assembly_time_b.pause(); + counters + .solver + .velocity_assembly_time_constraints_init + .pause(); counters.solver.velocity_assembly_time.pause(); // SOLVE diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index 243f5a766..dd530c1bd 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -155,8 +155,6 @@ pub struct ContactPair { /// [`Collider::contact_skin`] which only affects the constraint solver and the /// [`SolverContact`]. pub manifolds: Vec, - /// Is there any active contact in this contact pair? - pub has_any_active_contact: bool, /// Was a `CollisionEvent::Started` emitted for this collider? pub(crate) start_event_emitted: bool, pub(crate) workspace: Option, @@ -173,17 +171,22 @@ impl ContactPair { Self { collider1, collider2, - has_any_active_contact: false, manifolds: Vec::new(), start_event_emitted: false, workspace: None, } } + /// Is there any active contact in this contact pair? + pub fn has_any_active_contact(&self) -> bool { + self.manifolds + .iter() + .any(|m| !m.data.solver_contacts.is_empty()) + } + /// Clears all the contacts of this contact pair. pub fn clear(&mut self) { self.manifolds.clear(); - self.has_any_active_contact = false; self.workspace = None; } diff --git a/src/geometry/manifold_reduction.rs b/src/geometry/manifold_reduction.rs index 4244ba1df..027037b27 100644 --- a/src/geometry/manifold_reduction.rs +++ b/src/geometry/manifold_reduction.rs @@ -1,5 +1,5 @@ use crate::geometry::ContactManifold; -use crate::math::{Point, Real}; +use crate::math::Real; use crate::utils::SimdBasis; pub(crate) fn reduce_manifold_naive( @@ -12,8 +12,6 @@ pub(crate) fn reduce_manifold_naive( return; } - // Run contact reduction so we only have up to four solver contacts. - // We follow a logic similar to Bepu’s approach. // 1. Find the deepest contact. *selected = [usize::MAX; 4]; @@ -32,9 +30,9 @@ pub(crate) fn reduce_manifold_naive( // 2. Find the point that is the furthest from the deepest point. let selected_a = &manifold.points[selected[0]].local_p1; - let mut furthest_dist = -f32::MAX; + let mut furthest_dist = -Real::MAX; for (i, pt) in manifold.points.iter().enumerate() { - let dist = na::distance_squared(&pt.local_p1, &selected_a); + let dist = na::distance_squared(&pt.local_p1, selected_a); if i != selected[0] && pt.dist <= prediction_distance && dist > furthest_dist { furthest_dist = dist; selected[1] = i; @@ -87,10 +85,10 @@ pub(crate) fn reduce_manifold_naive( } // Run contact reduction using Bepu's InternalReduce algorithm. -// This reduces the manifold to at most 4 contacts using: -// 1. Deepest contact (with extremity bias for stability) -// 2. Most distant contact from the first -// 3. Two contacts that maximize positive and negative signed area with the first edge +// The general idea is quite similar to our naive approach except that they add some +// additional heuristics. This is implemented mainly for comparison purpose to see +// if there is a strong advantage to having the extra checks. +#[allow(dead_code)] pub(crate) fn reduce_manifold_bepu_like( manifold: &ContactManifold, selected: &mut [usize; 4], @@ -106,8 +104,8 @@ pub(crate) fn reduce_manifold_bepu_like( let mut best_score = -Real::MAX; const EXTREMITY_SCALE: Real = 1e-2; // Use an arbitrary direction (roughly 38 degrees from X axis) to break ties - const EXTREMITY_DIR_X: Real = 0.7946897654; - const EXTREMITY_DIR_Y: Real = 0.60701579614; + const EXTREMITY_DIR_X: Real = 0.7946898; + const EXTREMITY_DIR_Y: Real = 0.6070158; let tangents = manifold.local_n1.orthonormal_basis(); diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index f3d5c5d80..fef653f3c 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -7,11 +7,10 @@ use crate::dynamics::{ CoefficientCombineRule, ImpulseJointSet, IslandManager, RigidBodyDominance, RigidBodySet, RigidBodyType, }; -use crate::geometry::manifold_reduction::reduce_manifold_bepu_like; use crate::geometry::{ BoundingVolume, BroadPhasePairEvent, ColliderChanges, ColliderGraphIndex, ColliderHandle, ColliderPair, ColliderSet, CollisionEvent, ContactData, ContactManifold, ContactManifoldData, - ContactPair, InteractionGraph, IntersectionPair, Segment, SolverContact, SolverFlags, + ContactPair, InteractionGraph, IntersectionPair, SolverContact, SolverFlags, TemporaryInteractionIndex, }; use crate::math::{MAX_MANIFOLD_POINTS, Real, Vector}; @@ -583,7 +582,7 @@ impl NarrowPhase { // Emit a contact stopped event if we had a contact before removing the edge. // Also wake up the dynamic bodies that were in contact. if let Some(mut ctct) = contact_pair { - if ctct.has_any_active_contact { + if ctct.has_any_active_contact() { if let Some(islands) = islands { if let Some(co_parent1) = &co1.parent { islands.wake_up(bodies, co_parent1.handle, true); @@ -819,10 +818,10 @@ impl NarrowPhase { ) { let query_dispatcher = &*self.query_dispatcher; - // TODO: don't iterate on all the edges. + // TODO PERF: don't iterate on all the edges. par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { let pair = &mut edge.weight; - let had_any_active_contact = pair.has_any_active_contact; + let had_any_active_contact = pair.has_any_active_contact(); let co1 = &colliders[pair.collider1]; let co2 = &colliders[pair.collider2]; @@ -833,6 +832,7 @@ impl NarrowPhase { // No update needed for these colliders. return; } + if co1.parent.map(|p| p.handle) == co2.parent.map(|p| p.handle) && co1.parent.is_some() { @@ -990,8 +990,6 @@ impl NarrowPhase { let dominance1 = rb1.map(|rb| rb.dominance).unwrap_or(zero); let dominance2 = rb2.map(|rb| rb.dominance).unwrap_or(zero); - pair.has_any_active_contact = false; - for manifold in &mut pair.manifolds { let world_pos1 = manifold.subshape_pos1.prepend_to(&co1.pos); let world_pos2 = manifold.subshape_pos2.prepend_to(&co2.pos); @@ -1004,22 +1002,24 @@ impl NarrowPhase { manifold.data.normal = world_pos1 * manifold.local_n1; // Generate solver contacts. + #[allow(unused_mut)] // Mut not needed in 2D. let mut selected = [0, 1, 2, 3]; + #[allow(unused_mut)] // Mut not needed in 2D. let mut num_selected = MAX_MANIFOLD_POINTS.min(manifold.points.len()); #[cfg(feature = "dim3")] - super::manifold_reduction::reduce_manifold_bepu_like( - manifold, - &mut selected, - &mut num_selected, - ); - // #[cfg(feature = "dim3")] - // super::manifold_reduction::reduce_manifold_naive( + // super::manifold_reduction::reduce_manifold_bepu_like( // manifold, // &mut selected, // &mut num_selected, - // prediction_distance, // ); + #[cfg(feature = "dim3")] + super::manifold_reduction::reduce_manifold_naive( + manifold, + &mut selected, + &mut num_selected, + prediction_distance, + ); for contact_id in &selected[..num_selected] { // // manifold.points.iter().enumerate() { @@ -1065,7 +1065,6 @@ impl NarrowPhase { }; manifold.data.solver_contacts.push(solver_contact); - pair.has_any_active_contact = true; } } @@ -1095,35 +1094,6 @@ impl NarrowPhase { manifold.data.normal = modifiable_normal; manifold.data.user_data = modifiable_user_data; } - - /* - * TODO: When using the block solver in 3D, I’d expect this sort to help, but - * it makes the domino demo worse. Needs more investigation. - fn sort_solver_contacts(mut contacts: &mut [SolverContact]) { - while contacts.len() > 2 { - let first = contacts[0]; - let mut furthest_id = 1; - let mut furthest_dist = na::distance(&first.point, &contacts[1].point); - - for (candidate_id, candidate) in contacts.iter().enumerate().skip(2) { - let candidate_dist = na::distance(&first.point, &candidate.point); - - if candidate_dist > furthest_dist { - furthest_dist = candidate_dist; - furthest_id = candidate_id; - } - } - - if furthest_id > 1 { - contacts.swap(1, furthest_id); - } - - contacts = &mut contacts[2..]; - } - } - - sort_solver_contacts(&mut manifold.data.solver_contacts); - */ } } @@ -1132,10 +1102,11 @@ impl NarrowPhase { * - Emit event (if applicable). * - Notify the island manager to potentially wake up the bodies. */ - if pair.has_any_active_contact != had_any_active_contact { + let has_any_active_contact = pair.has_any_active_contact(); + if has_any_active_contact != had_any_active_contact { let active_events = co1.flags.active_events | co2.flags.active_events; if active_events.contains(ActiveEvents::COLLISION_EVENTS) { - if pair.has_any_active_contact { + if has_any_active_contact { pair.emit_start_event(bodies, colliders, events); } else { pair.emit_stop_event(bodies, colliders, events); @@ -1147,7 +1118,7 @@ impl NarrowPhase { bodies, co1.parent.map(|p| p.handle), co2.parent.map(|p| p.handle), - pair.has_any_active_contact, + has_any_active_contact, true, ); } diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs index a8c346c56..cbd086bfb 100644 --- a/src/pipeline/physics_pipeline.rs +++ b/src/pipeline/physics_pipeline.rs @@ -196,7 +196,7 @@ impl PhysicsPipeline { events: &dyn EventHandler, ) { self.counters.stages.island_construction_time.resume(); - // NOTE: this must be done after the narrow-phase. + // NOTE: islands update must be done after the narrow-phase. islands.update_islands( integration_parameters.dt, integration_parameters.length_unit, @@ -205,7 +205,6 @@ impl PhysicsPipeline { narrow_phase, impulse_joints, multibody_joints, - integration_parameters.min_island_size, ); let num_active_islands = islands.active_islands().len(); diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs index 3c7e1a0d4..1f486c0c3 100644 --- a/src_testbed/physx_backend.rs +++ b/src_testbed/physx_backend.rs @@ -134,7 +134,7 @@ pub static FOUNDATION: std::cell::RefCell = std::cell::RefC pub struct PhysxWorld { // physics: Physics, - materials: Vec>, + // materials: Vec>, shapes: Vec>, scene: Option>, } @@ -162,7 +162,6 @@ impl PhysxWorld { FOUNDATION.with(|physics| { let mut physics = physics.borrow_mut(); let mut shapes = Vec::new(); - let mut materials = Vec::new(); let friction_type = if use_two_friction_directions { FrictionType::TwoDirectional @@ -399,7 +398,6 @@ impl PhysxWorld { Self { scene: Some(scene), shapes, - materials, } }) } diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index be95082e7..ff0f1dc02 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -337,12 +337,18 @@ fn profiling_ui(ui: &mut Ui, counters: &Counters) { counters.solver.velocity_assembly_time.time_ms() )); ui.label(format!( - "> Velocity assembly A: {:.2}ms", - counters.solver.velocity_assembly_time_a.time_ms() + "> Velocity assembly - solver bodies: {:.2}ms", + counters + .solver + .velocity_assembly_time_solver_bodies + .time_ms() )); ui.label(format!( - "> Velocity assembly B: {:.2}ms", - counters.solver.velocity_assembly_time_b.time_ms() + "> Velocity assembly - constraints init: {:.2}ms", + counters + .solver + .velocity_assembly_time_constraints_init + .time_ms() )); ui.label(format!( "Velocity resolution: {:.2}ms", From 0044d5d6536fab202d22e5a6e42853da6a6d2efa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 7 Nov 2025 18:37:04 +0100 Subject: [PATCH 12/12] chore: remove redundant active_set_offset field --- src/dynamics/island_manager/island.rs | 3 -- src/dynamics/island_manager/manager.rs | 7 +-- src/dynamics/island_manager/validation.rs | 3 +- src/dynamics/rigid_body.rs | 2 +- src/dynamics/rigid_body_components.rs | 2 - .../contact_with_coulomb_friction.rs | 4 +- .../contact_with_twist_friction.rs | 4 +- .../generic_contact_constraint.rs | 4 +- src/dynamics/solver/interaction_groups.rs | 46 ++++++++----------- .../joint_constraint_builder.rs | 4 +- src/dynamics/solver/velocity_solver.rs | 11 ++--- 11 files changed, 34 insertions(+), 56 deletions(-) diff --git a/src/dynamics/island_manager/island.rs b/src/dynamics/island_manager/island.rs index c268b9094..f9cd27b53 100644 --- a/src/dynamics/island_manager/island.rs +++ b/src/dynamics/island_manager/island.rs @@ -77,7 +77,6 @@ impl IslandManager { ); rb.ids.active_island_id = new_island_id; rb.ids.active_set_id = id; - rb.ids.active_set_offset = id as u32; new_island.additional_solver_iterations = new_island .additional_solver_iterations @@ -87,7 +86,6 @@ impl IslandManager { if let Some(moved_handle) = source_island.bodies.get(id_to_remove).copied() { let moved_rb = bodies.index_mut_internal(moved_handle); moved_rb.ids.active_set_id = id_to_remove; - moved_rb.ids.active_set_offset = id_to_remove as u32; } } @@ -147,7 +145,6 @@ impl IslandManager { rb.wake_up(false); rb.ids.active_island_id = to_keep; rb.ids.active_set_id = target_island.bodies.len(); - rb.ids.active_set_offset = (target_island.bodies.len()) as u32; target_island.bodies.push(*handle); target_island.additional_solver_iterations = target_island .additional_solver_iterations diff --git a/src/dynamics/island_manager/manager.rs b/src/dynamics/island_manager/manager.rs index ed680678b..196d5f31b 100644 --- a/src/dynamics/island_manager/manager.rs +++ b/src/dynamics/island_manager/manager.rs @@ -67,16 +67,13 @@ impl IslandManager { }; let swapped_handle = island.bodies.last().copied().unwrap_or(removed_handle); - island - .bodies - .swap_remove(removed_ids.active_set_offset as usize); + island.bodies.swap_remove(removed_ids.active_set_id); // Remap the active_set_id of the body we moved with the `swap_remove`. if swapped_handle != removed_handle { let swapped_body = bodies .get_mut(swapped_handle) .expect("Internal error: bodies must be removed from islands on at a times"); - swapped_body.ids.active_set_offset = removed_ids.active_set_offset; swapped_body.ids.active_set_id = removed_ids.active_set_id; } @@ -187,7 +184,6 @@ impl IslandManager { rb.ids.active_island_id = id; rb.ids.active_set_id = target_island.bodies.len(); - rb.ids.active_set_offset = target_island.bodies.len() as u32; target_island.bodies.push(handle); } else { let mut new_island = Island::singleton(handle, rb); @@ -201,7 +197,6 @@ impl IslandManager { self.islands.insert(id, new_island); rb.ids.active_island_id = id; rb.ids.active_set_id = 0; - rb.ids.active_set_offset = 0; } } diff --git a/src/dynamics/island_manager/validation.rs b/src/dynamics/island_manager/validation.rs index fe912c4f1..e6e565ba1 100644 --- a/src/dynamics/island_manager/validation.rs +++ b/src/dynamics/island_manager/validation.rs @@ -26,8 +26,7 @@ impl IslandManager { assert_eq!(rb.is_sleeping(), island.is_sleeping()); // The body’s island id must match the island id. assert_eq!(rb.ids.active_island_id, island_id); - // The body’s offset must match its handle’s position in island.bodies. - assert_eq!(body_id, rb.ids.active_set_offset as usize); + // The body’s active set id must match its handle’s position in island.bodies. assert_eq!(body_id, rb.ids.active_set_id); } } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 63e394a4f..31f4065e4 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -665,7 +665,7 @@ impl RigidBody { // to all the fixed bodies active set offsets? pub fn effective_active_set_offset(&self) -> u32 { if self.is_dynamic_or_kinematic() { - self.ids.active_set_offset + self.ids.active_set_id as u32 } else { u32::MAX } diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index b07ee6282..899efa13a 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -1029,7 +1029,6 @@ impl RigidBodyCcd { pub struct RigidBodyIds { pub(crate) active_island_id: usize, pub(crate) active_set_id: usize, - pub(crate) active_set_offset: u32, pub(crate) active_set_timestamp: u32, } @@ -1038,7 +1037,6 @@ impl Default for RigidBodyIds { Self { active_island_id: usize::MAX, active_set_id: usize::MAX, - active_set_offset: u32::MAX, active_set_timestamp: 0, } } diff --git a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs index 09dd92908..1a9f7387c 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs @@ -52,7 +52,7 @@ impl ContactWithCoulombFrictionBuilder { && manifold_id[ii] != usize::MAX { let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check. - bodies[handle].ids.active_set_offset + bodies[handle].ids.active_set_id as u32 } else { u32::MAX }]; @@ -60,7 +60,7 @@ impl ContactWithCoulombFrictionBuilder { && manifold_id[ii] != usize::MAX { let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check. - bodies[handle].ids.active_set_offset + bodies[handle].ids.active_set_id as u32 } else { u32::MAX }]; diff --git a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs index 1fce11fb3..399f57eb3 100644 --- a/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs +++ b/src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs @@ -59,7 +59,7 @@ impl ContactWithTwistFrictionBuilder { && manifold_id[ii] != usize::MAX { let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check. - bodies[handle].ids.active_set_offset + bodies[handle].ids.active_set_id as u32 } else { u32::MAX }]; @@ -67,7 +67,7 @@ impl ContactWithTwistFrictionBuilder { && manifold_id[ii] != usize::MAX { let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check. - bodies[handle].ids.active_set_offset + bodies[handle].ids.active_set_id as u32 } else { u32::MAX }]; diff --git a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs index eed4bacf6..15f348a1d 100644 --- a/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs +++ b/src/dynamics/solver/contact_constraint/generic_contact_constraint.rs @@ -68,7 +68,7 @@ impl GenericContactConstraintBuilder { multibody1 .map(|mb| mb.0.solver_id) .unwrap_or(if type1.is_dynamic_or_kinematic() { - rb1.ids.active_set_offset + rb1.ids.active_set_id as u32 } else { u32::MAX }); @@ -76,7 +76,7 @@ impl GenericContactConstraintBuilder { multibody2 .map(|mb| mb.0.solver_id) .unwrap_or(if type2.is_dynamic_or_kinematic() { - rb2.ids.active_set_offset + rb2.ids.active_set_id as u32 } else { u32::MAX }); diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs index d537d894c..80d1457da 100644 --- a/src/dynamics/solver/interaction_groups.rs +++ b/src/dynamics/solver/interaction_groups.rs @@ -117,26 +117,26 @@ impl ParallelInteractionGroups { (false, false) => { let rb1 = &bodies[body_pair.0.unwrap()]; let rb2 = &bodies[body_pair.1.unwrap()]; - let color_mask = bcolors[rb1.ids.active_set_offset as usize] - | bcolors[rb2.ids.active_set_offset as usize]; + let color_mask = + bcolors[rb1.ids.active_set_id] | bcolors[rb2.ids.active_set_id]; *color = (!color_mask).trailing_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color; - bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color; + bcolors[rb1.ids.active_set_id] |= 1 << *color; + bcolors[rb2.ids.active_set_id] |= 1 << *color; } (true, false) => { let rb2 = &bodies[body_pair.1.unwrap()]; - let color_mask = bcolors[rb2.ids.active_set_offset as usize]; + let color_mask = bcolors[rb2.ids.active_set_id]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color; + bcolors[rb2.ids.active_set_id] |= 1 << *color; } (false, true) => { let rb1 = &bodies[body_pair.0.unwrap()]; - let color_mask = bcolors[rb1.ids.active_set_offset as usize]; + let color_mask = bcolors[rb1.ids.active_set_id]; *color = 127 - (!color_mask).leading_zeros() as usize; color_len[*color] += 1; - bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color; + bcolors[rb1.ids.active_set_id] |= 1 << *color; } (true, true) => unreachable!(), } @@ -270,18 +270,10 @@ impl InteractionGroups { } let ijoint = interaction.data.locked_axes.bits() as usize; - let i1 = rb1.ids.active_set_offset; - let i2 = rb2.ids.active_set_offset; - let conflicts = self - .body_masks - .get(i1 as usize) - .copied() - .unwrap_or_default() - | self - .body_masks - .get(i2 as usize) - .copied() - .unwrap_or_default() + let i1 = rb1.ids.active_set_id; + let i2 = rb2.ids.active_set_id; + let conflicts = self.body_masks.get(i1).copied().unwrap_or_default() + | self.body_masks.get(i2).copied().unwrap_or_default() | joint_type_conflicts[ijoint]; let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; @@ -420,17 +412,15 @@ impl InteractionGroups { continue; } - let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1 - { + let (status1, active_set_id1) = if let Some(rb1) = interaction.data.rigid_body1 { let rb1 = &bodies[rb1]; - (rb1.body_type, rb1.ids.active_set_offset) + (rb1.body_type, rb1.ids.active_set_id as u32) } else { (RigidBodyType::Fixed, u32::MAX) }; - let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2 - { + let (status2, active_set_id2) = if let Some(rb2) = interaction.data.rigid_body2 { let rb2 = &bodies[rb2]; - (rb2.body_type, rb2.ids.active_set_offset) + (rb2.body_type, rb2.ids.active_set_id as u32) } else { (RigidBodyType::Fixed, u32::MAX) }; @@ -443,8 +433,8 @@ impl InteractionGroups { continue; } - let i1 = active_set_offset1; - let i2 = active_set_offset2; + let i1 = active_set_id1; + let i2 = active_set_id2; let mask1 = if !is_fixed1 { self.body_masks[i1 as usize] } else { diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 2dc702d10..52f8de44e 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -123,12 +123,12 @@ impl JointConstraintBuilderSimd { let rb2 = array![|ii| &bodies[joint[ii].body2]]; let body1 = array![|ii| if rb1[ii].is_dynamic_or_kinematic() { - rb1[ii].ids.active_set_offset + rb1[ii].ids.active_set_id as u32 } else { u32::MAX }]; let body2 = array![|ii| if rb2[ii].is_dynamic_or_kinematic() { - rb2[ii].ids.active_set_offset + rb2[ii].ids.active_set_id as u32 } else { u32::MAX }]; diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index b718f2140..21845cbea 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -111,11 +111,10 @@ impl VelocitySolver { } } else { let rb = &bodies[*handle]; - assert_eq!(offset, rb.ids.active_set_offset as usize); - let solver_vel_incr = - &mut self.solver_vels_increment[rb.ids.active_set_offset as usize]; + assert_eq!(offset, rb.ids.active_set_id); + let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_id]; self.solver_bodies - .copy_from(total_step_dt, rb.ids.active_set_offset as usize, rb); + .copy_from(total_step_dt, rb.ids.active_set_id, rb); solver_vel_incr.angular = rb.mprops.effective_world_inv_inertia * rb.forces.torque * params.dt; @@ -312,8 +311,8 @@ impl VelocitySolver { } } else { let rb = bodies.index_mut_internal(*handle); - let solver_vels = &self.solver_bodies.vels[rb.ids.active_set_offset as usize]; - let solver_poses = &self.solver_bodies.poses[rb.ids.active_set_offset as usize]; + let solver_vels = &self.solver_bodies.vels[rb.ids.active_set_id]; + let solver_poses = &self.solver_bodies.poses[rb.ids.active_set_id]; let dangvel = solver_vels.angular;