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

Split rigid-bodies and colliders into multiple components

Browse Source
This commit is contained in:
Crozet Sébastien
2021-04-26 17:59:25 +02:00
parent aaf80bfa87
commit c32da78f2a
91 changed files with 5969 additions and 3653 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,5 +1,9 @@
use crate::dynamics::{RigidBody, RigidBodyHandle};
use crate::geometry::{Collider, ColliderHandle};
use crate::dynamics::{
RigidBodyCcd, RigidBodyHandle, RigidBodyMassProps, RigidBodyPosition, RigidBodyVelocity,
};
use crate::geometry::{
ColliderHandle, ColliderParent, ColliderPosition, ColliderShape, ColliderType,
};
use crate::math::Real;
use parry::query::{NonlinearRigidMotion, QueryDispatcher};
@@ -7,9 +11,9 @@ use parry::query::{NonlinearRigidMotion, QueryDispatcher};
pub struct TOIEntry {
pub toi: Real,
pub c1: ColliderHandle,
pub b1: RigidBodyHandle,
pub b1: Option<RigidBodyHandle>,
pub c2: ColliderHandle,
pub b2: RigidBodyHandle,
pub b2: Option<RigidBodyHandle>,
pub is_intersection_test: bool,
pub timestamp: usize,
}
@@ -18,9 +22,9 @@ impl TOIEntry {
fn new(
toi: Real,
c1: ColliderHandle,
b1: RigidBodyHandle,
b1: Option<RigidBodyHandle>,
c2: ColliderHandle,
b2: RigidBodyHandle,
b2: Option<RigidBodyHandle>,
is_intersection_test: bool,
timestamp: usize,
) -> Self {
@@ -39,10 +43,30 @@ impl TOIEntry {
query_dispatcher: &QD,
ch1: ColliderHandle,
ch2: ColliderHandle,
c1: &Collider,
c2: &Collider,
b1: &RigidBody,
b2: &RigidBody,
c1: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
Option<&ColliderParent>,
),
c2: (
&ColliderType,
&ColliderShape,
&ColliderPosition,
Option<&ColliderParent>,
),
b1: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
b2: Option<(
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
)>,
frozen1: Option<Real>,
frozen2: Option<Real>,
start_time: Real,
@@ -50,35 +74,46 @@ impl TOIEntry {
smallest_contact_dist: Real,
) -> Option<Self> {
assert!(start_time <= end_time);
if b1.is_none() && b2.is_none() {
return None;
}
let linvel1 = frozen1.is_none() as u32 as Real * b1.linvel();
let linvel2 = frozen2.is_none() as u32 as Real * b2.linvel();
let angvel1 = frozen1.is_none() as u32 as Real * b1.angvel();
let angvel2 = frozen2.is_none() as u32 as Real * b2.angvel();
let (co_type1, co_shape1, co_pos1, co_parent1) = c1;
let (co_type2, co_shape2, co_pos2, co_parent2) = c2;
let linvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.linvel).unwrap_or(na::zero());
let linvel2 =
frozen2.is_none() as u32 as Real * b2.map(|b| b.1.linvel).unwrap_or(na::zero());
let angvel1 =
frozen1.is_none() as u32 as Real * b1.map(|b| b.1.angvel).unwrap_or(na::zero());
let angvel2 =
frozen2.is_none() as u32 as Real * b2.map(|b| b.1.angvel).unwrap_or(na::zero());
#[cfg(feature = "dim2")]
let vel12 = (linvel2 - linvel1).norm()
+ angvel1.abs() * b1.ccd_max_dist
+ angvel2.abs() * b2.ccd_max_dist;
+ angvel1.abs() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0)
+ angvel2.abs() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0);
#[cfg(feature = "dim3")]
let vel12 = (linvel2 - linvel1).norm()
+ angvel1.norm() * b1.ccd_max_dist
+ angvel2.norm() * b2.ccd_max_dist;
+ angvel1.norm() * b1.map(|b| b.3.ccd_max_dist).unwrap_or(0.0)
+ angvel2.norm() * b2.map(|b| b.3.ccd_max_dist).unwrap_or(0.0);
// We may be slightly over-conservative by taking the `max(0.0)` here.
// But removing the `max` doesn't really affect performances so let's
// keep it since more conservatism is good at this stage.
let thickness = (c1.shape().ccd_thickness() + c2.shape().ccd_thickness())
let thickness = (co_shape1.0.ccd_thickness() + co_shape2.0.ccd_thickness())
+ smallest_contact_dist.max(0.0);
let is_intersection_test = c1.is_sensor() || c2.is_sensor();
let is_intersection_test = co_type1.is_sensor() || co_type2.is_sensor();
if (end_time - start_time) * vel12 < thickness {
return None;
}
// Compute the TOI.
let mut motion1 = Self::body_motion(b1);
let mut motion2 = Self::body_motion(b2);
let identity = NonlinearRigidMotion::identity();
let mut motion1 = b1.map(Self::body_motion).unwrap_or(identity);
let mut motion2 = b2.map(Self::body_motion).unwrap_or(identity);
if let Some(t) = frozen1 {
motion1.freeze(t);
@@ -88,8 +123,8 @@ impl TOIEntry {
motion2.freeze(t);
}
let motion_c1 = motion1.prepend(*c1.position_wrt_parent());
let motion_c2 = motion2.prepend(*c2.position_wrt_parent());
let motion_c1 = motion1.prepend(co_parent1.map(|p| p.pos_wrt_parent).unwrap_or(co_pos1.0));
let motion_c2 = motion2.prepend(co_parent2.map(|p| p.pos_wrt_parent).unwrap_or(co_pos2.0));
// println!("start_time: {}", start_time);
@@ -105,9 +140,9 @@ impl TOIEntry {
let res_toi = query_dispatcher
.nonlinear_time_of_impact(
&motion_c1,
c1.shape(),
co_shape1.as_ref(),
&motion_c2,
c2.shape(),
co_shape2.as_ref(),
start_time,
end_time,
stop_at_penetration,
@@ -119,24 +154,31 @@ impl TOIEntry {
Some(Self::new(
toi.toi,
ch1,
c1.parent(),
co_parent1.map(|p| p.handle),
ch2,
c2.parent(),
co_parent2.map(|p| p.handle),
is_intersection_test,
0,
))
}
fn body_motion(body: &RigidBody) -> NonlinearRigidMotion {
if body.is_ccd_active() {
fn body_motion(
(poss, vels, mprops, ccd): (
&RigidBodyPosition,
&RigidBodyVelocity,
&RigidBodyMassProps,
&RigidBodyCcd,
),
) -> NonlinearRigidMotion {
if ccd.ccd_active {
NonlinearRigidMotion::new(
body.position,
body.mass_properties.local_com,
body.linvel,
body.angvel,
poss.position,
mprops.mass_properties.local_com,
vels.linvel,
vels.angvel,
)
} else {
NonlinearRigidMotion::constant_position(body.next_position)
NonlinearRigidMotion::constant_position(poss.next_position)
}
}
}