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

Finalize refactoring

Browse Source
This commit is contained in:
Sébastien Crozet
2022-04-20 12:29:57 +02:00
committed by Sébastien Crozet
parent 2b1374c596
commit f108520b5a
32 changed files with 707 additions and 1030 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/dynamics/solver/categorization.rs
View File

@@ -50,18 +50,18 @@ pub(crate) fn categorize_joints(
) {
for joint_i in joint_indices {
let joint = &impulse_joints[*joint_i].weight;
let status1 = bodies.index(joint.body1.0);
let status2 = bodies.index(joint.body2.0);
let rb1 = &bodies[joint.body1.0];
let rb2 = &bodies[joint.body2.0];
if multibody_joints.rigid_body_link(joint.body1).is_some()
|| multibody_joints.rigid_body_link(joint.body2).is_some()
{
if !status1.is_dynamic() || !status2.is_dynamic() {
if !rb1.is_dynamic() || !rb2.is_dynamic() {
generic_ground_joints.push(*joint_i);
} else {
generic_nonground_joints.push(*joint_i);
}
} else if !status1.is_dynamic() || !status2.is_dynamic() {
} else if !rb1.is_dynamic() || !rb2.is_dynamic() {
ground_joints.push(*joint_i);
} else {
nonground_joints.push(*joint_i);

92
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -1,8 +1,5 @@
use crate::dynamics::solver::{GenericRhs, VelocityConstraint};
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
RigidBodyType, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WCross, WDot};
@@ -42,18 +39,12 @@ impl GenericVelocityConstraint {
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let (rb_ids1, rb_vels1, rb_mprops1, rb_type1): (
&RigidBodyIds,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyType,
) = bodies.index_bundle(handle1.0);
let (rb_ids2, rb_vels2, rb_mprops2, rb_type2): (
&RigidBodyIds,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyType,
) = bodies.index_bundle(handle2.0);
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
let multibody1 = multibodies
.rigid_body_link(handle1)
@@ -63,15 +54,15 @@ impl GenericVelocityConstraint {
.map(|m| (&multibodies[m.multibody], m.id));
let mj_lambda1 = multibody1
.map(|mb| mb.0.solver_id)
.unwrap_or(if rb_type1.is_dynamic() {
rb_ids1.active_set_offset
.unwrap_or(if type1.is_dynamic() {
rb1.ids.active_set_offset
} else {
0
});
let mj_lambda2 = multibody2
.map(|mb| mb.0.solver_id)
.unwrap_or(if rb_type2.is_dynamic() {
rb_ids2.active_set_offset
.unwrap_or(if type2.is_dynamic() {
rb2.ids.active_set_offset
} else {
0
});
@@ -80,11 +71,8 @@ impl GenericVelocityConstraint {
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions(
&force_dir1,
&rb_vels1.linvel,
&rb_vels2.linvel,
);
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let multibodies_ndof = multibody1.map(|m| m.0.ndofs()).unwrap_or(0)
+ multibody2.map(|m| m.0.ndofs()).unwrap_or(0);
@@ -109,13 +97,13 @@ impl GenericVelocityConstraint {
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: if rb_type1.is_dynamic() {
rb_mprops1.effective_inv_mass
im1: if type1.is_dynamic() {
mprops1.effective_inv_mass
} else {
na::zero()
},
im2: if rb_type2.is_dynamic() {
rb_mprops2.effective_inv_mass
im2: if type2.is_dynamic() {
mprops2.effective_inv_mass
} else {
na::zero()
},
@@ -129,11 +117,11 @@ impl GenericVelocityConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - rb_mprops1.world_com;
let dp2 = manifold_point.point - rb_mprops2.world_com;
let dp1 = manifold_point.point - mprops1.world_com;
let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2);
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
@@ -143,15 +131,15 @@ impl GenericVelocityConstraint {
let torque_dir1 = dp1.gcross(force_dir1);
let torque_dir2 = dp2.gcross(-force_dir1);
let gcross1 = if rb_type1.is_dynamic() {
rb_mprops1
let gcross1 = if type1.is_dynamic() {
mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(torque_dir1)
} else {
na::zero()
};
let gcross2 = if rb_type2.is_dynamic() {
rb_mprops2
let gcross2 = if type2.is_dynamic() {
mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(torque_dir2)
} else {
@@ -170,8 +158,8 @@ impl GenericVelocityConstraint {
jacobians,
)
.0
} else if rb_type1.is_dynamic() {
force_dir1.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1))
} else if type1.is_dynamic() {
force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
} else {
0.0
@@ -189,8 +177,8 @@ impl GenericVelocityConstraint {
jacobians,
)
.0
} else if rb_type2.is_dynamic() {
force_dir1.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1))
} else if type2.is_dynamic() {
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2)
} else {
0.0
@@ -224,8 +212,8 @@ impl GenericVelocityConstraint {
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
let gcross1 = if rb_type1.is_dynamic() {
rb_mprops1
let gcross1 = if type1.is_dynamic() {
mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(torque_dir1)
} else {
@@ -234,8 +222,8 @@ impl GenericVelocityConstraint {
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
let torque_dir2 = dp2.gcross(-tangents1[j]);
let gcross2 = if rb_type2.is_dynamic() {
rb_mprops2
let gcross2 = if type2.is_dynamic() {
mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(torque_dir2)
} else {
@@ -255,9 +243,8 @@ impl GenericVelocityConstraint {
jacobians,
)
.0
} else if rb_type1.is_dynamic() {
force_dir1
.dot(&rb_mprops1.effective_inv_mass.component_mul(&force_dir1))
} else if type1.is_dynamic() {
force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
} else {
0.0
@@ -275,9 +262,8 @@ impl GenericVelocityConstraint {
jacobians,
)
.0
} else if rb_type2.is_dynamic() {
force_dir1
.dot(&rb_mprops2.effective_inv_mass.component_mul(&force_dir1))
} else if type2.is_dynamic() {
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2)
} else {
0.0
@@ -303,8 +289,8 @@ impl GenericVelocityConstraint {
// reduce all ops to nothing because its ndofs will be zero.
let generic_constraint_mask = (multibody1.is_some() as u8)
| ((multibody2.is_some() as u8) << 1)
| (!rb_type1.is_dynamic() as u8)
| ((!rb_type2.is_dynamic() as u8) << 1);
| (!type1.is_dynamic() as u8)
| ((!type2.is_dynamic() as u8) << 1);
let constraint = GenericVelocityConstraint {
velocity_constraint: constraint,

30
src/dynamics/solver/generic_velocity_ground_constraint.rs
View File

@@ -1,7 +1,5 @@
use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WCross;
@@ -48,16 +46,15 @@ impl GenericVelocityGroundConstraint {
(-manifold.data.normal, 1.0)
};
let (rb_vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
let rb2 = &bodies[handle2.unwrap()];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let (mb2, link_id2) = handle2
.and_then(|h| multibodies.rigid_body_link(h))
@@ -68,11 +65,8 @@ impl GenericVelocityGroundConstraint {
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions(
&force_dir1,
&rb_vels1.linvel,
&rb_vels2.linvel,
);
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let multibodies_ndof = mb2.ndofs();
// For each solver contact we generate DIM constraints, and each constraints appends
@@ -96,7 +90,7 @@ impl GenericVelocityGroundConstraint {
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb_mprops2.effective_inv_mass,
im2: mprops2.effective_inv_mass,
limit: 0.0,
mj_lambda2,
manifold_id,
@@ -107,10 +101,10 @@ impl GenericVelocityGroundConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - world_com1;
let dp2 = manifold_point.point - rb_mprops2.world_com;
let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2);
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;

55
src/dynamics/solver/interaction_groups.rs
View File

@@ -3,7 +3,6 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use {
crate::data::BundleSet,
crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH},
vec_map::VecMap,
};
@@ -90,14 +89,8 @@ impl ParallelInteractionGroups {
.zip(self.interaction_colors.iter_mut())
{
let mut body_pair = interactions[*interaction_id].body_pair();
let is_fixed1 = body_pair
.0
.map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_fixed())
.unwrap_or(true);
let is_fixed2 = body_pair
.1
.map(|b| ComponentSet::<RigidBodyType>::index(bodies, b.0).is_fixed())
.unwrap_or(true);
let is_fixed1 = body_pair.0.map(|b| bodies[b].is_fixed()).unwrap_or(true);
let is_fixed2 = body_pair.1.map(|b| bodies[b].is_fixed()).unwrap_or(true);
let representative = |handle: RigidBodyHandle| {
if let Some(link) = multibodies.rigid_body_link(handle).copied() {
@@ -119,28 +112,28 @@ impl ParallelInteractionGroups {
match (is_fixed1, is_fixed2) {
(false, false) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
let rb1 = &bodies[body_pair.0.unwrap()];
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask =
bcolors[rb_ids1.active_set_offset] | bcolors[rb_ids2.active_set_offset];
bcolors[rb1.ids.active_set_offset] | bcolors[rb2.ids.active_set_offset];
*color = (!color_mask).trailing_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color;
bcolors[rb_ids2.active_set_offset] |= 1 << *color;
bcolors[rb1.ids.active_set_offset] |= 1 << *color;
bcolors[rb2.ids.active_set_offset] |= 1 << *color;
}
(true, false) => {
let rb_ids2: &RigidBodyIds = bodies.index(body_pair.1.unwrap().0);
let color_mask = bcolors[rb_ids2.active_set_offset];
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask = bcolors[rb2.ids.active_set_offset];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids2.active_set_offset] |= 1 << *color;
bcolors[rb2.ids.active_set_offset] |= 1 << *color;
}
(false, true) => {
let rb_ids1: &RigidBodyIds = bodies.index(body_pair.0.unwrap().0);
let color_mask = bcolors[rb_ids1.active_set_offset];
let rb1 = &bodies[body_pair.0.unwrap()];
let color_mask = bcolors[rb1.ids.active_set_offset];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb_ids1.active_set_offset] |= 1 << *color;
bcolors[rb1.ids.active_set_offset] |= 1 << *color;
}
(true, true) => unreachable!(),
}
@@ -258,13 +251,11 @@ impl InteractionGroups {
for interaction_i in interaction_indices {
let interaction = &interactions[*interaction_i].weight;
let (status1, ids1): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body1.0);
let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body2.0);
let rb1 = &bodies[interaction.body1];
let rb2 = &bodies[interaction.body2];
let is_fixed1 = !status1.is_dynamic();
let is_fixed2 = !status2.is_dynamic();
let is_fixed1 = !rb1.is_dynamic();
let is_fixed2 = !rb2.is_dynamic();
if is_fixed1 && is_fixed2 {
continue;
@@ -277,8 +268,8 @@ impl InteractionGroups {
}
let ijoint = interaction.data.locked_axes.bits() as usize;
let i1 = ids1.active_set_offset;
let i2 = ids2.active_set_offset;
let i1 = rb1.ids.active_set_offset;
let i2 = rb2.ids.active_set_offset;
let conflicts =
self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
@@ -421,15 +412,15 @@ impl InteractionGroups {
let (status1, active_set_offset1) = if let Some(rb1) = interaction.data.rigid_body1
{
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb1.0);
(*data.0, data.1.active_set_offset)
let rb1 = &bodies[rb1];
(rb1.body_type, rb1.ids.active_set_offset)
} else {
(RigidBodyType::Fixed, usize::MAX)
};
let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
{
let data: (_, &RigidBodyIds) = bodies.index_bundle(rb2.0);
(*data.0, data.1.active_set_offset)
let rb2 = &bodies[rb2];
(rb2.body_type, rb2.ids.active_set_offset)
} else {
(RigidBodyType::Fixed, usize::MAX)
};

129
src/dynamics/solver/joint_constraint/joint_constraint.rs
View File

@@ -6,8 +6,7 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodySet, RigidBodyType, RigidBodyVelocity,
ImpulseJoint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
};
use crate::math::{Real, SPATIAL_DIM};
use crate::prelude::MultibodyJointSet;
@@ -63,39 +62,26 @@ impl AnyJointVelocityConstraint {
) {
let local_frame1 = joint.data.local_frame1;
let local_frame2 = joint.data.local_frame2;
let rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(joint.body1.0);
let rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(joint.body2.0);
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rb1;
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
let frame1 = rb_pos1.position * local_frame1;
let frame2 = rb_pos2.position * local_frame2;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let frame1 = rb1.pos.position * local_frame1;
let frame2 = rb2.pos.position * local_frame2;
let body1 = SolverBody {
linvel: rb_vel1.linvel,
angvel: rb_vel1.angvel,
im: rb_mprops1.effective_inv_mass,
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
world_com: rb_mprops1.world_com,
mj_lambda: [rb_ids1.active_set_offset],
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.world_com,
mj_lambda: [rb1.ids.active_set_offset],
};
let body2 = SolverBody {
linvel: rb_vel2.linvel,
angvel: rb_vel2.angvel,
im: rb_mprops2.effective_inv_mass,
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
world_com: rb_mprops2.world_com,
mj_lambda: [rb_ids2.active_set_offset],
linvel: rb2.vels.linvel,
angvel: rb2.vels.angvel,
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.world_com,
mj_lambda: [rb2.ids.active_set_offset],
};
let mb1 = multibodies
@@ -186,16 +172,20 @@ impl AnyJointVelocityConstraint {
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
let rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| bodies.index(impulse_joints[ii].body1.0)],
gather![|ii| &bodies[impulse_joints[ii].body1].pos],
gather![|ii| &bodies[impulse_joints[ii].body1].vels],
gather![|ii| &bodies[impulse_joints[ii].body1].mprops],
gather![|ii| &bodies[impulse_joints[ii].body1].ids],
);
let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
@@ -203,10 +193,10 @@ impl AnyJointVelocityConstraint {
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| bodies.index(impulse_joints[ii].body2.0)],
gather![|ii| &bodies[impulse_joints[ii].body2].pos],
gather![|ii| &bodies[impulse_joints[ii].body2].vels],
gather![|ii| &bodies[impulse_joints[ii].body2].mprops],
gather![|ii| &bodies[impulse_joints[ii].body2].ids],
);
let (rb_pos1, rb_vel1, rb_mprops1, rb_ids1) = rbs1;
@@ -276,8 +266,7 @@ impl AnyJointVelocityConstraint {
) {
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
let flipped = !bodies[handle2].is_dynamic();
let (local_frame1, local_frame2) = if flipped {
std::mem::swap(&mut handle1, &mut handle2);
@@ -286,35 +275,27 @@ impl AnyJointVelocityConstraint {
(joint.data.local_frame1, joint.data.local_frame2)
};
let rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
let rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
) = bodies.index_bundle(handle2.0);
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let (rb_pos1, rb_vel1, rb_mprops1) = rb1;
let (rb_pos2, rb_vel2, rb_mprops2, rb_ids2) = rb2;
let frame1 = rb_pos1.position * local_frame1;
let frame2 = rb_pos2.position * local_frame2;
let frame1 = rb1.pos.position * local_frame1;
let frame2 = rb2.pos.position * local_frame2;
let body1 = SolverBody {
linvel: rb_vel1.linvel,
angvel: rb_vel1.angvel,
im: rb_mprops1.effective_inv_mass,
sqrt_ii: rb_mprops1.effective_world_inv_inertia_sqrt,
world_com: rb_mprops1.world_com,
linvel: rb1.vels.linvel,
angvel: rb1.vels.angvel,
im: rb1.mprops.effective_inv_mass,
sqrt_ii: rb1.mprops.effective_world_inv_inertia_sqrt,
world_com: rb1.mprops.world_com,
mj_lambda: [crate::INVALID_USIZE],
};
let body2 = SolverBody {
linvel: rb_vel2.linvel,
angvel: rb_vel2.angvel,
im: rb_mprops2.effective_inv_mass,
sqrt_ii: rb_mprops2.effective_world_inv_inertia_sqrt,
world_com: rb_mprops2.world_com,
mj_lambda: [rb_ids2.active_set_offset],
linvel: rb2.vels.linvel,
angvel: rb2.vels.angvel,
im: rb2.mprops.effective_inv_mass,
sqrt_ii: rb2.mprops.effective_world_inv_inertia_sqrt,
world_com: rb2.mprops.world_com,
mj_lambda: [rb2.ids.active_set_offset],
};
if let Some(mb2) = multibodies
@@ -399,9 +380,13 @@ impl AnyJointVelocityConstraint {
out: &mut Vec<Self>,
insert_at: Option<usize>,
) {
use crate::dynamics::{
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
let mut handles1 = gather![|ii| impulse_joints[ii].body1];
let mut handles2 = gather![|ii| impulse_joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].body_type];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
@@ -429,9 +414,9 @@ impl AnyJointVelocityConstraint {
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| &bodies[handles1[ii]].pos],
gather![|ii| &bodies[handles1[ii]].vels],
gather![|ii| &bodies[handles1[ii]].mprops],
);
let rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
@@ -439,10 +424,10 @@ impl AnyJointVelocityConstraint {
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
) = (
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| &bodies[handles2[ii]].pos],
gather![|ii| &bodies[handles2[ii]].vels],
gather![|ii| &bodies[handles2[ii]].mprops],
gather![|ii| &bodies[handles2[ii]].ids],
);
let (rb_pos1, rb_vel1, rb_mprops1) = rbs1;

13
src/dynamics/solver/parallel_island_solver.rs
View File

@@ -7,8 +7,7 @@ use crate::dynamics::solver::{
};
use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
RigidBodyDamping, RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use na::DVector;
@@ -316,15 +315,13 @@ impl ParallelIslandSolver {
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
}
} else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
let dvel = &mut velocity_solver.mj_lambdas[ids.active_set_offset];
let rb = &bodies[*handle];
let dvel = &mut velocity_solver.mj_lambdas[rb.ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
dvel.angular += rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
dvel.linear += rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
}
}
}

3
src/dynamics/solver/parallel_solver_constraints.rs
View File

@@ -8,8 +8,7 @@ use crate::dynamics::solver::{
};
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, IslandManager, JointGraphEdge, MultibodyIndex,
MultibodyJointSet, RigidBodyHandle, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType, RigidBodyVelocity,
MultibodyJointSet, RigidBodyHandle, RigidBodySet,
};
use crate::geometry::ContactManifold;
use crate::math::{Real, SPATIAL_DIM};

51
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -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);
}
}
}

17
src/dynamics/solver/velocity_constraint.rs
View File

@@ -3,9 +3,7 @@ use crate::dynamics::solver::{
};
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot};
@@ -160,13 +158,14 @@ impl VelocityConstraint {
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let (ids1, vels1, mprops1): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.0);
let mj_lambda1 = ids1.active_set_offset;
let mj_lambda2 = ids2.active_set_offset;
let rb1 = &bodies[handle1];
let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
let rb2 = &bodies[handle2];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let mj_lambda1 = rb1.ids.active_set_offset;
let mj_lambda2 = rb2.ids.active_set_offset;
let force_dir1 = -manifold.data.normal;
#[cfg(feature = "dim2")]

16
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -1,7 +1,9 @@
use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -49,12 +51,12 @@ impl WVelocityConstraint {
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].vels];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].vels];
let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].ids];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].ids];
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);

16
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -7,9 +7,7 @@ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WBasis;
use crate::utils::{self, WAngularInertia, WCross, WDot};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[derive(Copy, Clone, Debug)]
@@ -51,15 +49,15 @@ impl VelocityGroundConstraint {
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
let rb2 = &bodies[handle2.unwrap()];
let vels2 = &rb2.vels;
let mprops2 = &rb2.mprops;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
@@ -67,7 +65,7 @@ impl VelocityGroundConstraint {
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let mj_lambda2 = ids2.active_set_offset;
let mj_lambda2 = rb2.ids.active_set_offset;
for (_l, manifold_points) in manifold
.data

14
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -2,7 +2,9 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::dynamics::{
IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -54,20 +56,20 @@ impl WVelocityGroundConstraint {
let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
handles1[ii]
.map(|h| *bodies.index(h.0))
.map(|h| bodies[h].vels)
.unwrap_or_else(RigidBodyVelocity::zero)
}];
let world_com1 = Point::from(gather![|ii| {
handles1[ii]
.map(|h| ComponentSet::<RigidBodyMassProps>::index(bodies, h.0).world_com)
.map(|h| bodies[h].mprops.world_com)
.unwrap_or_else(Point::origin)
}]);
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)];
gather![|ii| &bodies[handles2[ii].unwrap()].vels];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii].unwrap()].ids];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
gather![|ii| &bodies[handles2[ii].unwrap()].mprops];
let flipped_sign = SimdReal::from(flipped);

