Finalize refactoring

1 files changed, 15 insertions, 36 deletions

diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs
index 5b062e0..ab34a42 100644
--- a/src/dynamics/solver/parallel_velocity_solver.rs
+++ b/src/dynamics/solver/parallel_velocity_solver.rs
@@ -2,8 +2,7 @@ use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadC
 use crate::concurrent_loop;
 use crate::dynamics::{
     solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge,
-    MultibodyJointSet, RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps,
-    RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
+    MultibodyJointSet, RigidBodySet,
 };
 use crate::geometry::ContactManifold;
 use crate::math::Real;
@@ -210,33 +209,22 @@ impl ParallelVelocitySolver {
                             multibody.velocities = prev_vels;
                         }
                     } else {
-                        let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
-                            bodies.index_bundle(handle.0);
-
-                        let dvel = self.mj_lambdas[rb_ids.active_set_offset];
-                        let dangvel = rb_mprops
+                        let rb = bodies.index_mut_internal(*handle);
+                        let dvel = self.mj_lambdas[rb.ids.active_set_offset];
+                        let dangvel = rb.mprops
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dvel.angular);
 
                         // Update positions.
-                        let (rb_pos, rb_vels, rb_damping, rb_mprops): (
-                            &RigidBodyPosition,
-                            &RigidBodyVelocity,
-                            &RigidBodyDamping,
-                            &RigidBodyMassProps,
-                        ) = bodies.index_bundle(handle.0);
-
-                        let mut new_pos = *rb_pos;
-                        let mut new_vels = *rb_vels;
+                        let mut new_vels = rb.vels;
                         new_vels.linvel += dvel.linear;
                         new_vels.angvel += dangvel;
-                        new_vels = new_vels.apply_damping(params.dt, rb_damping);
-                        new_pos.next_position = new_vels.integrate(
+                        new_vels = new_vels.apply_damping(params.dt, &rb.damping);
+                        rb.pos.next_position = new_vels.integrate(
                             params.dt,
-                            &rb_pos.position,
-                            &rb_mprops.local_mprops.local_com,
+                            &rb.pos.position,
+                            &rb.mprops.local_mprops.local_com,
                         );
-                        bodies.set_internal(handle.0, new_pos);
                     }
                 }
             }
@@ -321,23 +309,14 @@ impl ParallelVelocitySolver {
                             multibody.velocities += mj_lambdas;
                         }
                     } else {
-                        let (rb_ids, rb_damping, rb_mprops): (
-                            &RigidBodyIds,
-                            &RigidBodyDamping,
-                            &RigidBodyMassProps,
-                        ) = bodies.index_bundle(handle.0);
-
-                        let dvel = self.mj_lambdas[rb_ids.active_set_offset];
-                        let dangvel = rb_mprops
+                        let rb = bodies.index_mut_internal(*handle);
+                        let dvel = self.mj_lambdas[rb.ids.active_set_offset];
+                        let dangvel = rb.mprops
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dvel.angular);
-                        let damping = *rb_damping; // To avoid borrow issues.
-
-                        bodies.map_mut_internal(handle.0, |vels: &mut RigidBodyVelocity| {
-                            vels.linvel += dvel.linear;
-                            vels.angvel += dangvel;
-                            *vels = vels.apply_damping(params.dt, &damping);
-                        });
+                        rb.vels.linvel += dvel.linear;
+                        rb.vels.angvel += dangvel;
+                        rb.vels = rb.vels.apply_damping(params.dt, &rb.damping);
                     }
                 }
             }