From 2b628f9580a826722346983ef42672d4e8dd8053 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 26 Oct 2020 15:58:30 +0100 Subject: Redefine capsules as a segment with a radius, allowing us to reuse the pfm_pfm_contact generator for it. --- src/dynamics/mass_properties_capsule.rs | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs index cfd4345..dae6cfb 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -1,22 +1,24 @@ use crate::dynamics::MassProperties; use crate::math::Point; #[cfg(feature = "dim3")] -use crate::math::Rotation; +use crate::{geometry::Capsule, math::Rotation}; impl MassProperties { - pub(crate) fn from_capsule(density: f32, half_height: f32, radius: f32) -> Self { + pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { + let half_height = (b - a).norm() / 2.0; let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); let cap_vol = cyl_vol + ball_vol; let cap_mass = cap_vol * density; let mut cap_unit_i = cyl_unit_i + ball_unit_i; + let local_com = na::center(&a, &b); #[cfg(feature = "dim2")] { let h = half_height * 2.0; let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; cap_unit_i += extra; - Self::new(Point::origin(), cap_mass, cap_unit_i * cap_mass) + Self::new(local_com, cap_mass, cap_unit_i * cap_mass) } #[cfg(feature = "dim3")] @@ -25,11 +27,12 @@ impl MassProperties { let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; cap_unit_i.x += extra; cap_unit_i.z += extra; + let local_frame = Capsule::new(a, b, radius).rotation_wrt_y(); Self::with_principal_inertia_frame( - Point::origin(), + local_com, cap_mass, cap_unit_i * cap_mass, - Rotation::identity(), + local_frame, ) } } -- cgit