64
src/dynamics/solver/velocity_solver.rs
View File

@@ -1,9 +1,7 @@
use super::AnyJointVelocityConstraint;
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodyDamping,
RigidBodyForces, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodySet,
RigidBodyVelocity,
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet,
};
use crate::geometry::ContactManifold;
use crate::math::Real;
@@ -59,15 +57,15 @@ impl VelocitySolver {
mj_lambdas.axpy(params.dt, &multibody.accelerations, 0.0);
}
} else {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
let dvel = &mut self.mj_lambdas[ids.active_set_offset];
let rb = &bodies[*handle];
let dvel = &mut self.mj_lambdas[rb.ids.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += mprops.effective_world_inv_inertia_sqrt * forces.torque * params.dt;
dvel.linear += forces.force.component_mul(&mprops.effective_inv_mass) * params.dt;
dvel.angular +=
rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
dvel.linear +=
rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
}
}
@@ -151,33 +149,26 @@ impl VelocitySolver {
multibody.velocities = prev_vels;
}
} else {
let (rb_ids, rb_mprops): (&RigidBodyIds, &RigidBodyMassProps) =
bodies.index_bundle(handle.0);
let rb = bodies.index_mut_internal(*handle);
let dvel = self.mj_lambdas[rb_ids.active_set_offset];
let dangvel = rb_mprops
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_pos = rb.pos;
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_vels = new_vels.apply_damping(params.dt, &rb.damping);
new_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);
rb.pos = new_pos;
}
}
@@ -234,23 +225,16 @@ impl VelocitySolver {
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);
}
}