From d513c22d33ab44b0048355bcfd1db4173b3f7ece Mon Sep 17 00:00:00 2001
From: Crozet Sébastien <developer@crozet.re>
Date: Tue, 20 Oct 2020 14:16:01 +0200
Subject: Add cone support.

---
 src/dynamics/mass_properties_cone.rs | 30 ++++++++++++++++++++++++++++++
 src/dynamics/mod.rs                  |  2 ++
 2 files changed, 32 insertions(+)
 create mode 100644 src/dynamics/mass_properties_cone.rs

(limited to 'src/dynamics')

diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs
new file mode 100644
index 0000000..0fb61b6
--- /dev/null
+++ b/src/dynamics/mass_properties_cone.rs
@@ -0,0 +1,30 @@
+use crate::dynamics::MassProperties;
+use crate::geometry::Cone;
+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/mod.rs b/src/dynamics/mod.rs
index 512bd8b..10cdd29 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -22,6 +22,8 @@ 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")]
-- 
cgit