diff options
Diffstat (limited to 'src/dynamics/joint')
| -rw-r--r-- | src/dynamics/joint/impulse_joint/impulse_joint_set.rs | 30 | ||||
| -rw-r--r-- | src/dynamics/joint/multibody_joint/multibody.rs | 70 |
2 files changed, 41 insertions, 59 deletions
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 6b6d980..1cd177d 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -3,10 +3,7 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio use crate::data::arena::Arena; use crate::data::Coarena; -use crate::dynamics::{ - GenericJoint, IslandManager, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodySet, - RigidBodyType, -}; +use crate::dynamics::{GenericJoint, IslandManager, RigidBodyHandle, RigidBodySet}; /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. @@ -230,26 +227,17 @@ impl ImpulseJointSet { // FIXME: don't iterate through all the interactions. for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() { let joint = &edge.weight; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; - let (status1, activation1, ids1): ( - &RigidBodyType, - &RigidBodyActivation, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body1.0); - let (status2, activation2, ids2): ( - &RigidBodyType, - &RigidBodyActivation, - &RigidBodyIds, - ) = bodies.index_bundle(joint.body2.0); - - if (status1.is_dynamic() || status2.is_dynamic()) - && (!status1.is_dynamic() || !activation1.sleeping) - && (!status2.is_dynamic() || !activation2.sleeping) + if (rb1.is_dynamic() || rb2.is_dynamic()) + && (!rb1.is_dynamic() || !rb1.is_sleeping()) + && (!rb2.is_dynamic() || !rb2.is_sleeping()) { - let island_index = if !status1.is_dynamic() { - ids2.active_island_id + let island_index = if !rb1.is_dynamic() { + rb2.ids.active_island_id } else { - ids1.active_island_id + rb1.ids.active_island_id }; out[island_index].push(i); diff --git a/src/dynamics/joint/multibody_joint/multibody.rs b/src/dynamics/joint/multibody_joint/multibody.rs index 5bd790a..2eb90eb 100644 --- a/src/dynamics/joint/multibody_joint/multibody.rs +++ b/src/dynamics/joint/multibody_joint/multibody.rs @@ -1,8 +1,8 @@ use super::multibody_link::{MultibodyLink, MultibodyLinkVec}; use super::multibody_workspace::MultibodyWorkspace; use crate::dynamics::{ - solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle, - RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity, + solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyHandle, RigidBodySet, + RigidBodyType, RigidBodyVelocity, }; #[cfg(feature = "dim3")] use crate::math::Matrix; @@ -376,36 +376,32 @@ impl Multibody { for i in 0..self.links.len() { let link = &self.links[i]; - - let (rb_vels, rb_mprops, rb_forces): ( - &RigidBodyVelocity, - &RigidBodyMassProps, - &RigidBodyForces, - ) = bodies.index_bundle(link.rigid_body.0); + let rb = &bodies[link.rigid_body]; let mut acc = RigidBodyVelocity::zero(); if i != 0 { let parent_id = link.parent_internal_id; let parent_link = &self.links[parent_id]; - let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0); + let parent_rb = &bodies[parent_link.rigid_body]; acc += self.workspace.accs[parent_id]; // The 2.0 originates from the two identical terms of Jdot (the terms become // identical once they are multiplied by the generalized velocities). - acc.linvel += 2.0 * parent_rb_vels.angvel.gcross(link.joint_velocity.linvel); + acc.linvel += 2.0 * parent_rb.vels.angvel.gcross(link.joint_velocity.linvel); #[cfg(feature = "dim3")] { - acc.angvel += parent_rb_vels.angvel.cross(&link.joint_velocity.angvel); + acc.angvel += parent_rb.vels.angvel.cross(&link.joint_velocity.angvel); } - acc.linvel += parent_rb_vels + acc.linvel += parent_rb + .vels .angvel - .gcross(parent_rb_vels.angvel.gcross(link.shift02)); + .gcross(parent_rb.vels.angvel.gcross(link.shift02)); acc.linvel += self.workspace.accs[parent_id].angvel.gcross(link.shift02); } - acc.linvel += rb_vels.angvel.gcross(rb_vels.angvel.gcross(link.shift23)); + acc.linvel += rb.vels.angvel.gcross(rb.vels.angvel.gcross(link.shift23)); acc.linvel += self.workspace.accs[i].angvel.gcross(link.shift23); self.workspace.accs[i] = acc; @@ -413,12 +409,12 @@ impl Multibody { // TODO: should gyroscopic forces already be computed by the rigid-body itself // (at the same time that we add the gravity force)? let gyroscopic; - let rb_inertia = rb_mprops.effective_angular_inertia(); - let rb_mass = rb_mprops.effective_mass(); + let rb_inertia = rb.mprops.effective_angular_inertia(); + let rb_mass = rb.mprops.effective_mass(); #[cfg(feature = "dim3")] { - gyroscopic = rb_vels.angvel.cross(&(rb_inertia * rb_vels.angvel)); + gyroscopic = rb.vels.angvel.cross(&(rb_inertia * rb.vels.angvel)); } #[cfg(feature = "dim2")] { @@ -426,8 +422,8 @@ impl Multibody { } let external_forces = Force::new( - rb_forces.force - rb_mass.component_mul(&acc.linvel), - rb_forces.torque - gyroscopic - rb_inertia * acc.angvel, + rb.forces.force - rb_mass.component_mul(&acc.linvel), + rb.forces.torque - gyroscopic - rb_inertia * acc.angvel, ); self.accelerations.gemv_tr( 1.0, @@ -456,13 +452,12 @@ impl Multibody { .jacobian_mul_coordinates(&self.velocities.as_slice()[link.assembly_id..]); link.joint_velocity = joint_velocity; - bodies.set_internal(link.rigid_body.0, link.joint_velocity); + bodies.index_mut_internal(link.rigid_body).vels = link.joint_velocity; for i in 1..self.links.len() { let (link, parent_link) = self.links.get_mut_with_parent(i); - let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0); - let (parent_rb_vels, parent_rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(parent_link.rigid_body.0); + let rb = &bodies[link.rigid_body]; + let parent_rb = &bodies[parent_link.rigid_body]; let joint_velocity = link .joint @@ -470,12 +465,12 @@ impl Multibody { link.joint_velocity = joint_velocity.transformed( &(parent_link.local_to_world.rotation * link.joint.data.local_frame1.rotation), ); - let mut new_rb_vels = *parent_rb_vels + link.joint_velocity; - let shift = rb_mprops.world_com - parent_rb_mprops.world_com; - new_rb_vels.linvel += parent_rb_vels.angvel.gcross(shift); + let mut new_rb_vels = parent_rb.vels + link.joint_velocity; + let shift = rb.mprops.world_com - parent_rb.mprops.world_com; + new_rb_vels.linvel += parent_rb.vels.angvel.gcross(shift); new_rb_vels.linvel += link.joint_velocity.angvel.gcross(link.shift23); - bodies.set_internal(link.rigid_body.0, new_rb_vels); + bodies.index_mut_internal(link.rigid_body).vels = new_rb_vels; } /* @@ -563,10 +558,9 @@ impl Multibody { for i in 0..self.links.len() { let link = &self.links[i]; - let (rb_vels, rb_mprops): (&RigidBodyVelocity, &RigidBodyMassProps) = - bodies.index_bundle(link.rigid_body.0); - let rb_mass = rb_mprops.effective_mass(); - let rb_inertia = rb_mprops.effective_angular_inertia().into_matrix(); + let rb = &bodies[link.rigid_body]; + let rb_mass = rb.mprops.effective_mass(); + let rb_inertia = rb.mprops.effective_angular_inertia().into_matrix(); let body_jacobian = &self.body_jacobians[i]; @@ -576,8 +570,8 @@ impl Multibody { #[cfg(feature = "dim3")] { // Derivative of gyroscopic forces. - let gyroscopic_matrix = rb_vels.angvel.gcross_matrix() * rb_inertia - - (rb_inertia * rb_vels.angvel).gcross_matrix(); + let gyroscopic_matrix = rb.vels.angvel.gcross_matrix() * rb_inertia + - (rb_inertia * rb.vels.angvel).gcross_matrix(); augmented_inertia += gyroscopic_matrix * dt; } @@ -604,10 +598,10 @@ impl Multibody { if i != 0 { let parent_id = link.parent_internal_id; let parent_link = &self.links[parent_id]; - let parent_rb_vels: &RigidBodyVelocity = bodies.index(parent_link.rigid_body.0); + let parent_rb = &bodies[parent_link.rigid_body]; let parent_j = &self.body_jacobians[parent_id]; let parent_j_w = parent_j.fixed_rows::<ANG_DIM>(DIM); - let parent_w = parent_rb_vels.angvel.gcross_matrix(); + let parent_w = parent_rb.vels.angvel.gcross_matrix(); let (coriolis_v, parent_coriolis_v) = self.coriolis_v.index_mut2(i, parent_id); let (coriolis_w, parent_coriolis_w) = self.coriolis_w.index_mut2(i, parent_id); @@ -620,7 +614,7 @@ impl Multibody { coriolis_v.gemm(1.0, &shift_cross_tr, &parent_coriolis_w, 1.0); // JDot (but the 2.0 originates from the sum of two identical terms in JDot and JDot/u * gdot) - let dvel_cross = (rb_vels.angvel.gcross(link.shift02) + let dvel_cross = (rb.vels.angvel.gcross(link.shift02) + 2.0 * link.joint_velocity.linvel) .gcross_matrix_tr(); coriolis_v.gemm(1.0, &dvel_cross, &parent_j_w, 1.0); @@ -676,13 +670,13 @@ impl Multibody { coriolis_v.gemm(1.0, &shift_cross_tr, &coriolis_w, 1.0); // JDot - let dvel_cross = rb_vels.angvel.gcross(link.shift23).gcross_matrix_tr(); + let dvel_cross = rb.vels.angvel.gcross(link.shift23).gcross_matrix_tr(); coriolis_v.gemm(1.0, &dvel_cross, &rb_j_w, 1.0); // JDot/u * qdot coriolis_v.gemm( 1.0, - &(rb_vels.angvel.gcross_matrix() * shift_cross_tr), + &(rb.vels.angvel.gcross_matrix() * shift_cross_tr), &rb_j_w, 1.0, ); |