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

First working version of non-linear CCD based on single-substep motion-clamping.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-03-26 18:16:27 +01:00
parent 326469a1df
commit 97157c9423
29 changed files with 696 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/data/coarena.rs
View File

@@ -29,6 +29,20 @@ impl<T> Coarena<T> {
.and_then(|(gg, t)| if g == *gg { Some(t) } else { None })
}
/// Inserts an element into this coarena.
pub fn insert(&mut self, a: Index, value: T)
where
T: Clone + Default,
{
let (i1, g1) = a.into_raw_parts();
if self.data.len() <= i1 {
self.data.resize(i1 + 1, (u32::MAX as u64, T::default()));
}
self.data[i1] = (g1, value);
}
/// Ensure that elements at the two given indices exist in this coarena, and return their reference.
///
/// Missing elements are created automatically and initialized with the `default` value.

147
src/dynamics/ccd/toi_entry.rs Normal file
View File

@@ -0,0 +1,147 @@
use crate::data::Coarena;
use crate::dynamics::ccd::ccd_solver::CCDContact;
use crate::dynamics::ccd::CCDData;
use crate::dynamics::{IntegrationParameters, RigidBody, RigidBodyHandle};
use crate::geometry::{Collider, ColliderHandle};
use crate::math::{Isometry, Real};
use crate::parry::query::PersistentQueryDispatcher;
use crate::utils::WCross;
use na::{RealField, Unit};
use parry::query::{NonlinearRigidMotion, QueryDispatcher, TOI};
#[derive(Copy, Clone, Debug)]
pub struct TOIEntry {
pub toi: Real,
pub c1: ColliderHandle,
pub b1: RigidBodyHandle,
pub c2: ColliderHandle,
pub b2: RigidBodyHandle,
pub is_intersection_test: bool,
pub timestamp: usize,
}
impl TOIEntry {
fn new(
toi: Real,
c1: ColliderHandle,
b1: RigidBodyHandle,
c2: ColliderHandle,
b2: RigidBodyHandle,
is_intersection_test: bool,
timestamp: usize,
) -> Self {
Self {
toi,
c1,
b1,
c2,
b2,
is_intersection_test,
timestamp,
}
}
pub fn try_from_colliders<QD: ?Sized + PersistentQueryDispatcher<(), ()>>(
params: &IntegrationParameters,
query_dispatcher: &QD,
ch1: ColliderHandle,
ch2: ColliderHandle,
c1: &Collider,
c2: &Collider,
b1: &RigidBody,
b2: &RigidBody,
frozen1: Option<Real>,
frozen2: Option<Real>,
start_time: Real,
end_time: Real,
body_params: &Coarena<CCDData>,
) -> Option<Self> {
assert!(start_time <= end_time);
let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel;
let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel;
let vel12 = linvel2 - linvel1;
let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness());
if params.dt * vel12.norm() < thickness {
return None;
}
let is_intersection_test = c1.is_sensor() || c2.is_sensor();
let body_params1 = body_params.get(c1.parent.0)?;
let body_params2 = body_params.get(c2.parent.0)?;
// Compute the TOI.
let mut motion1 = body_params1.motion(params.dt, b1, 0.0);
let mut motion2 = body_params2.motion(params.dt, b2, 0.0);
if let Some(t) = frozen1 {
motion1.freeze(t);
}
if let Some(t) = frozen2 {
motion2.freeze(t);
}
let mut toi;
let motion_c1 = motion1.prepend(*c1.position_wrt_parent());
let motion_c2 = motion2.prepend(*c2.position_wrt_parent());
// println!("start_time: {}", start_time);
// If this is just an intersection test (i.e. with sensors)
// then we can stop the TOI search immediately if it starts with
// a penetration because we don't care about the whether the velocity
// at the impact is a separating velocity or not.
// If the TOI search involves two non-sensor colliders then
// we don't want to stop the TOI search at the first penetration
// because the colliders may be in a separating trajectory.
let stop_at_penetration = is_intersection_test;
let res_toi = query_dispatcher
.nonlinear_time_of_impact(
&motion_c1,
c1.shape(),
&motion_c2,
c2.shape(),
start_time,
end_time,
stop_at_penetration,
)
.ok();
toi = res_toi??;
Some(Self::new(
toi.toi,
ch1,
c1.parent(),
ch2,
c2.parent(),
is_intersection_test,
0,
))
}
}
impl PartialOrd for TOIEntry {
fn partial_cmp(&self, other: &Self) -> Option<std::cmp::Ordering> {
(-self.toi).partial_cmp(&(-other.toi))
}
}
impl Ord for TOIEntry {
fn cmp(&self, other: &Self) -> std::cmp::Ordering {
self.partial_cmp(other).unwrap()
}
}
impl PartialEq for TOIEntry {
fn eq(&self, other: &Self) -> bool {
self.toi == other.toi
}
}
impl Eq for TOIEntry {}

