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

Fix mass property update when adding a collider.

Browse Source
This commit is contained in:
Sébastien Crozet
2020-09-01 17:05:24 +02:00
parent 9622827dc6
commit 2f2a073ce4
7 changed files with 127 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

119
src/dynamics/mass_properties.rs
View File

@@ -24,59 +24,6 @@ pub struct MassProperties {
pub principal_inertia_local_frame: Rotation<f32>,
}
impl approx::AbsDiffEq for MassProperties {
type Epsilon = f32;
fn default_epsilon() -> Self::Epsilon {
f32::default_epsilon()
}
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
self.local_com.abs_diff_eq(&other.local_com, epsilon)
&& self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
&& self
.inv_principal_inertia_sqrt
.abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
// && self
// .principal_inertia_local_frame
// .abs_diff_eq(&other.principal_inertia_local_frame, epsilon)
}
}
impl approx::RelativeEq for MassProperties {
fn default_max_relative() -> Self::Epsilon {
f32::default_max_relative()
}
fn relative_eq(
&self,
other: &Self,
epsilon: Self::Epsilon,
max_relative: Self::Epsilon,
) -> bool {
#[cfg(feature = "dim2")]
let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
&other.inv_principal_inertia_sqrt,
epsilon,
max_relative,
);
#[cfg(feature = "dim3")]
let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
&other.reconstruct_inverse_inertia_matrix(),
epsilon,
max_relative,
);
inertia_is_ok
&& self
.local_com
.relative_eq(&other.local_com, epsilon, max_relative)
&& self
.inv_mass
.relative_eq(&other.inv_mass, epsilon, max_relative)
}
}
impl MassProperties {
#[cfg(feature = "dim2")]
pub(crate) fn new(local_com: Point<f32>, mass: f32, principal_inertia: f32) -> Self {
@@ -190,6 +137,19 @@ impl MassProperties {
Matrix3::zeros()
}
}
/// Transform each element of the mass properties.
pub fn transform_by(&self, m: &Isometry<f32>) -> Self {
// NOTE: we don't apply the parallel axis theorem here
// because the center of mass is also transformed.
Self {
local_com: m * self.local_com,
inv_mass: self.inv_mass,
inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt,
#[cfg(feature = "dim3")]
principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame,
}
}
}
impl Zero for MassProperties {
@@ -331,6 +291,59 @@ impl AddAssign<MassProperties> for MassProperties {
}
}
impl approx::AbsDiffEq for MassProperties {
type Epsilon = f32;
fn default_epsilon() -> Self::Epsilon {
f32::default_epsilon()
}
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
self.local_com.abs_diff_eq(&other.local_com, epsilon)
&& self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
&& self
.inv_principal_inertia_sqrt
.abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
// && self
// .principal_inertia_local_frame
// .abs_diff_eq(&other.principal_inertia_local_frame, epsilon)
}
}
impl approx::RelativeEq for MassProperties {
fn default_max_relative() -> Self::Epsilon {
f32::default_max_relative()
}
fn relative_eq(
&self,
other: &Self,
epsilon: Self::Epsilon,
max_relative: Self::Epsilon,
) -> bool {
#[cfg(feature = "dim2")]
let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
&other.inv_principal_inertia_sqrt,
epsilon,
max_relative,
);
#[cfg(feature = "dim3")]
let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
&other.reconstruct_inverse_inertia_matrix(),
epsilon,
max_relative,
);
inertia_is_ok
&& self
.local_com
.relative_eq(&other.local_com, epsilon, max_relative)
&& self
.inv_mass
.relative_eq(&other.inv_mass, epsilon, max_relative)
}
}
#[cfg(test)]
mod test {
use super::MassProperties;

10
src/dynamics/rigid_body.rs
View File

@@ -139,7 +139,9 @@ impl RigidBody {
/// Adds a collider to this rigid-body.
pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
let mass_properties = coll.mass_properties();
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.colliders.push(handle);
self.mass_properties += mass_properties;
self.update_world_mass_properties();
@@ -149,7 +151,9 @@ impl RigidBody {
pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
self.colliders.swap_remove(i);
let mass_properties = coll.mass_properties();
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.mass_properties -= mass_properties;
self.update_world_mass_properties();
}
@@ -189,7 +193,7 @@ impl RigidBody {
}
fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses.
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()
}

1
src/geometry/collider.rs
View File

@@ -150,6 +150,7 @@ impl Collider {
}
/// The position of this collider expressed in the local-space of the rigid-body it is attached to.
#[deprecated(note = "use `.position_wrt_parent()` instead.")]
pub fn delta(&self) -> &Isometry<f32> {
&self.delta
}

4
src/geometry/contact.rs
View File

@@ -360,8 +360,8 @@ impl ContactManifold {
pair,
(subshape1, subshape2),
BodyPair::new(coll1.parent, coll2.parent),
*coll1.delta(),
*coll2.delta(),
*coll1.position_wrt_parent(),
*coll2.position_wrt_parent(),
(coll1.friction + coll2.friction) * 0.5,
(coll1.restitution + coll2.restitution) * 0.5,
)

2
src_testbed/nphysics_backend.rs
View File

@@ -165,7 +165,7 @@ impl NPhysicsWorld {
for coll_handle in rb.colliders() {
let collider = &mut colliders[*coll_handle];
collider.set_position_debug(pos * collider.delta());
collider.set_position_debug(pos * collider.position_wrt_parent());
}
}
}

