nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

First round deleting the component sets.

Browse Source
This commit is contained in:
Sébastien Crozet
2022-04-19 18:57:40 +02:00
committed by Sébastien Crozet
parent ee679427cd
commit 2b1374c596
36 changed files with 722 additions and 1649 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -1,10 +1,8 @@
use super::multibody_link::{MultibodyLink, MultibodyLinkVec};
use super::multibody_workspace::MultibodyWorkspace;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::AnyJointVelocityConstraint;
use crate::dynamics::{
IntegrationParameters, RigidBodyForces, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
solver::AnyJointVelocityConstraint, IntegrationParameters, RigidBodyForces, RigidBodyHandle,
RigidBodyMassProps, RigidBodySet, RigidBodyType, RigidBodyVelocity,
};
#[cfg(feature = "dim3")]
use crate::math::Matrix;
@@ -369,12 +367,7 @@ impl Multibody {
.extend((0..num_jacobians).map(|_| Jacobian::zeros(0)));
}
pub(crate) fn update_acceleration<Bodies>(&mut self, bodies: &Bodies)
where
Bodies: ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyVelocity>,
{
pub(crate) fn update_acceleration(&mut self, bodies: &RigidBodySet) {
if self.ndofs == 0 {
return; // Nothing to do.
}
@@ -452,10 +445,7 @@ impl Multibody {
}
/// Computes the constant terms of the dynamics.
pub(crate) fn update_dynamics<Bodies>(&mut self, dt: Real, bodies: &mut Bodies)
where
Bodies: ComponentSetMut<RigidBodyVelocity> + ComponentSet<RigidBodyMassProps>,
{
pub(crate) fn update_dynamics(&mut self, dt: Real, bodies: &mut RigidBodySet) {
/*
* Compute velocities.
* NOTE: this is needed for kinematic bodies too.
@@ -545,10 +535,7 @@ impl Multibody {
}
}
fn update_inertias<Bodies>(&mut self, dt: Real, bodies: &Bodies)
where
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyVelocity>,
{
fn update_inertias(&mut self, dt: Real, bodies: &RigidBodySet) {
if self.ndofs == 0 {
return; // Nothing to do.
}
@@ -790,17 +777,11 @@ impl Multibody {
}
}
pub(crate) fn update_root_type<Bodies>(&mut self, bodies: &mut Bodies)
where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyPosition>,
{
let rb_type: Option<&RigidBodyType> = bodies.get(self.links[0].rigid_body.0);
if let Some(rb_type) = rb_type {
let rb_pos: &RigidBodyPosition = bodies.index(self.links[0].rigid_body.0);
if rb_type.is_dynamic() != self.root_is_dynamic {
if rb_type.is_dynamic() {
let free_joint = MultibodyJoint::free(rb_pos.position);
pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) {
if let Some(rb) = bodies.get(self.links[0].rigid_body) {
if rb.is_dynamic() != self.root_is_dynamic {
if rb.is_dynamic() {
let free_joint = MultibodyJoint::free(*rb.position());
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].joint = free_joint;
self.links[0].assembly_id = 0;
@@ -819,7 +800,7 @@ impl Multibody {
assert!(self.damping.len() >= SPATIAL_DIM);
assert!(self.accelerations.len() >= SPATIAL_DIM);
let fixed_joint = MultibodyJoint::fixed(rb_pos.position);
let fixed_joint = MultibodyJoint::fixed(*rb.position());
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].joint = fixed_joint;
self.links[0].assembly_id = 0;
@@ -844,39 +825,31 @@ impl Multibody {
}
}
self.root_is_dynamic = rb_type.is_dynamic();
self.root_is_dynamic = rb.is_dynamic();
}
// Make sure the positions are properly set to match the rigid-body’s.
if self.links[0].joint.data.locked_axes.is_empty() {
self.links[0].joint.set_free_pos(rb_pos.position);
self.links[0].joint.set_free_pos(*rb.position());
} else {
self.links[0].joint.data.local_frame1 = rb_pos.position;
self.links[0].joint.data.local_frame1 = *rb.position();
}
}
}
/// Apply forward-kinematics to this multibody and its related rigid-bodies.
pub fn forward_kinematics<Bodies>(&mut self, bodies: &mut Bodies, update_mass_props: bool)
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyMassProps>
+ ComponentSetMut<RigidBodyPosition>,
{
pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_mass_props: bool) {
// Special case for the root, which has no parent.
{
let link = &mut self.links[0];
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = link.local_to_parent;
bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
rb_pos.next_position = link.local_to_world;
});
if update_mass_props {
bodies.map_mut_internal(link.rigid_body.0, |mprops: &mut RigidBodyMassProps| {
mprops.update_world_mass_properties(&link.local_to_world)
});
if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
rb.pos.next_position = link.local_to_world;
if update_mass_props {
rb.mprops.update_world_mass_properties(&link.local_to_world);
}
}
}
@@ -888,32 +861,30 @@ impl Multibody {
link.local_to_world = parent_link.local_to_world * link.local_to_parent;
{
let parent_rb_mprops: &RigidBodyMassProps = bodies.index(parent_link.rigid_body.0);
let rb_mprops: &RigidBodyMassProps = bodies.index(link.rigid_body.0);
let c0 = parent_link.local_to_world * parent_rb_mprops.local_mprops.local_com;
let parent_rb = &bodies[parent_link.rigid_body];
let link_rb = &bodies[link.rigid_body];
let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
let c2 = link.local_to_world
* Point::from(link.joint.data.local_frame2.translation.vector);
let c3 = link.local_to_world * rb_mprops.local_mprops.local_com;
let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
link.shift02 = c2 - c0;
link.shift23 = c3 - c2;
}
bodies.map_mut_internal(link.rigid_body.0, |rb_pos: &mut RigidBodyPosition| {
rb_pos.next_position = link.local_to_world;
});
let link_rb = bodies.index_mut_internal(link.rigid_body);
link_rb.pos.next_position = link.local_to_world;
let rb_type: &RigidBodyType = bodies.index(link.rigid_body.0);
assert_eq!(
*rb_type,
link_rb.body_type,
RigidBodyType::Dynamic,
"A rigid-body that is not at the root of a multibody must be dynamic."
);
if update_mass_props {
bodies.map_mut_internal(link.rigid_body.0, |rb_mprops: &mut RigidBodyMassProps| {
rb_mprops.update_world_mass_properties(&link.local_to_world)
});
link_rb
.mprops
.update_world_mass_properties(&link.local_to_world);
}
}

35
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -1,8 +1,7 @@
use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index};
use crate::data::{Arena, Coarena, Index};
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{
GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
RigidBodyIds, RigidBodyType,
GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyHandle, RigidBodySet,
};
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
use crate::parry::partitioning::IndexedData;
@@ -163,17 +162,13 @@ impl MultibodyJointSet {
}
/// Removes an multibody_joint from this set.
pub fn remove<Bodies>(
pub fn remove(
&mut self,
handle: MultibodyJointHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
wake_up: bool,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
) {
if let Some(removed) = self.rb2mb.get(handle.0).copied() {
let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -216,17 +211,13 @@ impl MultibodyJointSet {
}
/// Removes all the multibody_joints from the multibody the given rigid-body is part of.
pub fn remove_multibody_articulations<Bodies>(
pub fn remove_multibody_articulations(
&mut self,
handle: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
bodies: &mut RigidBodySet,
wake_up: bool,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
) {
if let Some(removed) = self.rb2mb.get(handle.0).copied() {
// Remove the multibody.
let multibody = self.multibodies.remove(removed.multibody.0).unwrap();
@@ -248,16 +239,12 @@ impl MultibodyJointSet {
}
/// Removes all the multibody joints attached to a rigid-body.
pub fn remove_joints_attached_to_rigid_body<Bodies>(
pub fn remove_joints_attached_to_rigid_body(
&mut self,
rb_to_remove: RigidBodyHandle,
islands: &mut IslandManager,
bodies: &mut Bodies,
) where
Bodies: ComponentSetMut<RigidBodyActivation>
+ ComponentSet<RigidBodyType>
+ ComponentSetMut<RigidBodyIds>,
{
bodies: &mut RigidBodySet,
) {
// TODO: optimize this.
if let Some(link_to_remove) = self.rb2mb.get(rb_to_remove.0).copied() {
let mut articulations_to_remove = vec![];