Merge pull request #41 from dimforge/cylinder

Add cylinder and cone support + use a trait-object for shapes.

7 files changed, 79 insertions, 25 deletions

diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs
index 22e7da5..d64839c 100644
--- a/src/dynamics/mass_properties.rs
+++ b/src/dynamics/mass_properties.rs
@@ -91,7 +91,7 @@ impl MassProperties {
     }
 
     #[cfg(feature = "dim3")]
-    /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii.
+    /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes.
     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()
@@ -103,7 +103,7 @@ impl MassProperties {
     }
 
     #[cfg(feature = "dim3")]
-    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
+    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes.
     pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
         let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
         self.principal_inertia_local_frame.to_rotation_matrix()
diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs
index 5f08958..3b1b214 100644
--- a/src/dynamics/mass_properties_capsule.rs
+++ b/src/dynamics/mass_properties_capsule.rs
@@ -1,30 +1,9 @@
 use crate::dynamics::MassProperties;
 #[cfg(feature = "dim3")]
 use crate::geometry::Capsule;
-use crate::math::{Point, PrincipalAngularInertia, Vector};
+use crate::math::Point;
 
 impl MassProperties {
-    fn cylinder_y_volume_unit_inertia(
-        half_height: f32,
-        radius: f32,
-    ) -> (f32, PrincipalAngularInertia<f32>) {
-        #[cfg(feature = "dim2")]
-        {
-            Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
-            let sq_radius = radius * radius;
-            let sq_height = half_height * half_height * 4.0;
-            let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
-
-            let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
-            (volume, inertia)
-        }
-    }
-
     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);
diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs
new file mode 100644
index 0000000..12f831f
--- /dev/null
+++ b/src/dynamics/mass_properties_cone.rs
@@ -0,0 +1,29 @@
+use crate::dynamics::MassProperties;
+use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
+
+impl MassProperties {
+    pub(crate) fn cone_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0;
+        let sq_radius = radius * radius;
+        let sq_height = half_height * half_height * 4.0;
+        let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0;
+        let principal = sq_radius * 3.0 / 10.0;
+
+        (volume, Vector::new(off_principal, principal, off_principal))
+    }
+
+    pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self {
+        let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius);
+        let cyl_mass = cyl_vol * density;
+
+        Self::with_principal_inertia_frame(
+            Point::new(0.0, -half_height / 2.0, 0.0),
+            cyl_mass,
+            cyl_unit_i * cyl_mass,
+            Rotation::identity(),
+        )
+    }
+}
diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs
new file mode 100644
index 0000000..7c8054a
--- /dev/null
+++ b/src/dynamics/mass_properties_cylinder.rs
@@ -0,0 +1,40 @@
+use crate::dynamics::MassProperties;
+#[cfg(feature = "dim3")]
+use crate::math::{Point, Rotation};
+use crate::math::{PrincipalAngularInertia, Vector};
+
+impl MassProperties {
+    pub(crate) fn cylinder_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
+            let sq_radius = radius * radius;
+            let sq_height = half_height * half_height * 4.0;
+            let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
+
+            let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
+            (volume, inertia)
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self {
+        let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
+        let cyl_mass = cyl_vol * density;
+
+        Self::with_principal_inertia_frame(
+            Point::origin(),
+            cyl_mass,
+            cyl_unit_i * cyl_mass,
+            Rotation::identity(),
+        )
+    }
+}
diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs
index c87e888..8b0b811 100644
--- a/src/dynamics/mass_properties_polygon.rs
+++ b/src/dynamics/mass_properties_polygon.rs
@@ -1,3 +1,5 @@
+#![allow(dead_code)] // TODO: remove this
+
 use crate::dynamics::MassProperties;
 use crate::math::Point;
 
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 4499d95..10cdd29 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -22,7 +22,10 @@ mod joint;
 mod mass_properties;
 mod mass_properties_ball;
 mod mass_properties_capsule;
+#[cfg(feature = "dim3")]
+mod mass_properties_cone;
 mod mass_properties_cuboid;
+mod mass_properties_cylinder;
 #[cfg(feature = "dim2")]
 mod mass_properties_polygon;
 mod rigid_body;
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index af1fb4a..9fa5a8e 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -218,6 +218,7 @@ impl RigidBody {
         let shift = Translation::from(com.coords);
         shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
     }
+
     pub(crate) fn integrate(&mut self, dt: f32) {
         self.position = self.integrate_velocity(dt) * self.position;
     }
@@ -334,7 +335,7 @@ impl RigidBody {
     }
 }
 
-/// A builder for rigid-bodies.
+/// A builder for rigid-bodies.
 pub struct RigidBodyBuilder {
     position: Isometry<f32>,
     linvel: Vector<f32>,