24
src_testbed/physx_backend.rs
View File

@@ -225,6 +225,28 @@ impl PhysxWorld {
}
}
// Update mass properties.
for (rapier_handle, physx_handle) in rapier2physx.iter() {
let rb = &bodies[*rapier_handle];
if let Some(mut ra) = scene.get_dynamic_mut(*physx_handle) {
let densities: Vec<_> = rb
.colliders()
.iter()
.map(|h| colliders[*h].density())
.collect();
unsafe {
physx_sys::PxRigidBodyExt_updateMassAndInertia_mut(
ra.as_ptr_mut().ptr as *mut physx_sys::PxRigidBody,
densities.as_ptr(),
densities.len() as u32,
std::ptr::null(),
false,
);
}
}
}
let mut res = Self {
physics,
cooking,
@@ -390,7 +412,7 @@ impl PhysxWorld {
for coll_handle in rb.colliders() {
let collider = &mut colliders[*coll_handle];
collider.set_position_debug(iso * collider.delta());
collider.set_position_debug(iso * collider.position_wrt_parent());
}
}
}

31
src_testbed/testbed.rs
View File

@@ -791,6 +791,29 @@ impl Testbed {
.state
.action_flags
.set(TestbedActionFlags::EXAMPLE_CHANGED, true),
WindowEvent::Key(Key::C, Action::Release, _) => {
// Delete 1 collider of 10% of the remaining dynamic bodies.
let mut colliders: Vec<_> = self
.physics
.bodies
.iter()
.filter(|e| e.1.is_dynamic())
.filter(|e| !e.1.colliders().is_empty())
.map(|e| e.1.colliders().to_vec())
.collect();
colliders.sort_by_key(|co| -(co.len() as isize));
let num_to_delete = (colliders.len() / 10).max(1);
for to_delete in &colliders[..num_to_delete] {
self.physics.pipeline.remove_collider(
to_delete[0],
&mut self.physics.broad_phase,
&mut self.physics.narrow_phase,
&mut self.physics.bodies,
&mut self.physics.colliders,
);
}
}
WindowEvent::Key(Key::D, Action::Release, _) => {
// Delete 10% of the remaining dynamic bodies.
let dynamic_bodies: Vec<_> = self
@@ -1539,7 +1562,7 @@ impl State for Testbed {
}
if self.state.flags.contains(TestbedStateFlags::CONTACT_POINTS) {
draw_contacts(window, &self.physics.narrow_phase, &self.physics.bodies);
draw_contacts(window, &self.physics.narrow_phase, &self.physics.colliders);
}
if self.state.running == RunMode::Step {
@@ -1634,7 +1657,7 @@ Hashes at frame: {}
}
}
fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) {
fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) {
for (_, _, pair) in nf.contact_graph().interaction_pairs() {
for manifold in &pair.manifolds {
for pt in manifold.all_contacts() {
@@ -1643,8 +1666,8 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) {
} else {
Point3::new(1.0, 0.0, 0.0)
};
let pos1 = bodies[manifold.body_pair.body1].position;
let pos2 = bodies[manifold.body_pair.body2].position;
let pos1 = colliders[manifold.pair.collider1].position();
let pos2 = colliders[manifold.pair.collider2].position();
let start = pos1 * pt.local_p1;
let end = pos2 * pt.local_p2;
let n = pos1 * manifold.local_n1;