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

Merge pull request #120 from dimforge/contact_modification

Browse Source 
Add the ability to modify the contact points seen by the constraints solver
This commit is contained in:
Sébastien Crozet
2021-02-24 11:26:53 +01:00
committed by GitHub
parent d31a327b45 3cc2738e5f
commit 649eba1013
24 changed files with 739 additions and 195 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples2d/all_examples2.rs
View File

@@ -18,6 +18,7 @@ mod debug_box_ball2;
mod heightfield2;
mod joints2;
mod locked_rotations2;
mod one_way_platforms2;
mod platform2;
mod polyline2;
mod pyramid2;
@@ -65,6 +66,7 @@ pub fn main() {
("Heightfield", heightfield2::init_world),
("Joints", joints2::init_world),
("Locked rotations", locked_rotations2::init_world),
("One-way platforms", one_way_platforms2::init_world),
("Platform", platform2::init_world),
("Polyline", polyline2::init_world),
("Pyramid", pyramid2::init_world),

2
examples2d/damping2.rs
View File

@@ -40,6 +40,6 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Set up the testbed.
*/
testbed.set_world_with_gravity(bodies, colliders, joints, Vector2::zeros());
testbed.set_world_with_params(bodies, colliders, joints, Vector2::zeros(), ());
testbed.look_at(Point2::new(3.0, 2.0), 50.0);
}

143
examples2d/one_way_platforms2.rs Normal file
View File

@@ -0,0 +1,143 @@
use na::{Point2, Vector2};
use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier2d::geometry::{ColliderBuilder, ColliderHandle, ColliderSet};
use rapier2d::pipeline::{ContactModificationContext, PhysicsHooks, PhysicsHooksFlags};
use rapier_testbed2d::Testbed;
struct OneWayPlatformHook {
platform1: ColliderHandle,
platform2: ColliderHandle,
}
impl PhysicsHooks for OneWayPlatformHook {
fn active_hooks(&self) -> PhysicsHooksFlags {
PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS
}
fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
// The allowed normal for the first platform is its local +y axis, and the
// allowed normal for the second platform is its local -y axis.
//
// Now we have to be careful because the `manifold.local_n1` normal points
// toward the outside of the shape of `context.co1`. So we need to flip the
// allowed normal direction if the platform is in `context.collider_handle2`.
//
// Therefore:
// - If context.collider_handle1 == self.platform1 then the allowed normal is +y.
// - If context.collider_handle2 == self.platform1 then the allowed normal is -y.
// - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y.
// - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y.
let mut allowed_local_n1 = Vector2::zeros();
if context.collider_handle1 == self.platform1 {
allowed_local_n1 = Vector2::y();
} else if context.collider_handle2 == self.platform1 {
// Flip the allowed direction.
allowed_local_n1 = -Vector2::y();
}
if context.collider_handle1 == self.platform2 {
allowed_local_n1 = -Vector2::y();
} else if context.collider_handle2 == self.platform2 {
// Flip the allowed direction.
allowed_local_n1 = -Vector2::y();
}
// Call the helper function that simulates one-way platforms.
context.update_as_oneway_platform(&allowed_local_n1, 0.1);
// Set the surface velocity of the accepted contacts.
let tangent_velocity = if context.collider_handle1 == self.platform1
|| context.collider_handle2 == self.platform2
{
-12.0
} else {
12.0
};
for contact in context.solver_contacts.iter_mut() {
contact.tangent_velocity.x = tangent_velocity;
}
}
}
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
/*
* Ground
*/
let rigid_body = RigidBodyBuilder::new_static().build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(25.0, 0.5)
.translation(30.0, 2.0)
.modify_solver_contacts(true)
.build();
let platform1 = colliders.insert(collider, handle, &mut bodies);
let collider = ColliderBuilder::cuboid(25.0, 0.5)
.translation(-30.0, -2.0)
.modify_solver_contacts(true)
.build();
let platform2 = colliders.insert(collider, handle, &mut bodies);
/*
* Setup the one-way platform hook.
*/
let physics_hooks = OneWayPlatformHook {
platform1,
platform2,
};
/*
* Spawn cubes at regular intervals and apply a custom gravity
* depending on their position.
*/
testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| {
if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
// Spawn a new cube.
let collider = ColliderBuilder::cuboid(1.5, 2.0).build();
let body = RigidBodyBuilder::new_dynamic()
.translation(20.0, 10.0)
.build();
let handle = physics.bodies.insert(body);
physics
.colliders
.insert(collider, handle, &mut physics.bodies);
if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) {
graphics.add(window, handle, &physics.bodies, &physics.colliders);
}
}
physics.bodies.foreach_active_dynamic_body_mut(|_, body| {
if body.position().translation.y > 1.0 {
body.set_gravity_scale(1.0, false);
} else if body.position().translation.y < -1.0 {
body.set_gravity_scale(-1.0, false);
}
});
});
/*
* Set up the testbed.
*/
testbed.set_world_with_params(
bodies,
colliders,
joints,
Vector2::new(0.0, -9.81),
physics_hooks,
);
testbed.look_at(Point2::new(0.0, 0.0), 20.0);
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

2
examples3d/all_examples3.rs
View File

@@ -29,6 +29,7 @@ mod heightfield3;
mod joints3;
mod keva3;
mod locked_rotations3;
mod one_way_platforms3;
mod platform3;
mod primitives3;
mod restitution3;
@@ -83,6 +84,7 @@ pub fn main() {
("Heightfield", heightfield3::init_world),
("Joints", joints3::init_world),
("Locked rotations", locked_rotations3::init_world),
("One-way platforms", one_way_platforms3::init_world),
("Platform", platform3::init_world),
("Restitution", restitution3::init_world),
("Sensor", sensor3::init_world),

2
examples3d/damping3.rs
View File

@@ -40,6 +40,6 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Set up the testbed.
*/
testbed.set_world_with_gravity(bodies, colliders, joints, Vector3::zeros());
testbed.set_world_with_params(bodies, colliders, joints, Vector3::zeros(), ());
testbed.look_at(Point3::new(2.0, 2.5, 20.0), Point3::new(2.0, 2.5, 0.0));
}

6
examples3d/fountain3.rs
View File

