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

Fix some solver issues

Browse Source 
- Fix the wrong codepath taken  by the solver for contacts involving a collider without parent.
- Properly adress the non-linear treatment of the friction direction
- Simplify the sleeping strategy
- Add an impulse resolution multiplier
This commit is contained in:
Sébastien Crozet
2022-01-16 16:40:59 +01:00
parent 4454a845e9
commit 0703e5527f
43 changed files with 936 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/dynamics/integration_parameters.rs
View File

@@ -30,6 +30,13 @@ pub struct IntegrationParameters {
/// (default `0.0`).
pub erp: Real,
/// 0-1: multiplier applied to each accumulated impulse during constraints resolution.
/// This is similar to the concept of CFN (Constraint Force Mixing) except that it is
/// a multiplicative factor instead of an additive factor.
/// Larger values lead to stiffer constraints (1.0 being completely stiff).
/// Smaller values lead to more compliant constraints.
pub delassus_inv_factor: Real,
/// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
pub allowed_linear_error: Real,
/// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`).
@@ -96,6 +103,7 @@ impl Default for IntegrationParameters {
min_ccd_dt: 1.0 / 60.0 / 100.0,
velocity_solve_fraction: 1.0,
erp: 0.8,
delassus_inv_factor: 0.75,
allowed_linear_error: 0.001, // 0.005
prediction_distance: 0.002,
max_velocity_iterations: 4,

23
src/dynamics/island_manager.rs
View File

@@ -5,6 +5,7 @@ use crate::dynamics::{
};
use crate::geometry::{ColliderParent, NarrowPhase};
use crate::math::Real;
use crate::utils::WDot;
/// Structure responsible for maintaining the set of active rigid-bodies, and
/// putting non-moving rigid-bodies to sleep to save computation times.
@@ -172,6 +173,7 @@ impl IslandManager {
pub(crate) fn update_active_set_with_contacts<Bodies, Colliders>(
&mut self,
dt: Real,
bodies: &mut Bodies,
colliders: &Colliders,
narrow_phase: &NarrowPhase,
@@ -207,12 +209,15 @@ impl IslandManager {
let stack = &mut self.stack;
let vels: &RigidBodyVelocity = bodies.index(h.0);
let pseudo_kinetic_energy = vels.pseudo_kinetic_energy();
let sq_linvel = vels.linvel.norm_squared();
let sq_angvel = vels.angvel.gdot(vels.angvel);
bodies.map_mut_internal(h.0, |activation: &mut RigidBodyActivation| {
update_energy(activation, pseudo_kinetic_energy);
update_energy(activation, sq_linvel, sq_angvel, dt);
if activation.energy <= activation.threshold {
if activation.time_since_can_sleep
>= RigidBodyActivation::default_time_until_sleep()
{
// Mark them as sleeping for now. This will
// be set to false during the graph traversal
// if it should not be put to sleep.
@@ -346,8 +351,12 @@ impl IslandManager {
}
}
fn update_energy(activation: &mut RigidBodyActivation, pseudo_kinetic_energy: Real) {
let mix_factor = 0.01;
let new_energy = (1.0 - mix_factor) * activation.energy + mix_factor * pseudo_kinetic_energy;
activation.energy = new_energy.min(activation.threshold.abs() * 4.0);
fn update_energy(activation: &mut RigidBodyActivation, sq_linvel: Real, sq_angvel: Real, dt: Real) {
if sq_linvel < activation.linear_threshold * activation.linear_threshold
&& sq_angvel < activation.angular_threshold * activation.angular_threshold
{
activation.time_since_can_sleep += dt;
} else {
activation.time_since_can_sleep = 0.0;
}
}

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

@@ -100,7 +100,7 @@ impl ImpulseJointSet {
/// Gets the joint with the given handle without a known generation.
///
/// This is useful when you know you want the joint at position `i` but
/// This is useful when you know you want the joint at index `i` but
/// don't know what is its current generation number. Generation numbers are
/// used to protect from the ABA problem because the joint position `i`
/// are recycled between two insertion and a removal.

5
src/dynamics/joint/joint_data.rs
View File

@@ -218,12 +218,14 @@ impl JointData {
}
/// Set the spring-like model used by the motor to reach the desired target velocity and position.
#[must_use]
pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self {
self.motors[axis as usize].model = model;
self
}
/// Sets the target velocity this motor needs to reach.
#[must_use]
pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self {
self.motor_axis(
axis,
@@ -235,6 +237,7 @@ impl JointData {
}
/// Sets the target angle this motor needs to reach.
#[must_use]
pub fn motor_position(
self,
axis: JointAxis,
@@ -246,6 +249,7 @@ impl JointData {
}
/// Configure both the target angle and target velocity of the motor.
#[must_use]
pub fn motor_axis(
mut self,
axis: JointAxis,
@@ -263,6 +267,7 @@ impl JointData {
self
}
#[must_use]
pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self {
self.motors[axis as usize].max_impulse = max_impulse;
self

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

@@ -62,7 +62,10 @@ fn concat_rb_mass_matrix(
}
/// An articulated body simulated using the reduced-coordinates approach.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct Multibody {
// TODO: serialization: skip the workspace fields.
links: MultibodyLinkVec,
pub(crate) velocities: DVector<Real>,
pub(crate) damping: DVector<Real>,

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

@@ -11,6 +11,7 @@ use na::{DVector, DVectorSliceMut};
#[cfg(feature = "dim3")]
use na::{UnitQuaternion, Vector3};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug)]
pub struct MultibodyJoint {
pub data: JointData,

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

@@ -1,4 +1,4 @@
use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut};
use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index};
use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{
IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
@@ -6,19 +6,18 @@ use crate::dynamics::{
};
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
use crate::parry::partitioning::IndexedData;
use std::ops::Index;
/// The unique handle of an multibody_joint added to a `MultibodyJointSet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct MultibodyJointHandle(pub crate::data::arena::Index);
pub struct MultibodyJointHandle(pub Index);
/// The temporary index of a multibody added to a `MultibodyJointSet`.
#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[repr(transparent)]
pub struct MultibodyIndex(pub crate::data::arena::Index);
pub struct MultibodyIndex(pub Index);
impl MultibodyJointHandle {
/// Converts this handle into its (index, generation) components.
@@ -28,12 +27,12 @@ impl MultibodyJointHandle {
/// Reconstructs an handle from its (index, generation) components.
pub fn from_raw_parts(id: u32, generation: u32) -> Self {
Self(crate::data::arena::Index::from_raw_parts(id, generation))
Self(Index::from_raw_parts(id, generation))
}
/// An always-invalid rigid-body handle.
pub fn invalid() -> Self {
Self(crate::data::arena::Index::from_raw_parts(
Self(Index::from_raw_parts(
crate::INVALID_U32,
crate::INVALID_U32,
))
@@ -55,6 +54,7 @@ impl IndexedData for MultibodyJointHandle {
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct MultibodyJointLink {
pub graph_id: RigidBodyGraphIndex,
@@ -66,7 +66,7 @@ impl Default for MultibodyJointLink {
fn default() -> Self {
Self {
graph_id: RigidBodyGraphIndex::new(crate::INVALID_U32),
multibody: MultibodyIndex(crate::data::arena::Index::from_raw_parts(
multibody: MultibodyIndex(Index::from_raw_parts(
crate::INVALID_U32,
crate::INVALID_U32,
)),
@@ -76,6 +76,8 @@ impl Default for MultibodyJointLink {
}
/// A set of rigid bodies that can be handled by a physics pipeline.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct MultibodyJointSet {
pub(crate) multibodies: Arena<Multibody>, // NOTE: a Slab would be sufficient.
pub(crate) rb2mb: Coarena<MultibodyJointLink>,
@@ -316,6 +318,26 @@ impl MultibodyJointSet {
Some((multibody, link.id))
}
/// Gets the joint with the given handle without a known generation.
///
/// This is useful when you know you want the joint at index `i` but
/// don't know what is its current generation number. Generation numbers are
/// used to protect from the ABA problem because the joint position `i`
/// are recycled between two insertion and a removal.
///
/// Using this is discouraged in favor of `self.get(handle)` which does not
/// suffer form the ABA problem.
pub fn get_unknown_gen(&self, i: u32) -> Option<(&Multibody, usize, MultibodyJointHandle)> {
let link = self.rb2mb.get_unknown_gen(i)?;
let gen = self.rb2mb.get_gen(i)?;
let multibody = self.multibodies.get(link.multibody.0)?;
Some((
multibody,
link.id,
MultibodyJointHandle(Index::from_raw_parts(i, gen)),
))
}
/// Iterate through the handles of all the rigid-bodies attached to this rigid-body
/// by an multibody_joint.
pub fn attached_bodies<'a>(
@@ -335,7 +357,7 @@ impl MultibodyJointSet {
}
}
impl Index<MultibodyIndex> for MultibodyJointSet {
impl std::ops::Index<MultibodyIndex> for MultibodyJointSet {
type Output = Multibody;
fn index(&self, index: MultibodyIndex) -> &Multibody {

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

@@ -5,6 +5,8 @@ use crate::math::{Isometry, Real, Vector};
use crate::prelude::RigidBodyVelocity;
/// One link of a multibody.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub struct MultibodyLink {
// FIXME: make all those private.
pub(crate) internal_id: usize,
@@ -96,6 +98,8 @@ impl MultibodyLink {
}
// FIXME: keep this even if we already have the Index2 traits?
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
pub(crate) struct MultibodyLinkVec(pub Vec<MultibodyLink>);
impl MultibodyLinkVec {

3
src/dynamics/rigid_body.rs
View File

@@ -1089,7 +1089,8 @@ impl RigidBodyBuilder {
}
if !self.can_sleep {
rb.rb_activation.threshold = -1.0;
rb.rb_activation.linear_threshold = -1.0;
rb.rb_activation.angular_threshold = -1.0;
}
rb

50
src/dynamics/rigid_body_components.rs
View File

@@ -927,11 +927,13 @@ impl RigidBodyDominance {
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct RigidBodyActivation {
/// The threshold pseudo-kinetic energy bellow which the body can fall asleep.
pub threshold: Real,
/// The current pseudo-kinetic energy of the body.
pub energy: Real,
/// Is this body already sleeping?
/// The threshold linear velocity bellow which the body can fall asleep.
pub linear_threshold: Real,
/// The angular linear velocity bellow which the body can fall asleep.
pub angular_threshold: Real,
/// Since how much time can this body sleep?
pub time_since_can_sleep: Real,
/// Is this body sleeping?
pub sleeping: bool,
}
@@ -942,16 +944,28 @@ impl Default for RigidBodyActivation {
}
impl RigidBodyActivation {
/// The default amount of energy bellow which a body can be put to sleep by rapier.
pub fn default_threshold() -> Real {
0.01
/// The default linear velocity bellow which a body can be put to sleep.
pub fn default_linear_threshold() -> Real {
0.4
}
/// The default angular velocity bellow which a body can be put to sleep.
pub fn default_angular_threshold() -> Real {
0.5
}
/// The amount of time the rigid-body must remain bellow it’s linear and angular velocity
/// threshold before falling to sleep.
pub fn default_time_until_sleep() -> Real {
2.0
}
/// Create a new rb_activation status initialised with the default rb_activation threshold and is active.
pub fn active() -> Self {
RigidBodyActivation {
threshold: Self::default_threshold(),
energy: Self::default_threshold() * 4.0,
linear_threshold: Self::default_linear_threshold(),
angular_threshold: Self::default_angular_threshold(),
time_since_can_sleep: 0.0,
sleeping: false,
}
}
@@ -959,16 +973,18 @@ impl RigidBodyActivation {
/// Create a new rb_activation status initialised with the default rb_activation threshold and is inactive.
pub fn inactive() -> Self {
RigidBodyActivation {
threshold: Self::default_threshold(),
energy: 0.0,
linear_threshold: Self::default_linear_threshold(),
angular_threshold: Self::default_angular_threshold(),
sleeping: true,
time_since_can_sleep: Self::default_time_until_sleep(),
}
}
/// Create a new activation status that prevents the rigid-body from sleeping.
pub fn cannot_sleep() -> Self {
RigidBodyActivation {
threshold: -Real::MAX,
linear_threshold: -1.0,
angular_threshold: -1.0,
..Self::active()
}
}
@@ -976,22 +992,22 @@ impl RigidBodyActivation {
/// Returns `true` if the body is not asleep.
#[inline]
pub fn is_active(&self) -> bool {
self.energy != 0.0
!self.sleeping
}
/// Wakes up this rigid-body.
#[inline]
pub fn wake_up(&mut self, strong: bool) {
self.sleeping = false;
if strong || self.energy == 0.0 {
self.energy = self.threshold.abs() * 2.0;
if strong {
self.time_since_can_sleep = 0.0;
}
}
/// Put this rigid-body to sleep.
#[inline]
pub fn sleep(&mut self) {
self.energy = 0.0;
self.sleeping = true;
self.time_since_can_sleep = Self::default_time_until_sleep();
}
}

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

@@ -9,8 +9,8 @@ pub(crate) fn categorize_contacts(
manifold_indices: &[ContactManifoldIndex],
out_ground: &mut Vec<ContactManifoldIndex>,
out_not_ground: &mut Vec<ContactManifoldIndex>,
out_generic: &mut Vec<ContactManifoldIndex>,
_unused: &mut Vec<ContactManifoldIndex>, // Unused but useful to simplify the parallel code.
out_generic_ground: &mut Vec<ContactManifoldIndex>,
out_generic_not_ground: &mut Vec<ContactManifoldIndex>,
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
@@ -18,15 +18,19 @@ pub(crate) fn categorize_contacts(
if manifold
.data
.rigid_body1
.map(|rb| multibody_joints.rigid_body_link(rb))
.and_then(|h| multibody_joints.rigid_body_link(h))
.is_some()
|| manifold
.data
.rigid_body1
.map(|rb| multibody_joints.rigid_body_link(rb))
.rigid_body2
.and_then(|h| multibody_joints.rigid_body_link(h))
.is_some()
{
out_generic.push(*manifold_i);
if manifold.data.relative_dominance != 0 {
out_generic_ground.push(*manifold_i);
} else {
out_generic_not_ground.push(*manifold_i);
}
} else if manifold.data.relative_dominance != 0 {
out_ground.push(*manifold_i)
} else {

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

@@ -1,7 +1,7 @@
use crate::math::{AngVector, Vector, SPATIAL_DIM};
use na::{DVectorSlice, DVectorSliceMut};
use na::{Scalar, SimdRealField};
use std::ops::AddAssign;
use std::ops::{AddAssign, Sub};
#[derive(Copy, Clone, Debug)]
#[repr(C)]
@@ -44,3 +44,14 @@ impl<N: SimdRealField + Copy> AddAssign for DeltaVel<N> {
self.angular += rhs.angular;
}
}
impl<N: SimdRealField + Copy> Sub for DeltaVel<N> {
type Output = Self;
fn sub(self, rhs: Self) -> Self {
DeltaVel {
linear: self.linear - rhs.linear,
angular: self.angular - rhs.angular,
}
}
}

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

@@ -9,10 +9,60 @@ use crate::math::{Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::{WAngularInertia, WCross, WDot};
use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
use crate::dynamics::solver::GenericVelocityGroundConstraint;
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use na::DVector;
#[derive(Copy, Clone, Debug)]
pub(crate) enum AnyGenericVelocityConstraint {
NongroupedGround(GenericVelocityGroundConstraint),
Nongrouped(GenericVelocityConstraint),
}
impl AnyGenericVelocityConstraint {
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
mj_lambdas: &mut [DeltaVel<Real>],
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
match self {
AnyGenericVelocityConstraint::Nongrouped(c) => c.solve(
jacobians,
mj_lambdas,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
AnyGenericVelocityConstraint::NongroupedGround(c) => c.solve(
jacobians,
generic_mj_lambdas,
solve_restitution,
solve_friction,
),
}
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
match self {
AnyGenericVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifolds_all),
AnyGenericVelocityConstraint::NongroupedGround(c) => {
c.writeback_impulses(manifolds_all)
}
}
}
pub fn remove_bias_from_rhs(&mut self) {
match self {
AnyGenericVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(),
AnyGenericVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(),
}
}
}
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericVelocityConstraint {
// We just build the generic constraint on top of the velocity constraint,
@@ -31,7 +81,7 @@ impl GenericVelocityConstraint {
manifold: &ContactManifold,
bodies: &Bodies,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<GenericVelocityConstraint>,
out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
push: bool,
@@ -293,6 +343,9 @@ impl GenericVelocityConstraint {
(vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
constraint.elements[k].tangent_part.rhs[j] = rhs;
// 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.elements[k].tangent_part.r[j] = r;
}
}
@@ -316,9 +369,10 @@ impl GenericVelocityConstraint {
};
if push {
out_constraints.push(constraint);
out_constraints.push(AnyGenericVelocityConstraint::Nongrouped(constraint));
} else {
out_constraints[manifold.data.constraint_index + _l] = constraint;
out_constraints[manifold.data.constraint_index + _l] =
AnyGenericVelocityConstraint::Nongrouped(constraint);
}
}
}

29
src/dynamics/solver/generic_velocity_constraint_element.rs
View File

@@ -41,7 +41,7 @@ fn tangent_j_id(j_id: usize, ndofs1: usize, ndofs2: usize) -> usize {
impl GenericRhs {
#[inline(always)]
fn dimpulse(
fn dvel(
&self,
j_id: usize,
ndofs: usize,
@@ -110,14 +110,14 @@ impl VelocityConstraintTangentPart<Real> {
#[cfg(feature = "dim2")]
{
let dimpulse_0 = mj_lambda1.dimpulse(
let dvel_0 = mj_lambda1.dvel(
j_id1,
ndofs1,
jacobians,
&tangents1[0],
&self.gcross1[0],
mj_lambdas,
) + mj_lambda2.dimpulse(
) + mj_lambda2.dvel(
j_id2,
ndofs2,
jacobians,
@@ -126,7 +126,7 @@ impl VelocityConstraintTangentPart<Real> {
mj_lambdas,
) + self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dimpulse_0).simd_clamp(-limit, limit);
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;
@@ -154,14 +154,14 @@ impl VelocityConstraintTangentPart<Real> {
#[cfg(feature = "dim3")]
{
let dimpulse_0 = mj_lambda1.dimpulse(
let dvel_0 = mj_lambda1.dvel(
j_id1,
ndofs1,
jacobians,
&tangents1[0],
&self.gcross1[0],
mj_lambdas,
) + mj_lambda2.dimpulse(
) + mj_lambda2.dvel(
j_id2,
ndofs2,
jacobians,
@@ -169,14 +169,14 @@ impl VelocityConstraintTangentPart<Real> {
&self.gcross2[0],
mj_lambdas,
) + self.rhs[0];
let dimpulse_1 = mj_lambda1.dimpulse(
let dvel_1 = mj_lambda1.dvel(
j_id1 + j_step,
ndofs1,
jacobians,
&tangents1[1],
&self.gcross1[1],
mj_lambdas,
) + mj_lambda2.dimpulse(
) + mj_lambda2.dvel(
j_id2 + j_step,
ndofs2,
jacobians,
@@ -186,8 +186,8 @@ impl VelocityConstraintTangentPart<Real> {
) + self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dimpulse_0,
self.impulse[1] - self.r[1] * dimpulse_1,
self.impulse[0] - self.r[0] * dvel_0,
self.impulse[1] - self.r[1] * dvel_1,
);
let new_impulse = new_impulse.cap_magnitude(limit);
@@ -257,12 +257,11 @@ impl VelocityConstraintNormalPart<Real> {
let j_id1 = j_id1(j_id, ndofs1, ndofs2);
let j_id2 = j_id2(j_id, ndofs1, ndofs2);
let dimpulse =
mj_lambda1.dimpulse(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
+ mj_lambda2.dimpulse(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+ self.rhs;
let dvel = mj_lambda1.dvel(j_id1, ndofs1, jacobians, &dir1, &self.gcross1, mj_lambdas)
+ mj_lambda2.dvel(j_id2, ndofs2, jacobians, &-dir1, &self.gcross2, mj_lambdas)
+ self.rhs;
let new_impulse = (self.impulse - self.r * dimpulse).max(0.0);
let new_impulse = (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;

242
src/dynamics/solver/generic_velocity_ground_constraint.rs Normal file
View File

@@ -0,0 +1,242 @@
use crate::data::{BundleSet, ComponentSet};
use crate::dynamics::solver::VelocityGroundConstraint;
use crate::dynamics::{
IntegrationParameters, MultibodyJointSet, RigidBodyIds, RigidBodyMassProps, RigidBodyType,
RigidBodyVelocity,
};
use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Point, Real, DIM, MAX_MANIFOLD_POINTS};
use crate::utils::WCross;
use super::{VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart};
use crate::dynamics::solver::AnyGenericVelocityConstraint;
#[cfg(feature = "dim2")]
use crate::utils::WBasis;
use na::DVector;
#[derive(Copy, Clone, Debug)]
pub(crate) struct GenericVelocityGroundConstraint {
// We just build the generic constraint on top of the velocity constraint,
// adding some information we can use in the generic case.
pub velocity_constraint: VelocityGroundConstraint,
pub j_id: usize,
pub ndofs2: usize,
}
impl GenericVelocityGroundConstraint {
pub fn generate<Bodies>(
params: &IntegrationParameters,
manifold_id: ContactManifoldIndex,
manifold: &ContactManifold,
bodies: &Bodies,
multibodies: &MultibodyJointSet,
out_constraints: &mut Vec<AnyGenericVelocityConstraint>,
jacobians: &mut DVector<Real>,
jacobian_id: &mut usize,
push: bool,
) where
Bodies: ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyType>,
{
let inv_dt = params.inv_dt();
let erp_inv_dt = params.erp_inv_dt();
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 (rb_vels1, world_com1) = if let Some(handle1) = handle1 {
let (vels1, mprops1): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle1.0);
(*vels1, mprops1.world_com)
} else {
(RigidBodyVelocity::zero(), Point::origin())
};
let (rb_vels2, rb_mprops2): (&RigidBodyVelocity, &RigidBodyMassProps) =
bodies.index_bundle(handle2.unwrap().0);
let (mb2, link_id2) = handle2
.and_then(|h| multibodies.rigid_body_link(h))
.map(|m| (&multibodies[m.multibody], m.id))
.unwrap();
let mj_lambda2 = mb2.solver_id;
#[cfg(feature = "dim2")]
let tangents1 = force_dir1.orthonormal_basis();
#[cfg(feature = "dim3")]
let tangents1 = super::compute_tangent_contact_directions(
&force_dir1,
&rb_vels1.linvel,
&rb_vels2.linvel,
);
let 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 {
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 mut constraint = VelocityGroundConstraint {
dir1: force_dir1,
#[cfg(feature = "dim3")]
tangent1: tangents1[0],
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb_mprops2.effective_inv_mass,
limit: 0.0,
mj_lambda2,
manifold_id,
manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8,
};
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = manifold_point.point - world_com1;
let dp2 = manifold_point.point - rb_mprops2.world_com;
let vel1 = rb_vels1.linvel + rb_vels1.angvel.gcross(dp1);
let vel2 = rb_vels2.linvel + rb_vels2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
{
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 is_resting = 1.0 - is_bouncy;
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);
constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
gcross2: na::zero(), // Unused for generic constraints.
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
impulse: na::zero(),
r,
};
}
// Tangent parts.
{
constraint.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 = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.elements[k].tangent_part.rhs[j] = rhs;
// 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.elements[k].tangent_part.r[j] = r;
}
}
}
let constraint = GenericVelocityGroundConstraint {
velocity_constraint: constraint,
j_id: chunk_j_id,
ndofs2: mb2.ndofs(),
};
if push {
out_constraints.push(AnyGenericVelocityConstraint::NongroupedGround(constraint));
} else {
out_constraints[manifold.data.constraint_index + _l] =
AnyGenericVelocityConstraint::NongroupedGround(constraint);
}
}
}
pub fn solve(
&mut self,
jacobians: &DVector<Real>,
generic_mj_lambdas: &mut DVector<Real>,
solve_restitution: bool,
solve_friction: bool,
) {
let mj_lambda2 = self.velocity_constraint.mj_lambda2 as usize;
let elements = &mut self.velocity_constraint.elements
[..self.velocity_constraint.num_contacts as usize];
VelocityGroundConstraintElement::generic_solve_group(
elements,
jacobians,
self.velocity_constraint.limit,
self.ndofs2,
self.j_id,
mj_lambda2,
generic_mj_lambdas,
solve_restitution,
solve_friction,
);
}
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
self.velocity_constraint.writeback_impulses(manifolds_all);
}
pub fn remove_bias_from_rhs(&mut self) {
self.velocity_constraint.remove_bias_from_rhs();
}
}

141
src/dynamics/solver/generic_velocity_ground_constraint_element.rs Normal file
View File

@@ -0,0 +1,141 @@
use crate::dynamics::solver::{
VelocityGroundConstraintElement, VelocityGroundConstraintNormalPart,
VelocityGroundConstraintTangentPart,
};
use crate::math::{Real, DIM};
use na::DVector;
#[cfg(feature = "dim2")]
use na::SimdPartialOrd;
impl VelocityGroundConstraintTangentPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
limit: Real,
mj_lambda2: usize,
mj_lambdas: &mut DVector<Real>,
) {
#[cfg(feature = "dim2")]
{
let dvel_0 = jacobians
.rows(j_id2, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, 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;
mj_lambdas.rows_mut(mj_lambda2, 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(&mj_lambdas.rows(mj_lambda2, ndofs2))
+ self.rhs[0];
let dvel_1 = jacobians
.rows(j_id2 + j_step, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, 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;
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
dlambda[0],
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
dlambda[1],
&jacobians.rows(j_id2 + j_step + ndofs2, ndofs2),
1.0,
);
}
}
}
impl VelocityGroundConstraintNormalPart<Real> {
#[inline]
pub fn generic_solve(
&mut self,
j_id2: usize,
jacobians: &DVector<Real>,
ndofs2: usize,
mj_lambda2: usize,
mj_lambdas: &mut DVector<Real>,
) {
let dvel = jacobians
.rows(j_id2, ndofs2)
.dot(&mj_lambdas.rows(mj_lambda2, ndofs2))
+ self.rhs;
let new_impulse = (self.impulse - self.r * dvel).max(0.0);
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambdas.rows_mut(mj_lambda2, ndofs2).axpy(
dlambda,
&jacobians.rows(j_id2 + ndofs2, ndofs2),
1.0,
);
}
}
impl VelocityGroundConstraintElement<Real> {
#[inline]
pub fn generic_solve_group(
elements: &mut [Self],
jacobians: &DVector<Real>,
limit: Real,
ndofs2: usize,
// Jacobian index of the first constraint.
j_id: usize,
mj_lambda2: usize,
mj_lambdas: &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(nrm_j_id, jacobians, ndofs2, mj_lambda2, mj_lambdas);
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, mj_lambda2, mj_lambdas);
tng_j_id += j_step;
}
}
}
}

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

@@ -2,7 +2,8 @@ use super::VelocitySolver;
use crate::counters::Counters;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{
AnyJointVelocityConstraint, AnyVelocityConstraint, GenericVelocityConstraint, SolverConstraints,
AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
SolverConstraints,
};
use crate::dynamics::{
IntegrationParameters, JointGraphEdge, JointIndex, RigidBodyDamping, RigidBodyForces,
@@ -13,7 +14,7 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::prelude::{MultibodyJointSet, RigidBodyActivation};
pub struct IslandSolver {
contact_constraints: SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint>,
contact_constraints: SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint>,
joint_constraints: SolverConstraints<AnyJointVelocityConstraint, ()>,
velocity_solver: VelocitySolver,
}

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

@@ -13,6 +13,7 @@ pub(self) use self::velocity_solver::VelocitySolver;
pub(self) use delta_vel::DeltaVel;
pub(self) use generic_velocity_constraint::*;
pub(self) use generic_velocity_constraint_element::*;
pub(self) use generic_velocity_ground_constraint::*;
pub(self) use interaction_groups::*;
pub(crate) use joint_constraint::MotorParameters;
pub use joint_constraint::*;
@@ -29,6 +30,8 @@ mod categorization;
mod delta_vel;
mod generic_velocity_constraint;
mod generic_velocity_constraint_element;
mod generic_velocity_ground_constraint;
mod generic_velocity_ground_constraint_element;
mod interaction_groups;
#[cfg(not(feature = "parallel"))]
mod island_solver;

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

@@ -5,6 +5,8 @@ use super::{
use super::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::data::ComponentSet;
use crate::dynamics::solver::categorization::{categorize_contacts, categorize_joints};
use crate::dynamics::solver::generic_velocity_ground_constraint::GenericVelocityGroundConstraint;
use crate::dynamics::solver::AnyGenericVelocityConstraint;
use crate::dynamics::solver::GenericVelocityConstraint;
use crate::dynamics::{
solver::AnyVelocityConstraint, IntegrationParameters, JointGraphEdge, JointIndex,
@@ -58,7 +60,7 @@ impl<VelocityConstraint, GenVelocityConstraint>
}
}
impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
impl SolverConstraints<AnyVelocityConstraint, AnyGenericVelocityConstraint> {
pub fn init_constraint_groups<Bodies>(
&mut self,
island_id: usize,
@@ -82,8 +84,8 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
manifold_indices,
&mut self.ground_interactions,
&mut self.not_ground_interactions,
&mut self.generic_not_ground_interactions,
&mut self.generic_ground_interactions,
&mut self.generic_not_ground_interactions,
);
self.interaction_groups.clear_groups();
@@ -141,18 +143,32 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
manifold_indices,
);
let mut jacobian_id = 0;
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_constraints(params, bodies, manifolds);
}
self.compute_nongrouped_constraints(params, bodies, manifolds);
self.compute_generic_constraints(params, bodies, multibody_joints, manifolds);
self.compute_generic_constraints(
params,
bodies,
multibody_joints,
manifolds,
&mut jacobian_id,
);
#[cfg(feature = "simd-is-enabled")]
{
self.compute_grouped_ground_constraints(params, bodies, manifolds);
}
self.compute_nongrouped_ground_constraints(params, bodies, manifolds);
self.compute_generic_ground_constraints(
params,
bodies,
multibody_joints,
manifolds,
&mut jacobian_id,
);
}
#[cfg(feature = "simd-is-enabled")]
@@ -215,6 +231,7 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
bodies: &Bodies,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
@@ -222,7 +239,6 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
let mut jacobian_id = 0;
for manifold_i in &self.generic_not_ground_interactions {
let manifold = &manifolds_all[*manifold_i];
GenericVelocityConstraint::generate(
@@ -233,7 +249,37 @@ impl SolverConstraints<AnyVelocityConstraint, GenericVelocityConstraint> {
multibody_joints,
&mut self.generic_velocity_constraints,
&mut self.generic_jacobians,
&mut jacobian_id,
jacobian_id,
true,
);
}
}
fn compute_generic_ground_constraints<Bodies>(
&mut self,
params: &IntegrationParameters,
bodies: &Bodies,
multibody_joints: &MultibodyJointSet,
manifolds_all: &[&mut ContactManifold],
jacobian_id: &mut usize,
) where
Bodies: ComponentSet<RigidBodyVelocity>
+ ComponentSet<RigidBodyPosition>
+ ComponentSet<RigidBodyMassProps>
+ ComponentSet<RigidBodyIds>
+ ComponentSet<RigidBodyType>,
{
for manifold_i in &self.generic_ground_interactions {
let manifold = &manifolds_all[*manifold_i];
GenericVelocityGroundConstraint::generate(
params,
*manifold_i,
manifold,
bodies,
multibody_joints,
&mut self.generic_velocity_constraints,
&mut self.generic_jacobians,
jacobian_id,
true,
);
}

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

@@ -236,7 +236,7 @@ impl VelocityConstraint {
.transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = 1.0
let r = params.delassus_inv_factor
/ (force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
@@ -246,8 +246,7 @@ impl VelocityConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias +=
(manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
let rhs_bias = /* is_resting
* */ erp_inv_dt
@@ -275,17 +274,26 @@ impl VelocityConstraint {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let r = 1.0
/ (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs =
(vel1 - vel2 + 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[j] = rhs;
constraint.elements[k].tangent_part.r[j] = r;
constraint.elements[k].tangent_part.r[j] =
if cfg!(feature = "dim2") { 1.0 / 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]));
}
}
}

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

@@ -12,7 +12,10 @@ pub(crate) struct VelocityConstraintTangentPart<N: SimdRealField + Copy> {
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
pub r: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
@@ -22,7 +25,10 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
impulse: na::zero(),
r: [na::zero(); DIM - 1],
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
@@ -41,12 +47,12 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
{
#[cfg(feature = "dim2")]
{
let dimpulse = tangents1[0].dot(&mj_lambda1.linear)
let dvel = tangents1[0].dot(&mj_lambda1.linear)
+ self.gcross1[0].gdot(mj_lambda1.angular)
- tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit);
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;
@@ -59,25 +65,30 @@ impl<N: SimdRealField + Copy> VelocityConstraintTangentPart<N> {
#[cfg(feature = "dim3")]
{
let dimpulse_0 = tangents1[0].dot(&mj_lambda1.linear)
let dvel_0 = tangents1[0].dot(&mj_lambda1.linear)
+ self.gcross1[0].gdot(mj_lambda1.angular)
- tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let dimpulse_1 = tangents1[1].dot(&mj_lambda1.linear)
let dvel_1 = tangents1[1].dot(&mj_lambda1.linear)
+ self.gcross1[1].gdot(mj_lambda1.angular)
- tangents1[1].dot(&mj_lambda2.linear)
+ self.gcross2[1].gdot(mj_lambda2.angular)
+ self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dimpulse_0,
self.impulse[1] - self.r[1] * dimpulse_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();
crate::utils::DisableFloatingPointExceptionsFlags::
disable_floating_point_exceptions();
new_impulse.simd_cap_magnitude(limit)
};
@@ -128,11 +139,11 @@ impl<N: SimdRealField + Copy> VelocityConstraintNormalPart<N> {
) where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dimpulse = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular)
let dvel = dir1.dot(&mj_lambda1.linear) + self.gcross1.gdot(mj_lambda1.angular)
- dir1.dot(&mj_lambda2.linear)
+ self.gcross2.gdot(mj_lambda2.angular)
+ self.rhs;
let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero());
let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;

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

@@ -49,6 +49,7 @@ impl WVelocityConstraint {
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()];
@@ -136,14 +137,14 @@ impl WVelocityConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let imsum = im1 + im2;
let r = SimdReal::splat(1.0)
let r = delassus_inv_factor
/ (force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
@@ -165,16 +166,28 @@ impl WVelocityConstraint {
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let imsum = im1 + im2;
let r = SimdReal::splat(1.0)
/ (tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j]))
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2);
let rhs = (vel1 - vel2 + 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[j] = rhs;
constraint.elements[k].tangent_part.r[j] = r;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
SimdReal::splat(1.0) / 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]));
}
}

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

@@ -153,7 +153,7 @@ impl VelocityGroundConstraint {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-force_dir1));
let r = 1.0
let r = params.delassus_inv_factor
/ (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
+ gcross2.gdot(gcross2));
@@ -162,8 +162,7 @@ impl VelocityGroundConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias +=
(manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
let rhs_bias = /* is_resting
* */ erp_inv_dt
@@ -186,17 +185,24 @@ impl VelocityGroundConstraint {
let gcross2 = mprops2
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0
/ (tangents1[j]
.dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2));
let r = tangents1[j]
.dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2);
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = r;
constraint.elements[k].tangent_part.r[j] =
if cfg!(feature = "dim2") { 1.0 / 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]);
}
}
}

40
src/dynamics/solver/velocity_ground_constraint_element.rs
View File

@@ -11,17 +11,22 @@ pub(crate) struct VelocityGroundConstraintTangentPart<N: SimdRealField + Copy> {
pub impulse: na::Vector1<N>,
#[cfg(feature = "dim3")]
pub impulse: na::Vector2<N>,
pub r: [N; DIM - 1],
#[cfg(feature = "dim2")]
pub r: [N; 1],
#[cfg(feature = "dim3")]
pub r: [N; DIM],
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
fn zero() -> Self {
Self {
gcross2: [na::zero(); DIM - 1],
rhs: [na::zero(); DIM - 1],
impulse: na::zero(),
r: [na::zero(); DIM - 1],
#[cfg(feature = "dim2")]
r: [na::zero(); 1],
#[cfg(feature = "dim3")]
r: [na::zero(); DIM],
}
}
@@ -38,10 +43,10 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
{
#[cfg(feature = "dim2")]
{
let dimpulse = -tangents1[0].dot(&mj_lambda2.linear)
let dvel = -tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let new_impulse = (self.impulse[0] - self.r[0] * dimpulse).simd_clamp(-limit, limit);
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;
@@ -51,17 +56,22 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
#[cfg(feature = "dim3")]
{
let dimpulse_0 = -tangents1[0].dot(&mj_lambda2.linear)
let dvel_0 = -tangents1[0].dot(&mj_lambda2.linear)
+ self.gcross2[0].gdot(mj_lambda2.angular)
+ self.rhs[0];
let dimpulse_1 = -tangents1[1].dot(&mj_lambda2.linear)
let dvel_1 = -tangents1[1].dot(&mj_lambda2.linear)
+ self.gcross2[1].gdot(mj_lambda2.angular)
+ self.rhs[1];
let new_impulse = na::Vector2::new(
self.impulse[0] - self.r[0] * dimpulse_0,
self.impulse[1] - self.r[1] * dimpulse_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::
@@ -69,7 +79,6 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintTangentPart<N> {
new_impulse.simd_cap_magnitude(limit)
};
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
mj_lambda2.linear += tangents1[0].component_mul(im2) * -dlambda[0]
@@ -89,7 +98,6 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: SimdRealField + Copy> {
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
fn zero() -> Self {
Self {
gcross2: na::zero(),
@@ -105,9 +113,8 @@ impl<N: SimdRealField + Copy> VelocityGroundConstraintNormalPart<N> {
where
AngVector<N>: WDot<AngVector<N>, Result = N>,
{
let dimpulse =
-dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = (self.impulse - self.r * dimpulse).simd_max(N::zero());
let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
let new_impulse = (self.impulse - self.r * dvel).simd_max(N::zero());
let dlambda = new_impulse - self.impulse;
self.impulse = new_impulse;
@@ -123,7 +130,6 @@ pub(crate) struct VelocityGroundConstraintElement<N: SimdRealField + Copy> {
}
impl<N: SimdRealField + Copy> VelocityGroundConstraintElement<N> {
#[cfg(any(not(target_arch = "wasm32"), feature = "simd-is-enabled"))]
pub fn zero() -> Self {
Self {
normal_part: VelocityGroundConstraintNormalPart::zero(),

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

@@ -44,6 +44,7 @@ impl WVelocityGroundConstraint {
let inv_dt = SimdReal::splat(params.inv_dt());
let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction);
let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor);
let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
let mut handles1 = gather![|ii| manifolds[ii].data.rigid_body1];
@@ -142,12 +143,12 @@ impl WVelocityGroundConstraint {
{
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));
let r = SimdReal::splat(1.0)
let r = delassus_inv_factor
/ (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2));
let projected_velocity = (vel1 - vel2).dot(&force_dir1);
let mut rhs_wo_bias =
(SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity;
rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction;
let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero())
* (erp_inv_dt/* * is_resting */);
@@ -166,14 +167,24 @@ impl WVelocityGroundConstraint {
for j in 0..DIM - 1 {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdReal::splat(1.0)
/ (tangents1[j].dot(&im2.component_mul(&tangents1[j]))
+ gcross2.gdot(gcross2));
let r =
tangents1[j].dot(&im2.component_mul(&tangents1[j])) + gcross2.gdot(gcross2);
let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.r[j] = r;
constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
SimdReal::splat(1.0) / 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]);
}
}

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

@@ -1,6 +1,6 @@
use super::AnyJointVelocityConstraint;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut};
use crate::dynamics::solver::GenericVelocityConstraint;
use crate::dynamics::solver::AnyGenericVelocityConstraint;
use crate::dynamics::{
solver::{AnyVelocityConstraint, DeltaVel},
IntegrationParameters, JointGraphEdge, MultibodyJointSet, RigidBodyForces, RigidBodyType,
@@ -36,7 +36,7 @@ impl VelocitySolver {
manifolds_all: &mut [&mut ContactManifold],
joints_all: &mut [JointGraphEdge],
contact_constraints: &mut [AnyVelocityConstraint],
generic_contact_constraints: &mut [GenericVelocityConstraint],
generic_contact_constraints: &mut [AnyGenericVelocityConstraint],
generic_contact_jacobians: &DVector<Real>,
joint_constraints: &mut [AnyJointVelocityConstraint],
generic_joint_jacobians: &DVector<Real>,