diff options
Diffstat (limited to 'src/dynamics')
| -rw-r--r-- | src/dynamics/mass_properties_capsule.rs | 13 |
1 files changed, 8 insertions, 5 deletions
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<f32>, b: Point<f32>, 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, ) } } |