Profiling support (#743)

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,