Redefine capsules as a segment with a radius, allowing us to reuse the pfm_pfm_contact generator for it.

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,
             )
         }
     }