aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/dynamics/mass_properties_cone.rs30
-rw-r--r--src/dynamics/mod.rs2
-rw-r--r--src/geometry/collider.rs29
-rw-r--r--src/geometry/contact_generator/contact_dispatcher.rs9
-rw-r--r--src/geometry/mod.rs3
-rw-r--r--src/geometry/polygonal_feature_map.rs48
-rw-r--r--src/geometry/shape.rs41
7 files changed, 151 insertions, 11 deletions
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")]
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index c6cf6d0..f952b15 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,10 +1,10 @@
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
-#[cfg(feature = "dim3")]
-use crate::geometry::PolygonalFeatureMap;
use crate::geometry::{
- Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph,
- Polygon, Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh,
+ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
+ Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh,
};
+#[cfg(feature = "dim3")]
+use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap};
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use downcast_rs::{impl_downcast, DowncastSync};
use erased_serde::Serialize;
@@ -40,6 +40,13 @@ impl ColliderShape {
ColliderShape(Arc::new(Cylinder::new(half_height, radius)))
}
+ /// Initialize a cone shape defined by its half-height
+ /// (along along the y axis) and its basis radius.
+ #[cfg(feature = "dim3")]
+ pub fn cone(half_height: f32, radius: f32) -> Self {
+ ColliderShape(Arc::new(Cone::new(half_height, radius)))
+ }
+
/// Initialize a cuboid shape defined by its half-extents.
pub fn cuboid(half_extents: Vector<f32>) -> Self {
ColliderShape(Arc::new(Cuboid::new(half_extents)))
@@ -171,6 +178,13 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
.ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
Arc::new(shape) as Arc<dyn Shape>
}
+ #[cfg(feature = "dim3")]
+ Some(ShapeType::Cone) => {
+ let shape: Cone = seq
+ .next_element()?
+ .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
+ Arc::new(shape) as Arc<dyn Shape>
+ }
None => {
return Err(serde::de::Error::custom(
"found invalid shape type to deserialize",
@@ -328,6 +342,13 @@ impl ColliderBuilder {
Self::new(ColliderShape::cylinder(half_height, radius))
}
+ /// Initialize a new collider builder with a cone shape defined by its half-height
+ /// (along along the y axis) and its basis radius.
+ #[cfg(feature = "dim3")]
+ pub fn cone(half_height: f32, radius: f32) -> Self {
+ Self::new(ColliderShape::cone(half_height, radius))
+ }
+
/// Initialize a new collider builder with a cuboid shape defined by its half-extents.
#[cfg(feature = "dim2")]
pub fn cuboid(hx: f32, hy: f32) -> Self {
diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs
index 01bbc46..70ac84c 100644
--- a/src/geometry/contact_generator/contact_dispatcher.rs
+++ b/src/geometry/contact_generator/contact_dispatcher.rs
@@ -76,7 +76,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
| (ShapeType::Capsule, ShapeType::Ball)
| (ShapeType::Ball, ShapeType::Capsule)
| (ShapeType::Cylinder, ShapeType::Ball)
- | (ShapeType::Ball, ShapeType::Cylinder) => (
+ | (ShapeType::Ball, ShapeType::Cylinder)
+ | (ShapeType::Cone, ShapeType::Ball)
+ | (ShapeType::Ball, ShapeType::Cone) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_ball_convex,
..PrimitiveContactGenerator::default()
@@ -99,7 +101,10 @@ impl ContactDispatcher for DefaultContactDispatcher {
None,
)
}
- (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) => (
+ (ShapeType::Cylinder, _)
+ | (_, ShapeType::Cylinder)
+ | (ShapeType::Cone, _)
+ | (_, ShapeType::Cone) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_pfm_pfm,
..PrimitiveContactGenerator::default()
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 71573ed..d569248 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -35,6 +35,9 @@ pub type HeightField = ncollide::shape::HeightField<f32>;
/// A cylindrical shape.
#[cfg(feature = "dim3")]
pub type Cylinder = ncollide::shape::Cylinder<f32>;
+/// A cone shape.
+#[cfg(feature = "dim3")]
+pub type Cone = ncollide::shape::Cone<f32>;
/// An axis-aligned bounding box.
pub type AABB = ncollide::bounding_volume::AABB<f32>;
/// Event triggered when two non-sensor colliders start or stop being in contact.
diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs
index 70be5d0..fc5e066 100644
--- a/src/geometry/polygonal_feature_map.rs
+++ b/src/geometry/polygonal_feature_map.rs
@@ -1,5 +1,5 @@
use crate::geometry::PolyhedronFace;
-use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle};
+use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Triangle};
use crate::math::{Point, Vector};
use approx::AbsDiffEq;
use na::{Unit, Vector2, Vector3};
@@ -85,3 +85,49 @@ impl PolygonalFeatureMap for Cylinder {
}
}
}
+
+impl PolygonalFeatureMap for Cone {
+ fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
+ // About feature ids. It is very similar to the feature ids of cylinders.
+ // At all times, we consider our cone to be approximated as follows:
+ // - The curved part is approximated by a single segment.
+ // - The flat cap of the cone is approximated by a square.
+ // - The curved-part segment has a feature ID of 0, and its endpoint with negative
+ // `y` coordinate has an ID of 1.
+ // - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
+ // when looking at the cap with an eye looking towards +y).
+ // - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
+ // - The bottom cap has its face feature ID of 9.
+ // - Note that at all times, one of the cap's vertices are the same as the curved-part
+ // segment endpoints.
+ let dir2 = Vector2::new(dir.x, dir.z)
+ .try_normalize(f32::default_epsilon())
+ .unwrap_or(Vector2::x());
+
+ if dir.y > 0.0 {
+ // We return a segment lying on the cone's curved part.
+ out_features.vertices[0] = Point::new(
+ dir2.x * self.radius,
+ -self.half_height,
+ dir2.y * self.radius,
+ );
+ out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0);
+ out_features.eids = [0, 0, 0, 0];
+ out_features.fid = 0;
+ out_features.num_vertices = 2;
+ out_features.vids = [1, 11, 11, 11];
+ } else {
+ // We return a square approximation of the cone cap.
+ let y = -self.half_height;
+ out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
+ out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
+ out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
+ out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
+
+ out_features.eids = [2, 4, 6, 8];
+ out_features.fid = 9;
+ out_features.num_vertices = 4;
+ out_features.vids = [1, 3, 5, 7];
+ }
+ }
+}
diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs
index c9e0a3a..b972a3e 100644
--- a/src/geometry/shape.rs
+++ b/src/geometry/shape.rs
@@ -1,10 +1,10 @@
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
-#[cfg(feature = "dim3")]
-use crate::geometry::PolygonalFeatureMap;
use crate::geometry::{
- Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph,
- Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh,
+ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
+ Proximity, Ray, RayIntersection, Triangle, Trimesh,
};
+#[cfg(feature = "dim3")]
+use crate::geometry::{Cone, Cylinder, PolygonalFeatureMap};
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use downcast_rs::{impl_downcast, DowncastSync};
use erased_serde::Serialize;
@@ -35,6 +35,9 @@ pub enum ShapeType {
#[cfg(feature = "dim3")]
/// A cylindrical shape.
Cylinder,
+ #[cfg(feature = "dim3")]
+ /// A cylindrical shape.
+ Cone,
// /// A custom shape type.
// Custom(u8),
}
@@ -104,6 +107,11 @@ impl dyn Shape {
pub fn as_cylinder(&self) -> Option<&Cylinder> {
self.downcast_ref()
}
+
+ /// Converts this abstract shape to a cone, if it is one.
+ pub fn as_cone(&self) -> Option<&Cone> {
+ self.downcast_ref()
+ }
}
impl Shape for Ball {
@@ -273,3 +281,28 @@ impl Shape for Cylinder {
Some(self as &dyn PolygonalFeatureMap)
}
}
+
+#[cfg(feature = "dim3")]
+impl Shape for Cone {
+ #[cfg(feature = "serde-serialize")]
+ fn as_serialize(&self) -> Option<&dyn Serialize> {
+ Some(self as &dyn Serialize)
+ }
+
+ fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
+ self.bounding_volume(position)
+ }
+
+ fn mass_properties(&self, density: f32) -> MassProperties {
+ MassProperties::from_cone(density, self.half_height, self.radius)
+ }
+
+ fn shape_type(&self) -> ShapeType {
+ ShapeType::Cone
+ }
+
+ #[cfg(feature = "dim3")]
+ fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
+ Some(self as &dyn PolygonalFeatureMap)
+ }
+}
generated by cgit (git 2.34.1) at 2026-02-04 10:19:47 +0000