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

feat: solver improvements + release v0.29.0 (#876)

Browse Source 
* feat: solver improvements

* feat: add function to get/set whether gyroscopic forces are enabled on a rigid-body

* chore: switch to released versions of parry and wide instead of local patches

* fix cargo doc

* chore: typo fixes

* chore: clippy fix

* Release v0.29.0

* chore: more clippy fixes
This commit is contained in:
Sébastien Crozet
2025-09-05 19:31:58 +02:00
committed by GitHub
parent 317322b31b
commit 134f433903
94 changed files with 5066 additions and 8136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/dynamics/ccd/ccd_solver.rs
View File

@@ -36,7 +36,7 @@ impl CCDSolver {
let min_toi = (rb.ccd.ccd_thickness
* 0.15
* crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels)))
* crate::utils::inv(rb.ccd.max_point_velocity(&rb.ccd_vels)))
.min(dt);
// println!(
// "Min toi: {}, Toi: {}, thick: {}, max_vel: {}",
@@ -45,9 +45,9 @@ impl CCDSolver {
// rb.ccd.ccd_thickness,
// rb.ccd.max_point_velocity(&rb.integrated_vels)
// );
let new_pos =
rb.integrated_vels
.integrate(toi.max(min_toi), &rb.pos.position, local_com);
let new_pos = rb
.ccd_vels
.integrate(toi.max(min_toi), &rb.pos.position, local_com);
rb.pos.next_position = new_pos;
}
}
@@ -66,7 +66,7 @@ impl CCDSolver {
let mut ccd_active = false;
// println!("Checking CCD activation");
for handle in islands.active_dynamic_bodies() {
for handle in islands.active_bodies() {
let rb = bodies.index_mut_internal(*handle);
if rb.ccd.ccd_enabled {
@@ -75,7 +75,7 @@ impl CCDSolver {
} else {
None
};
let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces);
let moving_fast = rb.ccd.is_moving_fast(dt, &rb.ccd_vels, forces);
rb.ccd.ccd_active = moving_fast;
ccd_active = ccd_active || moving_fast;
}
@@ -121,14 +121,14 @@ impl CCDSolver {
let mut pairs_seen = HashMap::default();
let mut min_toi = dt;
for handle in islands.active_dynamic_bodies() {
for handle in islands.active_bodies() {
let rb1 = &bodies[*handle];
if rb1.ccd.ccd_active {
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
dt,
&rb1.forces,
&rb1.integrated_vels,
&rb1.ccd_vels,
&rb1.mprops,
);
@@ -254,14 +254,14 @@ impl CCDSolver {
*
*/
// TODO: don't iterate through all the colliders.
for handle in islands.active_dynamic_bodies() {
for handle in islands.active_bodies() {
let rb1 = &bodies[*handle];
if rb1.ccd.ccd_active {
let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
dt,
&rb1.forces,
&rb1.integrated_vels,
&rb1.ccd_vels,
&rb1.mprops,
);
@@ -487,10 +487,7 @@ impl CCDSolver {
let local_com1 = &rb1.mprops.local_mprops.local_com;
let frozen1 = frozen.get(&b1);
let pos1 = frozen1
.map(|t| {
rb1.integrated_vels
.integrate(*t, &rb1.pos.position, local_com1)
})
.map(|t| rb1.ccd_vels.integrate(*t, &rb1.pos.position, local_com1))
.unwrap_or(rb1.pos.next_position);
pos1 * co_parent1.pos_wrt_parent
} else {
@@ -503,10 +500,7 @@ impl CCDSolver {
let local_com2 = &rb2.mprops.local_mprops.local_com;
let frozen2 = frozen.get(&b2);
let pos2 = frozen2
.map(|t| {
rb2.integrated_vels
.integrate(*t, &rb2.pos.position, local_com2)
})
.map(|t| rb2.ccd_vels.integrate(*t, &rb2.pos.position, local_com2))
.unwrap_or(rb2.pos.next_position);
pos2 * co_parent2.pos_wrt_parent
} else {

20
src/dynamics/ccd/toi_entry.rs
View File

@@ -54,14 +54,14 @@ impl TOIEntry {
return None;
}
let linvel1 = frozen1.is_none() as u32 as Real
* rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
let linvel2 = frozen2.is_none() as u32 as Real
* rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
let angvel1 = frozen1.is_none() as u32 as Real
* rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
let angvel2 = frozen2.is_none() as u32 as Real
* rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
let linvel1 =
frozen1.is_none() as u32 as Real * rb1.map(|b| b.ccd_vels.linvel).unwrap_or(na::zero());
let linvel2 =
frozen2.is_none() as u32 as Real * rb2.map(|b| b.ccd_vels.linvel).unwrap_or(na::zero());
let angvel1 =
frozen1.is_none() as u32 as Real * rb1.map(|b| b.ccd_vels.angvel).unwrap_or(na::zero());
let angvel2 =
frozen2.is_none() as u32 as Real * rb2.map(|b| b.ccd_vels.angvel).unwrap_or(na::zero());
#[cfg(feature = "dim2")]
let vel12 = (linvel2 - linvel1).norm()
@@ -160,8 +160,8 @@ impl TOIEntry {
NonlinearRigidMotion::new(
rb.pos.position,
rb.mprops.local_mprops.local_com,
rb.integrated_vels.linvel,
rb.integrated_vels.angvel,
rb.ccd_vels.linvel,
rb.ccd_vels.angvel,
)
} else {
NonlinearRigidMotion::constant_position(rb.pos.next_position)

82
src/dynamics/integration_parameters.rs
View File

@@ -1,6 +1,5 @@
use crate::math::Real;
use na::RealField;
use std::num::NonZeroUsize;
#[cfg(doc)]
use super::RigidBodyActivation;
@@ -9,6 +8,28 @@ use super::RigidBodyActivation;
// the 3D domino demo. So for now we dont enable it in 3D.
pub(crate) static BLOCK_SOLVER_ENABLED: bool = cfg!(feature = "dim2");
/// Friction models used for all contact constraints between two rigid-bodies.
///
/// This selection does not apply to multibodies that always rely on the [`FrictionModel::Coulomb`].
#[cfg(feature = "dim3")]
#[derive(Default, Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub enum FrictionModel {
/// A simplified friction model significantly faster to solve than [`Self::Coulomb`]
/// but less accurate.
///
/// Instead of solving one Coulomb friction constraint per contact in a contact manifold,
/// this approximation only solves one Coulomb friction constraint per group of 4 contacts
/// in a contact manifold, plus one "twist" constraint. The "twist" constraint is purely
/// rotational and aims to eliminate angular movement in the manifold’s tangent plane.
#[default]
Simplified,
/// The coulomb friction model.
///
/// This results in one Coulomb friction constraint per contact point.
Coulomb,
}
/// Parameters for a time-step of the physics engine.
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -92,24 +113,25 @@ pub struct IntegrationParameters {
/// This value is implicitly scaled by [`IntegrationParameters::length_unit`].
pub normalized_prediction_distance: Real,
/// The number of solver iterations run by the constraints solver for calculating forces (default: `4`).
pub num_solver_iterations: NonZeroUsize,
/// Number of addition friction resolution iteration run during the last solver sub-step (default: `0`).
pub num_additional_friction_iterations: usize,
pub num_solver_iterations: usize,
/// Number of internal Project Gauss Seidel (PGS) iterations run at each solver iteration (default: `1`).
pub num_internal_pgs_iterations: usize,
/// The number of stabilization iterations run at each solver iterations (default: `2`).
/// The number of stabilization iterations run at each solver iterations (default: `1`).
pub num_internal_stabilization_iterations: usize,
/// Minimum number of dynamic bodies in each active island (default: `128`).
/// Minimum number of dynamic bodies on each active island (default: `128`).
pub min_island_size: usize,
/// Maximum number of substeps performed by the solver (default: `1`).
pub max_ccd_substeps: usize,
/// The type of friction constraints used in the simulation.
#[cfg(feature = "dim3")]
pub friction_model: FrictionModel,
}
impl IntegrationParameters {
/// The inverse of the time-stepping length, i.e. the steps per seconds (Hz).
///
/// This is zero if `self.dt` is zero.
#[inline(always)]
#[inline]
pub fn inv_dt(&self) -> Real {
if self.dt == 0.0 { 0.0 } else { 1.0 / self.dt }
}
@@ -260,12 +282,10 @@ impl IntegrationParameters {
pub fn prediction_distance(&self) -> Real {
self.normalized_prediction_distance * self.length_unit
}
}
/// Initialize the simulation parameters with settings matching the TGS-soft solver
/// with warmstarting.
///
/// This is the default configuration, equivalent to [`IntegrationParameters::default()`].
pub fn tgs_soft() -> Self {
impl Default for IntegrationParameters {
fn default() -> Self {
Self {
dt: 1.0 / 60.0,
min_ccd_dt: 1.0 / 60.0 / 100.0,
@@ -275,9 +295,8 @@ impl IntegrationParameters {
joint_damping_ratio: 1.0,
warmstart_coefficient: 1.0,
num_internal_pgs_iterations: 1,
num_internal_stabilization_iterations: 2,
num_additional_friction_iterations: 0,
num_solver_iterations: NonZeroUsize::new(4).unwrap(),
num_internal_stabilization_iterations: 1,
num_solver_iterations: 4,
// TODO: what is the optimal value for min_island_size?
// It should not be too big so that we don't end up with
// huge islands that don't fit in cache.
@@ -289,37 +308,8 @@ impl IntegrationParameters {
normalized_prediction_distance: 0.002,
max_ccd_substeps: 1,
length_unit: 1.0,
}
}
/// Initialize the simulation parameters with settings matching the TGS-soft solver
/// **without** warmstarting.
///
/// The [`IntegrationParameters::tgs_soft()`] configuration should be preferred unless
/// warmstarting proves to be undesirable for your use-case.
pub fn tgs_soft_without_warmstart() -> Self {
Self {
contact_damping_ratio: 0.25,
warmstart_coefficient: 0.0,
num_additional_friction_iterations: 4,
..Self::tgs_soft()
}
}
/// Initializes the integration parameters to match the legacy PGS solver from Rapier version <= 0.17.
///
/// This exists mainly for testing and comparison purpose.
pub fn pgs_legacy() -> Self {
Self {
num_solver_iterations: NonZeroUsize::new(1).unwrap(),
num_internal_pgs_iterations: 4,
..Self::tgs_soft_without_warmstart()
#[cfg(feature = "dim3")]
friction_model: FrictionModel::default(),
}
}
}
impl Default for IntegrationParameters {
fn default() -> Self {
Self::tgs_soft()
}
}

119
src/dynamics/island_manager.rs
View File

@@ -11,8 +11,7 @@ use crate::utils::SimdDot;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Default)]
pub struct IslandManager {
pub(crate) active_dynamic_set: Vec<RigidBodyHandle>,
pub(crate) active_kinematic_set: Vec<RigidBodyHandle>,
pub(crate) active_set: Vec<RigidBodyHandle>,
pub(crate) active_islands: Vec<usize>,
pub(crate) active_islands_additional_solver_iterations: Vec<usize>,
active_set_timestamp: u32,
@@ -26,8 +25,7 @@ impl IslandManager {
/// Creates a new empty island manager.
pub fn new() -> Self {
Self {
active_dynamic_set: vec![],
active_kinematic_set: vec![],
active_set: vec![],
active_islands: vec![],
active_islands_additional_solver_iterations: vec![],
active_set_timestamp: 0,
@@ -42,26 +40,22 @@ impl IslandManager {
/// Update this data-structure after one or multiple rigid-bodies have been removed for `bodies`.
pub fn cleanup_removed_rigid_bodies(&mut self, bodies: &mut RigidBodySet) {
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
let mut i = 0;
for active_set in &mut active_sets {
let mut i = 0;
while i < self.active_set.len() {
let handle = self.active_set[i];
if bodies.get(handle).is_none() {
// This rigid-body no longer exists, so we need to remove it from the active set.
self.active_set.swap_remove(i);
while i < active_set.len() {
let handle = active_set[i];
if bodies.get(handle).is_none() {
// This rigid-body no longer exists, so we need to remove it from the active set.
active_set.swap_remove(i);
if i < active_set.len() {
// Update the active_set_id for the body that has been swapped.
if let Some(swapped_rb) = bodies.get_mut_internal(active_set[i]) {
swapped_rb.ids.active_set_id = i;
}
if i < self.active_set.len() {
// Update the self.active_set_id for the body that has been swapped.
if let Some(swapped_rb) = bodies.get_mut_internal(self.active_set[i]) {
swapped_rb.ids.active_set_id = i;
}
} else {
i += 1;
}
} else {
i += 1;
}
}
}
@@ -72,18 +66,15 @@ impl IslandManager {
removed_ids: &RigidBodyIds,
bodies: &mut RigidBodySet,
) {
let mut active_sets = [&mut self.active_kinematic_set, &mut self.active_dynamic_set];
if self.active_set.get(removed_ids.active_set_id) == Some(&removed_handle) {
self.active_set.swap_remove(removed_ids.active_set_id);
for active_set in &mut active_sets {
if active_set.get(removed_ids.active_set_id) == Some(&removed_handle) {
active_set.swap_remove(removed_ids.active_set_id);
if let Some(replacement) = active_set
.get(removed_ids.active_set_id)
.and_then(|h| bodies.get_mut_internal(*h))
{
replacement.ids.active_set_id = removed_ids.active_set_id;
}
if let Some(replacement) = self
.active_set
.get(removed_ids.active_set_id)
.and_then(|h| bodies.get_mut_internal(*h))
{
replacement.ids.active_set_id = removed_ids.active_set_id;
}
}
}
@@ -104,41 +95,27 @@ impl IslandManager {
if !rb.changes.contains(RigidBodyChanges::SLEEP) {
rb.activation.wake_up(strong);
if rb.is_enabled()
&& self.active_dynamic_set.get(rb.ids.active_set_id) != Some(&handle)
{
rb.ids.active_set_id = self.active_dynamic_set.len();
self.active_dynamic_set.push(handle);
if rb.is_enabled() && self.active_set.get(rb.ids.active_set_id) != Some(&handle) {
rb.ids.active_set_id = self.active_set.len();
self.active_set.push(handle);
}
}
}
}
/// Iter through all the active kinematic rigid-bodies on this set.
pub fn active_kinematic_bodies(&self) -> &[RigidBodyHandle] {
&self.active_kinematic_set[..]
}
/// Iter through all the active dynamic rigid-bodies on this set.
pub fn active_dynamic_bodies(&self) -> &[RigidBodyHandle] {
&self.active_dynamic_set[..]
}
pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] {
let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1];
&self.active_dynamic_set[island_range]
&self.active_set[island_range]
}
pub(crate) fn active_island_additional_solver_iterations(&self, island_id: usize) -> usize {
self.active_islands_additional_solver_iterations[island_id]
}
#[inline(always)]
pub(crate) fn iter_active_bodies(&self) -> impl Iterator<Item = RigidBodyHandle> + '_ {
self.active_dynamic_set
.iter()
.copied()
.chain(self.active_kinematic_set.iter().copied())
/// Handls of dynamic and kinematic rigid-bodies that are currently active (i.e. not sleeping).
#[inline]
pub fn active_bodies(&self) -> &[RigidBodyHandle] {
&self.active_set
}
#[cfg(feature = "parallel")]
@@ -171,10 +148,10 @@ impl IslandManager {
self.can_sleep.clear();
// NOTE: the `.rev()` is here so that two successive timesteps preserve
// the order of the bodies in the `active_dynamic_set` vec. This reversal
// the order of the bodies in the `active_set` vec. This reversal
// does not seem to affect performances nor stability. However it makes
// debugging slightly nicer.
for h in self.active_dynamic_set.drain(..).rev() {
for h in self.active_set.drain(..).rev() {
let can_sleep = &mut self.can_sleep;
let stack = &mut self.stack;
@@ -196,7 +173,7 @@ impl IslandManager {
}
// Read all the contacts and push objects touching touching this rigid-body.
#[inline(always)]
#[inline]
fn push_contacting_bodies(
rb_colliders: &RigidBodyColliders,
colliders: &ColliderSet,
@@ -221,20 +198,6 @@ impl IslandManager {
}
}
// Now iterate on all active kinematic bodies and push all the bodies
// touching them to the stack so they can be woken up.
for h in self.active_kinematic_set.iter() {
let rb = &bodies[*h];
if rb.vels.is_zero() {
// If the kinematic body does not move, it does not have
// to wake up any dynamic body.
continue;
}
push_contacting_bodies(&rb.colliders, colliders, narrow_phase, &mut self.stack);
}
// println!("Selection: {}", Instant::now() - t);
// let t = Instant::now();
@@ -258,7 +221,9 @@ impl IslandManager {
while let Some(handle) = self.stack.pop() {
let rb = bodies.index_mut_internal(handle);
if rb.ids.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() {
if rb.ids.active_set_timestamp == self.active_set_timestamp
|| !rb.is_dynamic_or_kinematic()
{
// We already visited this body and its neighbors.
// Also, we don't propagate awake state through fixed bodies.
continue;
@@ -266,13 +231,13 @@ impl IslandManager {
if self.stack.len() < island_marker {
if additional_solver_iterations != rb.additional_solver_iterations
|| self.active_dynamic_set.len() - *self.active_islands.last().unwrap()
|| self.active_set.len() - *self.active_islands.last().unwrap()
>= min_island_size
{
// We are starting a new island.
self.active_islands_additional_solver_iterations
.push(additional_solver_iterations);
self.active_islands.push(self.active_dynamic_set.len());
self.active_islands.push(self.active_set.len());
additional_solver_iterations = 0;
}
@@ -297,17 +262,17 @@ impl IslandManager {
rb.activation.wake_up(false);
rb.ids.active_island_id = self.active_islands.len() - 1;
rb.ids.active_set_id = self.active_dynamic_set.len();
rb.ids.active_set_id = self.active_set.len();
rb.ids.active_set_offset =
rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id];
(rb.ids.active_set_id - self.active_islands[rb.ids.active_island_id]) as u32;
rb.ids.active_set_timestamp = self.active_set_timestamp;
self.active_dynamic_set.push(handle);
self.active_set.push(handle);
}
self.active_islands_additional_solver_iterations
.push(additional_solver_iterations);
self.active_islands.push(self.active_dynamic_set.len());
self.active_islands.push(self.active_set.len());
// println!(
// "Extraction: {}, num islands: {}",
// Instant::now() - t,

24
src/dynamics/joint/generic_joint.rs
View File

@@ -1,7 +1,9 @@
#![allow(clippy::bad_bit_mask)] // Clippy will complain about the bitmasks due to JointAxesMask::FREE_FIXED_AXES being 0.
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RopeJoint};
use crate::dynamics::{
FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint, RigidBody, RopeJoint,
};
use crate::math::{Isometry, Point, Real, Rotation, SPATIAL_DIM, UnitVector, Vector};
use crate::utils::{SimdBasis, SimdRealCopy};
@@ -230,13 +232,13 @@ pub struct GenericJoint {
/// coupled linear DoF is applied to all coupled linear DoF. Similarly, if multiple angular DoF are limited/motorized
/// only the limits/motor configuration for the first coupled angular DoF is applied to all coupled angular DoF.
pub coupled_axes: JointAxesMask,
/// The limits, along each degrees of freedoms of this joint.
/// The limits, along each degree of freedoms of this joint.
///
/// Note that the limit must also be explicitly enabled by the `limit_axes` bitmask.
/// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF limit and `limit_axis`
/// bitmask is applied to the coupled linear (resp. angular) axes.
pub limits: [JointLimits<Real>; SPATIAL_DIM],
/// The motors, along each degrees of freedoms of this joint.
/// The motors, along each degree of freedoms of this joint.
///
/// Note that the motor must also be explicitly enabled by the `motor_axes` bitmask.
/// For coupled degrees of freedoms (DoF), only the first linear (resp. angular) coupled DoF motor and `motor_axes`
@@ -244,7 +246,7 @@ pub struct GenericJoint {
pub motors: [JointMotor; SPATIAL_DIM],
/// Are contacts between the attached rigid-bodies enabled?
pub contacts_enabled: bool,
/// Whether or not the joint is enabled.
/// Whether the joint is enabled.
pub enabled: JointEnabled,
/// User-defined data associated to this joint.
pub user_data: u128,
@@ -516,6 +518,20 @@ impl GenericJoint {
self.motors[dim].target_pos = -self.motors[dim].target_pos;
}
}
pub(crate) fn transform_to_solver_body_space(&mut self, rb1: &RigidBody, rb2: &RigidBody) {
if rb1.is_fixed() {
self.local_frame1 = rb1.pos.position * self.local_frame1;
} else {
self.local_frame1.translation.vector -= rb1.mprops.local_mprops.local_com.coords;
}
if rb2.is_fixed() {
self.local_frame2 = rb2.pos.position * self.local_frame2;
} else {
self.local_frame2.translation.vector -= rb2.mprops.local_mprops.local_com.coords;
}
}
}
macro_rules! joint_conversion_methods(

8
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -311,11 +311,11 @@ impl ImpulseJointSet {
let rb2 = &bodies[joint.body2];
if joint.data.is_enabled()
&& (rb1.is_dynamic() || rb2.is_dynamic())
&& (!rb1.is_dynamic() || !rb1.is_sleeping())
&& (!rb2.is_dynamic() || !rb2.is_sleeping())
&& (rb1.is_dynamic_or_kinematic() || rb2.is_dynamic_or_kinematic())
&& (!rb1.is_dynamic_or_kinematic() || !rb1.is_sleeping())
&& (!rb2.is_dynamic_or_kinematic() || !rb2.is_sleeping())
{
let island_index = if !rb1.is_dynamic() {
let island_index = if !rb1.is_dynamic_or_kinematic() {
rb2.ids.active_island_id
} else {
rb1.ids.active_island_id

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

@@ -86,7 +86,7 @@ pub struct Multibody {
ndofs: usize,
pub(crate) root_is_dynamic: bool,
pub(crate) solver_id: usize,
pub(crate) solver_id: u32,
self_contacts_enabled: bool,
/*
@@ -822,7 +822,7 @@ impl Multibody {
/// The generalized velocity at the multibody_joint of the given link.
#[inline]
pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<Real> {
pub fn joint_velocity(&self, link: &MultibodyLink) -> DVectorView<'_, Real> {
let ndofs = link.joint().ndofs();
DVectorView::from_slice(
&self.velocities.as_slice()[link.assembly_id..link.assembly_id + ndofs],
@@ -832,13 +832,13 @@ impl Multibody {
/// The generalized accelerations of this multibodies.
#[inline]
pub fn generalized_acceleration(&self) -> DVectorView<Real> {
pub fn generalized_acceleration(&self) -> DVectorView<'_, Real> {
self.accelerations.rows(0, self.ndofs)
}
/// The generalized velocities of this multibodies.
#[inline]
pub fn generalized_velocity(&self) -> DVectorView<Real> {
pub fn generalized_velocity(&self) -> DVectorView<'_, Real> {
self.velocities.rows(0, self.ndofs)
}
@@ -850,7 +850,7 @@ impl Multibody {
/// The mutable generalized velocities of this multibodies.
#[inline]
pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<Real> {
pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<'_, Real> {
self.velocities.rows_mut(0, self.ndofs)
}
@@ -971,7 +971,8 @@ impl Multibody {
}
if update_mass_properties {
rb.mprops.update_world_mass_properties(&link.local_to_world);
rb.mprops
.update_world_mass_properties(rb.body_type, &link.local_to_world);
}
}
}

6
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -1,4 +1,4 @@
use crate::dynamics::solver::JointGenericOneBodyConstraint;
use crate::dynamics::solver::GenericJointConstraint;
use crate::dynamics::{
FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
RigidBodyVelocity, joint,
@@ -185,7 +185,7 @@ impl MultibodyJoint {
/// Multiply the multibody_joint jacobian by generalized velocities to obtain the
/// relative velocity of the multibody link containing this multibody_joint.
pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity {
pub fn jacobian_mul_coordinates(&self, acc: &[Real]) -> RigidBodyVelocity<Real> {
let locked_bits = self.data.locked_axes.bits();
let mut result = RigidBodyVelocity::zero();
let mut curr_free_dof = 0;
@@ -269,7 +269,7 @@ impl MultibodyJoint {
link: &MultibodyLink,
mut j_id: usize,
jacobians: &mut DVector<Real>,
constraints: &mut [JointGenericOneBodyConstraint],
constraints: &mut [GenericJointConstraint],
) -> usize {
let j_id = &mut j_id;
let locked_bits = self.data.locked_axes.bits();

2
src/dynamics/joint/multibody_joint/multibody_link.rs
View File

@@ -27,7 +27,7 @@ pub struct MultibodyLink {
pub(crate) shift23: Vector<Real>,
/// The velocity added by the joint, in world-space.
pub(crate) joint_velocity: RigidBodyVelocity,
pub(crate) joint_velocity: RigidBodyVelocity<Real>,
}
impl MultibodyLink {

2
src/dynamics/joint/multibody_joint/multibody_workspace.rs
View File

@@ -6,7 +6,7 @@ use na::DVector;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug)]
pub(crate) struct MultibodyWorkspace {
pub accs: Vec<RigidBodyVelocity>,
pub accs: Vec<RigidBodyVelocity<Real>>,
pub ndofs_vec: DVector<Real>,
}

26
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
View File

@@ -1,7 +1,7 @@
#![allow(missing_docs)] // For downcast.
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::solver::{JointGenericOneBodyConstraint, WritebackId};
use crate::dynamics::solver::{GenericJointConstraint, WritebackId};
use crate::dynamics::{IntegrationParameters, JointMotor, Multibody};
use crate::math::Real;
use na::DVector;
@@ -17,7 +17,7 @@ pub fn unit_joint_limit_constraint(
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut [JointGenericOneBodyConstraint],
constraints: &mut [GenericJointConstraint],
insert_at: &mut usize,
) {
let ndofs = multibody.ndofs();
@@ -42,11 +42,17 @@ pub fn unit_joint_limit_constraint(
max_enabled as u32 as Real * Real::MAX,
];
let constraint = JointGenericOneBodyConstraint {
let constraint = GenericJointConstraint {
is_rigid_body1: false,
solver_vel1: u32::MAX,
ndofs1: 0,
j_id1: 0,
is_rigid_body2: false,
solver_vel2: multibody.solver_id,
ndofs2: ndofs,
j_id2: *j_id,
joint_id: usize::MAX,
joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet.
impulse: 0.0,
impulse_bounds,
inv_lhs: crate::utils::inv(lhs),
@@ -75,7 +81,7 @@ pub fn unit_joint_motor_constraint(
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut [JointGenericOneBodyConstraint],
constraints: &mut [GenericJointConstraint],
insert_at: &mut usize,
) {
let inv_dt = params.inv_dt();
@@ -108,11 +114,17 @@ pub fn unit_joint_motor_constraint(
rhs_wo_bias += -target_vel;
let constraint = JointGenericOneBodyConstraint {
let constraint = GenericJointConstraint {
is_rigid_body1: false,
solver_vel1: u32::MAX,
ndofs1: 0,
j_id1: 0,
is_rigid_body2: false,
solver_vel2: multibody.solver_id,
ndofs2: ndofs,
j_id2: *j_id,
joint_id: usize::MAX,
joint_id: usize::MAX, // TODO: we don’t support impulse writeback for internal constraints yet.
impulse: 0.0,
impulse_bounds,
cfm_coeff: motor_params.cfm_coeff,

4
src/dynamics/mod.rs
View File

@@ -4,6 +4,10 @@ pub use self::ccd::CCDSolver;
pub use self::coefficient_combine_rule::CoefficientCombineRule;
pub use self::integration_parameters::IntegrationParameters;
pub use self::island_manager::IslandManager;
#[cfg(feature = "dim3")]
pub use self::integration_parameters::FrictionModel;
pub(crate) use self::joint::JointGraphEdge;
pub(crate) use self::joint::JointIndex;
pub use self::joint::*;

161
src/dynamics/rigid_body.rs
View File

@@ -17,18 +17,18 @@ use num::Zero;
///
/// To create a new rigid-body, use the [`RigidBodyBuilder`] structure.
#[derive(Debug, Clone)]
// #[repr(C)]
// #[repr(align(64))]
pub struct RigidBody {
pub(crate) pos: RigidBodyPosition,
pub(crate) mprops: RigidBodyMassProps,
// NOTE: we need this so that the CCD can use the actual velocities obtained
// by the velocity solver with bias. If we switch to interpolation, we
// should remove this field.
pub(crate) integrated_vels: RigidBodyVelocity,
pub(crate) vels: RigidBodyVelocity,
pub(crate) damping: RigidBodyDamping,
pub(crate) forces: RigidBodyForces,
pub(crate) ccd: RigidBodyCcd,
pub(crate) ids: RigidBodyIds,
pub(crate) pos: RigidBodyPosition,
pub(crate) damping: RigidBodyDamping<Real>,
pub(crate) vels: RigidBodyVelocity<Real>,
pub(crate) forces: RigidBodyForces,
pub(crate) mprops: RigidBodyMassProps,
pub(crate) ccd_vels: RigidBodyVelocity<Real>,
pub(crate) ccd: RigidBodyCcd,
pub(crate) colliders: RigidBodyColliders,
/// Whether or not this rigid-body is sleeping.
pub(crate) activation: RigidBodyActivation,
@@ -54,7 +54,7 @@ impl RigidBody {
Self {
pos: RigidBodyPosition::default(),
mprops: RigidBodyMassProps::default(),
integrated_vels: RigidBodyVelocity::default(),
ccd_vels: RigidBodyVelocity::default(),
vels: RigidBodyVelocity::default(),
damping: RigidBodyDamping::default(),
forces: RigidBodyForces::default(),
@@ -98,7 +98,7 @@ impl RigidBody {
let RigidBody {
pos,
mprops,
integrated_vels,
ccd_vels: integrated_vels,
vels,
damping,
forces,
@@ -116,7 +116,7 @@ impl RigidBody {
self.pos = *pos;
self.mprops = mprops.clone();
self.integrated_vels = *integrated_vels;
self.ccd_vels = *integrated_vels;
self.vels = *vels;
self.damping = *damping;
self.forces = *forces;
@@ -224,7 +224,7 @@ impl RigidBody {
self.vels = RigidBodyVelocity::zero();
}
if self.is_dynamic() && wake_up {
if self.is_dynamic_or_kinematic() && wake_up {
self.wake_up(true);
}
}
@@ -261,7 +261,7 @@ impl RigidBody {
#[inline]
pub fn set_locked_axes(&mut self, locked_axes: LockedAxes, wake_up: bool) {
if locked_axes != self.mprops.flags {
if self.is_dynamic() && wake_up {
if self.is_dynamic_or_kinematic() && wake_up {
self.wake_up(true);
}
@@ -280,7 +280,7 @@ impl RigidBody {
/// Locks or unlocks all the rotations of this rigid-body.
pub fn lock_rotations(&mut self, locked: bool, wake_up: bool) {
if locked != self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED) {
if self.is_dynamic() && wake_up {
if self.is_dynamic_or_kinematic() && wake_up {
self.wake_up(true);
}
@@ -304,7 +304,7 @@ impl RigidBody {
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) == allow_rotations_y
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) == allow_rotations_z
{
if self.is_dynamic() && wake_up {
if self.is_dynamic_or_kinematic() && wake_up {
self.wake_up(true);
}
@@ -342,7 +342,7 @@ impl RigidBody {
/// Locks or unlocks all the rotations of this rigid-body.
pub fn lock_translations(&mut self, locked: bool, wake_up: bool) {
if locked != self.mprops.flags.contains(LockedAxes::TRANSLATION_LOCKED) {
if self.is_dynamic() && wake_up {
if self.is_dynamic_or_kinematic() && wake_up {
self.wake_up(true);
}
@@ -378,7 +378,7 @@ impl RigidBody {
return;
}
if self.is_dynamic() && wake_up {
if self.is_dynamic_or_kinematic() && wake_up {
self.wake_up(true);
}
@@ -498,6 +498,7 @@ impl RigidBody {
self.mprops.recompute_mass_properties_from_colliders(
colliders,
&self.colliders,
self.body_type,
&self.pos.position,
);
}
@@ -565,7 +566,7 @@ impl RigidBody {
self.changes.insert(RigidBodyChanges::LOCAL_MASS_PROPERTIES);
self.mprops.additional_local_mprops = new_mprops;
if self.is_dynamic() && wake_up {
if self.is_dynamic_or_kinematic() && wake_up {
self.wake_up(true);
}
}
@@ -590,6 +591,26 @@ impl RigidBody {
self.body_type.is_kinematic()
}
/// Is this rigid-body a dynamic rigid-body or a kinematic rigid-body?
///
/// This method is mostly convenient internally where kinematic and dynamic rigid-body
/// are subject to the same behavior.
pub fn is_dynamic_or_kinematic(&self) -> bool {
self.body_type.is_dynamic_or_kinematic()
}
/// The offset index in the solver’s active set, or `u32::MAX` if
/// the rigid-body isn’t dynamic or kinematic.
// TODO: is this really necessary? Could we just always assign u32::MAX
// to all the fixed bodies active set offsets?
pub fn effective_active_set_offset(&self) -> u32 {
if self.is_dynamic_or_kinematic() {
self.ids.active_set_offset
} else {
u32::MAX
}
}
/// Is this rigid body fixed?
///
/// A fixed body cannot move and is not affected by forces.
@@ -653,6 +674,7 @@ impl RigidBody {
co_mprops: &ColliderMassProps,
) {
self.colliders.attach_collider(
self.body_type,
&mut self.changes,
&mut self.ccd,
&mut self.mprops,
@@ -710,7 +732,7 @@ impl RigidBody {
}
/// The linear and angular velocity of this rigid-body.
pub fn vels(&self) -> &RigidBodyVelocity {
pub fn vels(&self) -> &RigidBodyVelocity<Real> {
&self.vels
}
@@ -735,7 +757,7 @@ impl RigidBody {
///
/// If `wake_up` is `true` then the rigid-body will be woken up if it was
/// put to sleep because it did not move for a while.
pub fn set_vels(&mut self, vels: RigidBodyVelocity, wake_up: bool) {
pub fn set_vels(&mut self, vels: RigidBodyVelocity<Real>, wake_up: bool) {
self.set_linvel(vels.linvel, wake_up);
self.set_angvel(vels.angvel, wake_up);
}
@@ -831,7 +853,7 @@ impl RigidBody {
self.update_world_mass_properties();
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
if wake_up && self.is_dynamic_or_kinematic() {
self.wake_up(true)
}
}
@@ -855,7 +877,7 @@ impl RigidBody {
self.update_world_mass_properties();
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
if wake_up && self.is_dynamic_or_kinematic() {
self.wake_up(true)
}
}
@@ -880,7 +902,7 @@ impl RigidBody {
self.update_world_mass_properties();
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
if wake_up && self.is_dynamic_or_kinematic() {
self.wake_up(true)
}
}
@@ -890,13 +912,21 @@ impl RigidBody {
pub fn set_next_kinematic_rotation(&mut self, rotation: Rotation<Real>) {
if self.is_kinematic() {
self.pos.next_position.rotation = rotation;
if self.pos.position.rotation != rotation {
self.wake_up(true);
}
}
}
/// If this rigid body is kinematic, sets its future translation after the next timestep integration.
pub fn set_next_kinematic_translation(&mut self, translation: Vector<Real>) {
if self.is_kinematic() {
self.pos.next_position.translation = translation.into();
self.pos.next_position.translation.vector = translation;
if self.pos.position.translation.vector != translation {
self.wake_up(true);
}
}
}
@@ -905,6 +935,10 @@ impl RigidBody {
pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
if self.is_kinematic() {
self.pos.next_position = pos;
if self.pos.position != pos {
self.wake_up(true);
}
}
}
@@ -946,7 +980,8 @@ impl RigidBody {
}
pub(crate) fn update_world_mass_properties(&mut self) {
self.mprops.update_world_mass_properties(&self.pos.position);
self.mprops
.update_world_mass_properties(self.body_type, &self.pos.position);
}
}
@@ -1053,8 +1088,7 @@ impl RigidBody {
#[profiling::function]
pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) {
if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic {
self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt
* (self.mprops.effective_world_inv_inertia_sqrt * torque_impulse);
self.vels.angvel += self.mprops.effective_world_inv_inertia * torque_impulse;
if wake_up {
self.wake_up(true);
@@ -1069,8 +1103,7 @@ impl RigidBody {
#[profiling::function]
pub fn apply_torque_impulse(&mut self, torque_impulse: Vector<Real>, wake_up: bool) {
if !torque_impulse.is_zero() && self.body_type == RigidBodyType::Dynamic {
self.vels.angvel += self.mprops.effective_world_inv_inertia_sqrt
* (self.mprops.effective_world_inv_inertia_sqrt * torque_impulse);
self.vels.angvel += self.mprops.effective_world_inv_inertia * torque_impulse;
if wake_up {
self.wake_up(true);
@@ -1113,6 +1146,23 @@ impl RigidBody {
AngVector::zero()
}
}
/// Are gyroscopic forces enabled for rigid-bodies?
#[cfg(feature = "dim3")]
pub fn gyroscopic_forces_enabled(&self) -> bool {
self.forces.gyroscopic_forces_enabled
}
/// Enables or disables gyroscopic forces for this rigid-body.
///
/// Enabling gyroscopic forces allows more realistic behaviors like gyroscopic precession,
/// but result in a slight performance overhead.
///
/// Disabled by default.
#[cfg(feature = "dim3")]
pub fn enable_gyroscopic_forces(&mut self, enabled: bool) {
self.forces.gyroscopic_forces_enabled = enabled;
}
}
impl RigidBody {
@@ -1140,6 +1190,29 @@ impl RigidBody {
-self.mass() * self.forces.gravity_scale * gravity.dot(&world_com)
}
/// Computes the angular velocity of this rigid-body after application of gyroscopic forces.
#[cfg(feature = "dim3")]
pub fn angvel_with_gyroscopic_forces(&self, dt: Real) -> AngVector<Real> {
// NOTE: integrating the gyroscopic forces implicitly are both slower and
// very dissipative. Instead, we only keep the explicit term and
// ensure angular momentum is preserved (similar to Jolt).
let w = self.pos.position.inverse_transform_vector(self.angvel());
let i = self.mprops.local_mprops.principal_inertia();
let ii = self.mprops.local_mprops.inv_principal_inertia;
let curr_momentum = i.component_mul(&w);
let explicit_gyro_momentum = -w.cross(&curr_momentum) * dt;
let total_momentum = curr_momentum + explicit_gyro_momentum;
let total_momentum_sqnorm = total_momentum.norm_squared();
if total_momentum_sqnorm != 0.0 {
let capped_momentum =
total_momentum * (curr_momentum.norm_squared() / total_momentum_sqnorm).sqrt();
self.pos.position * (ii.component_mul(&capped_momentum))
} else {
*self.angvel()
}
}
}
/// A builder for rigid-bodies.
@@ -1193,6 +1266,8 @@ pub struct RigidBodyBuilder {
///
/// See [`RigidBody::set_additional_solver_iterations`] for additional information.
pub additional_solver_iterations: usize,
/// Are gyroscopic forces enabled for this rigid-body?
pub gyroscopic_forces_enabled: bool,
}
impl Default for RigidBodyBuilder {
@@ -1222,6 +1297,7 @@ impl RigidBodyBuilder {
enabled: true,
user_data: 0,
additional_solver_iterations: 0,
gyroscopic_forces_enabled: false,
}
}
@@ -1295,11 +1371,18 @@ impl RigidBodyBuilder {
}
/// Sets the initial position (translation and orientation) of the rigid-body to be created.
#[deprecated = "renamed to `RigidBodyBuilder::pose`"]
pub fn position(mut self, pos: Isometry<Real>) -> Self {
self.position = pos;
self
}
/// Sets the initial pose (translation and orientation) of the rigid-body to be created.
pub fn pose(mut self, pos: Isometry<Real>) -> Self {
self.position = pos;
self
}
/// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
pub fn user_data(mut self, data: u128) -> Self {
self.user_data = data;
@@ -1491,6 +1574,18 @@ impl RigidBodyBuilder {
self
}
/// Are gyroscopic forces enabled for this rigid-body?
///
/// Enabling gyroscopic forces allows more realistic behaviors like gyroscopic precession,
/// but result in a slight performance overhead.
///
/// Disabled by default.
#[cfg(feature = "dim3")]
pub fn gyroscopic_forces_enabled(mut self, enabled: bool) -> Self {
self.gyroscopic_forces_enabled = enabled;
self
}
/// Enable or disable the rigid-body after its creation.
pub fn enabled(mut self, enabled: bool) -> Self {
self.enabled = enabled;
@@ -1519,6 +1614,10 @@ impl RigidBodyBuilder {
rb.damping.linear_damping = self.linear_damping;
rb.damping.angular_damping = self.angular_damping;
rb.forces.gravity_scale = self.gravity_scale;
#[cfg(feature = "dim3")]
{
rb.forces.gyroscopic_forces_enabled = self.gyroscopic_forces_enabled;
}
rb.dominance = RigidBodyDominance(self.dominance_group);
rb.enabled = self.enabled;
rb.enable_ccd(self.ccd_enabled);

278
src/dynamics/rigid_body_components.rs
View File

@@ -1,6 +1,8 @@
#[cfg(doc)]
use super::IntegrationParameters;
use crate::control::PdErrors;
#[cfg(doc)]
use crate::control::PidController;
use crate::dynamics::MassProperties;
use crate::geometry::{
ColliderChanges, ColliderHandle, ColliderMassProps, ColliderParent, ColliderPosition,
@@ -9,12 +11,9 @@ use crate::geometry::{
use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, Translation, Vector,
};
use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
use crate::utils::{SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
use num::Zero;
#[cfg(doc)]
use crate::control::PidController;
/// The unique handle of a rigid body added to a `RigidBodySet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash, Default)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -87,6 +86,14 @@ impl RigidBodyType {
self == RigidBodyType::KinematicPositionBased
|| self == RigidBodyType::KinematicVelocityBased
}
/// Is this rigid-body a dynamic rigid-body or a kinematic rigid-body?
///
/// This method is mostly convenient internally where kinematic and dynamic rigid-body
/// are subject to the same behavior.
pub fn is_dynamic_or_kinematic(self) -> bool {
self != RigidBodyType::Fixed
}
}
bitflags::bitflags! {
@@ -150,7 +157,11 @@ impl RigidBodyPosition {
/// Computes the velocity need to travel from `self.position` to `self.next_position` in
/// a time equal to `1.0 / inv_dt`.
#[must_use]
pub fn interpolate_velocity(&self, inv_dt: Real, local_com: &Point<Real>) -> RigidBodyVelocity {
pub fn interpolate_velocity(
&self,
inv_dt: Real,
local_com: &Point<Real>,
) -> RigidBodyVelocity<Real> {
let pose_err = self.pose_errors(local_com);
RigidBodyVelocity {
linvel: pose_err.linear * inv_dt,
@@ -166,7 +177,7 @@ impl RigidBodyPosition {
&self,
dt: Real,
forces: &RigidBodyForces,
vels: &RigidBodyVelocity,
vels: &RigidBodyVelocity<Real>,
mprops: &RigidBodyMassProps,
) -> Isometry<Real> {
let new_vels = forces.integrate(dt, vels, mprops);
@@ -285,21 +296,22 @@ impl Default for RigidBodyAdditionalMassProps {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, PartialEq)]
// #[repr(C)]
/// The mass properties of a rigid-body.
pub struct RigidBodyMassProps {
/// Flags for locking rotation and translation.
pub flags: LockedAxes,
/// The local mass properties of the rigid-body.
pub local_mprops: MassProperties,
/// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders.
pub additional_local_mprops: Option<Box<RigidBodyAdditionalMassProps>>,
/// The world-space center of mass of the rigid-body.
pub world_com: Point<Real>,
/// The inverse mass taking into account translation locking.
pub effective_inv_mass: Vector<Real>,
/// The square-root of the world-space inverse angular inertia tensor of the rigid-body,
/// taking into account rotation locking.
pub effective_world_inv_inertia_sqrt: AngularInertia<Real>,
pub effective_world_inv_inertia: AngularInertia<Real>,
/// The local mass properties of the rigid-body.
pub local_mprops: MassProperties,
/// Flags for locking rotation and translation.
pub flags: LockedAxes,
/// Mass-properties of this rigid-bodies, added to the contributions of its attached colliders.
pub additional_local_mprops: Option<Box<RigidBodyAdditionalMassProps>>,
}
impl Default for RigidBodyMassProps {
@@ -310,7 +322,7 @@ impl Default for RigidBodyMassProps {
additional_local_mprops: None,
world_com: Point::origin(),
effective_inv_mass: Vector::zero(),
effective_world_inv_inertia_sqrt: AngularInertia::zero(),
effective_world_inv_inertia: AngularInertia::zero(),
}
}
}
@@ -350,9 +362,9 @@ impl RigidBodyMassProps {
/// The square root of the effective world-space angular inertia (that takes the potential rotation locking into account) of
/// this rigid-body.
#[must_use]
pub fn effective_angular_inertia_sqrt(&self) -> AngularInertia<Real> {
pub fn effective_angular_inertia(&self) -> AngularInertia<Real> {
#[allow(unused_mut)] // mut needed in 3D.
let mut ang_inertia = self.effective_world_inv_inertia_sqrt;
let mut ang_inertia = self.effective_world_inv_inertia;
// Make the matrix invertible.
#[cfg(feature = "dim3")]
@@ -388,18 +400,12 @@ impl RigidBodyMassProps {
result
}
/// The effective world-space angular inertia (that takes the potential rotation locking into account) of
/// this rigid-body.
#[must_use]
pub fn effective_angular_inertia(&self) -> AngularInertia<Real> {
self.effective_angular_inertia_sqrt().squared()
}
/// Recompute the mass-properties of this rigid-bodies based on its currently attached colliders.
pub fn recompute_mass_properties_from_colliders(
&mut self,
colliders: &ColliderSet,
attached_colliders: &RigidBodyColliders,
body_type: RigidBodyType,
position: &Isometry<Real>,
) {
let added_mprops = self
@@ -434,53 +440,56 @@ impl RigidBodyMassProps {
}
}
self.update_world_mass_properties(position);
self.update_world_mass_properties(body_type, position);
}
/// Update the world-space mass properties of `self`, taking into account the new position.
pub fn update_world_mass_properties(&mut self, position: &Isometry<Real>) {
pub fn update_world_mass_properties(
&mut self,
body_type: RigidBodyType,
position: &Isometry<Real>,
) {
self.world_com = self.local_mprops.world_com(position);
self.effective_inv_mass = Vector::repeat(self.local_mprops.inv_mass);
self.effective_world_inv_inertia_sqrt =
self.local_mprops.world_inv_inertia_sqrt(&position.rotation);
self.effective_world_inv_inertia = self.local_mprops.world_inv_inertia(&position.rotation);
// Take into account translation/rotation locking.
if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) {
if !body_type.is_dynamic() || self.flags.contains(LockedAxes::TRANSLATION_LOCKED_X) {
self.effective_inv_mass.x = 0.0;
}
if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) {
if !body_type.is_dynamic() || self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Y) {
self.effective_inv_mass.y = 0.0;
}
#[cfg(feature = "dim3")]
if self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) {
if !body_type.is_dynamic() || self.flags.contains(LockedAxes::TRANSLATION_LOCKED_Z) {
self.effective_inv_mass.z = 0.0;
}
#[cfg(feature = "dim2")]
{
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
self.effective_world_inv_inertia_sqrt = 0.0;
if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
self.effective_world_inv_inertia = 0.0;
}
}
#[cfg(feature = "dim3")]
{
if self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
self.effective_world_inv_inertia_sqrt.m11 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0;
if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_X) {
self.effective_world_inv_inertia.m11 = 0.0;
self.effective_world_inv_inertia.m12 = 0.0;
self.effective_world_inv_inertia.m13 = 0.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
self.effective_world_inv_inertia_sqrt.m22 = 0.0;
self.effective_world_inv_inertia_sqrt.m12 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0;
if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Y) {
self.effective_world_inv_inertia.m22 = 0.0;
self.effective_world_inv_inertia.m12 = 0.0;
self.effective_world_inv_inertia.m23 = 0.0;
}
if self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
self.effective_world_inv_inertia_sqrt.m33 = 0.0;
self.effective_world_inv_inertia_sqrt.m13 = 0.0;
self.effective_world_inv_inertia_sqrt.m23 = 0.0;
if !body_type.is_dynamic() || self.flags.contains(LockedAxes::ROTATION_LOCKED_Z) {
self.effective_world_inv_inertia.m33 = 0.0;
self.effective_world_inv_inertia.m13 = 0.0;
self.effective_world_inv_inertia.m23 = 0.0;
}
}
}
@@ -489,20 +498,20 @@ impl RigidBodyMassProps {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy, PartialEq)]
/// The velocities of this rigid-body.
pub struct RigidBodyVelocity {
pub struct RigidBodyVelocity<T: SimdRealCopy> {
/// The linear velocity of the rigid-body.
pub linvel: Vector<Real>,
pub linvel: Vector<T>,
/// The angular velocity of the rigid-body.
pub angvel: AngVector<Real>,
pub angvel: AngVector<T>,
}
impl Default for RigidBodyVelocity {
impl Default for RigidBodyVelocity<Real> {
fn default() -> Self {
Self::zero()
}
}
impl RigidBodyVelocity {
impl RigidBodyVelocity<Real> {
/// Create a new rigid-body velocity component.
#[must_use]
pub fn new(linvel: Vector<Real>, angvel: AngVector<Real>) -> Self {
@@ -618,15 +627,6 @@ impl RigidBodyVelocity {
0.5 * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel))
}
/// Returns the update velocities after applying the given damping.
#[must_use]
pub fn apply_damping(&self, dt: Real, damping: &RigidBodyDamping) -> Self {
RigidBodyVelocity {
linvel: self.linvel * (1.0 / (1.0 + dt * damping.linear_damping)),
angvel: self.angvel * (1.0 / (1.0 + dt * damping.angular_damping)),
}
}
/// The velocity of the given world-space point on this rigid-body.
#[must_use]
pub fn velocity_at_point(&self, point: &Point<Real>, world_com: &Point<Real>) -> Vector<Real> {
@@ -634,23 +634,6 @@ impl RigidBodyVelocity {
self.linvel + self.angvel.gcross(dpt)
}
/// Integrate the velocities in `self` to compute obtain new positions when moving from the given
/// initial position `init_pos`.
#[must_use]
pub fn integrate(
&self,
dt: Real,
init_pos: &Isometry<Real>,
local_com: &Point<Real>,
) -> Isometry<Real> {
let com = init_pos * local_com;
let shift = Translation::from(com.coords);
let mut result =
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() * init_pos;
result.rotation.renormalize_fast();
result
}
/// Are these velocities exactly equal to zero?
#[must_use]
pub fn is_zero(&self) -> bool {
@@ -664,17 +647,15 @@ impl RigidBodyVelocity {
let mut energy = (rb_mprops.mass() * self.linvel.norm_squared()) / 2.0;
#[cfg(feature = "dim2")]
if !rb_mprops.effective_world_inv_inertia_sqrt.is_zero() {
let inertia_sqrt = 1.0 / rb_mprops.effective_world_inv_inertia_sqrt;
energy += (inertia_sqrt * self.angvel).powi(2) / 2.0;
if !rb_mprops.effective_world_inv_inertia.is_zero() {
let inertia = 1.0 / rb_mprops.effective_world_inv_inertia;
energy += inertia * self.angvel * self.angvel / 2.0;
}
#[cfg(feature = "dim3")]
if !rb_mprops.effective_world_inv_inertia_sqrt.is_zero() {
let inertia_sqrt = rb_mprops
.effective_world_inv_inertia_sqrt
.inverse_unchecked();
energy += (inertia_sqrt * self.angvel).norm_squared() / 2.0;
if !rb_mprops.effective_world_inv_inertia.is_zero() {
let inertia = rb_mprops.effective_world_inv_inertia.inverse_unchecked();
energy += self.angvel.dot(&(inertia * self.angvel)) / 2.0;
}
energy
@@ -692,8 +673,7 @@ impl RigidBodyVelocity {
/// This does nothing on non-dynamic bodies.
#[cfg(feature = "dim2")]
pub fn apply_torque_impulse(&mut self, rb_mprops: &RigidBodyMassProps, torque_impulse: Real) {
self.angvel += rb_mprops.effective_world_inv_inertia_sqrt
* (rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse);
self.angvel += rb_mprops.effective_world_inv_inertia * torque_impulse;
}
/// Applies an angular impulse at the center-of-mass of this rigid-body.
@@ -705,8 +685,7 @@ impl RigidBodyVelocity {
rb_mprops: &RigidBodyMassProps,
torque_impulse: Vector<Real>,
) {
self.angvel += rb_mprops.effective_world_inv_inertia_sqrt
* (rb_mprops.effective_world_inv_inertia_sqrt * torque_impulse);
self.angvel += rb_mprops.effective_world_inv_inertia * torque_impulse;
}
/// Applies an impulse at the given world-space point of this rigid-body.
@@ -724,7 +703,74 @@ impl RigidBodyVelocity {
}
}
impl std::ops::Mul<Real> for RigidBodyVelocity {
impl<T: SimdRealCopy> RigidBodyVelocity<T> {
/// Returns the update velocities after applying the given damping.
#[must_use]
pub fn apply_damping(&self, dt: T, damping: &RigidBodyDamping<T>) -> Self {
let one = T::one();
RigidBodyVelocity {
linvel: self.linvel * (one / (one + dt * damping.linear_damping)),
angvel: self.angvel * (one / (one + dt * damping.angular_damping)),
}
}
/// Integrate the velocities in `self` to compute obtain new positions when moving from the given
/// initial position `init_pos`.
#[must_use]
#[inline]
pub fn integrate(&self, dt: T, init_pos: &Isometry<T>, local_com: &Point<T>) -> Isometry<T> {
let com = init_pos * local_com;
let shift = Translation::from(com.coords);
let mut result =
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() * init_pos;
result.rotation.renormalize_fast();
result
}
/// Same as [`Self::integrate`] but with a local center-of-mass assumed to be zero.
#[must_use]
#[inline]
pub fn integrate_centered(&self, dt: T, mut pose: Isometry<T>) -> Isometry<T> {
pose.translation.vector += self.linvel * dt;
pose.rotation = Rotation::new(self.angvel * dt) * pose.rotation;
pose.rotation.renormalize_fast();
pose
}
/// Same as [`Self::integrate`] but with the angular part linearized and the local
/// center-of-mass assumed to be zero.
#[inline]
#[cfg(feature = "dim2")]
pub fn integrate_linearized(&self, dt: T, pose: &mut Isometry<T>) {
let dang = self.angvel * dt;
let new_cos = pose.rotation.re - dang * pose.rotation.im;
let new_sin = pose.rotation.im + dang * pose.rotation.re;
pose.rotation = Rotation::from_cos_sin_unchecked(new_cos, new_sin);
// NOTE: don’t use renormalize_fast since the linearization might cause more drift.
// TODO: check for zeros?
pose.rotation.renormalize();
pose.translation.vector += self.linvel * dt;
}
/// Same as [`Self::integrate`] but with the angular part linearized and the local
/// center-of-mass assumed to be zero.
#[inline]
#[cfg(feature = "dim3")]
pub fn integrate_linearized(&self, dt: T, pose: &mut Isometry<T>) {
// Rotations linearization is inspired from
// https://ahrs.readthedocs.io/en/latest/filters/angular.html (not using the matrix form).
let hang = self.angvel * (dt * T::splat(0.5));
// Quaternion identity + `hang` seen as a quaternion.
let id_plus_hang = na::Quaternion::new(T::one(), hang.x, hang.y, hang.z);
pose.rotation = Rotation::new_unchecked(id_plus_hang * pose.rotation.into_inner());
// NOTE: don’t use renormalize_fast since the linearization might cause more drift.
// TODO: check for zeros?
pose.rotation.renormalize();
pose.translation.vector += self.linvel * dt;
}
}
impl std::ops::Mul<Real> for RigidBodyVelocity<Real> {
type Output = Self;
fn mul(self, rhs: Real) -> Self {
@@ -735,7 +781,7 @@ impl std::ops::Mul<Real> for RigidBodyVelocity {
}
}
impl std::ops::Add<RigidBodyVelocity> for RigidBodyVelocity {
impl std::ops::Add<RigidBodyVelocity<Real>> for RigidBodyVelocity<Real> {
type Output = Self;
fn add(self, rhs: Self) -> Self {
@@ -746,14 +792,14 @@ impl std::ops::Add<RigidBodyVelocity> for RigidBodyVelocity {
}
}
impl std::ops::AddAssign<RigidBodyVelocity> for RigidBodyVelocity {
impl std::ops::AddAssign<RigidBodyVelocity<Real>> for RigidBodyVelocity<Real> {
fn add_assign(&mut self, rhs: Self) {
self.linvel += rhs.linvel;
self.angvel += rhs.angvel;
}
}
impl std::ops::Sub<RigidBodyVelocity> for RigidBodyVelocity {
impl std::ops::Sub<RigidBodyVelocity<Real>> for RigidBodyVelocity<Real> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {
@@ -764,7 +810,7 @@ impl std::ops::Sub<RigidBodyVelocity> for RigidBodyVelocity {
}
}
impl std::ops::SubAssign<RigidBodyVelocity> for RigidBodyVelocity {
impl std::ops::SubAssign<RigidBodyVelocity<Real>> for RigidBodyVelocity<Real> {
fn sub_assign(&mut self, rhs: Self) {
self.linvel -= rhs.linvel;
self.angvel -= rhs.angvel;
@@ -774,18 +820,18 @@ impl std::ops::SubAssign<RigidBodyVelocity> for RigidBodyVelocity {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug, Copy, PartialEq)]
/// Damping factors to progressively slow down a rigid-body.
pub struct RigidBodyDamping {
pub struct RigidBodyDamping<T> {
/// Damping factor for gradually slowing down the translational motion of the rigid-body.
pub linear_damping: Real,
pub linear_damping: T,
/// Damping factor for gradually slowing down the angular motion of the rigid-body.
pub angular_damping: Real,
pub angular_damping: T,
}
impl Default for RigidBodyDamping {
impl<T: SimdRealCopy> Default for RigidBodyDamping<T> {
fn default() -> Self {
Self {
linear_damping: 0.0,
angular_damping: 0.0,
linear_damping: T::zero(),
angular_damping: T::zero(),
}
}
}
@@ -805,6 +851,9 @@ pub struct RigidBodyForces {
pub user_force: Vector<Real>,
/// Torque applied by the user.
pub user_torque: AngVector<Real>,
/// Are gyroscopic forces enabled for this rigid-body?
#[cfg(feature = "dim3")]
pub gyroscopic_forces_enabled: bool,
}
impl Default for RigidBodyForces {
@@ -815,6 +864,8 @@ impl Default for RigidBodyForces {
gravity_scale: 1.0,
user_force: na::zero(),
user_torque: na::zero(),
#[cfg(feature = "dim3")]
gyroscopic_forces_enabled: false,
}
}
}
@@ -825,12 +876,11 @@ impl RigidBodyForces {
pub fn integrate(
&self,
dt: Real,
init_vels: &RigidBodyVelocity,
init_vels: &RigidBodyVelocity<Real>,
mprops: &RigidBodyMassProps,
) -> RigidBodyVelocity {
) -> RigidBodyVelocity<Real> {
let linear_acc = self.force.component_mul(&mprops.effective_inv_mass);
let angular_acc = mprops.effective_world_inv_inertia_sqrt
* (mprops.effective_world_inv_inertia_sqrt * self.torque);
let angular_acc = mprops.effective_world_inv_inertia * self.torque;
RigidBodyVelocity {
linvel: init_vels.linvel + linear_acc * dt,
@@ -898,7 +948,7 @@ impl Default for RigidBodyCcd {
impl RigidBodyCcd {
/// The maximum velocity any point of any collider attached to this rigid-body
/// moving with the given velocity can have.
pub fn max_point_velocity(&self, vels: &RigidBodyVelocity) -> Real {
pub fn max_point_velocity(&self, vels: &RigidBodyVelocity<Real>) -> Real {
#[cfg(feature = "dim2")]
return vels.linvel.norm() + vels.angvel.abs() * self.ccd_max_dist;
#[cfg(feature = "dim3")]
@@ -909,7 +959,7 @@ impl RigidBodyCcd {
pub fn is_moving_fast(
&self,
dt: Real,
vels: &RigidBodyVelocity,
vels: &RigidBodyVelocity<Real>,
forces: Option<&RigidBodyForces>,
) -> bool {
// NOTE: for the threshold we don't use the exact CCD thickness. Theoretically, we
@@ -937,15 +987,26 @@ impl RigidBodyCcd {
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Default, Clone, Debug, Copy, PartialEq, Eq, Hash)]
#[derive(Clone, Debug, Copy, PartialEq, Eq, Hash)]
/// Internal identifiers used by the physics engine.
pub struct RigidBodyIds {
pub(crate) active_island_id: usize,
pub(crate) active_set_id: usize,
pub(crate) active_set_offset: usize,
pub(crate) active_set_offset: u32,
pub(crate) active_set_timestamp: u32,
}
impl Default for RigidBodyIds {
fn default() -> Self {
Self {
active_island_id: usize::MAX,
active_set_id: usize::MAX,
active_set_offset: u32::MAX,
active_set_timestamp: 0,
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Default, Clone, Debug, PartialEq, Eq)]
/// The set of colliders attached to this rigid-bodies.
@@ -974,6 +1035,7 @@ impl RigidBodyColliders {
/// Attach a collider to this rigid-body.
pub fn attach_collider(
&mut self,
rb_type: RigidBodyType,
rb_changes: &mut RigidBodyChanges,
rb_ccd: &mut RigidBodyCcd,
rb_mprops: &mut RigidBodyMassProps,
@@ -1002,7 +1064,7 @@ impl RigidBodyColliders {
.transform_by(&co_parent.pos_wrt_parent);
self.0.push(co_handle);
rb_mprops.local_mprops += mass_properties;
rb_mprops.update_world_mass_properties(&rb_pos.position);
rb_mprops.update_world_mass_properties(rb_type, &rb_pos.position);
}
/// Update the positions of all the colliders attached to this rigid-body.
@@ -1035,7 +1097,7 @@ pub struct RigidBodyDominance(pub i8);
impl RigidBodyDominance {
/// The actual dominance group of this rigid-body, after taking into account its type.
pub fn effective_group(&self, status: &RigidBodyType) -> i16 {
if status.is_dynamic() {
if status.is_dynamic_or_kinematic() {
self.0 as i16
} else {
i8::MAX as i16 + 1

7
src/dynamics/rigid_body_set.rs
View File

@@ -1,6 +1,7 @@
use crate::data::{Arena, HasModifiedFlag, ModifiedObjects};
use crate::dynamics::{
ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyChanges, RigidBodyHandle,
ImpulseJointSet, IslandManager, MultibodyJointSet, RigidBody, RigidBodyBuilder,
RigidBodyChanges, RigidBodyHandle,
};
use crate::geometry::ColliderSet;
use std::ops::{Index, IndexMut};
@@ -46,6 +47,8 @@ pub struct RigidBodySet {
// Could we avoid this?
pub(crate) bodies: Arena<RigidBody>,
pub(crate) modified_bodies: ModifiedRigidBodies,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub(crate) default_fixed: RigidBody,
}
impl RigidBodySet {
@@ -54,6 +57,7 @@ impl RigidBodySet {
RigidBodySet {
bodies: Arena::new(),
modified_bodies: ModifiedObjects::default(),
default_fixed: RigidBodyBuilder::fixed().build(),
}
}
@@ -62,6 +66,7 @@ impl RigidBodySet {
RigidBodySet {
bodies: Arena::with_capacity(capacity),
modified_bodies: ModifiedRigidBodies::with_capacity(capacity),
default_fixed: RigidBodyBuilder::fixed().build(),
}
}

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

@@ -6,9 +6,7 @@ pub(crate) fn categorize_contacts(
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
out_one_body: &mut Vec<ContactManifoldIndex>,
out_two_body: &mut Vec<ContactManifoldIndex>,
out_generic_one_body: &mut Vec<ContactManifoldIndex>,
out_generic_two_body: &mut Vec<ContactManifoldIndex>,
) {
for manifold_i in manifold_indices {
@@ -25,13 +23,7 @@ pub(crate) fn categorize_contacts(
.and_then(|h| multibody_joints.rigid_body_link(h))
.is_some()
{
if manifold.data.relative_dominance != 0 {
out_generic_one_body.push(*manifold_i);
} else {
out_generic_two_body.push(*manifold_i);
}
} else if manifold.data.relative_dominance != 0 {
out_one_body.push(*manifold_i)
out_generic_two_body.push(*manifold_i);
} else {
out_two_body.push(*manifold_i)
}
@@ -39,30 +31,19 @@ pub(crate) fn categorize_contacts(
}
pub(crate) fn categorize_joints(
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
impulse_joints: &[JointGraphEdge],
joint_indices: &[JointIndex],
one_body_joints: &mut Vec<JointIndex>,
two_body_joints: &mut Vec<JointIndex>,
generic_one_body_joints: &mut Vec<JointIndex>,
generic_two_body_joints: &mut Vec<JointIndex>,
) {
for joint_i in joint_indices {
let joint = &impulse_joints[*joint_i].weight;
let rb1 = &bodies[joint.body1.0];
let rb2 = &bodies[joint.body2.0];
if multibody_joints.rigid_body_link(joint.body1).is_some()
|| multibody_joints.rigid_body_link(joint.body2).is_some()
{
if !rb1.is_dynamic() || !rb2.is_dynamic() {
generic_one_body_joints.push(*joint_i);
} else {
generic_two_body_joints.push(*joint_i);
}
} else if !rb1.is_dynamic() || !rb2.is_dynamic() {
one_body_joints.push(*joint_i);
generic_two_body_joints.push(*joint_i);
} else {
two_body_joints.push(*joint_i);
}

63
src/dynamics/solver/contact_constraint/any_contact_constraint.rs Normal file
View File

@@ -0,0 +1,63 @@
use crate::dynamics::solver::solver_body::SolverBodies;
use crate::dynamics::solver::{ContactWithCoulombFriction, GenericContactConstraint};
use crate::math::Real;
use na::DVector;
#[cfg(feature = "dim3")]
use crate::dynamics::solver::ContactWithTwistFriction;
use crate::prelude::ContactManifold;
#[derive(Debug)]
pub enum AnyContactConstraintMut<'a> {
Generic(&'a mut GenericContactConstraint),
WithCoulombFriction(&'a mut ContactWithCoulombFriction),
#[cfg(feature = "dim3")]
WithTwistFriction(&'a mut ContactWithTwistFriction),
}
impl AnyContactConstraintMut<'_> {
pub fn remove_bias(&mut self) {
match self {
Self::Generic(c) => c.remove_cfm_and_bias_from_rhs(),
Self::WithCoulombFriction(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "dim3")]
Self::WithTwistFriction(c) => c.remove_cfm_and_bias_from_rhs(),
}
}
pub fn warmstart(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::Generic(c) => c.warmstart(generic_jacobians, solver_vels, generic_solver_vels),
Self::WithCoulombFriction(c) => c.warmstart(solver_vels),
#[cfg(feature = "dim3")]
Self::WithTwistFriction(c) => c.warmstart(solver_vels),
}
}
pub fn solve(
&mut self,
generic_jacobians: &DVector<Real>,
bodies: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::Generic(c) => c.solve(generic_jacobians, bodies, generic_solver_vels, true, true),
Self::WithCoulombFriction(c) => c.solve(bodies, true, true),
#[cfg(feature = "dim3")]
Self::WithTwistFriction(c) => c.solve(bodies, true, true),
}
}
pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) {
match self {
Self::Generic(c) => c.writeback_impulses(manifolds_all),
Self::WithCoulombFriction(c) => c.writeback_impulses(manifolds_all),
#[cfg(feature = "dim3")]
Self::WithTwistFriction(c) => c.writeback_impulses(manifolds_all),
}
}
}

262
src/dynamics/solver/contact_constraint/two_body_constraint_element.rs → src/dynamics/solver/contact_constraint/contact_constraint_element.rs
View File

@@ -1,14 +1,57 @@
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::SolverVel;
use crate::math::{AngVector, DIM, TangentImpulse, Vector};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
use crate::utils::{SimdDot, SimdRealCopy};
use na::Vector2;
use simba::simd::SimdValue;
#[cfg(feature = "dim3")]
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
pub gcross1: [AngVector<N>; DIM - 1],
pub gcross2: [AngVector<N>; DIM - 1],
pub(crate) struct ContactConstraintTwistPart<N: SimdRealCopy> {
// pub twist_dir: AngVector<N>, // NOTE: The torque direction equals the normal in 3D and 1.0 in 2D.
pub ii_twist_dir1: AngVector<N>,
pub ii_twist_dir2: AngVector<N>,
pub rhs: N,
pub impulse: N,
pub impulse_accumulator: N,
pub r: N,
}
#[cfg(feature = "dim3")]
impl<N: SimdRealCopy> ContactConstraintTwistPart<N> {
#[inline]
pub fn warmstart(&mut self, solver_vel1: &mut SolverVel<N>, solver_vel2: &mut SolverVel<N>)
where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
solver_vel1.angular += self.ii_twist_dir1 * self.impulse;
solver_vel2.angular += self.ii_twist_dir2 * self.impulse;
}
#[inline]
pub fn solve(
&mut self,
twist_dir1: &AngVector<N>,
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
let dvel = twist_dir1.gdot(solver_vel1.angular - solver_vel2.angular) + self.rhs;
let new_impulse = (self.impulse - self.r * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel1.angular += self.ii_twist_dir1 * dlambda;
solver_vel2.angular += self.ii_twist_dir2 * dlambda;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct ContactConstraintTangentPart<N: SimdRealCopy> {
pub torque_dir1: [AngVector<N>; DIM - 1],
pub torque_dir2: [AngVector<N>; DIM - 1],
pub ii_torque_dir1: [AngVector<N>; DIM - 1],
pub ii_torque_dir2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
pub rhs_wo_bias: [N; DIM - 1],
#[cfg(feature = "dim2")]
@@ -25,11 +68,13 @@ pub(crate) struct TwoBodyConstraintTangentPart<N: SimdRealCopy> {
pub r: [N; DIM],
}
impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
fn zero() -> Self {
impl<N: SimdRealCopy> ContactConstraintTangentPart<N> {
pub fn zero() -> Self {
Self {
gcross1: [na::zero(); DIM - 1],
gcross2: [na::zero(); DIM - 1],
torque_dir1: [na::zero(); DIM - 1],
torque_dir2: [na::zero(); DIM - 1],
ii_torque_dir1: [na::zero(); DIM - 1],
ii_torque_dir2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
rhs_wo_bias: [na::zero(); DIM - 1],
impulse: na::zero(),
@@ -61,23 +106,24 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
#[cfg(feature = "dim2")]
{
solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0];
solver_vel1.angular += self.gcross1[0] * self.impulse[0];
solver_vel1.angular += self.ii_torque_dir1[0] * self.impulse[0];
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
solver_vel2.angular += self.gcross2[0] * self.impulse[0];
solver_vel2.angular += self.ii_torque_dir2[0] * self.impulse[0];
}
#[cfg(feature = "dim3")]
{
solver_vel1.linear += tangents1[0].component_mul(im1) * self.impulse[0]
+ tangents1[1].component_mul(im1) * self.impulse[1];
solver_vel1.linear += (tangents1[0] * self.impulse[0] + tangents1[1] * self.impulse[1])
.component_mul(im1);
solver_vel1.angular +=
self.gcross1[0] * self.impulse[0] + self.gcross1[1] * self.impulse[1];
self.ii_torque_dir1[0] * self.impulse[0] + self.ii_torque_dir1[1] * self.impulse[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
+ tangents1[1].component_mul(im2) * -self.impulse[1];
solver_vel2.linear += (tangents1[0] * -self.impulse[0]
+ tangents1[1] * -self.impulse[1])
.component_mul(im2);
solver_vel2.angular +=
self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
self.ii_torque_dir2[0] * self.impulse[0] + self.ii_torque_dir2[1] * self.impulse[1];
}
}
@@ -96,32 +142,32 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
#[cfg(feature = "dim2")]
{
let dvel = tangents1[0].dot(&solver_vel1.linear)
+ self.gcross1[0].gdot(solver_vel1.angular)
+ self.torque_dir1[0].gdot(solver_vel1.angular)
- tangents1[0].dot(&solver_vel2.linear)
+ self.gcross2[0].gdot(solver_vel2.angular)
+ self.torque_dir2[0].gdot(solver_vel2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda;
solver_vel1.angular += self.gcross1[0] * dlambda;
solver_vel1.angular += self.ii_torque_dir1[0] * dlambda;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
solver_vel2.angular += self.ii_torque_dir2[0] * dlambda;
}
#[cfg(feature = "dim3")]
{
let dvel_0 = tangents1[0].dot(&solver_vel1.linear)
+ self.gcross1[0].gdot(solver_vel1.angular)
+ self.torque_dir1[0].gdot(solver_vel1.angular)
- tangents1[0].dot(&solver_vel2.linear)
+ self.gcross2[0].gdot(solver_vel2.angular)
+ self.torque_dir2[0].gdot(solver_vel2.angular)
+ self.rhs[0];
let dvel_1 = tangents1[1].dot(&solver_vel1.linear)
+ self.gcross1[1].gdot(solver_vel1.angular)
+ self.torque_dir1[1].gdot(solver_vel1.angular)
- tangents1[1].dot(&solver_vel2.linear)
+ self.gcross2[1].gdot(solver_vel2.angular)
+ self.torque_dir2[1].gdot(solver_vel2.angular)
+ self.rhs[1];
let dvel_00 = dvel_0 * dvel_0;
@@ -143,21 +189,25 @@ impl<N: SimdRealCopy> TwoBodyConstraintTangentPart<N> {
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel1.linear += tangents1[0].component_mul(im1) * dlambda[0]
+ tangents1[1].component_mul(im1) * dlambda[1];
solver_vel1.angular += self.gcross1[0] * dlambda[0] + self.gcross1[1] * dlambda[1];
solver_vel1.linear +=
(tangents1[0] * dlambda[0] + tangents1[1] * dlambda[1]).component_mul(im1);
solver_vel1.angular +=
self.ii_torque_dir1[0] * dlambda[0] + self.ii_torque_dir1[1] * dlambda[1];
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
solver_vel2.linear +=
(tangents1[0] * -dlambda[0] + tangents1[1] * -dlambda[1]).component_mul(im2);
solver_vel2.angular +=
self.ii_torque_dir2[0] * dlambda[0] + self.ii_torque_dir2[1] * dlambda[1];
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
pub gcross1: AngVector<N>,
pub gcross2: AngVector<N>,
pub(crate) struct ContactConstraintNormalPart<N: SimdRealCopy> {
pub torque_dir1: AngVector<N>,
pub torque_dir2: AngVector<N>,
pub ii_torque_dir1: AngVector<N>,
pub ii_torque_dir2: AngVector<N>,
pub rhs: N,
pub rhs_wo_bias: N,
pub impulse: N,
@@ -170,11 +220,13 @@ pub(crate) struct TwoBodyConstraintNormalPart<N: SimdRealCopy> {
pub r_mat_elts: [N; 2],
}
impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
fn zero() -> Self {
impl<N: SimdRealCopy> ContactConstraintNormalPart<N> {
pub fn zero() -> Self {
Self {
gcross1: na::zero(),
gcross2: na::zero(),
torque_dir1: na::zero(),
torque_dir2: na::zero(),
ii_torque_dir1: na::zero(),
ii_torque_dir2: na::zero(),
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
@@ -200,10 +252,10 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
solver_vel2: &mut SolverVel<N>,
) {
solver_vel1.linear += dir1.component_mul(im1) * self.impulse;
solver_vel1.angular += self.gcross1 * self.impulse;
solver_vel1.angular += self.ii_torque_dir1 * self.impulse;
solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
solver_vel2.angular += self.gcross2 * self.impulse;
solver_vel2.angular += self.ii_torque_dir2 * self.impulse;
}
#[inline]
@@ -218,22 +270,22 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
let dvel = dir1.dot(&solver_vel1.linear) + self.gcross1.gdot(solver_vel1.angular)
let dvel = dir1.dot(&solver_vel1.linear) + self.torque_dir1.gdot(solver_vel1.angular)
- dir1.dot(&solver_vel2.linear)
+ self.gcross2.gdot(solver_vel2.angular)
+ self.torque_dir2.gdot(solver_vel2.angular)
+ self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel1.linear += dir1.component_mul(im1) * dlambda;
solver_vel1.angular += self.gcross1 * dlambda;
solver_vel1.angular += self.ii_torque_dir1 * dlambda;
solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2 * dlambda;
solver_vel2.angular += self.ii_torque_dir2 * dlambda;
}
#[inline(always)]
#[inline]
pub(crate) fn solve_mlcp_two_constraints(
dvel: Vector2<N>,
prev_impulse: Vector2<N>,
@@ -280,12 +332,12 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
{
let dvel_lin = dir1.dot(&solver_vel1.linear) - dir1.dot(&solver_vel2.linear);
let dvel_a = dvel_lin
+ constraint_a.gcross1.gdot(solver_vel1.angular)
+ constraint_a.gcross2.gdot(solver_vel2.angular)
+ constraint_a.torque_dir1.gdot(solver_vel1.angular)
+ constraint_a.torque_dir2.gdot(solver_vel2.angular)
+ constraint_a.rhs;
let dvel_b = dvel_lin
+ constraint_b.gcross1.gdot(solver_vel1.angular)
+ constraint_b.gcross2.gdot(solver_vel2.angular)
+ constraint_b.torque_dir1.gdot(solver_vel1.angular)
+ constraint_b.torque_dir2.gdot(solver_vel2.angular)
+ constraint_b.rhs;
let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse);
@@ -305,114 +357,10 @@ impl<N: SimdRealCopy> TwoBodyConstraintNormalPart<N> {
constraint_b.impulse = new_impulse.y;
solver_vel1.linear += dir1.component_mul(im1) * (dlambda.x + dlambda.y);
solver_vel1.angular += constraint_a.gcross1 * dlambda.x + constraint_b.gcross1 * dlambda.y;
solver_vel1.angular +=
constraint_a.ii_torque_dir1 * dlambda.x + constraint_b.ii_torque_dir1 * dlambda.y;
solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y);
solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintElement<N: SimdRealCopy> {
pub normal_part: TwoBodyConstraintNormalPart<N>,
pub tangent_part: TwoBodyConstraintTangentPart<N>,
}
impl<N: SimdRealCopy> TwoBodyConstraintElement<N> {
pub fn zero() -> Self {
Self {
normal_part: TwoBodyConstraintNormalPart::zero(),
tangent_part: TwoBodyConstraintTangentPart::zero(),
}
}
#[inline]
pub fn warmstart_group(
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
) {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
element
.normal_part
.warmstart(dir1, im1, im2, solver_vel1, solver_vel2);
element
.tangent_part
.warmstart(tangents1, im1, im2, solver_vel1, solver_vel2);
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im1: &Vector<N>,
im2: &Vector<N>,
limit: N,
solver_vel1: &mut SolverVel<N>,
solver_vel2: &mut SolverVel<N>,
solve_restitution: bool,
solve_friction: bool,
) where
Vector<N>: SimdBasis,
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
if solve_restitution {
if BLOCK_SOLVER_ENABLED {
for elements in elements.chunks_exact_mut(2) {
let [element_a, element_b] = elements else {
unreachable!()
};
TwoBodyConstraintNormalPart::solve_pair(
&mut element_a.normal_part,
&mut element_b.normal_part,
cfm_factor,
dir1,
im1,
im2,
solver_vel1,
solver_vel2,
);
}
// There is one constraint left to solve if there isn’t an even number.
if elements.len() % 2 == 1 {
let element = elements.last_mut().unwrap();
element
.normal_part
.solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
}
} else {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, dir1, im1, im2, solver_vel1, solver_vel2);
}
}
}
if solve_friction {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.solve(tangents1, im1, im2, limit, solver_vel1, solver_vel2);
}
}
solver_vel2.angular +=
constraint_a.ii_torque_dir2 * dlambda.x + constraint_b.ii_torque_dir2 * dlambda.y;
}
}

580
src/dynamics/solver/contact_constraint/contact_constraints_set.rs
View File

@@ -1,28 +1,26 @@
use crate::dynamics::solver::categorization::categorize_contacts;
use crate::dynamics::solver::contact_constraint::{
GenericOneBodyConstraint, GenericOneBodyConstraintBuilder, GenericTwoBodyConstraint,
GenericTwoBodyConstraintBuilder, OneBodyConstraint, OneBodyConstraintBuilder,
TwoBodyConstraint, TwoBodyConstraintBuilder,
ContactWithCoulombFriction, ContactWithCoulombFrictionBuilder, GenericContactConstraint,
GenericContactConstraintBuilder,
};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::solver_vel::SolverVel;
use crate::dynamics::solver::{ConstraintTypes, SolverConstraintsSet, reset_buffer};
use crate::dynamics::solver::interaction_groups::InteractionGroups;
use crate::dynamics::solver::reset_buffer;
use crate::dynamics::solver::solver_body::SolverBodies;
use crate::dynamics::{
ImpulseJoint, IntegrationParameters, IslandManager, JointAxesMask, MultibodyJointSet,
RigidBodySet,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::SIMD_WIDTH;
use crate::math::{MAX_MANIFOLD_POINTS, Real};
use na::DVector;
use parry::math::DIM;
#[cfg(feature = "simd-is-enabled")]
use {
crate::dynamics::solver::contact_constraint::{
OneBodyConstraintSimd, SimdOneBodyConstraintBuilder, TwoBodyConstraintBuilderSimd,
TwoBodyConstraintSimd,
},
crate::math::SIMD_WIDTH,
use crate::dynamics::solver::contact_constraint::any_contact_constraint::AnyContactConstraintMut;
#[cfg(feature = "dim3")]
use crate::dynamics::{
FrictionModel,
solver::contact_constraint::{ContactWithTwistFriction, ContactWithTwistFrictionBuilder},
};
#[derive(Debug)]
@@ -63,30 +61,85 @@ impl ConstraintsCounts {
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct ContactConstraintTypes;
pub(crate) struct ContactConstraintsSet {
pub generic_jacobians: DVector<Real>,
pub two_body_interactions: Vec<ContactManifoldIndex>,
pub generic_two_body_interactions: Vec<ContactManifoldIndex>,
pub interaction_groups: InteractionGroups,
impl ConstraintTypes for ContactConstraintTypes {
type OneBody = OneBodyConstraint;
type TwoBodies = TwoBodyConstraint;
type GenericOneBody = GenericOneBodyConstraint;
type GenericTwoBodies = GenericTwoBodyConstraint;
#[cfg(feature = "simd-is-enabled")]
type SimdOneBody = OneBodyConstraintSimd;
#[cfg(feature = "simd-is-enabled")]
type SimdTwoBodies = TwoBodyConstraintSimd;
pub generic_velocity_constraints: Vec<GenericContactConstraint>,
pub simd_velocity_coulomb_constraints: Vec<ContactWithCoulombFriction>,
#[cfg(feature = "dim3")]
pub simd_velocity_twist_constraints: Vec<ContactWithTwistFriction>,
type BuilderOneBody = OneBodyConstraintBuilder;
type BuilderTwoBodies = TwoBodyConstraintBuilder;
type GenericBuilderOneBody = GenericOneBodyConstraintBuilder;
type GenericBuilderTwoBodies = GenericTwoBodyConstraintBuilder;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderOneBody = SimdOneBodyConstraintBuilder;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderTwoBodies = TwoBodyConstraintBuilderSimd;
pub generic_velocity_constraints_builder: Vec<GenericContactConstraintBuilder>,
pub simd_velocity_coulomb_constraints_builder: Vec<ContactWithCoulombFrictionBuilder>,
#[cfg(feature = "dim3")]
pub simd_velocity_twist_constraints_builder: Vec<ContactWithTwistFrictionBuilder>,
}
pub type ContactConstraintsSet = SolverConstraintsSet<ContactConstraintTypes>;
impl ContactConstraintsSet {
pub fn new() -> Self {
Self {
generic_jacobians: DVector::zeros(0),
two_body_interactions: vec![],
generic_two_body_interactions: vec![],
interaction_groups: InteractionGroups::new(),
generic_velocity_constraints: vec![],
simd_velocity_coulomb_constraints: vec![],
generic_velocity_constraints_builder: vec![],
simd_velocity_coulomb_constraints_builder: vec![],
#[cfg(feature = "dim3")]
simd_velocity_twist_constraints: vec![],
#[cfg(feature = "dim3")]
simd_velocity_twist_constraints_builder: vec![],
}
}
pub fn clear_constraints(&mut self) {
self.generic_jacobians.fill(0.0);
self.generic_velocity_constraints.clear();
self.simd_velocity_coulomb_constraints.clear();
#[cfg(feature = "dim3")]
self.simd_velocity_twist_constraints.clear();
}
pub fn clear_builders(&mut self) {
self.generic_velocity_constraints_builder.clear();
self.simd_velocity_coulomb_constraints_builder.clear();
#[cfg(feature = "dim3")]
self.simd_velocity_twist_constraints_builder.clear();
}
// Returns the generic jacobians and a mutable iterator through all the constraints.
pub fn iter_constraints_mut(
&mut self,
) -> (
&DVector<Real>,
impl Iterator<Item = AnyContactConstraintMut<'_>>,
) {
let jac = &self.generic_jacobians;
let a = self
.generic_velocity_constraints
.iter_mut()
.map(AnyContactConstraintMut::Generic);
let b = self
.simd_velocity_coulomb_constraints
.iter_mut()
.map(AnyContactConstraintMut::WithCoulombFriction);
#[cfg(feature = "dim3")]
{
let c = self
.simd_velocity_twist_constraints
.iter_mut()
.map(AnyContactConstraintMut::WithTwistFriction);
(jac, a.chain(b).chain(c))
}
#[cfg(feature = "dim2")]
return (jac, a.chain(b));
}
}
impl ContactConstraintsSet {
pub fn init_constraint_groups(
@@ -99,18 +152,14 @@ impl ContactConstraintsSet {
manifold_indices: &[ContactManifoldIndex],
) {
self.two_body_interactions.clear();
self.one_body_interactions.clear();
self.generic_two_body_interactions.clear();
self.generic_one_body_interactions.clear();
categorize_contacts(
bodies,
multibody_joints,
manifolds,
manifold_indices,
&mut self.one_body_interactions,
&mut self.two_body_interactions,
&mut self.generic_one_body_interactions,
&mut self.generic_two_body_interactions,
);
@@ -123,22 +172,13 @@ impl ContactConstraintsSet {
&self.two_body_interactions,
);
self.one_body_interaction_groups.clear_groups();
self.one_body_interaction_groups.group_manifolds(
island_id,
islands,
bodies,
manifolds,
&self.one_body_interactions,
);
// NOTE: uncomment this do disable SIMD contact resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.simd_interactions);
// self.one_body_interaction_groups
// .nongrouped_interactions
// .append(&mut self.one_body_interaction_groups.simd_interactions);
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.simd_interactions);
// self.one_body_interaction_groups
// .nongrouped_interactions
// .append(&mut self.one_body_interaction_groups.simd_interactions);
}
pub fn init(
@@ -146,10 +186,13 @@ impl ContactConstraintsSet {
island_id: usize,
islands: &IslandManager,
bodies: &RigidBodySet,
solver_bodies: &SolverBodies,
multibody_joints: &MultibodyJointSet,
manifolds: &[&mut ContactManifold],
manifold_indices: &[ContactManifoldIndex],
#[cfg(feature = "dim3")] friction_model: FrictionModel,
) {
// let t0 = std::time::Instant::now();
self.clear_constraints();
self.clear_builders();
@@ -162,32 +205,133 @@ impl ContactConstraintsSet {
manifold_indices,
);
// let t_init_groups = t0.elapsed().as_secs_f32();
// let t0 = std::time::Instant::now();
let mut jacobian_id = 0;
#[cfg(feature = "simd-is-enabled")]
{
self.simd_compute_constraints(bodies, manifolds);
}
self.compute_constraints(bodies, manifolds);
self.compute_generic_constraints(bodies, multibody_joints, manifolds, &mut jacobian_id);
// let t_init_constraints = t0.elapsed().as_secs_f32();
#[cfg(feature = "simd-is-enabled")]
{
self.simd_compute_one_body_constraints(bodies, manifolds);
// let t0 = std::time::Instant::now();
// #[cfg(feature = "simd-is-enabled")]
// {
// self.simd_compute_constraints_bench(bodies, solver_bodies, manifolds);
// }
// let t_init_constraint_bench = t0.elapsed().as_secs_f32();
// let t0 = std::time::Instant::now();
#[cfg(feature = "dim2")]
self.simd_compute_coulomb_constraints(bodies, solver_bodies, manifolds);
#[cfg(feature = "dim3")]
match friction_model {
FrictionModel::Simplified => {
self.simd_compute_twist_constraints(bodies, solver_bodies, manifolds)
}
FrictionModel::Coulomb => {
self.simd_compute_coulomb_constraints(bodies, solver_bodies, manifolds)
}
}
self.compute_one_body_constraints(bodies, manifolds);
self.compute_generic_one_body_constraints(
bodies,
multibody_joints,
manifolds,
&mut jacobian_id,
);
// let t_init_constraints_simd = t0.elapsed().as_secs_f32();
// let num_simd_constraints = self.simd_velocity_constraints.len();
// println!(
// "t_init_group: {:?}, t_init_constraints_simd: {}: {:?}, t_debug: {:?}",
// t_init_groups * 1000.0,
// num_simd_constraints,
// t_init_constraints_simd * 1000.0,
// t_init_constraint_bench * 1000.0,
// );
// println!(
// "Solver constraints init: {}",
// t0.elapsed().as_secs_f32() * 1000.0
// );
}
#[cfg(feature = "simd-is-enabled")]
fn simd_compute_constraints(
// #[cfg(feature = "simd-is-enabled")]
// fn simd_compute_constraints_bench(
// &mut self,
// bodies: &RigidBodySet,
// solver_bodies: &SolverBodies,
// manifolds_all: &[&mut ContactManifold],
// ) {
// let total_num_constraints = self
// .interaction_groups
// .simd_interactions
// .chunks_exact(SIMD_WIDTH)
// .map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0] as usize]).num_constraints)
// .sum::<usize>();
//
// unsafe {
// reset_buffer(
// &mut self.simd_velocity_constraints_builder,
// total_num_constraints as usize,
// );
// reset_buffer(
// &mut self.simd_velocity_constraints,
// total_num_constraints as usize,
// );
// }
//
// let mut curr_start = 0;
//
// let t0 = std::time::Instant::now();
// let preload = TwoBodyConstraintBuilderSimd::collect_constraint_gen_data(
// bodies,
// &*manifolds_all,
// &self.interaction_groups.simd_interactions,
// );
// println!("Preload: {:?}", t0.elapsed().as_secs_f32() * 1000.0);
//
// let t0 = std::time::Instant::now();
// for i in (0..self.interaction_groups.simd_interactions.len()).step_by(SIMD_WIDTH) {
// let num_to_add = 1; // preload.solver_contact_headers[i].num_contacts;
// TwoBodyConstraintBuilderSimd::generate_bench_preloaded(
// &preload,
// i,
// solver_bodies,
// &mut self.simd_velocity_constraints_builder[curr_start..],
// &mut self.simd_velocity_constraints[curr_start..],
// );
//
// curr_start += num_to_add;
// }
// println!("Preloaded init: {:?}", t0.elapsed().as_secs_f32() * 1000.0);
//
// /*
// for manifolds_i in self
// .interaction_groups
// .simd_interactions
// .chunks_exact(SIMD_WIDTH)
// {
// let num_to_add =
// ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints;
// let manifold_id = array![|ii| manifolds_i[ii]];
// let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]];
//
// TwoBodyConstraintBuilderSimd::generate_bench(
// manifold_id,
// manifolds,
// bodies,
// solver_bodies,
// &mut self.simd_velocity_constraints_builder[curr_start..],
// &mut self.simd_velocity_constraints[curr_start..],
// );
//
// curr_start += num_to_add;
// }
// */
//
// // assert_eq!(curr_start, total_num_constraints);
// }
// TODO: could we somehow combine that with the simd_compute_coulomb_constraints function since
// both are very similar and mutually exclusive?
#[cfg(feature = "dim3")]
fn simd_compute_twist_constraints(
&mut self,
bodies: &RigidBodySet,
solver_bodies: &SolverBodies,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
@@ -195,14 +339,23 @@ impl ContactConstraintsSet {
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints)
.sum();
.sum::<usize>()
+ self
.interaction_groups
.nongrouped_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum::<usize>();
unsafe {
reset_buffer(
&mut self.simd_velocity_constraints_builder,
&mut self.simd_velocity_twist_constraints_builder,
total_num_constraints,
);
reset_buffer(
&mut self.simd_velocity_twist_constraints,
total_num_constraints,
);
reset_buffer(&mut self.simd_velocity_constraints, total_num_constraints);
}
let mut curr_start = 0;
@@ -214,15 +367,35 @@ impl ContactConstraintsSet {
{
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints;
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
let manifold_id = array![|ii| manifolds_i[ii]];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]];
TwoBodyConstraintBuilderSimd::generate(
ContactWithTwistFrictionBuilder::generate(
manifold_id,
manifolds,
bodies,
&mut self.simd_velocity_constraints_builder[curr_start..],
&mut self.simd_velocity_constraints[curr_start..],
solver_bodies,
&mut self.simd_velocity_twist_constraints_builder[curr_start..],
&mut self.simd_velocity_twist_constraints[curr_start..],
);
curr_start += num_to_add;
}
for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() {
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints;
let mut manifold_id = [usize::MAX; SIMD_WIDTH];
manifold_id[0] = *manifolds_i;
let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH];
ContactWithTwistFrictionBuilder::generate(
manifold_id,
manifolds,
bodies,
solver_bodies,
&mut self.simd_velocity_twist_constraints_builder[curr_start..],
&mut self.simd_velocity_twist_constraints[curr_start..],
);
curr_start += num_to_add;
@@ -231,42 +404,79 @@ impl ContactConstraintsSet {
assert_eq!(curr_start, total_num_constraints);
}
fn compute_constraints(
fn simd_compute_coulomb_constraints(
&mut self,
bodies: &RigidBodySet,
solver_bodies: &SolverBodies,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.interaction_groups
.nongrouped_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum();
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints)
.sum::<usize>()
+ self
.interaction_groups
.nongrouped_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum::<usize>();
unsafe {
reset_buffer(
&mut self.velocity_constraints_builder,
&mut self.simd_velocity_coulomb_constraints_builder,
total_num_constraints,
);
reset_buffer(
&mut self.simd_velocity_coulomb_constraints,
total_num_constraints,
);
reset_buffer(&mut self.velocity_constraints, total_num_constraints);
}
let mut curr_start = 0;
for manifold_i in &self.interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
for manifolds_i in self
.interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
{
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints;
let manifold_id = array![|ii| manifolds_i[ii]];
let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]];
TwoBodyConstraintBuilder::generate(
*manifold_i,
manifold,
ContactWithCoulombFrictionBuilder::generate(
manifold_id,
manifolds,
bodies,
&mut self.velocity_constraints_builder[curr_start..],
&mut self.velocity_constraints[curr_start..],
solver_bodies,
&mut self.simd_velocity_coulomb_constraints_builder[curr_start..],
&mut self.simd_velocity_coulomb_constraints[curr_start..],
);
curr_start += num_to_add;
}
for manifolds_i in self.interaction_groups.nongrouped_interactions.iter() {
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[*manifolds_i]).num_constraints;
let mut manifold_id = [usize::MAX; SIMD_WIDTH];
manifold_id[0] = *manifolds_i;
let manifolds = [&*manifolds_all[*manifolds_i]; SIMD_WIDTH];
ContactWithCoulombFrictionBuilder::generate(
manifold_id,
manifolds,
bodies,
solver_bodies,
&mut self.simd_velocity_coulomb_constraints_builder[curr_start..],
&mut self.simd_velocity_coulomb_constraints[curr_start..],
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
@@ -281,14 +491,14 @@ impl ContactConstraintsSet {
.generic_two_body_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum();
.sum::<usize>();
self.generic_velocity_constraints_builder.resize(
total_num_constraints,
GenericTwoBodyConstraintBuilder::invalid(),
GenericContactConstraintBuilder::invalid(),
);
self.generic_velocity_constraints
.resize(total_num_constraints, GenericTwoBodyConstraint::invalid());
.resize(total_num_constraints, GenericContactConstraint::invalid());
let mut curr_start = 0;
@@ -296,7 +506,7 @@ impl ContactConstraintsSet {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
GenericTwoBodyConstraintBuilder::generate(
GenericContactConstraintBuilder::generate(
*manifold_i,
manifold,
bodies,
@@ -313,181 +523,39 @@ impl ContactConstraintsSet {
assert_eq!(curr_start, total_num_constraints);
}
fn compute_generic_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) {
let total_num_constraints = self
.generic_one_body_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum();
self.generic_velocity_one_body_constraints_builder.resize(
total_num_constraints,
GenericOneBodyConstraintBuilder::invalid(),
);
self.generic_velocity_one_body_constraints
.resize(total_num_constraints, GenericOneBodyConstraint::invalid());
let mut curr_start = 0;
for manifold_i in &self.generic_one_body_interactions {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
GenericOneBodyConstraintBuilder::generate(
*manifold_i,
manifold,
bodies,
multibody_joints,
&mut self.generic_velocity_one_body_constraints_builder[curr_start..],
&mut self.generic_velocity_one_body_constraints[curr_start..],
&mut self.generic_jacobians,
jacobian_id,
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
#[cfg(feature = "simd-is-enabled")]
fn simd_compute_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.one_body_interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[i[0]]).num_constraints)
.sum();
unsafe {
reset_buffer(
&mut self.simd_velocity_one_body_constraints_builder,
total_num_constraints,
);
reset_buffer(
&mut self.simd_velocity_one_body_constraints,
total_num_constraints,
);
}
let mut curr_start = 0;
for manifolds_i in self
.one_body_interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
{
let num_to_add =
ConstraintsCounts::from_contacts(manifolds_all[manifolds_i[0]]).num_constraints;
let manifold_id = gather![|ii| manifolds_i[ii]];
let manifolds = gather![|ii| &*manifolds_all[manifolds_i[ii]]];
SimdOneBodyConstraintBuilder::generate(
manifold_id,
manifolds,
bodies,
&mut self.simd_velocity_one_body_constraints_builder[curr_start..],
&mut self.simd_velocity_one_body_constraints[curr_start..],
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
fn compute_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
manifolds_all: &[&mut ContactManifold],
) {
let total_num_constraints = self
.one_body_interaction_groups
.nongrouped_interactions
.iter()
.map(|i| ConstraintsCounts::from_contacts(manifolds_all[*i]).num_constraints)
.sum();
unsafe {
reset_buffer(
&mut self.velocity_one_body_constraints_builder,
total_num_constraints,
);
reset_buffer(
&mut self.velocity_one_body_constraints,
total_num_constraints,
);
}
let mut curr_start = 0;
for manifold_i in &self.one_body_interaction_groups.nongrouped_interactions {
let manifold = &manifolds_all[*manifold_i];
let num_to_add = ConstraintsCounts::from_contacts(manifold).num_constraints;
OneBodyConstraintBuilder::generate(
*manifold_i,
manifold,
bodies,
&mut self.velocity_one_body_constraints_builder[curr_start..],
&mut self.velocity_one_body_constraints[curr_start..],
);
curr_start += num_to_add;
}
assert_eq!(curr_start, total_num_constraints);
}
pub fn warmstart(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solver_bodies: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.warmstart(jac, solver_vels, generic_solver_vels);
c.warmstart(jac, solver_bodies, generic_solver_vels);
}
}
#[profiling::function]
pub fn solve_restitution(
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solver_bodies: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.solve_restitution(jac, solver_vels, generic_solver_vels);
c.solve(jac, solver_bodies, generic_solver_vels);
}
}
#[profiling::function]
pub fn solve_restitution_wo_bias(
pub fn solve_wo_bias(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solver_bodies: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.remove_bias();
c.solve_restitution(jac, solver_vels, generic_solver_vels);
}
}
#[profiling::function]
pub fn solve_friction(
&mut self,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
for mut c in constraints {
c.solve_friction(jac, solver_vels, generic_solver_vels);
c.solve(jac, solver_bodies, generic_solver_vels);
}
}
@@ -504,7 +572,7 @@ impl ContactConstraintsSet {
params: &IntegrationParameters,
small_step_id: usize,
multibodies: &MultibodyJointSet,
solver_bodies: &[SolverBody],
solver_bodies: &SolverBodies,
) {
macro_rules! update_contacts(
($builders: ident, $constraints: ident) => {
@@ -524,22 +592,14 @@ impl ContactConstraintsSet {
generic_velocity_constraints_builder,
generic_velocity_constraints
);
update_contacts!(velocity_constraints_builder, velocity_constraints);
#[cfg(feature = "simd-is-enabled")]
update_contacts!(simd_velocity_constraints_builder, simd_velocity_constraints);
update_contacts!(
generic_velocity_one_body_constraints_builder,
generic_velocity_one_body_constraints
simd_velocity_coulomb_constraints_builder,
simd_velocity_coulomb_constraints
);
#[cfg(feature = "dim3")]
update_contacts!(
velocity_one_body_constraints_builder,
velocity_one_body_constraints
);
#[cfg(feature = "simd-is-enabled")]
update_contacts!(
simd_velocity_one_body_constraints_builder,
simd_velocity_one_body_constraints
simd_velocity_twist_constraints_builder,
simd_velocity_twist_constraints
);
}
}

505
src/dynamics/solver/contact_constraint/contact_with_coulomb_friction.rs Normal file
View File

@@ -0,0 +1,505 @@
use super::{ContactConstraintNormalPart, ContactConstraintTangentPart};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::solver_body::SolverBodies;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex, SimdSolverContact};
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH, SimdReal, Vector};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
use num::Zero;
use parry::utils::SdpMatrix2;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub struct CoulombContactPointInfos<N: SimdRealCopy> {
pub tangent_vel: Vector<N>, // PERF: could be one float less, be shared by both contact point infos?
pub normal_vel: N,
pub local_p1: Point<N>,
pub local_p2: Point<N>,
pub dist: N,
}
impl<N: SimdRealCopy> Default for CoulombContactPointInfos<N> {
fn default() -> Self {
Self {
tangent_vel: Vector::zeros(),
normal_vel: N::zero(),
local_p1: Point::origin(),
local_p2: Point::origin(),
dist: N::zero(),
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct ContactWithCoulombFrictionBuilder {
infos: [CoulombContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
}
impl ContactWithCoulombFrictionBuilder {
pub fn generate(
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
solver_bodies: &SolverBodies,
out_builders: &mut [ContactWithCoulombFrictionBuilder],
out_constraints: &mut [ContactWithCoulombFriction],
) {
// TODO: could we avoid having to fetch the ids here? It’s the only thing we
// read from the original rigid-bodies.
let ids1: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance <= 0
&& manifold_id[ii] != usize::MAX
{
let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check.
bodies[handle].ids.active_set_offset
} else {
u32::MAX
}];
let ids2: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance >= 0
&& manifold_id[ii] != usize::MAX
{
let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check.
bodies[handle].ids.active_set_offset
} else {
u32::MAX
}];
let vels1 = solver_bodies.gather_vels(ids1);
let poses1 = solver_bodies.gather_poses(ids1);
let vels2 = solver_bodies.gather_vels(ids2);
let poses2 = solver_bodies.gather_poses(ids2);
let world_com1 = Point::from(poses1.pose.translation.vector);
let world_com2 = Point::from(poses2.pose.translation.vector);
// TODO PERF: implement SIMD gather
let force_dir1 = -Vector::<SimdReal>::from(gather![|ii| manifolds[ii].data.normal]);
let num_active_contacts = manifolds[0].data.num_active_contacts();
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear);
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]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
constraint.dir1 = force_dir1;
constraint.im1 = poses1.im;
constraint.im2 = poses2.im;
constraint.solver_vel1 = ids1;
constraint.solver_vel2 = ids2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = num_points as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
for k in 0..num_points {
// SAFETY: we already know that the `manifold_points` has `num_points` elements
// so `k` isn’t out of bounds.
let solver_contact =
unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) };
let is_bouncy = solver_contact.is_bouncy();
let dp1 = solver_contact.point - world_com1;
let dp2 = solver_contact.point - world_com2;
let vel1 = vels1.linear + vels1.angular.gcross(dp1);
let vel2 = vels2.linear + vels2.angular.gcross(dp2);
constraint.limit = solver_contact.friction;
constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8);
// Normal part.
let normal_rhs_wo_bias;
{
let torque_dir1 = dp1.gcross(force_dir1);
let torque_dir2 = dp2.gcross(-force_dir1);
let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1);
let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2);
let imsum = poses1.im + poses2.im;
let projected_mass = utils::simd_inv(
force_dir1.dot(&imsum.component_mul(&force_dir1))
+ ii_torque_dir1.gdot(torque_dir1)
+ ii_torque_dir2.gdot(torque_dir2),
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
normal_rhs_wo_bias =
is_bouncy * solver_contact.restitution * projected_velocity;
constraint.normal_part[k].torque_dir1 = torque_dir1;
constraint.normal_part[k].torque_dir2 = torque_dir2;
constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1;
constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2;
constraint.normal_part[k].impulse = solver_contact.warmstart_impulse;
constraint.normal_part[k].r = projected_mass;
}
// tangent parts.
constraint.tangent_part[k].impulse = solver_contact.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
let torque_dir2 = dp2.gcross(-tangents1[j]);
let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1);
let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2);
let imsum = poses1.im + poses2.im;
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ ii_torque_dir1.gdot(torque_dir1)
+ ii_torque_dir2.gdot(torque_dir2);
let rhs_wo_bias = solver_contact.tangent_velocity.dot(&tangents1[j]);
constraint.tangent_part[k].torque_dir1[j] = torque_dir1;
constraint.tangent_part[k].torque_dir2[j] = torque_dir2;
constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1;
constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2;
constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias;
constraint.tangent_part[k].rhs[j] = rhs_wo_bias;
constraint.tangent_part[k].r[j] = if cfg!(feature = "dim2") {
utils::simd_inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
// TODO PERF: we already applied the inverse inertia to the torque
// dire before. Could we reuse the value instead of retransforming?
constraint.tangent_part[k].r[2] = SimdReal::splat(2.0)
* (constraint.tangent_part[k].ii_torque_dir1[0]
.gdot(constraint.tangent_part[k].torque_dir1[1])
+ constraint.tangent_part[k].ii_torque_dir2[0]
.gdot(constraint.tangent_part[k].torque_dir2[1]));
}
// Builder.
builder.infos[k].local_p1 =
poses1.pose.inverse_transform_point(&solver_contact.point);
builder.infos[k].local_p2 =
poses2.pose.inverse_transform_point(&solver_contact.point);
builder.infos[k].tangent_vel = solver_contact.tangent_velocity;
builder.infos[k].dist = solver_contact.dist;
builder.infos[k].normal_vel = normal_rhs_wo_bias;
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..num_points / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let imsum = poses1.im + poses2.im;
let r0 = constraint.normal_part[k0].r;
let r1 = constraint.normal_part[k1].r;
let mut r_mat = SdpMatrix2::zero();
// TODO PERF: we already applied the inverse inertia to the torque
// dire before. Could we reuse the value instead of retransforming?
r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+ constraint.normal_part[k0]
.ii_torque_dir1
.gdot(constraint.normal_part[k1].torque_dir1)
+ constraint.normal_part[k0]
.ii_torque_dir2
.gdot(constraint.normal_part[k1].torque_dir2);
r_mat.m11 = utils::simd_inv(r0);
r_mat.m22 = utils::simd_inv(r1);
let (inv, det) = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
r_mat.inverse_and_get_determinant_unchecked()
};
let is_invertible = det.simd_gt(SimdReal::zero());
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.normal_part[k0].r_mat_elts = [
inv.m11.select(is_invertible, r0),
inv.m22.select(is_invertible, SimdReal::zero()),
];
constraint.normal_part[k1].r_mat_elts = [
inv.m12.select(is_invertible, SimdReal::zero()),
r_mat.m12.select(is_invertible, SimdReal::zero()),
];
}
}
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &SolverBodies,
_multibodies: &MultibodyJointSet,
constraint: &mut ContactWithCoulombFriction,
) {
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
let poses1 = bodies.gather_poses(constraint.solver_vel1);
let poses2 = bodies.gather_poses(constraint.solver_vel2);
let all_infos = &self.infos[..constraint.num_contacts as usize];
let normal_parts = &mut constraint.normal_part[..constraint.num_contacts as usize];
let tangent_parts = &mut constraint.tangent_part[..constraint.num_contacts as usize];
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
let solved_dt = SimdReal::splat(solved_dt);
for ((info, normal_part), tangent_part) in all_infos
.iter()
.zip(normal_parts.iter_mut())
.zip(tangent_parts.iter_mut())
{
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
let p1 = poses1.pose * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = poses2.pose * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias = info.normal_vel + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
let new_rhs = rhs_wo_bias + rhs_bias;
normal_part.rhs_wo_bias = rhs_wo_bias;
normal_part.rhs = new_rhs;
normal_part.impulse_accumulator += normal_part.impulse;
normal_part.impulse *= warmstart_coeff;
}
// tangent parts.
{
tangent_part.impulse_accumulator += tangent_part.impulse;
tangent_part.impulse *= warmstart_coeff;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
tangent_part.rhs[j] = tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = cfm_factor;
}
}
#[derive(Copy, Clone, Debug)]
#[repr(C)]
pub(crate) struct ContactWithCoulombFriction {
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
pub im1: Vector<SimdReal>,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
#[cfg(feature = "dim3")]
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub normal_part: [ContactConstraintNormalPart<SimdReal>; MAX_MANIFOLD_POINTS],
pub tangent_part: [ContactConstraintTangentPart<SimdReal>; MAX_MANIFOLD_POINTS],
pub solver_vel1: [u32; SIMD_WIDTH],
pub solver_vel2: [u32; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub num_contacts: u8,
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
}
impl ContactWithCoulombFriction {
pub fn warmstart(&mut self, bodies: &mut SolverBodies) {
let mut solver_vel1 = bodies.gather_vels(self.solver_vel1);
let mut solver_vel2 = bodies.gather_vels(self.solver_vel2);
let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize];
/*
* Warmstart restitution.
*/
for normal_part in normal_parts.iter_mut() {
normal_part.warmstart(
&self.dir1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
}
/*
* Warmstart friction.
*/
#[cfg(feature = "dim3")]
let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&self.dir1.orthonormal_vector()];
for tangent_part in tangent_parts.iter_mut() {
tangent_part.warmstart(
tangents1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
}
bodies.scatter_vels(self.solver_vel1, solver_vel1);
bodies.scatter_vels(self.solver_vel2, solver_vel2);
}
pub fn solve(
&mut self,
bodies: &mut SolverBodies,
solve_restitution: bool,
solve_friction: bool,
) {
let mut solver_vel1 = bodies.gather_vels(self.solver_vel1);
let mut solver_vel2 = bodies.gather_vels(self.solver_vel2);
let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize];
/*
* Solve restitution.
*/
if solve_restitution {
if BLOCK_SOLVER_ENABLED {
for normal_part in normal_parts.chunks_exact_mut(2) {
let [normal_part_a, normal_part_b] = normal_part else {
unreachable!()
};
ContactConstraintNormalPart::solve_pair(
normal_part_a,
normal_part_b,
self.cfm_factor,
&self.dir1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
}
// There is one constraint left to solve if there isn’t an even number.
if normal_parts.len() % 2 == 1 {
let normal_part = normal_parts.last_mut().unwrap();
normal_part.solve(
self.cfm_factor,
&self.dir1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
}
} else {
for normal_part in normal_parts.iter_mut() {
normal_part.solve(
self.cfm_factor,
&self.dir1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
}
}
}
/*
* Solve friction.
*/
if solve_friction {
#[cfg(feature = "dim3")]
let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&self.dir1.orthonormal_vector()];
for (tangent_part, normal_part) in tangent_parts.iter_mut().zip(normal_parts.iter()) {
let limit = self.limit * normal_part.impulse;
tangent_part.solve(
tangents1,
&self.im1,
&self.im2,
limit,
&mut solver_vel1,
&mut solver_vel2,
);
}
}
bodies.scatter_vels(self.solver_vel1, solver_vel1);
bodies.scatter_vels(self.solver_vel2, solver_vel2);
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
#[cfg(not(feature = "simd-is-enabled"))]
let warmstart_impulses: [_; SIMD_WIDTH] = [self.normal_part[k].impulse];
#[cfg(feature = "simd-is-enabled")]
let warmstart_impulses: [_; SIMD_WIDTH] = self.normal_part[k].impulse.into();
let warmstart_tangent_impulses = self.tangent_part[k].impulse;
#[cfg(not(feature = "simd-is-enabled"))]
let impulses: [_; SIMD_WIDTH] = [self.normal_part[k].total_impulse()];
#[cfg(feature = "simd-is-enabled")]
let impulses: [_; SIMD_WIDTH] = self.normal_part[k].total_impulse().into();
let tangent_impulses = self.tangent_part[k].total_impulse();
for ii in 0..SIMD_WIDTH {
if self.manifold_id[ii] != usize::MAX {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.warmstart_impulse = warmstart_impulses[ii];
active_contact.data.warmstart_tangent_impulse =
warmstart_tangent_impulses.extract(ii);
active_contact.data.impulse = impulses[ii];
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
}
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = SimdReal::splat(1.0);
for elt in &mut self.normal_part {
elt.rhs = elt.rhs_wo_bias;
}
for elt in &mut self.tangent_part {
elt.rhs = elt.rhs_wo_bias;
}
}
}

515
src/dynamics/solver/contact_constraint/contact_with_twist_friction.rs Normal file
View File

@@ -0,0 +1,515 @@
use super::{
ContactConstraintNormalPart, ContactConstraintTangentPart, ContactConstraintTwistPart,
};
use crate::dynamics::solver::solver_body::SolverBodies;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex, SimdSolverContact};
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH, SimdReal, Vector};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
use num::Zero;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub struct TwistContactPointInfos<N: SimdRealCopy> {
// This is different from the Coulomb version because it doesn’t
// have the `tangent_vel` per-contact here.
pub normal_vel: N,
pub local_p1: Point<N>,
pub local_p2: Point<N>,
pub dist: N,
}
impl<N: SimdRealCopy> Default for TwistContactPointInfos<N> {
fn default() -> Self {
Self {
normal_vel: N::zero(),
local_p1: Point::origin(),
local_p2: Point::origin(),
dist: N::zero(),
}
}
}
/*
* FIXME: this involves a lot of duplicate code wrt. the contact with coulomb friction.
* Find a way to refactor so we can at least share the code for the ristution part.
*/
#[derive(Copy, Clone, Debug)]
pub(crate) struct ContactWithTwistFrictionBuilder {
infos: [TwistContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
local_friction_center1: Point<SimdReal>,
local_friction_center2: Point<SimdReal>,
tangent_vel: Vector<SimdReal>,
}
impl ContactWithTwistFrictionBuilder {
pub fn generate(
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
solver_bodies: &SolverBodies,
out_builders: &mut [ContactWithTwistFrictionBuilder],
out_constraints: &mut [ContactWithTwistFriction],
) {
// TODO: could we avoid having to fetch the ids here? It’s the only thing we
// read from the original rigid-bodies.
let ids1: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance <= 0
&& manifold_id[ii] != usize::MAX
{
let handle = manifolds[ii].data.rigid_body1.unwrap(); // Can unwrap thanks to the dominance check.
bodies[handle].ids.active_set_offset
} else {
u32::MAX
}];
let ids2: [u32; SIMD_WIDTH] = array![|ii| if manifolds[ii].data.relative_dominance >= 0
&& manifold_id[ii] != usize::MAX
{
let handle = manifolds[ii].data.rigid_body2.unwrap(); // Can unwrap thanks to the dominance check.
bodies[handle].ids.active_set_offset
} else {
u32::MAX
}];
let vels1 = solver_bodies.gather_vels(ids1);
let poses1 = solver_bodies.gather_poses(ids1);
let vels2 = solver_bodies.gather_vels(ids2);
let poses2 = solver_bodies.gather_poses(ids2);
let world_com1 = Point::from(poses1.pose.translation.vector);
let world_com2 = Point::from(poses2.pose.translation.vector);
// TODO PERF: implement SIMD gather
let force_dir1 = -Vector::<SimdReal>::from(gather![|ii| manifolds[ii].data.normal]);
let num_active_contacts = manifolds[0].data.num_active_contacts();
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linear, &vels2.linear);
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]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let inv_num_points = SimdReal::splat(1.0 / num_points as Real);
let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
constraint.dir1 = force_dir1;
constraint.im1 = poses1.im;
constraint.im2 = poses2.im;
constraint.solver_vel1 = ids1;
constraint.solver_vel2 = ids2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = num_points as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
let mut friction_center = Point::origin();
let mut twist_warmstart = na::zero();
let mut tangent_warmstart = na::zero();
let mut tangent_vel: Vector<_> = na::zero();
for k in 0..num_points {
// SAFETY: we already know that the `manifold_points` has `num_points` elements
// so `k` isn’t out of bounds.
let solver_contact =
unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) };
let is_bouncy = solver_contact.is_bouncy();
friction_center += solver_contact.point.coords * inv_num_points;
let dp1 = solver_contact.point - world_com1;
let dp2 = solver_contact.point - world_com2;
let vel1 = vels1.linear + vels1.angular.gcross(dp1);
let vel2 = vels2.linear + vels2.angular.gcross(dp2);
twist_warmstart += solver_contact.warmstart_twist_impulse * inv_num_points;
tangent_warmstart += solver_contact.warmstart_tangent_impulse * inv_num_points;
tangent_vel += solver_contact.tangent_velocity * inv_num_points;
constraint.limit = solver_contact.friction;
constraint.manifold_contact_id[k] = solver_contact.contact_id.map(|id| id as u8);
// Normal part.
let normal_rhs_wo_bias;
{
let torque_dir1 = dp1.gcross(force_dir1);
let torque_dir2 = dp2.gcross(-force_dir1);
let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1);
let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2);
let imsum = poses1.im + poses2.im;
let projected_mass = utils::simd_inv(
force_dir1.dot(&imsum.component_mul(&force_dir1))
+ ii_torque_dir1.gdot(torque_dir1)
+ ii_torque_dir2.gdot(torque_dir2),
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
normal_rhs_wo_bias =
is_bouncy * solver_contact.restitution * projected_velocity;
constraint.normal_part[k].torque_dir1 = torque_dir1;
constraint.normal_part[k].torque_dir2 = torque_dir2;
constraint.normal_part[k].ii_torque_dir1 = ii_torque_dir1;
constraint.normal_part[k].ii_torque_dir2 = ii_torque_dir2;
constraint.normal_part[k].impulse = solver_contact.warmstart_impulse;
constraint.normal_part[k].r = projected_mass;
}
// Builder.
builder.infos[k].local_p1 =
poses1.pose.inverse_transform_point(&solver_contact.point);
builder.infos[k].local_p2 =
poses2.pose.inverse_transform_point(&solver_contact.point);
builder.infos[k].dist = solver_contact.dist;
builder.infos[k].normal_vel = normal_rhs_wo_bias;
}
/*
* Tangent/twist part
*/
constraint.tangent_part.impulse = tangent_warmstart;
constraint.twist_part.impulse = twist_warmstart;
builder.local_friction_center1 = poses1.pose.inverse_transform_point(&friction_center);
builder.local_friction_center2 = poses2.pose.inverse_transform_point(&friction_center);
let dp1 = friction_center - world_com1;
let dp2 = friction_center - world_com2;
// Twist part. It has no effect when there is only one point.
if num_points > 1 {
let mut twist_dists = [SimdReal::zero(); MAX_MANIFOLD_POINTS];
for k in 0..num_points {
// FIXME PERF: we don’t want to re-fetch here just to get the solver contact point!
let solver_contact =
unsafe { SimdSolverContact::gather_unchecked(&manifold_points, k) };
twist_dists[k] = nalgebra::distance(&friction_center, &solver_contact.point);
}
let ii_twist_dir1 = poses1.ii.transform_vector(force_dir1);
let ii_twist_dir2 = poses2.ii.transform_vector(-force_dir1);
constraint.twist_part.rhs = SimdReal::zero();
constraint.twist_part.ii_twist_dir1 = ii_twist_dir1;
constraint.twist_part.ii_twist_dir2 = ii_twist_dir2;
constraint.twist_part.r = utils::simd_inv(
ii_twist_dir1.gdot(force_dir1) + ii_twist_dir2.gdot(-force_dir1),
);
constraint.twist_dists = twist_dists;
}
// Tangent part.
for j in 0..2 {
let torque_dir1 = dp1.gcross(tangents1[j]);
let torque_dir2 = dp2.gcross(-tangents1[j]);
let ii_torque_dir1 = poses1.ii.transform_vector(torque_dir1);
let ii_torque_dir2 = poses2.ii.transform_vector(torque_dir2);
let imsum = poses1.im + poses2.im;
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ ii_torque_dir1.gdot(torque_dir1)
+ ii_torque_dir2.gdot(torque_dir2);
// TODO: add something similar to tangent velocity to the twist
// constraint for the case where the different points don’t
// have the same tangent vel?
let rhs_wo_bias = tangent_vel.dot(&tangents1[j]);
constraint.tangent_part.torque_dir1[j] = torque_dir1;
constraint.tangent_part.torque_dir2[j] = torque_dir2;
constraint.tangent_part.ii_torque_dir1[j] = ii_torque_dir1;
constraint.tangent_part.ii_torque_dir2[j] = ii_torque_dir2;
constraint.tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.tangent_part.rhs[j] = rhs_wo_bias;
constraint.tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::simd_inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
// TODO PERF: we already applied the inverse inertia to the torque
// dire before. Could we reuse the value instead of retransforming?
constraint.tangent_part.r[2] = SimdReal::splat(2.0)
* (constraint.tangent_part.ii_torque_dir1[0]
.gdot(constraint.tangent_part.torque_dir1[1])
+ constraint.tangent_part.ii_torque_dir2[0]
.gdot(constraint.tangent_part.torque_dir2[1]));
}
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &SolverBodies,
_multibodies: &MultibodyJointSet,
constraint: &mut ContactWithTwistFriction,
) {
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
let poses1 = bodies.gather_poses(constraint.solver_vel1);
let poses2 = bodies.gather_poses(constraint.solver_vel2);
let all_infos = &self.infos[..constraint.num_contacts as usize];
let normal_parts = &mut constraint.normal_part[..constraint.num_contacts as usize];
let tangent_part = &mut constraint.tangent_part;
let twist_part = &mut constraint.twist_part;
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
let solved_dt = SimdReal::splat(solved_dt);
let tangent_delta = self.tangent_vel * solved_dt;
for (info, normal_part) in all_infos.iter().zip(normal_parts.iter_mut()) {
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
let p1 = poses1.pose * info.local_p1 + tangent_delta;
let p2 = poses2.pose * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias = info.normal_vel + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
let new_rhs = rhs_wo_bias + rhs_bias;
normal_part.rhs_wo_bias = rhs_wo_bias;
normal_part.rhs = new_rhs;
normal_part.impulse_accumulator += normal_part.impulse;
normal_part.impulse *= warmstart_coeff;
}
}
// tangent parts.
{
let p1 = poses1.pose * self.local_friction_center1 + tangent_delta;
let p2 = poses2.pose * self.local_friction_center2;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
tangent_part.rhs[j] = tangent_part.rhs_wo_bias[j] + bias;
}
tangent_part.impulse_accumulator += tangent_part.impulse;
tangent_part.impulse *= warmstart_coeff;
twist_part.impulse_accumulator += twist_part.impulse;
twist_part.impulse *= warmstart_coeff;
}
constraint.cfm_factor = cfm_factor;
}
}
#[derive(Copy, Clone, Debug)]
#[repr(C)]
pub(crate) struct ContactWithTwistFriction {
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
pub im1: Vector<SimdReal>,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
#[cfg(feature = "dim3")]
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub normal_part: [ContactConstraintNormalPart<SimdReal>; MAX_MANIFOLD_POINTS],
// The twist friction model emulates coulomb with only one tangent
// constraint + one twist constraint per manifold.
pub tangent_part: ContactConstraintTangentPart<SimdReal>,
// Twist constraint (angular-only) to compensate the lack of angular resistance on the tangent plane.
pub twist_part: ContactConstraintTwistPart<SimdReal>,
// Distances between the friction center and the contact point.
pub twist_dists: [SimdReal; MAX_MANIFOLD_POINTS],
pub solver_vel1: [u32; SIMD_WIDTH],
pub solver_vel2: [u32; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub num_contacts: u8,
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
}
impl ContactWithTwistFriction {
pub fn warmstart(&mut self, bodies: &mut SolverBodies) {
let mut solver_vel1 = bodies.gather_vels(self.solver_vel1);
let mut solver_vel2 = bodies.gather_vels(self.solver_vel2);
let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
/*
* Warmstart restitution.
*/
for normal_part in normal_parts.iter_mut() {
normal_part.warmstart(
&self.dir1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
}
/*
* Warmstart friction.
*/
#[cfg(feature = "dim3")]
let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&self.dir1.orthonormal_vector()];
self.tangent_part.warmstart(
tangents1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
self.twist_part
.warmstart(&mut solver_vel1, &mut solver_vel2);
bodies.scatter_vels(self.solver_vel1, solver_vel1);
bodies.scatter_vels(self.solver_vel2, solver_vel2);
}
pub fn solve(
&mut self,
bodies: &mut SolverBodies,
solve_restitution: bool,
solve_friction: bool,
) {
let mut solver_vel1 = bodies.gather_vels(self.solver_vel1);
let mut solver_vel2 = bodies.gather_vels(self.solver_vel2);
let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
/*
* Solve restitution.
*/
if solve_restitution {
for normal_part in normal_parts.iter_mut() {
normal_part.solve(
self.cfm_factor,
&self.dir1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
}
}
/*
* Solve friction.
*/
if solve_friction {
#[cfg(feature = "dim3")]
let tangents1 = [&self.tangent1, &self.dir1.cross(&self.tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&self.dir1.orthonormal_vector()];
let mut tangent_limit = SimdReal::zero();
let mut twist_limit = SimdReal::zero();
for (normal_part, dist) in normal_parts.iter().zip(self.twist_dists.iter()) {
tangent_limit += normal_part.impulse;
// The twist limit is computed as the sum of impulses multiplied by the
// lever-arm length relative to the friction center. The rational is that
// the further the point is from the friction center, the stronger angular
// resistance it can offer.
twist_limit += normal_part.impulse * *dist;
}
// Multiply by the friction coefficient.
tangent_limit *= self.limit;
twist_limit *= self.limit;
self.tangent_part.solve(
tangents1,
&self.im1,
&self.im2,
tangent_limit,
&mut solver_vel1,
&mut solver_vel2,
);
// NOTE: if there is only 1 contact, the twist part has no effect.
if self.num_contacts > 1 {
self.twist_part
.solve(&self.dir1, twist_limit, &mut solver_vel1, &mut solver_vel2);
}
}
bodies.scatter_vels(self.solver_vel1, solver_vel1);
bodies.scatter_vels(self.solver_vel2, solver_vel2);
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let warmstart_tangent_impulses = self.tangent_part.impulse;
#[cfg(feature = "simd-is-enabled")]
let warmstart_twist_impulses: [_; SIMD_WIDTH] = self.twist_part.impulse.into();
#[cfg(not(feature = "simd-is-enabled"))]
let warmstart_twist_impulses: Real = self.twist_part.impulse;
for k in 0..self.num_contacts as usize {
#[cfg(not(feature = "simd-is-enabled"))]
let warmstart_impulses: [_; SIMD_WIDTH] = [self.normal_part[k].impulse];
#[cfg(feature = "simd-is-enabled")]
let warmstart_impulses: [_; SIMD_WIDTH] = self.normal_part[k].impulse.into();
#[cfg(not(feature = "simd-is-enabled"))]
let impulses: [_; SIMD_WIDTH] = [self.normal_part[k].total_impulse()];
#[cfg(feature = "simd-is-enabled")]
let impulses: [_; SIMD_WIDTH] = self.normal_part[k].total_impulse().into();
for ii in 0..SIMD_WIDTH {
if self.manifold_id[ii] != usize::MAX {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.warmstart_impulse = warmstart_impulses[ii];
active_contact.data.impulse = impulses[ii];
active_contact.data.warmstart_tangent_impulse =
warmstart_tangent_impulses.extract(ii);
#[cfg(feature = "simd-is-enabled")]
{
active_contact.data.warmstart_twist_impulse = warmstart_twist_impulses[ii];
}
#[cfg(not(feature = "simd-is-enabled"))]
{
active_contact.data.warmstart_twist_impulse = warmstart_twist_impulses;
}
}
}
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = SimdReal::splat(1.0);
for elt in &mut self.normal_part {
elt.rhs = elt.rhs_wo_bias;
}
self.tangent_part.rhs = self.tangent_part.rhs_wo_bias;
}
}

371
src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs → src/dynamics/solver/contact_constraint/generic_contact_constraint.rs
View File

@@ -1,32 +1,33 @@
use crate::dynamics::solver::{GenericRhs, TwoBodyConstraint};
use crate::dynamics::solver::GenericRhs;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Real};
use crate::utils::{SimdAngularInertia, SimdCross, SimdDot};
use super::{TwoBodyConstraintBuilder, TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, SolverVel};
use super::{ContactConstraintNormalPart, ContactConstraintTangentPart};
use crate::dynamics::solver::CoulombContactPointInfos;
use crate::dynamics::solver::solver_body::SolverBodies;
use crate::prelude::RigidBodyHandle;
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use na::DVector;
use parry::math::Vector;
#[derive(Copy, Clone)]
pub(crate) struct GenericTwoBodyConstraintBuilder {
pub(crate) struct GenericContactConstraintBuilder {
infos: [CoulombContactPointInfos<Real>; MAX_MANIFOLD_POINTS],
handle1: RigidBodyHandle,
handle2: RigidBodyHandle,
ccd_thickness: Real,
inner: TwoBodyConstraintBuilder,
}
impl GenericTwoBodyConstraintBuilder {
impl GenericContactConstraintBuilder {
pub fn invalid() -> Self {
Self {
infos: [CoulombContactPointInfos::default(); MAX_MANIFOLD_POINTS],
handle1: RigidBodyHandle::invalid(),
handle2: RigidBodyHandle::invalid(),
ccd_thickness: Real::MAX,
inner: TwoBodyConstraintBuilder::invalid(),
}
}
@@ -35,16 +36,24 @@ impl GenericTwoBodyConstraintBuilder {
manifold: &ContactManifold,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_builders: &mut [GenericTwoBodyConstraintBuilder],
out_constraints: &mut [GenericTwoBodyConstraint],
out_builders: &mut [GenericContactConstraintBuilder],
out_constraints: &mut [GenericContactConstraint],
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
) {
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
// TODO PERF: we haven’t tried to optimized this codepath yet (since it relies
// on multibodies which are already much slower than regular bodies).
let handle1 = manifold
.data
.rigid_body1
.unwrap_or(RigidBodyHandle::invalid());
let handle2 = manifold
.data
.rigid_body2
.unwrap_or(RigidBodyHandle::invalid());
let rb1 = &bodies[handle1];
let rb2 = &bodies[handle2];
let rb1 = &bodies.get(handle1).unwrap_or(&bodies.default_fixed);
let rb2 = &bodies.get(handle2).unwrap_or(&bodies.default_fixed);
let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
@@ -55,20 +64,22 @@ impl GenericTwoBodyConstraintBuilder {
let multibody2 = multibodies
.rigid_body_link(handle2)
.map(|m| (&multibodies[m.multibody], m.id));
let solver_vel1 = multibody1
.map(|mb| mb.0.solver_id)
.unwrap_or(if type1.is_dynamic() {
rb1.ids.active_set_offset
} else {
0
});
let solver_vel2 = multibody2
.map(|mb| mb.0.solver_id)
.unwrap_or(if type2.is_dynamic() {
rb2.ids.active_set_offset
} else {
0
});
let solver_vel1 =
multibody1
.map(|mb| mb.0.solver_id)
.unwrap_or(if type1.is_dynamic_or_kinematic() {
rb1.ids.active_set_offset
} else {
u32::MAX
});
let solver_vel2 =
multibody2
.map(|mb| mb.0.solver_id)
.unwrap_or(if type2.is_dynamic_or_kinematic() {
rb2.ids.active_set_offset
} else {
u32::MAX
});
let force_dir1 = -manifold.data.normal;
#[cfg(feature = "dim2")]
@@ -98,24 +109,24 @@ impl GenericTwoBodyConstraintBuilder {
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
constraint.inner.dir1 = force_dir1;
constraint.inner.im1 = if type1.is_dynamic() {
constraint.dir1 = force_dir1;
constraint.im1 = if type1.is_dynamic_or_kinematic() {
mprops1.effective_inv_mass
} else {
na::zero()
};
constraint.inner.im2 = if type2.is_dynamic() {
constraint.im2 = if type2.is_dynamic_or_kinematic() {
mprops2.effective_inv_mass
} else {
na::zero()
};
constraint.inner.solver_vel1 = solver_vel1;
constraint.inner.solver_vel2 = solver_vel2;
constraint.inner.manifold_id = manifold_id;
constraint.inner.num_contacts = manifold_points.len() as u8;
constraint.solver_vel1 = solver_vel1;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.inner.tangent1 = tangents1[0];
constraint.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
@@ -127,8 +138,8 @@ impl GenericTwoBodyConstraintBuilder {
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.inner.limit = manifold_point.friction;
constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8;
// Normal part.
let normal_rhs_wo_bias;
@@ -136,16 +147,16 @@ impl GenericTwoBodyConstraintBuilder {
let torque_dir1 = dp1.gcross(force_dir1);
let torque_dir2 = dp2.gcross(-force_dir1);
let gcross1 = if type1.is_dynamic() {
let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() {
mprops1
.effective_world_inv_inertia_sqrt
.effective_world_inv_inertia
.transform_vector(torque_dir1)
} else {
na::zero()
};
let gcross2 = if type2.is_dynamic() {
let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() {
mprops2
.effective_world_inv_inertia_sqrt
.effective_world_inv_inertia
.transform_vector(torque_dir2)
} else {
na::zero()
@@ -163,9 +174,9 @@ impl GenericTwoBodyConstraintBuilder {
jacobians,
)
.0
} else if type1.is_dynamic() {
} else if type1.is_dynamic_or_kinematic() {
force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ ii_torque_dir1.gdot(torque_dir1)
} else {
0.0
};
@@ -182,9 +193,9 @@ impl GenericTwoBodyConstraintBuilder {
jacobians,
)
.0
} else if type2.is_dynamic() {
} else if type2.is_dynamic_or_kinematic() {
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2)
+ ii_torque_dir2.gdot(torque_dir2)
} else {
0.0
};
@@ -196,9 +207,11 @@ impl GenericTwoBodyConstraintBuilder {
normal_rhs_wo_bias =
(is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1);
constraint.inner.elements[k].normal_part = TwoBodyConstraintNormalPart {
gcross1,
gcross2,
constraint.normal_part[k] = ContactConstraintNormalPart {
torque_dir1,
torque_dir2,
ii_torque_dir1,
ii_torque_dir2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse_accumulator: na::zero(),
@@ -210,29 +223,30 @@ impl GenericTwoBodyConstraintBuilder {
// Tangent parts.
{
constraint.inner.elements[k].tangent_part.impulse =
manifold_point.warmstart_tangent_impulse;
constraint.tangent_part[k].impulse = manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let torque_dir1 = dp1.gcross(tangents1[j]);
let gcross1 = if type1.is_dynamic() {
let ii_torque_dir1 = if type1.is_dynamic_or_kinematic() {
mprops1
.effective_world_inv_inertia_sqrt
.effective_world_inv_inertia
.transform_vector(torque_dir1)
} else {
na::zero()
};
constraint.inner.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.tangent_part[k].torque_dir1[j] = torque_dir1;
constraint.tangent_part[k].ii_torque_dir1[j] = ii_torque_dir1;
let torque_dir2 = dp2.gcross(-tangents1[j]);
let gcross2 = if type2.is_dynamic() {
let ii_torque_dir2 = if type2.is_dynamic_or_kinematic() {
mprops2
.effective_world_inv_inertia_sqrt
.effective_world_inv_inertia
.transform_vector(torque_dir2)
} else {
na::zero()
};
constraint.inner.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.tangent_part[k].torque_dir2[j] = torque_dir2;
constraint.tangent_part[k].ii_torque_dir2[j] = ii_torque_dir2;
let inv_r1 = if let Some((mb1, link_id1)) = multibody1.as_ref() {
mb1.fill_jacobians(
@@ -246,9 +260,9 @@ impl GenericTwoBodyConstraintBuilder {
jacobians,
)
.0
} else if type1.is_dynamic() {
} else if type1.is_dynamic_or_kinematic() {
force_dir1.dot(&mprops1.effective_inv_mass.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ ii_torque_dir1.gdot(torque_dir1)
} else {
0.0
};
@@ -265,9 +279,9 @@ impl GenericTwoBodyConstraintBuilder {
jacobians,
)
.0
} else if type2.is_dynamic() {
} else if type2.is_dynamic_or_kinematic() {
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2)
+ ii_torque_dir2.gdot(torque_dir2)
} else {
0.0
};
@@ -275,18 +289,18 @@ impl GenericTwoBodyConstraintBuilder {
let r = crate::utils::inv(inv_r1 + inv_r2);
let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]);
constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.tangent_part[k].rhs_wo_bias[j] = rhs_wo_bias;
constraint.tangent_part[k].rhs[j] = rhs_wo_bias;
// TODO: in 3D, we should take into account gcross[0].dot(gcross[1])
// in lhs. See the corresponding code on the `velocity_constraint.rs`
// file.
constraint.inner.elements[k].tangent_part.r[j] = r;
constraint.tangent_part[k].r[j] = r;
}
}
// Builder.
let infos = ContactPointInfos {
let infos = CoulombContactPointInfos {
local_p1: rb1
.pos
.position
@@ -297,14 +311,14 @@ impl GenericTwoBodyConstraintBuilder {
.inverse_transform_point(&manifold_point.point),
tangent_vel: manifold_point.tangent_velocity,
dist: manifold_point.dist,
normal_rhs_wo_bias,
normal_vel: normal_rhs_wo_bias,
};
builder.handle1 = handle1;
builder.handle2 = handle2;
builder.ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
builder.inner.infos[k] = infos;
constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
builder.infos[k] = infos;
constraint.manifold_contact_id[k] = manifold_point.contact_id[0] as u8;
}
let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0);
@@ -314,8 +328,8 @@ impl GenericTwoBodyConstraintBuilder {
// reduce all ops to nothing because its ndofs will be zero.
let generic_constraint_mask = (multibody1.is_some() as u8)
| ((multibody2.is_some() as u8) << 1)
| (!type1.is_dynamic() as u8)
| ((!type2.is_dynamic() as u8) << 1);
| (!type1.is_dynamic_or_kinematic() as u8)
| ((!type2.is_dynamic_or_kinematic() as u8) << 1);
constraint.j_id = chunk_j_id;
constraint.ndofs1 = ndofs1;
@@ -328,74 +342,160 @@ impl GenericTwoBodyConstraintBuilder {
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
bodies: &SolverBodies,
multibodies: &MultibodyJointSet,
constraint: &mut GenericTwoBodyConstraint,
constraint: &mut GenericContactConstraint,
) {
// We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
let pos1 = multibodies
.rigid_body_link(self.handle1)
.map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
.unwrap_or_else(|| &bodies[constraint.inner.solver_vel1].position);
let pos2 = multibodies
.rigid_body_link(self.handle2)
.map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
.unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position);
let cfm_factor = params.contact_cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.contact_erp_inv_dt();
self.inner
.update_with_positions(params, solved_dt, pos1, pos2, &mut constraint.inner);
// We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
let pose1 = multibodies
.rigid_body_link(self.handle1)
.map(|m| multibodies[m.multibody].link(m.id).unwrap().local_to_world)
.unwrap_or_else(|| bodies.get_pose(constraint.solver_vel1).pose);
let pose2 = multibodies
.rigid_body_link(self.handle2)
.map(|m| multibodies[m.multibody].link(m.id).unwrap().local_to_world)
.unwrap_or_else(|| bodies.get_pose(constraint.solver_vel2).pose);
let all_infos = &self.infos[..constraint.num_contacts as usize];
let normal_parts = &mut constraint.normal_part[..constraint.num_contacts as usize];
let tangent_parts = &mut constraint.tangent_part[..constraint.num_contacts as usize];
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
for ((info, normal_part), tangent_part) in all_infos
.iter()
.zip(normal_parts.iter_mut())
.zip(tangent_parts.iter_mut())
{
// Tangent velocity is equivalent to the first body’s surface moving artificially.
let p1 = pose1 * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = pose2 * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias = info.normal_vel + dist.max(0.0) * inv_dt;
let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
.clamp(-params.max_corrective_velocity(), 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
normal_part.rhs_wo_bias = rhs_wo_bias;
normal_part.rhs = new_rhs;
normal_part.impulse_accumulator += normal_part.impulse;
normal_part.impulse *= params.warmstart_coefficient;
}
// Tangent part.
{
tangent_part.impulse_accumulator += tangent_part.impulse;
tangent_part.impulse *= params.warmstart_coefficient;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
tangent_part.rhs[j] = tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = cfm_factor;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericTwoBodyConstraint {
// We just build the generic constraint on top of the velocity constraint,
// adding some information we can use in the generic case.
pub inner: TwoBodyConstraint,
pub(crate) struct GenericContactConstraint {
/*
* Fields specific to multibodies.
*/
pub j_id: usize,
pub ndofs1: usize,
pub ndofs2: usize,
pub generic_constraint_mask: u8,
/*
* Fields similar to the rigid-body constraints.
*/
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im1: Vector<Real>,
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub solver_vel1: u32,
pub solver_vel2: u32,
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub normal_part: [ContactConstraintNormalPart<Real>; MAX_MANIFOLD_POINTS],
pub tangent_part: [ContactConstraintTangentPart<Real>; MAX_MANIFOLD_POINTS],
}
impl GenericTwoBodyConstraint {
impl GenericContactConstraint {
pub fn invalid() -> Self {
Self {
inner: TwoBodyConstraint::invalid(),
j_id: usize::MAX,
ndofs1: usize::MAX,
ndofs2: usize::MAX,
generic_constraint_mask: u8::MAX,
dir1: Vector::zeros(),
#[cfg(feature = "dim3")]
tangent1: Vector::zeros(),
im1: Vector::zeros(),
im2: Vector::zeros(),
cfm_factor: 0.0,
limit: 0.0,
solver_vel1: u32::MAX,
solver_vel2: u32::MAX,
manifold_id: ContactManifoldIndex::MAX,
manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS],
num_contacts: u8::MAX,
normal_part: [ContactConstraintNormalPart::zero(); MAX_MANIFOLD_POINTS],
tangent_part: [ContactConstraintTangentPart::zero(); MAX_MANIFOLD_POINTS],
}
}
pub fn warmstart(
&mut self,
jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
bodies: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
let mut solver_vel1 = if self.solver_vel1 == u32::MAX {
GenericRhs::Fixed
} else if self.generic_constraint_mask & 0b01 == 0 {
GenericRhs::SolverVel(bodies.vels[self.solver_vel1 as usize])
} else {
GenericRhs::GenericId(self.inner.solver_vel1)
GenericRhs::GenericId(self.solver_vel1)
};
let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
let mut solver_vel2 = if self.solver_vel2 == u32::MAX {
GenericRhs::Fixed
} else if self.generic_constraint_mask & 0b10 == 0 {
GenericRhs::SolverVel(bodies.vels[self.solver_vel2 as usize])
} else {
GenericRhs::GenericId(self.inner.solver_vel2)
GenericRhs::GenericId(self.solver_vel2)
};
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
TwoBodyConstraintElement::generic_warmstart_group(
elements,
let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize];
let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
Self::generic_warmstart_group(
normal_parts,
tangent_parts,
jacobians,
&self.inner.dir1,
&self.dir1,
#[cfg(feature = "dim3")]
&self.inner.tangent1,
&self.inner.im1,
&self.inner.im2,
&self.tangent1,
&self.im1,
&self.im2,
self.ndofs1,
self.ndofs2,
self.j_id,
@@ -405,45 +505,51 @@ impl GenericTwoBodyConstraint {
);
if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
solver_vels[self.inner.solver_vel1] = solver_vel1;
bodies.vels[self.solver_vel1 as usize] = solver_vel1;
}
if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
solver_vels[self.inner.solver_vel2] = solver_vel2;
bodies.vels[self.solver_vel2 as usize] = solver_vel2;
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
bodies: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let mut solver_vel1 = if self.generic_constraint_mask & 0b01 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel1])
let mut solver_vel1 = if self.solver_vel1 == u32::MAX {
GenericRhs::Fixed
} else if self.generic_constraint_mask & 0b01 == 0 {
GenericRhs::SolverVel(bodies.vels[self.solver_vel1 as usize])
} else {
GenericRhs::GenericId(self.inner.solver_vel1)
GenericRhs::GenericId(self.solver_vel1)
};
let mut solver_vel2 = if self.generic_constraint_mask & 0b10 == 0 {
GenericRhs::SolverVel(solver_vels[self.inner.solver_vel2])
let mut solver_vel2 = if self.solver_vel2 == u32::MAX {
GenericRhs::Fixed
} else if self.generic_constraint_mask & 0b10 == 0 {
GenericRhs::SolverVel(bodies.vels[self.solver_vel2 as usize])
} else {
GenericRhs::GenericId(self.inner.solver_vel2)
GenericRhs::GenericId(self.solver_vel2)
};
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
TwoBodyConstraintElement::generic_solve_group(
self.inner.cfm_factor,
elements,
let normal_parts = &mut self.normal_part[..self.num_contacts as usize];
let tangent_parts = &mut self.tangent_part[..self.num_contacts as usize];
Self::generic_solve_group(
self.cfm_factor,
normal_parts,
tangent_parts,
jacobians,
&self.inner.dir1,
&self.dir1,
#[cfg(feature = "dim3")]
&self.inner.tangent1,
&self.inner.im1,
&self.inner.im2,
self.inner.limit,
&self.tangent1,
&self.im1,
&self.im2,
self.limit,
self.ndofs1,
self.ndofs2,
self.j_id,
@@ -455,19 +561,34 @@ impl GenericTwoBodyConstraint {
);
if let GenericRhs::SolverVel(solver_vel1) = solver_vel1 {
solver_vels[self.inner.solver_vel1] = solver_vel1;
bodies.vels[self.solver_vel1 as usize] = solver_vel1;
}
if let GenericRhs::SolverVel(solver_vel2) = solver_vel2 {
solver_vels[self.inner.solver_vel2] = solver_vel2;
bodies.vels[self.solver_vel2 as usize] = solver_vel2;
}
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
self.inner.writeback_impulses(manifolds_all);
let manifold = &mut manifolds_all[self.manifold_id];
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.warmstart_impulse = self.normal_part[k].impulse;
active_contact.data.warmstart_tangent_impulse = self.tangent_part[k].impulse;
active_contact.data.impulse = self.normal_part[k].total_impulse();
active_contact.data.tangent_impulse = self.tangent_part[k].total_impulse();
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.inner.remove_cfm_and_bias_from_rhs();
self.cfm_factor = 1.0;
for normal_part in &mut self.normal_part {
normal_part.rhs = normal_part.rhs_wo_bias;
}
for tangent_part in &mut self.tangent_part {
tangent_part.rhs = tangent_part.rhs_wo_bias;
}
}
}

126
src/dynamics/solver/contact_constraint/generic_two_body_constraint_element.rs → src/dynamics/solver/contact_constraint/generic_contact_constraint_element.rs
View File

@@ -1,7 +1,6 @@
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::{
TwoBodyConstraintElement, TwoBodyConstraintNormalPart, TwoBodyConstraintTangentPart,
};
use crate::dynamics::solver::contact_constraint::GenericContactConstraint;
use crate::dynamics::solver::{ContactConstraintNormalPart, ContactConstraintTangentPart};
use crate::math::{AngVector, DIM, Real, Vector};
use crate::utils::SimdDot;
use na::DVector;
@@ -10,37 +9,38 @@ use {crate::utils::SimdBasis, na::SimdPartialOrd};
pub(crate) enum GenericRhs {
SolverVel(SolverVel<Real>),
GenericId(usize),
GenericId(u32),
Fixed,
}
// Offset between the jacobians of two consecutive constraints.
#[inline(always)]
#[inline]
fn j_step(ndofs1: usize, ndofs2: usize) -> usize {
(ndofs1 + ndofs2) * 2
}
#[inline(always)]
#[inline]
fn j_id1(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize {
j_id
}
#[inline(always)]
#[inline]
fn j_id2(j_id: usize, ndofs1: usize, _ndofs2: usize) -> usize {
j_id + ndofs1 * 2
}
#[inline(always)]
#[inline]
fn normal_j_id(j_id: usize, _ndofs1: usize, _ndofs2: usize) -> usize {
j_id
}
#[inline(always)]
#[inline]
fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize {
j_id + (ndofs1 + ndofs2) * 2
}
impl GenericRhs {
#[inline(always)]
#[inline]
fn dvel(
&self,
j_id: usize,
@@ -54,13 +54,14 @@ impl GenericRhs {
GenericRhs::SolverVel(rhs) => dir.dot(&rhs.linear) + gcross.gdot(rhs.angular),
GenericRhs::GenericId(solver_vel) => {
let j = jacobians.rows(j_id, ndofs);
let rhs = solver_vels.rows(*solver_vel, ndofs);
let rhs = solver_vels.rows(*solver_vel as usize, ndofs);
j.dot(&rhs)
}
GenericRhs::Fixed => 0.0,
}
}
#[inline(always)]
#[inline]
fn apply_impulse(
&mut self,
j_id: usize,
@@ -68,26 +69,27 @@ impl GenericRhs {
impulse: Real,
jacobians: &DVector<Real>,
dir: &Vector<Real>,
gcross: &AngVector<Real>,
ii_torque_dir: &AngVector<Real>,
solver_vels: &mut DVector<Real>,
inv_mass: &Vector<Real>,
) {
match self {
GenericRhs::SolverVel(rhs) => {
rhs.linear += dir.component_mul(inv_mass) * impulse;
rhs.angular += gcross * impulse;
rhs.angular += ii_torque_dir * impulse;
}
GenericRhs::GenericId(solver_vel) => {
let wj_id = j_id + ndofs;
let wj = jacobians.rows(wj_id, ndofs);
let mut rhs = solver_vels.rows_mut(*solver_vel, ndofs);
let mut rhs = solver_vels.rows_mut(*solver_vel as usize, ndofs);
rhs.axpy(impulse, &wj, 1.0);
}
GenericRhs::Fixed => {}
}
}
}
impl TwoBodyConstraintTangentPart<Real> {
impl ContactConstraintTangentPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
@@ -115,7 +117,7 @@ impl TwoBodyConstraintTangentPart<Real> {
self.impulse[0],
jacobians,
tangents1[0],
&self.gcross1[0],
&self.ii_torque_dir1[0],
solver_vels,
im1,
);
@@ -125,7 +127,7 @@ impl TwoBodyConstraintTangentPart<Real> {
self.impulse[0],
jacobians,
&-tangents1[0],
&self.gcross2[0],
&self.ii_torque_dir2[0],
solver_vels,
im2,
);
@@ -139,7 +141,7 @@ impl TwoBodyConstraintTangentPart<Real> {
self.impulse[0],
jacobians,
tangents1[0],
&self.gcross1[0],
&self.ii_torque_dir1[0],
solver_vels,
im1,
);
@@ -149,7 +151,7 @@ impl TwoBodyConstraintTangentPart<Real> {
self.impulse[1],
jacobians,
tangents1[1],
&self.gcross1[1],
&self.ii_torque_dir1[1],
solver_vels,
im1,
);
@@ -160,7 +162,7 @@ impl TwoBodyConstraintTangentPart<Real> {
self.impulse[0],
jacobians,
&-tangents1[0],
&self.gcross2[0],
&self.ii_torque_dir2[0],
solver_vels,
im2,
);
@@ -170,7 +172,7 @@ impl TwoBodyConstraintTangentPart<Real> {
self.impulse[1],
jacobians,
&-tangents1[1],
&self.gcross2[1],
&self.ii_torque_dir2[1],
solver_vels,
im2,
);
@@ -204,14 +206,14 @@ impl TwoBodyConstraintTangentPart<Real> {
ndofs1,
jacobians,
tangents1[0],
&self.gcross1[0],
&self.torque_dir1[0],
solver_vels,
) + solver_vel2.dvel(
j_id2,
ndofs2,
jacobians,
&-tangents1[0],
&self.gcross2[0],
&self.torque_dir2[0],
solver_vels,
) + self.rhs[0];
@@ -225,7 +227,7 @@ impl TwoBodyConstraintTangentPart<Real> {
dlambda,
jacobians,
tangents1[0],
&self.gcross1[0],
&self.ii_torque_dir1[0],
solver_vels,
im1,
);
@@ -235,7 +237,7 @@ impl TwoBodyConstraintTangentPart<Real> {
dlambda,
jacobians,
&-tangents1[0],
&self.gcross2[0],
&self.ii_torque_dir2[0],
solver_vels,
im2,
);
@@ -248,14 +250,14 @@ impl TwoBodyConstraintTangentPart<Real> {
ndofs1,
jacobians,
tangents1[0],
&self.gcross1[0],
&self.torque_dir1[0],
solver_vels,
) + solver_vel2.dvel(
j_id2,
ndofs2,
jacobians,
&-tangents1[0],
&self.gcross2[0],
&self.torque_dir2[0],
solver_vels,
) + self.rhs[0];
let dvel_1 = solver_vel1.dvel(
@@ -263,14 +265,14 @@ impl TwoBodyConstraintTangentPart<Real> {
ndofs1,
jacobians,
tangents1[1],
&self.gcross1[1],
&self.torque_dir1[1],
solver_vels,
) + solver_vel2.dvel(
j_id2 + j_step,
ndofs2,
jacobians,
&-tangents1[1],
&self.gcross2[1],
&self.torque_dir2[1],
solver_vels,
) + self.rhs[1];
@@ -289,7 +291,7 @@ impl TwoBodyConstraintTangentPart<Real> {
dlambda[0],
jacobians,
tangents1[0],
&self.gcross1[0],
&self.ii_torque_dir1[0],
solver_vels,
im1,
);
@@ -299,7 +301,7 @@ impl TwoBodyConstraintTangentPart<Real> {
dlambda[1],
jacobians,
tangents1[1],
&self.gcross1[1],
&self.ii_torque_dir1[1],
solver_vels,
im1,
);
@@ -310,7 +312,7 @@ impl TwoBodyConstraintTangentPart<Real> {
dlambda[0],
jacobians,
&-tangents1[0],
&self.gcross2[0],
&self.ii_torque_dir2[0],
solver_vels,
im2,
);
@@ -320,7 +322,7 @@ impl TwoBodyConstraintTangentPart<Real> {
dlambda[1],
jacobians,
&-tangents1[1],
&self.gcross2[1],
&self.ii_torque_dir2[1],
solver_vels,
im2,
);
@@ -328,7 +330,7 @@ impl TwoBodyConstraintTangentPart<Real> {
}
}
impl TwoBodyConstraintNormalPart<Real> {
impl ContactConstraintNormalPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
@@ -352,7 +354,7 @@ impl TwoBodyConstraintNormalPart<Real> {
self.impulse,
jacobians,
dir1,
&self.gcross1,
&self.ii_torque_dir1,
solver_vels,
im1,
);
@@ -362,7 +364,7 @@ impl TwoBodyConstraintNormalPart<Real> {
self.impulse,
jacobians,
&-dir1,
&self.gcross2,
&self.ii_torque_dir2,
solver_vels,
im2,
);
@@ -386,9 +388,21 @@ impl TwoBodyConstraintNormalPart<Real> {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
let dvel = solver_vel1.dvel(j_id1, ndofs1, jacobians, dir1, &self.gcross1, solver_vels)
+ solver_vel2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, solver_vels)
+ self.rhs;
let dvel = solver_vel1.dvel(
j_id1,
ndofs1,
jacobians,
dir1,
&self.torque_dir1,
solver_vels,
) + solver_vel2.dvel(
j_id2,
ndofs2,
jacobians,
&-dir1,
&self.torque_dir2,
solver_vels,
) + self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
@@ -400,7 +414,7 @@ impl TwoBodyConstraintNormalPart<Real> {
dlambda,
jacobians,
dir1,
&self.gcross1,
&self.ii_torque_dir1,
solver_vels,
im1,
);
@@ -410,17 +424,18 @@ impl TwoBodyConstraintNormalPart<Real> {
dlambda,
jacobians,
&-dir1,
&self.gcross2,
&self.ii_torque_dir2,
solver_vels,
im2,
);
}
}
impl TwoBodyConstraintElement<Real> {
impl GenericContactConstraint {
#[inline]
pub fn generic_warmstart_group(
elements: &mut [Self],
normal_parts: &mut [ContactConstraintNormalPart<Real>],
tangent_parts: &mut [ContactConstraintTangentPart<Real>],
jacobians: &DVector<Real>,
dir1: &Vector<Real>,
#[cfg(feature = "dim3")] tangent1: &Vector<Real>,
@@ -442,8 +457,8 @@ impl TwoBodyConstraintElement<Real> {
{
let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
for element in elements.iter_mut() {
element.normal_part.generic_warmstart(
for normal_part in normal_parts {
normal_part.generic_warmstart(
nrm_j_id,
jacobians,
dir1,
@@ -467,9 +482,8 @@ impl TwoBodyConstraintElement<Real> {
let tangents1 = [&dir1.orthonormal_vector()];
let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
for element in elements.iter_mut() {
let part = &mut element.tangent_part;
part.generic_warmstart(
for tangent_part in tangent_parts {
tangent_part.generic_warmstart(
tng_j_id,
jacobians,
tangents1,
@@ -489,7 +503,8 @@ impl TwoBodyConstraintElement<Real> {
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,
elements: &mut [Self],
normal_parts: &mut [ContactConstraintNormalPart<Real>],
tangent_parts: &mut [ContactConstraintTangentPart<Real>],
jacobians: &DVector<Real>,
dir1: &Vector<Real>,
#[cfg(feature = "dim3")] tangent1: &Vector<Real>,
@@ -514,8 +529,8 @@ impl TwoBodyConstraintElement<Real> {
if solve_restitution {
let mut nrm_j_id = normal_j_id(j_id, ndofs1, ndofs2);
for element in elements.iter_mut() {
element.normal_part.generic_solve(
for normal_part in &mut *normal_parts {
normal_part.generic_solve(
cfm_factor,
nrm_j_id,
jacobians,
@@ -540,10 +555,9 @@ impl TwoBodyConstraintElement<Real> {
let tangents1 = [&dir1.orthonormal_vector()];
let mut tng_j_id = tangent_j_id(j_id, ndofs1, ndofs2);
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.generic_solve(
for (normal_part, tangent_part) in normal_parts.iter().zip(tangent_parts.iter_mut()) {
let limit = limit * normal_part.impulse;
tangent_part.generic_solve(
tng_j_id,
jacobians,
tangents1,

307
src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
View File

@@ -1,307 +0,0 @@
use crate::dynamics::solver::OneBodyConstraint;
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, MultibodyLinkId, RigidBodySet, RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real};
use crate::utils::SimdCross;
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, OneBodyConstraintBuilder};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use na::DVector;
#[derive(Copy, Clone)]
pub(crate) struct GenericOneBodyConstraintBuilder {
link2: MultibodyLinkId,
ccd_thickness: Real,
inner: OneBodyConstraintBuilder,
}
impl GenericOneBodyConstraintBuilder {
pub fn invalid() -> Self {
Self {
link2: MultibodyLinkId::default(),
ccd_thickness: 0.0,
inner: OneBodyConstraintBuilder::invalid(),
}
}
pub fn generate(
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_builders: &mut [GenericOneBodyConstraintBuilder],
out_constraints: &mut [GenericOneBodyConstraint],
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
) {
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 handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let rb1 = handle1
.map(|h| SolverBody::from(&bodies[h]))
.unwrap_or_default();
let rb2 = &bodies[handle2.unwrap()];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let link2 = *multibodies.rigid_body_link(handle2.unwrap()).unwrap();
let (mb2, link_id2) = (&multibodies[link2.multibody], link2.id);
let solver_vel2 = mb2.solver_id;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let multibodies_ndof = mb2.ndofs();
// For each solver contact we generate DIM constraints, and each constraints appends
// the multibodies jacobian and weighted jacobians
let required_jacobian_len =
*jacobian_id + manifold.data.solver_contacts.len() * multibodies_ndof * 2 * DIM;
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
for (l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let chunk_j_id = *jacobian_id;
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
builder.inner.rb1 = rb1;
builder.inner.vels1 = vels1;
constraint.inner.dir1 = force_dir1;
constraint.inner.im2 = mprops2.effective_inv_mass;
constraint.inner.solver_vel2 = solver_vel2;
constraint.inner.manifold_id = manifold_id;
constraint.inner.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.inner.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let point = manifold_point.point;
let dp1 = point - world_com1;
let dp2 = point - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.inner.limit = manifold_point.friction;
constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
let normal_rhs_wo_bias;
{
let torque_dir2 = dp2.gcross(-force_dir1);
let inv_r2 = mb2
.fill_jacobians(
link_id2,
-force_dir1,
#[cfg(feature = "dim2")]
na::vector!(torque_dir2),
#[cfg(feature = "dim3")]
torque_dir2,
jacobian_id,
jacobians,
)
.0;
let r = crate::utils::inv(inv_r2);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let proj_vel1 = vel1.dot(&force_dir1);
let proj_vel2 = vel2.dot(&force_dir1);
let dvel = proj_vel1 - proj_vel2;
// NOTE: we add proj_vel1 since it’s not accessible through solver_vel.
normal_rhs_wo_bias =
proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel;
constraint.inner.elements[k].normal_part = OneBodyConstraintNormalPart {
gcross2: na::zero(), // Unused for generic constraints.
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
impulse_accumulator: na::zero(),
r,
r_mat_elts: [0.0; 2],
};
}
// Tangent parts.
{
constraint.inner.elements[k].tangent_part.impulse = na::zero();
for j in 0..DIM - 1 {
let torque_dir2 = dp2.gcross(-tangents1[j]);
let inv_r2 = mb2
.fill_jacobians(
link_id2,
-tangents1[j],
#[cfg(feature = "dim2")]
na::vector![torque_dir2],
#[cfg(feature = "dim3")]
torque_dir2,
jacobian_id,
jacobians,
)
.0;
let r = crate::utils::inv(inv_r2);
let rhs_wo_bias = (vel1
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.inner.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.inner.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
// FIXME: in 3D, we should take into account gcross[0].dot(gcross[1])
// in lhs. See the corresponding code on the `velocity_constraint.rs`
// file.
constraint.inner.elements[k].tangent_part.r[j] = r;
}
}
// Builder.
let infos = ContactPointInfos {
local_p1: rb1.position.inverse_transform_point(&manifold_point.point),
local_p2: rb2
.pos
.position
.inverse_transform_point(&manifold_point.point),
tangent_vel: manifold_point.tangent_velocity,
dist: manifold_point.dist,
normal_rhs_wo_bias,
};
builder.link2 = link2;
builder.ccd_thickness = rb2.ccd.ccd_thickness;
builder.inner.infos[k] = infos;
constraint.inner.manifold_contact_id[k] = manifold_point.contact_id;
}
constraint.j_id = chunk_j_id;
constraint.ndofs2 = mb2.ndofs();
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
_solver_bodies: &[SolverBody],
multibodies: &MultibodyJointSet,
constraint: &mut GenericOneBodyConstraint,
) {
// We don’t update jacobians so the update is mostly identical to the non-generic velocity constraint.
let pos2 = &multibodies[self.link2.multibody]
.link(self.link2.id)
.unwrap()
.local_to_world;
self.inner
.update_with_positions(params, solved_dt, pos2, &mut constraint.inner);
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericOneBodyConstraint {
// We just build the generic constraint on top of the velocity constraint,
// adding some information we can use in the generic case.
pub inner: OneBodyConstraint,
pub j_id: usize,
pub ndofs2: usize,
}
impl GenericOneBodyConstraint {
pub fn invalid() -> Self {
Self {
inner: OneBodyConstraint::invalid(),
j_id: usize::MAX,
ndofs2: usize::MAX,
}
}
pub fn warmstart(
&mut self,
jacobians: &DVector<Real>,
generic_solver_vels: &mut DVector<Real>,
) {
let solver_vel2 = self.inner.solver_vel2;
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
OneBodyConstraintElement::generic_warmstart_group(
elements,
jacobians,
self.ndofs2,
self.j_id,
solver_vel2,
generic_solver_vels,
);
}
#[profiling::function]
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
generic_solver_vels: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let solver_vel2 = self.inner.solver_vel2;
let elements = &mut self.inner.elements[..self.inner.num_contacts as usize];
OneBodyConstraintElement::generic_solve_group(
self.inner.cfm_factor,
elements,
jacobians,
self.inner.limit,
self.ndofs2,
self.j_id,
solver_vel2,
generic_solver_vels,
solve_restitution,
solve_friction,
);
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
self.inner.writeback_impulses(manifolds_all);
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.inner.remove_cfm_and_bias_from_rhs();
}
}

233
src/dynamics/solver/contact_constraint/generic_one_body_constraint_element.rs
View File

@@ -1,233 +0,0 @@
use crate::dynamics::solver::{
OneBodyConstraintElement, OneBodyConstraintNormalPart, OneBodyConstraintTangentPart,
};
use crate::math::{DIM, Real};
use na::DVector;
#[cfg(feature = "dim2")]
use na::SimdPartialOrd;
impl OneBodyConstraintTangentPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
#[cfg(feature = "dim2")]
{
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
#[cfg(feature = "dim3")]
{
let j_step = ndofs2 * 2;
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse[1],
&jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
1.0,
);
}
}
#[inline]
pub fn generic_solve(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
limit: Real,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
#[cfg(feature = "dim2")]
{
let dvel_0 = jacobians
.rows(j_id2, ndofs2)
.dot(&solver_vels.rows(solver_vel2, ndofs2))
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel_0).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
dlambda,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
#[cfg(feature = "dim3")]
{
let j_step = ndofs2 * 2;
let dvel_0 = jacobians
.rows(j_id2, ndofs2)
.dot(&solver_vels.rows(solver_vel2, ndofs2))
+ self.rhs[0];
let dvel_1 = jacobians
.rows(j_id2 + j_step, ndofs2)
.dot(&solver_vels.rows(solver_vel2, ndofs2))
+ self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dvel_0,
self.impulse[1] - self.r[1] * dvel_1,
);
let new_impulse = new_impulse.cap_magnitude(limit);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
dlambda[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
dlambda[1],
&jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
1.0,
);
}
}
}
impl OneBodyConstraintNormalPart<Real> {
#[inline]
pub fn generic_warmstart(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
self.impulse,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
#[inline]
pub fn generic_solve(
&mut self,
cfm_factor: Real,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
let dvel = jacobians
.rows(j_id2, ndofs2)
.dot(&solver_vels.rows(solver_vel2, ndofs2))
+ self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vels.rows_mut(solver_vel2, ndofs2).axpy(
dlambda,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
}
impl OneBodyConstraintElement<Real> {
#[inline]
pub fn generic_warmstart_group(
elements: &mut [Self],
jacobians: &DVector<Real>,
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
) {
let j_step = ndofs2 * 2 * DIM;
// Solve penetration.
let mut nrm_j_id = j_id;
for element in elements.iter_mut() {
element.normal_part.generic_warmstart(
nrm_j_id,
jacobians,
ndofs2,
solver_vel2,
solver_vels,
);
nrm_j_id += j_step;
}
// Solve friction.
let mut tng_j_id = j_id + ndofs2 * 2;
for element in elements.iter_mut() {
let part = &mut element.tangent_part;
part.generic_warmstart(tng_j_id, jacobians, ndofs2, solver_vel2, solver_vels);
tng_j_id += j_step;
}
}
#[inline]
pub fn generic_solve_group(
cfm_factor: Real,
elements: &mut [Self],
jacobians: &DVector<Real>,
limit: Real,
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
solver_vel2: usize,
solver_vels: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let j_step = ndofs2 * 2 * DIM;
// Solve penetration.
if solve_restitution {
let mut nrm_j_id = j_id;
for element in elements.iter_mut() {
element.normal_part.generic_solve(
cfm_factor,
nrm_j_id,
jacobians,
ndofs2,
solver_vel2,
solver_vels,
);
nrm_j_id += j_step;
}
}
// Solve friction.
if solve_friction {
let mut tng_j_id = j_id + ndofs2 * 2;
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.generic_solve(tng_j_id, jacobians, ndofs2, limit, solver_vel2, solver_vels);
tng_j_id += j_step;
}
}
}
}

86
src/dynamics/solver/contact_constraint/mod.rs
View File

@@ -1,29 +1,61 @@
pub(crate) use generic_one_body_constraint::*;
// pub(crate) use generic_one_body_constraint_element::*;
pub(crate) use contact_constraints_set::{
ConstraintsCounts, ContactConstraintTypes, ContactConstraintsSet,
};
pub(crate) use generic_two_body_constraint::*;
pub(crate) use generic_two_body_constraint_element::*;
pub(crate) use one_body_constraint::*;
pub(crate) use one_body_constraint_element::*;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use one_body_constraint_simd::*;
pub(crate) use two_body_constraint::*;
pub(crate) use two_body_constraint_element::*;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use two_body_constraint_simd::*;
pub(crate) use contact_constraint_element::*;
pub(crate) use contact_constraints_set::{ConstraintsCounts, ContactConstraintsSet};
pub(crate) use contact_with_coulomb_friction::*;
pub(crate) use generic_contact_constraint::*;
pub(crate) use generic_contact_constraint_element::*;
#[cfg(feature = "dim3")]
pub(crate) use contact_with_twist_friction::*;
mod contact_constraint_element;
mod contact_constraints_set;
mod generic_one_body_constraint;
mod generic_one_body_constraint_element;
mod generic_two_body_constraint;
mod generic_two_body_constraint_element;
mod one_body_constraint;
mod one_body_constraint_element;
#[cfg(feature = "simd-is-enabled")]
mod one_body_constraint_simd;
mod two_body_constraint;
mod two_body_constraint_element;
#[cfg(feature = "simd-is-enabled")]
mod two_body_constraint_simd;
mod contact_with_coulomb_friction;
mod generic_contact_constraint;
mod generic_contact_constraint_element;
mod any_contact_constraint;
#[cfg(feature = "dim3")]
mod contact_with_twist_friction;
#[cfg(feature = "dim3")]
use crate::{
math::{DIM, Real, Vector},
utils::{DisableFloatingPointExceptionsFlags, SimdBasis, SimdRealCopy},
};
#[inline]
#[cfg(feature = "dim3")]
pub(crate) fn compute_tangent_contact_directions<N>(
force_dir1: &Vector<N>,
linvel1: &Vector<N>,
linvel2: &Vector<N>,
) -> [Vector<N>; DIM - 1]
where
N: SimdRealCopy,
Vector<N>: SimdBasis,
{
use SimdBasis;
use na::SimdValue;
// Compute the tangent direction. Pick the direction of
// the linear relative velocity, if it is not too small.
// Otherwise use a fallback direction.
let relative_linvel = linvel1 - linvel2;
let mut tangent_relative_linvel =
relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel));
let tangent_linvel_norm = {
let _disable_fe_except =
DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions();
tangent_relative_linvel.normalize_mut()
};
const THRESHOLD: Real = 1.0e-4;
let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD));
let tangent_fallback = force_dir1.orthonormal_vector();
let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);
let bitangent1 = force_dir1.cross(&tangent1);
[tangent1, bitangent1]
}

424
src/dynamics/solver/contact_constraint/one_body_constraint.rs
View File

@@ -1,424 +0,0 @@
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
use crate::math::{DIM, MAX_MANIFOLD_POINTS, Point, Real, Vector};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot, SimdRealCopy};
use na::Matrix2;
use parry::math::Isometry;
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
// TODO: move this struct somewhere else.
#[derive(Copy, Clone, Debug)]
pub struct ContactPointInfos<N: SimdRealCopy> {
pub tangent_vel: Vector<N>,
pub local_p1: Point<N>,
pub local_p2: Point<N>,
pub dist: N,
pub normal_rhs_wo_bias: N,
}
impl<N: SimdRealCopy> Default for ContactPointInfos<N> {
fn default() -> Self {
Self {
tangent_vel: Vector::zeros(),
local_p1: Point::origin(),
local_p2: Point::origin(),
dist: N::zero(),
normal_rhs_wo_bias: N::zero(),
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintBuilder {
// PERF: only store what’s necessary for the bias updates instead of the complete solver body.
pub rb1: SolverBody,
pub vels1: RigidBodyVelocity,
pub infos: [ContactPointInfos<Real>; MAX_MANIFOLD_POINTS],
}
impl OneBodyConstraintBuilder {
pub fn invalid() -> Self {
Self {
rb1: SolverBody::default(),
vels1: RigidBodyVelocity::zero(),
infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS],
}
}
pub fn generate(
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
out_builders: &mut [OneBodyConstraintBuilder],
out_constraints: &mut [OneBodyConstraint],
) {
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 handle1, &mut handle2);
(manifold.data.normal, -1.0)
} else {
(-manifold.data.normal, 1.0)
};
let (vels1, world_com1) = if let Some(handle1) = handle1 {
let rb1 = &bodies[handle1];
(rb1.vels, rb1.mprops.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let rb1 = handle1
.map(|h| SolverBody::from(&bodies[h]))
.unwrap_or_default();
let rb2 = &bodies[handle2.unwrap()];
let vels2 = &rb2.vels;
let mprops2 = &rb2.mprops;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
let solver_vel2 = rb2.ids.active_set_offset;
for (l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
builder.rb1 = rb1;
builder.vels1 = vels1;
constraint.dir1 = force_dir1;
constraint.im2 = mprops2.effective_inv_mass;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
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);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
let normal_rhs_wo_bias;
{
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let projected_lin_mass =
force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1));
let projected_ang_mass = gcross2.gdot(gcross2);
let projected_mass = utils::inv(projected_lin_mass + projected_ang_mass);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let proj_vel1 = vel1.dot(&force_dir1);
let proj_vel2 = vel2.dot(&force_dir1);
let dvel = proj_vel1 - proj_vel2;
// NOTE: we add proj_vel1 since it’s not accessible through solver_vel.
normal_rhs_wo_bias =
proj_vel1 + (is_bouncy * manifold_point.restitution) * dvel;
constraint.elements[k].normal_part = OneBodyConstraintNormalPart {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: manifold_point.warmstart_impulse,
impulse_accumulator: na::zero(),
r: projected_mass,
r_mat_elts: [0.0; 2],
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse =
manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = tangents1[j]
.dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2);
let rhs_wo_bias = (vel1
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = 2.0
* constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]);
}
}
// Builder.
{
let local_p1 = rb1.position.inverse_transform_point(&manifold_point.point);
let local_p2 = rb2
.pos
.position
.inverse_transform_point(&manifold_point.point);
let infos = ContactPointInfos {
local_p1,
local_p2,
tangent_vel: flipped_multiplier * manifold_point.tangent_velocity,
dist: manifold_point.dist,
normal_rhs_wo_bias,
};
builder.infos[k] = infos;
}
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..manifold_points.len() / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let mut r_mat = Matrix2::zeros();
let r0 = constraint.elements[k0].normal_part.r;
let r1 = constraint.elements[k1].normal_part.r;
r_mat.m12 = force_dir1
.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ constraint.elements[k0]
.normal_part
.gcross2
.gdot(constraint.elements[k1].normal_part.gcross2);
r_mat.m21 = r_mat.m12;
r_mat.m11 = utils::inv(r0);
r_mat.m22 = utils::inv(r1);
if let Some(inv) = r_mat.try_inverse() {
constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22];
constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12];
} else {
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.elements[k0].normal_part.r_mat_elts =
if manifold_points[k0].dist <= manifold_points[k1].dist {
[r0, 0.0]
} else {
[0.0, r1]
};
constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2];
}
}
}
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
_multibodies: &MultibodyJointSet,
constraint: &mut OneBodyConstraint,
) {
let rb2 = &bodies[constraint.solver_vel2];
self.update_with_positions(params, solved_dt, &rb2.position, constraint)
}
// TODO: this code is SOOOO similar to TwoBodyConstraint::update.
// In fact the only differences are types and the `rb1` and ignoring its ccd thickness.
pub fn update_with_positions(
&self,
params: &IntegrationParameters,
solved_dt: Real,
rb2_pos: &Isometry<Real>,
constraint: &mut OneBodyConstraint,
) {
let cfm_factor = params.contact_cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.contact_erp_inv_dt();
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
let rb1 = &self.rb1;
// Integrate the velocity of the static rigid-body, if it’s kinematic.
let new_pos1 = self
.vels1
.integrate(solved_dt, &rb1.position, &rb1.local_com);
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = rb2_pos * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
.clamp(-params.max_corrective_velocity(), 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse *= params.warmstart_coefficient;
}
// Tangent part.
{
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse *= params.warmstart_coefficient;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = cfm_factor;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraint {
pub solver_vel2: usize,
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub elements: [OneBodyConstraintElement<Real>; MAX_MANIFOLD_POINTS],
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
}
impl OneBodyConstraint {
pub fn invalid() -> Self {
Self {
solver_vel2: usize::MAX,
dir1: Vector::zeros(),
#[cfg(feature = "dim3")]
tangent1: Vector::zeros(),
im2: Vector::zeros(),
cfm_factor: 0.0,
limit: 0.0,
elements: [OneBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS],
manifold_id: ContactManifoldIndex::MAX,
manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS],
num_contacts: u8::MAX,
}
}
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = solver_vels[self.solver_vel2];
OneBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
&mut solver_vel2,
);
solver_vels[self.solver_vel2] = solver_vel2;
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut solver_vel2 = solver_vels[self.solver_vel2];
OneBodyConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
self.limit,
&mut solver_vel2,
solve_normal,
solve_friction,
);
solver_vels[self.solver_vel2] = solver_vel2;
}
// FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id];
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse;
active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse;
active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = 1.0;
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
}
}
}

311
src/dynamics/solver/contact_constraint/one_body_constraint_element.rs
View File

@@ -1,311 +0,0 @@
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::contact_constraint::TwoBodyConstraintNormalPart;
use crate::math::{AngVector, DIM, TangentImpulse, Vector};
use crate::utils::{SimdBasis, SimdDot, SimdRealCopy};
use na::Vector2;
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintTangentPart<N: SimdRealCopy> {
pub gcross2: [AngVector<N>; DIM - 1],
pub rhs: [N; DIM - 1],
pub rhs_wo_bias: [N; DIM - 1],
pub impulse: TangentImpulse<N>,
pub impulse_accumulator: TangentImpulse<N>,
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: SimdRealCopy> OneBodyConstraintTangentPart<N> {
fn zero() -> Self {
Self {
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
rhs_wo_bias: [na::zero(); DIM - 1],
impulse: na::zero(),
impulse_accumulator: na::zero(),
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
/// Total impulse applied across all the solver substeps.
#[inline]
pub fn total_impulse(&self) -> TangentImpulse<N> {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn warmstart(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im2: &Vector<N>,
solver_vel2: &mut SolverVel<N>,
) {
#[cfg(feature = "dim2")]
{
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0];
solver_vel2.angular += self.gcross2[0] * self.impulse[0];
}
#[cfg(feature = "dim3")]
{
solver_vel2.linear += tangents1[0].component_mul(im2) * -self.impulse[0]
+ tangents1[1].component_mul(im2) * -self.impulse[1];
solver_vel2.angular +=
self.gcross2[0] * self.impulse[0] + self.gcross2[1] * self.impulse[1];
}
}
#[inline]
pub fn solve(
&mut self,
tangents1: [&Vector<N>; DIM - 1],
im2: &Vector<N>,
limit: N,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim2")]
{
let dvel = -tangents1[0].dot(&solver_vel2.linear)
+ self.gcross2[0].gdot(solver_vel2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dvel).simd_clamp(-limit, limit);
let dlambda = new_impulse - self.impulse[0];
self.impulse[0] = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2[0] * dlambda;
}
#[cfg(feature = "dim3")]
{
let dvel_0 = -tangents1[0].dot(&solver_vel2.linear)
+ self.gcross2[0].gdot(solver_vel2.angular)
+ self.rhs[0];
let dvel_1 = -tangents1[1].dot(&solver_vel2.linear)
+ self.gcross2[1].gdot(solver_vel2.angular)
+ self.rhs[1];
let dvel_00 = dvel_0 * dvel_0;
let dvel_11 = dvel_1 * dvel_1;
let dvel_01 = dvel_0 * dvel_1;
let inv_lhs = (dvel_00 + dvel_11)
* crate::utils::simd_inv(
dvel_00 * self.r[0] + dvel_11 * self.r[1] + dvel_01 * self.r[2],
);
let delta_impulse = na::vector![inv_lhs * dvel_0, inv_lhs * dvel_1];
let new_impulse = self.impulse - delta_impulse;
let new_impulse = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
+ tangents1[1].component_mul(im2) * -dlambda[1];
solver_vel2.angular += self.gcross2[0] * dlambda[0] + self.gcross2[1] * dlambda[1];
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintNormalPart<N: SimdRealCopy> {
pub gcross2: AngVector<N>,
pub rhs: N,
pub rhs_wo_bias: N,
pub impulse: N,
pub impulse_accumulator: N,
pub r: N,
pub r_mat_elts: [N; 2],
}
impl<N: SimdRealCopy> OneBodyConstraintNormalPart<N> {
fn zero() -> Self {
Self {
gcross2: na::zero(),
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: na::zero(),
impulse_accumulator: na::zero(),
r: na::zero(),
r_mat_elts: [N::zero(); 2],
}
}
/// Total impulse applied across all the solver substeps.
#[inline]
pub fn total_impulse(&self) -> N {
self.impulse_accumulator + self.impulse
}
#[inline]
pub fn warmstart(&mut self, dir1: &Vector<N>, im2: &Vector<N>, solver_vel2: &mut SolverVel<N>) {
solver_vel2.linear += dir1.component_mul(im2) * -self.impulse;
solver_vel2.angular += self.gcross2 * self.impulse;
}
#[inline]
pub fn solve(
&mut self,
cfm_factor: N,
dir1: &Vector<N>,
im2: &Vector<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
let dvel =
-dir1.dot(&solver_vel2.linear) + self.gcross2.gdot(solver_vel2.angular) + self.rhs;
let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
solver_vel2.linear += dir1.component_mul(im2) * -dlambda;
solver_vel2.angular += self.gcross2 * dlambda;
}
#[inline]
pub fn solve_pair(
constraint_a: &mut Self,
constraint_b: &mut Self,
cfm_factor: N,
dir1: &Vector<N>,
im2: &Vector<N>,
solver_vel2: &mut SolverVel<N>,
) where
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
let dvel_a = -dir1.dot(&solver_vel2.linear)
+ constraint_a.gcross2.gdot(solver_vel2.angular)
+ constraint_a.rhs;
let dvel_b = -dir1.dot(&solver_vel2.linear)
+ constraint_b.gcross2.gdot(solver_vel2.angular)
+ constraint_b.rhs;
let prev_impulse = Vector2::new(constraint_a.impulse, constraint_b.impulse);
let new_impulse = TwoBodyConstraintNormalPart::solve_mlcp_two_constraints(
Vector2::new(dvel_a, dvel_b),
prev_impulse,
constraint_a.r,
constraint_b.r,
constraint_a.r_mat_elts,
constraint_b.r_mat_elts,
cfm_factor,
);
let dlambda = new_impulse - prev_impulse;
constraint_a.impulse = new_impulse.x;
constraint_b.impulse = new_impulse.y;
solver_vel2.linear += dir1.component_mul(im2) * (-dlambda.x - dlambda.y);
solver_vel2.angular += constraint_a.gcross2 * dlambda.x + constraint_b.gcross2 * dlambda.y;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintElement<N: SimdRealCopy> {
pub normal_part: OneBodyConstraintNormalPart<N>,
pub tangent_part: OneBodyConstraintTangentPart<N>,
}
impl<N: SimdRealCopy> OneBodyConstraintElement<N> {
pub fn zero() -> Self {
Self {
normal_part: OneBodyConstraintNormalPart::zero(),
tangent_part: OneBodyConstraintTangentPart::zero(),
}
}
#[inline]
pub fn warmstart_group(
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im2: &Vector<N>,
solver_vel2: &mut SolverVel<N>,
) {
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
for element in elements.iter_mut() {
element.normal_part.warmstart(dir1, im2, solver_vel2);
element.tangent_part.warmstart(tangents1, im2, solver_vel2);
}
}
#[inline]
pub fn solve_group(
cfm_factor: N,
elements: &mut [Self],
dir1: &Vector<N>,
#[cfg(feature = "dim3")] tangent1: &Vector<N>,
im2: &Vector<N>,
limit: N,
solver_vel2: &mut SolverVel<N>,
solve_normal: bool,
solve_friction: bool,
) where
Vector<N>: SimdBasis,
AngVector<N>: SimdDot<AngVector<N>, Result = N>,
{
#[cfg(feature = "dim3")]
let tangents1 = [tangent1, &dir1.cross(tangent1)];
#[cfg(feature = "dim2")]
let tangents1 = [&dir1.orthonormal_vector()];
// Solve penetration.
if solve_normal {
if BLOCK_SOLVER_ENABLED {
for elements in elements.chunks_exact_mut(2) {
let [element_a, element_b] = elements else {
unreachable!()
};
OneBodyConstraintNormalPart::solve_pair(
&mut element_a.normal_part,
&mut element_b.normal_part,
cfm_factor,
dir1,
im2,
solver_vel2,
);
}
if elements.len() % 2 == 1 {
let element = elements.last_mut().unwrap();
element
.normal_part
.solve(cfm_factor, dir1, im2, solver_vel2);
}
} else {
for element in elements.iter_mut() {
element
.normal_part
.solve(cfm_factor, dir1, im2, solver_vel2);
}
}
}
// Solve friction.
if solve_friction {
for element in elements.iter_mut() {
let limit = limit * element.normal_part.impulse;
let part = &mut element.tangent_part;
part.solve(tangents1, im2, limit, solver_vel2);
}
}
}
}

426
src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
View File

@@ -1,426 +0,0 @@
use super::{OneBodyConstraintElement, OneBodyConstraintNormalPart};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, SolverVel};
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, DIM, Isometry, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH,
SimdReal, TangentImpulse, Vector,
};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
use parry::utils::SdpMatrix2;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub(crate) struct SimdOneBodyConstraintBuilder {
// PERF: only store what’s needed, and store it in simd form.
rb1: [SolverBody; SIMD_WIDTH],
vels1: [RigidBodyVelocity; SIMD_WIDTH],
infos: [ContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
}
impl SimdOneBodyConstraintBuilder {
pub fn generate(
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
out_builders: &mut [SimdOneBodyConstraintBuilder],
out_constraints: &mut [OneBodyConstraintSimd],
) {
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 handles1[ii], &mut handles2[ii]);
flipped[ii] = -1.0;
}
}
let rb1: [SolverBody; SIMD_WIDTH] = gather![|ii| {
handles1[ii]
.map(|h| SolverBody::from(&bodies[h]))
.unwrap_or_else(SolverBody::default)
}];
let vels1: [RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| {
handles1[ii]
.map(|h| bodies[h].vels)
.unwrap_or_else(RigidBodyVelocity::default)
}];
let world_com1 = Point::from(gather![|ii| { rb1[ii].world_com }]);
let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops];
let flipped_sign = SimdReal::from(flipped);
let im2 = Vector::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(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let linvel2 = Vector::from(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let poss2 = Isometry::from(gather![|ii| bodies2[ii].pos.position]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let normal1 = Vector::from(gather![|ii| manifolds[ii].data.normal]);
let force_dir1 = normal1 * -flipped_sign;
let solver_vel2 = gather![|ii| ids2[ii].active_set_offset];
let num_active_contacts = manifolds[0].data.num_active_contacts();
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
let constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
builder.rb1 = rb1;
builder.vels1 = vels1;
constraint.dir1 = force_dir1;
constraint.im2 = im2;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = num_points as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
for k in 0..num_points {
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 warmstart_impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points
[ii][k]
.warmstart_tangent_impulse]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
let tangent_velocity =
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
// Normal part.
let normal_rhs_wo_bias;
{
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let projected_mass = utils::simd_inv(
force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2),
);
let projected_vel1 = vel1.dot(&force_dir1);
let projected_vel2 = vel2.dot(&force_dir1);
let projected_velocity = projected_vel1 - projected_vel2;
normal_rhs_wo_bias =
(is_bouncy * restitution) * projected_velocity + projected_vel1; // Add projected_vel1 since it’s not accessible through solver_vel.
constraint.elements[k].normal_part = OneBodyConstraintNormalPart {
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: warmstart_impulse,
impulse_accumulator: na::zero(),
r: projected_mass,
r_mat_elts: [SimdReal::zero(); 2],
};
}
// tangent parts.
constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r =
tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2);
let rhs_wo_bias = (vel1 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::simd_inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0)
* constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]);
}
// Builder.
{
let local_p1 = poss1.inverse_transform_point(&point);
let local_p2 = poss2.inverse_transform_point(&point);
let infos = ContactPointInfos {
local_p1,
local_p2,
tangent_vel: tangent_velocity * flipped_sign,
dist,
normal_rhs_wo_bias,
};
builder.infos[k] = infos;
}
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..num_points / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let r0 = constraint.elements[k0].normal_part.r;
let r1 = constraint.elements[k1].normal_part.r;
let mut r_mat = SdpMatrix2::zero();
r_mat.m12 = force_dir1.dot(&im2.component_mul(&force_dir1))
+ constraint.elements[k0]
.normal_part
.gcross2
.gdot(constraint.elements[k1].normal_part.gcross2);
r_mat.m11 = utils::simd_inv(r0);
r_mat.m22 = utils::simd_inv(r1);
let (inv, det) = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
r_mat.inverse_and_get_determinant_unchecked()
};
let is_invertible = det.simd_gt(SimdReal::zero());
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.elements[k0].normal_part.r_mat_elts = [
inv.m11.select(is_invertible, r0),
inv.m22.select(is_invertible, SimdReal::zero()),
];
constraint.elements[k1].normal_part.r_mat_elts = [
inv.m12.select(is_invertible, SimdReal::zero()),
r_mat.m12.select(is_invertible, SimdReal::zero()),
];
}
}
}
}
// TODO: this code is SOOOO similar to TwoBodyConstraintSimd::update.
// In fact the only differences are types and the `rb1` and ignoring its ccd thickness.
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
_multibodies: &MultibodyJointSet,
constraint: &mut OneBodyConstraintSimd,
) {
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
// Integrate the velocity of the static rigid-body, if it’s kinematic.
let new_pos1 = Isometry::from(gather![|ii| self.vels1[ii].integrate(
solved_dt,
&self.rb1[ii].position,
&self.rb1[ii].local_com
)]);
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
let solved_dt = SimdReal::splat(solved_dt);
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
let p1 = new_pos1 * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = poss2 * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias =
info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
let new_rhs = rhs_wo_bias + rhs_bias;
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse *= warmstart_coeff;
}
// tangent parts.
{
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse *= warmstart_coeff;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = cfm_factor;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct OneBodyConstraintSimd {
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub elements: [OneBodyConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
pub solver_vel2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
}
impl OneBodyConstraintSimd {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
OneBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
&mut solver_vel2,
);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
OneBodyConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im2,
self.limit,
&mut solver_vel2,
solve_normal,
solve_friction,
);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
}
// FIXME: duplicated code. This is exactly the same as in the two-body velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse;
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
let tangent_impulses = self.elements[k].tangent_part.total_impulse();
for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.warmstart_impulse = warmstart_impulses[ii];
active_contact.data.warmstart_tangent_impulse =
warmstart_tangent_impulses.extract(ii);
active_contact.data.impulse = impulses[ii];
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = SimdReal::splat(1.0);
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
}
}
}

533
src/dynamics/solver/contact_constraint/two_body_constraint.rs
View File

@@ -1,533 +0,0 @@
use super::{ContactConstraintTypes, ContactPointInfos};
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::{AnyConstraintMut, SolverBody};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::{IntegrationParameters, MultibodyJointSet, RigidBodySet};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{DIM, Isometry, MAX_MANIFOLD_POINTS, Real, Vector};
use crate::utils::{self, SimdAngularInertia, SimdBasis, SimdCross, SimdDot};
use na::{DVector, Matrix2};
use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
impl AnyConstraintMut<'_, ContactConstraintTypes> {
pub fn remove_bias(&mut self) {
match self {
Self::OneBody(c) => c.remove_cfm_and_bias_from_rhs(),
Self::TwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
Self::GenericOneBody(c) => c.remove_cfm_and_bias_from_rhs(),
Self::GenericTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.remove_cfm_and_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.remove_cfm_and_bias_from_rhs(),
}
}
pub fn warmstart(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.warmstart(solver_vels),
Self::TwoBodies(c) => c.warmstart(solver_vels),
Self::GenericOneBody(c) => c.warmstart(generic_jacobians, generic_solver_vels),
Self::GenericTwoBodies(c) => {
c.warmstart(generic_jacobians, solver_vels, generic_solver_vels)
}
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.warmstart(solver_vels),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.warmstart(solver_vels),
}
}
pub fn solve_restitution(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.solve(solver_vels, true, false),
Self::TwoBodies(c) => c.solve(solver_vels, true, false),
Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, true, false),
Self::GenericTwoBodies(c) => c.solve(
generic_jacobians,
solver_vels,
generic_solver_vels,
true,
false,
),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.solve(solver_vels, true, false),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.solve(solver_vels, true, false),
}
}
pub fn solve_friction(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.solve(solver_vels, false, true),
Self::TwoBodies(c) => c.solve(solver_vels, false, true),
Self::GenericOneBody(c) => c.solve(generic_jacobians, generic_solver_vels, false, true),
Self::GenericTwoBodies(c) => c.solve(
generic_jacobians,
solver_vels,
generic_solver_vels,
false,
true,
),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.solve(solver_vels, false, true),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.solve(solver_vels, false, true),
}
}
pub fn writeback_impulses(&mut self, manifolds_all: &mut [&mut ContactManifold]) {
match self {
Self::OneBody(c) => c.writeback_impulses(manifolds_all),
Self::TwoBodies(c) => c.writeback_impulses(manifolds_all),
Self::GenericOneBody(c) => c.writeback_impulses(manifolds_all),
Self::GenericTwoBodies(c) => c.writeback_impulses(manifolds_all),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.writeback_impulses(manifolds_all),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.writeback_impulses(manifolds_all),
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraint {
pub dir1: Vector<Real>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<Real>, // One of the friction force directions.
pub im1: Vector<Real>,
pub im2: Vector<Real>,
pub cfm_factor: Real,
pub limit: Real,
pub solver_vel1: usize,
pub solver_vel2: usize,
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub elements: [TwoBodyConstraintElement<Real>; MAX_MANIFOLD_POINTS],
}
impl TwoBodyConstraint {
pub fn invalid() -> Self {
Self {
dir1: Vector::zeros(),
#[cfg(feature = "dim3")]
tangent1: Vector::zeros(),
im1: Vector::zeros(),
im2: Vector::zeros(),
cfm_factor: 0.0,
limit: 0.0,
solver_vel1: usize::MAX,
solver_vel2: usize::MAX,
manifold_id: ContactManifoldIndex::MAX,
manifold_contact_id: [u8::MAX; MAX_MANIFOLD_POINTS],
num_contacts: u8::MAX,
elements: [TwoBodyConstraintElement::zero(); MAX_MANIFOLD_POINTS],
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintBuilder {
pub infos: [ContactPointInfos<Real>; MAX_MANIFOLD_POINTS],
}
impl TwoBodyConstraintBuilder {
pub fn invalid() -> Self {
Self {
infos: [ContactPointInfos::default(); MAX_MANIFOLD_POINTS],
}
}
pub fn generate(
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &RigidBodySet,
out_builders: &mut [TwoBodyConstraintBuilder],
out_constraints: &mut [TwoBodyConstraint],
) {
assert_eq!(manifold.data.relative_dominance, 0);
let handle1 = manifold.data.rigid_body1.unwrap();
let handle2 = manifold.data.rigid_body2.unwrap();
let rb1 = &bodies[handle1];
let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
let rb2 = &bodies[handle2];
let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
let solver_vel1 = rb1.ids.active_set_offset;
let solver_vel2 = rb2.ids.active_set_offset;
let force_dir1 = -manifold.data.normal;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 =
super::compute_tangent_contact_directions(&force_dir1, &vels1.linvel, &vels2.linvel);
for (l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let builder = &mut out_builders[l];
let constraint = &mut out_constraints[l];
constraint.dir1 = force_dir1;
constraint.im1 = mprops1.effective_inv_mass;
constraint.im2 = mprops2.effective_inv_mass;
constraint.solver_vel1 = solver_vel1;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = manifold_points.len() as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let point = manifold_point.point;
let dp1 = point - mprops1.world_com;
let dp2 = point - mprops2.world_com;
let vel1 = vels1.linvel + vels1.angvel.gcross(dp1);
let vel2 = vels2.linvel + vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
let normal_rhs_wo_bias;
{
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(force_dir1));
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let projected_mass = utils::inv(
force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2),
);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
normal_rhs_wo_bias =
(is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1);
constraint.elements[k].normal_part = TwoBodyConstraintNormalPart {
gcross1,
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: manifold_point.warmstart_impulse,
impulse_accumulator: na::zero(),
r: projected_mass,
r_mat_elts: [0.0; 2],
};
}
// Tangent parts.
{
constraint.elements[k].tangent_part.impulse =
manifold_point.warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross1 = mprops1
.effective_world_inv_inertia_sqrt
.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs_wo_bias = manifold_point.tangent_velocity.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = 2.0
* (constraint.elements[k].tangent_part.gcross1[0]
.gdot(constraint.elements[k].tangent_part.gcross1[1])
+ constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]));
}
}
// Builder.
let infos = ContactPointInfos {
local_p1: rb1
.pos
.position
.inverse_transform_point(&manifold_point.point),
local_p2: rb2
.pos
.position
.inverse_transform_point(&manifold_point.point),
tangent_vel: manifold_point.tangent_velocity,
dist: manifold_point.dist,
normal_rhs_wo_bias,
};
builder.infos[k] = infos;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..manifold_points.len() / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let mut r_mat = Matrix2::zeros();
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r0 = constraint.elements[k0].normal_part.r;
let r1 = constraint.elements[k1].normal_part.r;
r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+ constraint.elements[k0]
.normal_part
.gcross1
.gdot(constraint.elements[k1].normal_part.gcross1)
+ constraint.elements[k0]
.normal_part
.gcross2
.gdot(constraint.elements[k1].normal_part.gcross2);
r_mat.m21 = r_mat.m12;
r_mat.m11 = utils::inv(r0);
r_mat.m22 = utils::inv(r1);
if let Some(inv) = r_mat.try_inverse() {
constraint.elements[k0].normal_part.r_mat_elts = [inv.m11, inv.m22];
constraint.elements[k1].normal_part.r_mat_elts = [inv.m12, r_mat.m12];
} else {
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.elements[k0].normal_part.r_mat_elts =
if manifold_points[k0].dist <= manifold_points[k1].dist {
[r0, 0.0]
} else {
[0.0, r1]
};
constraint.elements[k1].normal_part.r_mat_elts = [0.0; 2];
}
}
}
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
_multibodies: &MultibodyJointSet,
constraint: &mut TwoBodyConstraint,
) {
let rb1 = &bodies[constraint.solver_vel1];
let rb2 = &bodies[constraint.solver_vel2];
self.update_with_positions(params, solved_dt, &rb1.position, &rb2.position, constraint)
}
// Used by both generic and non-generic builders..
pub fn update_with_positions(
&self,
params: &IntegrationParameters,
solved_dt: Real,
rb1_pos: &Isometry<Real>,
rb2_pos: &Isometry<Real>,
constraint: &mut TwoBodyConstraint,
) {
let cfm_factor = params.contact_cfm_factor();
let inv_dt = params.inv_dt();
let erp_inv_dt = params.contact_erp_inv_dt();
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
// Tangent velocity is equivalent to the first body’s surface moving artificially.
let p1 = rb1_pos * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = rb2_pos * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
.clamp(-params.max_corrective_velocity(), 0.0);
let new_rhs = rhs_wo_bias + rhs_bias;
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse *= params.warmstart_coefficient;
}
// Tangent part.
{
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse *= params.warmstart_coefficient;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = cfm_factor;
}
}
impl TwoBodyConstraint {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = solver_vels[self.solver_vel1];
let mut solver_vel2 = solver_vels[self.solver_vel2];
TwoBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
solver_vels[self.solver_vel1] = solver_vel1;
solver_vels[self.solver_vel2] = solver_vel2;
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut solver_vel1 = solver_vels[self.solver_vel1];
let mut solver_vel2 = solver_vels[self.solver_vel2];
TwoBodyConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
self.limit,
&mut solver_vel1,
&mut solver_vel2,
solve_normal,
solve_friction,
);
solver_vels[self.solver_vel1] = solver_vel1;
solver_vels[self.solver_vel2] = solver_vel2;
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id];
for k in 0..self.num_contacts as usize {
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.warmstart_impulse = self.elements[k].normal_part.impulse;
active_contact.data.warmstart_tangent_impulse = self.elements[k].tangent_part.impulse;
active_contact.data.impulse = self.elements[k].normal_part.total_impulse();
active_contact.data.tangent_impulse = self.elements[k].tangent_part.total_impulse();
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = 1.0;
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
// elt.normal_part.impulse = elt.normal_part.total_impulse;
elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
// elt.tangent_part.impulse = elt.tangent_part.total_impulse;
}
}
}
#[inline(always)]
#[cfg(feature = "dim3")]
pub(crate) fn compute_tangent_contact_directions<N>(
force_dir1: &Vector<N>,
linvel1: &Vector<N>,
linvel2: &Vector<N>,
) -> [Vector<N>; DIM - 1]
where
N: utils::SimdRealCopy,
Vector<N>: SimdBasis,
{
use na::SimdValue;
// Compute the tangent direction. Pick the direction of
// the linear relative velocity, if it is not too small.
// Otherwise use a fallback direction.
let relative_linvel = linvel1 - linvel2;
let mut tangent_relative_linvel =
relative_linvel - force_dir1 * (force_dir1.dot(&relative_linvel));
let tangent_linvel_norm = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::disable_floating_point_exceptions();
tangent_relative_linvel.normalize_mut()
};
const THRESHOLD: Real = 1.0e-4;
let use_fallback = tangent_linvel_norm.simd_lt(N::splat(THRESHOLD));
let tangent_fallback = force_dir1.orthonormal_vector();
let tangent1 = tangent_fallback.select(use_fallback, tangent_relative_linvel);
let bitangent1 = force_dir1.cross(&tangent1);
[tangent1, bitangent1]
}

433
src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
View File

@@ -1,433 +0,0 @@
use super::{TwoBodyConstraintElement, TwoBodyConstraintNormalPart};
use crate::dynamics::integration_parameters::BLOCK_SOLVER_ENABLED;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::{ContactPointInfos, SolverVel};
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodySet,
RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{
AngVector, AngularInertia, DIM, Isometry, MAX_MANIFOLD_POINTS, Point, Real, SIMD_WIDTH,
SimdReal, TangentImpulse, Vector,
};
#[cfg(feature = "dim2")]
use crate::utils::SimdBasis;
use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
use num::Zero;
use parry::utils::SdpMatrix2;
use simba::simd::{SimdPartialOrd, SimdValue};
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintBuilderSimd {
infos: [super::ContactPointInfos<SimdReal>; MAX_MANIFOLD_POINTS],
}
impl TwoBodyConstraintBuilderSimd {
pub fn generate(
manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
manifolds: [&ContactManifold; SIMD_WIDTH],
bodies: &RigidBodySet,
out_builders: &mut [TwoBodyConstraintBuilderSimd],
out_constraints: &mut [TwoBodyConstraintSimd],
) {
for ii in 0..SIMD_WIDTH {
assert_eq!(manifolds[ii].data.relative_dominance, 0);
}
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
let handles2 = gather![|ii| manifolds[ii].data.rigid_body2.unwrap()];
let vels1: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].vels];
let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].vels];
let ids1: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].ids];
let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].ids];
let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
let poss1 = Isometry::from(gather![|ii| bodies[handles1[ii]].pos.position]);
let poss2 = Isometry::from(gather![|ii| bodies[handles2[ii]].pos.position]);
let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);
let ii1: AngularInertia<SimdReal> =
AngularInertia::from(gather![|ii| mprops1[ii].effective_world_inv_inertia_sqrt]);
let linvel1 = Vector::from(gather![|ii| vels1[ii].linvel]);
let angvel1 = AngVector::<SimdReal>::from(gather![|ii| vels1[ii].angvel]);
let world_com2 = Point::from(gather![|ii| mprops2[ii].world_com]);
let im2 = Vector::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(gather![|ii| vels2[ii].linvel]);
let angvel2 = AngVector::<SimdReal>::from(gather![|ii| vels2[ii].angvel]);
let force_dir1 = -Vector::from(gather![|ii| manifolds[ii].data.normal]);
let solver_vel1 = gather![|ii| ids1[ii].active_set_offset];
let solver_vel2 = gather![|ii| ids2[ii].active_set_offset];
let num_active_contacts = manifolds[0].data.num_active_contacts();
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions(&force_dir1, &linvel1, &linvel2);
for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
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 constraint = &mut out_constraints[l / MAX_MANIFOLD_POINTS];
let builder = &mut out_builders[l / MAX_MANIFOLD_POINTS];
constraint.dir1 = force_dir1;
constraint.im1 = im1;
constraint.im2 = im2;
constraint.solver_vel1 = solver_vel1;
constraint.solver_vel2 = solver_vel2;
constraint.manifold_id = manifold_id;
constraint.num_contacts = num_points as u8;
#[cfg(feature = "dim3")]
{
constraint.tangent1 = tangents1[0];
}
for k in 0..num_points {
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 warmstart_impulse =
SimdReal::from(gather![|ii| manifold_points[ii][k].warmstart_impulse]);
let warmstart_tangent_impulse = TangentImpulse::from(gather![|ii| manifold_points
[ii][k]
.warmstart_tangent_impulse]);
let dist = SimdReal::from(gather![|ii| manifold_points[ii][k].dist]);
let point = Point::from(gather![|ii| manifold_points[ii][k].point]);
let tangent_velocity =
Vector::from(gather![|ii| manifold_points[ii][k].tangent_velocity]);
let dp1 = point - world_com1;
let dp2 = point - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
constraint.manifold_contact_id[k] = gather![|ii| manifold_points[ii][k].contact_id];
// Normal part.
let normal_rhs_wo_bias;
{
let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let imsum = im1 + im2;
let projected_mass = utils::simd_inv(
force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2),
);
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
normal_rhs_wo_bias = is_bouncy * restitution * projected_velocity;
constraint.elements[k].normal_part = TwoBodyConstraintNormalPart {
gcross1,
gcross2,
rhs: na::zero(),
rhs_wo_bias: na::zero(),
impulse: warmstart_impulse,
impulse_accumulator: SimdReal::splat(0.0),
r: projected_mass,
r_mat_elts: [SimdReal::zero(); 2],
};
}
// tangent parts.
constraint.elements[k].tangent_part.impulse = warmstart_tangent_impulse;
for j in 0..DIM - 1 {
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = im1 + im2;
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs_wo_bias = tangent_velocity.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs_wo_bias[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.rhs[j] = rhs_wo_bias;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
utils::simd_inv(r)
} else {
r
};
}
#[cfg(feature = "dim3")]
{
constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0)
* (constraint.elements[k].tangent_part.gcross1[0]
.gdot(constraint.elements[k].tangent_part.gcross1[1])
+ constraint.elements[k].tangent_part.gcross2[0]
.gdot(constraint.elements[k].tangent_part.gcross2[1]));
}
// Builder.
let infos = ContactPointInfos {
local_p1: poss1.inverse_transform_point(&point),
local_p2: poss2.inverse_transform_point(&point),
tangent_vel: tangent_velocity,
dist,
normal_rhs_wo_bias,
};
builder.infos[k] = infos;
}
if BLOCK_SOLVER_ENABLED {
// Coupling between consecutive pairs.
for k in 0..num_points / 2 {
let k0 = k * 2;
let k1 = k * 2 + 1;
let imsum = im1 + im2;
let r0 = constraint.elements[k0].normal_part.r;
let r1 = constraint.elements[k1].normal_part.r;
let mut r_mat = SdpMatrix2::zero();
r_mat.m12 = force_dir1.dot(&imsum.component_mul(&force_dir1))
+ constraint.elements[k0]
.normal_part
.gcross1
.gdot(constraint.elements[k1].normal_part.gcross1)
+ constraint.elements[k0]
.normal_part
.gcross2
.gdot(constraint.elements[k1].normal_part.gcross2);
r_mat.m11 = utils::simd_inv(r0);
r_mat.m22 = utils::simd_inv(r1);
let (inv, det) = {
let _disable_fe_except =
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
r_mat.inverse_and_get_determinant_unchecked()
};
let is_invertible = det.simd_gt(SimdReal::zero());
// If inversion failed, the contacts are redundant.
// Ignore the one with the smallest depth (it is too late to
// have the constraint removed from the constraint set, so just
// set the mass (r) matrix elements to 0.
constraint.elements[k0].normal_part.r_mat_elts = [
inv.m11.select(is_invertible, r0),
inv.m22.select(is_invertible, SimdReal::zero()),
];
constraint.elements[k1].normal_part.r_mat_elts = [
inv.m12.select(is_invertible, SimdReal::zero()),
r_mat.m12.select(is_invertible, SimdReal::zero()),
];
}
}
}
}
pub fn update(
&self,
params: &IntegrationParameters,
solved_dt: Real,
bodies: &[SolverBody],
_multibodies: &MultibodyJointSet,
constraint: &mut TwoBodyConstraintSimd,
) {
let cfm_factor = SimdReal::splat(params.contact_cfm_factor());
let inv_dt = SimdReal::splat(params.inv_dt());
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error());
let erp_inv_dt = SimdReal::splat(params.contact_erp_inv_dt());
let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]];
let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
let all_infos = &self.infos[..constraint.num_contacts as usize];
let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
#[cfg(feature = "dim2")]
let tangents1 = constraint.dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = [
constraint.tangent1,
constraint.dir1.cross(&constraint.tangent1),
];
let solved_dt = SimdReal::splat(solved_dt);
for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
// NOTE: the tangent velocity is equivalent to an additional movement of the first body’s surface.
let p1 = poss1 * info.local_p1 + info.tangent_vel * solved_dt;
let p2 = poss2 * info.local_p2;
let dist = info.dist + (p1 - p2).dot(&constraint.dir1);
// Normal part.
{
let rhs_wo_bias =
info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
.simd_clamp(-max_corrective_velocity, SimdReal::zero());
let new_rhs = rhs_wo_bias + rhs_bias;
element.normal_part.rhs_wo_bias = rhs_wo_bias;
element.normal_part.rhs = new_rhs;
element.normal_part.impulse_accumulator += element.normal_part.impulse;
element.normal_part.impulse *= warmstart_coeff;
}
// tangent parts.
{
element.tangent_part.impulse_accumulator += element.tangent_part.impulse;
element.tangent_part.impulse *= warmstart_coeff;
for j in 0..DIM - 1 {
let bias = (p1 - p2).dot(&tangents1[j]) * inv_dt;
element.tangent_part.rhs[j] = element.tangent_part.rhs_wo_bias[j] + bias;
}
}
}
constraint.cfm_factor = cfm_factor;
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct TwoBodyConstraintSimd {
pub dir1: Vector<SimdReal>, // Non-penetration force direction for the first body.
#[cfg(feature = "dim3")]
pub tangent1: Vector<SimdReal>, // One of the friction force directions.
pub elements: [TwoBodyConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub im1: Vector<SimdReal>,
pub im2: Vector<SimdReal>,
pub cfm_factor: SimdReal,
pub limit: SimdReal,
pub solver_vel1: [usize; SIMD_WIDTH],
pub solver_vel2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
}
impl TwoBodyConstraintSimd {
pub fn warmstart(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]),
};
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
TwoBodyConstraintElement::warmstart_group(
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
&mut solver_vel1,
&mut solver_vel2,
);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii);
solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii);
}
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
}
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solve_normal: bool,
solve_friction: bool,
) {
let mut solver_vel1 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]),
};
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
TwoBodyConstraintElement::solve_group(
self.cfm_factor,
&mut self.elements[..self.num_contacts as usize],
&self.dir1,
#[cfg(feature = "dim3")]
&self.tangent1,
&self.im1,
&self.im2,
self.limit,
&mut solver_vel1,
&mut solver_vel2,
solve_normal,
solve_friction,
);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii);
solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii);
}
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
for k in 0..self.num_contacts as usize {
let warmstart_impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into();
let warmstart_tangent_impulses = self.elements[k].tangent_part.impulse;
let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.total_impulse().into();
let tangent_impulses = self.elements[k].tangent_part.total_impulse();
for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.warmstart_impulse = warmstart_impulses[ii];
active_contact.data.warmstart_tangent_impulse =
warmstart_tangent_impulses.extract(ii);
active_contact.data.impulse = impulses[ii];
active_contact.data.tangent_impulse = tangent_impulses.extract(ii);
}
}
}
pub fn remove_cfm_and_bias_from_rhs(&mut self) {
self.cfm_factor = SimdReal::splat(1.0);
for elt in &mut self.elements {
elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
elt.tangent_part.rhs = elt.tangent_part.rhs_wo_bias;
}
}
}

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

@@ -117,26 +117,26 @@ impl ParallelInteractionGroups {
(false, false) => {
let rb1 = &bodies[body_pair.0.unwrap()];
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask =
bcolors[rb1.ids.active_set_offset] | bcolors[rb2.ids.active_set_offset];
let color_mask = bcolors[rb1.ids.active_set_offset as usize]
| bcolors[rb2.ids.active_set_offset as usize];
*color = (!color_mask).trailing_zeros() as usize;
color_len[*color] += 1;
bcolors[rb1.ids.active_set_offset] |= 1 << *color;
bcolors[rb2.ids.active_set_offset] |= 1 << *color;
bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color;
bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color;
}
(true, false) => {
let rb2 = &bodies[body_pair.1.unwrap()];
let color_mask = bcolors[rb2.ids.active_set_offset];
let color_mask = bcolors[rb2.ids.active_set_offset as usize];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb2.ids.active_set_offset] |= 1 << *color;
bcolors[rb2.ids.active_set_offset as usize] |= 1 << *color;
}
(false, true) => {
let rb1 = &bodies[body_pair.0.unwrap()];
let color_mask = bcolors[rb1.ids.active_set_offset];
let color_mask = bcolors[rb1.ids.active_set_offset as usize];
*color = 127 - (!color_mask).leading_zeros() as usize;
color_len[*color] += 1;
bcolors[rb1.ids.active_set_offset] |= 1 << *color;
bcolors[rb1.ids.active_set_offset as usize] |= 1 << *color;
}
(true, true) => unreachable!(),
}
@@ -173,9 +173,8 @@ pub(crate) struct InteractionGroups {
buckets: VecMap<([usize; SIMD_WIDTH], usize)>,
#[cfg(feature = "simd-is-enabled")]
body_masks: Vec<u128>,
#[cfg(feature = "simd-is-enabled")]
pub simd_interactions: Vec<usize>,
pub nongrouped_interactions: Vec<usize>,
pub simd_interactions: Vec<ContactManifoldIndex>,
pub nongrouped_interactions: Vec<ContactManifoldIndex>,
}
impl InteractionGroups {
@@ -185,7 +184,6 @@ impl InteractionGroups {
buckets: VecMap::new(),
#[cfg(feature = "simd-is-enabled")]
body_masks: Vec::new(),
#[cfg(feature = "simd-is-enabled")]
simd_interactions: Vec::new(),
nongrouped_interactions: Vec::new(),
}
@@ -258,8 +256,8 @@ impl InteractionGroups {
let rb1 = &bodies[interaction.body1];
let rb2 = &bodies[interaction.body2];
let is_fixed1 = !rb1.is_dynamic();
let is_fixed2 = !rb2.is_dynamic();
let is_fixed1 = !rb1.is_dynamic_or_kinematic();
let is_fixed2 = !rb2.is_dynamic_or_kinematic();
if is_fixed1 && is_fixed2 {
continue;
@@ -274,8 +272,17 @@ impl InteractionGroups {
let ijoint = interaction.data.locked_axes.bits() as usize;
let i1 = rb1.ids.active_set_offset;
let i2 = rb2.ids.active_set_offset;
let conflicts =
self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint];
let conflicts = self
.body_masks
.get(i1 as usize)
.copied()
.unwrap_or_default()
| self
.body_masks
.get(i2 as usize)
.copied()
.unwrap_or_default()
| joint_type_conflicts[ijoint];
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;
@@ -288,7 +295,7 @@ impl InteractionGroups {
if target_index == 128 {
// The interaction conflicts with every bucket we can manage.
// So push it in an nongrouped interaction list that won't be combined with
// So push it in a nongrouped interaction list that won't be combined with
// any other interactions.
self.nongrouped_interactions.push(*interaction_i);
continue;
@@ -327,11 +334,11 @@ impl InteractionGroups {
// NOTE: fixed bodies don't transmit forces. Therefore they don't
// imply any interaction conflicts.
if !is_fixed1 {
self.body_masks[i1] |= target_mask_bit;
self.body_masks[i1 as usize] |= target_mask_bit;
}
if !is_fixed2 {
self.body_masks[i2] |= target_mask_bit;
self.body_masks[i2 as usize] |= target_mask_bit;
}
}
@@ -356,7 +363,6 @@ impl InteractionGroups {
}
pub fn clear_groups(&mut self) {
#[cfg(feature = "simd-is-enabled")]
self.simd_interactions.clear();
self.nongrouped_interactions.clear();
}
@@ -419,18 +425,18 @@ impl InteractionGroups {
let rb1 = &bodies[rb1];
(rb1.body_type, rb1.ids.active_set_offset)
} else {
(RigidBodyType::Fixed, usize::MAX)
(RigidBodyType::Fixed, u32::MAX)
};
let (status2, active_set_offset2) = if let Some(rb2) = interaction.data.rigid_body2
{
let rb2 = &bodies[rb2];
(rb2.body_type, rb2.ids.active_set_offset)
} else {
(RigidBodyType::Fixed, usize::MAX)
(RigidBodyType::Fixed, u32::MAX)
};
let is_fixed1 = !status1.is_dynamic();
let is_fixed2 = !status2.is_dynamic();
let is_fixed1 = !status1.is_dynamic_or_kinematic();
let is_fixed2 = !status2.is_dynamic_or_kinematic();
// TODO: don't generate interactions between fixed bodies in the first place.
if is_fixed1 && is_fixed2 {
@@ -439,8 +445,16 @@ impl InteractionGroups {
let i1 = active_set_offset1;
let i2 = active_set_offset2;
let mask1 = if !is_fixed1 { self.body_masks[i1] } else { 0 };
let mask2 = if !is_fixed2 { self.body_masks[i2] } else { 0 };
let mask1 = if !is_fixed1 {
self.body_masks[i1 as usize]
} else {
0
};
let mask2 = if !is_fixed2 {
self.body_masks[i2 as usize]
} else {
0
};
let conflicts = mask1 | mask2;
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;
@@ -482,11 +496,11 @@ impl InteractionGroups {
// NOTE: fixed bodies don't transmit forces. Therefore they don't
// imply any interaction conflicts.
if !is_fixed1 {
self.body_masks[i1] |= target_mask_bit;
self.body_masks[i1 as usize] |= target_mask_bit;
}
if !is_fixed2 {
self.body_masks[i2] |= target_mask_bit;
self.body_masks[i2 as usize] |= target_mask_bit;
}
}

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

@@ -43,7 +43,7 @@ impl IslandSolver {
multibodies: &mut MultibodyJointSet,
) {
counters.solver.velocity_assembly_time.resume();
let num_solver_iterations = base_params.num_solver_iterations.get()
let num_solver_iterations = base_params.num_solver_iterations
+ islands.active_island_additional_solver_iterations(island_id);
let mut params = *base_params;
@@ -55,14 +55,18 @@ impl IslandSolver {
*
*/
// INIT
// let t0 = std::time::Instant::now();
self.velocity_solver
.init_solver_velocities_and_solver_bodies(
base_params.dt,
&params,
island_id,
islands,
bodies,
multibodies,
);
// let t_solver_body_init = t0.elapsed().as_secs_f32();
// let t0 = std::time::Instant::now();
self.velocity_solver.init_constraints(
island_id,
islands,
@@ -74,8 +78,16 @@ impl IslandSolver {
joint_indices,
&mut self.contact_constraints,
&mut self.joint_constraints,
#[cfg(feature = "dim3")]
params.friction_model,
);
// let t_init_constraints = t0.elapsed().as_secs_f32();
counters.solver.velocity_assembly_time.pause();
// println!(
// "Solver body init: {}, init constraints: {}",
// t_solver_body_init * 1000.0,
// t_init_constraints * 1000.0
// );
// SOLVE
counters.solver.velocity_resolution_time.resume();
@@ -93,14 +105,8 @@ impl IslandSolver {
counters.solver.velocity_writeback_time.resume();
self.joint_constraints.writeback_impulses(impulse_joints);
self.contact_constraints.writeback_impulses(manifolds);
self.velocity_solver.writeback_bodies(
base_params,
num_solver_iterations,
islands,
island_id,
bodies,
multibodies,
);
self.velocity_solver
.writeback_bodies(base_params, islands, island_id, bodies, multibodies);
counters.solver.velocity_writeback_time.pause();
}
}

89
src/dynamics/solver/joint_constraint/any_joint_constraint.rs
View File

@@ -1,97 +1,52 @@
use crate::dynamics::JointGraphEdge;
use crate::dynamics::solver::joint_constraint::joint_generic_constraint::{
JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint,
};
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointOneBodyConstraint, JointTwoBodyConstraint,
};
use crate::dynamics::solver::{AnyConstraintMut, ConstraintTypes};
use crate::dynamics::solver::joint_constraint::generic_joint_constraint::GenericJointConstraint;
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::JointConstraint;
use crate::math::Real;
use na::DVector;
use crate::dynamics::solver::joint_constraint::joint_generic_constraint_builder::{
JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder,
};
use crate::dynamics::solver::solver_vel::SolverVel;
#[cfg(feature = "simd-is-enabled")]
use crate::{
dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
},
math::{SIMD_WIDTH, SimdReal},
};
use crate::math::{SIMD_WIDTH, SimdReal};
use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
};
use crate::dynamics::solver::solver_body::SolverBodies;
pub struct JointConstraintTypes;
#[derive(Debug)]
pub enum AnyJointConstraintMut<'a> {
Generic(&'a mut GenericJointConstraint),
Rigid(&'a mut JointConstraint<Real, 1>),
#[cfg(feature = "simd-is-enabled")]
SimdRigid(&'a mut JointConstraint<SimdReal, SIMD_WIDTH>),
}
impl AnyConstraintMut<'_, JointConstraintTypes> {
impl AnyJointConstraintMut<'_> {
pub fn remove_bias(&mut self) {
match self {
Self::OneBody(c) => c.remove_bias_from_rhs(),
Self::TwoBodies(c) => c.remove_bias_from_rhs(),
Self::GenericOneBody(c) => c.remove_bias_from_rhs(),
Self::GenericTwoBodies(c) => c.remove_bias_from_rhs(),
Self::Rigid(c) => c.remove_bias_from_rhs(),
Self::Generic(c) => c.remove_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.remove_bias_from_rhs(),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.remove_bias_from_rhs(),
Self::SimdRigid(c) => c.remove_bias_from_rhs(),
}
}
pub fn solve(
&mut self,
generic_jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
solver_vels: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
match self {
Self::OneBody(c) => c.solve(solver_vels),
Self::TwoBodies(c) => c.solve(solver_vels),
Self::GenericOneBody(c) => c.solve(generic_jacobians, solver_vels, generic_solver_vels),
Self::GenericTwoBodies(c) => {
c.solve(generic_jacobians, solver_vels, generic_solver_vels)
}
Self::Rigid(c) => c.solve(solver_vels),
Self::Generic(c) => c.solve(generic_jacobians, solver_vels, generic_solver_vels),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.solve(solver_vels),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.solve(solver_vels),
Self::SimdRigid(c) => c.solve(solver_vels),
}
}
pub fn writeback_impulses(&mut self, joints_all: &mut [JointGraphEdge]) {
match self {
Self::OneBody(c) => c.writeback_impulses(joints_all),
Self::TwoBodies(c) => c.writeback_impulses(joints_all),
Self::GenericOneBody(c) => c.writeback_impulses(joints_all),
Self::GenericTwoBodies(c) => c.writeback_impulses(joints_all),
Self::Rigid(c) => c.writeback_impulses(joints_all),
Self::Generic(c) => c.writeback_impulses(joints_all),
#[cfg(feature = "simd-is-enabled")]
Self::SimdOneBody(c) => c.writeback_impulses(joints_all),
#[cfg(feature = "simd-is-enabled")]
Self::SimdTwoBodies(c) => c.writeback_impulses(joints_all),
Self::SimdRigid(c) => c.writeback_impulses(joints_all),
}
}
}
impl ConstraintTypes for JointConstraintTypes {
type OneBody = JointOneBodyConstraint<Real, 1>;
type TwoBodies = JointTwoBodyConstraint<Real, 1>;
type GenericOneBody = JointGenericOneBodyConstraint;
type GenericTwoBodies = JointGenericTwoBodyConstraint;
#[cfg(feature = "simd-is-enabled")]
type SimdOneBody = JointOneBodyConstraint<SimdReal, SIMD_WIDTH>;
#[cfg(feature = "simd-is-enabled")]
type SimdTwoBodies = JointTwoBodyConstraint<SimdReal, SIMD_WIDTH>;
type BuilderOneBody = JointOneBodyConstraintBuilder;
type BuilderTwoBodies = JointTwoBodyConstraintBuilder;
type GenericBuilderOneBody = JointGenericOneBodyConstraintBuilder;
type GenericBuilderTwoBodies = JointGenericTwoBodyConstraintBuilder;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderOneBody = JointOneBodyConstraintBuilderSimd;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderTwoBodies = JointTwoBodyConstraintBuilderSimd;
}

320
src/dynamics/solver/joint_constraint/generic_joint_constraint.rs Normal file
View File

@@ -0,0 +1,320 @@
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
use crate::dynamics::solver::joint_constraint::{JointConstraintHelper, JointSolverBody};
use crate::dynamics::solver::solver_body::SolverBodies;
use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex};
use crate::math::{DIM, Isometry, Real};
use crate::prelude::SPATIAL_DIM;
use na::{DVector, DVectorView, DVectorViewMut};
use super::LinkOrBodyRef;
#[derive(Debug, Copy, Clone)]
pub struct GenericJointConstraint {
pub is_rigid_body1: bool,
pub is_rigid_body2: bool,
pub solver_vel1: u32,
pub solver_vel2: u32,
pub ndofs1: usize,
pub j_id1: usize,
pub ndofs2: usize,
pub j_id2: usize,
pub joint_id: JointIndex,
pub impulse: Real,
pub impulse_bounds: [Real; 2],
pub inv_lhs: Real,
pub rhs: Real,
pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId,
}
impl Default for GenericJointConstraint {
fn default() -> Self {
GenericJointConstraint::invalid()
}
}
impl GenericJointConstraint {
pub fn invalid() -> Self {
Self {
is_rigid_body1: false,
is_rigid_body2: false,
solver_vel1: u32::MAX,
solver_vel2: u32::MAX,
ndofs1: usize::MAX,
j_id1: usize::MAX,
ndofs2: usize::MAX,
j_id2: usize::MAX,
joint_id: usize::MAX,
impulse: 0.0,
impulse_bounds: [-Real::MAX, Real::MAX],
inv_lhs: Real::MAX,
rhs: Real::MAX,
rhs_wo_bias: Real::MAX,
cfm_coeff: Real::MAX,
cfm_gain: Real::MAX,
writeback_id: WritebackId::Dof(0),
}
}
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &GenericJoint,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
out: &mut [Self],
) -> usize {
let mut len = 0;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits();
let limit_axes = joint.limit_axes.bits();
let builder = JointConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
&body2.world_com,
locked_axes,
);
let start = len;
for i in DIM..SPATIAL_DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_angular_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
// locked_ang_axes,
i,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
JointConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i - DIM,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_angular_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i - DIM,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
JointConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]);
len
}
fn wj_id1(&self) -> usize {
self.j_id1 + self.ndofs1
}
fn wj_id2(&self) -> usize {
self.j_id2 + self.ndofs2
}
fn solver_vel1<'a>(
&self,
solver_vels: &'a SolverBodies,
generic_solver_vels: &'a DVector<Real>,
) -> DVectorView<'a, Real> {
if self.solver_vel1 == u32::MAX {
generic_solver_vels.rows(0, 0) // empty
} else if self.is_rigid_body1 {
solver_vels.vels[self.solver_vel1 as usize].as_vector_slice()
} else {
generic_solver_vels.rows(self.solver_vel1 as usize, self.ndofs1)
}
}
fn solver_vel1_mut<'a>(
&self,
solver_vels: &'a mut SolverBodies,
generic_solver_vels: &'a mut DVector<Real>,
) -> DVectorViewMut<'a, Real> {
if self.solver_vel1 == u32::MAX {
generic_solver_vels.rows_mut(0, 0) // empty
} else if self.is_rigid_body1 {
solver_vels.vels[self.solver_vel1 as usize].as_vector_slice_mut()
} else {
generic_solver_vels.rows_mut(self.solver_vel1 as usize, self.ndofs1)
}
}
fn solver_vel2<'a>(
&self,
solver_vels: &'a SolverBodies,
generic_solver_vels: &'a DVector<Real>,
) -> DVectorView<'a, Real> {
if self.solver_vel2 == u32::MAX {
generic_solver_vels.rows(0, 0) // empty
} else if self.is_rigid_body2 {
solver_vels.vels[self.solver_vel2 as usize].as_vector_slice()
} else {
generic_solver_vels.rows(self.solver_vel2 as usize, self.ndofs2)
}
}
fn solver_vel2_mut<'a>(
&self,
solver_vels: &'a mut SolverBodies,
generic_solver_vels: &'a mut DVector<Real>,
) -> DVectorViewMut<'a, Real> {
if self.solver_vel2 == u32::MAX {
generic_solver_vels.rows_mut(0, 0) // empty
} else if self.is_rigid_body2 {
solver_vels.vels[self.solver_vel2 as usize].as_vector_slice_mut()
} else {
generic_solver_vels.rows_mut(self.solver_vel2 as usize, self.ndofs2)
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
solver_vels: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
let jacobians = jacobians.as_slice();
let solver_vel1 = self.solver_vel1(solver_vels, generic_solver_vels);
let j1 = DVectorView::from_slice(&jacobians[self.j_id1..], self.ndofs1);
let vel1 = j1.dot(&solver_vel1);
let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels);
let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2);
let vel2 = j2.dot(&solver_vel2);
let dvel = self.rhs + (vel2 - vel1);
let total_impulse = na::clamp(
self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
self.impulse_bounds[0],
self.impulse_bounds[1],
);
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
let mut solver_vel1 = self.solver_vel1_mut(solver_vels, generic_solver_vels);
let wj1 = DVectorView::from_slice(&jacobians[self.wj_id1()..], self.ndofs1);
solver_vel1.axpy(delta_impulse, &wj1, 1.0);
let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels);
let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
solver_vel2.axpy(-delta_impulse, &wj2, 1.0);
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
// TODO: we don’t support impulse writeback for internal constraints yet.
if self.joint_id != JointIndex::MAX {
let joint = &mut joints_all[self.joint_id].weight;
match self.writeback_id {
WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
}
}
}
pub fn remove_bias_from_rhs(&mut self) {
self.rhs = self.rhs_wo_bias;
}
}

749
src/dynamics/solver/joint_constraint/generic_joint_constraint_builder.rs Normal file
View File

@@ -0,0 +1,749 @@
use crate::dynamics::solver::MotorParameters;
use crate::dynamics::solver::joint_constraint::generic_joint_constraint::GenericJointConstraint;
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
use crate::dynamics::solver::joint_constraint::{JointConstraintHelper, JointSolverBody};
use crate::dynamics::{
GenericJoint, ImpulseJoint, IntegrationParameters, JointIndex, Multibody, MultibodyJointSet,
MultibodyLinkId, RigidBodySet,
};
use crate::math::{ANG_DIM, DIM, Real, SPATIAL_DIM, Vector};
use crate::utils;
use crate::utils::IndexMut2;
use na::{DVector, SVector};
use crate::dynamics::solver::ConstraintsCounts;
use crate::dynamics::solver::solver_body::SolverBodies;
#[cfg(feature = "dim3")]
use crate::utils::SimdAngularInertia;
#[cfg(feature = "dim2")]
use na::Vector1;
use parry::math::Isometry;
#[derive(Copy, Clone)]
enum LinkOrBody {
Link(MultibodyLinkId),
Body(u32),
Fixed,
}
#[derive(Copy, Clone)]
pub enum LinkOrBodyRef<'a> {
Link(&'a Multibody, usize),
Body(u32),
Fixed,
}
#[allow(clippy::large_enum_variant)]
#[derive(Copy, Clone)]
pub enum GenericJointConstraintBuilder {
Internal(JointGenericInternalConstraintBuilder),
External(JointGenericExternalConstraintBuilder),
Empty, // No constraint
}
#[derive(Copy, Clone)]
pub struct JointGenericExternalConstraintBuilder {
link1: LinkOrBody,
link2: LinkOrBody,
joint_id: JointIndex,
joint: GenericJoint,
j_id: usize,
// These are solver body for both joints, except that
// the world_com is actually in local-space.
local_body1: JointSolverBody<Real, 1>,
local_body2: JointSolverBody<Real, 1>,
multibodies_ndof: usize,
constraint_id: usize,
}
impl JointGenericExternalConstraintBuilder {
pub fn generate(
joint_id: JointIndex,
joint: &ImpulseJoint,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
out_builder: &mut GenericJointConstraintBuilder,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out_constraint_id: &mut usize,
) {
let starting_j_id = *j_id;
let rb1 = &bodies[joint.body1];
let rb2 = &bodies[joint.body2];
let solver_vel1 = rb1.effective_active_set_offset();
let solver_vel2 = rb2.effective_active_set_offset();
let local_body1 = JointSolverBody {
im: rb1.mprops.effective_inv_mass,
ii: rb1.mprops.effective_world_inv_inertia,
world_com: rb1.mprops.local_mprops.local_com,
solver_vel: [solver_vel1],
};
let local_body2 = JointSolverBody {
im: rb2.mprops.effective_inv_mass,
ii: rb2.mprops.effective_world_inv_inertia,
world_com: rb2.mprops.local_mprops.local_com,
solver_vel: [solver_vel2],
};
let mut multibodies_ndof = 0;
let link1 = if solver_vel1 == u32::MAX {
LinkOrBody::Fixed
} else if let Some(link) = multibodies.rigid_body_link(joint.body1) {
multibodies_ndof += multibodies[link.multibody].ndofs();
LinkOrBody::Link(*link)
} else {
// Dynamic rigid-body.
multibodies_ndof += SPATIAL_DIM;
LinkOrBody::Body(solver_vel1)
};
let link2 = if solver_vel2 == u32::MAX {
LinkOrBody::Fixed
} else if let Some(link) = multibodies.rigid_body_link(joint.body2) {
multibodies_ndof += multibodies[link.multibody].ndofs();
LinkOrBody::Link(*link)
} else {
// Dynamic rigid-body.
multibodies_ndof += SPATIAL_DIM;
LinkOrBody::Body(solver_vel2)
};
if multibodies_ndof == 0 {
return;
}
// For each solver contact we generate up to SPATIAL_DIM constraints, and each
// constraints appends the multibodies jacobian and weighted jacobians.
// Also note that for impulse_joints, the rigid-bodies will also add their jacobians
// to the generic DVector.
// TODO: is this count correct when we take both motors and limits into account?
let required_jacobian_len = *j_id + multibodies_ndof * 2 * SPATIAL_DIM;
// TODO: use a more precise increment.
*j_id += multibodies_ndof * 2 * SPATIAL_DIM;
if jacobians.nrows() < required_jacobian_len && !cfg!(feature = "parallel") {
jacobians.resize_vertically_mut(required_jacobian_len, 0.0);
}
let mut joint_data = joint.data;
joint_data.transform_to_solver_body_space(rb1, rb2);
*out_builder = GenericJointConstraintBuilder::External(Self {
link1,
link2,
joint_id,
joint: joint_data,
j_id: starting_j_id,
local_body1,
local_body2,
multibodies_ndof,
constraint_id: *out_constraint_id,
});
*out_constraint_id += ConstraintsCounts::from_joint(joint).num_constraints;
}
pub fn update(
&self,
params: &IntegrationParameters,
multibodies: &MultibodyJointSet,
bodies: &SolverBodies,
jacobians: &mut DVector<Real>,
out: &mut [GenericJointConstraint],
) {
if self.multibodies_ndof == 0 {
// The joint is between two static bodies, no constraint needed.
return;
}
// NOTE: right now, the "update", is basically reconstructing all the
// constraints. Could we make this more incremental?
let pos1;
let pos2;
let mb1;
let mb2;
match self.link1 {
LinkOrBody::Link(link) => {
let mb = &multibodies[link.multibody];
pos1 = mb.link(link.id).unwrap().local_to_world;
mb1 = LinkOrBodyRef::Link(mb, link.id);
}
LinkOrBody::Body(body1) => {
pos1 = bodies.get_pose(body1).pose;
mb1 = LinkOrBodyRef::Body(body1);
}
LinkOrBody::Fixed => {
pos1 = Isometry::identity();
mb1 = LinkOrBodyRef::Fixed;
}
};
match self.link2 {
LinkOrBody::Link(link) => {
let mb = &multibodies[link.multibody];
pos2 = mb.link(link.id).unwrap().local_to_world;
mb2 = LinkOrBodyRef::Link(mb, link.id);
}
LinkOrBody::Body(body2) => {
pos2 = bodies.get_pose(body2).pose;
mb2 = LinkOrBodyRef::Body(body2);
}
LinkOrBody::Fixed => {
pos2 = Isometry::identity();
mb2 = LinkOrBodyRef::Fixed;
}
};
let frame1 = pos1 * self.joint.local_frame1;
let frame2 = pos2 * self.joint.local_frame2;
let joint_body1 = JointSolverBody {
world_com: pos1.translation.vector.into(), // the solver body pose is at the center of mass.
..self.local_body1
};
let joint_body2 = JointSolverBody {
world_com: pos2.translation.vector.into(), // the solver body pose is at the center of mass.
..self.local_body2
};
let mut j_id = self.j_id;
GenericJointConstraint::lock_axes(
params,
self.joint_id,
&joint_body1,
&joint_body2,
mb1,
mb2,
&frame1,
&frame2,
&self.joint,
jacobians,
&mut j_id,
&mut out[self.constraint_id..],
);
}
}
#[derive(Copy, Clone)]
pub struct JointGenericInternalConstraintBuilder {
link: MultibodyLinkId,
j_id: usize,
constraint_id: usize,
}
impl JointGenericInternalConstraintBuilder {
pub fn num_constraints(multibodies: &MultibodyJointSet, link_id: &MultibodyLinkId) -> usize {
let multibody = &multibodies[link_id.multibody];
let link = multibody.link(link_id.id).unwrap();
link.joint().num_velocity_constraints()
}
pub fn generate(
multibodies: &MultibodyJointSet,
link_id: &MultibodyLinkId,
out_builder: &mut GenericJointConstraintBuilder,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
out_constraint_id: &mut usize,
) {
let multibody = &multibodies[link_id.multibody];
let link = multibody.link(link_id.id).unwrap();
let num_constraints = link.joint().num_velocity_constraints();
if num_constraints == 0 {
return;
}
*out_builder = GenericJointConstraintBuilder::Internal(Self {
link: *link_id,
j_id: *j_id,
constraint_id: *out_constraint_id,
});
*j_id += num_constraints * multibody.ndofs() * 2;
if jacobians.nrows() < *j_id {
jacobians.resize_vertically_mut(*j_id, 0.0);
}
*out_constraint_id += num_constraints;
}
pub fn update(
&self,
params: &IntegrationParameters,
multibodies: &MultibodyJointSet,
jacobians: &mut DVector<Real>,
out: &mut [GenericJointConstraint],
) {
let mb = &multibodies[self.link.multibody];
let link = mb.link(self.link.id).unwrap();
link.joint().velocity_constraints(
params,
mb,
link,
self.j_id,
jacobians,
&mut out[self.constraint_id..],
);
}
}
impl JointSolverBody<Real, 1> {
pub fn fill_jacobians(
&self,
unit_force: Vector<Real>,
unit_torque: SVector<Real, ANG_DIM>,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
) {
let wj_id = *j_id + SPATIAL_DIM;
jacobians
.fixed_rows_mut::<DIM>(*j_id)
.copy_from(&unit_force);
jacobians
.fixed_rows_mut::<ANG_DIM>(*j_id + DIM)
.copy_from(&unit_torque);
{
let mut out_invm_j = jacobians.fixed_rows_mut::<SPATIAL_DIM>(wj_id);
out_invm_j
.fixed_rows_mut::<DIM>(0)
.copy_from(&self.im.component_mul(&unit_force));
#[cfg(feature = "dim2")]
{
out_invm_j[DIM] *= self.ii;
}
#[cfg(feature = "dim3")]
{
out_invm_j.fixed_rows_mut::<ANG_DIM>(DIM).gemv(
1.0,
&self.ii.into_matrix(),
&unit_torque,
0.0,
);
}
}
*j_id += SPATIAL_DIM * 2;
}
}
impl JointConstraintHelper<Real> {
pub fn lock_jacobians_generic(
&self,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
writeback_id: WritebackId,
lin_jac: Vector<Real>,
ang_jac1: SVector<Real, ANG_DIM>,
ang_jac2: SVector<Real, ANG_DIM>,
) -> GenericJointConstraint {
let j_id1 = *j_id;
let (ndofs1, solver_vel1, is_rigid_body1) = match mb1 {
LinkOrBodyRef::Link(mb1, link_id1) => {
mb1.fill_jacobians(link_id1, lin_jac, ang_jac1, j_id, jacobians);
(mb1.ndofs(), mb1.solver_id, false)
}
LinkOrBodyRef::Body(_) => {
body1.fill_jacobians(lin_jac, ang_jac1, j_id, jacobians);
(SPATIAL_DIM, body1.solver_vel[0], true)
}
LinkOrBodyRef::Fixed => (0, u32::MAX, true),
};
let j_id2 = *j_id;
let (ndofs2, solver_vel2, is_rigid_body2) = match mb2 {
LinkOrBodyRef::Link(mb2, link_id2) => {
mb2.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians);
(mb2.ndofs(), mb2.solver_id, false)
}
LinkOrBodyRef::Body(_) => {
body2.fill_jacobians(lin_jac, ang_jac2, j_id, jacobians);
(SPATIAL_DIM, body2.solver_vel[0], true)
}
LinkOrBodyRef::Fixed => (0, u32::MAX, true),
};
let rhs_wo_bias = 0.0;
GenericJointConstraint {
is_rigid_body1,
is_rigid_body2,
solver_vel1,
solver_vel2,
ndofs1,
j_id1,
ndofs2,
j_id2,
joint_id,
impulse: 0.0,
impulse_bounds: [-Real::MAX, Real::MAX],
inv_lhs: 0.0,
rhs: rhs_wo_bias,
rhs_wo_bias,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id,
}
}
pub fn lock_linear_generic(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
locked_axis: usize,
writeback_id: WritebackId,
) -> GenericJointConstraint {
let lin_jac = self.basis.column(locked_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(locked_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(locked_axis).into_owned();
let mut c = self.lock_jacobians_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
writeback_id,
lin_jac,
ang_jac1,
ang_jac2,
);
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
c.rhs += rhs_bias;
c
}
pub fn limit_linear_generic(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
limited_axis: usize,
limits: [Real; 2],
writeback_id: WritebackId,
) -> GenericJointConstraint {
let lin_jac = self.basis.column(limited_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(limited_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
writeback_id,
lin_jac,
ang_jac1,
ang_jac2,
);
let dist = self.lin_err.dot(&lin_jac);
let min_enabled = dist <= limits[0];
let max_enabled = limits[1] <= dist;
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
constraint.rhs += rhs_bias;
constraint.impulse_bounds = [
min_enabled as u32 as Real * -Real::MAX,
max_enabled as u32 as Real * Real::MAX,
];
constraint
}
pub fn motor_linear_generic(
&self,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
motor_axis: usize,
motor_params: &MotorParameters<Real>,
writeback_id: WritebackId,
) -> GenericJointConstraint {
let lin_jac = self.basis.column(motor_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
let ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
// TODO: do we need the same trick as for the non-generic constraint?
// if locked_ang_axes & (1 << motor_axis) != 0 {
// // FIXME: check that this also works for cases
// // whene not all the angular axes are locked.
// constraint.ang_jac1.fill(0.0);
// constraint.ang_jac2.fill(0.0);
// }
let mut constraint = self.lock_jacobians_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
writeback_id,
lin_jac,
ang_jac1,
ang_jac2,
);
let mut rhs_wo_bias = 0.0;
if motor_params.erp_inv_dt != 0.0 {
let dist = self.lin_err.dot(&lin_jac);
rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt;
}
rhs_wo_bias += -motor_params.target_vel;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint
}
pub fn lock_angular_generic(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
_locked_axis: usize,
writeback_id: WritebackId,
) -> GenericJointConstraint {
#[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_locked_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
writeback_id,
na::zero(),
ang_jac,
ang_jac,
);
let erp_inv_dt = params.joint_erp_inv_dt();
#[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")]
let rhs_bias = self.ang_err.imag()[_locked_axis] * erp_inv_dt;
constraint.rhs += rhs_bias;
constraint
}
pub fn limit_angular_generic(
&self,
params: &IntegrationParameters,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
_limited_axis: usize,
limits: [Real; 2],
writeback_id: WritebackId,
) -> GenericJointConstraint {
#[cfg(feature = "dim2")]
let ang_jac = Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(_limited_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
writeback_id,
na::zero(),
ang_jac,
ang_jac,
);
let s_limits = [(limits[0] / 2.0).sin(), (limits[1] / 2.0).sin()];
#[cfg(feature = "dim2")]
let s_ang = (self.ang_err.angle() / 2.0).sin();
#[cfg(feature = "dim3")]
let s_ang = self.ang_err.imag()[_limited_axis];
let min_enabled = s_ang <= s_limits[0];
let max_enabled = s_limits[1] <= s_ang;
let impulse_bounds = [
min_enabled as u32 as Real * -Real::MAX,
max_enabled as u32 as Real * Real::MAX,
];
let erp_inv_dt = params.joint_erp_inv_dt();
let rhs_bias =
((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt;
constraint.rhs += rhs_bias;
constraint.impulse_bounds = impulse_bounds;
constraint
}
pub fn motor_angular_generic(
&self,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: LinkOrBodyRef,
mb2: LinkOrBodyRef,
_motor_axis: usize,
motor_params: &MotorParameters<Real>,
writeback_id: WritebackId,
) -> GenericJointConstraint {
#[cfg(feature = "dim2")]
let ang_jac = na::Vector1::new(1.0);
#[cfg(feature = "dim3")]
let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut constraint = self.lock_jacobians_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
writeback_id,
na::zero(),
ang_jac,
ang_jac,
);
let mut rhs_wo_bias = 0.0;
if motor_params.erp_inv_dt != 0.0 {
#[cfg(feature = "dim2")]
let s_ang_dist = (self.ang_err.angle() / 2.0).sin();
#[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = (motor_params.target_pos / 2.0).sin();
rhs_wo_bias += utils::smallest_abs_diff_between_sin_angles(s_ang_dist, s_target_ang)
* motor_params.erp_inv_dt;
}
rhs_wo_bias += -motor_params.target_vel;
constraint.rhs_wo_bias = rhs_wo_bias;
constraint.rhs = rhs_wo_bias;
constraint.cfm_coeff = motor_params.cfm_coeff;
constraint.cfm_gain = motor_params.cfm_gain;
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint
}
pub fn finalize_generic_constraints(
jacobians: &mut DVector<Real>,
constraints: &mut [GenericJointConstraint],
) {
// TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false;
let len = constraints.len();
if len == 0 {
return;
}
let ndofs1 = constraints[0].ndofs1;
let ndofs2 = constraints[0].ndofs2;
// Use the modified Gramm-Schmidt orthogonalization.
for j in 0..len {
let c_j = &mut constraints[j];
let jac_j1 = jacobians.rows(c_j.j_id1, ndofs1);
let jac_j2 = jacobians.rows(c_j.j_id2, ndofs2);
let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1);
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2);
let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
let inv_dot_jj = crate::utils::simd_inv(dot_jj);
c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs.
c_j.cfm_gain = cfm_gain;
if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
// Don't remove constraints with limited forces from the others
// because they may not deliver the necessary forces to fulfill
// the removed parts of other constraints.
continue;
}
if !ORTHOGONALIZE {
continue;
}
for i in (j + 1)..len {
let (c_i, c_j) = constraints.index_mut_const(i, j);
let jac_i1 = jacobians.rows(c_i.j_id1, ndofs1);
let jac_i2 = jacobians.rows(c_i.j_id2, ndofs2);
let w_jac_j1 = jacobians.rows(c_j.j_id1 + ndofs1, ndofs1);
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
let dot_ij = jac_i1.dot(&w_jac_j1) + jac_i2.dot(&w_jac_j2);
let coeff = dot_ij * inv_dot_jj;
let (mut jac_i, jac_j) = jacobians.rows_range_pair_mut(
c_i.j_id1..c_i.j_id1 + 2 * (ndofs1 + ndofs2),
c_j.j_id1..c_j.j_id1 + 2 * (ndofs1 + ndofs2),
);
jac_i.axpy(-coeff, &jac_j, 1.0);
c_i.rhs_wo_bias -= c_j.rhs_wo_bias * coeff;
c_i.rhs -= c_j.rhs * coeff;
}
}
}
}

961
src/dynamics/solver/joint_constraint/joint_constraint_builder.rs
View File

File diff suppressed because it is too large Load Diff

355
src/dynamics/solver/joint_constraint/joint_constraints_set.rs
View File

@@ -1,11 +1,7 @@
use crate::dynamics::solver::categorization::categorize_joints;
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::solver_vel::SolverVel;
use crate::dynamics::solver::{
JointConstraintTypes, JointGenericOneBodyConstraint, JointGenericOneBodyConstraintBuilder,
JointGenericTwoBodyConstraint, JointGenericTwoBodyConstraintBuilder,
JointGenericVelocityOneBodyExternalConstraintBuilder,
JointGenericVelocityOneBodyInternalConstraintBuilder, SolverConstraintsSet, reset_buffer,
AnyJointConstraintMut, GenericJointConstraint, JointGenericExternalConstraintBuilder,
JointGenericInternalConstraintBuilder, reset_buffer,
};
use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
@@ -14,18 +10,92 @@ use crate::dynamics::{
use na::DVector;
use parry::math::Real;
use crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilder, JointTwoBodyConstraintBuilder,
};
use crate::dynamics::solver::interaction_groups::InteractionGroups;
use crate::dynamics::solver::joint_constraint::generic_joint_constraint_builder::GenericJointConstraintBuilder;
use crate::dynamics::solver::joint_constraint::joint_constraint_builder::JointConstraintBuilder;
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::JointConstraint;
use crate::dynamics::solver::solver_body::SolverBodies;
#[cfg(feature = "simd-is-enabled")]
use {
crate::dynamics::solver::joint_constraint::joint_constraint_builder::{
JointOneBodyConstraintBuilderSimd, JointTwoBodyConstraintBuilderSimd,
},
crate::math::SIMD_WIDTH,
crate::dynamics::solver::joint_constraint::joint_constraint_builder::JointConstraintBuilderSimd,
crate::math::{SIMD_WIDTH, SimdReal},
};
pub type JointConstraintsSet = SolverConstraintsSet<JointConstraintTypes>;
pub struct JointConstraintsSet {
pub generic_jacobians: DVector<Real>,
pub two_body_interactions: Vec<usize>,
pub generic_two_body_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub generic_velocity_constraints: Vec<GenericJointConstraint>,
pub velocity_constraints: Vec<JointConstraint<Real, 1>>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_constraints: Vec<JointConstraint<SimdReal, SIMD_WIDTH>>,
pub generic_velocity_constraints_builder: Vec<GenericJointConstraintBuilder>,
pub velocity_constraints_builder: Vec<JointConstraintBuilder>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_constraints_builder: Vec<JointConstraintBuilderSimd>,
}
impl JointConstraintsSet {
pub fn new() -> Self {
Self {
generic_jacobians: DVector::zeros(0),
two_body_interactions: vec![],
generic_two_body_interactions: vec![],
interaction_groups: InteractionGroups::new(),
velocity_constraints: vec![],
generic_velocity_constraints: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_constraints: vec![],
velocity_constraints_builder: vec![],
generic_velocity_constraints_builder: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_constraints_builder: vec![],
}
}
pub fn clear_constraints(&mut self) {
self.generic_jacobians.fill(0.0);
self.generic_velocity_constraints.clear();
#[cfg(feature = "simd-is-enabled")]
self.simd_velocity_constraints.clear();
}
pub fn clear_builders(&mut self) {
self.generic_velocity_constraints_builder.clear();
#[cfg(feature = "simd-is-enabled")]
self.simd_velocity_constraints_builder.clear();
}
// Returns the generic jacobians and a mutable iterator through all the constraints.
pub fn iter_constraints_mut(
&mut self,
) -> (
&DVector<Real>,
impl Iterator<Item = AnyJointConstraintMut<'_>>,
) {
let jac = &self.generic_jacobians;
let a = self
.generic_velocity_constraints
.iter_mut()
.map(AnyJointConstraintMut::Generic);
let b = self
.velocity_constraints
.iter_mut()
.map(AnyJointConstraintMut::Rigid);
#[cfg(feature = "simd-is-enabled")]
let c = self
.simd_velocity_constraints
.iter_mut()
.map(AnyJointConstraintMut::SimdRigid);
#[cfg(not(feature = "simd-is-enabled"))]
return (jac, a.chain(b));
#[cfg(feature = "simd-is-enabled")]
return (jac, a.chain(b).chain(c));
}
}
impl JointConstraintsSet {
pub fn init(
@@ -39,18 +109,13 @@ impl JointConstraintsSet {
) {
// Generate constraints for impulse_joints.
self.two_body_interactions.clear();
self.one_body_interactions.clear();
self.generic_two_body_interactions.clear();
self.generic_one_body_interactions.clear();
categorize_joints(
bodies,
multibody_joints,
impulse_joints,
joint_constraint_indices,
&mut self.one_body_interactions,
&mut self.two_body_interactions,
&mut self.generic_one_body_interactions,
&mut self.generic_two_body_interactions,
);
@@ -66,21 +131,10 @@ impl JointConstraintsSet {
&self.two_body_interactions,
);
self.one_body_interaction_groups.clear_groups();
self.one_body_interaction_groups.group_joints(
island_id,
islands,
bodies,
impulse_joints,
&self.one_body_interactions,
);
// NOTE: uncomment this do disable SIMD joint resolution.
// self.interaction_groups
// .nongrouped_interactions
// .append(&mut self.interaction_groups.simd_interactions);
// self.one_body_interaction_groups
// .nongrouped_interactions
// .append(&mut self.one_body_interaction_groups.simd_interactions);
let mut j_id = 0;
self.compute_joint_constraints(bodies, impulse_joints);
@@ -88,14 +142,7 @@ impl JointConstraintsSet {
{
self.simd_compute_joint_constraints(bodies, impulse_joints);
}
self.compute_generic_joint_constraints(bodies, multibody_joints, impulse_joints, &mut j_id);
self.compute_joint_one_body_constraints(bodies, impulse_joints);
#[cfg(feature = "simd-is-enabled")]
{
self.simd_compute_joint_one_body_constraints(bodies, impulse_joints);
}
self.compute_generic_one_body_joint_constraints(
self.compute_generic_joint_constraints(
island_id,
islands,
bodies,
@@ -105,87 +152,6 @@ impl JointConstraintsSet {
);
}
fn compute_joint_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
let total_num_builders = self
.one_body_interaction_groups
.nongrouped_interactions
.len();
unsafe {
reset_buffer(
&mut self.velocity_one_body_constraints_builder,
total_num_builders,
);
}
let mut num_constraints = 0;
for (joint_i, builder) in self
.one_body_interaction_groups
.nongrouped_interactions
.iter()
.zip(self.velocity_one_body_constraints_builder.iter_mut())
{
let joint = &joints_all[*joint_i].weight;
JointOneBodyConstraintBuilder::generate(
joint,
bodies,
*joint_i,
builder,
&mut num_constraints,
);
}
unsafe {
reset_buffer(&mut self.velocity_one_body_constraints, num_constraints);
}
}
#[cfg(feature = "simd-is-enabled")]
fn simd_compute_joint_one_body_constraints(
&mut self,
bodies: &RigidBodySet,
joints_all: &[JointGraphEdge],
) {
let total_num_builders =
self.one_body_interaction_groups.simd_interactions.len() / SIMD_WIDTH;
unsafe {
reset_buffer(
&mut self.simd_velocity_one_body_constraints_builder,
total_num_builders,
);
}
let mut num_constraints = 0;
for (joints_i, builder) in self
.one_body_interaction_groups
.simd_interactions
.chunks_exact(SIMD_WIDTH)
.zip(self.simd_velocity_one_body_constraints_builder.iter_mut())
{
let joints_id = gather![|ii| joints_i[ii]];
let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
JointOneBodyConstraintBuilderSimd::generate(
impulse_joints,
bodies,
joints_id,
builder,
&mut num_constraints,
);
}
unsafe {
reset_buffer(
&mut self.simd_velocity_one_body_constraints,
num_constraints,
);
}
}
fn compute_joint_constraints(&mut self, bodies: &RigidBodySet, joints_all: &[JointGraphEdge]) {
let total_num_builders = self.interaction_groups.nongrouped_interactions.len();
@@ -201,7 +167,7 @@ impl JointConstraintsSet {
.zip(self.velocity_constraints_builder.iter_mut())
{
let joint = &joints_all[*joint_i].weight;
JointTwoBodyConstraintBuilder::generate(
JointConstraintBuilder::generate(
joint,
bodies,
*joint_i,
@@ -216,42 +182,6 @@ impl JointConstraintsSet {
}
fn compute_generic_joint_constraints(
&mut self,
bodies: &RigidBodySet,
multibodies: &MultibodyJointSet,
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) {
let total_num_builders = self.generic_two_body_interactions.len();
self.generic_velocity_constraints_builder.resize(
total_num_builders,
JointGenericTwoBodyConstraintBuilder::invalid(),
);
let mut num_constraints = 0;
for (joint_i, builder) in self
.generic_two_body_interactions
.iter()
.zip(self.generic_velocity_constraints_builder.iter_mut())
{
let joint = &joints_all[*joint_i].weight;
JointGenericTwoBodyConstraintBuilder::generate(
*joint_i,
joint,
bodies,
multibodies,
builder,
j_id,
&mut self.generic_jacobians,
&mut num_constraints,
);
}
self.generic_velocity_constraints
.resize(num_constraints, JointGenericTwoBodyConstraint::invalid());
}
fn compute_generic_one_body_joint_constraints(
&mut self,
island_id: usize,
islands: &IslandManager,
@@ -260,32 +190,33 @@ impl JointConstraintsSet {
joints_all: &[JointGraphEdge],
j_id: &mut usize,
) {
let mut total_num_builders = self.generic_one_body_interactions.len();
// Count the internal and external constraints builder.
let num_external_constraint_builders = self.generic_two_body_interactions.len();
let mut num_internal_constraint_builders = 0;
for handle in islands.active_island(island_id) {
if let Some(link_id) = multibodies.rigid_body_link(*handle) {
if JointGenericVelocityOneBodyInternalConstraintBuilder::num_constraints(
multibodies,
link_id,
) > 0
if JointGenericInternalConstraintBuilder::num_constraints(multibodies, link_id) > 0
{
total_num_builders += 1;
num_internal_constraint_builders += 1;
}
}
}
let total_num_builders =
num_external_constraint_builders + num_internal_constraint_builders;
self.generic_velocity_one_body_constraints_builder.resize(
total_num_builders,
JointGenericOneBodyConstraintBuilder::invalid(),
);
// Preallocate builders buffer.
self.generic_velocity_constraints_builder
.resize(total_num_builders, GenericJointConstraintBuilder::Empty);
// Generate external constraints builders.
let mut num_constraints = 0;
for (joint_i, builder) in self.generic_one_body_interactions.iter().zip(
self.generic_velocity_one_body_constraints_builder
.iter_mut(),
) {
for (joint_i, builder) in self
.generic_two_body_interactions
.iter()
.zip(self.generic_velocity_constraints_builder.iter_mut())
{
let joint = &joints_all[*joint_i].weight;
JointGenericVelocityOneBodyExternalConstraintBuilder::generate(
JointGenericExternalConstraintBuilder::generate(
*joint_i,
joint,
bodies,
@@ -297,18 +228,19 @@ impl JointConstraintsSet {
);
}
let mut curr_builder = self.generic_one_body_interactions.len();
// Generate internal constraints builder. They are indexed after the
let mut curr_builder = self.generic_two_body_interactions.len();
for handle in islands.active_island(island_id) {
if curr_builder >= self.generic_velocity_one_body_constraints_builder.len() {
if curr_builder >= self.generic_velocity_constraints_builder.len() {
break; // No more builder need to be generated.
}
if let Some(link_id) = multibodies.rigid_body_link(*handle) {
let prev_num_constraints = num_constraints;
JointGenericVelocityOneBodyInternalConstraintBuilder::generate(
JointGenericInternalConstraintBuilder::generate(
multibodies,
link_id,
&mut self.generic_velocity_one_body_constraints_builder[curr_builder],
&mut self.generic_velocity_constraints_builder[curr_builder],
j_id,
&mut self.generic_jacobians,
&mut num_constraints,
@@ -319,8 +251,9 @@ impl JointConstraintsSet {
}
}
self.generic_velocity_one_body_constraints
.resize(num_constraints, JointGenericOneBodyConstraint::invalid());
// Resize constraints buffer now that we know the total count.
self.generic_velocity_constraints
.resize(num_constraints, GenericJointConstraint::invalid());
}
#[cfg(feature = "simd-is-enabled")]
@@ -345,9 +278,9 @@ impl JointConstraintsSet {
.chunks_exact(SIMD_WIDTH)
.zip(self.simd_velocity_constraints_builder.iter_mut())
{
let joints_id = gather![|ii| joints_i[ii]];
let impulse_joints = gather![|ii| &joints_all[joints_i[ii]].weight];
JointTwoBodyConstraintBuilderSimd::generate(
let joints_id = array![|ii| joints_i[ii]];
let impulse_joints = array![|ii| &joints_all[joints_i[ii]].weight];
JointConstraintBuilderSimd::generate(
impulse_joints,
bodies,
joints_id,
@@ -364,7 +297,7 @@ impl JointConstraintsSet {
#[profiling::function]
pub fn solve(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solver_vels: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
@@ -375,7 +308,7 @@ impl JointConstraintsSet {
pub fn solve_wo_bias(
&mut self,
solver_vels: &mut [SolverVel<Real>],
solver_vels: &mut SolverBodies,
generic_solver_vels: &mut DVector<Real>,
) {
let (jac, constraints) = self.iter_constraints_mut();
@@ -397,26 +330,29 @@ impl JointConstraintsSet {
&mut self,
params: &IntegrationParameters,
multibodies: &MultibodyJointSet,
solver_bodies: &[SolverBody],
solver_bodies: &SolverBodies,
) {
for builder in &mut self.generic_velocity_constraints_builder {
builder.update(
params,
multibodies,
solver_bodies,
&mut self.generic_jacobians,
&mut self.generic_velocity_constraints,
);
}
for builder in &mut self.generic_velocity_one_body_constraints_builder {
builder.update(
params,
multibodies,
solver_bodies,
&mut self.generic_jacobians,
&mut self.generic_velocity_one_body_constraints,
);
match builder {
GenericJointConstraintBuilder::External(builder) => {
builder.update(
params,
multibodies,
solver_bodies,
&mut self.generic_jacobians,
&mut self.generic_velocity_constraints,
);
}
GenericJointConstraintBuilder::Internal(builder) => {
builder.update(
params,
multibodies,
&mut self.generic_jacobians,
&mut self.generic_velocity_constraints,
);
}
GenericJointConstraintBuilder::Empty => {}
}
}
for builder in &mut self.velocity_constraints_builder {
@@ -427,22 +363,5 @@ impl JointConstraintsSet {
for builder in &mut self.simd_velocity_constraints_builder {
builder.update(params, solver_bodies, &mut self.simd_velocity_constraints);
}
for builder in &mut self.velocity_one_body_constraints_builder {
builder.update(
params,
solver_bodies,
&mut self.velocity_one_body_constraints,
);
}
#[cfg(feature = "simd-is-enabled")]
for builder in &mut self.simd_velocity_one_body_constraints_builder {
builder.update(
params,
solver_bodies,
&mut self.simd_velocity_one_body_constraints,
);
}
}
}

552
src/dynamics/solver/joint_constraint/joint_generic_constraint.rs
View File

@@ -1,552 +0,0 @@
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
JointFixedSolverBody, WritebackId,
};
use crate::dynamics::solver::joint_constraint::{JointSolverBody, JointTwoBodyConstraintHelper};
use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody};
use crate::math::{DIM, Isometry, Real};
use crate::prelude::SPATIAL_DIM;
use na::{DVector, DVectorView, DVectorViewMut};
#[derive(Debug, Copy, Clone)]
pub struct JointGenericTwoBodyConstraint {
pub is_rigid_body1: bool,
pub is_rigid_body2: bool,
pub solver_vel1: usize,
pub solver_vel2: usize,
pub ndofs1: usize,
pub j_id1: usize,
pub ndofs2: usize,
pub j_id2: usize,
pub joint_id: JointIndex,
pub impulse: Real,
pub impulse_bounds: [Real; 2],
pub inv_lhs: Real,
pub rhs: Real,
pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId,
}
impl Default for JointGenericTwoBodyConstraint {
fn default() -> Self {
JointGenericTwoBodyConstraint::invalid()
}
}
impl JointGenericTwoBodyConstraint {
pub fn invalid() -> Self {
Self {
is_rigid_body1: false,
is_rigid_body2: false,
solver_vel1: usize::MAX,
solver_vel2: usize::MAX,
ndofs1: usize::MAX,
j_id1: usize::MAX,
ndofs2: usize::MAX,
j_id2: usize::MAX,
joint_id: usize::MAX,
impulse: 0.0,
impulse_bounds: [-Real::MAX, Real::MAX],
inv_lhs: Real::MAX,
rhs: Real::MAX,
rhs_wo_bias: Real::MAX,
cfm_coeff: Real::MAX,
cfm_gain: Real::MAX,
writeback_id: WritebackId::Dof(0),
}
}
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
body2: &JointSolverBody<Real, 1>,
mb1: Option<(&Multibody, usize)>,
mb2: Option<(&Multibody, usize)>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &GenericJoint,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
out: &mut [Self],
) -> usize {
let mut len = 0;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits();
let limit_axes = joint.limit_axes.bits();
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
&body2.world_com,
locked_axes,
);
let start = len;
for i in DIM..SPATIAL_DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_angular_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_generic(
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
// locked_ang_axes,
i,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i - DIM,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_angular_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i - DIM,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic(
params,
jacobians,
j_id,
joint_id,
body1,
body2,
mb1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
JointTwoBodyConstraintHelper::finalize_generic_constraints(jacobians, &mut out[start..len]);
len
}
fn wj_id1(&self) -> usize {
self.j_id1 + self.ndofs1
}
fn wj_id2(&self) -> usize {
self.j_id2 + self.ndofs2
}
fn solver_vel1<'a>(
&self,
solver_vels: &'a [SolverVel<Real>],
generic_solver_vels: &'a DVector<Real>,
) -> DVectorView<'a, Real> {
if self.is_rigid_body1 {
solver_vels[self.solver_vel1].as_vector_slice()
} else {
generic_solver_vels.rows(self.solver_vel1, self.ndofs1)
}
}
fn solver_vel1_mut<'a>(
&self,
solver_vels: &'a mut [SolverVel<Real>],
generic_solver_vels: &'a mut DVector<Real>,
) -> DVectorViewMut<'a, Real> {
if self.is_rigid_body1 {
solver_vels[self.solver_vel1].as_vector_slice_mut()
} else {
generic_solver_vels.rows_mut(self.solver_vel1, self.ndofs1)
}
}
fn solver_vel2<'a>(
&self,
solver_vels: &'a [SolverVel<Real>],
generic_solver_vels: &'a DVector<Real>,
) -> DVectorView<'a, Real> {
if self.is_rigid_body2 {
solver_vels[self.solver_vel2].as_vector_slice()
} else {
generic_solver_vels.rows(self.solver_vel2, self.ndofs2)
}
}
fn solver_vel2_mut<'a>(
&self,
solver_vels: &'a mut [SolverVel<Real>],
generic_solver_vels: &'a mut DVector<Real>,
) -> DVectorViewMut<'a, Real> {
if self.is_rigid_body2 {
solver_vels[self.solver_vel2].as_vector_slice_mut()
} else {
generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2)
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let jacobians = jacobians.as_slice();
let solver_vel1 = self.solver_vel1(solver_vels, generic_solver_vels);
let j1 = DVectorView::from_slice(&jacobians[self.j_id1..], self.ndofs1);
let vel1 = j1.dot(&solver_vel1);
let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels);
let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2);
let vel2 = j2.dot(&solver_vel2);
let dvel = self.rhs + (vel2 - vel1);
let total_impulse = na::clamp(
self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
self.impulse_bounds[0],
self.impulse_bounds[1],
);
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
let mut solver_vel1 = self.solver_vel1_mut(solver_vels, generic_solver_vels);
let wj1 = DVectorView::from_slice(&jacobians[self.wj_id1()..], self.ndofs1);
solver_vel1.axpy(delta_impulse, &wj1, 1.0);
let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels);
let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
solver_vel2.axpy(-delta_impulse, &wj2, 1.0);
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
let joint = &mut joints_all[self.joint_id].weight;
match self.writeback_id {
WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
}
}
pub fn remove_bias_from_rhs(&mut self) {
self.rhs = self.rhs_wo_bias;
}
}
#[derive(Debug, Copy, Clone)]
pub struct JointGenericOneBodyConstraint {
pub solver_vel2: usize,
pub ndofs2: usize,
pub j_id2: usize,
pub joint_id: JointIndex,
pub impulse: Real,
pub impulse_bounds: [Real; 2],
pub inv_lhs: Real,
pub rhs: Real,
pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId,
}
impl Default for JointGenericOneBodyConstraint {
fn default() -> Self {
JointGenericOneBodyConstraint::invalid()
}
}
impl JointGenericOneBodyConstraint {
pub fn invalid() -> Self {
Self {
solver_vel2: crate::INVALID_USIZE,
ndofs2: 0,
j_id2: crate::INVALID_USIZE,
joint_id: 0,
impulse: 0.0,
impulse_bounds: [-Real::MAX, Real::MAX],
inv_lhs: 0.0,
rhs: 0.0,
rhs_wo_bias: 0.0,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id: WritebackId::Dof(0),
}
}
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &JointFixedSolverBody<Real>,
body2: &JointSolverBody<Real, 1>,
mb2: (&Multibody, usize),
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &GenericJoint,
jacobians: &mut DVector<Real>,
j_id: &mut usize,
out: &mut [Self],
) -> usize {
let mut len = 0;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits();
let limit_axes = joint.limit_axes.bits();
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
&body2.world_com,
locked_axes,
);
let start = len;
for i in DIM..SPATIAL_DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_angular_generic_one_body(
jacobians,
j_id,
joint_id,
body1,
mb2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_generic_one_body(
jacobians,
j_id,
joint_id,
body1,
mb2,
// locked_ang_axes,
i,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body(
jacobians,
&mut out[start..len],
);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_generic_one_body(
params,
jacobians,
j_id,
joint_id,
body1,
mb2,
i - DIM,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_generic_one_body(
params,
jacobians,
j_id,
joint_id,
body1,
mb2,
i,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_angular_generic_one_body(
params,
jacobians,
j_id,
joint_id,
body1,
mb2,
i - DIM,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
for i in 0..DIM {
if limit_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic_one_body(
params,
jacobians,
j_id,
joint_id,
body1,
mb2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
JointTwoBodyConstraintHelper::finalize_generic_constraints_one_body(
jacobians,
&mut out[start..len],
);
len
}
fn wj_id2(&self) -> usize {
self.j_id2 + self.ndofs2
}
fn solver_vel2<'a>(
&self,
_solver_vels: &'a [SolverVel<Real>],
generic_solver_vels: &'a DVector<Real>,
) -> DVectorView<'a, Real> {
generic_solver_vels.rows(self.solver_vel2, self.ndofs2)
}
fn solver_vel2_mut<'a>(
&self,
_solver_vels: &'a mut [SolverVel<Real>],
generic_solver_vels: &'a mut DVector<Real>,
) -> DVectorViewMut<'a, Real> {
generic_solver_vels.rows_mut(self.solver_vel2, self.ndofs2)
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
solver_vels: &mut [SolverVel<Real>],
generic_solver_vels: &mut DVector<Real>,
) {
let jacobians = jacobians.as_slice();
let solver_vel2 = self.solver_vel2(solver_vels, generic_solver_vels);
let j2 = DVectorView::from_slice(&jacobians[self.j_id2..], self.ndofs2);
let vel2 = j2.dot(&solver_vel2);
let dvel = self.rhs + vel2;
let total_impulse = na::clamp(
self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse),
self.impulse_bounds[0],
self.impulse_bounds[1],
);
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
let mut solver_vel2 = self.solver_vel2_mut(solver_vels, generic_solver_vels);
let wj2 = DVectorView::from_slice(&jacobians[self.wj_id2()..], self.ndofs2);
solver_vel2.axpy(-delta_impulse, &wj2, 1.0);
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
// FIXME: impulse writeback isn’t supported yet for internal multibody_joint constraints.
if self.joint_id != usize::MAX {
let joint = &mut joints_all[self.joint_id].weight;
match self.writeback_id {
WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
}
}
}
pub fn remove_bias_from_rhs(&mut self) {
self.rhs = self.rhs_wo_bias;
}
}

1279
src/dynamics/solver/joint_constraint/joint_generic_constraint_builder.rs
View File

File diff suppressed because it is too large Load Diff

446
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -1,17 +1,16 @@
use crate::dynamics::solver::SolverVel;
use crate::dynamics::solver::joint_constraint::JointTwoBodyConstraintHelper;
use crate::dynamics::solver::joint_constraint::JointConstraintHelper;
use crate::dynamics::{
GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex,
};
use crate::math::{AngVector, AngularInertia, DIM, Isometry, Point, Real, SPATIAL_DIM, Vector};
use crate::num::Zero;
use crate::utils::{SimdDot, SimdRealCopy};
use crate::dynamics::solver::solver_body::SolverBodies;
#[cfg(feature = "simd-is-enabled")]
use {
crate::math::{SIMD_WIDTH, SimdReal},
na::SimdValue,
};
use crate::math::{SIMD_WIDTH, SimdReal};
#[cfg(feature = "dim2")]
use crate::num::Zero;
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct MotorParameters<N: SimdRealCopy> {
@@ -50,44 +49,26 @@ pub enum WritebackId {
#[derive(Copy, Clone)]
pub struct JointSolverBody<N: SimdRealCopy, const LANES: usize> {
pub im: Vector<N>,
pub sqrt_ii: AngularInertia<N>,
pub world_com: Point<N>,
pub solver_vel: [usize; LANES],
pub ii: AngularInertia<N>,
pub world_com: Point<N>, // TODO: is this still needed now that the solver body poses are expressed at the center of mass?
pub solver_vel: [u32; LANES],
}
impl<N: SimdRealCopy, const LANES: usize> JointSolverBody<N, LANES> {
pub fn invalid() -> Self {
Self {
im: Vector::zeros(),
sqrt_ii: AngularInertia::zero(),
world_com: Point::origin(),
solver_vel: [usize::MAX; LANES],
}
}
}
#[derive(Copy, Clone)]
pub struct JointFixedSolverBody<N: SimdRealCopy> {
pub linvel: Vector<N>,
pub angvel: AngVector<N>,
// TODO: is this really needed?
pub world_com: Point<N>,
}
impl<N: SimdRealCopy> JointFixedSolverBody<N> {
pub fn invalid() -> Self {
Self {
linvel: Vector::zeros(),
angvel: AngVector::zero(),
ii: AngularInertia::zero(),
world_com: Point::origin(),
solver_vel: [u32::MAX; LANES],
}
}
}
#[derive(Debug, Copy, Clone)]
pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
pub solver_vel1: [usize; LANES],
pub solver_vel2: [usize; LANES],
pub struct JointConstraint<N: SimdRealCopy, const LANES: usize> {
pub solver_vel1: [u32; LANES],
pub solver_vel2: [u32; LANES],
pub joint_id: [JointIndex; LANES],
@@ -97,6 +78,9 @@ pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
pub ang_jac1: AngVector<N>,
pub ang_jac2: AngVector<N>,
pub ii_ang_jac1: AngVector<N>,
pub ii_ang_jac2: AngVector<N>,
pub inv_lhs: N,
pub rhs: N,
pub rhs_wo_bias: N,
@@ -109,7 +93,7 @@ pub struct JointTwoBodyConstraint<N: SimdRealCopy, const LANES: usize> {
pub writeback_id: WritebackId,
}
impl<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
impl<N: SimdRealCopy, const LANES: usize> JointConstraint<N, LANES> {
#[profiling::function]
pub fn solve_generic(
&mut self,
@@ -127,13 +111,13 @@ impl<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
self.impulse = total_impulse;
let lin_impulse = self.lin_jac * delta_impulse;
let ang_impulse1 = self.ang_jac1 * delta_impulse;
let ang_impulse2 = self.ang_jac2 * delta_impulse;
let ii_ang_impulse1 = self.ii_ang_jac1 * delta_impulse;
let ii_ang_impulse2 = self.ii_ang_jac2 * delta_impulse;
solver_vel1.linear += lin_impulse.component_mul(&self.im1);
solver_vel1.angular += ang_impulse1;
solver_vel1.angular += ii_ang_impulse1;
solver_vel2.linear -= lin_impulse.component_mul(&self.im2);
solver_vel2.angular -= ang_impulse2;
solver_vel2.angular -= ii_ang_impulse2;
}
pub fn remove_bias_from_rhs(&mut self) {
@@ -141,8 +125,8 @@ impl<N: SimdRealCopy, const LANES: usize> JointTwoBodyConstraint<N, LANES> {
}
}
impl JointTwoBodyConstraint<Real, 1> {
pub fn lock_axes(
impl JointConstraint<Real, 1> {
pub fn update(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &JointSolverBody<Real, 1>,
@@ -169,7 +153,7 @@ impl JointTwoBodyConstraint<Real, 1> {
let first_coupled_ang_axis_id =
(coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize;
let builder = JointTwoBodyConstraintHelper::new(
let builder = JointConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
@@ -240,7 +224,7 @@ impl JointTwoBodyConstraint<Real, 1> {
len += 1;
}
JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
JointConstraintHelper::finalize_constraints(&mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
@@ -325,19 +309,19 @@ impl JointTwoBodyConstraint<Real, 1> {
);
len += 1;
}
JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]);
JointConstraintHelper::finalize_constraints(&mut out[start..len]);
len
}
pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = solver_vels[self.solver_vel1[0]];
let mut solver_vel2 = solver_vels[self.solver_vel2[0]];
pub fn solve(&mut self, solver_vels: &mut SolverBodies) {
let mut solver_vel1 = solver_vels.get_vel(self.solver_vel1[0]);
let mut solver_vel2 = solver_vels.get_vel(self.solver_vel2[0]);
self.solve_generic(&mut solver_vel1, &mut solver_vel2);
solver_vels[self.solver_vel1[0]] = solver_vel1;
solver_vels[self.solver_vel2[0]] = solver_vel2;
solver_vels.set_vel(self.solver_vel1[0], solver_vel1);
solver_vels.set_vel(self.solver_vel2[0], solver_vel2);
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
@@ -351,8 +335,8 @@ impl JointTwoBodyConstraint<Real, 1> {
}
#[cfg(feature = "simd-is-enabled")]
impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
pub fn lock_axes(
impl JointConstraint<SimdReal, SIMD_WIDTH> {
pub fn update(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
body1: &JointSolverBody<SimdReal, SIMD_WIDTH>,
@@ -362,7 +346,7 @@ impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
locked_axes: u8,
out: &mut [Self],
) -> usize {
let builder = JointTwoBodyConstraintHelper::new(
let builder = JointConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
@@ -393,372 +377,24 @@ impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> {
}
}
JointTwoBodyConstraintHelper::finalize_constraints(&mut out[..len]);
JointConstraintHelper::finalize_constraints(&mut out[..len]);
len
}
pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel1 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel1[ii]].angular]),
};
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
pub fn solve(&mut self, solver_vels: &mut SolverBodies) {
let mut solver_vel1 = solver_vels.gather_vels(self.solver_vel1);
let mut solver_vel2 = solver_vels.gather_vels(self.solver_vel2);
self.solve_generic(&mut solver_vel1, &mut solver_vel2);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel1[ii]].linear = solver_vel1.linear.extract(ii);
solver_vels[self.solver_vel1[ii]].angular = solver_vel1.angular.extract(ii);
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
solver_vels.scatter_vels(self.solver_vel1, solver_vel1);
solver_vels.scatter_vels(self.solver_vel2, solver_vel2);
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
let impulses: [_; SIMD_WIDTH] = self.impulse.into();
// TODO: should we move the iteration on ii deeper in the mested match?
for ii in 0..SIMD_WIDTH {
let joint = &mut joints_all[self.joint_id[ii]].weight;
match self.writeback_id {
WritebackId::Dof(i) => joint.impulses[i] = impulses[ii],
WritebackId::Limit(i) => joint.data.limits[i].impulse = impulses[ii],
WritebackId::Motor(i) => joint.data.motors[i].impulse = impulses[ii],
}
}
}
}
#[derive(Debug, Copy, Clone)]
pub struct JointOneBodyConstraint<N: SimdRealCopy, const LANES: usize> {
pub solver_vel2: [usize; LANES],
pub joint_id: [JointIndex; LANES],
pub impulse: N,
pub impulse_bounds: [N; 2],
pub lin_jac: Vector<N>,
pub ang_jac2: AngVector<N>,
pub inv_lhs: N,
pub cfm_coeff: N,
pub cfm_gain: N,
pub rhs: N,
pub rhs_wo_bias: N,
pub im2: Vector<N>,
pub writeback_id: WritebackId,
}
impl<N: SimdRealCopy, const LANES: usize> JointOneBodyConstraint<N, LANES> {
pub fn solve_generic(&mut self, solver_vel2: &mut SolverVel<N>) {
let dlinvel = solver_vel2.linear;
let dangvel = solver_vel2.angular;
let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs;
let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse))
.simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse;
let lin_impulse = self.lin_jac * delta_impulse;
let ang_impulse = self.ang_jac2 * delta_impulse;
solver_vel2.linear -= lin_impulse.component_mul(&self.im2);
solver_vel2.angular -= ang_impulse;
}
pub fn remove_bias_from_rhs(&mut self) {
self.rhs = self.rhs_wo_bias;
}
}
impl JointOneBodyConstraint<Real, 1> {
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: JointIndex,
body1: &JointFixedSolverBody<Real>,
body2: &JointSolverBody<Real, 1>,
frame1: &Isometry<Real>,
frame2: &Isometry<Real>,
joint: &GenericJoint,
out: &mut [Self],
) -> usize {
let mut len = 0;
let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
// The has_lin/ang_coupling test is needed to avoid shl overflow later.
let has_lin_coupling = (coupled_axes & JointAxesMask::LIN_AXES.bits()) != 0;
let first_coupled_lin_axis_id =
(coupled_axes & JointAxesMask::LIN_AXES.bits()).trailing_zeros() as usize;
#[cfg(feature = "dim3")]
let has_ang_coupling = (coupled_axes & JointAxesMask::ANG_AXES.bits()) != 0;
#[cfg(feature = "dim3")]
let first_coupled_ang_axis_id =
(coupled_axes & JointAxesMask::ANG_AXES.bits()).trailing_zeros() as usize;
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
&body2.world_com,
locked_axes,
);
let start = len;
for i in DIM..SPATIAL_DIM {
if (motor_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.motor_angular_one_body(
[joint_id],
body1,
body2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1;
}
}
for i in 0..DIM {
if (motor_axes & !coupled_axes) & (1 << i) != 0 {
let limits = if limit_axes & (1 << i) != 0 {
Some([joint.limits[i].min, joint.limits[i].max])
} else {
None
};
out[len] = builder.motor_linear_one_body(
params,
[joint_id],
body1,
body2,
i,
&joint.motors[i].motor_params(params.dt),
limits,
WritebackId::Motor(i),
);
len += 1;
}
}
#[cfg(feature = "dim3")]
if has_ang_coupling && (motor_axes & (1 << first_coupled_ang_axis_id)) != 0 {
// TODO: coupled angular motor constraint.
}
if has_lin_coupling && (motor_axes & (1 << first_coupled_lin_axis_id)) != 0 {
let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
Some([
joint.limits[first_coupled_lin_axis_id].min,
joint.limits[first_coupled_lin_axis_id].max,
])
} else {
None
};
out[len] = builder.motor_linear_coupled_one_body(
params,
[joint_id],
body1,
body2,
coupled_axes,
&joint.motors[first_coupled_lin_axis_id].motor_params(params.dt),
limits,
WritebackId::Motor(first_coupled_lin_axis_id),
);
len += 1;
}
JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]);
let start = len;
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_one_body(
params,
[joint_id],
body1,
body2,
i - DIM,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_one_body(
params,
[joint_id],
body1,
body2,
i,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_angular_one_body(
params,
[joint_id],
body1,
body2,
i - DIM,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
for i in 0..DIM {
if (limit_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.limit_linear_one_body(
params,
[joint_id],
body1,
body2,
i,
[joint.limits[i].min, joint.limits[i].max],
WritebackId::Limit(i),
);
len += 1;
}
}
#[cfg(feature = "dim3")]
if has_ang_coupling && (limit_axes & (1 << first_coupled_ang_axis_id)) != 0 {
out[len] = builder.limit_angular_coupled_one_body(
params,
[joint_id],
body1,
body2,
coupled_axes,
[
joint.limits[first_coupled_ang_axis_id].min,
joint.limits[first_coupled_ang_axis_id].max,
],
WritebackId::Limit(first_coupled_ang_axis_id),
);
len += 1;
}
if has_lin_coupling && (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 {
out[len] = builder.limit_linear_coupled_one_body(
params,
[joint_id],
body1,
body2,
coupled_axes,
[
joint.limits[first_coupled_lin_axis_id].min,
joint.limits[first_coupled_lin_axis_id].max,
],
WritebackId::Limit(first_coupled_lin_axis_id),
);
len += 1;
}
JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[start..len]);
len
}
pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = solver_vels[self.solver_vel2[0]];
self.solve_generic(&mut solver_vel2);
solver_vels[self.solver_vel2[0]] = solver_vel2;
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
let joint = &mut joints_all[self.joint_id[0]].weight;
match self.writeback_id {
WritebackId::Dof(i) => joint.impulses[i] = self.impulse,
WritebackId::Limit(i) => joint.data.limits[i].impulse = self.impulse,
WritebackId::Motor(i) => joint.data.motors[i].impulse = self.impulse,
}
}
}
#[cfg(feature = "simd-is-enabled")]
impl JointOneBodyConstraint<SimdReal, SIMD_WIDTH> {
pub fn lock_axes(
params: &IntegrationParameters,
joint_id: [JointIndex; SIMD_WIDTH],
body1: &JointFixedSolverBody<SimdReal>,
body2: &JointSolverBody<SimdReal, SIMD_WIDTH>,
frame1: &Isometry<SimdReal>,
frame2: &Isometry<SimdReal>,
locked_axes: u8,
out: &mut [Self],
) -> usize {
let mut len = 0;
let builder = JointTwoBodyConstraintHelper::new(
frame1,
frame2,
&body1.world_com,
&body2.world_com,
locked_axes,
);
for i in 0..DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_linear_one_body(
params,
joint_id,
body1,
body2,
i,
WritebackId::Dof(i),
);
len += 1;
}
}
for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_one_body(
params,
joint_id,
body1,
body2,
i - DIM,
WritebackId::Dof(i),
);
len += 1;
}
}
JointTwoBodyConstraintHelper::finalize_one_body_constraints(&mut out[..len]);
len
}
pub fn solve(&mut self, solver_vels: &mut [SolverVel<Real>]) {
let mut solver_vel2 = SolverVel {
linear: Vector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].linear]),
angular: AngVector::from(gather![|ii| solver_vels[self.solver_vel2[ii]].angular]),
};
self.solve_generic(&mut solver_vel2);
for ii in 0..SIMD_WIDTH {
solver_vels[self.solver_vel2[ii]].linear = solver_vel2.linear.extract(ii);
solver_vels[self.solver_vel2[ii]].angular = solver_vel2.angular.extract(ii);
}
}
pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) {
let impulses: [_; SIMD_WIDTH] = self.impulse.into();
// TODO: should we move the iteration on ii deeper in the mested match?
// TODO: should we move the iteration on ii deeper in the nested match?
for ii in 0..SIMD_WIDTH {
let joint = &mut joints_all[self.joint_id[ii]].weight;
match self.writeback_id {

18
src/dynamics/solver/joint_constraint/mod.rs
View File

@@ -1,18 +1,16 @@
pub use joint_velocity_constraint::{JointSolverBody, MotorParameters, WritebackId};
pub use any_joint_constraint::JointConstraintTypes;
pub use joint_constraint_builder::JointTwoBodyConstraintHelper;
pub use joint_constraints_set::JointConstraintsSet;
pub use joint_generic_constraint::{JointGenericOneBodyConstraint, JointGenericTwoBodyConstraint};
pub use joint_generic_constraint_builder::{
JointGenericOneBodyConstraintBuilder, JointGenericTwoBodyConstraintBuilder,
JointGenericVelocityOneBodyExternalConstraintBuilder,
JointGenericVelocityOneBodyInternalConstraintBuilder,
pub use any_joint_constraint::AnyJointConstraintMut;
pub use generic_joint_constraint::GenericJointConstraint;
pub use generic_joint_constraint_builder::{
JointGenericExternalConstraintBuilder, JointGenericInternalConstraintBuilder, LinkOrBodyRef,
};
pub use joint_constraint_builder::JointConstraintHelper;
pub use joint_constraints_set::JointConstraintsSet;
mod any_joint_constraint;
mod generic_joint_constraint;
mod generic_joint_constraint_builder;
mod joint_constraint_builder;
mod joint_constraints_set;
mod joint_generic_constraint;
mod joint_generic_constraint_builder;
mod joint_velocity_constraint;

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

@@ -7,17 +7,12 @@ pub(crate) use self::island_solver::IslandSolver;
// #[cfg(feature = "parallel")]
// pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver;
// #[cfg(not(feature = "parallel"))]
use self::solver_constraints_set::SolverConstraintsSet;
// #[cfg(not(feature = "parallel"))]
use self::velocity_solver::VelocitySolver;
use contact_constraint::*;
use interaction_groups::*;
pub(crate) use joint_constraint::MotorParameters;
pub use joint_constraint::*;
use solver_body::SolverBody;
use solver_constraints_set::{AnyConstraintMut, ConstraintTypes};
use solver_vel::SolverVel;
use solver_body::SolverVel;
mod categorization;
mod contact_constraint;
@@ -33,18 +28,17 @@ mod joint_constraint;
// mod parallel_velocity_solver;
mod solver_body;
// #[cfg(not(feature = "parallel"))]
mod solver_constraints_set;
mod solver_vel;
// #[cfg(not(feature = "parallel"))]
mod velocity_solver;
// TODO: SAFETY: restrict with bytemuck::AnyBitPattern to make this safe.
// TODO: SAFETY: restrict with bytemuck::Zeroable to make this safe.
pub unsafe fn reset_buffer<T>(buffer: &mut Vec<T>, len: usize) {
buffer.clear();
buffer.reserve(len);
unsafe {
buffer.as_mut_ptr().write_bytes(u8::MAX, len);
// NOTE: writing zeros is faster than u8::MAX.
buffer.as_mut_ptr().write_bytes(0, len);
buffer.set_len(len);
}
}

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

@@ -235,7 +235,7 @@ impl ParallelIslandSolver {
{
let mut solver_id = 0;
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
let active_bodies = &islands.active_set[island_range];
for handle in active_bodies {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
let multibody = multibodies
@@ -299,7 +299,7 @@ impl ParallelIslandSolver {
// Initialize `solver_vels` (per-body velocity deltas) with external accelerations (gravity etc):
{
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
let active_bodies = &islands.active_set[island_range];
concurrent_loop! {
let batch_size = thread.batch_size;
@@ -321,7 +321,7 @@ impl ParallelIslandSolver {
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
dvel.angular += rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
dvel.angular += rb.mprops.effective_world_inv_inertia * rb.forces.torque * params.dt;
dvel.linear += rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
}
}

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

@@ -164,7 +164,7 @@ macro_rules! impl_init_constraints_group {
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::TwoBodyGrouped(
gather![|ii| interaction_i[ii]],
array![|ii| interaction_i[ii]],
),
));
total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
@@ -190,7 +190,7 @@ macro_rules! impl_init_constraints_group {
self.constraint_descs.push((
total_num_constraints,
ConstraintDesc::OneBodyGrouped(
gather![|ii| interaction_i[ii]],
array![|ii| interaction_i[ii]],
),
));
total_num_constraints += $num_active_constraints_and_jacobian_lines(interaction).0;
@@ -337,12 +337,12 @@ impl ParallelSolverConstraints<ContactConstraintTypes> {
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::TwoBodyGrouped(manifold_id) => {
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]];
TwoBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::OneBodyGrouped(manifold_id) => {
let manifolds = gather![|ii| &*manifolds_all[manifold_id[ii]]];
let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]];
OneBodyConstraintSimd::generate(params, *manifold_id, manifolds, bodies, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericTwoBodyNongrouped(manifold_id, j_id) => {
@@ -387,12 +387,12 @@ impl ParallelSolverConstraints<JointConstraintTypes> {
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::TwoBodyGrouped(joint_id) => {
let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
let impulse_joints = array![|ii| &joints_all[joint_id[ii]].weight];
JointConstraintTypes::from_wide_joint(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
}
#[cfg(feature = "simd-is-enabled")]
ConstraintDesc::OneBodyGrouped(joint_id) => {
let impulse_joints = gather![|ii| &joints_all[joint_id[ii]].weight];
let impulse_joints = array![|ii| &joints_all[joint_id[ii]].weight];
JointConstraintTypes::from_wide_joint_one_body(params, *joint_id, impulse_joints, bodies, &mut self.velocity_constraints, Some(desc.0));
}
ConstraintDesc::GenericTwoBodyNongrouped(joint_id, j_id) => {

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

@@ -1,8 +1,8 @@
use super::{ContactConstraintTypes, JointConstraintTypes, SolverVel, ThreadContext};
use crate::concurrent_loop;
use crate::dynamics::{
solver::ParallelSolverConstraints, IntegrationParameters, IslandManager, JointGraphEdge,
MultibodyJointSet, RigidBodySet,
IntegrationParameters, IslandManager, JointGraphEdge, MultibodyJointSet, RigidBodySet,
solver::ParallelSolverConstraints,
};
use crate::geometry::ContactManifold;
use crate::math::Real;
@@ -182,7 +182,7 @@ impl ParallelVelocitySolver {
// Integrate positions.
{
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
let active_bodies = &islands.active_set[island_range];
concurrent_loop! {
let batch_size = thread.batch_size;
@@ -206,7 +206,7 @@ impl ParallelVelocitySolver {
let rb = bodies.index_mut_internal(*handle);
let dvel = self.solver_vels[rb.ids.active_set_offset];
let dangvel = rb.mprops
.effective_world_inv_inertia_sqrt
.effective_world_inv_inertia
.transform_vector(dvel.angular);
// Update positions.
@@ -284,7 +284,7 @@ impl ParallelVelocitySolver {
// Update velocities.
{
let island_range = islands.active_island_range(island_id);
let active_bodies = &islands.active_dynamic_set[island_range];
let active_bodies = &islands.active_set[island_range];
concurrent_loop! {
let batch_size = thread.batch_size;
@@ -304,7 +304,7 @@ impl ParallelVelocitySolver {
let rb = bodies.index_mut_internal(*handle);
let dvel = self.solver_vels[rb.ids.active_set_offset];
let dangvel = rb.mprops
.effective_world_inv_inertia_sqrt
.effective_world_inv_inertia
.transform_vector(dvel.angular);
rb.vels.linvel += dvel.linear;
rb.vels.angvel += dangvel;

582
src/dynamics/solver/solver_body.rs
View File

@@ -1,59 +1,555 @@
use crate::dynamics::{RigidBody, RigidBodyVelocity};
use crate::math::{AngularInertia, Isometry, Point, Real, Vector};
use crate::prelude::RigidBodyDamping;
use crate::dynamics::RigidBody;
use crate::math::{AngularInertia, Isometry, Real, SPATIAL_DIM, Vector};
use crate::utils::SimdRealCopy;
use na::{DVectorView, DVectorViewMut};
use parry::math::{AngVector, SIMD_WIDTH, SimdReal, Translation};
use std::ops::{AddAssign, Sub, SubAssign};
#[cfg(feature = "dim2")]
use crate::num::Zero;
#[cfg(feature = "simd-is-enabled")]
use crate::utils::transmute_to_wide;
#[derive(Copy, Clone, Debug)]
pub(crate) struct SolverBody {
pub position: Isometry<Real>,
pub integrated_vels: RigidBodyVelocity,
pub im: Vector<Real>,
pub sqrt_ii: AngularInertia<Real>,
pub world_com: Point<Real>,
pub ccd_thickness: Real,
pub damping: RigidBodyDamping,
pub local_com: Point<Real>,
#[cfg(feature = "simd-is-enabled")]
macro_rules! aos(
($data_repr: ident [ $idx: ident ] . $data_n: ident, $fallback: ident) => {
[
if ($idx[0] as usize) < $data_repr.len() {
$data_repr[$idx[0] as usize].$data_n.0
} else {
$fallback.$data_n.0
},
if ($idx[1] as usize) < $data_repr.len() {
$data_repr[$idx[1] as usize].$data_n.0
} else {
$fallback.$data_n.0
},
if ($idx[2] as usize) < $data_repr.len() {
$data_repr[$idx[2] as usize].$data_n.0
} else {
$fallback.$data_n.0
},
if ($idx[3] as usize) < $data_repr.len() {
$data_repr[$idx[3] as usize].$data_n.0
} else {
$fallback.$data_n.0
},
]
}
);
#[cfg(feature = "simd-is-enabled")]
macro_rules! aos_unchecked(
($data_repr: ident [ $idx: ident ] . $data_n: ident) => {
[
unsafe { $data_repr.get_unchecked($idx[0] as usize).$data_n.0 },
unsafe { $data_repr.get_unchecked($idx[1] as usize).$data_n.0 },
unsafe { $data_repr.get_unchecked($idx[2] as usize).$data_n.0 },
unsafe { $data_repr.get_unchecked($idx[3] as usize).$data_n.0 },
]
}
);
#[cfg(feature = "simd-is-enabled")]
macro_rules! scatter(
($data: ident [ $idx: ident [ $i: expr ] ] = [$($aos: ident),*]) => {
unsafe {
#[allow(clippy::missing_transmute_annotations)] // Different macro calls transmute to different types
if ($idx[$i] as usize) < $data.len() {
$data[$idx[$i] as usize] = std::mem::transmute([$($aos[$i]),*]);
}
}
}
);
#[cfg(feature = "simd-is-enabled")]
macro_rules! scatter_unchecked(
($data: ident [ $idx: ident [ $i: expr ] ] = [$($aos: ident),*]) => {
#[allow(clippy::missing_transmute_annotations)] // Different macro calls transmute to different types
unsafe {
*$data.get_unchecked_mut($idx[$i] as usize) = std::mem::transmute([$($aos[$i]),*]);
}
}
);
#[derive(Default)]
pub struct SolverBodies {
pub vels: Vec<SolverVel<Real>>,
pub poses: Vec<SolverPose<Real>>,
}
impl Default for SolverBody {
impl SolverBodies {
pub fn clear(&mut self) {
self.vels.clear();
self.poses.clear();
}
pub fn resize(&mut self, sz: usize) {
self.vels.resize(sz, Default::default());
self.poses.resize(sz, Default::default());
}
pub fn len(&self) -> usize {
self.vels.len()
}
// TODO: add a SIMD version?
pub fn copy_from(&mut self, _dt: Real, i: usize, rb: &RigidBody) {
let poses = &mut self.poses[i];
let vels = &mut self.vels[i];
#[cfg(feature = "dim2")]
{
vels.angular = rb.vels.angvel;
}
#[cfg(feature = "dim3")]
{
if rb.forces.gyroscopic_forces_enabled {
vels.angular = rb.angvel_with_gyroscopic_forces(_dt);
} else {
vels.angular = *rb.angvel();
}
}
vels.linear = rb.vels.linvel;
poses.pose = rb.pos.position * Translation::from(rb.mprops.local_mprops.local_com);
if rb.is_dynamic_or_kinematic() {
poses.ii = rb.mprops.effective_world_inv_inertia;
poses.im = rb.mprops.effective_inv_mass;
} else {
poses.ii = Default::default();
poses.im = Default::default();
}
}
#[inline]
pub unsafe fn gather_vels_unchecked(&self, idx: [u32; SIMD_WIDTH]) -> SolverVel<SimdReal> {
#[cfg(not(feature = "simd-is-enabled"))]
unsafe {
*self.vels.get_unchecked(idx[0] as usize)
}
#[cfg(feature = "simd-is-enabled")]
unsafe {
SolverVel::gather_unchecked(&self.vels, idx)
}
}
#[inline]
pub fn gather_vels(&self, idx: [u32; SIMD_WIDTH]) -> SolverVel<SimdReal> {
#[cfg(not(feature = "simd-is-enabled"))]
return self.vels.get(idx[0] as usize).copied().unwrap_or_default();
#[cfg(feature = "simd-is-enabled")]
return SolverVel::gather(&self.vels, idx);
}
#[inline]
pub fn get_vel(&self, i: u32) -> SolverVel<Real> {
self.vels.get(i as usize).copied().unwrap_or_default()
}
#[inline]
pub fn scatter_vels(&mut self, idx: [u32; SIMD_WIDTH], vels: SolverVel<SimdReal>) {
#[cfg(not(feature = "simd-is-enabled"))]
if (idx[0] as usize) < self.vels.len() {
self.vels[idx[0] as usize] = vels
}
#[cfg(feature = "simd-is-enabled")]
vels.scatter(&mut self.vels, idx);
}
#[inline]
pub fn set_vel(&mut self, i: u32, vel: SolverVel<Real>) {
if (i as usize) < self.vels.len() {
self.vels[i as usize] = vel;
}
}
#[inline]
pub fn get_pose(&self, i: u32) -> SolverPose<Real> {
self.poses.get(i as usize).copied().unwrap_or_default()
}
#[inline]
pub unsafe fn gather_poses_unchecked(&self, idx: [u32; SIMD_WIDTH]) -> SolverPose<SimdReal> {
#[cfg(not(feature = "simd-is-enabled"))]
unsafe {
*self.poses.get_unchecked(idx[0] as usize)
}
#[cfg(feature = "simd-is-enabled")]
unsafe {
SolverPose::gather_unchecked(&self.poses, idx)
}
}
#[inline]
pub fn gather_poses(&self, idx: [u32; SIMD_WIDTH]) -> SolverPose<SimdReal> {
#[cfg(not(feature = "simd-is-enabled"))]
return self.poses.get(idx[0] as usize).copied().unwrap_or_default();
#[cfg(feature = "simd-is-enabled")]
return SolverPose::gather(&self.poses, idx);
}
#[inline]
pub fn scatter_poses(&mut self, idx: [u32; SIMD_WIDTH], poses: SolverPose<SimdReal>) {
#[cfg(not(feature = "simd-is-enabled"))]
if (idx[0] as usize) < self.poses.len() {
self.poses[idx[0] as usize] = poses;
}
#[cfg(feature = "simd-is-enabled")]
poses.scatter(&mut self.poses, idx);
}
#[inline]
pub fn scatter_poses_unchecked(&mut self, idx: [u32; SIMD_WIDTH], poses: SolverPose<SimdReal>) {
#[cfg(not(feature = "simd-is-enabled"))]
unsafe {
*self.poses.get_unchecked_mut(idx[0] as usize) = poses
}
#[cfg(feature = "simd-is-enabled")]
poses.scatter_unchecked(&mut self.poses, idx);
}
}
// Total 7/13
#[repr(C)]
#[cfg_attr(feature = "simd-is-enabled", repr(align(16)))]
#[derive(Copy, Clone, Default)]
pub struct SolverVel<T: SimdRealCopy> {
pub linear: Vector<T>, // 2/3
pub angular: AngVector<T>, // 1/3
// TODO: explicit padding are useful for static assertions.
// But might be wasteful for the SolverVel<SimdReal>
// specialization.
#[cfg(feature = "simd-is-enabled")]
#[cfg(feature = "dim2")]
padding: [T; 1],
#[cfg(feature = "simd-is-enabled")]
#[cfg(feature = "dim3")]
padding: [T; 2],
}
#[cfg(feature = "simd-is-enabled")]
#[repr(C)]
struct SolverVelRepr {
data0: SimdReal,
#[cfg(feature = "dim3")]
data1: SimdReal,
}
#[cfg(feature = "simd-is-enabled")]
impl SolverVelRepr {
pub fn zero() -> Self {
Self {
data0: na::zero(),
#[cfg(feature = "dim3")]
data1: na::zero(),
}
}
}
#[cfg(feature = "simd-is-enabled")]
impl SolverVel<SimdReal> {
#[inline]
pub unsafe fn gather_unchecked(data: &[SolverVel<Real>], idx: [u32; SIMD_WIDTH]) -> Self {
// TODO: double-check that the compiler is using simd loads and
// isn’t generating useless copies.
let data_repr: &[SolverVelRepr] = unsafe { std::mem::transmute(data) };
#[cfg(feature = "dim2")]
{
let aos = aos_unchecked!(data_repr[idx].data0);
let soa = wide::f32x4::transpose(transmute_to_wide(aos));
unsafe { std::mem::transmute(soa) }
}
#[cfg(feature = "dim3")]
{
let aos0 = aos_unchecked!(data_repr[idx].data0);
let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0));
let aos1 = aos_unchecked!(data_repr[idx].data1);
let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1));
unsafe { std::mem::transmute((soa0, soa1)) }
}
}
#[inline]
pub fn gather(data: &[SolverVel<Real>], idx: [u32; SIMD_WIDTH]) -> Self {
// TODO: double-check that the compiler is using simd loads and
// isn’t generating useless copies.
let zero = SolverVelRepr::zero();
let data_repr: &[SolverVelRepr] = unsafe { std::mem::transmute(data) };
#[cfg(feature = "dim2")]
{
let aos = aos!(data_repr[idx].data0, zero);
let soa = wide::f32x4::transpose(transmute_to_wide(aos));
unsafe { std::mem::transmute(soa) }
}
#[cfg(feature = "dim3")]
{
let aos0 = aos!(data_repr[idx].data0, zero);
let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0));
let aos1 = aos!(data_repr[idx].data1, zero);
let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1));
unsafe { std::mem::transmute((soa0, soa1)) }
}
}
#[inline]
#[cfg(feature = "dim2")]
pub fn scatter(self, data: &mut [SolverVel<Real>], idx: [u32; SIMD_WIDTH]) {
// TODO: double-check that the compiler is using simd loads and no useless copies.
let soa: [wide::f32x4; 4] = unsafe { std::mem::transmute(self) };
let aos = wide::f32x4::transpose(soa);
scatter!(data[idx[0]] = [aos]);
scatter!(data[idx[1]] = [aos]);
scatter!(data[idx[2]] = [aos]);
scatter!(data[idx[3]] = [aos]);
}
#[inline]
#[cfg(feature = "dim3")]
pub fn scatter(self, data: &mut [SolverVel<Real>], idx: [u32; SIMD_WIDTH]) {
let soa: [[wide::f32x4; 4]; 2] = unsafe { std::mem::transmute(self) };
// TODO: double-check that the compiler is using simd loads and no useless copies.
let aos0 = wide::f32x4::transpose(soa[0]);
let aos1 = wide::f32x4::transpose(soa[1]);
scatter!(data[idx[0]] = [aos0, aos1]);
scatter!(data[idx[1]] = [aos0, aos1]);
scatter!(data[idx[2]] = [aos0, aos1]);
scatter!(data[idx[3]] = [aos0, aos1]);
}
}
// Total: 7/16
#[repr(C)]
#[cfg_attr(feature = "simd-is-enabled", repr(align(16)))]
#[derive(Copy, Clone)]
pub struct SolverPose<T> {
/// Positional change of the rigid-body’s center of mass.
pub pose: Isometry<T>, // 4/7
pub ii: AngularInertia<T>, // 1/6
pub im: Vector<T>, // 2/3
#[cfg(feature = "dim2")]
pub padding: [T; 1],
}
impl Default for SolverPose<Real> {
#[inline]
fn default() -> Self {
Self {
position: Isometry::identity(),
integrated_vels: RigidBodyVelocity::zero(),
im: na::zero(),
sqrt_ii: AngularInertia::zero(),
world_com: Point::origin(),
ccd_thickness: 0.0,
damping: RigidBodyDamping::default(),
local_com: Point::origin(),
pose: Isometry::identity(),
ii: Default::default(),
im: Default::default(),
#[cfg(feature = "dim2")]
padding: Default::default(),
}
}
}
impl SolverBody {
pub fn from(rb: &RigidBody) -> Self {
#[cfg(feature = "simd-is-enabled")]
#[repr(C)]
struct SolverPoseRepr {
data0: SimdReal,
data1: SimdReal,
#[cfg(feature = "dim3")]
data2: SimdReal,
#[cfg(feature = "dim3")]
data3: SimdReal,
}
#[cfg(feature = "simd-is-enabled")]
impl SolverPoseRepr {
pub fn identity() -> Self {
// TODO PERF: will the compiler handle this efficiently and generate
// everything at compile-time?
unsafe { std::mem::transmute(SolverPose::default()) }
}
}
#[cfg(feature = "simd-is-enabled")]
impl SolverPose<SimdReal> {
#[inline]
pub unsafe fn gather_unchecked(data: &[SolverPose<Real>], idx: [u32; SIMD_WIDTH]) -> Self {
// TODO: double-check that the compiler is using simd loads and
// isn’t generating useless copies.
let data_repr: &[SolverPoseRepr] = unsafe { std::mem::transmute(data) };
#[cfg(feature = "dim2")]
{
let aos0 = aos_unchecked!(data_repr[idx].data0);
let aos1 = aos_unchecked!(data_repr[idx].data1);
let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0));
let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1));
unsafe { std::mem::transmute([soa0, soa1]) }
}
#[cfg(feature = "dim3")]
{
let aos0 = aos_unchecked!(data_repr[idx].data0);
let aos1 = aos_unchecked!(data_repr[idx].data1);
let aos2 = aos_unchecked!(data_repr[idx].data2);
let aos3 = aos_unchecked!(data_repr[idx].data3);
let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0));
let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1));
let soa2 = wide::f32x4::transpose(transmute_to_wide(aos2));
let soa3 = wide::f32x4::transpose(transmute_to_wide(aos3));
unsafe { std::mem::transmute([soa0, soa1, soa2, soa3]) }
}
}
#[inline]
pub fn gather(data: &[SolverPose<Real>], idx: [u32; SIMD_WIDTH]) -> Self {
// TODO: double-check that the compiler is using simd loads and
// isn’t generating useless copies.
let identity = SolverPoseRepr::identity();
let data_repr: &[SolverPoseRepr] = unsafe { std::mem::transmute(data) };
#[cfg(feature = "dim2")]
{
let aos0 = aos!(data_repr[idx].data0, identity);
let aos1 = aos!(data_repr[idx].data1, identity);
let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0));
let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1));
unsafe { std::mem::transmute([soa0, soa1]) }
}
#[cfg(feature = "dim3")]
{
let aos0 = aos!(data_repr[idx].data0, identity);
let aos1 = aos!(data_repr[idx].data1, identity);
let aos2 = aos!(data_repr[idx].data2, identity);
let aos3 = aos!(data_repr[idx].data3, identity);
let soa0 = wide::f32x4::transpose(transmute_to_wide(aos0));
let soa1 = wide::f32x4::transpose(transmute_to_wide(aos1));
let soa2 = wide::f32x4::transpose(transmute_to_wide(aos2));
let soa3 = wide::f32x4::transpose(transmute_to_wide(aos3));
unsafe { std::mem::transmute([soa0, soa1, soa2, soa3]) }
}
}
#[inline]
#[cfg(feature = "dim2")]
pub fn scatter_unchecked(self, data: &mut [SolverPose<Real>], idx: [u32; SIMD_WIDTH]) {
// TODO: double-check that the compiler is using simd loads and no useless copies.
let soa: [[wide::f32x4; 4]; 2] = unsafe { std::mem::transmute(self) };
let aos0 = wide::f32x4::transpose(soa[0]);
let aos1 = wide::f32x4::transpose(soa[1]);
scatter_unchecked!(data[idx[0]] = [aos0, aos1]);
scatter_unchecked!(data[idx[1]] = [aos0, aos1]);
scatter_unchecked!(data[idx[2]] = [aos0, aos1]);
scatter_unchecked!(data[idx[3]] = [aos0, aos1]);
}
#[inline]
#[cfg(feature = "dim3")]
pub fn scatter_unchecked(self, data: &mut [SolverPose<Real>], idx: [u32; SIMD_WIDTH]) {
let soa: [[wide::f32x4; 4]; 4] = unsafe { std::mem::transmute(self) };
// TODO: double-check that the compiler is using simd loads and no useless copies.
let aos0 = wide::f32x4::transpose(soa[0]);
let aos1 = wide::f32x4::transpose(soa[1]);
let aos2 = wide::f32x4::transpose(soa[2]);
let aos3 = wide::f32x4::transpose(soa[3]);
scatter_unchecked!(data[idx[0]] = [aos0, aos1, aos2, aos3]);
scatter_unchecked!(data[idx[1]] = [aos0, aos1, aos2, aos3]);
scatter_unchecked!(data[idx[2]] = [aos0, aos1, aos2, aos3]);
scatter_unchecked!(data[idx[3]] = [aos0, aos1, aos2, aos3]);
}
#[inline]
#[cfg(feature = "dim2")]
pub fn scatter(self, data: &mut [SolverPose<Real>], idx: [u32; SIMD_WIDTH]) {
// TODO: double-check that the compiler is using simd loads and no useless copies.
let soa: [[wide::f32x4; 4]; 2] = unsafe { std::mem::transmute(self) };
let aos0 = wide::f32x4::transpose(soa[0]);
let aos1 = wide::f32x4::transpose(soa[1]);
scatter!(data[idx[0]] = [aos0, aos1]);
scatter!(data[idx[1]] = [aos0, aos1]);
scatter!(data[idx[2]] = [aos0, aos1]);
scatter!(data[idx[3]] = [aos0, aos1]);
}
#[inline]
#[cfg(feature = "dim3")]
pub fn scatter(self, data: &mut [SolverPose<Real>], idx: [u32; SIMD_WIDTH]) {
let soa: [[wide::f32x4; 4]; 4] = unsafe { std::mem::transmute(self) };
// TODO: double-check that the compiler is using simd loads and no useless copies.
let aos0 = wide::f32x4::transpose(soa[0]);
let aos1 = wide::f32x4::transpose(soa[1]);
let aos2 = wide::f32x4::transpose(soa[2]);
let aos3 = wide::f32x4::transpose(soa[3]);
scatter!(data[idx[0]] = [aos0, aos1, aos2, aos3]);
scatter!(data[idx[1]] = [aos0, aos1, aos2, aos3]);
scatter!(data[idx[2]] = [aos0, aos1, aos2, aos3]);
scatter!(data[idx[3]] = [aos0, aos1, aos2, aos3]);
}
}
impl<N: SimdRealCopy> SolverVel<N> {
pub fn as_slice(&self) -> &[N; SPATIAL_DIM] {
unsafe { std::mem::transmute(self) }
}
pub fn as_mut_slice(&mut self) -> &mut [N; SPATIAL_DIM] {
unsafe { std::mem::transmute(self) }
}
pub fn as_vector_slice(&self) -> DVectorView<'_, N> {
DVectorView::from_slice(&self.as_slice()[..], SPATIAL_DIM)
}
pub fn as_vector_slice_mut(&mut self) -> DVectorViewMut<'_, N> {
DVectorViewMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM)
}
}
impl<N: SimdRealCopy> SolverVel<N> {
pub fn zero() -> Self {
Self {
position: rb.pos.position,
integrated_vels: RigidBodyVelocity::zero(),
im: rb.mprops.effective_inv_mass,
sqrt_ii: rb.mprops.effective_world_inv_inertia_sqrt,
world_com: rb.mprops.world_com,
ccd_thickness: rb.ccd.ccd_thickness,
damping: rb.damping,
local_com: rb.mprops.local_mprops.local_com,
linear: na::zero(),
angular: na::zero(),
#[cfg(feature = "simd-is-enabled")]
#[cfg(feature = "dim2")]
padding: [na::zero(); 1],
#[cfg(feature = "simd-is-enabled")]
#[cfg(feature = "dim3")]
padding: [na::zero(); 2],
}
}
}
pub fn copy_from(&mut self, rb: &RigidBody) {
self.position = rb.pos.position;
self.integrated_vels = RigidBodyVelocity::zero();
self.im = rb.mprops.effective_inv_mass;
self.sqrt_ii = rb.mprops.effective_world_inv_inertia_sqrt;
self.world_com = rb.mprops.world_com;
self.ccd_thickness = rb.ccd.ccd_thickness;
self.damping = rb.damping;
self.local_com = rb.mprops.local_mprops.local_com;
impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
fn add_assign(&mut self, rhs: Self) {
self.linear += rhs.linear;
self.angular += rhs.angular;
}
}
impl<N: SimdRealCopy> SubAssign for SolverVel<N> {
fn sub_assign(&mut self, rhs: Self) {
self.linear -= rhs.linear;
self.angular -= rhs.angular;
}
}
impl<N: SimdRealCopy> Sub for SolverVel<N> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {
SolverVel {
linear: self.linear - rhs.linear,
angular: self.angular - rhs.angular,
#[cfg(feature = "simd-is-enabled")]
padding: self.padding,
}
}
}

242
src/dynamics/solver/solver_constraints_set.rs
View File

@@ -1,242 +0,0 @@
use super::InteractionGroups;
use crate::math::Real;
use na::DVector;
pub(crate) trait ConstraintTypes {
type OneBody;
type TwoBodies;
type GenericOneBody;
type GenericTwoBodies;
#[cfg(feature = "simd-is-enabled")]
type SimdOneBody;
#[cfg(feature = "simd-is-enabled")]
type SimdTwoBodies;
type BuilderOneBody;
type BuilderTwoBodies;
type GenericBuilderOneBody;
type GenericBuilderTwoBodies;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderOneBody;
#[cfg(feature = "simd-is-enabled")]
type SimdBuilderTwoBodies;
}
#[derive(Debug)]
pub enum AnyConstraintMut<'a, Constraints: ConstraintTypes> {
OneBody(&'a mut Constraints::OneBody),
TwoBodies(&'a mut Constraints::TwoBodies),
GenericOneBody(&'a mut Constraints::GenericOneBody),
GenericTwoBodies(&'a mut Constraints::GenericTwoBodies),
#[cfg(feature = "simd-is-enabled")]
SimdOneBody(&'a mut Constraints::SimdOneBody),
#[cfg(feature = "simd-is-enabled")]
SimdTwoBodies(&'a mut Constraints::SimdTwoBodies),
}
pub(crate) struct SolverConstraintsSet<Constraints: ConstraintTypes> {
pub generic_jacobians: DVector<Real>,
pub two_body_interactions: Vec<usize>,
pub one_body_interactions: Vec<usize>,
pub generic_two_body_interactions: Vec<usize>,
pub generic_one_body_interactions: Vec<usize>,
pub interaction_groups: InteractionGroups,
pub one_body_interaction_groups: InteractionGroups,
pub velocity_constraints: Vec<Constraints::TwoBodies>,
pub generic_velocity_constraints: Vec<Constraints::GenericTwoBodies>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_constraints: Vec<Constraints::SimdTwoBodies>,
pub velocity_one_body_constraints: Vec<Constraints::OneBody>,
pub generic_velocity_one_body_constraints: Vec<Constraints::GenericOneBody>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_one_body_constraints: Vec<Constraints::SimdOneBody>,
pub velocity_constraints_builder: Vec<Constraints::BuilderTwoBodies>,
pub generic_velocity_constraints_builder: Vec<Constraints::GenericBuilderTwoBodies>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_constraints_builder: Vec<Constraints::SimdBuilderTwoBodies>,
pub velocity_one_body_constraints_builder: Vec<Constraints::BuilderOneBody>,
pub generic_velocity_one_body_constraints_builder: Vec<Constraints::GenericBuilderOneBody>,
#[cfg(feature = "simd-is-enabled")]
pub simd_velocity_one_body_constraints_builder: Vec<Constraints::SimdBuilderOneBody>,
}
impl<Constraints: ConstraintTypes> SolverConstraintsSet<Constraints> {
pub fn new() -> Self {
Self {
generic_jacobians: DVector::zeros(0),
two_body_interactions: vec![],
one_body_interactions: vec![],
generic_two_body_interactions: vec![],
generic_one_body_interactions: vec![],
interaction_groups: InteractionGroups::new(),
one_body_interaction_groups: InteractionGroups::new(),
velocity_constraints: vec![],
generic_velocity_constraints: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_constraints: vec![],
velocity_one_body_constraints: vec![],
generic_velocity_one_body_constraints: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_one_body_constraints: vec![],
velocity_constraints_builder: vec![],
generic_velocity_constraints_builder: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_constraints_builder: vec![],
velocity_one_body_constraints_builder: vec![],
generic_velocity_one_body_constraints_builder: vec![],
#[cfg(feature = "simd-is-enabled")]
simd_velocity_one_body_constraints_builder: vec![],
}
}
#[allow(dead_code)] // Useful for debugging.
pub fn print_counts(&self) {
println!("Solver constraints:");
println!(
"|__ two_body_interactions: {}",
self.two_body_interactions.len()
);
println!(
"|__ one_body_interactions: {}",
self.one_body_interactions.len()
);
println!(
"|__ generic_two_body_interactions: {}",
self.generic_two_body_interactions.len()
);
println!(
"|__ generic_one_body_interactions: {}",
self.generic_one_body_interactions.len()
);
println!(
"|__ velocity_constraints: {}",
self.velocity_constraints.len()
);
println!(
"|__ generic_velocity_constraints: {}",
self.generic_velocity_constraints.len()
);
#[cfg(feature = "simd-is-enabled")]
println!(
"|__ simd_velocity_constraints: {}",
self.simd_velocity_constraints.len()
);
println!(
"|__ velocity_one_body_constraints: {}",
self.velocity_one_body_constraints.len()
);
println!(
"|__ generic_velocity_one_body_constraints: {}",
self.generic_velocity_one_body_constraints.len()
);
#[cfg(feature = "simd-is-enabled")]
println!(
"|__ simd_velocity_one_body_constraints: {}",
self.simd_velocity_one_body_constraints.len()
);
println!(
"|__ velocity_constraints_builder: {}",
self.velocity_constraints_builder.len()
);
println!(
"|__ generic_velocity_constraints_builder: {}",
self.generic_velocity_constraints_builder.len()
);
#[cfg(feature = "simd-is-enabled")]
println!(
"|__ simd_velocity_constraints_builder: {}",
self.simd_velocity_constraints_builder.len()
);
println!(
"|__ velocity_one_body_constraints_builder: {}",
self.velocity_one_body_constraints_builder.len()
);
println!(
"|__ generic_velocity_one_body_constraints_builder: {}",
self.generic_velocity_one_body_constraints_builder.len()
);
#[cfg(feature = "simd-is-enabled")]
println!(
"|__ simd_velocity_one_body_constraints_builder: {}",
self.simd_velocity_one_body_constraints_builder.len()
);
}
pub fn clear_constraints(&mut self) {
self.generic_jacobians.fill(0.0);
self.velocity_constraints.clear();
self.velocity_one_body_constraints.clear();
self.generic_velocity_constraints.clear();
self.generic_velocity_one_body_constraints.clear();
#[cfg(feature = "simd-is-enabled")]
{
self.simd_velocity_constraints.clear();
self.simd_velocity_one_body_constraints.clear();
}
}
pub fn clear_builders(&mut self) {
self.velocity_constraints_builder.clear();
self.velocity_one_body_constraints_builder.clear();
self.generic_velocity_constraints_builder.clear();
self.generic_velocity_one_body_constraints_builder.clear();
#[cfg(feature = "simd-is-enabled")]
{
self.simd_velocity_constraints_builder.clear();
self.simd_velocity_one_body_constraints_builder.clear();
}
}
// Returns the generic jacobians and a mutable iterator through all the constraints.
pub fn iter_constraints_mut(
&mut self,
) -> (
&DVector<Real>,
impl Iterator<Item = AnyConstraintMut<Constraints>>,
) {
let jac = &self.generic_jacobians;
let a = self
.velocity_constraints
.iter_mut()
.map(AnyConstraintMut::TwoBodies);
let b = self
.generic_velocity_constraints
.iter_mut()
.map(AnyConstraintMut::GenericTwoBodies);
#[cfg(feature = "simd-is-enabled")]
let c = self
.simd_velocity_constraints
.iter_mut()
.map(AnyConstraintMut::SimdTwoBodies);
let d = self
.velocity_one_body_constraints
.iter_mut()
.map(AnyConstraintMut::OneBody);
let e = self
.generic_velocity_one_body_constraints
.iter_mut()
.map(AnyConstraintMut::GenericOneBody);
#[cfg(feature = "simd-is-enabled")]
let f = self
.simd_velocity_one_body_constraints
.iter_mut()
.map(AnyConstraintMut::SimdOneBody);
#[cfg(feature = "simd-is-enabled")]
return (jac, a.chain(b).chain(c).chain(d).chain(e).chain(f));
#[cfg(not(feature = "simd-is-enabled"))]
return (jac, a.chain(b).chain(d).chain(e));
}
}

66
src/dynamics/solver/solver_vel.rs
View File

@@ -1,66 +0,0 @@
use crate::math::{AngVector, SPATIAL_DIM, Vector};
use crate::utils::SimdRealCopy;
use na::{DVectorView, DVectorViewMut, Scalar};
use std::ops::{AddAssign, Sub, SubAssign};
#[derive(Copy, Clone, Debug, Default)]
#[repr(C)]
//#[repr(align(64))]
pub struct SolverVel<N: Scalar + Copy> {
// The linear velocity of a solver body.
pub linear: Vector<N>,
// The angular velocity, multiplied by the inverse sqrt angular inertia, of a solver body.
pub angular: AngVector<N>,
}
impl<N: Scalar + Copy> SolverVel<N> {
pub fn as_slice(&self) -> &[N; SPATIAL_DIM] {
unsafe { std::mem::transmute(self) }
}
pub fn as_mut_slice(&mut self) -> &mut [N; SPATIAL_DIM] {
unsafe { std::mem::transmute(self) }
}
pub fn as_vector_slice(&self) -> DVectorView<N> {
DVectorView::from_slice(&self.as_slice()[..], SPATIAL_DIM)
}
pub fn as_vector_slice_mut(&mut self) -> DVectorViewMut<N> {
DVectorViewMut::from_slice(&mut self.as_mut_slice()[..], SPATIAL_DIM)
}
}
impl<N: SimdRealCopy> SolverVel<N> {
pub fn zero() -> Self {
Self {
linear: na::zero(),
angular: na::zero(),
}
}
}
impl<N: SimdRealCopy> AddAssign for SolverVel<N> {
fn add_assign(&mut self, rhs: Self) {
self.linear += rhs.linear;
self.angular += rhs.angular;
}
}
impl<N: SimdRealCopy> SubAssign for SolverVel<N> {
fn sub_assign(&mut self, rhs: Self) {
self.linear -= rhs.linear;
self.angular -= rhs.angular;
}
}
impl<N: SimdRealCopy> Sub for SolverVel<N> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {
SolverVel {
linear: self.linear - rhs.linear,
angular: self.angular - rhs.angular,
}
}
}

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

@@ -1,19 +1,21 @@
use super::{JointConstraintTypes, SolverConstraintsSet};
use crate::dynamics::solver::solver_body::SolverBody;
use crate::dynamics::solver::JointConstraintsSet;
use crate::dynamics::solver::contact_constraint::ContactConstraintsSet;
use crate::dynamics::solver::solver_body::SolverBodies;
use crate::dynamics::{
IntegrationParameters, IslandManager, JointGraphEdge, JointIndex, MultibodyJointSet,
MultibodyLinkId, RigidBodySet,
solver::{ContactConstraintTypes, SolverVel},
MultibodyLinkId, RigidBodySet, RigidBodyType, solver::SolverVel,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::Real;
use crate::prelude::RigidBodyVelocity;
use crate::utils::SimdAngularInertia;
use na::DVector;
use parry::math::{SIMD_WIDTH, Translation};
#[cfg(feature = "dim3")]
use crate::dynamics::FrictionModel;
pub(crate) struct VelocitySolver {
pub solver_bodies: Vec<SolverBody>,
pub solver_vels: Vec<SolverVel<Real>>,
pub solver_bodies: SolverBodies,
pub solver_vels_increment: Vec<SolverVel<Real>>,
pub generic_solver_vels: DVector<Real>,
pub generic_solver_vels_increment: DVector<Real>,
@@ -23,8 +25,7 @@ pub(crate) struct VelocitySolver {
impl VelocitySolver {
pub fn new() -> Self {
Self {
solver_bodies: Vec::new(),
solver_vels: Vec::new(),
solver_bodies: SolverBodies::default(),
solver_vels_increment: Vec::new(),
generic_solver_vels: DVector::zeros(0),
generic_solver_vels_increment: DVector::zeros(0),
@@ -42,16 +43,20 @@ impl VelocitySolver {
manifold_indices: &[ContactManifoldIndex],
joints_all: &mut [JointGraphEdge],
joint_indices: &[JointIndex],
contact_constraints: &mut SolverConstraintsSet<ContactConstraintTypes>,
joint_constraints: &mut SolverConstraintsSet<JointConstraintTypes>,
contact_constraints: &mut ContactConstraintsSet,
joint_constraints: &mut JointConstraintsSet,
#[cfg(feature = "dim3")] friction_model: FrictionModel,
) {
contact_constraints.init(
island_id,
islands,
bodies,
&self.solver_bodies,
multibodies,
manifolds_all,
manifold_indices,
#[cfg(feature = "dim3")]
friction_model,
);
joint_constraints.init(
@@ -66,6 +71,7 @@ impl VelocitySolver {
pub fn init_solver_velocities_and_solver_bodies(
&mut self,
total_step_dt: Real,
params: &IntegrationParameters,
island_id: usize,
islands: &IslandManager,
@@ -74,17 +80,14 @@ impl VelocitySolver {
) {
self.multibody_roots.clear();
self.solver_bodies.clear();
self.solver_bodies.resize(
islands.active_island(island_id).len(),
SolverBody::default(),
);
let aligned_solver_bodies_len =
islands.active_island(island_id).len().div_ceil(SIMD_WIDTH) * SIMD_WIDTH;
self.solver_bodies.resize(aligned_solver_bodies_len);
self.solver_vels_increment.clear();
self.solver_vels_increment
.resize(islands.active_island(island_id).len(), SolverVel::zero());
self.solver_vels.clear();
self.solver_vels
.resize(islands.active_island(island_id).len(), SolverVel::zero());
.resize(aligned_solver_bodies_len, SolverVel::zero());
/*
* Initialize solver bodies and delta-velocities (`solver_vels_increment`) with external forces (gravity etc):
@@ -92,7 +95,7 @@ impl VelocitySolver {
*/
// Assign solver ids to multibodies, and collect the relevant roots.
// And init solver_vels for rigidb-bodies.
// And init solver_vels for rigid-bodies.
let mut multibody_solver_id = 0;
for handle in islands.active_island(island_id) {
if let Some(link) = multibodies.rigid_body_link(*handle).copied() {
@@ -102,32 +105,26 @@ impl VelocitySolver {
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
multibody.solver_id = multibody_solver_id;
multibody_solver_id += multibody.ndofs();
multibody_solver_id += multibody.ndofs() as u32;
self.multibody_roots.push(link);
}
} else {
let rb = &bodies[*handle];
let solver_vel = &mut self.solver_vels[rb.ids.active_set_offset];
let solver_vel_incr = &mut self.solver_vels_increment[rb.ids.active_set_offset];
let solver_body = &mut self.solver_bodies[rb.ids.active_set_offset];
solver_body.copy_from(rb);
let solver_vel_incr =
&mut self.solver_vels_increment[rb.ids.active_set_offset as usize];
self.solver_bodies
.copy_from(total_step_dt, rb.ids.active_set_offset as usize, rb);
// NOTE: `dvel.angular` is actually storing angular velocity delta multiplied
// by the square root of the inertia tensor:
solver_vel_incr.angular =
rb.mprops.effective_world_inv_inertia_sqrt * rb.forces.torque * params.dt;
rb.mprops.effective_world_inv_inertia * rb.forces.torque * params.dt;
solver_vel_incr.linear =
rb.forces.force.component_mul(&rb.mprops.effective_inv_mass) * params.dt;
solver_vel.linear = rb.vels.linvel;
// PERF: can we avoid the call to effective_angular_inertia_sqrt?
solver_vel.angular = rb.mprops.effective_angular_inertia_sqrt() * rb.vels.angvel;
}
}
// PERF: don’t reallocate at each iteration.
self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id);
self.generic_solver_vels = DVector::zeros(multibody_solver_id);
// TODO PERF: don’t reallocate at each iteration.
self.generic_solver_vels_increment = DVector::zeros(multibody_solver_id as usize);
self.generic_solver_vels = DVector::zeros(multibody_solver_id as usize);
// init solver_vels for multibodies.
for link in &self.multibody_roots {
@@ -139,10 +136,10 @@ impl VelocitySolver {
let mut solver_vels_incr = self
.generic_solver_vels_increment
.rows_mut(multibody.solver_id, multibody.ndofs());
.rows_mut(multibody.solver_id as usize, multibody.ndofs());
let mut solver_vels = self
.generic_solver_vels
.rows_mut(multibody.solver_id, multibody.ndofs());
.rows_mut(multibody.solver_id as usize, multibody.ndofs());
solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0);
solver_vels.copy_from(&multibody.velocities);
@@ -156,14 +153,16 @@ impl VelocitySolver {
num_substeps: usize,
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
contact_constraints: &mut SolverConstraintsSet<ContactConstraintTypes>,
joint_constraints: &mut SolverConstraintsSet<JointConstraintTypes>,
contact_constraints: &mut ContactConstraintsSet,
joint_constraints: &mut JointConstraintsSet,
) {
for substep_id in 0..num_substeps {
let is_last_substep = substep_id == num_substeps - 1;
// TODO PERF: could easily use SIMD.
for (solver_vels, incr) in self
.solver_vels
.solver_bodies
.vels
.iter_mut()
.zip(self.solver_vels_increment.iter())
{
@@ -174,28 +173,23 @@ impl VelocitySolver {
self.generic_solver_vels += &self.generic_solver_vels_increment;
/*
* Update & solve constraints.
* Update & solve constraints with bias.
*/
joint_constraints.update(params, multibodies, &self.solver_bodies);
contact_constraints.update(params, substep_id, multibodies, &self.solver_bodies);
if params.warmstart_coefficient != 0.0 {
contact_constraints.warmstart(&mut self.solver_vels, &mut self.generic_solver_vels);
// TODO PERF: we could probably figure out a way to avoid this warmstart when
// step_id > 0? Maybe for that to happen `solver_vels` needs to
// represent velocity changes instead of total rigid-boody velocities.
// Need to be careful wrt. multibody and joints too.
contact_constraints
.warmstart(&mut self.solver_bodies, &mut self.generic_solver_vels);
}
for _ in 0..params.num_internal_pgs_iterations {
joint_constraints.solve(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints
.solve_restitution(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints
.solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
}
if is_last_substep {
for _ in 0..params.num_additional_friction_iterations {
contact_constraints
.solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
}
joint_constraints.solve(&mut self.solver_bodies, &mut self.generic_solver_vels);
contact_constraints.solve(&mut self.solver_bodies, &mut self.generic_solver_vels);
}
/*
@@ -206,18 +200,11 @@ impl VelocitySolver {
/*
* Resolution without bias.
*/
if params.num_internal_stabilization_iterations > 0 {
for _ in 0..params.num_internal_stabilization_iterations {
joint_constraints
.solve_wo_bias(&mut self.solver_vels, &mut self.generic_solver_vels);
contact_constraints.solve_restitution_wo_bias(
&mut self.solver_vels,
&mut self.generic_solver_vels,
);
}
for _ in 0..params.num_internal_stabilization_iterations {
joint_constraints
.solve_wo_bias(&mut self.solver_bodies, &mut self.generic_solver_vels);
contact_constraints
.solve_friction(&mut self.solver_vels, &mut self.generic_solver_vels);
.solve_wo_bias(&mut self.solver_bodies, &mut self.generic_solver_vels);
}
}
}
@@ -230,21 +217,43 @@ impl VelocitySolver {
bodies: &mut RigidBodySet,
multibodies: &mut MultibodyJointSet,
) {
// Integrate positions.
for (solver_vels, solver_body) in self.solver_vels.iter().zip(self.solver_bodies.iter_mut())
for (solver_vels, solver_pose) in self
.solver_bodies
.vels
.iter()
.zip(self.solver_bodies.poses.iter_mut())
{
let linvel = solver_vels.linear;
let angvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular);
let angvel = solver_vels.angular;
let mut new_vels = RigidBodyVelocity { linvel, angvel };
new_vels = new_vels.apply_damping(params.dt, &solver_body.damping);
let new_pos =
new_vels.integrate(params.dt, &solver_body.position, &solver_body.local_com);
solver_body.integrated_vels += new_vels;
solver_body.position = new_pos;
solver_body.world_com = new_pos * solver_body.local_com;
// TODO: should we add a compile flag (or a simulation parameter)
// to disable the rotation linearization?
let new_vels = RigidBodyVelocity { linvel, angvel };
new_vels.integrate_linearized(params.dt, &mut solver_pose.pose);
}
// TODO PERF: SIMD-optimized integration. Works fine, but doesn’t run faster than the scalar
// one (tested on Apple Silicon/Neon, might be worth double-checking on x86_64/SSE2).
// // SAFETY: this assertion ensures the unchecked gathers are sound.
// assert_eq!(self.solver_bodies.len() % SIMD_WIDTH, 0);
// let dt = SimdReal::splat(params.dt);
// for i in (0..self.solver_bodies.len()).step_by(SIMD_WIDTH) {
// let idx = [i, i + 1, i + 2, i + 3];
// let solver_vels = unsafe { self.solver_bodies.gather_vels_unchecked(idx) };
// let mut solver_poses = unsafe { self.solver_bodies.gather_poses_unchecked(idx) };
// // let solver_consts = unsafe { self.solver_bodies.gather_consts_unchecked(idx) };
//
// let linvel = solver_vels.linear;
// let angvel = solver_poses.ii_sqrt.transform_vector(solver_vels.angular);
//
// let mut new_vels = RigidBodyVelocity { linvel, angvel };
// // TODO: store the post-damping velocity?
// // new_vels = new_vels.apply_damping(dt, &solver_consts.damping);
// new_vels.integrate_linearized(dt, &mut solver_poses.pose);
// self.solver_bodies
// .scatter_poses_unchecked(idx, solver_poses);
// }
// Integrate multibody positions.
for link in &self.multibody_roots {
let multibody = multibodies
@@ -252,7 +261,7 @@ impl VelocitySolver {
.unwrap();
let solver_vels = self
.generic_solver_vels
.rows(multibody.solver_id, multibody.ndofs());
.rows(multibody.solver_id as usize, multibody.ndofs());
multibody.velocities.copy_from(&solver_vels);
multibody.integrate(params.dt);
// PERF: don’t write back to the rigid-body poses `bodies` before the last step?
@@ -267,7 +276,7 @@ impl VelocitySolver {
let mut solver_vels_incr = self
.generic_solver_vels_increment
.rows_mut(multibody.solver_id, multibody.ndofs());
.rows_mut(multibody.solver_id as usize, multibody.ndofs());
solver_vels_incr.axpy(params.dt, &multibody.accelerations, 0.0);
}
}
@@ -276,7 +285,6 @@ impl VelocitySolver {
pub fn writeback_bodies(
&mut self,
params: &IntegrationParameters,
num_substeps: usize,
islands: &IslandManager,
island_id: usize,
bodies: &mut RigidBodySet,
@@ -297,32 +305,41 @@ impl VelocitySolver {
if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic {
let solver_vels = self
.generic_solver_vels
.rows(multibody.solver_id, multibody.ndofs());
.rows(multibody.solver_id as usize, multibody.ndofs());
multibody.velocities.copy_from(&solver_vels);
}
} else {
let rb = bodies.index_mut_internal(*handle);
let solver_body = &self.solver_bodies[rb.ids.active_set_offset];
let solver_vels = &self.solver_vels[rb.ids.active_set_offset];
let solver_vels = &self.solver_bodies.vels[rb.ids.active_set_offset as usize];
let solver_poses = &self.solver_bodies.poses[rb.ids.active_set_offset as usize];
let dangvel = solver_body.sqrt_ii.transform_vector(solver_vels.angular);
let dangvel = solver_vels.angular;
let mut new_vels = RigidBodyVelocity {
linvel: solver_vels.linear,
angvel: dangvel,
};
new_vels = new_vels.apply_damping(params.dt, &solver_body.damping);
new_vels = new_vels.apply_damping(params.dt, &rb.damping);
// NOTE: using integrated_vels instead of interpolation is interesting for
// high angular velocities. However, it is a bit inexact due to the
// solver integrating at intermediate sub-steps. Should we just switch
// to interpolation?
rb.integrated_vels.linvel =
solver_body.integrated_vels.linvel / num_substeps as Real;
rb.integrated_vels.angvel =
solver_body.integrated_vels.angvel / num_substeps as Real;
rb.vels = new_vels;
rb.pos.next_position = solver_body.position;
// NOTE: if it’s a position-based kinematic body, don’t writeback as we want
// to preserve exactly the value given by the user (it might not be exactly
// equal to the integrated position because of rounding errors).
if rb.body_type != RigidBodyType::KinematicPositionBased {
rb.pos.next_position =
solver_poses.pose * Translation::from(-rb.mprops.local_mprops.local_com);
}
if rb.ccd.ccd_enabled {
// TODO: Is storing this still necessary instead of just recomputing it
// during CCD?
rb.ccd_vels = rb
.pos
.interpolate_velocity(params.inv_dt(), rb.local_center_of_mass());
} else {
rb.ccd_vels = RigidBodyVelocity::zero();
}
}
}
}