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

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,8 +1,9 @@
use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet};
use crate::data::ComponentSet;
use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodyType};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
pub(crate) fn categorize_contacts(
_bodies: &RigidBodySet, // Unused but useful to simplify the parallel code.
_bodies: &impl ComponentSet<RigidBodyType>, // Unused but useful to simplify the parallel code.
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
out_ground: &mut Vec<ContactManifoldIndex>,
@@ -20,7 +21,7 @@ pub(crate) fn categorize_contacts(
}
pub(crate) fn categorize_joints(
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBodyType>,
joints: &[JointGraphEdge],
joint_indices: &[JointIndex],
ground_joints: &mut Vec<JointIndex>,
@@ -28,10 +29,10 @@ pub(crate) fn categorize_joints(
) {
for joint_i in joint_indices {
let joint = &joints[*joint_i].weight;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let status1 = bodies.index(joint.body1.0);
let status2 = bodies.index(joint.body2.0);
if !rb1.is_dynamic() || !rb2.is_dynamic() {
if !status1.is_dynamic() || !status2.is_dynamic() {
ground_joints.push(*joint_i);
} else {
nonground_joints.push(*joint_i);

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

@@ -1,4 +1,8 @@
use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
#[cfg(feature = "parallel")]
use crate::dynamics::BodyPair;
use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyType};
use crate::dynamics::{JointGraphEdge, JointIndex};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use {
@@ -6,16 +10,19 @@ use {
vec_map::VecMap,
};
#[cfg(feature = "parallel")]
pub(crate) trait PairInteraction {
fn body_pair(&self) -> BodyPair;
}
#[cfg(feature = "parallel")]
impl<'a> PairInteraction for &'a mut ContactManifold {
fn body_pair(&self) -> BodyPair {
self.data.body_pair
}
}
#[cfg(feature = "parallel")]
impl<'a> PairInteraction for JointGraphEdge {
fn body_pair(&self) -> BodyPair {
BodyPair::new(self.weight.body1, self.weight.body2)
@@ -54,7 +61,7 @@ impl ParallelInteractionGroups {
pub fn group_interactions<Interaction: PairInteraction>(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBody>,
interactions: &[Interaction],
interaction_indices: &[usize],
) {
@@ -78,8 +85,8 @@ impl ParallelInteractionGroups {
.zip(self.interaction_colors.iter_mut())
{
let body_pair = interactions[*interaction_id].body_pair();
let rb1 = &bodies[body_pair.body1];
let rb2 = &bodies[body_pair.body2];
let rb1 = bodies.index(body_pair.body1);
let rb2 = bodies.index(body_pair.body2);
match (rb1.is_static(), rb2.is_static()) {
(false, false) => {
@@ -168,14 +175,15 @@ impl InteractionGroups {
self.nongrouped_interactions.clear();
}
// FIXME: there is a lot of duplicated code with group_manifolds here.
// TODO: there is a lot of duplicated code with group_manifolds here.
// But we don't refactor just now because we may end up with distinct
// grouping strategies in the future.
#[cfg(not(feature = "simd-is-enabled"))]
pub fn group_joints(
&mut self,
_island_id: usize,
_bodies: &RigidBodySet,
_islands: &IslandManager,
_bodies: &impl ComponentSet<RigidBodyIds>,
_interactions: &[JointGraphEdge],
interaction_indices: &[JointIndex],
) {
@@ -184,13 +192,16 @@ impl InteractionGroups {
}
#[cfg(feature = "simd-is-enabled")]
pub fn group_joints(
pub fn group_joints<Bodies>(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
interactions: &[JointGraphEdge],
interaction_indices: &[JointIndex],
) {
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
// NOTE: in 3D we have up to 10 different joint types.
// In 2D we only have 5 joint types.
#[cfg(feature = "dim3")]
@@ -204,11 +215,11 @@ impl InteractionGroups {
// Note: each bit of a body mask indicates what bucket already contains
// a constraints involving this body.
// FIXME: currently, this is a bit overconservative because when a bucket
// TODO: currently, this is a bit overconservative because when a bucket
// is full, we don't clear the corresponding body mask bit. This may result
// in less grouped constraints.
self.body_masks
.resize(bodies.active_island(island_id).len(), 0u128);
.resize(islands.active_island(island_id).len(), 0u128);
// NOTE: each bit of the occupied mask indicates what bucket already
// contains at least one constraint.
@@ -216,10 +227,14 @@ impl InteractionGroups {
for interaction_i in interaction_indices {
let interaction = &interactions[*interaction_i].weight;
let body1 = &bodies[interaction.body1];
let body2 = &bodies[interaction.body2];
let is_static1 = !body1.is_dynamic();
let is_static2 = !body2.is_dynamic();
let (status1, ids1): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body1.0);
let (status2, ids2): (&RigidBodyType, &RigidBodyIds) =
bodies.index_bundle(interaction.body2.0);
let is_static1 = !status1.is_dynamic();
let is_static2 = !status2.is_dynamic();
if is_static1 && is_static2 {
continue;
@@ -232,8 +247,8 @@ impl InteractionGroups {
}
let ijoint = interaction.params.type_id();
let i1 = body1.active_set_offset;
let i2 = body2.active_set_offset;
let i1 = ids1.active_set_offset;
let i2 = ids2.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.
@@ -325,7 +340,8 @@ impl InteractionGroups {
pub fn group_manifolds(
&mut self,
_island_id: usize,
_bodies: &RigidBodySet,
_islands: &IslandManager,
_bodies: &impl ComponentSet<RigidBodyIds>,
_interactions: &[&mut ContactManifold],
interaction_indices: &[ContactManifoldIndex],
) {
@@ -334,21 +350,24 @@ impl InteractionGroups {
}
#[cfg(feature = "simd-is-enabled")]
pub fn group_manifolds(
pub fn group_manifolds<Bodies>(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
interactions: &[&mut ContactManifold],
interaction_indices: &[ContactManifoldIndex],
) {
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
// Note: each bit of a body mask indicates what bucket already contains
// a constraints involving this body.
// FIXME: currently, this is a bit overconservative because when a bucket
// TODO: currently, this is a bit overconservative because when a bucket
// is full, we don't clear the corresponding body mask bit. This may result
// in less grouped contacts.
// NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop.
self.body_masks
.resize(bodies.active_island(island_id).len(), 0u128);
.resize(islands.active_island(island_id).len(), 0u128);
// NOTE: each bit of the occupied mask indicates what bucket already
// contains at least one constraint.
@@ -359,31 +378,44 @@ impl InteractionGroups {
.max()
.unwrap_or(1);
// FIXME: find a way to reduce the number of iteration.
// TODO: find a way to reduce the number of iteration.
// There must be a way to iterate just once on every interaction indices
// instead of MAX_MANIFOLD_POINTS times.
for k in 1..=max_interaction_points {
for interaction_i in interaction_indices {
let interaction = &interactions[*interaction_i];
// FIXME: how could we avoid iterating
// TODO: how could we avoid iterating
// on each interaction at every iteration on k?
if interaction.data.num_active_contacts() != k {
continue;
}
let body1 = &bodies[interaction.data.body_pair.body1];
let body2 = &bodies[interaction.data.body_pair.body2];
let is_static1 = !body1.is_dynamic();
let is_static2 = !body2.is_dynamic();
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)
} else {
(RigidBodyType::Static, 0)
};
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)
} else {
(RigidBodyType::Static, 0)
};
// FIXME: don't generate interactions between static bodies in the first place.
let is_static1 = !status1.is_dynamic();
let is_static2 = !status2.is_dynamic();
// TODO: don't generate interactions between static bodies in the first place.
if is_static1 && is_static2 {
continue;
}
let i1 = body1.active_set_offset;
let i2 = body2.active_set_offset;
let i1 = active_set_offset1;
let i2 = active_set_offset2;
let conflicts = self.body_masks[i1] | self.body_masks[i2];
let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts.
let conflictfree_occupied_targets = conflictfree_targets & occupied_mask;

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

@@ -1,10 +1,15 @@
use super::{PositionSolver, VelocitySolver};
use crate::counters::Counters;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
AnyVelocityConstraint, SolverConstraints,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
RigidBodyIds, RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
};
use crate::dynamics::{IslandManager, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
pub struct IslandSolver {
@@ -24,17 +29,21 @@ impl IslandSolver {
}
}
pub fn solve_position_constraints(
pub fn solve_position_constraints<Bodies>(
&mut self,
island_id: usize,
islands: &IslandManager,
counters: &mut Counters,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
) {
bodies: &mut Bodies,
) where
Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
{
counters.solver.position_resolution_time.resume();
self.position_solver.solve(
island_id,
params,
islands,
bodies,
&self.contact_constraints.position_constraints,
&self.joint_constraints.position_constraints,
@@ -42,31 +51,47 @@ impl IslandSolver {
counters.solver.position_resolution_time.pause();
}
pub fn init_constraints_and_solve_velocity_constraints(
pub fn init_constraints_and_solve_velocity_constraints<Bodies>(
&mut self,
island_id: usize,
counters: &mut Counters,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
islands: &IslandManager,
bodies: &mut Bodies,
manifolds: &mut [&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
joints: &mut [JointGraphEdge],
joint_indices: &[JointIndex],
) {
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSetMut<RigidBodyPosition>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyDamping>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
let has_constraints = manifold_indices.len() != 0 || joint_indices.len() != 0;
if has_constraints {
counters.solver.velocity_assembly_time.resume();
self.contact_constraints
.init(island_id, params, bodies, manifolds, manifold_indices);
self.contact_constraints.init(
island_id,
params,
islands,
bodies,
manifolds,
manifold_indices,
);
self.joint_constraints
.init(island_id, params, bodies, joints, joint_indices);
.init(island_id, params, islands, bodies, joints, joint_indices);
counters.solver.velocity_assembly_time.pause();
counters.solver.velocity_resolution_time.resume();
self.velocity_solver.solve(
island_id,
params,
islands,
bodies,
manifolds,
joints,
@@ -76,21 +101,50 @@ impl IslandSolver {
counters.solver.velocity_resolution_time.pause();
counters.solver.velocity_update_time.resume();
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.apply_damping(params.dt);
rb.integrate_next_position(params.dt);
});
for handle in islands.active_island(island_id) {
let (poss, vels, damping, mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_poss = *poss;
let new_vels = vels.apply_damping(params.dt, damping);
new_poss.next_position =
vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
bodies.set_internal(handle.0, new_vels);
bodies.set_internal(handle.0, new_poss);
}
counters.solver.velocity_update_time.pause();
} else {
self.contact_constraints.clear();
self.joint_constraints.clear();
counters.solver.velocity_update_time.resume();
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
for handle in islands.active_island(island_id) {
// Since we didn't run the velocity solver we need to integrate the accelerations here
rb.integrate_accelerations(params.dt);
rb.apply_damping(params.dt);
rb.integrate_next_position(params.dt);
});
let (poss, vels, forces, damping, mprops): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyForces,
&RigidBodyDamping,
&RigidBodyMassProps,
) = bodies.index_bundle(handle.0);
let mut new_poss = *poss;
let new_vels = forces
.integrate(params.dt, vels, mprops)
.apply_damping(params.dt, &damping);
new_poss.next_position =
vels.integrate(params.dt, &poss.position, &mprops.mass_properties.local_com);
bodies.set_internal(handle.0, new_vels);
bodies.set_internal(handle.0, new_poss);
}
counters.solver.velocity_update_time.pause();
}
}

56
src/dynamics/solver/joint_constraint/ball_position_constraint.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
@@ -23,18 +25,25 @@ pub(crate) struct BallPositionConstraint {
}
impl BallPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self {
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &BallJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
Self {
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
}
}
@@ -104,31 +113,34 @@ pub(crate) struct BallPositionGroundConstraint {
impl BallPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &BallJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
if flipped {
// Note the only thing that is flipped here
// are the local_anchors. The rb1 and rb2 have
// already been flipped by the caller.
Self {
anchor1: rb1.next_position * cparams.local_anchor2,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
anchor1: poss1.next_position * cparams.local_anchor2,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor1,
position2: rb2.active_set_offset,
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
local_com2: mprops2.mass_properties.local_com,
}
} else {
Self {
anchor1: rb1.next_position * cparams.local_anchor1,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
anchor1: poss1.next_position * cparams.local_anchor1,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor2,
position2: rb2.active_set_offset,
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
local_com2: mprops2.mass_properties.local_com,
}
}
}

83
src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
BallJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
#[cfg(feature = "dim2")]
use crate::math::SdpMatrix;
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, SimdReal, SIMD_WIDTH};
@@ -25,26 +27,35 @@ pub(crate) struct WBallPositionConstraint {
impl WBallPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
) -> Self {
let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]);
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1 = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
let (mprops1, ids1) = rbs1;
let (mprops2, ids2) = rbs2;
let local_com1 = Point::from(gather![|ii| mprops1[ii].mass_properties.local_com]);
let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii1 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
])
.squared();
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
let ii2 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
])
.squared();
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let position1 = gather![|ii| ids1[ii].active_set_offset];
let position2 = gather![|ii| ids2[ii].active_set_offset];
Self {
local_com1,
@@ -61,8 +72,8 @@ impl WBallPositionConstraint {
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]);
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
let mut position1 = Isometry::from(gather![|ii| positions[self.position1[ii]]]);
let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
let anchor1 = position1 * self.local_anchor1;
let anchor2 = position2 * self.local_anchor2;
@@ -129,30 +140,36 @@ pub(crate) struct WBallPositionGroundConstraint {
impl WBallPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]);
let poss1 = rbs1;
let (mprops2, ids2) = rbs2;
let position1 = Isometry::from(gather![|ii| poss1[ii].next_position]);
let anchor1 = position1
* Point::from(array![|ii| if flipped[ii] {
* Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2 = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
)
}]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2 = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
])
.squared();
let local_anchor2 = Point::from(array![|ii| if flipped[ii] {
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}; SIMD_WIDTH]);
let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]);
}]);
let position2 = gather![|ii| ids2[ii].active_set_offset];
let local_com2 = Point::from(gather![|ii| mprops2[ii].mass_properties.local_com]);
Self {
anchor1,
@@ -165,7 +182,7 @@ impl WBallPositionGroundConstraint {
}
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]);
let mut position2 = Isometry::from(gather![|ii| positions[self.position2[ii]]]);
let anchor2 = position2 * self.local_anchor2;
let com2 = position2 * self.local_com2;

114
src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngVector, AngularInertia, Real, SdpMatrix, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -36,19 +37,32 @@ impl BallVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &BallJoint,
) -> Self {
let anchor_world1 = rb1.position * joint.local_anchor1;
let anchor_world2 = rb2.position * joint.local_anchor2;
let anchor1 = anchor_world1 - rb1.world_com;
let anchor2 = anchor_world2 - rb2.world_com;
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let anchor_world1 = poss1.position * joint.local_anchor1;
let anchor_world2 = poss2.position * joint.local_anchor2;
let anchor1 = anchor_world1 - mprops1.world_com;
let anchor2 = anchor_world2 - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -59,12 +73,12 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
{
lhs = rb2
lhs = mprops2
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
.add_diagonal(im2)
+ rb1
+ mprops1
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat1)
@@ -75,8 +89,8 @@ impl BallVelocityConstraint {
// it's just easier that way.
#[cfg(feature = "dim2")]
{
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2;
let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2;
let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2;
@@ -100,8 +114,8 @@ impl BallVelocityConstraint {
);
if stiffness != 0.0 {
let dpos = rb2.position.rotation
* (rb1.position.rotation * joint.motor_target_pos).inverse();
let dpos = poss2.position.rotation
* (poss1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")]
{
motor_rhs += dpos.angle() * stiffness;
@@ -113,15 +127,15 @@ impl BallVelocityConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.angvel - rb1.angvel;
let curr_vel = vels2.angvel - vels1.angvel;
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
#[cfg(feature = "dim2")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(gamma / (ii1 + ii2))
} else {
Some(gamma)
@@ -132,8 +146,8 @@ impl BallVelocityConstraint {
#[cfg(feature = "dim3")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some((ii1 + ii2).inverse_unchecked() * gamma)
} else {
Some(SdpMatrix::diagonal(gamma))
@@ -151,8 +165,8 @@ impl BallVelocityConstraint {
BallVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
im2,
impulse: joint.impulse * params.warmstart_coeff,
@@ -164,8 +178,8 @@ impl BallVelocityConstraint {
motor_impulse,
motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
}
}
@@ -269,29 +283,37 @@ impl BallVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &BallJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let (anchor_world1, anchor_world2) = if flipped {
(
rb1.position * joint.local_anchor2,
rb2.position * joint.local_anchor1,
poss1.position * joint.local_anchor2,
poss2.position * joint.local_anchor1,
)
} else {
(
rb1.position * joint.local_anchor1,
rb2.position * joint.local_anchor2,
poss1.position * joint.local_anchor1,
poss2.position * joint.local_anchor2,
)
};
let anchor1 = anchor_world1 - rb1.world_com;
let anchor2 = anchor_world2 - rb2.world_com;
let anchor1 = anchor_world1 - mprops1.world_com;
let anchor2 = anchor_world2 - mprops2.world_com;
let im2 = rb2.effective_inv_mass;
let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1);
let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2);
let im2 = mprops2.effective_inv_mass;
let vel1 = vels1.linvel + vels1.angvel.gcross(anchor1);
let vel2 = vels2.linvel + vels2.angvel.gcross(anchor2);
let rhs = (vel2 - vel1) * params.velocity_solve_fraction
+ (anchor_world2 - anchor_world1) * params.velocity_based_erp_inv_dt();
@@ -302,7 +324,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
lhs = rb2
lhs = mprops2
.effective_world_inv_inertia_sqrt
.squared()
.quadform(&cmat2)
@@ -311,7 +333,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim2")]
{
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let m11 = im2 + cmat2.x * cmat2.x * ii2;
let m12 = cmat2.x * cmat2.y * ii2;
let m22 = im2 + cmat2.y * cmat2.y * ii2;
@@ -335,8 +357,8 @@ impl BallVelocityGroundConstraint {
);
if stiffness != 0.0 {
let dpos = rb2.position.rotation
* (rb1.position.rotation * joint.motor_target_pos).inverse();
let dpos = poss2.position.rotation
* (poss1.position.rotation * joint.motor_target_pos).inverse();
#[cfg(feature = "dim2")]
{
motor_rhs += dpos.angle() * stiffness;
@@ -348,14 +370,14 @@ impl BallVelocityGroundConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.angvel - rb1.angvel;
let curr_vel = vels2.angvel - vels1.angvel;
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
#[cfg(feature = "dim2")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(gamma / ii2)
} else {
Some(gamma)
@@ -366,7 +388,7 @@ impl BallVelocityGroundConstraint {
#[cfg(feature = "dim3")]
if stiffness != 0.0 || damping != 0.0 {
motor_inv_lhs = if keep_lhs {
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
Some(ii2.inverse_unchecked() * gamma)
} else {
Some(SdpMatrix::diagonal(gamma))
@@ -384,7 +406,7 @@ impl BallVelocityGroundConstraint {
BallVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
impulse: joint.impulse * params.warmstart_coeff,
r2: anchor2,
@@ -394,7 +416,7 @@ impl BallVelocityGroundConstraint {
motor_impulse,
motor_inv_lhs,
motor_max_impulse: joint.motor_max_impulse,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
}
}

182
src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SdpMatrix, SimdReal, Vector, SIMD_WIDTH,
@@ -34,33 +35,46 @@ impl WBallVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
let anchor_world1 = position1 * local_anchor1;
let anchor_world2 = position2 * local_anchor2;
@@ -114,20 +128,16 @@ impl WBallVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
mj_lambda1.linear += self.impulse * self.im1;
@@ -147,20 +157,16 @@ impl WBallVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -214,33 +220,49 @@ impl WBallVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = Vector::from(gather![|ii| cparams[ii].impulse]);
let anchor_world1 = position1 * local_anchor1;
let anchor_world2 = position2 * local_anchor2;
@@ -287,12 +309,10 @@ impl WBallVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
mj_lambda2.linear -= self.impulse * self.im2;
@@ -306,12 +326,10 @@ impl WBallVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let angvel = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

48
src/dynamics/solver/joint_constraint/fixed_position_constraint.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation};
use crate::utils::WAngularInertia;
@@ -20,25 +22,32 @@ pub(crate) struct FixedPositionConstraint {
}
impl FixedPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &FixedJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();
Self {
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
im1,
im2,
ii1,
ii2,
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
lin_inv_lhs,
ang_inv_lhs,
}
@@ -91,29 +100,32 @@ pub(crate) struct FixedPositionGroundConstraint {
impl FixedPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &FixedJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
let anchor1;
let local_anchor2;
if flipped {
anchor1 = rb1.next_position * cparams.local_anchor2;
anchor1 = poss1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
} else {
anchor1 = rb1.next_position * cparams.local_anchor1;
anchor1 = poss1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
};
Self {
anchor1,
local_anchor2,
position2: rb2.active_set_offset,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com2: rb2.mass_properties.local_com,
position2: ids2.active_set_offset,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_com2: mprops2.mass_properties.local_com,
impulse: 0.0,
}
}

34
src/dynamics/solver/joint_constraint/fixed_position_constraint_wide.rs
View File

@@ -1,5 +1,7 @@
use super::{FixedPositionConstraint, FixedPositionGroundConstraint};
use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{
FixedJoint, IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WFixedPositionConstraint {
impl WFixedPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| FixedPositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WFixedPositionGroundConstraint {
impl WFixedPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| FixedPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| FixedPositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

86
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, SpacialVector, Vector, DIM};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -45,18 +46,31 @@ impl FixedVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
cparams: &FixedJoint,
) -> Self {
let anchor1 = rb1.position * cparams.local_anchor1;
let anchor2 = rb2.position * cparams.local_anchor2;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1.translation.vector - rb1.world_com.coords;
let r2 = anchor2.translation.vector - rb2.world_com.coords;
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor1 = poss1.position * cparams.local_anchor1;
let anchor2 = poss2.position * cparams.local_anchor2;
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1.translation.vector - mprops1.world_com.coords;
let r2 = anchor2.translation.vector - mprops2.world_com.coords;
let rmat1 = r1.gcross_matrix();
let rmat2 = r2.gcross_matrix();
@@ -99,8 +113,9 @@ impl FixedVelocityConstraint {
#[cfg(feature = "dim3")]
let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2);
let ang_dvel = -rb1.angvel + rb2.angvel;
let lin_dvel =
-vels1.linvel - vels1.angvel.gcross(r1) + vels2.linvel + vels2.angvel.gcross(r2);
let ang_dvel = -vels1.angvel + vels2.angvel;
#[cfg(feature = "dim2")]
let mut rhs =
@@ -133,14 +148,14 @@ impl FixedVelocityConstraint {
FixedVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
im2,
ii1,
ii2,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
r1,
@@ -250,28 +265,36 @@ impl FixedVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
cparams: &FixedJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let (anchor1, anchor2) = if flipped {
(
rb1.position * cparams.local_anchor2,
rb2.position * cparams.local_anchor1,
poss1.position * cparams.local_anchor2,
poss2.position * cparams.local_anchor1,
)
} else {
(
rb1.position * cparams.local_anchor1,
rb2.position * cparams.local_anchor2,
poss1.position * cparams.local_anchor1,
poss2.position * cparams.local_anchor2,
)
};
let r1 = anchor1.translation.vector - rb1.world_com.coords;
let r1 = anchor1.translation.vector - mprops1.world_com.coords;
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2.translation.vector - rb2.world_com.coords;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2.translation.vector - mprops2.world_com.coords;
let rmat2 = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -310,8 +333,9 @@ impl FixedVelocityGroundConstraint {
#[cfg(feature = "dim3")]
let inv_lhs = lhs.cholesky().expect("Singular system.").inverse();
let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1);
let ang_dvel = rb2.angvel - rb1.angvel;
let lin_dvel =
vels2.linvel + vels2.angvel.gcross(r2) - vels1.linvel - vels1.angvel.gcross(r1);
let ang_dvel = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs =
@@ -343,10 +367,10 @@ impl FixedVelocityGroundConstraint {
FixedVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: cparams.impulse * params.warmstart_coeff,
inv_lhs,
r2,

188
src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs
View File

@@ -2,7 +2,8 @@ use simba::simd::SimdValue;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody,
FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, CrossMatrix, Isometry, Point, Real, SimdReal, SpacialVector, Vector,
@@ -53,33 +54,46 @@ impl WFixedVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -157,8 +171,7 @@ impl WFixedVelocityConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector6::new(
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
) * velocity_based_erp_inv_dt;
@@ -185,20 +198,16 @@ impl WFixedVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
@@ -229,20 +238,16 @@ impl WFixedVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -326,33 +331,49 @@ impl WFixedVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&FixedJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -423,8 +444,7 @@ impl WFixedVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector6::new(
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
) * velocity_based_erp_inv_dt;
@@ -446,12 +466,10 @@ impl WFixedVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<DIM>(0).into_owned();
@@ -473,12 +491,10 @@ impl WFixedVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

2
src/dynamics/solver/joint_constraint/generic_position_constraint.rs
View File

@@ -1,6 +1,6 @@
use super::{GenericVelocityConstraint, GenericVelocityGroundConstraint};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{GenericJoint, IntegrationParameters, RigidBody};
use crate::dynamics::{GenericJoint, IntegrationParameters};
use crate::math::{
AngDim, AngVector, AngularInertia, Dim, Isometry, Point, Real, Rotation, SpatialVector, Vector,
DIM,

13
src/dynamics/solver/joint_constraint/generic_position_constraint_wide.rs
View File

@@ -15,7 +15,11 @@ impl WGenericPositionConstraint {
cparams: [&GenericJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| GenericPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| GenericPositionConstraint::from_params(
rbs1[ii],
rbs2[ii],
cparams[ii]
)],
}
}
@@ -39,7 +43,12 @@ impl WGenericPositionGroundConstraint {
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| GenericPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| GenericPositionGroundConstraint::from_params(
rbs1[ii],
rbs2[ii],
cparams[ii],
flipped[ii]
)],
}
}

16
src/dynamics/solver/joint_constraint/generic_velocity_constraint.rs
View File

@@ -50,8 +50,8 @@ impl GenericVelocityConstraint {
rb1: &RigidBody,
rb2: &RigidBody,
) -> SpatialVector<Real> {
let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel - rb1.angvel.gcross(*r1)))
+ basis2.inverse_transform_vector(&(rb2.linvel + rb2.angvel.gcross(*r2)));
let lin_dvel = basis1.inverse_transform_vector(&(-rb1.linvel() - rb1.angvel().gcross(*r1)))
+ basis2.inverse_transform_vector(&(rb2.linvel() + rb2.angvel().gcross(*r2)));
let ang_dvel = basis1.inverse_transform_vector(&-rb1.angvel)
+ basis2.inverse_transform_vector(&rb2.angvel);
@@ -203,8 +203,8 @@ impl GenericVelocityConstraint {
rb2: &RigidBody,
joint: &GenericJoint,
) -> Self {
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let anchor1 = rb1.position() * joint.local_anchor1;
let anchor2 = rb2.position() * joint.local_anchor2;
let basis1 = anchor1.rotation;
let basis2 = anchor2.rotation;
let im1 = rb1.effective_inv_mass;
@@ -405,13 +405,13 @@ impl GenericVelocityGroundConstraint {
) -> Self {
let (anchor1, anchor2) = if flipped {
(
rb1.position * joint.local_anchor2,
rb2.position * joint.local_anchor1,
rb1.position() * joint.local_anchor2,
rb2.position() * joint.local_anchor1,
)
} else {
(
rb1.position * joint.local_anchor1,
rb2.position * joint.local_anchor2,
rb1.position() * joint.local_anchor1,
rb2.position() * joint.local_anchor2,
)
};

146
src/dynamics/solver/joint_constraint/generic_velocity_constraint_wide.rs
View File

@@ -57,29 +57,29 @@ impl WGenericVelocityConstraint {
rbs2: [&RigidBody; SIMD_WIDTH],
cparams: [&GenericJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| rbs1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| rbs1[ii].active_set_offset];
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let local_anchor1 = Isometry::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Isometry::from(gather![|ii| cparams[ii].local_anchor2]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -160,20 +160,16 @@ impl WGenericVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -204,20 +200,16 @@ impl WGenericVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -306,28 +298,32 @@ impl WGenericVelocityGroundConstraint {
cparams: [&GenericJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let position1 = Isometry::from(gather![|ii| rbs1[ii].position]);
let linvel1 = Vector::from(gather![|ii| *rbs1[ii].linvel()]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| *rbs1[ii].angvel()]);
let world_com1 = Point::from(gather![|ii| rbs1[ii].world_com]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position2 = Isometry::from(gather![|ii| rbs2[ii].position]);
let linvel2 = Vector::from(gather![|ii| *rbs2[ii].linvel()]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| *rbs2[ii].angvel()]);
let world_com2 = Point::from(gather![|ii| rbs2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| rbs2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| rbs2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| rbs2[ii].active_set_offset];
let local_anchor1 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let local_anchor1 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let impulse = SpacialVector::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -395,12 +391,10 @@ impl WGenericVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<Dim>(0).into_owned();
@@ -422,12 +416,10 @@ impl WGenericVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2: DeltaVel<SimdReal> = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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

@@ -16,9 +16,11 @@ use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint};
// use crate::dynamics::solver::joint_constraint::generic_velocity_constraint::{
// GenericVelocityConstraint, GenericVelocityGroundConstraint,
// };
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet,
IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyType, RigidBodyVelocity,
};
use crate::math::Real;
#[cfg(feature = "simd-is-enabled")]
@@ -69,14 +71,30 @@ impl AnyJointVelocityConstraint {
1
}
pub fn from_joint(
pub fn from_joint<Bodies>(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &Joint,
bodies: &RigidBodySet,
) -> Self {
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let rb1 = (
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
bodies.index(joint.body1.0),
);
let rb2 = (
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
bodies.index(joint.body2.0),
);
match &joint.params {
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint(
@@ -99,45 +117,59 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(
pub fn from_wide_joint<Bodies>(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
joints: [&Joint; SIMD_WIDTH],
bodies: &RigidBodySet,
) -> Self {
let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let rbs1 = (
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
);
let rbs2 = (
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params(
params, joint_id, rbs1, rbs2, joints,
))
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params(
params, joint_id, rbs1, rbs2, joints,
))
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointVelocityConstraint::WGenericConstraint(
// WGenericVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointVelocityConstraint::WPrismaticConstraint(
WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointVelocityConstraint::WRevoluteConstraint(
WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints),
)
@@ -145,20 +177,31 @@ impl AnyJointVelocityConstraint {
}
}
pub fn from_joint_ground(
pub fn from_joint_ground<Bodies>(
params: &IntegrationParameters,
joint_id: JointIndex,
joint: &Joint,
bodies: &RigidBodySet,
) -> Self {
let mut rb1 = &bodies[joint.body1];
let mut rb2 = &bodies[joint.body2];
let flipped = !rb2.is_dynamic();
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
if flipped {
std::mem::swap(&mut rb1, &mut rb2);
std::mem::swap(&mut handle1, &mut handle2);
}
let rb1 = bodies.index_bundle(handle1.0);
let rb2 = bodies.index_bundle(handle2.0);
match &joint.params {
JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint(
BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped),
@@ -186,26 +229,46 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(
pub fn from_wide_joint_ground<Bodies>(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
joints: [&Joint; SIMD_WIDTH],
bodies: &RigidBodySet,
) -> Self {
let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
bodies: &Bodies,
) -> Self
where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handles1 = gather![|ii| joints[ii].body1];
let mut handles2 = gather![|ii| joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !rbs2[ii].is_dynamic() {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
if !status2[ii].is_dynamic() {
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = true;
}
}
let rbs1 = (
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
gather![|ii| bodies.index(handles1[ii].0)],
);
let rbs2 = (
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)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointVelocityConstraint::WBallGroundConstraint(
WBallVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -213,7 +276,7 @@ impl AnyJointVelocityConstraint {
)
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointVelocityConstraint::WFixedGroundConstraint(
WFixedVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -221,7 +284,7 @@ impl AnyJointVelocityConstraint {
)
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointVelocityConstraint::WGenericGroundConstraint(
// WGenericVelocityGroundConstraint::from_params(
// params, joint_id, rbs1, rbs2, joints, flipped,
@@ -229,8 +292,7 @@ impl AnyJointVelocityConstraint {
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointVelocityConstraint::WPrismaticGroundConstraint(
WPrismaticVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,
@@ -239,8 +301,7 @@ impl AnyJointVelocityConstraint {
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointVelocityConstraint::WRevoluteGroundConstraint(
WRevoluteVelocityGroundConstraint::from_params(
params, joint_id, rbs1, rbs2, joints, flipped,

99
src/dynamics/solver/joint_constraint/joint_position_constraint.rs
View File

@@ -13,7 +13,11 @@ use super::{
WFixedPositionGroundConstraint, WPrismaticPositionConstraint,
WPrismaticPositionGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{
IntegrationParameters, Joint, JointParams, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
RigidBodyType,
};
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
use crate::math::{Isometry, Real};
@@ -56,9 +60,12 @@ pub(crate) enum AnyJointPositionConstraint {
}
impl AnyJointPositionConstraint {
pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self {
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
pub fn from_joint<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
{
let rb1 = bodies.index_bundle(joint.body1.0);
let rb2 = bodies.index_bundle(joint.body2.0);
match &joint.params {
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint(
@@ -81,40 +88,47 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
pub fn from_wide_joint<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyMassProps> + ComponentSet<RigidBodyIds>,
{
let rbs1 = (
gather![|ii| bodies.index(joints[ii].body1.0)],
gather![|ii| bodies.index(joints[ii].body1.0)],
);
let rbs2 = (
gather![|ii| bodies.index(joints[ii].body2.0)],
gather![|ii| bodies.index(joints[ii].body2.0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointPositionConstraint::WBallJoint(WBallPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointPositionConstraint::WFixedJoint(WFixedPositionConstraint::from_params(
rbs1, rbs2, joints,
))
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointPositionConstraint::WGenericJoint(WGenericPositionConstraint::from_params(
// rbs1, rbs2, joints,
// ))
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointPositionConstraint::WPrismaticJoint(
WPrismaticPositionConstraint::from_params(rbs1, rbs2, joints),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointPositionConstraint::WRevoluteJoint(
WRevolutePositionConstraint::from_params(rbs1, rbs2, joints),
)
@@ -122,15 +136,26 @@ impl AnyJointPositionConstraint {
}
}
pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self {
let mut rb1 = &bodies[joint.body1];
let mut rb2 = &bodies[joint.body2];
let flipped = !rb2.is_dynamic();
pub fn from_joint_ground<Bodies>(joint: &Joint, bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handle1 = joint.body1;
let mut handle2 = joint.body2;
let status2: &RigidBodyType = bodies.index(handle2.0);
let flipped = !status2.is_dynamic();
if flipped {
std::mem::swap(&mut rb1, &mut rb2);
std::mem::swap(&mut handle1, &mut handle2);
}
let rb1 = bodies.index(handle1.0);
let rb2 = (bodies.index(handle2.0), bodies.index(handle2.0));
match &joint.params {
JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint(
BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped),
@@ -154,48 +179,60 @@ impl AnyJointPositionConstraint {
}
#[cfg(feature = "simd-is-enabled")]
pub fn from_wide_joint_ground(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Self {
let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH];
pub fn from_wide_joint_ground<Bodies>(joints: [&Joint; SIMD_WIDTH], bodies: &Bodies) -> Self
where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let mut handles1 = gather![|ii| joints[ii].body1];
let mut handles2 = gather![|ii| joints[ii].body2];
let status2: [&RigidBodyType; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].0)];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !rbs2[ii].is_dynamic() {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
if !status2[ii].is_dynamic() {
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = true;
}
}
let rbs1 = gather![|ii| bodies.index(handles1[ii].0)];
let rbs2 = (
gather![|ii| bodies.index(handles2[ii].0)],
gather![|ii| bodies.index(handles2[ii].0)],
);
match &joints[0].params {
JointParams::BallJoint(_) => {
let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_ball_joint().unwrap()];
AnyJointPositionConstraint::WBallGroundConstraint(
WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
JointParams::FixedJoint(_) => {
let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_fixed_joint().unwrap()];
AnyJointPositionConstraint::WFixedGroundConstraint(
WFixedPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
// JointParams::GenericJoint(_) => {
// let joints = array![|ii| joints[ii].params.as_generic_joint().unwrap(); SIMD_WIDTH];
// let joints = gather![|ii| joints[ii].params.as_generic_joint().unwrap()];
// AnyJointPositionConstraint::WGenericGroundConstraint(
// WGenericPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
// )
// }
JointParams::PrismaticJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_prismatic_joint().unwrap()];
AnyJointPositionConstraint::WPrismaticGroundConstraint(
WPrismaticPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)
}
#[cfg(feature = "dim3")]
JointParams::RevoluteJoint(_) => {
let joints =
array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH];
let joints = gather![|ii| joints[ii].params.as_revolute_joint().unwrap()];
AnyJointPositionConstraint::WRevoluteGroundConstraint(
WRevolutePositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped),
)

42
src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
use crate::utils::WAngularInertia;
use na::Unit;
@@ -27,11 +29,18 @@ pub(crate) struct PrismaticPositionConstraint {
}
impl PrismaticPositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &PrismaticJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let lin_inv_lhs = 1.0 / (im1 + im2);
let ang_inv_lhs = (ii1 + ii2).inverse();
@@ -46,8 +55,8 @@ impl PrismaticPositionConstraint {
local_frame2: cparams.local_frame2(),
local_axis1: cparams.local_axis1,
local_axis2: cparams.local_axis2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
limits: cparams.limits,
}
}
@@ -108,25 +117,28 @@ pub(crate) struct PrismaticPositionGroundConstraint {
impl PrismaticPositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &PrismaticJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (_, ids2) = rb2;
let frame1;
let local_frame2;
let axis1;
let local_axis2;
if flipped {
frame1 = rb1.next_position * cparams.local_frame2();
frame1 = poss1.next_position * cparams.local_frame2();
local_frame2 = cparams.local_frame1();
axis1 = rb1.next_position * cparams.local_axis2;
axis1 = poss1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
} else {
frame1 = rb1.next_position * cparams.local_frame1();
frame1 = poss1.next_position * cparams.local_frame1();
local_frame2 = cparams.local_frame2();
axis1 = rb1.next_position * cparams.local_axis1;
axis1 = poss1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
};
@@ -135,7 +147,7 @@ impl PrismaticPositionGroundConstraint {
local_frame2,
axis1,
local_axis2,
position2: rb2.active_set_offset,
position2: ids2.active_set_offset,
limits: cparams.limits,
}
}

34
src/dynamics/solver/joint_constraint/prismatic_position_constraint_wide.rs
View File

@@ -1,5 +1,7 @@
use super::{PrismaticPositionConstraint, PrismaticPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, PrismaticJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WPrismaticPositionConstraint {
impl WPrismaticPositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| PrismaticPositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WPrismaticPositionGroundConstraint {
impl WPrismaticPositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| PrismaticPositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| PrismaticPositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

138
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix, WDot};
@@ -74,32 +75,45 @@ impl PrismaticVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &PrismaticJoint,
) -> Self {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
// Linear part.
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let axis1 = rb1.position * joint.local_axis1;
let axis2 = rb2.position * joint.local_axis2;
let anchor1 = poss1.position * joint.local_anchor1;
let anchor2 = poss2.position * joint.local_anchor2;
let axis1 = poss1.position * joint.local_axis1;
let axis2 = poss2.position * joint.local_axis2;
#[cfg(feature = "dim2")]
let basis1 = rb1.position * joint.basis1[0];
let basis1 = poss1.position * joint.basis1[0];
#[cfg(feature = "dim3")]
let basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
let im1 = rb1.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let im1 = mprops1.effective_inv_mass;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r1_mat = r1.gcross_matrix();
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -131,8 +145,8 @@ impl PrismaticVelocityConstraint {
lhs = SdpMatrix2::new(m11, m12, m22);
}
let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
// for which a textbook inverse is still efficient.
@@ -142,7 +156,7 @@ impl PrismaticVelocityConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
let angvel_err = rb2.angvel - rb1.angvel;
let angvel_err = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
@@ -159,8 +173,8 @@ impl PrismaticVelocityConstraint {
if velocity_based_erp_inv_dt != 0.0 {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = rb1.position * joint.local_frame1();
let frame2 = rb2.position * joint.local_frame2();
let frame1 = poss1.position * joint.local_frame1();
let frame2 = poss2.position * joint.local_frame2();
let ang_err = frame2.rotation * frame1.rotation.inverse();
#[cfg(feature = "dim2")]
@@ -195,9 +209,9 @@ impl PrismaticVelocityConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.linvel.dot(&axis2) + rb2.angvel.gdot(gcross2)
- rb1.linvel.dot(&axis1)
- rb1.angvel.gdot(gcross1);
let curr_vel = vels2.linvel.dot(&axis2) + vels2.angvel.gdot(gcross2)
- vels1.linvel.dot(&axis1)
- vels1.angvel.gdot(gcross1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -266,12 +280,12 @@ impl PrismaticVelocityConstraint {
PrismaticVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
limits_active,
limits_impulse: limits_impulse * params.warmstart_coeff,
@@ -501,11 +515,19 @@ impl PrismaticVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &PrismaticJoint,
flipped: bool,
) -> Self {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor2;
let anchor1;
let axis2;
@@ -513,35 +535,35 @@ impl PrismaticVelocityGroundConstraint {
let basis1;
if flipped {
anchor2 = rb2.position * joint.local_anchor1;
anchor1 = rb1.position * joint.local_anchor2;
axis2 = rb2.position * joint.local_axis1;
axis1 = rb1.position * joint.local_axis2;
anchor2 = poss2.position * joint.local_anchor1;
anchor1 = poss1.position * joint.local_anchor2;
axis2 = poss2.position * joint.local_axis1;
axis1 = poss1.position * joint.local_axis2;
#[cfg(feature = "dim2")]
{
basis1 = rb1.position * joint.basis2[0];
basis1 = poss1.position * joint.basis2[0];
}
#[cfg(feature = "dim3")]
{
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis2[0],
rb1.position * joint.basis2[1],
poss1.position * joint.basis2[0],
poss1.position * joint.basis2[1],
]);
}
} else {
anchor2 = rb2.position * joint.local_anchor2;
anchor1 = rb1.position * joint.local_anchor1;
axis2 = rb2.position * joint.local_axis2;
axis1 = rb1.position * joint.local_axis1;
anchor2 = poss2.position * joint.local_anchor2;
anchor1 = poss1.position * joint.local_anchor1;
axis2 = poss2.position * joint.local_axis2;
axis1 = poss1.position * joint.local_axis1;
#[cfg(feature = "dim2")]
{
basis1 = rb1.position * joint.basis1[0];
basis1 = poss1.position * joint.basis1[0];
}
#[cfg(feature = "dim3")]
{
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
}
};
@@ -560,10 +582,10 @@ impl PrismaticVelocityGroundConstraint {
// simplifications of the computation without introducing
// much instabilities.
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
#[allow(unused_mut)] // For 2D.
@@ -592,8 +614,8 @@ impl PrismaticVelocityGroundConstraint {
lhs = SdpMatrix2::new(m11, m12, m22);
}
let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1);
let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2);
let anchor_linvel1 = vels1.linvel + vels1.angvel.gcross(r1);
let anchor_linvel2 = vels2.linvel + vels2.angvel.gcross(r2);
// NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix
// for which a textbook inverse is still efficient.
@@ -603,7 +625,7 @@ impl PrismaticVelocityGroundConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1));
let angvel_err = rb2.angvel - rb1.angvel;
let angvel_err = vels2.angvel - vels1.angvel;
#[cfg(feature = "dim2")]
let mut rhs = Vector2::new(linvel_err.x, angvel_err) * params.velocity_solve_fraction;
@@ -622,11 +644,11 @@ impl PrismaticVelocityGroundConstraint {
let (frame1, frame2);
if flipped {
frame1 = rb1.position * joint.local_frame2();
frame2 = rb2.position * joint.local_frame1();
frame1 = poss1.position * joint.local_frame2();
frame2 = poss2.position * joint.local_frame1();
} else {
frame1 = rb1.position * joint.local_frame1();
frame2 = rb2.position * joint.local_frame2();
frame1 = poss1.position * joint.local_frame1();
frame2 = poss2.position * joint.local_frame2();
}
let ang_err = frame2.rotation * frame1.rotation.inverse();
@@ -660,7 +682,7 @@ impl PrismaticVelocityGroundConstraint {
}
if damping != 0.0 {
let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1);
let curr_vel = vels2.linvel.dot(&axis2) - vels1.linvel.dot(&axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -714,9 +736,9 @@ impl PrismaticVelocityGroundConstraint {
PrismaticVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
limits_active,
limits_forcedir2,

288
src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs
View File

@@ -2,7 +2,8 @@ use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, SimdBool, SimdReal, Vector, SIMD_WIDTH,
@@ -71,47 +72,60 @@ impl WPrismaticVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]);
let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]);
let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| cparams[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| cparams[ii].local_anchor2]);
let local_axis1 = Vector::from(gather![|ii| *cparams[ii].local_axis1]);
let local_axis2 = Vector::from(gather![|ii| *cparams[ii].local_axis2]);
#[cfg(feature = "dim2")]
let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])];
let local_basis1 = [Vector::from(gather![|ii| cparams[ii].basis1[0]])];
#[cfg(feature = "dim3")]
let local_basis1 = [
Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]),
Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| cparams[ii].basis1[0]]),
Vector::from(gather![|ii| cparams[ii].basis1[1]]),
];
#[cfg(feature = "dim2")]
let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
#[cfg(feature = "dim3")]
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -207,8 +221,8 @@ impl WPrismaticVelocityConstraint {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let local_frame1 = Isometry::from(array![|ii| cparams[ii].local_frame1(); SIMD_WIDTH]);
let local_frame2 = Isometry::from(array![|ii| cparams[ii].local_frame2(); SIMD_WIDTH]);
let local_frame1 = Isometry::from(gather![|ii| cparams[ii].local_frame1()]);
let local_frame2 = Isometry::from(gather![|ii| cparams[ii].local_frame2()]);
let frame1 = position1 * local_frame1;
let frame2 = position2 * local_frame2;
@@ -221,8 +235,7 @@ impl WPrismaticVelocityConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
* velocity_based_erp_inv_dt;
}
@@ -237,15 +250,15 @@ impl WPrismaticVelocityConstraint {
let mut limits_inv_lhs = zero;
let mut limits_impulse_limits = (zero, zero);
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
if limits_enabled.any() {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
// TODO: we should allow predictive constraint activation.
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
@@ -265,10 +278,9 @@ impl WPrismaticVelocityConstraint {
- (min_limit - dist).simd_max(zero))
* SimdReal::splat(velocity_based_erp_inv_dt);
limits_impulse =
SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_inv_lhs = SimdReal::splat(1.0)
/ (im1
@@ -303,20 +315,16 @@ impl WPrismaticVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
@@ -428,20 +436,16 @@ impl WPrismaticVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
self.solve_dofs(&mut mj_lambda1, &mut mj_lambda2);
@@ -510,59 +514,85 @@ impl WPrismaticVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&PrismaticJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
#[cfg(feature = "dim2")]
let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector2::from(gather![|ii| cparams[ii].impulse]);
#[cfg(feature = "dim3")]
let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| cparams[ii].impulse]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH],
);
let local_axis1 = Vector::from(
array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH],
);
let local_axis2 = Vector::from(
array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH],
);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor2
} else {
cparams[ii].local_anchor1
}]);
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
cparams[ii].local_anchor1
} else {
cparams[ii].local_anchor2
}]);
let local_axis1 = Vector::from(gather![|ii| if flipped[ii] {
*cparams[ii].local_axis2
} else {
*cparams[ii].local_axis1
}]);
let local_axis2 = Vector::from(gather![|ii| if flipped[ii] {
*cparams[ii].local_axis1
} else {
*cparams[ii].local_axis2
}]);
#[cfg(feature = "dim2")]
let basis1 = position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
);
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[0]
} else {
cparams[ii].basis1[0]
}]);
#[cfg(feature = "dim3")]
let basis1 = Matrix3x2::from_columns(&[
position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[0]
} else {
cparams[ii].basis1[0]
}]),
position1
* Vector::from(
array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
cparams[ii].basis2[1]
} else {
cparams[ii].basis1[1]
}]),
]);
let anchor1 = position1 * local_anchor1;
@@ -634,13 +664,17 @@ impl WPrismaticVelocityGroundConstraint {
let linear_err = basis1.tr_mul(&(anchor2 - anchor1));
let frame1 = position1
* Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_frame2() } else { cparams[ii].local_frame1() }; SIMD_WIDTH],
);
* Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_frame2()
} else {
cparams[ii].local_frame1()
}]);
let frame2 = position2
* Isometry::from(
array![|ii| if flipped[ii] { cparams[ii].local_frame1() } else { cparams[ii].local_frame2() }; SIMD_WIDTH],
);
* Isometry::from(gather![|ii| if flipped[ii] {
cparams[ii].local_frame1()
} else {
cparams[ii].local_frame2()
}]);
let ang_err = frame2.rotation * frame1.rotation.inverse();
@@ -651,8 +685,7 @@ impl WPrismaticVelocityGroundConstraint {
#[cfg(feature = "dim3")]
{
let ang_err =
Vector3::from(array![|ii| ang_err.extract(ii).scaled_axis(); SIMD_WIDTH]);
let ang_err = Vector3::from(gather![|ii| ang_err.extract(ii).scaled_axis()]);
rhs += Vector5::new(linear_err.x, linear_err.y, ang_err.x, ang_err.y, ang_err.z)
* velocity_based_erp_inv_dt;
}
@@ -666,14 +699,14 @@ impl WPrismaticVelocityGroundConstraint {
let mut limits_impulse = zero;
let mut limits_impulse_limits = (zero, zero);
let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]);
let limits_enabled = SimdBool::from(gather![|ii| cparams[ii].limits_enabled]);
if limits_enabled.any() {
let danchor = anchor2 - anchor1;
let dist = danchor.dot(&axis1);
// TODO: we should allow predictive constraint activation.
let min_limit = SimdReal::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]);
let max_limit = SimdReal::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]);
let min_limit = SimdReal::from(gather![|ii| cparams[ii].limits[0]]);
let max_limit = SimdReal::from(gather![|ii| cparams[ii].limits[1]]);
let min_enabled = dist.simd_lt(min_limit);
let max_enabled = dist.simd_gt(max_limit);
@@ -690,10 +723,9 @@ impl WPrismaticVelocityGroundConstraint {
- (min_limit - dist).simd_max(zero))
* SimdReal::splat(velocity_based_erp_inv_dt);
limits_impulse =
SimdReal::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
limits_impulse = SimdReal::from(gather![|ii| cparams[ii].limits_impulse])
.simd_max(limits_impulse_limits.0)
.simd_min(limits_impulse_limits.1);
}
}
@@ -718,12 +750,10 @@ impl WPrismaticVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.basis1 * self.impulse.fixed_rows::<LIN_IMPULSE_DIM>(0).into_owned();
@@ -791,12 +821,10 @@ impl WPrismaticVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
self.solve_dofs(&mut mj_lambda2);

60
src/dynamics/solver/joint_constraint/revolute_position_constraint.rs
View File

@@ -1,4 +1,6 @@
use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{AngularInertia, Isometry, Point, Real, Rotation, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::Unit;
@@ -29,11 +31,18 @@ pub(crate) struct RevolutePositionConstraint {
}
impl RevolutePositionConstraint {
pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self {
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
pub fn from_params(
rb1: (&RigidBodyMassProps, &RigidBodyIds),
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &RevoluteJoint,
) -> Self {
let (mprops1, ids1) = rb1;
let (mprops2, ids2) = rb2;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ang_inv_lhs = (ii1 + ii2).inverse();
Self {
@@ -42,14 +51,14 @@ impl RevolutePositionConstraint {
ii1,
ii2,
ang_inv_lhs,
local_com1: rb1.mass_properties.local_com,
local_com2: rb2.mass_properties.local_com,
local_com1: mprops1.mass_properties.local_com,
local_com2: mprops2.mass_properties.local_com,
local_anchor1: cparams.local_anchor1,
local_anchor2: cparams.local_anchor2,
local_axis1: cparams.local_axis1,
local_axis2: cparams.local_axis2,
position1: rb1.active_set_offset,
position2: rb2.active_set_offset,
position1: ids1.active_set_offset,
position2: ids2.active_set_offset,
local_basis1: cparams.basis1,
local_basis2: cparams.basis2,
}
@@ -132,11 +141,14 @@ pub(crate) struct RevolutePositionGroundConstraint {
impl RevolutePositionGroundConstraint {
pub fn from_params(
rb1: &RigidBody,
rb2: &RigidBody,
rb1: &RigidBodyPosition,
rb2: (&RigidBodyMassProps, &RigidBodyIds),
cparams: &RevoluteJoint,
flipped: bool,
) -> Self {
let poss1 = rb1;
let (mprops2, ids2) = rb2;
let anchor1;
let local_anchor2;
let axis1;
@@ -145,23 +157,23 @@ impl RevolutePositionGroundConstraint {
let local_basis2;
if flipped {
anchor1 = rb1.next_position * cparams.local_anchor2;
anchor1 = poss1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
axis1 = rb1.next_position * cparams.local_axis2;
axis1 = poss1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
basis1 = [
rb1.next_position * cparams.basis2[0],
rb1.next_position * cparams.basis2[1],
poss1.next_position * cparams.basis2[0],
poss1.next_position * cparams.basis2[1],
];
local_basis2 = cparams.basis1;
} else {
anchor1 = rb1.next_position * cparams.local_anchor1;
anchor1 = poss1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
axis1 = rb1.next_position * cparams.local_axis1;
axis1 = poss1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
basis1 = [
rb1.next_position * cparams.basis1[0],
rb1.next_position * cparams.basis1[1],
poss1.next_position * cparams.basis1[0],
poss1.next_position * cparams.basis1[1],
];
local_basis2 = cparams.basis2;
};
@@ -169,12 +181,12 @@ impl RevolutePositionGroundConstraint {
Self {
anchor1,
local_anchor2,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_com2: rb2.mass_properties.local_com,
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
local_com2: mprops2.mass_properties.local_com,
axis1,
local_axis2,
position2: rb2.active_set_offset,
position2: ids2.active_set_offset,
basis1,
local_basis2,
}

34
src/dynamics/solver/joint_constraint/revolute_position_constraint_wide.rs
View File

@@ -1,5 +1,7 @@
use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody};
use crate::dynamics::{
IntegrationParameters, RevoluteJoint, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition,
};
use crate::math::{Isometry, Real, SIMD_WIDTH};
// TODO: this does not uses SIMD optimizations yet.
@@ -10,12 +12,22 @@ pub(crate) struct WRevolutePositionConstraint {
impl WRevolutePositionConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&RevoluteJoint; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii]); SIMD_WIDTH],
constraints: gather![|ii| RevolutePositionConstraint::from_params(
(rbs1.0[ii], rbs1.1[ii]),
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii]
)],
}
}
@@ -33,13 +45,21 @@ pub(crate) struct WRevolutePositionGroundConstraint {
impl WRevolutePositionGroundConstraint {
pub fn from_params(
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: [&RigidBodyPosition; SIMD_WIDTH],
rbs2: (
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
cparams: [&RevoluteJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
Self {
constraints: array![|ii| RevolutePositionGroundConstraint::from_params(rbs1[ii], rbs2[ii], cparams[ii], flipped[ii]); SIMD_WIDTH],
constraints: gather![|ii| RevolutePositionGroundConstraint::from_params(
rbs1[ii],
(rbs2.0[ii], rbs2.1[ii]),
cparams[ii],
flipped[ii]
)],
}
}

140
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -1,6 +1,7 @@
use crate::dynamics::solver::{AnyJointVelocityConstraint, DeltaVel};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{AngularInertia, Real, Rotation, Vector};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
@@ -43,34 +44,47 @@ impl RevoluteVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &RevoluteJoint,
) -> Self {
let (poss1, vels1, mprops1, ids1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
// Linear part.
let anchor1 = rb1.position * joint.local_anchor1;
let anchor2 = rb2.position * joint.local_anchor2;
let anchor1 = poss1.position * joint.local_anchor1;
let anchor2 = poss2.position * joint.local_anchor2;
let basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
let basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis2[0],
rb2.position * joint.basis2[1],
poss2.position * joint.basis2[0],
poss2.position * joint.basis2[1],
]);
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
let im1 = rb1.effective_inv_mass;
let im2 = rb2.effective_inv_mass;
let im1 = mprops1.effective_inv_mass;
let im2 = mprops2.effective_inv_mass;
let ii1 = rb1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let ii1 = mprops1.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r1_mat = r1.gcross_matrix();
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - rb2.world_com;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
let mut lhs = Matrix5::zeros();
@@ -90,8 +104,8 @@ impl RevoluteVelocityConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err =
(rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
(vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
let mut rhs = Vector5::new(
linvel_err.x,
@@ -105,8 +119,8 @@ impl RevoluteVelocityConstraint {
if velocity_based_erp_inv_dt != 0.0 {
let lin_err = anchor2 - anchor1;
let axis1 = rb1.position * joint.local_axis1;
let axis2 = rb2.position * joint.local_axis2;
let axis1 = poss1.position * joint.local_axis1;
let axis2 = poss2.position * joint.local_axis2;
let axis_error = axis1.cross(&axis2);
let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5;
@@ -118,8 +132,8 @@ impl RevoluteVelocityConstraint {
/*
* Motor.
*/
let motor_axis1 = rb1.position * *joint.local_axis1;
let motor_axis2 = rb2.position * *joint.local_axis2;
let motor_axis1 = poss1.position * *joint.local_axis1;
let motor_axis2 = poss2.position * *joint.local_axis2;
let mut motor_rhs = 0.0;
let mut motor_inv_lhs = 0.0;
let mut motor_angle = 0.0;
@@ -132,12 +146,12 @@ impl RevoluteVelocityConstraint {
);
if stiffness != 0.0 {
motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
}
if damping != 0.0 {
let curr_vel = rb2.angvel.dot(&motor_axis2) - rb1.angvel.dot(&motor_axis1);
let curr_vel = vels2.angvel.dot(&motor_axis2) - vels1.angvel.dot(&motor_axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -171,14 +185,14 @@ impl RevoluteVelocityConstraint {
RevoluteVelocityConstraint {
joint_id,
mj_lambda1: rb1.active_set_offset,
mj_lambda2: rb2.active_set_offset,
mj_lambda1: ids1.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im1,
ii1_sqrt: rb1.effective_world_inv_inertia_sqrt,
ii1_sqrt: mprops1.effective_world_inv_inertia_sqrt,
basis1,
basis2,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse,
inv_lhs,
rhs,
@@ -330,11 +344,19 @@ impl RevoluteVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: JointIndex,
rb1: &RigidBody,
rb2: &RigidBody,
rb1: (&RigidBodyPosition, &RigidBodyVelocity, &RigidBodyMassProps),
rb2: (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyIds,
),
joint: &RevoluteJoint,
flipped: bool,
) -> AnyJointVelocityConstraint {
let (poss1, vels1, mprops1) = rb1;
let (poss2, vels2, mprops2, ids2) = rb2;
let anchor2;
let anchor1;
let axis1;
@@ -343,39 +365,39 @@ impl RevoluteVelocityGroundConstraint {
let basis2;
if flipped {
axis1 = rb1.position * *joint.local_axis2;
axis2 = rb2.position * *joint.local_axis1;
anchor1 = rb1.position * joint.local_anchor2;
anchor2 = rb2.position * joint.local_anchor1;
axis1 = poss1.position * *joint.local_axis2;
axis2 = poss2.position * *joint.local_axis1;
anchor1 = poss1.position * joint.local_anchor2;
anchor2 = poss2.position * joint.local_anchor1;
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis2[0],
rb1.position * joint.basis2[1],
poss1.position * joint.basis2[0],
poss1.position * joint.basis2[1],
]);
basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis1[0],
rb2.position * joint.basis1[1],
poss2.position * joint.basis1[0],
poss2.position * joint.basis1[1],
]);
} else {
axis1 = rb1.position * *joint.local_axis1;
axis2 = rb2.position * *joint.local_axis2;
anchor1 = rb1.position * joint.local_anchor1;
anchor2 = rb2.position * joint.local_anchor2;
axis1 = poss1.position * *joint.local_axis1;
axis2 = poss2.position * *joint.local_axis2;
anchor1 = poss1.position * joint.local_anchor1;
anchor2 = poss2.position * joint.local_anchor2;
basis1 = Matrix3x2::from_columns(&[
rb1.position * joint.basis1[0],
rb1.position * joint.basis1[1],
poss1.position * joint.basis1[0],
poss1.position * joint.basis1[1],
]);
basis2 = Matrix3x2::from_columns(&[
rb2.position * joint.basis2[0],
rb2.position * joint.basis2[1],
poss2.position * joint.basis2[0],
poss2.position * joint.basis2[1],
]);
};
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
let im2 = rb2.effective_inv_mass;
let ii2 = rb2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - rb1.world_com;
let r2 = anchor2 - rb2.world_com;
let im2 = mprops2.effective_inv_mass;
let ii2 = mprops2.effective_world_inv_inertia_sqrt.squared();
let r1 = anchor1 - mprops1.world_com;
let r2 = anchor2 - mprops2.world_com;
let r2_mat = r2.gcross_matrix();
let mut lhs = Matrix5::zeros();
@@ -393,8 +415,8 @@ impl RevoluteVelocityGroundConstraint {
let inv_lhs = Cholesky::new_unchecked(lhs).inverse();
let linvel_err =
(rb2.linvel + rb2.angvel.gcross(r2)) - (rb1.linvel + rb1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&rb2.angvel) - basis1.tr_mul(&rb1.angvel);
(vels2.linvel + vels2.angvel.gcross(r2)) - (vels1.linvel + vels1.angvel.gcross(r1));
let angvel_err = basis2.tr_mul(&vels2.angvel) - basis1.tr_mul(&vels1.angvel);
let mut rhs = Vector5::new(
linvel_err.x,
linvel_err.y,
@@ -409,11 +431,11 @@ impl RevoluteVelocityGroundConstraint {
let (axis1, axis2);
if flipped {
axis1 = rb1.position * joint.local_axis2;
axis2 = rb2.position * joint.local_axis1;
axis1 = poss1.position * joint.local_axis2;
axis2 = poss2.position * joint.local_axis1;
} else {
axis1 = rb1.position * joint.local_axis1;
axis2 = rb2.position * joint.local_axis2;
axis1 = poss1.position * joint.local_axis1;
axis2 = poss2.position * joint.local_axis2;
}
let axis_error = axis1.cross(&axis2);
let ang_err = basis2.tr_mul(&axis_error);
@@ -437,12 +459,12 @@ impl RevoluteVelocityGroundConstraint {
);
if stiffness != 0.0 {
motor_angle = joint.estimate_motor_angle(&rb1.position, &rb2.position);
motor_angle = joint.estimate_motor_angle(&poss1.position, &poss2.position);
motor_rhs += (motor_angle - joint.motor_target_pos) * stiffness;
}
if damping != 0.0 {
let curr_vel = rb2.angvel.dot(&axis2) - rb1.angvel.dot(&axis1);
let curr_vel = vels2.angvel.dot(&axis2) - vels1.angvel.dot(&axis1);
motor_rhs += (curr_vel - joint.motor_target_vel) * damping;
}
@@ -460,9 +482,9 @@ impl RevoluteVelocityGroundConstraint {
let result = RevoluteVelocityGroundConstraint {
joint_id,
mj_lambda2: rb2.active_set_offset,
mj_lambda2: ids2.active_set_offset,
im2,
ii2_sqrt: rb2.effective_world_inv_inertia_sqrt,
ii2_sqrt: mprops2.effective_world_inv_inertia_sqrt,
impulse: joint.impulse * params.warmstart_coeff,
basis2,
inv_lhs,

250
src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
View File

@@ -2,7 +2,8 @@ use simba::simd::SimdValue;
use crate::dynamics::solver::DeltaVel;
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody,
IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
@@ -39,41 +40,54 @@ impl WRevoluteVelocityConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
joints: [&RevoluteJoint; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let (poss1, vels1, mprops1, ids1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops1[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let local_anchor1 = Point::from(array![|ii| joints[ii].local_anchor1; SIMD_WIDTH]);
let local_anchor2 = Point::from(array![|ii| joints[ii].local_anchor2; SIMD_WIDTH]);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let local_anchor1 = Point::from(gather![|ii| joints[ii].local_anchor1]);
let local_anchor2 = Point::from(gather![|ii| joints[ii].local_anchor2]);
let local_basis1 = [
Vector::from(array![|ii| joints[ii].basis1[0]; SIMD_WIDTH]),
Vector::from(array![|ii| joints[ii].basis1[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| joints[ii].basis1[0]]),
Vector::from(gather![|ii| joints[ii].basis1[1]]),
];
let local_basis2 = [
Vector::from(array![|ii| joints[ii].basis2[0]; SIMD_WIDTH]),
Vector::from(array![|ii| joints[ii].basis2[1]; SIMD_WIDTH]),
Vector::from(gather![|ii| joints[ii].basis2[0]]),
Vector::from(gather![|ii| joints[ii].basis2[1]]),
];
let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
let anchor1 = position1 * local_anchor1;
let anchor2 = position2 * local_anchor2;
@@ -124,10 +138,8 @@ impl WRevoluteVelocityConstraint {
let lin_err = anchor2 - anchor1;
let local_axis1 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);
let local_axis2 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis2; SIMD_WIDTH]);
let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis1]);
let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| joints[ii].local_axis2]);
let axis1 = position1 * local_axis1;
let axis2 = position2 * local_axis2;
@@ -150,12 +162,12 @@ impl WRevoluteVelocityConstraint {
let warmstart_coeff = SimdReal::splat(params.warmstart_coeff);
let mut impulse = impulse * warmstart_coeff;
let axis1 = array![|ii| rbs1[ii].position * *joints[ii].local_axis1; SIMD_WIDTH];
let rotated_impulse = Vector::from(array![|ii| {
let axis1 = gather![|ii| poss1[ii].position * *joints[ii].local_axis1];
let rotated_impulse = Vector::from(gather![|ii| {
let axis_rot = Rotation::rotation_between(&joints[ii].prev_axis1, &axis1[ii])
.unwrap_or_else(Rotation::identity);
axis_rot * joints[ii].world_ang_impulse
}; SIMD_WIDTH]);
}]);
let rotated_basis_impulse = basis1.tr_mul(&rotated_impulse);
impulse[3] = rotated_basis_impulse.x * warmstart_coeff;
@@ -182,20 +194,16 @@ impl WRevoluteVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse1 = self.impulse.fixed_rows::<3>(0).into_owned();
@@ -225,20 +233,16 @@ impl WRevoluteVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular);
@@ -314,52 +318,76 @@ impl WRevoluteVelocityGroundConstraint {
pub fn from_params(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
rbs1: [&RigidBody; SIMD_WIDTH],
rbs2: [&RigidBody; SIMD_WIDTH],
rbs1: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
),
rbs2: (
[&RigidBodyPosition; SIMD_WIDTH],
[&RigidBodyVelocity; SIMD_WIDTH],
[&RigidBodyMassProps; SIMD_WIDTH],
[&RigidBodyIds; SIMD_WIDTH],
),
joints: [&RevoluteJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let (poss1, vels1, mprops1) = rbs1;
let (poss2, vels2, mprops2, ids2) = rbs2;
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let impulse = Vector5::from(array![|ii| joints[ii].impulse; SIMD_WIDTH]);
let position1 = Isometry::from(gather![|ii| poss1[ii].position]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let local_anchor1 = Point::from(
array![|ii| if flipped[ii] { joints[ii].local_anchor2 } else { joints[ii].local_anchor1 }; SIMD_WIDTH],
);
let local_anchor2 = Point::from(
array![|ii| if flipped[ii] { joints[ii].local_anchor1 } else { joints[ii].local_anchor2 }; SIMD_WIDTH],
);
let position2 = Isometry::from(gather![|ii| poss2[ii].position]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2_sqrt = AngularInertia::<SimdReal>::from(gather![
|ii| mprops2[ii].effective_world_inv_inertia_sqrt
]);
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let impulse = Vector5::from(gather![|ii| joints[ii].impulse]);
let local_anchor1 = Point::from(gather![|ii| if flipped[ii] {
joints[ii].local_anchor2
} else {
joints[ii].local_anchor1
}]);
let local_anchor2 = Point::from(gather![|ii| if flipped[ii] {
joints[ii].local_anchor1
} else {
joints[ii].local_anchor2
}]);
let basis1 = Matrix3x2::from_columns(&[
position1
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis2[0] } else { joints[ii].basis1[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis2[0]
} else {
joints[ii].basis1[0]
}]),
position1
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis2[1] } else { joints[ii].basis1[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis2[1]
} else {
joints[ii].basis1[1]
}]),
]);
let basis2 = Matrix3x2::from_columns(&[
position2
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis1[0] } else { joints[ii].basis2[0] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis1[0]
} else {
joints[ii].basis2[0]
}]),
position2
* Vector::from(
array![|ii| if flipped[ii] { joints[ii].basis1[1] } else { joints[ii].basis2[1] }; SIMD_WIDTH],
),
* Vector::from(gather![|ii| if flipped[ii] {
joints[ii].basis1[1]
} else {
joints[ii].basis2[1]
}]),
]);
let basis_projection2 = basis2 * basis2.transpose();
let basis2 = basis_projection2 * basis1;
@@ -403,12 +431,16 @@ impl WRevoluteVelocityGroundConstraint {
let lin_err = anchor2 - anchor1;
let local_axis1 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH],
);
let local_axis2 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; SIMD_WIDTH],
);
let local_axis1 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
joints[ii].local_axis2
} else {
joints[ii].local_axis1
}]);
let local_axis2 = Unit::<Vector<_>>::from(gather![|ii| if flipped[ii] {
joints[ii].local_axis1
} else {
joints[ii].local_axis2
}]);
let axis1 = position1 * local_axis1;
let axis2 = position2 * local_axis2;
@@ -434,12 +466,10 @@ impl WRevoluteVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let lin_impulse = self.impulse.fixed_rows::<3>(0).into_owned();
@@ -458,12 +488,10 @@ impl WRevoluteVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);

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

@@ -1,9 +1,10 @@
use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver};
use crate::data::ComponentSet;
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyJointVelocityConstraint, AnyPositionConstraint,
AnyVelocityConstraint, ParallelPositionSolver, ParallelSolverConstraints,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Isometry, Real};
use crate::utils::WAngularInertia;
@@ -150,13 +151,15 @@ impl ParallelIslandSolver {
}
}
pub fn solve_position_constraints<'s>(
pub fn solve_position_constraints<'s, Bodies>(
&'s mut self,
scope: &Scope<'s>,
island_id: usize,
params: &'s IntegrationParameters,
bodies: &'s mut RigidBodySet,
) {
bodies: &'s mut Bodies,
) where
Bodies: ComponentSet<RigidBody>,
{
let num_threads = rayon::current_num_threads();
let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
@@ -179,7 +182,7 @@ impl ParallelIslandSolver {
// Transmute *mut -> &mut
let positions: &mut Vec<Isometry<Real>> =
unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) };
let bodies: &mut RigidBodySet =
let bodies: &mut Bodies =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
let parallel_contact_constraints: &mut ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> = unsafe {
std::mem::transmute(parallel_contact_constraints.load(Ordering::Relaxed))
@@ -197,8 +200,8 @@ impl ParallelIslandSolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
let rb = &mut bodies[handle.0];
for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) {
let rb = &mut bodies.index(handle.0);
positions[rb.active_set_offset] = rb.next_position;
}
}
@@ -216,8 +219,8 @@ impl ParallelIslandSolver {
// Write results back to rigid bodies.
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.position_writeback_index] {
let rb = &mut bodies[handle.0];
for handle in active_bodies.index(thread.position_writeback_index) {
let rb = &mut bodies.index(handle.0);
rb.set_next_position(positions[rb.active_set_offset]);
}
}
@@ -225,17 +228,19 @@ impl ParallelIslandSolver {
}
}
pub fn init_constraints_and_solve_velocity_constraints<'s>(
pub fn init_constraints_and_solve_velocity_constraints<'s, Bodies>(
&'s mut self,
scope: &Scope<'s>,
island_id: usize,
params: &'s IntegrationParameters,
bodies: &'s mut RigidBodySet,
bodies: &'s mut Bodies,
manifolds: &'s mut Vec<&'s mut ContactManifold>,
manifold_indices: &'s [ContactManifoldIndex],
joints: &'s mut Vec<JointGraphEdge>,
joint_indices: &[JointIndex],
) {
) where
Bodies: ComponentSet<RigidBody>,
{
let num_threads = rayon::current_num_threads();
let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island?
self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here?
@@ -280,7 +285,7 @@ impl ParallelIslandSolver {
// Transmute *mut -> &mut
let mj_lambdas: &mut Vec<DeltaVel<Real>> =
unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) };
let bodies: &mut RigidBodySet =
let bodies: &mut Bodies =
unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) };
let manifolds: &mut Vec<&mut ContactManifold> =
unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) };
@@ -303,8 +308,8 @@ impl ParallelIslandSolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_force_integration_index, thread.num_force_integrated_bodies] {
let rb = &mut bodies[handle.0];
for handle in active_bodies.index(thread.body_force_integration_index, thread.num_force_integrated_bodies) {
let rb = &mut bodies.index(handle.0);
let dvel = &mut mj_lambdas[rb.active_set_offset];
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
@@ -348,8 +353,8 @@ impl ParallelIslandSolver {
concurrent_loop! {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] {
let rb = &mut bodies[handle.0];
for handle in active_bodies.index(thread.body_integration_index, thread.num_integrated_bodies) {
let rb = &mut bodies.index(handle.0);
let dvel = mj_lambdas[rb.active_set_offset];
rb.linvel += dvel.linear;
rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);

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

@@ -5,7 +5,7 @@ use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, InteractionGroups, PositionConstraint,
PositionGroundConstraint, VelocityConstraint, VelocityGroundConstraint,
};
use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet};
use crate::dynamics::{IntegrationParameters, JointGraphEdge};
use crate::geometry::ContactManifold;
#[cfg(feature = "simd-is-enabled")]
use crate::{
@@ -20,7 +20,7 @@ use std::sync::atomic::Ordering;
// pub fn init_constraint_groups(
// &mut self,
// island_id: usize,
// bodies: &RigidBodySet,
// bodies: &impl ComponentSet<RigidBody>,
// manifolds: &mut [&mut ContactManifold],
// manifold_groups: &ParallelInteractionGroups,
// joints: &mut [JointGraphEdge],
@@ -36,9 +36,9 @@ pub(crate) enum ConstraintDesc {
NongroundNongrouped(usize),
GroundNongrouped(usize),
#[cfg(feature = "simd-is-enabled")]
NongroundGrouped([usize; SIMD_WIDTH]),
NongroundGrouped([usize]),
#[cfg(feature = "simd-is-enabled")]
GroundGrouped([usize; SIMD_WIDTH]),
GroundGrouped([usize]),
}
pub(crate) struct ParallelSolverConstraints<VelocityConstraint, PositionConstraint> {
@@ -78,7 +78,7 @@ macro_rules! impl_init_constraints_group {
pub fn init_constraint_groups(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBody>,
interactions: &mut [$Interaction],
interaction_groups: &ParallelInteractionGroups,
) {
@@ -144,7 +144,7 @@ macro_rules! impl_init_constraints_group {
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::NongroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
gather![|ii| interaction_i[ii]],
),
));
total_num_constraints += $num_active_constraints(interaction);
@@ -172,7 +172,7 @@ macro_rules! impl_init_constraints_group {
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::GroundGrouped(
array![|ii| interaction_i[ii]; SIMD_WIDTH],
gather![|ii| interaction_i[ii]],
),
));
total_num_constraints += $num_active_constraints(interaction);
@@ -223,7 +223,7 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBody>,
manifolds_all: &[&mut ContactManifold],
) {
let descs = &self.constraint_descs;
@@ -244,13 +244,13 @@ impl ParallelSolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
WPositionConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(manifold_id) => {
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH];
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, false);
WPositionGroundConstraint::generate(params, manifolds, bodies, &mut self.position_constraints, false);
}
@@ -265,7 +265,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
&mut self,
thread: &ThreadContext,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &impl ComponentSet<RigidBody>,
joints_all: &[JointGraphEdge],
) {
let descs = &self.constraint_descs;
@@ -290,7 +290,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::NongroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
let position_constraint = AnyJointPositionConstraint::from_wide_joint(joints, bodies);
self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;
@@ -298,7 +298,7 @@ impl ParallelSolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConst
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::GroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let joints = gather![|ii| &joints_all[joint_id[ii]].weight];
let velocity_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
let position_constraint = AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies);
self.velocity_constraints[joints[0].constraint_index] = velocity_constraint;

44
src/dynamics/solver/position_constraint.rs
View File

@@ -1,7 +1,8 @@
use crate::data::ComponentSet;
use crate::dynamics::solver::PositionGroundConstraint;
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
@@ -51,15 +52,26 @@ pub(crate) struct PositionConstraint {
}
impl PositionConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold: &ContactManifold,
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let ids1: &RigidBodyIds = bodies.index(handle1.0);
let ids2: &RigidBodyIds = bodies.index(handle2.0);
let poss1: &RigidBodyPosition = bodies.index(handle1.0);
let poss2: &RigidBodyPosition = bodies.index(handle2.0);
let mprops1: &RigidBodyMassProps = bodies.index(handle1.0);
let mprops2: &RigidBodyMassProps = bodies.index(handle2.0);
for (l, manifold_points) in manifold
.data
@@ -72,26 +84,28 @@ impl PositionConstraint {
let mut dists = [0.0; MAX_MANIFOLD_POINTS];
for l in 0..manifold_points.len() {
local_p1[l] = rb1
local_p1[l] = poss1
.position
.inverse_transform_point(&manifold_points[l].point);
local_p2[l] = rb2
local_p2[l] = poss2
.position
.inverse_transform_point(&manifold_points[l].point);
dists[l] = manifold_points[l].dist;
}
let constraint = PositionConstraint {
rb1: rb1.active_set_offset,
rb2: rb2.active_set_offset,
rb1: ids1.active_set_offset,
rb2: ids2.active_set_offset,
local_p1,
local_p2,
local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal),
local_n1: poss1
.position
.inverse_transform_vector(&manifold.data.normal),
dists,
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
ii1: rb1.effective_world_inv_inertia_sqrt.squared(),
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
ii1: mprops1.effective_world_inv_inertia_sqrt.squared(),
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
num_contacts: manifold_points.len() as u8,
erp: params.erp,
max_linear_correction: params.max_linear_correction,

61
src/dynamics/solver/position_constraint_wide.rs
View File

@@ -1,5 +1,5 @@
use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
@@ -7,6 +7,7 @@ use crate::math::{
};
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::data::ComponentSet;
use num::Zero;
use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
@@ -28,39 +29,47 @@ pub(crate) struct WPositionConstraint {
}
impl WPositionConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let poss1: [&RigidBodyPosition; SIMD_WIDTH] = gather![|ii| bodies.index(handles1[ii].0)];
let poss2: [&RigidBodyPosition; 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 pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let sqrt_ii1: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
let local_n1 = pos1.inverse_transform_vector(&Vector::from(
array![|ii| manifolds[ii].data.normal; SIMD_WIDTH],
));
let pos1 = Isometry::from(gather![|ii| poss1[ii].position]);
let pos2 = Isometry::from(gather![|ii| poss2[ii].position]);
let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let local_n1 =
pos1.inverse_transform_vector(&Vector::from(gather![|ii| manifolds[ii].data.normal]));
let rb1 = gather![|ii| ids1[ii].active_set_offset];
let rb2 = gather![|ii| ids2[ii].active_set_offset];
let num_active_contacts = manifolds[0].data.num_active_contacts();
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WPositionConstraint {
@@ -80,8 +89,8 @@ impl WPositionConstraint {
};
for i in 0..num_points {
let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
let point = Point::from(gather![|ii| manifold_points[ii][i].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]);
constraint.local_p1[i] = pos1.inverse_transform_point(&point);
constraint.local_p2[i] = pos2.inverse_transform_point(&point);
constraint.dists[i] = dist;
@@ -99,8 +108,8 @@ impl WPositionConstraint {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let mut pos1 = Isometry::from(gather![|ii| positions[self.rb1[ii]]]);
let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
for k in 0..self.num_contacts as usize {

33
src/dynamics/solver/position_ground_constraint.rs
View File

@@ -1,5 +1,6 @@
use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS,
@@ -21,24 +22,28 @@ pub(crate) struct PositionGroundConstraint {
}
impl PositionGroundConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold: &ContactManifold,
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2];
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
let flip = manifold.data.relative_dominance < 0;
let n1 = if flip {
std::mem::swap(&mut rb1, &mut rb2);
-manifold.data.normal
let (handle2, n1) = if flip {
(manifold.data.rigid_body1.unwrap(), -manifold.data.normal)
} else {
manifold.data.normal
(manifold.data.rigid_body2.unwrap(), manifold.data.normal)
};
let (ids2, poss2, mprops2): (&RigidBodyIds, &RigidBodyPosition, &RigidBodyMassProps) =
bodies.index_bundle(handle2.0);
for (l, manifold_contacts) in manifold
.data
.solver_contacts
@@ -51,20 +56,20 @@ impl PositionGroundConstraint {
for k in 0..manifold_contacts.len() {
p1[k] = manifold_contacts[k].point;
local_p2[k] = rb2
local_p2[k] = poss2
.position
.inverse_transform_point(&manifold_contacts[k].point);
dists[k] = manifold_contacts[k].dist;
}
let constraint = PositionGroundConstraint {
rb2: rb2.active_set_offset,
rb2: ids2.active_set_offset,
p1,
local_p2,
n1,
dists,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
im2: mprops2.effective_inv_mass,
ii2: mprops2.effective_world_inv_inertia_sqrt.squared(),
num_contacts: manifold_contacts.len() as u8,
erp: params.erp,
max_linear_correction: params.max_linear_correction,

54
src/dynamics/solver/position_ground_constraint_wide.rs
View File

@@ -1,5 +1,5 @@
use super::AnyPositionConstraint;
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyPosition};
use crate::geometry::ContactManifold;
use crate::math::{
AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Translation, Vector,
@@ -7,6 +7,7 @@ use crate::math::{
};
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::data::ComponentSet;
use num::Zero;
use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue};
@@ -25,42 +26,51 @@ pub(crate) struct WPositionGroundConstraint {
}
impl WPositionGroundConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let mut rbs1 =
array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let mut rbs2 =
array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>,
{
let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if manifolds[ii].data.relative_dominance < 0 {
flipped[ii] = true;
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
}
}
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let sqrt_ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let poss2: [&RigidBodyPosition; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let n1 = Vector::from(
array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH],
);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let n1 = Vector::from(gather![|ii| if flipped[ii] {
-manifolds[ii].data.normal
} else {
manifolds[ii].data.normal
}]);
let pos2 = Isometry::from(gather![|ii| poss2[ii].position]);
let rb2 = gather![|ii| ids2[ii].active_set_offset];
let num_active_contacts = manifolds[0].data.num_active_contacts();
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WPositionGroundConstraint {
@@ -77,8 +87,8 @@ impl WPositionGroundConstraint {
};
for i in 0..num_points {
let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
let point = Point::from(gather![|ii| manifold_points[ii][i].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][i].dist]);
constraint.p1[i] = point;
constraint.local_p2[i] = pos2.inverse_transform_point(&point);
constraint.dists[i] = dist;
@@ -96,7 +106,7 @@ impl WPositionGroundConstraint {
pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry<Real>]) {
// FIXME: can we avoid most of the multiplications by pos1/pos2?
// Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(gather![|ii| positions[self.rb2[ii]]]);
let allowed_err = SimdReal::splat(params.allowed_linear_error);
for k in 0..self.num_contacts as usize {

31
src/dynamics/solver/position_solver.rs
View File

@@ -1,5 +1,7 @@
use super::AnyJointPositionConstraint;
use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters, RigidBodySet};
use crate::data::{ComponentSet, ComponentSetMut};
use crate::dynamics::{solver::AnyPositionConstraint, IntegrationParameters};
use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyPosition};
use crate::math::{Isometry, Real};
pub(crate) struct PositionSolver {
@@ -13,25 +15,28 @@ impl PositionSolver {
}
}
pub fn solve(
pub fn solve<Bodies>(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
islands: &IslandManager,
bodies: &mut Bodies,
contact_constraints: &[AnyPositionConstraint],
joint_constraints: &[AnyJointPositionConstraint],
) {
) where
Bodies: ComponentSet<RigidBodyIds> + ComponentSetMut<RigidBodyPosition>,
{
if contact_constraints.is_empty() && joint_constraints.is_empty() {
// Nothing to do.
return;
}
self.positions.clear();
self.positions.extend(
bodies
.iter_active_island(island_id)
.map(|(_, b)| b.next_position),
);
self.positions
.extend(islands.active_island(island_id).iter().map(|h| {
let poss: &RigidBodyPosition = bodies.index(h.0);
poss.next_position
}));
for _ in 0..params.max_position_iterations {
for constraint in joint_constraints {
@@ -43,8 +48,10 @@ impl PositionSolver {
}
}
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.set_next_position(self.positions[rb.active_set_offset])
});
for handle in islands.active_island(island_id) {
let ids: &RigidBodyIds = bodies.index(handle.0);
let next_pos = &self.positions[ids.active_set_offset];
bodies.map_mut_internal(handle.0, |poss| poss.next_position = *next_pos);
}
}
}

152
src/dynamics/solver/solver_constraints.rs
View File

@@ -5,13 +5,16 @@ use super::{
use super::{
WPositionConstraint, WPositionGroundConstraint, WVelocityConstraint, WVelocityGroundConstraint,
};
use crate::data::ComponentSet;
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::{
AnyJointPositionConstraint, AnyPositionConstraint, PositionConstraint, PositionGroundConstraint,
};
use crate::dynamics::{
solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet,
solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyIds,
RigidBodyMassProps, RigidBodyPosition, RigidBodyType,
};
use crate::dynamics::{IslandManager, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[cfg(feature = "simd-is-enabled")]
use crate::math::SIMD_WIDTH;
@@ -50,13 +53,16 @@ impl<VelocityConstraint, PositionConstraint>
}
impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
pub fn init_constraint_groups(
pub fn init_constraint_groups<Bodies>(
&mut self,
island_id: usize,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
) where
Bodies: ComponentSet<RigidBodyType> + ComponentSet<RigidBodyIds>,
{
self.not_ground_interactions.clear();
self.ground_interactions.clear();
categorize_contacts(
@@ -70,6 +76,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
self.interaction_groups.clear_groups();
self.interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.not_ground_interactions,
@@ -78,6 +85,7 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.ground_interactions,
@@ -92,18 +100,25 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
// .append(&mut self.ground_interaction_groups.grouped_interactions);
}
pub fn init(
pub fn init<Bodies>(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
) {
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
self.velocity_constraints.clear();
self.position_constraints.clear();
self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices);
self.init_constraint_groups(island_id, islands, bodies, manifolds, manifold_indices);
#[cfg(feature = "simd-is-enabled")]
{
@@ -118,19 +133,24 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_constraints(
fn compute_grouped_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
manifolds_all: &[&mut ContactManifold],
) {
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
for manifolds_i in self
.interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
WVelocityConstraint::generate(
params,
manifold_id,
@@ -149,12 +169,17 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
}
fn compute_nongrouped_constraints(
fn compute_nongrouped_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
manifolds_all: &[&mut ContactManifold],
) {
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
for manifold_i in &self.interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityConstraint::generate(
@@ -176,19 +201,24 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_ground_constraints(
fn compute_grouped_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
manifolds_all: &[&mut ContactManifold],
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
for manifolds_i in self
.ground_interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH];
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
WVelocityGroundConstraint::generate(
params,
manifold_id,
@@ -207,12 +237,17 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
}
fn compute_nongrouped_ground_constraints(
fn compute_nongrouped_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
manifolds_all: &[&mut ContactManifold],
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
for manifold_i in &self.ground_interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
VelocityGroundConstraint::generate(
@@ -235,14 +270,21 @@ impl SolverConstraints<AnyVelocityConstraint, AnyPositionConstraint> {
}
impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
pub fn init(
pub fn init<Bodies>(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &RigidBodySet,
islands: &IslandManager,
bodies: &Bodies,
joints: &[JointGraphEdge],
joint_constraint_indices: &[JointIndex],
) {
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>,
{
// Generate constraints for joints.
self.not_ground_interactions.clear();
self.ground_interactions.clear();
@@ -260,6 +302,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
self.interaction_groups.clear_groups();
self.interaction_groups.group_joints(
island_id,
islands,
bodies,
joints,
&self.not_ground_interactions,
@@ -268,6 +311,7 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
self.ground_interaction_groups.clear_groups();
self.ground_interaction_groups.group_joints(
island_id,
islands,
bodies,
joints,
&self.ground_interactions,
@@ -292,12 +336,18 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
}
}
fn compute_nongrouped_joint_ground_constraints(
fn compute_nongrouped_joint_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
joints_all: &[JointGraphEdge],
) {
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyIds>,
{
for joint_i in &self.ground_interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
let vel_constraint =
@@ -309,19 +359,25 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_ground_constraints(
fn compute_grouped_joint_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
joints_all: &[JointGraphEdge],
) {
) where
Bodies: ComponentSet<RigidBodyType>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
for joints_i in self
.ground_interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
let joints_id = gather![|ii| joints_i[ii]];
let joints = gather![|ii| &joints_all[joints_i[ii]].weight];
let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground(
params, joints_id, joints, bodies,
);
@@ -332,12 +388,17 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
}
}
fn compute_nongrouped_joint_constraints(
fn compute_nongrouped_joint_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
joints_all: &[JointGraphEdge],
) {
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
for joint_i in &self.interaction_groups.nongrouped_interactions {
let joint = &joints_all[*joint_i].weight;
let vel_constraint =
@@ -349,19 +410,24 @@ impl SolverConstraints<AnyJointVelocityConstraint, AnyJointPositionConstraint> {
}
#[cfg(feature = "simd-is-enabled")]
fn compute_grouped_joint_constraints(
fn compute_grouped_joint_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &RigidBodySet,
bodies: &Bodies,
joints_all: &[JointGraphEdge],
) {
) where
Bodies: ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>,
{
for joints_i in self
.interaction_groups
.grouped_interactions
.chunks_exact(SIMD_WIDTH)
{
let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH];
let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH];
let joints_id = gather![|ii| joints_i[ii]];
let joints = gather![|ii| &joints_all[joints_i[ii]].weight];
let vel_constraint =
AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies);
self.velocity_constraints.push(vel_constraint);

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

@@ -1,7 +1,8 @@
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::VelocityGroundConstraint;
#[cfg(feature = "simd-is-enabled")]
use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WBasis, WCross, WDot};
@@ -102,23 +103,32 @@ impl VelocityConstraint {
manifold.data.solver_contacts.len() / MAX_MANIFOLD_POINTS + rest as usize
}
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
assert_eq!(manifold.data.relative_dominance, 0);
let inv_dt = params.inv_dt();
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
let mj_lambda1 = rb1.active_set_offset;
let mj_lambda2 = rb2.active_set_offset;
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 force_dir1 = -manifold.data.normal;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
@@ -126,7 +136,7 @@ impl VelocityConstraint {
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let (tangents1, tangent_rot1) =
super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel);
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
for (_l, manifold_points) in manifold
.data
@@ -142,8 +152,8 @@ impl VelocityConstraint {
#[cfg(feature = "dim3")]
tangent_rot1,
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: rb1.effective_inv_mass,
im2: rb2.effective_inv_mass,
im1: mprops1.effective_inv_mass,
im2: mprops2.effective_inv_mass,
limit: 0.0,
mj_lambda1,
mj_lambda2,
@@ -190,8 +200,8 @@ impl VelocityConstraint {
constraint.tangent1 = tangents1[0];
constraint.tangent_rot1 = tangent_rot1;
}
constraint.im1 = rb1.effective_inv_mass;
constraint.im2 = rb2.effective_inv_mass;
constraint.im1 = mprops1.effective_inv_mass;
constraint.im2 = mprops2.effective_inv_mass;
constraint.limit = 0.0;
constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2;
@@ -202,11 +212,11 @@ impl VelocityConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - rb1.world_com;
let dp2 = manifold_point.point - rb2.world_com;
let dp1 = manifold_point.point - mprops1.world_com;
let dp2 = manifold_point.point - mprops2.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
let warmstart_correction;
@@ -215,16 +225,16 @@ impl VelocityConstraint {
// Normal part.
{
let gcross1 = rb1
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(force_dir1));
let gcross2 = rb2
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0
/ (rb1.effective_inv_mass
+ rb2.effective_inv_mass
/ (mprops1.effective_inv_mass
+ mprops2.effective_inv_mass
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
@@ -261,15 +271,15 @@ impl VelocityConstraint {
constraint.elements[k].tangent_part.impulse = impulse;
for j in 0..DIM - 1 {
let gcross1 = rb1
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = rb2
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0
/ (rb1.effective_inv_mass
+ rb2.effective_inv_mass
/ (mprops1.effective_inv_mass
+ mprops2.effective_inv_mass
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let rhs =

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

@@ -1,7 +1,8 @@
use super::{
AnyVelocityConstraint, DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::data::ComponentSet;
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -32,14 +33,18 @@ pub(crate) struct WVelocityConstraint {
}
impl WVelocityConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
for ii in 0..SIMD_WIDTH {
assert_eq!(manifolds[ii].data.relative_dominance, 0);
}
@@ -49,36 +54,39 @@ impl WVelocityConstraint {
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let im1 = SimdReal::from(array![|ii| rbs1[ii].effective_inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs1[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
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 linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = SimdReal::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]);
let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda1 = gather![|ii| ids1[ii].active_set_offset];
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
let num_active_contacts = manifolds[0].data.num_active_contacts();
@@ -89,9 +97,8 @@ impl WVelocityConstraint {
super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii|
&manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH
];
let manifold_points =
gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WVelocityConstraint {
@@ -112,24 +119,20 @@ impl WVelocityConstraint {
};
for k in 0..num_points {
let friction =
SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
let restitution =
SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
let is_bouncy = SimdReal::from(
array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
);
let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]);
let is_bouncy = SimdReal::from(gather![
|ii| manifold_points[ii][k].is_bouncy() as u32 as Real
]);
let is_resting = SimdReal::splat(1.0) - is_bouncy;
let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
);
let prev_rhs =
SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
let impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
@@ -140,8 +143,7 @@ impl WVelocityConstraint {
let warmstart_correction;
constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
// Normal part.
{
@@ -172,15 +174,15 @@ impl WVelocityConstraint {
// tangent parts.
#[cfg(feature = "dim2")]
let impulse = [SimdReal::from(
array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
) * warmstart_correction];
let impulse = [SimdReal::from(gather![
|ii| manifold_points[ii][k].warmstart_tangent_impulse
]) * warmstart_correction];
#[cfg(feature = "dim3")]
let impulse = tangent_rot1
* na::Vector2::from(
array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
)
* na::Vector2::from(gather![
|ii| manifold_points[ii][k].warmstart_tangent_impulse
])
* warmstart_correction;
constraint.elements[k].tangent_part.impulse = impulse;
@@ -210,21 +212,17 @@ impl WVelocityConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
VelocityConstraintElement::warmstart_group(
@@ -250,21 +248,17 @@ impl WVelocityConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda1 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular
]),
};
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
VelocityConstraintElement::solve_group(

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

@@ -2,12 +2,13 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS};
use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS};
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use crate::utils::{WAngularInertia, WCross, WDot};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
#[derive(Copy, Clone, Debug)]
@@ -28,35 +29,50 @@ pub(crate) struct VelocityGroundConstraint {
}
impl VelocityGroundConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
let inv_dt = params.inv_dt();
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let mut handle1 = manifold.data.rigid_body1;
let mut handle2 = manifold.data.rigid_body2;
let flipped = manifold.data.relative_dominance < 0;
let (force_dir1, flipped_multiplier) = if flipped {
std::mem::swap(&mut rb1, &mut rb2);
std::mem::swap(&mut handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (ids2, vels2, mprops2): (&RigidBodyIds, &RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let (tangents1, tangent_rot1) =
super::compute_tangent_contact_directions(&force_dir1, &rb1.linvel, &rb2.linvel);
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let mj_lambda2 = rb2.active_set_offset;
let mj_lambda2 = ids2.active_set_offset;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
for (_l, manifold_points) in manifold
@@ -73,7 +89,7 @@ impl VelocityGroundConstraint {
#[cfg(feature = "dim3")]
tangent_rot1,
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb2.effective_inv_mass,
im2: mprops2.effective_inv_mass,
limit: 0.0,
mj_lambda2,
manifold_id,
@@ -119,7 +135,7 @@ impl VelocityGroundConstraint {
constraint.tangent1 = tangents1[0];
constraint.tangent_rot1 = tangent_rot1;
}
constraint.im2 = rb2.effective_inv_mass;
constraint.im2 = mprops2.effective_inv_mass;
constraint.limit = 0.0;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
@@ -129,10 +145,10 @@ impl VelocityGroundConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp2 = manifold_point.point - rb2.world_com;
let dp1 = manifold_point.point - rb1.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
let dp2 = manifold_point.point - mprops2.world_com;
let dp1 = manifold_point.point - world_com1;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
let warmstart_correction;
constraint.limit = manifold_point.friction;
@@ -140,11 +156,11 @@ impl VelocityGroundConstraint {
// Normal part.
{
let gcross2 = rb2
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2));
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy;
@@ -178,10 +194,10 @@ impl VelocityGroundConstraint {
constraint.elements[k].tangent_part.impulse = impulse;
for j in 0..DIM - 1 {
let gcross2 = rb2
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
let r = 1.0 / (mprops2.effective_inv_mass + gcross2.gdot(gcross2));
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);

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

@@ -2,7 +2,8 @@ use super::{
AnyVelocityConstraint, DeltaVel, VelocityGroundConstraintElement,
VelocityGroundConstraintNormalPart,
};
use crate::dynamics::{IntegrationParameters, RigidBodySet};
use crate::data::ComponentSet;
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
@@ -31,52 +32,71 @@ pub(crate) struct WVelocityGroundConstraint {
}
impl WVelocityGroundConstraint {
pub fn generate(
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
bodies: &Bodies,
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let velocity_based_erp_inv_dt = SimdReal::splat(params.velocity_based_erp_inv_dt());
let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
let mut handles2 = gather![|ii| manifolds[ii].data.rigid_body2];
let mut flipped = [1.0; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if manifolds[ii].data.relative_dominance < 0 {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
std::mem::swap(&mut handles1[ii], &mut handles2[ii]);
flipped[ii] = -1.0;
}
}
let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
handles1[ii]
.map(|h| *bodies.index(h.0))
.unwrap_or_else(RigidBodyVelocity::zero)
}];
let world_com1 = Point::from(gather![|ii| {
handles1[ii]
.map(|h| ComponentSet::<RigidBodyMassProps>::index(bodies, h.0).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)];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
gather![|ii| bodies.index(handles2[ii].unwrap().0)];
let flipped_sign = SimdReal::from(flipped);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
);
let im2 = SimdReal::from(gather![|ii| mprops2[ii].effective_inv_mass]);
let ii2: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops2[ii].effective_world_inv_inertia_sqrt]);
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let normal1 = Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]);
let force_dir1 = normal1 * -flipped_sign;
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = gather![|ii| ids2[ii].active_set_offset];
let warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
SimdReal::from(gather![|ii| manifolds[ii].data.warmstart_multiplier]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
let warmstart_correction_slope = SimdReal::splat(params.warmstart_correction_slope);
let num_active_contacts = manifolds[0].data.num_active_contacts();
@@ -88,7 +108,7 @@ impl WVelocityGroundConstraint {
super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..]; SIMD_WIDTH];
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WVelocityGroundConstraint {
@@ -107,24 +127,20 @@ impl WVelocityGroundConstraint {
};
for k in 0..num_points {
let friction =
SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
let restitution =
SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
let is_bouncy = SimdReal::from(
array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
);
let friction = SimdReal::from(gather![|ii| manifold_points[ii][k].friction]);
let restitution = SimdReal::from(gather![|ii| manifold_points[ii][k].restitution]);
let is_bouncy = SimdReal::from(gather![
|ii| manifold_points[ii][k].is_bouncy() as u32 as Real
]);
let is_resting = SimdReal::splat(1.0) - is_bouncy;
let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
let impulse = SimdReal::from(
array![|ii| manifold_points[ii][k].warmstart_impulse; SIMD_WIDTH],
);
let prev_rhs =
SimdReal::from(array![|ii| manifold_points[ii][k].prev_rhs; SIMD_WIDTH]);
let impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let prev_rhs = SimdReal::from(gather![|ii| manifold_points[ii][k].prev_rhs]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
@@ -133,8 +149,7 @@ impl WVelocityGroundConstraint {
let warmstart_correction;
constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
// Normal part.
{
@@ -162,14 +177,14 @@ impl WVelocityGroundConstraint {
// tangent parts.
#[cfg(feature = "dim2")]
let impulse = [SimdReal::from(
array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
) * warmstart_correction];
let impulse = [SimdReal::from(gather![
|ii| manifold_points[ii][k].warmstart_tangent_impulse
]) * warmstart_correction];
#[cfg(feature = "dim3")]
let impulse = tangent_rot1
* na::Vector2::from(
array![|ii| manifold_points[ii][k].warmstart_tangent_impulse; SIMD_WIDTH],
)
* na::Vector2::from(gather![
|ii| manifold_points[ii][k].warmstart_tangent_impulse
])
* warmstart_correction;
constraint.elements[k].tangent_part.impulse = impulse;
@@ -195,12 +210,10 @@ impl WVelocityGroundConstraint {
pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
VelocityGroundConstraintElement::warmstart_group(
@@ -220,12 +233,10 @@ impl WVelocityGroundConstraint {
pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel<Real>]) {
let mut mj_lambda2 = DeltaVel {
linear: Vector::from(
array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH],
),
angular: AngVector::from(
array![|ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH],
),
linear: Vector::from(gather![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear]),
angular: AngVector::from(gather![
|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular
]),
};
VelocityGroundConstraintElement::solve_group(

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

@@ -1,8 +1,10 @@
use super::AnyJointVelocityConstraint;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, JointGraphEdge, RigidBodySet,
IntegrationParameters, JointGraphEdge, RigidBodyForces, RigidBodyVelocity,
};
use crate::dynamics::{IslandManager, RigidBodyIds, RigidBodyMassProps};
use crate::geometry::ContactManifold;
use crate::math::Real;
use crate::utils::WAngularInertia;
@@ -18,31 +20,38 @@ impl VelocitySolver {
}
}
pub fn solve(
pub fn solve<Bodies>(
&mut self,
island_id: usize,
params: &IntegrationParameters,
bodies: &mut RigidBodySet,
islands: &IslandManager,
bodies: &mut Bodies,
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut [AnyVelocityConstraint],
joint_constraints: &mut [AnyJointVelocityConstraint],
) {
) where
Bodies: ComponentSet<RigidBodyForces>
+ ComponentSet<RigidBodyIds>
+ ComponentSetMut<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>,
{
self.mj_lambdas.clear();
self.mj_lambdas
.resize(bodies.active_island(island_id).len(), DeltaVel::zero());
.resize(islands.active_island(island_id).len(), DeltaVel::zero());
// Initialize delta-velocities (`mj_lambdas`) with external forces (gravity etc):
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
let dvel = &mut self.mj_lambdas[rb.active_set_offset];
for handle in islands.active_island(island_id) {
let (ids, mprops, forces): (&RigidBodyIds, &RigidBodyMassProps, &RigidBodyForces) =
bodies.index_bundle(handle.0);
dvel.linear += rb.force * (rb.effective_inv_mass * params.dt);
rb.force = na::zero();
let dvel = &mut self.mj_lambdas[ids.active_set_offset];
// dvel.angular is actually storing angular velocity delta multiplied by the square root of the inertia tensor:
dvel.angular += rb.effective_world_inv_inertia_sqrt * rb.torque * params.dt;
rb.torque = na::zero();
});
// 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 * (mprops.effective_inv_mass * params.dt);
}
/*
* Warmstart constraints.
@@ -69,13 +78,19 @@ impl VelocitySolver {
}
// Update velocities.
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
let dvel = self.mj_lambdas[rb.active_set_offset];
rb.linvel += dvel.linear;
rb.angvel += rb
for handle in islands.active_island(island_id) {
let (ids, mprops): (&RigidBodyIds, &RigidBodyMassProps) = bodies.index_bundle(handle.0);
let dvel = self.mj_lambdas[ids.active_set_offset];
let dangvel = mprops
.effective_world_inv_inertia_sqrt
.transform_vector(dvel.angular);
});
bodies.map_mut_internal(handle.0, |vels| {
vels.linvel += dvel.linear;
vels.angvel += dangvel;
});
}
// Write impulses back into the manifold structures.
for constraint in &*joint_constraints {