aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/dynamics/mass_properties.rs165
-rw-r--r--src/dynamics/rigid_body.rs20
-rw-r--r--src/geometry/broad_phase_multi_sap.rs4
-rw-r--r--src/geometry/collider.rs41
-rw-r--r--src/geometry/collider_set.rs6
-rw-r--r--src/pipeline/physics_pipeline.rs24
6 files changed, 250 insertions, 10 deletions
diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs
index cc2979c..8affe0a 100644
--- a/src/dynamics/mass_properties.rs
+++ b/src/dynamics/mass_properties.rs
@@ -1,7 +1,7 @@
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector};
use crate::utils;
use num::Zero;
-use std::ops::{Add, AddAssign};
+use std::ops::{Add, AddAssign, Sub, SubAssign};
#[cfg(feature = "dim3")]
use {na::Matrix3, std::ops::MulAssign};
@@ -24,6 +24,59 @@ 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 {
@@ -91,6 +144,18 @@ impl MassProperties {
}
#[cfg(feature = "dim3")]
+ /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii.
+ pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> {
+ let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e);
+ self.principal_inertia_local_frame.to_rotation_matrix()
+ * Matrix3::from_diagonal(&inv_principal_inertia)
+ * self
+ .principal_inertia_local_frame
+ .inverse()
+ .to_rotation_matrix()
+ }
+
+ #[cfg(feature = "dim3")]
/// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
@@ -143,6 +208,67 @@ impl Zero for MassProperties {
}
}
+impl Sub<MassProperties> for MassProperties {
+ type Output = Self;
+
+ #[cfg(feature = "dim2")]
+ fn sub(self, other: MassProperties) -> Self {
+ if self.is_zero() || other.is_zero() {
+ return self;
+ }
+
+ let m1 = utils::inv(self.inv_mass);
+ let m2 = utils::inv(other.inv_mass);
+ let inv_mass = utils::inv(m1 - m2);
+
+ let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass;
+ let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
+ let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
+ let inertia = i1 - i2;
+ // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
+ let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt());
+
+ Self {
+ local_com,
+ inv_mass,
+ inv_principal_inertia_sqrt,
+ }
+ }
+
+ #[cfg(feature = "dim3")]
+ fn sub(self, other: MassProperties) -> Self {
+ if self.is_zero() || other.is_zero() {
+ return self;
+ }
+
+ let m1 = utils::inv(self.inv_mass);
+ let m2 = utils::inv(other.inv_mass);
+ let inv_mass = utils::inv(m1 - m2);
+ let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass;
+ let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
+ let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
+ let inertia = i1 - i2;
+ let eigen = inertia.symmetric_eigen();
+ let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors);
+ let principal_inertia = eigen.eigenvalues;
+ // NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
+ let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt()));
+
+ Self {
+ local_com,
+ inv_mass,
+ inv_principal_inertia_sqrt,
+ principal_inertia_local_frame,
+ }
+ }
+}
+
+impl SubAssign<MassProperties> for MassProperties {
+ fn sub_assign(&mut self, rhs: MassProperties) {
+ *self = *self - rhs
+ }
+}
+
impl Add<MassProperties> for MassProperties {
type Output = Self;
@@ -204,3 +330,40 @@ impl AddAssign<MassProperties> for MassProperties {
*self = *self + rhs
}
}
+
+#[cfg(test)]
+mod test {
+ use super::MassProperties;
+ use crate::geometry::ColliderBuilder;
+ use approx::assert_relative_eq;
+ use num::Zero;
+
+ #[test]
+ fn mass_properties_add_sub() {
+ // Check that addition and subtraction of mass properties behave as expected.
+ let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build();
+ let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build();
+ let c3 = ColliderBuilder::ball(5.0).build();
+
+ let m1 = c1.mass_properties();
+ let m2 = c2.mass_properties();
+ let m3 = c3.mass_properties();
+ let m1m2m3 = m1 + m2 + m3;
+
+ assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6);
+ assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6);
+ assert_relative_eq!(
+ m1m2m3 - m1 - m2 - m3,
+ MassProperties::zero(),
+ epsilon = 1.0e-6
+ );
+ }
+}
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 584dc96..d9cddd1 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -1,5 +1,5 @@
use crate::dynamics::MassProperties;
-use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
+use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
use crate::utils::{WCross, WDot};
use num::Zero;
@@ -137,6 +137,24 @@ impl RigidBody {
crate::utils::inv(self.mass_properties.inv_mass)
}
+ /// 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();
+ self.colliders.push(handle);
+ self.mass_properties += mass_properties;
+ self.update_world_mass_properties();
+ }
+
+ /// Removes a collider from this rigid-body.
+ 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();
+ self.mass_properties -= mass_properties;
+ self.update_world_mass_properties();
+ }
+ }
+
/// Put this rigid body to sleep.
///
/// A sleeping body no longer moves and is no longer simulated by the physics engine unless
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs
index 0505e21..054fccf 100644
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -662,7 +662,7 @@ mod test {
let rb = RigidBodyBuilder::new_dynamic().build();
let co = ColliderBuilder::ball(0.5).build();
let hrb = bodies.insert(rb);
- let hco = colliders.insert(co, hrb, &mut bodies);
+ colliders.insert(co, hrb, &mut bodies);
broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
@@ -681,7 +681,7 @@ mod test {
let rb = RigidBodyBuilder::new_dynamic().build();
let co = ColliderBuilder::ball(0.5).build();
let hrb = bodies.insert(rb);
- let hco = colliders.insert(co, hrb, &mut bodies);
+ colliders.insert(co, hrb, &mut bodies);
// Make sure the proxy handles is recycled properly.
broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 2457212..01c78ac 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -3,7 +3,7 @@ use crate::geometry::{
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
Proximity, Triangle, Trimesh,
};
-use crate::math::{Isometry, Point, Vector};
+use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use na::Point3;
use ncollide::bounding_volume::{HasBoundingVolume, AABB};
use num::Zero;
@@ -159,6 +159,11 @@ impl Collider {
&self.position
}
+ /// The position of this collider wrt the body it is attached to.
+ pub fn position_wrt_parent(&self) -> &Isometry<f32> {
+ &self.delta
+ }
+
/// The density of this collider.
pub fn density(&self) -> f32 {
self.density
@@ -347,7 +352,41 @@ impl ColliderBuilder {
self
}
+ /// Sets the initial translation of the collider to be created,
+ /// relative to the rigid-body it is attached to.
+ #[cfg(feature = "dim2")]
+ pub fn translation(mut self, x: f32, y: f32) -> Self {
+ self.delta.translation.x = x;
+ self.delta.translation.y = y;
+ self
+ }
+
+ /// Sets the initial translation of the collider to be created,
+ /// relative to the rigid-body it is attached to.
+ #[cfg(feature = "dim3")]
+ pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self {
+ self.delta.translation.x = x;
+ self.delta.translation.y = y;
+ self.delta.translation.z = z;
+ self
+ }
+
+ /// Sets the initial orientation of the collider to be created,
+ /// relative to the rigid-body it is attached to.
+ pub fn rotation(mut self, angle: AngVector<f32>) -> Self {
+ self.delta.rotation = Rotation::new(angle);
+ self
+ }
+
+ /// Sets the initial position (translation and orientation) of the collider to be created,
+ /// relative to the rigid-body it is attached to.
+ pub fn position(mut self, pos: Isometry<f32>) -> Self {
+ self.delta = pos;
+ self
+ }
+
/// Set the position of this collider in the local-space of the rigid-body it is attached to.
+ #[deprecated(note = "Use `.position` instead.")]
pub fn delta(mut self, delta: Isometry<f32>) -> Self {
self.delta = delta;
self
diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs
index 73d4a06..22bba1b 100644
--- a/src/geometry/collider_set.rs
+++ b/src/geometry/collider_set.rs
@@ -47,7 +47,6 @@ impl ColliderSet {
parent_handle: RigidBodyHandle,
bodies: &mut RigidBodySet,
) -> ColliderHandle {
- let mass_properties = coll.mass_properties();
coll.parent = parent_handle;
let parent = bodies
.get_mut_internal(parent_handle)
@@ -55,9 +54,8 @@ impl ColliderSet {
coll.position = parent.position * coll.delta;
coll.predicted_position = parent.predicted_position * coll.delta;
let handle = self.colliders.insert(coll);
- parent.colliders.push(handle);
- parent.mass_properties += mass_properties;
- parent.update_world_mass_properties();
+ let coll = self.colliders.get(handle).unwrap();
+ parent.add_collider_internal(handle, &coll);
bodies.activate(parent_handle);
handle
}
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 6e18a03..8449477 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -7,7 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandl
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
- BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
+ BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet,
+ ContactManifoldIndex, NarrowPhase,
};
use crate::math::Vector;
use crate::pipeline::EventHandler;
@@ -245,6 +246,27 @@ impl PhysicsPipeline {
self.counters.step_completed();
}
+ /// Remove a collider and all its associated data.
+ pub fn remove_collider(
+ &mut self,
+ handle: ColliderHandle,
+ broad_phase: &mut BroadPhase,
+ narrow_phase: &mut NarrowPhase,
+ bodies: &mut RigidBodySet,
+ colliders: &mut ColliderSet,
+ ) -> Option<Collider> {
+ broad_phase.remove_colliders(&[handle], colliders);
+ narrow_phase.remove_colliders(&[handle], colliders, bodies);
+ let collider = colliders.remove_internal(handle)?;
+
+ if let Some(parent) = bodies.get_mut_internal(collider.parent) {
+ parent.remove_collider_internal(handle, &collider);
+ bodies.wake_up(collider.parent);
+ }
+
+ Some(collider)
+ }
+
/// Remove a rigid-body and all its associated data.
pub fn remove_rigid_body(
&mut self,
generated by cgit (git 2.34.1) at 2026-02-04 11:51:23 +0000