@@ -23,16 +23,14 @@ pub fn init_world(testbed: &mut Testbed) {
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
colliders.insert(collider, handle, &mut bodies);
let mut k = 0;
// Callback that will be executed on the main loop to handle proximities.
testbed.add_callback(move |mut window, mut graphics, physics, _, _| {
k += 1;
testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| {
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(0.0, 10.0, 0.0)
.build();
let handle = physics.bodies.insert(rigid_body);
let collider = match k % 3 {
let collider = match run_state.timestep_id % 3 {
0 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
1 => ColliderBuilder::cone(rad, rad).build(),
_ => ColliderBuilder::cuboid(rad, rad, rad).build(),

143
examples3d/one_way_platforms3.rs Normal file
View File

@@ -0,0 +1,143 @@
use na::{Point3, Vector3};
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderHandle, ColliderSet};
use rapier3d::pipeline::{ContactModificationContext, PhysicsHooks, PhysicsHooksFlags};
use rapier_testbed3d::Testbed;
struct OneWayPlatformHook {
platform1: ColliderHandle,
platform2: ColliderHandle,
}
impl PhysicsHooks for OneWayPlatformHook {
fn active_hooks(&self) -> PhysicsHooksFlags {
PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS
}
fn modify_solver_contacts(&self, context: &mut ContactModificationContext) {
// The allowed normal for the first platform is its local +y axis, and the
// allowed normal for the second platform is its local -y axis.
//
// Now we have to be careful because the `manifold.local_n1` normal points
// toward the outside of the shape of `context.co1`. So we need to flip the
// allowed normal direction if the platform is in `context.collider_handle2`.
//
// Therefore:
// - If context.collider_handle1 == self.platform1 then the allowed normal is +y.
// - If context.collider_handle2 == self.platform1 then the allowed normal is -y.
// - If context.collider_handle1 == self.platform2 then its allowed normal +y needs to be flipped to -y.
// - If context.collider_handle2 == self.platform2 then the allowed normal -y needs to be flipped to +y.
let mut allowed_local_n1 = Vector3::zeros();
if context.collider_handle1 == self.platform1 {
allowed_local_n1 = Vector3::y();
} else if context.collider_handle2 == self.platform1 {
// Flip the allowed direction.
allowed_local_n1 = -Vector3::y();
}
if context.collider_handle1 == self.platform2 {
allowed_local_n1 = -Vector3::y();
} else if context.collider_handle2 == self.platform2 {
// Flip the allowed direction.
allowed_local_n1 = -Vector3::y();
}
// Call the helper function that simulates one-way platforms.
context.update_as_oneway_platform(&allowed_local_n1, 0.1);
// Set the surface velocity of the accepted contacts.
let tangent_velocity = if context.collider_handle1 == self.platform1
|| context.collider_handle2 == self.platform2
{
-12.0
} else {
12.0
};
for contact in context.solver_contacts.iter_mut() {
contact.tangent_velocity.z = tangent_velocity;
}
}
}
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
/*
* Ground
*/
let rigid_body = RigidBodyBuilder::new_static().build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0)
.translation(0.0, 2.0, 30.0)
.modify_solver_contacts(true)
.build();
let platform1 = colliders.insert(collider, handle, &mut bodies);
let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0)
.translation(0.0, -2.0, -30.0)
.modify_solver_contacts(true)
.build();
let platform2 = colliders.insert(collider, handle, &mut bodies);
/*
* Setup the one-way platform hook.
*/
let physics_hooks = OneWayPlatformHook {
platform1,
platform2,
};
/*
* Spawn cubes at regular intervals and apply a custom gravity
* depending on their position.
*/
testbed.add_callback(move |mut window, mut graphics, physics, _, run_state| {
if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
// Spawn a new cube.
let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5).build();
let body = RigidBodyBuilder::new_dynamic()
.translation(0.0, 6.0, 20.0)
.build();
let handle = physics.bodies.insert(body);
physics
.colliders
.insert(collider, handle, &mut physics.bodies);
if let (Some(graphics), Some(window)) = (&mut graphics, &mut window) {
graphics.add(window, handle, &physics.bodies, &physics.colliders);
}
}
physics.bodies.foreach_active_dynamic_body_mut(|_, body| {
if body.position().translation.y > 1.0 {
body.set_gravity_scale(1.0, false);
} else if body.position().translation.y < -1.0 {
body.set_gravity_scale(-1.0, false);
}
});
});
/*
* Set up the testbed.
*/
testbed.set_world_with_params(
bodies,
colliders,
joints,
Vector3::new(0.0, -9.81, 0.0),
physics_hooks,
);
testbed.look_at(Point3::new(-100.0, 0.0, 0.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

42
src/dynamics/rigid_body_set.rs
View File

@@ -234,13 +234,27 @@ impl RigidBodySet {
self.bodies.get(handle.0)
}
fn mark_as_modified(
handle: RigidBodyHandle,
rb: &mut RigidBody,
modified_bodies: &mut Vec<RigidBodyHandle>,
modified_all_bodies: bool,
) {
if !modified_all_bodies && !rb.changes.contains(RigidBodyChanges::MODIFIED) {
rb.changes = RigidBodyChanges::MODIFIED;
modified_bodies.push(handle);
}
}
/// Gets a mutable reference to the rigid-body with the given handle.
pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> {
let result = self.bodies.get_mut(handle.0)?;
if !self.modified_all_bodies && !result.changes.contains(RigidBodyChanges::MODIFIED) {
result.changes = RigidBodyChanges::MODIFIED;
self.modified_bodies.push(handle);
}
Self::mark_as_modified(
handle,
result,
&mut self.modified_bodies,
self.modified_all_bodies,
);
Some(result)
}
@@ -300,6 +314,26 @@ impl RigidBodySet {
.filter_map(move |h| Some((*h, bodies.get(h.0)?)))
}
/// Applies the given function on all the active dynamic rigid-bodies
/// contained by this set.
#[inline(always)]
pub fn foreach_active_dynamic_body_mut(
&mut self,
mut f: impl FnMut(RigidBodyHandle, &mut RigidBody),
) {
for handle in &self.active_dynamic_set {
if let Some(rb) = self.bodies.get_mut(handle.0) {
Self::mark_as_modified(
*handle,
rb,
&mut self.modified_bodies,
self.modified_all_bodies,
);
f(*handle, rb)
}
}
}
#[inline(always)]
pub(crate) fn foreach_active_body_mut_internal(
&mut self,

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

@@ -124,7 +124,7 @@ pub(crate) struct VelocityConstraint {
pub mj_lambda1: usize,
pub mj_lambda2: usize,
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: usize,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS],
}
@@ -152,7 +152,7 @@ impl VelocityConstraint {
let force_dir1 = -manifold.data.normal;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
for (l, manifold_points) in manifold
for (_l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
@@ -168,7 +168,7 @@ impl VelocityConstraint {
mj_lambda1,
mj_lambda2,
manifold_id,
manifold_contact_id: l * MAX_MANIFOLD_POINTS,
manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8,
};
@@ -211,7 +211,7 @@ impl VelocityConstraint {
constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
constraint.num_contacts = manifold_points.len() as u8;
}
@@ -224,6 +224,8 @@ impl VelocityConstraint {
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
{
let gcross1 = rb1
@@ -271,7 +273,8 @@ impl VelocityConstraint {
+ rb2.effective_inv_mass
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
let rhs =
(vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_point.data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")]
@@ -292,7 +295,7 @@ impl VelocityConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
} else {
out_constraints[manifold.data.constraint_index + l] =
out_constraints[manifold.data.constraint_index + _l] =
AnyVelocityConstraint::Nongrouped(constraint);
}
}
@@ -382,19 +385,18 @@ impl VelocityConstraint {
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id];
let k_base = self.manifold_contact_id;
for k in 0..self.num_contacts as usize {
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].data.tangent_impulse =
self.elements[k].tangent_part[0].impulse;
active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].data.tangent_impulse = [
active_contact.data.tangent_impulse = [
self.elements[k].tangent_part[0].impulse,
self.elements[k].tangent_part[1].impulse,
];

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

@@ -55,7 +55,7 @@ pub(crate) struct WVelocityConstraint {
pub mj_lambda1: [usize; SIMD_WIDTH],
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: usize,
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
}
impl WVelocityConstraint {
@@ -116,7 +116,7 @@ impl WVelocityConstraint {
mj_lambda1,
mj_lambda2,
manifold_id,
manifold_contact_id: l,
manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
num_contacts: num_points as u8,
};
@@ -130,6 +130,8 @@ impl WVelocityConstraint {
);
let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
let impulse =
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
@@ -141,6 +143,8 @@ impl WVelocityConstraint {
let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
// Normal part.
{
@@ -179,7 +183,7 @@ impl WVelocityConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdReal::splat(1.0)
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart {
gcross1,
@@ -332,17 +336,17 @@ impl WVelocityConstraint {
for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id;
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
active_contacts[k_base + k].data.impulse = impulses[ii];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
active_contact.data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].data.tangent_impulse =
active_contact.data.tangent_impulse =
[tangent_impulses[ii], bitangent_impulses[ii]];
}
}

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

@@ -49,7 +49,7 @@ pub(crate) struct VelocityGroundConstraint {
pub limit: Real,
pub mj_lambda2: usize,
pub manifold_id: ContactManifoldIndex,
pub manifold_contact_id: usize,
pub manifold_contact_id: [u8; MAX_MANIFOLD_POINTS],
pub num_contacts: u8,
pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS],
}
@@ -68,17 +68,17 @@ impl VelocityGroundConstraint {
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flipped = !rb2.is_dynamic();
let force_dir1 = if flipped {
let (force_dir1, flipped_multiplier) = if flipped {
std::mem::swap(&mut rb1, &mut rb2);
manifold.data.normal
(manifold.data.normal, -1.0)
} else {
-manifold.data.normal
(-manifold.data.normal, 1.0)
};
let mj_lambda2 = rb2.active_set_offset;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
for (l, manifold_points) in manifold
for (_l, manifold_points) in manifold
.data
.solver_contacts
.chunks(MAX_MANIFOLD_POINTS)
@@ -92,7 +92,7 @@ impl VelocityGroundConstraint {
limit: 0.0,
mj_lambda2,
manifold_id,
manifold_contact_id: l * MAX_MANIFOLD_POINTS,
manifold_contact_id: [0; MAX_MANIFOLD_POINTS],
num_contacts: manifold_points.len() as u8,
};
@@ -123,7 +123,7 @@ impl VelocityGroundConstraint {
.as_nongrouped_ground_mut()
.unwrap()
} else {
unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
};
#[cfg(target_arch = "wasm32")]
@@ -133,7 +133,7 @@ impl VelocityGroundConstraint {
constraint.limit = 0.0;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
constraint.manifold_contact_id = [0; MAX_MANIFOLD_POINTS];
constraint.num_contacts = manifold_points.len() as u8;
}
@@ -145,6 +145,7 @@ impl VelocityGroundConstraint {
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
constraint.manifold_contact_id[k] = manifold_point.contact_id;
// Normal part.
{
@@ -178,7 +179,9 @@ impl VelocityGroundConstraint {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")]
@@ -199,7 +202,7 @@ impl VelocityGroundConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
} else {
out_constraints[manifold.data.constraint_index + l] =
out_constraints[manifold.data.constraint_index + _l] =
AnyVelocityConstraint::NongroupedGround(constraint);
}
}
@@ -267,19 +270,18 @@ impl VelocityGroundConstraint {
// FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint.
pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) {
let manifold = &mut manifolds_all[self.manifold_id];
let k_base = self.manifold_contact_id;
for k in 0..self.num_contacts as usize {
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
let contact_id = self.manifold_contact_id[k];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].data.tangent_impulse =
self.elements[k].tangent_part[0].impulse;
active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].data.tangent_impulse = [
active_contact.data.tangent_impulse = [
self.elements[k].tangent_part[0].impulse,
self.elements[k].tangent_part[1].impulse,
];

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

@@ -51,7 +51,7 @@ pub(crate) struct WVelocityGroundConstraint {
pub limit: SimdReal,
pub mj_lambda2: [usize; SIMD_WIDTH],
pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
pub manifold_contact_id: usize,
pub manifold_contact_id: [[u8; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
}
impl WVelocityGroundConstraint {
@@ -66,15 +66,17 @@ impl WVelocityGroundConstraint {
let inv_dt = SimdReal::splat(params.inv_dt());
let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let mut flipped = [false; SIMD_WIDTH];
let mut flipped = [1.0; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !rbs2[ii].is_dynamic() {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
flipped[ii] = true;
flipped[ii] = -1.0;
}
}
let flipped_sign = SimdReal::from(flipped);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
@@ -89,9 +91,8 @@ impl WVelocityGroundConstraint {
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let force_dir1 = Vector::from(
array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH],
);
let normal1 = Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
let force_dir1 = normal1 * -flipped_sign;
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -111,7 +112,7 @@ impl WVelocityGroundConstraint {
limit: SimdReal::splat(0.0),
mj_lambda2,
manifold_id,
manifold_contact_id: l,
manifold_contact_id: [[0; SIMD_WIDTH]; MAX_MANIFOLD_POINTS],
num_contacts: num_points as u8,
};
@@ -125,6 +126,8 @@ impl WVelocityGroundConstraint {
);
let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
let impulse =
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
@@ -135,6 +138,8 @@ impl WVelocityGroundConstraint {
let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
constraint.manifold_contact_id[k] =
array![|ii| manifold_points[ii][k].contact_id; SIMD_WIDTH];
// Normal part.
{
@@ -168,7 +173,7 @@ impl WVelocityGroundConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
constraint.elements[k].tangent_parts[j] =
WVelocityGroundConstraintElementPart {
@@ -281,17 +286,17 @@ impl WVelocityGroundConstraint {
for ii in 0..SIMD_WIDTH {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id;
let active_contacts = &mut manifold.points[..manifold.data.num_active_contacts()];
active_contacts[k_base + k].data.impulse = impulses[ii];
let contact_id = self.manifold_contact_id[k][ii];
let active_contact = &mut manifold.points[contact_id as usize];
active_contact.data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
active_contact.data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].data.tangent_impulse =
active_contact.data.tangent_impulse =
[tangent_impulses[ii], bitangent_impulses[ii]];
}
}

20
src/geometry/collider.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
use crate::geometry::{InteractionGroups, SharedShape};
use crate::geometry::{InteractionGroups, SharedShape, SolverFlags};
use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
use crate::parry::transformation::vhacd::VHACDParameters;
use parry::bounding_volume::AABB;
@@ -50,6 +50,7 @@ pub struct Collider {
shape: SharedShape,
density: Real,
pub(crate) flags: ColliderFlags,
pub(crate) solver_flags: SolverFlags,
pub(crate) parent: RigidBodyHandle,
pub(crate) delta: Isometry<Real>,
pub(crate) position: Isometry<Real>,
@@ -159,6 +160,9 @@ pub struct ColliderBuilder {
pub delta: Isometry<Real>,
/// Is this collider a sensor?
pub is_sensor: bool,
/// Do we have to always call the contact modifier
/// on this collider?
pub modify_solver_contacts: bool,
/// The user-data of the collider being built.
pub user_data: u128,
/// The collision groups for the collider being built.
@@ -182,6 +186,7 @@ impl ColliderBuilder {
solver_groups: InteractionGroups::all(),
friction_combine_rule: CoefficientCombineRule::Average,
restitution_combine_rule: CoefficientCombineRule::Average,
modify_solver_contacts: false,
}
}
@@ -456,6 +461,13 @@ impl ColliderBuilder {
self
}
/// If set to `true` then the physics hooks will always run to modify
/// contacts involving this collider.
pub fn modify_solver_contacts(mut self, modify_solver_contacts: bool) -> Self {
self.modify_solver_contacts = modify_solver_contacts;
self
}
/// Sets the friction coefficient of the collider this builder will build.
pub fn friction(mut self, friction: Real) -> Self {
self.friction = friction;
@@ -534,6 +546,11 @@ impl ColliderBuilder {
flags = flags
.with_friction_combine_rule(self.friction_combine_rule)
.with_restitution_combine_rule(self.restitution_combine_rule);
let mut solver_flags = SolverFlags::default();
solver_flags.set(
SolverFlags::MODIFY_SOLVER_CONTACTS,
self.modify_solver_contacts,
);
Collider {
shape: self.shape.clone(),
@@ -542,6 +559,7 @@ impl ColliderBuilder {
restitution: self.restitution,
delta: self.delta,
flags,
solver_flags,
parent: RigidBodyHandle::invalid(),
position: Isometry::identity(),
predicted_position: Isometry::identity(),

21
src/geometry/contact_pair.rs
View File

@@ -9,7 +9,16 @@ bitflags::bitflags! {
pub struct SolverFlags: u32 {
/// The constraint solver will take this contact manifold into
/// account for force computation.
const COMPUTE_IMPULSES = 0b01;
const COMPUTE_IMPULSES = 0b001;
/// The user-defined physics hooks will be used to
/// modify the solver contacts of this contact manifold.
const MODIFY_SOLVER_CONTACTS = 0b010;
}
}
impl Default for SolverFlags {
fn default() -> Self {
SolverFlags::COMPUTE_IMPULSES
}
}
@@ -104,12 +113,16 @@ pub struct ContactManifoldData {
/// The contacts that will be seen by the constraints solver for computing forces.
#[cfg_attr(feature = "serde-serialize", serde(skip))]
pub solver_contacts: Vec<SolverContact>,
/// A user-defined piece of data.
pub user_data: u32,
}
/// A contact seen by the constraints solver for computing forces.
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SolverContact {
/// The index of the manifold contact used to generate this solver contact.
pub contact_id: u8,
/// The world-space contact point.
pub point: Point<Real>,
/// The distance between the two original contacts points along the contact normal.
@@ -119,10 +132,11 @@ pub struct SolverContact {
pub friction: Real,
/// The effective restitution coefficient at this contact point.
pub restitution: Real,
/// The artificially add relative velocity at the contact point.
/// The desired tangent relative velocity at the contact point.
///
/// This is set to zero by default. Set to a non-zero value to
/// simulate, e.g., conveyor belts.
pub surface_velocity: Vector<Real>,
pub tangent_velocity: Vector<Real>,
/// Associated contact data used to warm-start the constraints
/// solver.
pub data: ContactData,
@@ -163,6 +177,7 @@ impl ContactManifoldData {
solver_flags,
normal: Vector::zeros(),
solver_contacts: Vec::new(),
user_data: 0,
}
}

2
src/geometry/mod.rs
View File

@@ -10,7 +10,6 @@ pub use self::interaction_graph::{
};
pub use self::interaction_groups::InteractionGroups;
pub use self::narrow_phase::NarrowPhase;
pub use self::pair_filter::{ContactPairFilter, IntersectionPairFilter, PairFilterContext};
pub use parry::query::TrackedContact;
@@ -109,4 +108,3 @@ mod contact_pair;
mod interaction_graph;
mod interaction_groups;
mod narrow_phase;
mod pair_filter;

91
src/geometry/narrow_phase.rs
View File

@@ -5,13 +5,14 @@ use crate::data::pubsub::Subscription;
use crate::data::Coarena;
use crate::dynamics::{BodyPair, CoefficientCombineRule, RigidBodySet};
use crate::geometry::{
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent,
ContactManifoldData, ContactPairFilter, IntersectionEvent, IntersectionPairFilter,
PairFilterContext, RemovedCollider, SolverContact, SolverFlags,
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ColliderSet, ContactData,
ContactEvent, ContactManifold, ContactManifoldData, ContactPair, InteractionGraph,
IntersectionEvent, RemovedCollider, SolverContact, SolverFlags,
};
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
use crate::math::{Real, Vector};
use crate::pipeline::EventHandler;
use crate::pipeline::{
ContactModificationContext, EventHandler, PairFilterContext, PhysicsHooks, PhysicsHooksFlags,
};
use parry::query::{DefaultQueryDispatcher, PersistentQueryDispatcher};
use parry::utils::IsometryOpt;
use std::collections::HashMap;
@@ -387,11 +388,13 @@ impl NarrowPhase {
&mut self,
bodies: &RigidBodySet,
colliders: &ColliderSet,
pair_filter: Option<&dyn IntersectionPairFilter>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
let nodes = &self.intersection_graph.graph.nodes;
let query_dispatcher = &*self.query_dispatcher;
let active_hooks = hooks.active_hooks();
par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| {
let handle1 = nodes[edge.source().index()].weight;
let handle2 = nodes[edge.target().index()].weight;
@@ -415,12 +418,15 @@ impl NarrowPhase {
return;
}
if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
if !active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR)
&& !rb1.is_dynamic()
&& !rb2.is_dynamic()
{
// Default filtering rule: no intersection between two non-dynamic bodies.
return;
}
if let Some(filter) = pair_filter {
if active_hooks.contains(PhysicsHooksFlags::FILTER_INTERSECTION_PAIR) {
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
@@ -430,7 +436,7 @@ impl NarrowPhase {
collider2: co2,
};
if !filter.filter_intersection_pair(&context) {
if !hooks.filter_intersection_pair(&context) {
// No intersection allowed.
return;
}
@@ -458,10 +464,11 @@ impl NarrowPhase {
prediction_distance: Real,
bodies: &RigidBodySet,
colliders: &ColliderSet,
pair_filter: Option<&dyn ContactPairFilter>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
let query_dispatcher = &*self.query_dispatcher;
let active_hooks = hooks.active_hooks();
par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
let pair = &mut edge.weight;
@@ -485,12 +492,16 @@ impl NarrowPhase {
return;
}
if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
if !active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
&& !rb1.is_dynamic()
&& !rb2.is_dynamic()
{
// Default filtering rule: no contact between two non-dynamic bodies.
return;
}
let mut solver_flags = if let Some(filter) = pair_filter {
let mut solver_flags = if active_hooks.contains(PhysicsHooksFlags::FILTER_CONTACT_PAIR)
{
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
@@ -500,14 +511,14 @@ impl NarrowPhase {
collider2: co2,
};
if let Some(solver_flags) = filter.filter_contact_pair(&context) {
if let Some(solver_flags) = hooks.filter_contact_pair(&context) {
solver_flags
} else {
// No contact allowed.
return;
}
} else {
SolverFlags::COMPUTE_IMPULSES
co1.solver_flags | co2.solver_flags
};
if !co1.solver_groups.test(co2.solver_groups) {
@@ -546,38 +557,58 @@ impl NarrowPhase {
manifold.data.solver_flags = solver_flags;
manifold.data.normal = world_pos1 * manifold.local_n1;
// Sort contacts to keep only these with distances bellow
// the prediction, and generate solver contacts.
let mut first_inactive_index = manifold.points.len();
// Generate solver contacts.
for (contact_id, contact) in manifold.points.iter().enumerate() {
assert!(
contact_id <= u8::MAX as usize,
"A contact manifold cannot contain more than 255 contacts currently."
);
while manifold.data.num_active_contacts() != first_inactive_index {
let contact = &manifold.points[manifold.data.num_active_contacts()];
if contact.dist < prediction_distance {
// Generate the solver contact.
let solver_contact = SolverContact {
contact_id: contact_id as u8,
point: world_pos1 * contact.local_p1
+ manifold.data.normal * contact.dist / 2.0,
dist: contact.dist,
friction,
restitution,
surface_velocity: Vector::zeros(),
tangent_velocity: Vector::zeros(),
data: contact.data,
};
// TODO: apply the user-defined contact modification/removal, if needed.
manifold.data.solver_contacts.push(solver_contact);
has_any_active_contact = true;
continue;
}
}
// If we reach this code, then the contact must be ignored by the constraints solver.
// Swap with the last contact.
manifold.points.swap(
manifold.data.num_active_contacts(),
first_inactive_index - 1,
);
first_inactive_index -= 1;
// Apply the user-defined contact modification.
if active_hooks.contains(PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS)
&& manifold
.data
.solver_flags
.contains(SolverFlags::MODIFY_SOLVER_CONTACTS)
{
let mut modifiable_solver_contacts =
std::mem::replace(&mut manifold.data.solver_contacts, Vec::new());
let mut modifiable_user_data = manifold.data.user_data;
let mut context = ContactModificationContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider_handle1: pair.pair.collider1,
collider_handle2: pair.pair.collider2,
collider1: co1,
collider2: co2,
manifold,
solver_contacts: &mut modifiable_solver_contacts,
user_data: &mut modifiable_user_data,
};
hooks.modify_solver_contacts(&mut context);
manifold.data.solver_contacts = modifiable_solver_contacts;
manifold.data.user_data = modifiable_user_data;
}
}

61
src/geometry/pair_filter.rs
View File

@@ -1,61 +0,0 @@
use crate::dynamics::RigidBody;
use crate::geometry::{Collider, ColliderHandle, SolverFlags};
/// Context given to custom collision filters to filter-out collisions.
pub struct PairFilterContext<'a> {
/// The first collider involved in the potential collision.
pub rigid_body1: &'a RigidBody,
/// The first collider involved in the potential collision.
pub rigid_body2: &'a RigidBody,
/// The first collider involved in the potential collision.
pub collider_handle1: ColliderHandle,
/// The first collider involved in the potential collision.
pub collider_handle2: ColliderHandle,
/// The first collider involved in the potential collision.
pub collider1: &'a Collider,
/// The first collider involved in the potential collision.
pub collider2: &'a Collider,
}
/// User-defined filter for potential contact pairs detected by the broad-phase.
///
/// This can be used to apply custom logic in order to decide whether two colliders
/// should have their contact computed by the narrow-phase, and if these contact
/// should be solved by the constraints solver
pub trait ContactPairFilter: Send + Sync {
/// Applies the contact pair filter.
///
/// Note that using a contact pair filter will replace the default contact filtering
/// which consists of preventing contact computation between two non-dynamic bodies.
///
/// This filtering method is called after taking into account the colliders collision groups.
///
/// If this returns `None`, then the narrow-phase will ignore this contact pair and
/// not compute any contact manifolds for it.
/// If this returns `Some`, then the narrow-phase will compute contact manifolds for
/// this pair of colliders, and configure them with the returned solver flags. For
/// example, if this returns `Some(SolverFlags::COMPUTE_IMPULSES)` then the contacts
/// will be taken into account by the constraints solver. If this returns
/// `Some(SolverFlags::empty())` then the constraints solver will ignore these
/// contacts.
fn filter_contact_pair(&self, context: &PairFilterContext) -> Option<SolverFlags>;
}
/// User-defined filter for potential intersection pairs detected by the broad-phase.
///
/// This can be used to apply custom logic in order to decide whether two colliders
/// should have their intersection computed by the narrow-phase.
pub trait IntersectionPairFilter: Send + Sync {
/// Applies the intersection pair filter.
///
/// Note that using an intersection pair filter will replace the default intersection filtering
/// which consists of preventing intersection computation between two non-dynamic bodies.
///
/// This filtering method is called after taking into account the colliders collision groups.
///
/// If this returns `false`, then the narrow-phase will ignore this pair and
/// not compute any intersection information for it.
/// If this return `true` then the narrow-phase will compute intersection
/// information for this pair.
fn filter_intersection_pair(&self, context: &PairFilterContext) -> bool;
}

20
src/pipeline/collision_pipeline.rs
View File

@@ -1,12 +1,9 @@
//! Physics pipeline structures.
use crate::dynamics::{JointSet, RigidBodySet};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter,
IntersectionPairFilter, NarrowPhase,
};
use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase};
use crate::math::Real;
use crate::pipeline::EventHandler;
use crate::pipeline::{EventHandler, PhysicsHooks};
/// The collision pipeline, responsible for performing collision detection between colliders.
///
@@ -44,8 +41,7 @@ impl CollisionPipeline {
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
contact_pair_filter: Option<&dyn ContactPairFilter>,
proximity_pair_filter: Option<&dyn IntersectionPairFilter>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
bodies.maintain(colliders);
@@ -58,14 +54,8 @@ impl CollisionPipeline {
narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
narrow_phase.compute_contacts(
prediction_distance,
bodies,
colliders,
contact_pair_filter,
events,
);
narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events);
narrow_phase.compute_contacts(prediction_distance, bodies, colliders, hooks, events);
narrow_phase.compute_intersections(bodies, colliders, hooks, events);
bodies.update_active_set_with_contacts(
colliders,

4
src/pipeline/mod.rs
View File

@@ -2,10 +2,14 @@
pub use collision_pipeline::CollisionPipeline;
pub use event_handler::{ChannelEventCollector, EventHandler};
pub use physics_hooks::{
ContactModificationContext, PairFilterContext, PhysicsHooks, PhysicsHooksFlags,
};
pub use physics_pipeline::PhysicsPipeline;
pub use query_pipeline::QueryPipeline;
mod collision_pipeline;
mod event_handler;
mod physics_hooks;
mod physics_pipeline;
mod query_pipeline;

215
src/pipeline/physics_hooks.rs Normal file
View File

@@ -0,0 +1,215 @@
use crate::dynamics::RigidBody;
use crate::geometry::{Collider, ColliderHandle, ContactManifold, SolverContact, SolverFlags};
use crate::math::{Real, Vector};
use na::ComplexField;
/// Context given to custom collision filters to filter-out collisions.
pub struct PairFilterContext<'a> {
/// The first collider involved in the potential collision.
pub rigid_body1: &'a RigidBody,
/// The first collider involved in the potential collision.
pub rigid_body2: &'a RigidBody,
/// The first collider involved in the potential collision.
pub collider_handle1: ColliderHandle,
/// The first collider involved in the potential collision.
pub collider_handle2: ColliderHandle,
/// The first collider involved in the potential collision.
pub collider1: &'a Collider,
/// The first collider involved in the potential collision.
pub collider2: &'a Collider,
}
/// Context given to custom contact modifiers to modify the contacts seen by the constrainst solver.
pub struct ContactModificationContext<'a> {
/// The first collider involved in the potential collision.
pub rigid_body1: &'a RigidBody,
/// The first collider involved in the potential collision.
pub rigid_body2: &'a RigidBody,
/// The first collider involved in the potential collision.
pub collider_handle1: ColliderHandle,
/// The first collider involved in the potential collision.
pub collider_handle2: ColliderHandle,
/// The first collider involved in the potential collision.
pub collider1: &'a Collider,
/// The first collider involved in the potential collision.
pub collider2: &'a Collider,
/// The contact manifold.
pub manifold: &'a ContactManifold,
/// The solver contacts that can be modified.
pub solver_contacts: &'a mut Vec<SolverContact>,
/// User-defined data attached to the manifold.
// NOTE: we keep this a &'a mut u32 to emphasize the
// fact that this can be modified.
pub user_data: &'a mut u32,
}
impl<'a> ContactModificationContext<'a> {
/// Helper function to update `self` to emulate a oneway-platform.
///
/// The "oneway" behavior will only allow contacts between two colliders
/// if the local contact normal of the first collider involved in the contact
/// is almost aligned with the provided `allowed_local_n1` direction.
///
/// To make this method work properly it must be called as part of the
/// `PhysicsHooks::modify_solver_contacts` method at each timestep, for each
/// contact manifold involving a one-way platform. The `self.user_data` field
/// must not be modified from the outside of this method.
pub fn update_as_oneway_platform(
&mut self,
allowed_local_n1: &Vector<Real>,
allowed_angle: Real,
) {
const CONTACT_CONFIGURATION_UNKNOWN: u32 = 0;
const CONTACT_CURRENTLY_ALLOWED: u32 = 1;
const CONTACT_CURRENTLY_FORBIDDEN: u32 = 2;
let cang = ComplexField::cos(allowed_angle);
// Test the allowed normal with the local-space contact normal that
// points towards the exterior of context.collider1.
let contact_is_ok = self.manifold.local_n1.dot(&allowed_local_n1) >= cang;
match *self.user_data {
CONTACT_CONFIGURATION_UNKNOWN => {
if contact_is_ok {
// The contact is close enough to the allowed normal.
*self.user_data = CONTACT_CURRENTLY_ALLOWED;
} else {
// The contact normal isn't close enough to the allowed
// normal, so remove all the contacts and mark further contacts
// as forbidden.
self.solver_contacts.clear();
*self.user_data = CONTACT_CURRENTLY_FORBIDDEN;
}
}
CONTACT_CURRENTLY_FORBIDDEN => {
// Contacts are forbidden so we need to continue forbidding contacts
// until all the contacts are non-penetrating again. In that case, if
// the contacts are OK wrt. the contact normal, then we can mark them as allowed.
if contact_is_ok && self.solver_contacts.iter().all(|c| c.dist > 0.0) {
*self.user_data = CONTACT_CURRENTLY_ALLOWED;
} else {
// Discard all the contacts.
self.solver_contacts.clear();
}
}
CONTACT_CURRENTLY_ALLOWED => {
// We allow all the contacts right now. The configuration becomes
// uncertain again when the contact manifold no longer contains any contact.
if self.solver_contacts.is_empty() {
*self.user_data = CONTACT_CONFIGURATION_UNKNOWN;
}
}
_ => unreachable!(),
}
}
}
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Flags affecting the behavior of the constraints solver for a given contact manifold.
pub struct PhysicsHooksFlags: u32 {
/// If set, Rapier will call `PhysicsHooks::filter_contact_pair` whenever relevant.
const FILTER_CONTACT_PAIR = 0b0001;
/// If set, Rapier will call `PhysicsHooks::filter_intersection_pair` whenever relevant.
const FILTER_INTERSECTION_PAIR = 0b0010;
/// If set, Rapier will call `PhysicsHooks::modify_solver_contact` whenever relevant.
const MODIFY_SOLVER_CONTACTS = 0b0100;
}
}
/// User-defined functions called by the physics engines during one timestep in order to customize its behavior.
pub trait PhysicsHooks: Send + Sync {
/// The sets of hooks that must be taken into account.
fn active_hooks(&self) -> PhysicsHooksFlags;
/// Applies the contact pair filter.
///
/// Note that this method will only be called if `self.active_hooks()`
/// contains the `PhysicsHooksFlags::FILTER_CONTACT_PAIR` flags.
///
/// User-defined filter for potential contact pairs detected by the broad-phase.
/// This can be used to apply custom logic in order to decide whether two colliders
/// should have their contact computed by the narrow-phase, and if these contact
/// should be solved by the constraints solver
///
/// Note that using a contact pair filter will replace the default contact filtering
/// which consists of preventing contact computation between two non-dynamic bodies.
///
/// This filtering method is called after taking into account the colliders collision groups.
///
/// If this returns `None`, then the narrow-phase will ignore this contact pair and
/// not compute any contact manifolds for it.
/// If this returns `Some`, then the narrow-phase will compute contact manifolds for
/// this pair of colliders, and configure them with the returned solver flags. For
/// example, if this returns `Some(SolverFlags::COMPUTE_IMPULSES)` then the contacts
/// will be taken into account by the constraints solver. If this returns
/// `Some(SolverFlags::empty())` then the constraints solver will ignore these
/// contacts.
fn filter_contact_pair(&self, _context: &PairFilterContext) -> Option<SolverFlags> {
None
}
/// Applies the intersection pair filter.
///
/// Note that this method will only be called if `self.active_hooks()`
/// contains the `PhysicsHooksFlags::FILTER_INTERSECTION_PAIR` flags.
///
/// User-defined filter for potential intersection pairs detected by the broad-phase.
///
/// This can be used to apply custom logic in order to decide whether two colliders
/// should have their intersection computed by the narrow-phase.
///
/// Note that using an intersection pair filter will replace the default intersection filtering
/// which consists of preventing intersection computation between two non-dynamic bodies.
///
/// This filtering method is called after taking into account the colliders collision groups.
///
/// If this returns `false`, then the narrow-phase will ignore this pair and
/// not compute any intersection information for it.
/// If this return `true` then the narrow-phase will compute intersection
/// information for this pair.
fn filter_intersection_pair(&self, _context: &PairFilterContext) -> bool {
false
}
/// Modifies the set of contacts seen by the constraints solver.
///
/// Note that this method will only be called if `self.active_hooks()`
/// contains the `PhysicsHooksFlags::MODIFY_SOLVER_CONTACTS` flags.
///
/// By default, the content of `solver_contacts` is computed from `manifold.points`.
/// This method will be called on each contact manifold which have the flag `SolverFlags::modify_solver_contacts` set.
/// This method can be used to modify the set of solver contacts seen by the constraints solver: contacts
/// can be removed and modified.
///
/// Note that if all the contacts have to be ignored by the constraint solver, you may simply
/// do `context.solver_contacts.clear()`.
///
/// Modifying the solver contacts allow you to achieve various effects, including:
/// - Simulating conveyor belts by setting the `surface_velocity` of a solver contact.
/// - Simulating shapes with multiply materials by modifying the friction and restitution
/// coefficient depending of the features in contacts.
/// - Simulating one-way platforms depending on the contact normal.
///
/// Each contact manifold is given a `u32` user-defined data that is persistent between
/// timesteps (as long as the contact manifold exists). This user-defined data is initialized
/// as 0 and can be modified in `context.user_data`.
fn modify_solver_contacts(&self, _context: &mut ContactModificationContext) {}
}
impl PhysicsHooks for () {
fn active_hooks(&self) -> PhysicsHooksFlags {
PhysicsHooksFlags::empty()
}
fn filter_contact_pair(&self, _: &PairFilterContext) -> Option<SolverFlags> {
None
}
fn filter_intersection_pair(&self, _: &PairFilterContext) -> bool {
false
}
fn modify_solver_contacts(&self, _: &mut ContactModificationContext) {}
}

18
src/pipeline/physics_pipeline.rs
View File

@@ -7,11 +7,10 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex,
ContactPairFilter, IntersectionPairFilter, NarrowPhase,
BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
};
use crate::math::{Real, Vector};
use crate::pipeline::EventHandler;
use crate::pipeline::{EventHandler, PhysicsHooks};
/// The physics pipeline, responsible for stepping the whole physics simulation.
///
@@ -69,8 +68,7 @@ impl PhysicsPipeline {
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
contact_pair_filter: Option<&dyn ContactPairFilter>,
proximity_pair_filter: Option<&dyn IntersectionPairFilter>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
self.counters.step_started();
@@ -115,10 +113,10 @@ impl PhysicsPipeline {
integration_parameters.prediction_distance,
bodies,
colliders,
contact_pair_filter,
hooks,
events,
);
narrow_phase.compute_intersections(bodies, colliders, proximity_pair_filter, events);
narrow_phase.compute_intersections(bodies, colliders, hooks, events);
// println!("Compute contact time: {}", instant::now() - t);
self.counters.stages.island_construction_time.start();
@@ -280,8 +278,7 @@ mod test {
&mut bodies,
&mut colliders,
&mut joints,
None,
None,
&(),
&(),
);
}
@@ -324,8 +321,7 @@ mod test {
&mut bodies,
&mut colliders,
&mut joints,
None,
None,
&(),
&(),
);
}

15
src_testbed/harness/mod.rs
View File

@@ -7,7 +7,7 @@ use plugin::HarnessPlugin;
use rapier::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
use rapier::geometry::{BroadPhase, ColliderSet, NarrowPhase};
use rapier::math::Vector;
use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline};
use rapier::pipeline::{ChannelEventCollector, PhysicsHooks, PhysicsPipeline, QueryPipeline};
pub mod plugin;
@@ -111,15 +111,16 @@ impl Harness {
}
pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) {
self.set_world_with_gravity(bodies, colliders, joints, Vector::y() * -9.81)
self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ())
}
pub fn set_world_with_gravity(
pub fn set_world_with_params(
&mut self,
bodies: RigidBodySet,
colliders: ColliderSet,
joints: JointSet,
gravity: Vector<f32>,
hooks: impl PhysicsHooks + 'static,
) {
// println!("Num bodies: {}", bodies.len());
// println!("Num joints: {}", joints.len());
@@ -127,6 +128,8 @@ impl Harness {
self.physics.bodies = bodies;
self.physics.colliders = colliders;
self.physics.joints = joints;
self.physics.hooks = Box::new(hooks);
self.physics.broad_phase = BroadPhase::new();
self.physics.narrow_phase = NarrowPhase::new();
self.state.timestep_id = 0;
@@ -176,8 +179,7 @@ impl Harness {
&mut physics.bodies,
&mut physics.colliders,
&mut physics.joints,
None,
None,
&*physics.hooks,
event_handler,
);
});
@@ -192,8 +194,7 @@ impl Harness {
&mut self.physics.bodies,
&mut self.physics.colliders,
&mut self.physics.joints,
None,
None,
&*self.physics.hooks,
&self.event_handler,
);

4
src_testbed/physics/mod.rs
View File

@@ -2,7 +2,7 @@ use crossbeam::channel::Receiver;
use rapier::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, IntersectionEvent, NarrowPhase};
use rapier::math::Vector;
use rapier::pipeline::{PhysicsPipeline, QueryPipeline};
use rapier::pipeline::{PhysicsHooks, PhysicsPipeline, QueryPipeline};
pub struct PhysicsSnapshot {
timestep_id: usize,
@@ -77,6 +77,7 @@ pub struct PhysicsState {
pub query_pipeline: QueryPipeline,
pub integration_parameters: IntegrationParameters,
pub gravity: Vector<f32>,
pub hooks: Box<dyn PhysicsHooks>,
}
impl PhysicsState {
@@ -91,6 +92,7 @@ impl PhysicsState {
query_pipeline: QueryPipeline::new(),
integration_parameters: IntegrationParameters::default(),
gravity: Vector::y() * -9.81,
hooks: Box::new(()),
}
}
}

10
src_testbed/testbed.rs
View File

@@ -25,6 +25,7 @@ use rapier::geometry::{ColliderHandle, ColliderSet, NarrowPhase};
#[cfg(feature = "dim3")]
use rapier::geometry::{InteractionGroups, Ray};
use rapier::math::{Isometry, Vector};
use rapier::pipeline::PhysicsHooks;
#[cfg(all(feature = "dim2", feature = "other-backends"))]
use crate::box2d_backend::Box2dWorld;
@@ -245,20 +246,19 @@ impl Testbed {
}
pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) {
self.set_world_with_gravity(bodies, colliders, joints, Vector::y() * -9.81)
self.set_world_with_params(bodies, colliders, joints, Vector::y() * -9.81, ())
}
pub fn set_world_with_gravity(
pub fn set_world_with_params(
&mut self,
bodies: RigidBodySet,
colliders: ColliderSet,
joints: JointSet,
gravity: Vector<f32>,
hooks: impl PhysicsHooks + 'static,
) {
println!("Num bodies: {}", bodies.len());
println!("Num joints: {}", joints.len());
self.harness
.set_world_with_gravity(bodies, colliders, joints, gravity);
.set_world_with_params(bodies, colliders, joints, gravity, hooks);
self.state
.action_flags