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

Merge pull request #277 from dimforge/solver-fixes

Browse Source 
Fix some solver issues
This commit is contained in:
Sébastien Crozet
2022-01-16 08:20:22 -08:00
committed by GitHub
parent 4454a845e9 0703e5527f
commit 1880619d29
43 changed files with 936 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
crates/rapier_testbed2d/Cargo.toml
View File

@@ -21,7 +21,7 @@ required-features = [ "dim2" ]
[features]
default = [ "dim2" ]
dim2 = [ ]
parallel = [ "rapier2d/parallel", "num_cpus" ]
parallel = [ "rapier/parallel", "num_cpus" ]
other-backends = [ "wrapped2d" ]
@@ -33,7 +33,6 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
parry2d = "0.8"
crossbeam = "0.8"
bincode = "1"
Inflector = "0.11"
@@ -51,7 +50,8 @@ bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "re
bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render"]}
#bevy_webgl2 = "0.5"
[dependencies.rapier2d]
[dependencies.rapier]
package = "rapier2d"
path = "../rapier2d"
version = "0.12.0-alpha.0"
features = [ "serde-serialize" ]

6
crates/rapier_testbed3d/Cargo.toml
View File

@@ -21,7 +21,7 @@ required-features = [ "dim3" ]
[features]
default = [ "dim3" ]
dim3 = [ ]
parallel = [ "rapier3d/parallel", "num_cpus" ]
parallel = [ "rapier/parallel", "num_cpus" ]
other-backends = [ "physx", "physx-sys", "glam" ]
[dependencies]
@@ -32,7 +32,6 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
glam = { version = "0.12", optional = true }
num_cpus = { version = "1", optional = true }
parry3d = "0.8"
physx = { version = "0.11", optional = true }
physx-sys = { version = "0.4", optional = true }
crossbeam = "0.8"
@@ -53,7 +52,8 @@ bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "re
bevy = {version = "0.6", default-features = false, features = ["bevy_winit", "render"]}
#bevy_webgl2 = "0.5"
[dependencies.rapier3d]
[dependencies.rapier]
package = "rapier3d"
path = "../rapier3d"
version = "0.12.0-alpha.0"
features = [ "serde-serialize" ]

2
examples2d/all_examples2.rs
View File

