Profiling support (#743)

src/dynamics/ccd/ccd_solver.rs

@@ -108,6 +108,7 @@ impl CCDSolver {
     }
 
     /// Find the first time a CCD-enabled body has a non-sensor collider hitting another non-sensor collider.
+    #[profiling::function]
     pub fn find_first_impact(
         &mut self,
         dt: Real,
@@ -225,6 +226,7 @@ impl CCDSolver {
     }
 
     /// Outputs the set of bodies as well as their first time-of-impact event.
+    #[profiling::function]
     pub fn predict_impacts_at_next_positions(
         &mut self,
         dt: Real,

src/dynamics/ccd/toi_entry.rs

@@ -34,6 +34,7 @@ impl TOIEntry {
         }
     }
 
+    #[profiling::function]
     pub fn try_from_colliders<QD: ?Sized + QueryDispatcher>(
         query_dispatcher: &QD,
         ch1: ColliderHandle,

src/dynamics/joint/impulse_joint/impulse_joint_set.rs

@@ -244,6 +244,7 @@ impl ImpulseJointSet {
     ///
     /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
     /// automatically woken up during the next timestep.
+    #[profiling::function]
     pub fn insert(
         &mut self,
         body1: RigidBodyHandle,
@@ -329,6 +330,7 @@ impl ImpulseJointSet {
     ///
     /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
     /// automatically woken up.
+    #[profiling::function]
     pub fn remove(&mut self, handle: ImpulseJointHandle, wake_up: bool) -> Option<ImpulseJoint> {
         let id = self.joint_ids.remove(handle.0)?;
         let endpoints = self.joint_graph.graph.edge_endpoints(id)?;
@@ -356,6 +358,7 @@ impl ImpulseJointSet {
     /// The provided rigid-body handle is not required to identify a rigid-body that
     /// is still contained by the `bodies` component set.
     /// Returns the (now invalid) handles of the removed impulse_joints.
+    #[profiling::function]
     pub fn remove_joints_attached_to_rigid_body(
         &mut self,
         handle: RigidBodyHandle,

src/dynamics/joint/multibody_joint/multibody.rs

@@ -489,6 +489,7 @@ impl Multibody {
     }
 
     /// Computes the constant terms of the dynamics.
+    #[profiling::function]
     pub(crate) fn update_dynamics(&mut self, dt: Real, bodies: &mut RigidBodySet) {
         /*
          * Compute velocities.
@@ -1094,6 +1095,7 @@ impl Multibody {
     /// is the sum of the current position of `self` and this `displacement`.
     // TODO: this shares a lot of code with `forward_kinematics` and `update_body_jacobians`, except
     //       that we are only traversing a single kinematic chain. Could this be refactored?
+    #[profiling::function]
     pub fn forward_kinematics_single_branch(
         &self,
         bodies: &RigidBodySet,

src/dynamics/joint/multibody_joint/multibody_ik.rs

@@ -64,6 +64,7 @@ impl Multibody {
     /// desired transform.
     ///
     /// The displacement calculated by this function is added to the `displacement` vector.
+    #[profiling::function]
     pub fn inverse_kinematics_delta_with_jacobian(
         jacobian: &Jacobian<Real>,
         desired_movement: &SpacialVector<Real>,
@@ -88,6 +89,7 @@ impl Multibody {
     /// can be moved through the inverse-kinematics process. Any joint for which `joint_can_move`
     /// returns `false` will have its corresponding displacement constrained to 0.
     /// Set the closure to `|_| true` if all the joints are free to move.
+    #[profiling::function]
     pub fn inverse_kinematics(
         &self,
         bodies: &RigidBodySet,

src/dynamics/joint/multibody_joint/multibody_joint.rs

@@ -86,6 +86,7 @@ impl MultibodyJoint {
     }
 
     /// Integrate the position of this multibody_joint.
+    #[profiling::function]
     pub fn integrate(&mut self, dt: Real, vels: &[Real]) {
         let locked_bits = self.data.locked_axes.bits();
         let mut curr_free_dof = 0;

src/dynamics/joint/multibody_joint/multibody_joint_set.rs

@@ -153,6 +153,7 @@ impl MultibodyJointSet {
     }
 
     /// Inserts a new multibody_joint into this set.
+    #[profiling::function]
     fn do_insert(
         &mut self,
         body1: RigidBodyHandle,
@@ -213,6 +214,7 @@ impl MultibodyJointSet {
     }
 
     /// Removes a multibody_joint from this set.
+    #[profiling::function]
     pub fn remove(&mut self, handle: MultibodyJointHandle, wake_up: bool) {
         if let Some(removed) = self.rb2mb.get(handle.0).copied() {
             let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -259,6 +261,7 @@ impl MultibodyJointSet {
     }
 
     /// Removes all the multibody_joints from the multibody the given rigid-body is part of.
+    #[profiling::function]
     pub fn remove_multibody_articulations(&mut self, handle: RigidBodyHandle, wake_up: bool) {
         if let Some(removed) = self.rb2mb.get(handle.0).copied() {
             // Remove the multibody.
@@ -281,6 +284,7 @@ impl MultibodyJointSet {
     }
 
     /// Removes all the multibody joints attached to a rigid-body.
+    #[profiling::function]
     pub fn remove_joints_attached_to_rigid_body(&mut self, rb_to_remove: RigidBodyHandle) {
         // TODO: optimize this.
         if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
@@ -412,6 +416,7 @@ impl MultibodyJointSet {
     }
 
     /// Iterates through all the joints attached to the given rigid-body.
+    #[profiling::function]
     pub fn attached_joints(
         &self,
         rb: RigidBodyHandle,
@@ -441,6 +446,7 @@ impl MultibodyJointSet {
 
     /// Iterate through the handles of all the rigid-bodies attached to this rigid-body
     /// by an enabled multibody_joint.
+    #[profiling::function]
     pub fn bodies_attached_with_enabled_joint(
         &self,
         body: RigidBodyHandle,

src/dynamics/rigid_body.rs

@@ -1016,6 +1016,7 @@ impl RigidBody {
     /// Applies an impulse at the center-of-mass of this rigid-body.
     /// The impulse is applied right away, changing the linear velocity.
     /// This does nothing on non-dynamic bodies.
+    #[profiling::function]
     pub fn apply_impulse(&mut self, impulse: Vector<Real>, wake_up: bool) {
         if !impulse.is_zero() && self.body_type == RigidBodyType::Dynamic {
             self.vels.linvel += impulse.component_mul(&self.mprops.effective_inv_mass);
@@ -1030,6 +1031,7 @@ impl RigidBody {
     /// The impulse is applied right away, changing the angular velocity.
     /// This does nothing on non-dynamic bodies.
     #[cfg(feature = "dim2")]
+    #[profiling::function]
     pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
         if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic {
             self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt
@@ -1045,6 +1047,7 @@ impl RigidBody {
     /// The impulse is applied right away, changing the angular velocity.
     /// This does nothing on non-dynamic bodies.
     #[cfg(feature = "dim3")]
+    #[profiling::function]
     pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
         if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic {
             self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt

src/dynamics/rigid_body_components.rs

@@ -624,6 +624,7 @@ impl RigidBodyVelocity {
 
     /// The kinetic energy of this rigid-body.
     #[must_use]
+    #[profiling::function]
     pub fn kinetic_energy(&self, rb_mprops: &RigidBodyMassProps) -> Real {
         let mut energy = (rb_mprops.mass() * self.linvel.norm_squared()) / 2.0;
 

src/dynamics/rigid_body_set.rs

@@ -84,6 +84,7 @@ impl RigidBodySet {
     }
 
     /// Removes a rigid-body, and all its attached colliders and impulse_joints, from these sets.
+    #[profiling::function]
     pub fn remove(
         &mut self,
         handle: RigidBodyHandle,

src/dynamics/solver/contact_constraint/contact_constraints_set.rs

@@ -454,6 +454,7 @@ impl ContactConstraintsSet {
         }
     }
 
+    #[profiling::function]
     pub fn solve_restitution(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -465,6 +466,7 @@ impl ContactConstraintsSet {
         }
     }
 
+    #[profiling::function]
     pub fn solve_restitution_wo_bias(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -477,6 +479,7 @@ impl ContactConstraintsSet {
         }
     }
 
+    #[profiling::function]
     pub fn solve_friction(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -495,6 +498,7 @@ impl ContactConstraintsSet {
         }
     }
 
+    #[profiling::function]
     pub fn update(
         &mut self,
         params: &IntegrationParameters,

src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs

@@ -272,6 +272,7 @@ impl GenericOneBodyConstraint {
         );
     }
 
+    #[profiling::function]
     pub fn solve(
         &mut self,
         jacobians: &DVector<Real>,

src/dynamics/solver/interaction_groups.rs

@@ -218,6 +218,7 @@ impl InteractionGroups {
     }
 
     #[cfg(feature = "simd-is-enabled")]
+    #[profiling::function]
     pub fn group_joints(
         &mut self,
         island_id: usize,

src/dynamics/solver/island_solver.rs

@@ -28,6 +28,7 @@ impl IslandSolver {
         }
     }
 
+    #[profiling::function]
     pub fn init_and_solve(
         &mut self,
         island_id: usize,

src/dynamics/solver/joint_constraint/joint_constraints_set.rs

@@ -361,6 +361,7 @@ impl JointConstraintsSet {
         }
     }
 
+    #[profiling::function]
     pub fn solve(
         &mut self,
         solver_vels: &mut [SolverVel<Real>],
@@ -391,6 +392,7 @@ impl JointConstraintsSet {
         }
     }
 
+    #[profiling::function]
     pub fn update(
         &mut self,
         params: &IntegrationParameters,

src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs

@@ -110,6 +110,7 @@ pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
 }
 
 impl<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
+    #[profiling::function]
     pub fn solve_generic(
         &mut self,
         solver_vel1: &mut SolverVel<N>,

src/dynamics/solver/parallel_island_solver.rs

@@ -160,6 +160,7 @@ impl ParallelIslandSolver {
         }
     }
 
+    #[profiling::function]
     pub fn init_and_solve<'s>(
         &'s mut self,
         scope: &Scope<'s>,

src/dynamics/solver/velocity_solver.rs

@@ -149,6 +149,7 @@ impl VelocitySolver {
         }
     }
 
+    #[profiling::function]
     pub fn solve_constraints(
         &mut self,
         params: &IntegrationParameters,
@@ -221,6 +222,7 @@ impl VelocitySolver {
         }
     }
 
+    #[profiling::function]
     pub fn integrate_positions(
         &mut self,
         params: &IntegrationParameters,