2
src/dynamics/mod.rs
View File

@@ -18,6 +18,7 @@ pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBui
pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet};
pub use parry::mass_properties::MassProperties;
// #[cfg(not(feature = "parallel"))]
pub use self::ccd::CCDSolver;
pub use self::coefficient_combine_rule::CoefficientCombineRule;
pub(crate) use self::joint::JointGraphEdge;
pub(crate) use self::rigid_body::RigidBodyChanges;
@@ -26,6 +27,7 @@ pub(crate) use self::solver::IslandSolver;
#[cfg(feature = "parallel")]
pub(crate) use self::solver::ParallelIslandSolver;
mod ccd;
mod coefficient_combine_rule;
mod integration_parameters;
mod joint;

112
src/dynamics/rigid_body.rs
View File

@@ -36,6 +36,7 @@ bitflags::bitflags! {
const ROTATION_LOCKED_X = 1 << 1;
const ROTATION_LOCKED_Y = 1 << 2;
const ROTATION_LOCKED_Z = 1 << 3;
const CCD_ENABLED = 1 << 4;
}
}
@@ -58,7 +59,16 @@ bitflags::bitflags! {
pub struct RigidBody {
/// The world-space position of the rigid-body.
pub(crate) position: Isometry<Real>,
pub(crate) predicted_position: Isometry<Real>,
/// The next position of the rigid-body.
///
/// At the beginning of the timestep, and when the
/// timestep is complete we must have position == next_position
/// except for kinematic bodies.
///
/// The next_position is updated after the velocity and position
/// resolution. Then it is either validated (ie. we set position := set_position)
/// or clamped by CCD.
pub(crate) next_position: Isometry<Real>,
/// The local mass properties of the rigid-body.
pub(crate) mass_properties: MassProperties,
/// The world-space center of mass of the rigid-body.
@@ -76,6 +86,10 @@ pub struct RigidBody {
pub linear_damping: Real,
/// Damping factor for gradually slowing down the angular motion of the rigid-body.
pub angular_damping: Real,
/// The maximum linear velocity this rigid-body can reach.
pub max_linear_velocity: Real,
/// The maximum angular velocity this rigid-body can reach.
pub max_angular_velocity: Real,
/// Accumulation of external forces (only for dynamic bodies).
pub(crate) force: Vector<Real>,
/// Accumulation of external torques (only for dynamic bodies).
@@ -97,13 +111,14 @@ pub struct RigidBody {
dominance_group: i8,
/// User-defined data associated to this rigid-body.
pub user_data: u128,
pub(crate) ccd_thickness: Real,
}
impl RigidBody {
fn new() -> Self {
Self {
position: Isometry::identity(),
predicted_position: Isometry::identity(),
next_position: Isometry::identity(),
mass_properties: MassProperties::zero(),
world_com: Point::origin(),
effective_inv_mass: 0.0,
@@ -115,6 +130,8 @@ impl RigidBody {
gravity_scale: 1.0,
linear_damping: 0.0,
angular_damping: 0.0,
max_linear_velocity: Real::MAX,
max_angular_velocity: 100.0,
colliders: Vec::new(),
activation: ActivationStatus::new_active(),
joint_graph_index: InteractionGraph::<(), ()>::invalid_graph_index(),
@@ -127,6 +144,7 @@ impl RigidBody {
body_status: BodyStatus::Dynamic,
dominance_group: 0,
user_data: 0,
ccd_thickness: Real::MAX,
}
}
@@ -176,6 +194,20 @@ impl RigidBody {
}
}
/// Enables of disable CCD (continuous collision-detection) for this rigid-body.
pub fn enable_ccd(&mut self, enabled: bool) {
self.flags.set(RigidBodyFlags::CCD_ENABLED, enabled)
}
/// Is CCD (continous collision-detection) enabled for this rigid-body?
pub fn is_ccd_enabled(&self) -> bool {
self.flags.contains(RigidBodyFlags::CCD_ENABLED)
}
pub(crate) fn should_resolve_ccd(&self, dt: Real) -> bool {
self.is_ccd_enabled() && self.is_dynamic() && self.linvel.norm() * dt > self.ccd_thickness
}
/// Sets the rigid-body's mass properties.
///
/// If `wake_up` is `true` then the rigid-body will be woken up if it was
@@ -228,8 +260,8 @@ impl RigidBody {
/// If this rigid-body is kinematic this value is set by the `set_next_kinematic_position`
/// method and is used for estimating the kinematic body velocity at the next timestep.
/// For non-kinematic bodies, this value is currently unspecified.
pub fn predicted_position(&self) -> &Isometry<Real> {
&self.predicted_position
pub fn next_position(&self) -> &Isometry<Real> {
&self.next_position
}
/// The scale factor applied to the gravity affecting this rigid-body.
@@ -254,6 +286,8 @@ impl RigidBody {
true,
);
self.ccd_thickness = self.ccd_thickness.min(coll.shape().ccd_thickness());
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
@@ -265,8 +299,8 @@ impl RigidBody {
pub(crate) fn update_colliders_positions(&mut self, colliders: &mut ColliderSet) {
for handle in &self.colliders {
let collider = &mut colliders[*handle];
collider.prev_position = self.position;
collider.position = self.position * collider.delta;
collider.predicted_position = self.predicted_position * collider.delta;
}
}
@@ -331,18 +365,39 @@ impl RigidBody {
!self.linvel.is_zero() || !self.angvel.is_zero()
}
fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
pub(crate) fn integrate_velocity(&self, dt: Real) -> Isometry<Real> {
let com = self.position * self.mass_properties.local_com;
let shift = Translation::from(com.coords);
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
}
pub(crate) fn integrate(&mut self, dt: Real) {
// TODO: do we want to apply damping before or after the velocity integration?
self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
pub(crate) fn position_at_time(&self, dt: Real) -> Isometry<Real> {
self.integrate_velocity(dt) * self.position
}
self.position = self.integrate_velocity(dt) * self.position;
pub(crate) fn integrate_next_position(&mut self, dt: Real, apply_damping: bool) {
// TODO: do we want to apply damping before or after the velocity integration?
if apply_damping {
self.linvel *= 1.0 / (1.0 + dt * self.linear_damping);
self.angvel *= 1.0 / (1.0 + dt * self.angular_damping);
// self.linvel = self.linvel.cap_magnitude(self.max_linear_velocity);
// #[cfg(feature = "dim2")]
// {
// self.angvel = na::clamp(
// self.angvel,
// -self.max_angular_velocity,
// self.max_angular_velocity,
// );
// }
// #[cfg(feature = "dim3")]
// {
// self.angvel = self.angvel.cap_magnitude(self.max_angular_velocity);
// }
}
self.next_position = self.integrate_velocity(dt) * self.position;
let _ = self.next_position.rotation.renormalize();
}
/// The linear velocity of this rigid-body.
@@ -416,7 +471,8 @@ impl RigidBody {
/// put to sleep because it did not move for a while.
pub fn set_position(&mut self, pos: Isometry<Real>, wake_up: bool) {
self.changes.insert(RigidBodyChanges::POSITION);
self.set_position_internal(pos);
self.position = pos;
self.next_position = pos;
// TODO: Do we really need to check that the body isn't dynamic?
if wake_up && self.is_dynamic() {
@@ -424,24 +480,19 @@ impl RigidBody {
}
}
pub(crate) fn set_position_internal(&mut self, pos: Isometry<Real>) {
self.position = pos;
// TODO: update the predicted position for dynamic bodies too?
if self.is_static() || self.is_kinematic() {
self.predicted_position = pos;
}
pub(crate) fn set_next_position(&mut self, pos: Isometry<Real>) {
self.next_position = pos;
}
/// If this rigid body is kinematic, sets its future position after the next timestep integration.
pub fn set_next_kinematic_position(&mut self, pos: Isometry<Real>) {
if self.is_kinematic() {
self.predicted_position = pos;
self.next_position = pos;
}
}
pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: Real) {
let dpos = self.predicted_position * self.position.inverse();
pub(crate) fn compute_velocity_from_next_position(&mut self, inv_dt: Real) {
let dpos = self.next_position * self.position.inverse();
#[cfg(feature = "dim2")]
{
self.angvel = dpos.rotation.angle() * inv_dt;
@@ -453,8 +504,8 @@ impl RigidBody {
self.linvel = dpos.translation.vector * inv_dt;
}
pub(crate) fn update_predicted_position(&mut self, dt: Real) {
self.predicted_position = self.integrate_velocity(dt) * self.position;
pub(crate) fn update_next_position(&mut self, dt: Real) {
self.next_position = self.integrate_velocity(dt) * self.position;
}
pub(crate) fn update_world_mass_properties(&mut self) {
@@ -666,6 +717,7 @@ pub struct RigidBodyBuilder {
mass_properties: MassProperties,
can_sleep: bool,
sleeping: bool,
ccd_enabled: bool,
dominance_group: i8,
user_data: u128,
}
@@ -685,6 +737,7 @@ impl RigidBodyBuilder {
mass_properties: MassProperties::zero(),
can_sleep: true,
sleeping: false,
ccd_enabled: false,
dominance_group: 0,
user_data: 0,
}
@@ -888,6 +941,12 @@ impl RigidBodyBuilder {
self
}
/// Enabled continuous collision-detection for this rigid-body.
pub fn ccd_enabled(mut self, enabled: bool) -> Self {
self.ccd_enabled = enabled;
self
}
/// Sets whether or not the rigid-body is to be created asleep.
pub fn sleeping(mut self, sleeping: bool) -> Self {
self.sleeping = sleeping;
@@ -897,8 +956,8 @@ impl RigidBodyBuilder {
/// Build a new rigid-body with the parameters configured with this builder.
pub fn build(&self) -> RigidBody {
let mut rb = RigidBody::new();
rb.predicted_position = self.position; // FIXME: compute the correct value?
rb.set_position_internal(self.position);
rb.next_position = self.position; // FIXME: compute the correct value?
rb.position = self.position;
rb.linvel = self.linvel;
rb.angvel = self.angvel;
rb.body_status = self.body_status;
@@ -909,6 +968,7 @@ impl RigidBodyBuilder {
rb.gravity_scale = self.gravity_scale;
rb.flags = self.flags;
rb.dominance_group = self.dominance_group;
rb.enable_ccd(self.ccd_enabled);
if self.can_sleep && self.sleeping {
rb.sleep();

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

@@ -59,7 +59,7 @@ impl IslandSolver {
counters.solver.velocity_update_time.resume();
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.integrate(params.dt)
rb.integrate_next_position(params.dt, true)
});
counters.solver.velocity_update_time.pause();
@@ -77,7 +77,7 @@ impl IslandSolver {
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
// Since we didn't run the velocity solver we need to integrate the accelerations here
rb.integrate_accelerations(params.dt);
rb.integrate(params.dt);
rb.integrate_next_position(params.dt, true);
});
counters.solver.velocity_update_time.pause();
}

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

@@ -114,7 +114,7 @@ impl BallPositionGroundConstraint {
// are the local_anchors. The rb1 and rb2 have
// already been flipped by the caller.
Self {
anchor1: rb1.predicted_position * cparams.local_anchor2,
anchor1: rb1.next_position * cparams.local_anchor2,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor1,
@@ -123,7 +123,7 @@ impl BallPositionGroundConstraint {
}
} else {
Self {
anchor1: rb1.predicted_position * cparams.local_anchor1,
anchor1: rb1.next_position * cparams.local_anchor1,
im2: rb2.effective_inv_mass,
ii2: rb2.effective_world_inv_inertia_sqrt.squared(),
local_anchor2: cparams.local_anchor2,

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

@@ -134,7 +134,7 @@ impl WBallPositionGroundConstraint {
cparams: [&BallJoint; SIMD_WIDTH],
flipped: [bool; SIMD_WIDTH],
) -> Self {
let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
let position1 = Isometry::from(array![|ii| rbs1[ii].next_position; SIMD_WIDTH]);
let anchor1 = position1
* Point::from(array![|ii| if flipped[ii] {
cparams[ii].local_anchor2

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

@@ -100,10 +100,10 @@ impl FixedPositionGroundConstraint {
let local_anchor2;
if flipped {
anchor1 = rb1.predicted_position * cparams.local_anchor2;
anchor1 = rb1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
} else {
anchor1 = rb1.predicted_position * cparams.local_anchor1;
anchor1 = rb1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
};

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

@@ -119,14 +119,14 @@ impl PrismaticPositionGroundConstraint {
let local_axis2;
if flipped {
frame1 = rb1.predicted_position * cparams.local_frame2();
frame1 = rb1.next_position * cparams.local_frame2();
local_frame2 = cparams.local_frame1();
axis1 = rb1.predicted_position * cparams.local_axis2;
axis1 = rb1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
} else {
frame1 = rb1.predicted_position * cparams.local_frame1();
frame1 = rb1.next_position * cparams.local_frame1();
local_frame2 = cparams.local_frame2();
axis1 = rb1.predicted_position * cparams.local_axis1;
axis1 = rb1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
};

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

@@ -145,23 +145,23 @@ impl RevolutePositionGroundConstraint {
let local_basis2;
if flipped {
anchor1 = rb1.predicted_position * cparams.local_anchor2;
anchor1 = rb1.next_position * cparams.local_anchor2;
local_anchor2 = cparams.local_anchor1;
axis1 = rb1.predicted_position * cparams.local_axis2;
axis1 = rb1.next_position * cparams.local_axis2;
local_axis2 = cparams.local_axis1;
basis1 = [
rb1.predicted_position * cparams.basis2[0],
rb1.predicted_position * cparams.basis2[1],
rb1.next_position * cparams.basis2[0],
rb1.next_position * cparams.basis2[1],
];
local_basis2 = cparams.basis1;
} else {
anchor1 = rb1.predicted_position * cparams.local_anchor1;
anchor1 = rb1.next_position * cparams.local_anchor1;
local_anchor2 = cparams.local_anchor2;
axis1 = rb1.predicted_position * cparams.local_axis1;
axis1 = rb1.next_position * cparams.local_axis1;
local_axis2 = cparams.local_axis2;
basis1 = [
rb1.predicted_position * cparams.basis1[0],
rb1.predicted_position * cparams.basis1[1],
rb1.next_position * cparams.basis1[0],
rb1.next_position * cparams.basis1[1],
];
local_basis2 = cparams.basis2;
};

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

@@ -277,7 +277,7 @@ impl ParallelIslandSolver {
rb.linvel += dvel.linear;
rb.angvel += rb.effective_world_inv_inertia_sqrt.transform_vector(dvel.angular);
rb.integrate(params.dt);
positions[rb.active_set_offset] = rb.position;
positions[rb.active_set_offset] = rb.next_position;
}
}
@@ -298,7 +298,7 @@ impl ParallelIslandSolver {
let batch_size = thread.batch_size;
for handle in active_bodies[thread.position_writeback_index] {
let rb = &mut bodies[handle.0];
rb.set_position_internal(positions[rb.active_set_offset]);
rb.set_next_position(positions[rb.active_set_offset]);
}
}
})

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

@@ -25,7 +25,7 @@ impl PositionSolver {
self.positions.extend(
bodies
.iter_active_island(island_id)
.map(|(_, b)| b.position),
.map(|(_, b)| b.next_position),
);
for _ in 0..params.max_position_iterations {
@@ -39,7 +39,7 @@ impl PositionSolver {
}
bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| {
rb.set_position_internal(self.positions[rb.active_set_offset])
rb.set_next_position(self.positions[rb.active_set_offset])
});
}
}

17
src/geometry/collider.rs
View File

@@ -2,7 +2,7 @@ use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
use crate::geometry::{InteractionGroups, SAPProxyIndex, SharedShape, SolverFlags};
use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
use crate::parry::transformation::vhacd::VHACDParameters;
use parry::bounding_volume::AABB;
use parry::bounding_volume::{BoundingVolume, AABB};
use parry::shape::Shape;
bitflags::bitflags! {
@@ -62,7 +62,7 @@ pub struct Collider {
pub(crate) parent: RigidBodyHandle,
pub(crate) delta: Isometry<Real>,
pub(crate) position: Isometry<Real>,
pub(crate) predicted_position: Isometry<Real>,
pub(crate) prev_position: Isometry<Real>,
/// The friction coefficient of this collider.
pub friction: Real,
/// The restitution coefficient of this collider.
@@ -139,11 +139,12 @@ impl Collider {
self.shape.compute_aabb(&self.position)
}
// pub(crate) fn compute_aabb_with_prediction(&self) -> AABB {
// let aabb1 = self.shape.compute_aabb(&self.position);
// let aabb2 = self.shape.compute_aabb(&self.predicted_position);
// aabb1.merged(&aabb2)
// }
/// Compute the axis-aligned bounding box of this collider.
pub fn compute_swept_aabb(&self, next_position: &Isometry<Real>) -> AABB {
let aabb1 = self.shape.compute_aabb(&self.position);
let aabb2 = self.shape.compute_aabb(next_position);
aabb1.merged(&aabb2)
}
/// Compute the local-space mass properties of this collider.
pub fn mass_properties(&self) -> MassProperties {
@@ -595,8 +596,8 @@ impl ColliderBuilder {
flags,
solver_flags,
parent: RigidBodyHandle::invalid(),
prev_position: Isometry::identity(),
position: Isometry::identity(),
predicted_position: Isometry::identity(),
proxy_index: crate::INVALID_U32,
collision_groups: self.collision_groups,
solver_groups: self.solver_groups,

2
src/geometry/collider_set.rs
View File

@@ -108,8 +108,8 @@ impl ColliderSet {
let parent = bodies
.get_mut(parent_handle)
.expect("Parent rigid body not found.");
coll.prev_position = parent.position * coll.delta;
coll.position = parent.position * coll.delta;
coll.predicted_position = parent.predicted_position * coll.delta;
let handle = ColliderHandle(self.colliders.insert(coll));
let coll = self.colliders.get(handle.0).unwrap();
parent.add_collider(handle, &coll);

8
src/geometry/narrow_phase.rs
View File

@@ -71,6 +71,14 @@ impl NarrowPhase {
}
}
/// The query dispatcher used by this narrow-phase to select the right collision-detection
/// algorithms depending of the shape types.
pub fn query_dispatcher(
&self,
) -> &dyn PersistentQueryDispatcher<ContactManifoldData, ContactData> {
&*self.query_dispatcher
}
/// The contact graph containing all contact pairs and their contact information.
pub fn contact_graph(&self) -> &InteractionGraph<ColliderHandle, ContactPair> {
&self.contact_graph

11
src/pipeline/collision_pipeline.rs
View File

@@ -69,21 +69,18 @@ impl CollisionPipeline {
// // Update kinematic bodies velocities.
// bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
// body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
// body.compute_velocity_from_next_position(integration_parameters.inv_dt());
// });
// Update colliders positions and kinematic bodies positions.
bodies.foreach_active_body_mut_internal(|_, rb| {
if rb.is_kinematic() {
rb.position = rb.predicted_position;
} else {
rb.update_predicted_position(0.0);
}
rb.position = rb.next_position;
rb.update_colliders_positions(colliders);
for handle in &rb.colliders {
let collider = &mut colliders[*handle];
collider.prev_position = collider.position;
collider.position = rb.position * collider.delta;
collider.predicted_position = rb.predicted_position * collider.delta;
}
});

25
src/pipeline/physics_pipeline.rs
View File

@@ -3,7 +3,7 @@
use crate::counters::Counters;
#[cfg(not(feature = "parallel"))]
use crate::dynamics::IslandSolver;
use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
use crate::dynamics::{CCDSolver, IntegrationParameters, JointSet, RigidBodySet};
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
@@ -68,6 +68,7 @@ impl PhysicsPipeline {
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
ccd_solver: Option<&mut CCDSolver>,
hooks: &dyn PhysicsHooks,
events: &dyn EventHandler,
) {
@@ -81,7 +82,7 @@ impl PhysicsPipeline {
// there to determine if this kinematic body should wake-up dynamic
// bodies it is touching.
bodies.foreach_active_kinematic_body_mut_internal(|_, body| {
body.compute_velocity_from_predicted_position(integration_parameters.inv_dt());
body.compute_velocity_from_next_position(integration_parameters.inv_dt());
});
self.counters.stages.collision_detection_time.start();
@@ -218,23 +219,33 @@ impl PhysicsPipeline {
});
}
// Update colliders positions and kinematic bodies positions.
// FIXME: do this in the solver?
// Handle CCD
if let Some(ccd_solver) = ccd_solver {
let impacts = ccd_solver.predict_next_impacts(
integration_parameters,
bodies,
colliders,
integration_parameters.dt,
events,
);
ccd_solver.clamp_motions(integration_parameters.dt, bodies, &impacts);
}
// Set the rigid-bodies and kinematic bodies to their final position.
bodies.foreach_active_body_mut_internal(|_, rb| {
if rb.is_kinematic() {
rb.position = rb.predicted_position;
rb.linvel = na::zero();
rb.angvel = na::zero();
} else {
rb.update_predicted_position(integration_parameters.dt);
}
rb.position = rb.next_position;
rb.update_colliders_positions(colliders);
});
self.counters.stages.solver_time.pause();
bodies.modified_inactive_set.clear();
self.counters.step_completed();
}
}

74
src/pipeline/query_pipeline.rs
View File

@@ -1,10 +1,9 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{
Collider, ColliderHandle, ColliderSet, InteractionGroups, PointProjection, Ray,
RayIntersection, SimdQuadTree,
RayIntersection, SimdQuadTree, AABB,
};
use crate::math::{Isometry, Point, Real, Vector};
use crate::parry::motion::RigidMotion;
use parry::query::details::{
IntersectionCompositeShapeShapeBestFirstVisitor,
NonlinearTOICompositeShapeShapeBestFirstVisitor, PointCompositeShapeProjBestFirstVisitor,
@@ -15,7 +14,7 @@ use parry::query::details::{
use parry::query::visitors::{
BoundingVolumeIntersectionsVisitor, PointIntersectionsVisitor, RayIntersectionsVisitor,
};
use parry::query::{DefaultQueryDispatcher, QueryDispatcher, TOI};
use parry::query::{DefaultQueryDispatcher, NonlinearRigidMotion, QueryDispatcher, TOI};
use parry::shape::{FeatureId, Shape, TypedSimdCompositeShape};
use std::sync::Arc;
@@ -95,7 +94,7 @@ impl QueryPipeline {
/// Initializes an empty query pipeline with a custom `QueryDispatcher`.
///
/// Use this constructor in order to use a custom `QueryDispatcher` that is
/// awary of your own user-defined shapes.
/// aware of your own user-defined shapes.
pub fn with_query_dispatcher<D>(d: D) -> Self
where
D: 'static + QueryDispatcher,
@@ -108,11 +107,26 @@ impl QueryPipeline {
}
}
/// The query dispatcher used by this query pipeline for running scene queries.
pub fn query_dispatcher(&self) -> &dyn QueryDispatcher {
&*self.query_dispatcher
}
/// Update the acceleration structure on the query pipeline.
pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet) {
pub fn update(&mut self, bodies: &RigidBodySet, colliders: &ColliderSet, use_swept_aabb: bool) {
if !self.tree_built {
let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
if !use_swept_aabb {
let data = colliders.iter().map(|(h, c)| (h, c.compute_aabb()));
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
} else {
let data = colliders.iter().map(|(h, co)| {
let next_position =
bodies[co.parent()].next_position * co.position_wrt_parent();
(h, co.compute_swept_aabb(&next_position))
});
self.quadtree.clear_and_rebuild(data, self.dilation_factor);
}
// FIXME: uncomment this once we handle insertion/removals properly.
// self.tree_built = true;
return;
@@ -127,10 +141,22 @@ impl QueryPipeline {
}
}
self.quadtree.update(
|handle| colliders[*handle].compute_aabb(),
self.dilation_factor,
);
if !use_swept_aabb {
self.quadtree.update(
|handle| colliders[*handle].compute_aabb(),
self.dilation_factor,
);
} else {
self.quadtree.update(
|handle| {
let co = &colliders[*handle];
let next_position =
bodies[co.parent()].next_position * co.position_wrt_parent();
co.compute_swept_aabb(&next_position)
},
self.dilation_factor,
);
}
}
/// Find the closest intersection between a ray and a set of collider.
@@ -336,6 +362,16 @@ impl QueryPipeline {
.map(|h| (h.1 .1 .0, h.1 .0, h.1 .1 .1))
}
/// Finds all handles of all the colliders with an AABB intersecting the given AABB.
pub fn colliders_with_aabb_intersecting_aabb(
&self,
aabb: &AABB,
mut callback: impl FnMut(&ColliderHandle) -> bool,
) {
let mut visitor = BoundingVolumeIntersectionsVisitor::new(aabb, &mut callback);
self.quadtree.traverse_depth_first(&mut visitor);
}
/// Casts a shape at a constant linear velocity and retrieve the first collider it hits.
///
/// This is similar to ray-casting except that we are casting a whole shape instead of
@@ -386,20 +422,24 @@ impl QueryPipeline {
pub fn nonlinear_cast_shape(
&self,
colliders: &ColliderSet,
shape_motion: &dyn RigidMotion,
shape_motion: &NonlinearRigidMotion,
shape: &dyn Shape,
max_toi: Real,
target_distance: Real,
start_time: Real,
end_time: Real,
stop_at_penetration: bool,
groups: InteractionGroups,
) -> Option<(ColliderHandle, TOI)> {
let pipeline_shape = self.as_composite_shape(colliders, groups);
let pipeline_motion = NonlinearRigidMotion::identity();
let mut visitor = NonlinearTOICompositeShapeShapeBestFirstVisitor::new(
&*self.query_dispatcher,
shape_motion,
&pipeline_motion,
&pipeline_shape,
shape_motion,
shape,
max_toi,
target_distance,
start_time,
end_time,
stop_at_penetration,
);
self.quadtree.traverse_best_first(&mut visitor).map(|h| h.1)
}