@@ -14,6 +14,7 @@ mod collision_groups2;
mod convex_polygons2;
mod damping2;
mod debug_box_ball2;
mod drum2;
mod heightfield2;
mod joints2;
mod locked_rotations2;
@@ -63,6 +64,7 @@ pub fn main() {
("Collision groups", collision_groups2::init_world),
("Convex polygons", convex_polygons2::init_world),
("Damping", damping2::init_world),
("Drum", drum2::init_world),
("Heightfield", heightfield2::init_world),
("Joints", joints2::init_world),
("Locked rotations", locked_rotations2::init_world),

81
examples2d/drum2.rs Normal file
View File

@@ -0,0 +1,81 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new();
/*
* Create the boxes
*/
let num = 30;
let rad = 0.2;
let shift = rad * 2.0;
let centerx = shift * num as f32 / 2.0;
let centery = shift * num as f32 / 2.0;
for i in 0usize..num {
for j in 0usize..num {
let x = i as f32 * shift - centerx;
let y = j as f32 * shift - centery;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad).build();
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}
/*
* Setup a velocity-based kinematic rigid body.
*/
let platform_body = RigidBodyBuilder::new_kinematic_velocity_based().build();
let velocity_based_platform_handle = bodies.insert(platform_body);
let sides = [
(10.0, 0.25, vector![0.0, 10.0]),
(10.0, 0.25, vector![0.0, -10.0]),
(0.25, 10.0, vector![10.0, 0.0]),
(0.25, 10.0, vector![-10.0, 0.0]),
];
let balls = [
(1.25, vector![6.0, 6.0]),
(1.25, vector![-6.0, 6.0]),
(1.25, vector![6.0, -6.0]),
(1.25, vector![-6.0, -6.0]),
];
for (hx, hy, pos) in sides {
let collider = ColliderBuilder::cuboid(hx, hy).translation(pos).build();
colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
}
for (r, pos) in balls {
let collider = ColliderBuilder::ball(r).translation(pos).build();
colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
}
/*
* Setup a callback to control the platform.
*/
testbed.add_callback(move |_, physics, _, _| {
// Update the velocity-based kinematic body by setting its velocity.
if let Some(platform) = physics.bodies.get_mut(velocity_based_platform_handle) {
platform.set_angvel(-0.15, true);
}
});
/*
* Run the simulation.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 1.0], 40.0);
}

14
examples3d/debug_articulations3.rs
View File

@@ -66,17 +66,15 @@ pub fn init_world(testbed: &mut Testbed) {
let mut impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
let rigid_body = RigidBodyBuilder::new_dynamic().build();
let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) // Vector::y_axis())
.translation(vector![0.0, -3.0, 0.0])
let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0)
.translation(vector![0.0, -3.02, 0.0])
.rotation(vector![0.1, 0.0, 0.1])
.build();
let handle = bodies.insert(rigid_body);
colliders.insert_with_parent(collider, handle, &mut bodies);
colliders.insert(collider);
let rigid_body = RigidBodyBuilder::new_static().build();
let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) // Vector::y_axis())
.translation(vector![0.0, -3.02, 0.0])
let rigid_body = RigidBodyBuilder::new_dynamic().build();
let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0)
.translation(vector![0.0, -3.0, 0.0])
.rotation(vector![0.1, 0.0, 0.1])
.build();
let handle = bodies.insert(rigid_body);

7
src/data/arena.rs
View File

@@ -70,11 +70,8 @@ impl Index {
///
/// Providing arbitrary values will lead to malformed indices and ultimately
/// panics.
pub fn from_raw_parts(a: u32, b: u32) -> Index {
Index {
index: a,
generation: b,
}
pub fn from_raw_parts(index: u32, generation: u32) -> Index {
Index { index, generation }
}
/// Convert this `Index` into its raw parts.

4
src/data/coarena.rs
View File

@@ -29,6 +29,10 @@ impl<T> Coarena<T> {
self.data.get(index as usize).map(|(_, t)| t)
}
pub(crate) fn get_gen(&self, index: u32) -> Option<u32> {
self.data.get(index as usize).map(|(gen, _)| *gen)
}
/// Deletes an element for the coarena and returns its value.
///
/// This method will reset the value to the given `removed_value`.

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>,

1
src/pipeline/physics_pipeline.rs
View File

@@ -179,6 +179,7 @@ impl PhysicsPipeline {
{
self.counters.stages.island_construction_time.resume();
islands.update_active_set_with_contacts(
integration_parameters.dt,
bodies,
colliders,
narrow_phase,

6
src_testbed/camera2d.rs
View File

@@ -39,10 +39,8 @@ impl OrbitCameraPlugin {
mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
) {
for (camera, mut transform) in query.iter_mut() {
if camera.enabled {
transform.translation = camera.center;
transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
}
transform.translation = camera.center;
transform.scale = Vec3::new(1.0 / camera.zoom, 1.0 / camera.zoom, 1.0);
}
}

10
src_testbed/camera3d.rs
View File

@@ -50,12 +50,10 @@ impl OrbitCameraPlugin {
mut query: Query<(&OrbitCamera, &mut Transform), (Changed<OrbitCamera>, With<Camera>)>,
) {
for (camera, mut transform) in query.iter_mut() {
if camera.enabled {
let rot = Quat::from_axis_angle(Vec3::Y, camera.x)
* Quat::from_axis_angle(-Vec3::X, camera.y);
transform.translation = (rot * Vec3::Y) * camera.distance + camera.center;
transform.look_at(camera.center, Vec3::Y);
}
let rot = Quat::from_axis_angle(Vec3::Y, camera.x)
* Quat::from_axis_angle(-Vec3::X, camera.y);
transform.translation = (rot * Vec3::Y) * camera.distance + camera.center;
transform.look_at(camera.center, Vec3::Y);
}
}

30
src_testbed/graphics.rs
View File

@@ -2,10 +2,10 @@ use bevy::prelude::*;
use na::{point, Point3};
use crate::math::Isometry;
use crate::objects::node::EntityWithGraphics;
use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
use rapier::math::{Isometry, Real};
//use crate::objects::capsule::Capsule;
//#[cfg(feature = "dim3")]
//use crate::objects::mesh::Mesh;
@@ -301,8 +301,8 @@ impl GraphicsManager {
handle: Option<ColliderHandle>,
shape: &dyn Shape,
sensor: bool,
pos: &Isometry<f32>,
delta: &Isometry<f32>,
pos: &Isometry<Real>,
delta: &Isometry<Real>,
color: Point3<f32>,
out: &mut Vec<EntityWithGraphics>,
) {
@@ -347,18 +347,24 @@ impl GraphicsManager {
_bodies: &RigidBodySet,
colliders: &ColliderSet,
components: &mut Query<(&mut Transform,)>,
_materials: &mut Assets<BevyMaterial>,
) {
for (_, ns) in self.b2sn.iter_mut() {
for n in ns.iter_mut() {
// if let Some(co) = colliders.get(n.collider()) {
// let bo = &_bodies[co.parent()];
//
// if bo.is_dynamic() {
// if bo.is_ccd_active() {
// n.set_color(point![1.0, 0.0, 0.0]);
// } else {
// n.set_color(point![0.0, 1.0, 0.0]);
// }
// if let Some(bo) = n
// .collider
// .and_then(|h| bodies.get(colliders.get(h)?.parent()?))
// {
// if bo.activation().time_since_can_sleep
// >= RigidBodyActivation::default_time_until_sleep()
// {
// n.set_color(materials, point![1.0, 0.0, 0.0]);
// }
// /* else if bo.activation().energy < bo.activation().threshold {
// n.set_color(materials, point![0.0, 0.0, 1.0]);
// } */
// else {
// n.set_color(materials, point![0.0, 1.0, 0.0]);
// }
// }

6
src_testbed/harness/mod.rs
View File

@@ -8,7 +8,7 @@ use rapier::dynamics::{
RigidBodySet,
};
use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
use rapier::math::Vector;
use rapier::math::{Real, Vector};
use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
pub mod plugin;
@@ -131,7 +131,7 @@ impl Harness {
colliders: ColliderSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
gravity: Vector<f32>,
gravity: Vector<Real>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
) {
// println!("Num bodies: {}", bodies.len());
@@ -235,7 +235,7 @@ impl Harness {
self.events.poll_all();
self.state.time += self.physics.integration_parameters.dt;
self.state.time += self.physics.integration_parameters.dt as f32;
self.state.timestep_id += 1;
}

8
src_testbed/lib.rs
View File

@@ -1,12 +1,4 @@
extern crate nalgebra as na;
#[cfg(feature = "dim2")]
extern crate parry2d as parry;
#[cfg(feature = "dim3")]
extern crate parry3d as parry;
#[cfg(feature = "dim2")]
extern crate rapier2d as rapier;
#[cfg(feature = "dim3")]
extern crate rapier3d as rapier;
#[macro_use]
extern crate bitflags;

78
src_testbed/objects/node.rs
View File

@@ -10,7 +10,7 @@ use bevy::render::render_resource::PrimitiveTopology;
use rapier::geometry::{ColliderHandle, ColliderSet, Shape, ShapeType};
#[cfg(feature = "dim3")]
use rapier::geometry::{Cone, Cylinder};
use rapier::math::Isometry;
use rapier::math::{Isometry, Real};
use crate::graphics::BevyMaterial;
#[cfg(feature = "dim2")]
@@ -26,7 +26,7 @@ pub struct EntityWithGraphics {
pub color: Point3<f32>,
pub base_color: Point3<f32>,
pub collider: Option<ColliderHandle>,
pub delta: Isometry<f32>,
pub delta: Isometry<Real>,
pub opacity: f32,
material: Handle<BevyMaterial>,
}
@@ -39,8 +39,8 @@ impl EntityWithGraphics {
prefab_meshs: &HashMap<ShapeType, Handle<Mesh>>,
shape: &dyn Shape,
collider: Option<ColliderHandle>,
collider_pos: Isometry<f32>,
delta: Isometry<f32>,
collider_pos: Isometry<Real>,
delta: Isometry<Real>,
color: Point3<f32>,
sensor: bool,
) -> Self {
@@ -56,16 +56,16 @@ impl EntityWithGraphics {
let bevy_color = Color::rgba(color.x, color.y, color.z, opacity);
let shape_pos = collider_pos * delta;
let mut transform = Transform::from_scale(scale);
transform.translation.x = shape_pos.translation.vector.x;
transform.translation.y = shape_pos.translation.vector.y;
transform.translation.x = shape_pos.translation.vector.x as f32;
transform.translation.y = shape_pos.translation.vector.y as f32;
#[cfg(feature = "dim3")]
{
transform.translation.z = shape_pos.translation.vector.z;
transform.translation.z = shape_pos.translation.vector.z as f32;
transform.rotation = Quat::from_xyzw(
shape_pos.rotation.i,
shape_pos.rotation.j,
shape_pos.rotation.k,
shape_pos.rotation.w,
shape_pos.rotation.i as f32,
shape_pos.rotation.j as f32,
shape_pos.rotation.k as f32,
shape_pos.rotation.w as f32,
);
}
#[cfg(feature = "dim2")]
@@ -73,7 +73,7 @@ impl EntityWithGraphics {
if sensor {
transform.translation.z = -10.0;
}
transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle());
transform.rotation = Quat::from_rotation_z(shape_pos.rotation.angle() as f32);
}
#[cfg(feature = "dim2")]
@@ -172,21 +172,21 @@ impl EntityWithGraphics {
if let Some(Some(co)) = self.collider.map(|c| colliders.get(c)) {
if let Ok(mut pos) = components.get_component_mut::<Transform>(self.entity) {
let co_pos = co.position() * self.delta;
pos.translation.x = co_pos.translation.vector.x;
pos.translation.y = co_pos.translation.vector.y;
pos.translation.x = co_pos.translation.vector.x as f32;
pos.translation.y = co_pos.translation.vector.y as f32;
#[cfg(feature = "dim3")]
{
pos.translation.z = co_pos.translation.vector.z;
pos.translation.z = co_pos.translation.vector.z as f32;
pos.rotation = Quat::from_xyzw(
co_pos.rotation.i,
co_pos.rotation.j,
co_pos.rotation.k,
co_pos.rotation.w,
co_pos.rotation.i as f32,
co_pos.rotation.j as f32,
co_pos.rotation.k as f32,
co_pos.rotation.w as f32,
);
}
#[cfg(feature = "dim2")]
{
pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle());
pos.rotation = Quat::from_rotation_z(co_pos.rotation.angle() as f32);
}
}
}
@@ -266,7 +266,7 @@ impl EntityWithGraphics {
}
#[cfg(feature = "dim2")]
fn bevy_mesh_from_polyline(vertices: Vec<Point2<f32>>) -> Mesh {
fn bevy_mesh_from_polyline(vertices: Vec<Point2<Real>>) -> Mesh {
let n = vertices.len();
let idx = (1..n as u32 - 1).map(|i| [0, i, i + 1]).collect();
let vtx = vertices
@@ -277,7 +277,7 @@ fn bevy_mesh_from_polyline(vertices: Vec<Point2<f32>>) -> Mesh {
}
#[cfg(feature = "dim2")]
fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
fn bevy_polyline(buffers: (Vec<Point2<Real>>, Option<Vec<[u32; 2]>>)) -> Mesh {
let (vtx, idx) = buffers;
// let mut normals: Vec<[f32; 3]> = vec![];
let mut vertices: Vec<[f32; 3]> = vec![];
@@ -287,11 +287,11 @@ fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
let a = vtx[idx[0] as usize];
let b = vtx[idx[1] as usize];
vertices.push([a.x, a.y, 0.0]);
vertices.push([b.x, b.y, 0.0]);
vertices.push([a.x as f32, a.y as f32, 0.0]);
vertices.push([b.x as f32, b.y as f32, 0.0]);
}
} else {
vertices = vtx.iter().map(|v| [v.x, v.y, 0.0]).collect();
vertices = vtx.iter().map(|v| [v.x as f32, v.y as f32, 0.0]).collect();
}
let indices: Vec<_> = (0..vertices.len() as u32).collect();
@@ -310,7 +310,7 @@ fn bevy_polyline(buffers: (Vec<Point2<f32>>, Option<Vec<[u32; 2]>>)) -> Mesh {
mesh
}
fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
fn bevy_mesh(buffers: (Vec<Point3<Real>>, Vec<[u32; 3]>)) -> Mesh {
let (vtx, idx) = buffers;
let mut normals: Vec<[f32; 3]> = vec![];
let mut vertices: Vec<[f32; 3]> = vec![];
@@ -320,9 +320,9 @@ fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
let b = vtx[idx[1] as usize];
let c = vtx[idx[2] as usize];
vertices.push(a.into());
vertices.push(b.into());
vertices.push(c.into());
vertices.push(a.cast::<f32>().into());
vertices.push(b.cast::<f32>().into());
vertices.push(c.cast::<f32>().into());
}
for vtx in vertices.chunks(3) {
@@ -330,9 +330,9 @@ fn bevy_mesh(buffers: (Vec<Point3<f32>>, Vec<[u32; 3]>)) -> Mesh {
let b = Point3::from(vtx[1]);
let c = Point3::from(vtx[2]);
let n = (b - a).cross(&(c - a)).normalize();
normals.push(n.into());
normals.push(n.into());
normals.push(n.into());
normals.push(n.cast::<f32>().into());
normals.push(n.cast::<f32>().into());
normals.push(n.cast::<f32>().into());
}
normals
@@ -358,36 +358,36 @@ fn collider_mesh_scale(co_shape: &dyn Shape) -> Vec3 {
#[cfg(feature = "dim2")]
ShapeType::Cuboid => {
let c = co_shape.as_cuboid().unwrap();
Vec3::new(c.half_extents.x, c.half_extents.y, 1.0)
Vec3::new(c.half_extents.x as f32, c.half_extents.y as f32, 1.0)
}
ShapeType::Ball => {
let b = co_shape.as_ball().unwrap();
Vec3::new(b.radius, b.radius, b.radius)
Vec3::new(b.radius as f32, b.radius as f32, b.radius as f32)
}
#[cfg(feature = "dim3")]
ShapeType::Cuboid => {
let c = co_shape.as_cuboid().unwrap();
Vec3::from_slice(c.half_extents.as_slice())
Vec3::from_slice(c.half_extents.cast::<f32>().as_slice())
}
#[cfg(feature = "dim3")]
ShapeType::Cylinder => {
let c = co_shape.as_cylinder().unwrap();
Vec3::new(c.radius, c.half_height, c.radius)
Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
}
#[cfg(feature = "dim3")]
ShapeType::RoundCylinder => {
let c = &co_shape.as_round_cylinder().unwrap().base_shape;
Vec3::new(c.radius, c.half_height, c.radius)
Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
}
#[cfg(feature = "dim3")]
ShapeType::Cone => {
let c = co_shape.as_cone().unwrap();
Vec3::new(c.radius, c.half_height, c.radius)
Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
}
#[cfg(feature = "dim3")]
ShapeType::RoundCone => {
let c = &co_shape.as_round_cone().unwrap().base_shape;
Vec3::new(c.radius, c.half_height, c.radius)
Vec3::new(c.radius as f32, c.half_height as f32, c.radius as f32)
}
_ => Vec3::ONE,
}

4
src_testbed/physics/mod.rs
View File

@@ -4,7 +4,7 @@ use rapier::dynamics::{
RigidBodySet,
};
use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
use rapier::math::Vector;
use rapier::math::{Real, Vector};
use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
pub struct PhysicsSnapshot {
@@ -82,7 +82,7 @@ pub struct PhysicsState {
pub pipeline: PhysicsPipeline,
pub query_pipeline: QueryPipeline,
pub integration_parameters: IntegrationParameters,
pub gravity: Vector<f32>,
pub gravity: Vector<Real>,
pub hooks: Box<dyn PhysicsHooks<RigidBodySet, ColliderSet>>,
}

18
src_testbed/testbed.rs
View File

@@ -16,7 +16,7 @@ use rapier::dynamics::{
use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
#[cfg(feature = "dim3")]
use rapier::geometry::{InteractionGroups, Ray};
use rapier::math::Vector;
use rapier::math::{Real, Vector};
use rapier::pipeline::PhysicsHooks;
#[cfg(all(feature = "dim2", feature = "other-backends"))]
@@ -487,7 +487,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
colliders: ColliderSet,
impulse_joints: ImpulseJointSet,
multibody_joints: MultibodyJointSet,
gravity: Vector<f32>,
gravity: Vector<Real>,
hooks: impl PhysicsHooks<RigidBodySet, ColliderSet> + 'static,
) {
self.harness.set_world_with_params(
@@ -1129,12 +1129,15 @@ fn update_testbed(
{
if state.flags.contains(TestbedStateFlags::SLEEP) {
for (_, body) in harness.physics.bodies.iter_mut() {
body.activation_mut().threshold = RigidBodyActivation::default_threshold();
body.activation_mut().linear_threshold =
RigidBodyActivation::default_linear_threshold();
body.activation_mut().angular_threshold =
RigidBodyActivation::default_angular_threshold();
}
} else {
for (_, body) in harness.physics.bodies.iter_mut() {
body.wake_up(true);
body.activation_mut().threshold = -1.0;
body.activation_mut().linear_threshold = -1.0;
}
}
}
@@ -1226,6 +1229,7 @@ fn update_testbed(
&harness.physics.bodies,
&harness.physics.colliders,
&mut gfx_components,
&mut *materials,
);
for plugin in &mut plugins.0 {
@@ -1299,14 +1303,14 @@ fn highlight_hovered_body(
let ray_pt1 = ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0));
let ray_pt2 = ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0));
let ray_dir = ray_pt2 - ray_pt1;
let ray_origin = Point3::new(ray_pt1.x, ray_pt1.y, ray_pt1.z);
let ray_dir = Vector3::new(ray_dir.x, ray_dir.y, ray_dir.z);
let ray_origin = Point3::new(ray_pt1.x as Real, ray_pt1.y as Real, ray_pt1.z as Real);
let ray_dir = Vector3::new(ray_dir.x as Real, ray_dir.y as Real, ray_dir.z as Real);
let ray = Ray::new(ray_origin, ray_dir);
let hit = physics.query_pipeline.cast_ray(
&physics.colliders,
&ray,
f32::MAX,
Real::MAX,
true,
InteractionGroups::all(),
None,

3
src_testbed/ui.rs
View File

@@ -1,4 +1,5 @@
use rapier::counters::Counters;
use rapier::math::Real;
use crate::harness::Harness;
use crate::testbed::{
@@ -147,7 +148,7 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m
);
let mut frequency = integration_parameters.inv_dt().round() as u32;
ui.add(Slider::new(&mut frequency, 0..=240).text("frequency (Hz)"));
integration_parameters.set_inv_dt(frequency as f32);
integration_parameters.set_inv_dt(frequency as Real);
let mut sleep = state.flags.contains(TestbedStateFlags::SLEEP);
// let mut contact_points = state.flags.contains(TestbedStateFlags::CONTACT_POINTS);