From 865ce8a8e5301b23ca474adaaffe8b43e725803e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crozet=20S=C3=A9bastien?= Date: Tue, 20 Oct 2020 11:55:33 +0200 Subject: [PATCH] Collider shape: use a trait-object instead of an enum. --- build/rapier2d/Cargo.toml | 5 +- build/rapier3d/Cargo.toml | 6 +- src/dynamics/mass_properties.rs | 4 +- src/dynamics/mass_properties_capsule.rs | 11 +- src/geometry/broad_phase_multi_sap.rs | 16 +- src/geometry/collider.rs | 395 +++++++++--------- .../ball_convex_contact_generator.rs | 23 +- .../capsule_capsule_contact_generator.rs | 15 +- .../contact_generator/contact_dispatcher.rs | 78 ++-- .../cuboid_capsule_contact_generator.rs | 8 +- .../cuboid_cuboid_contact_generator.rs | 2 +- .../cuboid_triangle_contact_generator.rs | 6 +- .../heightfield_shape_contact_generator.rs | 35 +- src/geometry/contact_generator/mod.rs | 2 +- .../polygon_polygon_contact_generator.rs | 29 +- .../trimesh_shape_contact_generator.rs | 11 +- src/geometry/mod.rs | 12 +- src/geometry/narrow_phase.rs | 12 +- .../ball_convex_proximity_detector.rs | 18 +- .../cuboid_cuboid_proximity_detector.rs | 2 +- .../cuboid_triangle_proximity_detector.rs | 6 +- .../polygon_polygon_proximity_detector.rs | 23 +- .../proximity_dispatcher.rs | 40 +- .../trimesh_shape_proximity_detector.rs | 11 +- src/geometry/rounded.rs | 7 + src/geometry/sat.rs | 20 +- src/geometry/shape.rs | 275 ++++++++++++ src/geometry/trimesh.rs | 45 +- src/pipeline/query_pipeline.rs | 12 +- src/utils.rs | 26 ++ src_testbed/engine.rs | 65 +-- 31 files changed, 782 insertions(+), 438 deletions(-) create mode 100644 src/geometry/rounded.rs create mode 100644 src/geometry/shape.rs diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index 03ccd98..44038e6 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -22,7 +22,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -46,7 +46,10 @@ arrayvec = "0.5" bit-vec = "0.6" rustc-hash = "1" serde = { version = "1", features = [ "derive" ], optional = true } +erased-serde = { version = "0.3", optional = true } indexmap = { version = "1", features = [ "serde-1" ], optional = true } +downcast-rs = "1.2" +num-derive = "0.3" [dev-dependencies] bincode = "1" diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index 7a0139e..adc453c 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -22,7 +22,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] enhanced-determinism = [ "simba/libm_force", "indexmap" ] [lib] @@ -46,7 +46,11 @@ arrayvec = "0.5" bit-vec = "0.6" rustc-hash = "1" serde = { version = "1", features = [ "derive" ], optional = true } +erased-serde = { version = "0.3", optional = true } indexmap = { version = "1", features = [ "serde-1" ], optional = true } +downcast-rs = "1.2" +num-derive = "0.3" + [dev-dependencies] bincode = "1" 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 { 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 { 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 647cfc7..c4e039c 100644 --- a/src/dynamics/mass_properties_capsule.rs +++ b/src/dynamics/mass_properties_capsule.rs @@ -4,21 +4,19 @@ use crate::geometry::Capsule; use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector}; impl MassProperties { - pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { - let half_height = (b - a).norm() / 2.0; + pub(crate) fn from_capsule(density: f32, half_height: f32, radius: f32) -> Self { 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(local_com, cap_mass, cap_unit_i * cap_mass) + Self::new(Point::origin(), cap_mass, cap_unit_i * cap_mass) } #[cfg(feature = "dim3")] @@ -27,12 +25,11 @@ 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( - local_com, + Point::origin(), cap_mass, cap_unit_i * cap_mass, - local_frame, + Rotation::identity(), ) } } diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 8e69d24..3562c2e 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -335,7 +335,7 @@ impl SAPAxis { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] struct SAPRegion { - axii: [SAPAxis; DIM], + axes: [SAPAxis; DIM], existing_proxies: BitVec, #[cfg_attr(feature = "serde-serialize", serde(skip))] to_insert: Vec, // Workspace @@ -344,14 +344,14 @@ struct SAPRegion { impl SAPRegion { pub fn new(bounds: AABB) -> Self { - let axii = [ + let axes = [ SAPAxis::new(bounds.mins.x, bounds.maxs.x), SAPAxis::new(bounds.mins.y, bounds.maxs.y), #[cfg(feature = "dim3")] SAPAxis::new(bounds.mins.z, bounds.maxs.z), ]; SAPRegion { - axii, + axes, existing_proxies: BitVec::new(), to_insert: Vec::new(), need_update: false, @@ -386,15 +386,15 @@ impl SAPRegion { // Update endpoints. let mut deleted_any = false; for dim in 0..DIM { - self.axii[dim].update_endpoints(dim, proxies, reporting); - deleted_any = self.axii[dim] + self.axes[dim].update_endpoints(dim, proxies, reporting); + deleted_any = self.axes[dim] .delete_out_of_bounds_proxies(&mut self.existing_proxies) || deleted_any; } if deleted_any { for dim in 0..DIM { - self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); + self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); } } @@ -404,9 +404,9 @@ impl SAPRegion { if !self.to_insert.is_empty() { // Insert new proxies. for dim in 1..DIM { - self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None); + self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None); } - self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); + self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); self.to_insert.clear(); } } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index fe42bd7..c6cf6d0 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -3,172 +3,186 @@ use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; use crate::geometry::PolygonalFeatureMap; use crate::geometry::{ Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph, - Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh, + Polygon, Proximity, Ray, RayIntersection, Shape, ShapeType, Triangle, Trimesh, }; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; +use downcast_rs::{impl_downcast, DowncastSync}; +use erased_serde::Serialize; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; use ncollide::query::RayCast; use num::Zero; +use std::any::Any; +use std::ops::Deref; +use std::sync::Arc; +/// The shape of a collider. #[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// An enum grouping all the possible shape of a collider. -pub enum Shape { - /// A ball shape. - Ball(Ball), - /// A convex polygon shape. - Polygon(Polygon), - /// A cuboid shape. - Cuboid(Cuboid), - /// A capsule shape. - Capsule(Capsule), - /// A triangle shape. - Triangle(Triangle), - /// A triangle mesh shape. - Trimesh(Trimesh), - /// A heightfield shape. - HeightField(HeightField), - #[cfg(feature = "dim3")] - /// A cylindrical shape. - Cylinder(Cylinder), +pub struct ColliderShape(pub Arc); + +impl Deref for ColliderShape { + type Target = Shape; + fn deref(&self) -> &Shape { + &*self.0 + } } -impl Shape { - /// Gets a reference to the underlying ball shape, if `self` is one. - pub fn as_ball(&self) -> Option<&Ball> { - match self { - Shape::Ball(b) => Some(b), - _ => None, - } +impl ColliderShape { + /// Initialize a ball shape defined by its radius. + pub fn ball(radius: f32) -> Self { + ColliderShape(Arc::new(Ball::new(radius))) } - /// Gets a reference to the underlying polygon shape, if `self` is one. - pub fn as_polygon(&self) -> Option<&Polygon> { - match self { - Shape::Polygon(p) => Some(p), - _ => None, - } - } - - /// Gets a reference to the underlying cuboid shape, if `self` is one. - pub fn as_cuboid(&self) -> Option<&Cuboid> { - match self { - Shape::Cuboid(c) => Some(c), - _ => None, - } - } - - /// Gets a reference to the underlying capsule shape, if `self` is one. - pub fn as_capsule(&self) -> Option<&Capsule> { - match self { - Shape::Capsule(c) => Some(c), - _ => None, - } - } - - /// Gets a reference to the underlying triangle mesh shape, if `self` is one. - pub fn as_trimesh(&self) -> Option<&Trimesh> { - match self { - Shape::Trimesh(c) => Some(c), - _ => None, - } - } - - /// Gets a reference to the underlying heightfield shape, if `self` is one. - pub fn as_heightfield(&self) -> Option<&HeightField> { - match self { - Shape::HeightField(h) => Some(h), - _ => None, - } - } - - /// Gets a reference to the underlying triangle shape, if `self` is one. - pub fn as_triangle(&self) -> Option<&Triangle> { - match self { - Shape::Triangle(c) => Some(c), - _ => None, - } - } - - /// Gets a reference to the underlying cylindrical shape, if `self` is one. - pub fn as_cylinder(&self) -> Option<&Cylinder> { - match self { - Shape::Cylinder(c) => Some(c), - _ => None, - } - } - - /// gets a reference to this shape seen as a PolygonalFeatureMap. + /// Initialize a cylindrical shape defined by its half-height + /// (along along the y axis) and its radius. #[cfg(feature = "dim3")] - pub fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { - match self { - Shape::Triangle(t) => Some(t), - Shape::Cuboid(c) => Some(c), - Shape::Cylinder(c) => Some(c), - _ => None, - } + pub fn cylinder(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } - /// Computes the axis-aligned bounding box of this shape. - pub fn compute_aabb(&self, position: &Isometry) -> AABB { - match self { - Shape::Ball(ball) => ball.bounding_volume(position), - Shape::Polygon(poly) => poly.aabb(position), - Shape::Capsule(caps) => caps.aabb(position), - Shape::Cuboid(cuboid) => cuboid.bounding_volume(position), - Shape::Triangle(triangle) => triangle.bounding_volume(position), - Shape::Trimesh(trimesh) => trimesh.aabb(position), - Shape::HeightField(heightfield) => heightfield.bounding_volume(position), - Shape::Cylinder(cylinder) => cylinder.bounding_volume(position), - } + /// Initialize a cuboid shape defined by its half-extents. + pub fn cuboid(half_extents: Vector) -> Self { + ColliderShape(Arc::new(Cuboid::new(half_extents))) } - /// Computes the first intersection point between a ray in this collider. - /// - /// Some shapes are not supported yet and will always return `None`. - /// - /// # Parameters - /// - `position`: the position of this shape. - /// - `ray`: the ray to cast. - /// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively - /// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray. - pub fn cast_ray( - &self, - position: &Isometry, - ray: &Ray, - max_toi: f32, - ) -> Option { - match self { - Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true), - Shape::Polygon(_poly) => None, - Shape::Capsule(caps) => { - let pos = position * caps.transform_wrt_y(); - let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius); - caps.toi_and_normal_with_ray(&pos, ray, max_toi, true) + /// Initialize a capsule shape aligned with the `y` axis. + pub fn capsule(half_height: f32, radius: f32) -> Self { + ColliderShape(Arc::new(Capsule::new(half_height, radius))) + } + + /// Initializes a triangle shape. + pub fn triangle(a: Point, b: Point, c: Point) -> Self { + ColliderShape(Arc::new(Triangle::new(a, b, c))) + } + + /// Initializes a triangle mesh shape defined by its vertex and index buffers. + pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { + ColliderShape(Arc::new(Trimesh::new(vertices, indices))) + } + + /// Initializes an heightfield shape defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim2")] + pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) + } + + /// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim3")] + pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) + } +} + +#[cfg(feature = "serde-serialize")] +impl serde::Serialize for ColliderShape { + fn serialize(&self, serializer: S) -> Result + where + S: serde::Serializer, + { + use crate::serde::ser::SerializeStruct; + + if let Some(ser) = self.0.as_serialize() { + let typ = self.0.shape_type(); + let mut state = serializer.serialize_struct("ColliderShape", 2)?; + state.serialize_field("tag", &(typ as i32))?; + state.serialize_field("inner", ser)?; + state.end() + } else { + Err(serde::ser::Error::custom( + "Found a non-serializable custom shape.", + )) + } + } +} + +#[cfg(feature = "serde-serialize")] +impl<'de> serde::Deserialize<'de> for ColliderShape { + fn deserialize(deserializer: D) -> Result + where + D: serde::Deserializer<'de>, + { + struct Visitor {}; + impl<'de> serde::de::Visitor<'de> for Visitor { + type Value = ColliderShape; + fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { + write!(formatter, "one shape type tag and the inner shape data") } - Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true), - #[cfg(feature = "dim2")] - Shape::Triangle(_) | Shape::Trimesh(_) => { - // This is not implemented yet in 2D. - None - } - #[cfg(feature = "dim3")] - Shape::Triangle(triangle) => { - triangle.toi_and_normal_with_ray(position, ray, max_toi, true) - } - #[cfg(feature = "dim3")] - Shape::Trimesh(trimesh) => { - trimesh.toi_and_normal_with_ray(position, ray, max_toi, true) - } - Shape::HeightField(heightfield) => { - heightfield.toi_and_normal_with_ray(position, ray, max_toi, true) - } - #[cfg(feature = "dim3")] - Shape::Cylinder(cylinder) => { - cylinder.toi_and_normal_with_ray(position, ray, max_toi, true) + + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + use num::cast::FromPrimitive; + + let tag: i32 = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + + let shape = match ShapeType::from_i32(tag) { + Some(ShapeType::Ball) => { + let shape: Ball = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::Polygon) => { + unimplemented!() + // let shape: Polygon = seq + // .next_element()? + // .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + // Arc::new(shape) as Arc + } + Some(ShapeType::Cuboid) => { + let shape: Cuboid = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::Capsule) => { + let shape: Capsule = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::Triangle) => { + let shape: Triangle = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::Trimesh) => { + let shape: Trimesh = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + Some(ShapeType::HeightField) => { + let shape: HeightField = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + #[cfg(feature = "dim3")] + Some(ShapeType::Cylinder) => { + let shape: Cylinder = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + Arc::new(shape) as Arc + } + None => { + return Err(serde::de::Error::custom( + "found invalid shape type to deserialize", + )) + } + }; + + Ok(ColliderShape(shape)) } } + + deserializer.deserialize_struct("ColliderShape", &["tag", "inner"], Visitor {}) } } @@ -177,7 +191,7 @@ impl Shape { /// /// To build a new collider, use the `ColliderBuilder` structure. pub struct Collider { - shape: Shape, + shape: ColliderShape, density: f32, is_sensor: bool, pub(crate) parent: RigidBodyHandle, @@ -245,7 +259,7 @@ impl Collider { /// The geometric shape of this collider. pub fn shape(&self) -> &Shape { - &self.shape + &*self.shape.0 } /// Compute the axis-aligned bounding box of this collider. @@ -261,23 +275,7 @@ impl Collider { /// Compute the local-space mass properties of this collider. pub fn mass_properties(&self) -> MassProperties { - match &self.shape { - Shape::Ball(ball) => MassProperties::from_ball(self.density, ball.radius), - #[cfg(feature = "dim2")] - Shape::Polygon(p) => MassProperties::from_polygon(self.density, p.vertices()), - #[cfg(feature = "dim3")] - Shape::Polygon(_p) => unimplemented!(), - Shape::Cuboid(c) => MassProperties::from_cuboid(self.density, c.half_extents), - Shape::Capsule(caps) => { - MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius) - } - Shape::Triangle(_) | Shape::Trimesh(_) | Shape::HeightField(_) => { - MassProperties::zero() - } - Shape::Cylinder(c) => { - MassProperties::from_cylinder(self.density, c.half_height, c.radius) - } - } + self.shape.mass_properties(self.density) } } @@ -286,7 +284,7 @@ impl Collider { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct ColliderBuilder { /// The shape of the collider to be built. - pub shape: Shape, + pub shape: ColliderShape, /// The density of the collider to be built. density: Option, /// The friction coefficient of the collider to be built. @@ -301,7 +299,7 @@ pub struct ColliderBuilder { impl ColliderBuilder { /// Initialize a new collider builder with the given shape. - pub fn new(shape: Shape) -> Self { + pub fn new(shape: ColliderShape) -> Self { Self { shape, density: None, @@ -320,102 +318,81 @@ impl ColliderBuilder { /// Initialize a new collider builder with a ball shape defined by its radius. pub fn ball(radius: f32) -> Self { - Self::new(Shape::Ball(Ball::new(radius))) + Self::new(ColliderShape::ball(radius)) } /// Initialize a new collider builder with a cylindrical shape defined by its half-height /// (along along the y axis) and its radius. + #[cfg(feature = "dim3")] pub fn cylinder(half_height: f32, radius: f32) -> Self { - Self::new(Shape::Cylinder(Cylinder::new(half_height, radius))) + Self::new(ColliderShape::cylinder(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 { - let cuboid = Cuboid { - half_extents: Vector::new(hx, hy), - }; - - Self::new(Shape::Cuboid(cuboid)) - - /* - use crate::math::Point; - let vertices = vec![ - Point::new(hx, -hy), - Point::new(hx, hy), - Point::new(-hx, hy), - Point::new(-hx, -hy), - ]; - let normals = vec![Vector::x(), Vector::y(), -Vector::x(), -Vector::y()]; - let polygon = Polygon::new(vertices, normals); - - Self::new(Shape::Polygon(polygon)) - */ + Self::new(ColliderShape::cuboid(Vector::new(hx, hy))) } /// Initialize a new collider builder with a capsule shape aligned with the `x` axis. pub fn capsule_x(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_x(half_height, radius); - Self::new(Shape::Capsule(capsule)) + #[cfg(feature = "dim2")] + let rot = -std::f32::consts::FRAC_PI_2; + #[cfg(feature = "dim3")] + let rot = Vector::z() * -std::f32::consts::FRAC_PI_2; + Self::new(ColliderShape::capsule(half_height, radius)) + .position(Isometry::new(na::zero(), rot)) } /// Initialize a new collider builder with a capsule shape aligned with the `y` axis. pub fn capsule_y(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_y(half_height, radius); - Self::new(Shape::Capsule(capsule)) + Self::new(ColliderShape::capsule(half_height, radius)) } /// Initialize a new collider builder with a capsule shape aligned with the `z` axis. #[cfg(feature = "dim3")] pub fn capsule_z(half_height: f32, radius: f32) -> Self { - let capsule = Capsule::new_z(half_height, radius); - Self::new(Shape::Capsule(capsule)) + let rot = Vector::x() * std::f32::consts::FRAC_PI_2; + Self::new(ColliderShape::capsule(half_height, radius)) + .position(Isometry::new(na::zero(), rot)) } /// Initialize a new collider builder with a cuboid shape defined by its half-extents. #[cfg(feature = "dim3")] pub fn cuboid(hx: f32, hy: f32, hz: f32) -> Self { - let cuboid = Cuboid { - half_extents: Vector::new(hx, hy, hz), - }; - - Self::new(Shape::Cuboid(cuboid)) + Self::new(ColliderShape::cuboid(Vector::new(hx, hy, hz))) } /// Initializes a collider builder with a segment shape. /// /// A segment shape is modeled by a capsule with a 0 radius. pub fn segment(a: Point, b: Point) -> Self { - let capsule = Capsule::new(a, b, 0.0); - Self::new(Shape::Capsule(capsule)) + let (pos, half_height) = crate::utils::segment_to_capsule(&a, &b); + Self::new(ColliderShape::capsule(half_height, 0.0)).position(pos) } /// Initializes a collider builder with a triangle shape. pub fn triangle(a: Point, b: Point, c: Point) -> Self { - let triangle = Triangle::new(a, b, c); - Self::new(Shape::Triangle(triangle)) + Self::new(ColliderShape::triangle(a, b, c)) } /// Initializes a collider builder with a triangle mesh shape defined by its vertex and index buffers. pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { - let trimesh = Trimesh::new(vertices, indices); - Self::new(Shape::Trimesh(trimesh)) + Self::new(ColliderShape::trimesh(vertices, indices)) } /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim2")] pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { - let heightfield = HeightField::new(heights, scale); - Self::new(Shape::HeightField(heightfield)) + Self::new(ColliderShape::heightfield(heights, scale)) } /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale /// factor along each coordinate axis. #[cfg(feature = "dim3")] pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { - let heightfield = HeightField::new(heights, scale); - Self::new(Shape::HeightField(heightfield)) + Self::new(ColliderShape::heightfield(heights, scale)) } /// The default friction coefficient used by the collider builder. diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs index 0856029..62ebfab 100644 --- a/src/geometry/contact_generator/ball_convex_contact_generator.rs +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -5,30 +5,17 @@ use na::Unit; use ncollide::query::PointQuery; pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { - if let Shape::Ball(ball1) = ctxt.shape1 { + if let Some(ball1) = ctxt.shape1.as_ball() { ctxt.manifold.swap_identifiers(); - - match ctxt.shape2 { - Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true), - Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true), - Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true), - Shape::Cylinder(cylinder2) => do_generate_contacts(cylinder2, ball1, ctxt, true), - _ => unimplemented!(), - } - } else if let Shape::Ball(ball2) = ctxt.shape2 { - match ctxt.shape1 { - Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false), - Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false), - Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false), - Shape::Cylinder(cylinder1) => do_generate_contacts(cylinder1, ball2, ctxt, false), - _ => unimplemented!(), - } + do_generate_contacts(ctxt.shape2, ball1, ctxt, true); + } else if let Some(ball2) = ctxt.shape2.as_ball() { + do_generate_contacts(ctxt.shape1, ball2, ctxt, false); } ctxt.manifold.sort_contacts(ctxt.prediction_distance); } -fn do_generate_contacts>( +fn do_generate_contacts>( point_query1: &P, ball2: &Ball, ctxt: &mut PrimitiveContactGenerationContext, diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs index 3800ce6..4d9bbc7 100644 --- a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -8,7 +8,7 @@ use na::Unit; use ncollide::shape::{Segment, SegmentPointLocation}; pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Capsule(capsule1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) { generate_contacts( ctxt.prediction_distance, capsule1, @@ -94,7 +94,7 @@ pub fn generate_contacts<'a>( if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8 && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8 { - // Capsules axii are almost parallel and are almost perpendicular to the normal. + // Capsules axes are almost parallel and are almost perpendicular to the normal. // Find a second contact point. if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal( (capsule1.a, capsule1.b), @@ -156,17 +156,18 @@ pub fn generate_contacts<'a>( let pos12 = pos1.inverse() * pos2; let pos21 = pos12.inverse(); - let capsule2_1 = capsule1.transform_by(&pos12); + let seg1 = capsule1.segment(); + let seg2_1 = capsule2.segment().transformed(&pos12); let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( - (&capsule1.a, &capsule1.b), - (&capsule2_1.a, &capsule2_1.b), + (&seg1.a, &seg1.b), + (&seg2_1.a, &seg2_1.b), ); { let bcoords1 = loc1.barycentric_coordinates(); let bcoords2 = loc2.barycentric_coordinates(); - let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1]; - let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1]; + let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1]; + let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1]; let local_n1 = Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis()); diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs index e925fd5..01bbc46 100644 --- a/src/geometry/contact_generator/contact_dispatcher.rs +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -3,7 +3,7 @@ use crate::geometry::contact_generator::{ PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace, }; -use crate::geometry::Shape; +use crate::geometry::{Shape, ShapeType}; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm @@ -12,8 +12,8 @@ pub trait ContactDispatcher { /// Select the collision-detection algorithm for the given pair of primitive shapes. fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveContactGenerator, Option>, @@ -21,8 +21,8 @@ pub trait ContactDispatcher { /// Select the collision-detection algorithm for the given pair of non-primitive shapes. fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ContactPhase, Option>); } @@ -32,14 +32,14 @@ pub struct DefaultContactDispatcher; impl ContactDispatcher for DefaultContactDispatcher { fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveContactGenerator, Option>, ) { match (shape1, shape2) { - (Shape::Ball(_), Shape::Ball(_)) => ( + (ShapeType::Ball, ShapeType::Ball) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_ball, #[cfg(feature = "simd-is-enabled")] @@ -48,56 +48,58 @@ impl ContactDispatcher for DefaultContactDispatcher { }, None, ), - (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + (ShapeType::Cuboid, ShapeType::Cuboid) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_cuboid_cuboid, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Polygon(_), Shape::Polygon(_)) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_polygon_polygon, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (Shape::Capsule(_), Shape::Capsule(_)) => ( + // (ShapeType::Polygon, ShapeType::Polygon) => ( + // PrimitiveContactGenerator { + // generate_contacts: super::generate_contacts_polygon_polygon, + // ..PrimitiveContactGenerator::default() + // }, + // None, + // ), + (ShapeType::Capsule, ShapeType::Capsule) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_capsule_capsule, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Cuboid(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Cuboid(_)) - | (Shape::Triangle(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Triangle(_)) - | (Shape::Capsule(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Capsule(_)) - | (Shape::Cylinder(_), Shape::Ball(_)) - | (Shape::Ball(_), Shape::Cylinder(_)) => ( + (ShapeType::Cuboid, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Cuboid) + | (ShapeType::Triangle, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Triangle) + | (ShapeType::Capsule, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Capsule) + | (ShapeType::Cylinder, ShapeType::Ball) + | (ShapeType::Ball, ShapeType::Cylinder) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_ball_convex, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => ( + (ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_cuboid_capsule, ..PrimitiveContactGenerator::default() }, None, ), - (Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => ( - PrimitiveContactGenerator { - generate_contacts: super::generate_contacts_cuboid_triangle, - ..PrimitiveContactGenerator::default() - }, - None, - ), - (Shape::Cylinder(_), _) | (_, Shape::Cylinder(_)) => ( + (ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => { + ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_cuboid_triangle, + ..PrimitiveContactGenerator::default() + }, + None, + ) + } + (ShapeType::Cylinder, _) | (_, ShapeType::Cylinder) => ( PrimitiveContactGenerator { generate_contacts: super::generate_contacts_pfm_pfm, ..PrimitiveContactGenerator::default() @@ -110,18 +112,18 @@ impl ContactDispatcher for DefaultContactDispatcher { fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ContactPhase, Option>) { match (shape1, shape2) { - (Shape::Trimesh(_), _) | (_, Shape::Trimesh(_)) => ( + (ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_trimesh_shape, ..ContactGenerator::default() }), Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())), ), - (Shape::HeightField(_), _) | (_, Shape::HeightField(_)) => ( + (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_heightfield_shape, ..ContactGenerator::default() diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs index a7857a1..c3cf588 100644 --- a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -9,7 +9,7 @@ use crate::math::Vector; use ncollide::shape::Segment; pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) { generate_contacts( ctxt.prediction_distance, cube1, @@ -20,7 +20,9 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon false, ); ctxt.manifold.update_warmstart_multiplier(); - } else if let (Shape::Capsule(capsule1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(capsule1), Some(cube2)) = + (ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid()) + { generate_contacts( ctxt.prediction_distance, cube2, @@ -53,7 +55,7 @@ pub fn generate_contacts<'a>( return; } - let segment2 = Segment::new(capsule2.a, capsule2.b); + let segment2 = capsule2.segment(); /* * diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index d879a22..596cfb1 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -6,7 +6,7 @@ use crate::math::Vector; use ncollide::shape::Cuboid; pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { generate_contacts( ctxt.prediction_distance, cube1, diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs index 1a0358d..6744c0e 100644 --- a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -10,7 +10,7 @@ use crate::{ }; pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { generate_contacts( ctxt.prediction_distance, cube1, @@ -21,7 +21,9 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo false, ); ctxt.manifold.update_warmstart_multiplier(); - } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(triangle1), Some(cube2)) = + (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) + { generate_contacts( ctxt.prediction_distance, cube2, diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 04afc65..f507caf 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -3,7 +3,7 @@ use crate::geometry::contact_generator::{ }; #[cfg(feature = "dim2")] use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, HeightField, Shape}; +use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType}; use crate::ncollide::bounding_volume::BoundingVolume; #[cfg(feature = "dim3")] use crate::{geometry::Triangle, math::Point}; @@ -38,9 +38,9 @@ pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext) let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::HeightField(heightfield1) = collider1.shape() { + if let Some(heightfield1) = collider1.shape().as_heightfield() { do_generate_contacts(heightfield1, collider1, collider2, ctxt, false) - } else if let Shape::HeightField(heightfield2) = collider2.shape() { + } else if let Some(heightfield2) = collider2.shape().as_heightfield() { do_generate_contacts(heightfield2, collider2, collider1, ctxt, true) } } @@ -59,6 +59,7 @@ fn do_generate_contacts( .expect("The HeightFieldShapeContactGeneratorWorkspace is missing.") .downcast_mut() .expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace."); + let shape_type2 = collider2.shape().shape_type(); /* * Detect if the detector context has been reset. @@ -76,19 +77,9 @@ fn do_generate_contacts( // subshape_id, manifold.subshape_index_pair // ); - // Use dummy shapes for the dispatch. - #[cfg(feature = "dim2")] - let sub_shape1 = - Shape::Capsule(Capsule::new(na::Point::origin(), na::Point::origin(), 0.0)); - #[cfg(feature = "dim3")] - let sub_shape1 = Shape::Triangle(Triangle::new( - Point::origin(), - Point::origin(), - Point::origin(), - )); let (generator, workspace2) = ctxt .dispatcher - .dispatch_primitives(&sub_shape1, collider2.shape()); + .dispatch_primitives(ShapeType::Capsule, shape_type2); let sub_detector = SubDetector { generator, @@ -120,12 +111,18 @@ fn do_generate_contacts( let manifolds = &mut ctxt.pair.manifolds; let prediction_distance = ctxt.prediction_distance; let dispatcher = ctxt.dispatcher; + let shape_type2 = collider2.shape().shape_type(); heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| { + let position1 = *collider1.position(); #[cfg(feature = "dim2")] - let sub_shape1 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0)); + let (position1, sub_shape1) = { + let (dpos, height) = crate::utils::segment_to_capsule(&part1.a, &part1.b); + (position1 * dpos, Capsule::new(height, 0.0)); + }; #[cfg(feature = "dim3")] - let sub_shape1 = Shape::Triangle(*part1); + let sub_shape1 = *part1; + let sub_detector = match workspace.sub_detectors.entry(i) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); @@ -137,7 +134,7 @@ fn do_generate_contacts( } Entry::Vacant(entry) => { let (generator, workspace2) = - dispatcher.dispatch_primitives(&sub_shape1, collider2.shape()); + dispatcher.dispatch_primitives(ShapeType::Triangle, shape_type2); let sub_detector = SubDetector { generator, manifold_id: manifolds.len(), @@ -162,7 +159,7 @@ fn do_generate_contacts( shape1: collider2.shape(), shape2: &sub_shape1, position1: collider2.position(), - position2: collider1.position(), + position2: &position1, manifold, workspace: sub_detector.workspace.as_deref_mut(), } @@ -173,7 +170,7 @@ fn do_generate_contacts( collider2, shape1: &sub_shape1, shape2: collider2.shape(), - position1: collider1.position(), + position1: &position1, position2: collider2.position(), manifold, workspace: sub_detector.workspace.as_deref_mut(), diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index d8a523f..0549420 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -22,7 +22,7 @@ pub use self::heightfield_shape_contact_generator::{ pub use self::pfm_pfm_contact_generator::{ generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace, }; -pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; +// pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; pub use self::trimesh_shape_contact_generator::{ generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, }; diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs index 33b54e4..c150e83 100644 --- a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -5,20 +5,21 @@ use crate::math::{Isometry, Point}; use crate::{math::Vector, utils}; pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) { - if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { - generate_contacts( - polygon1, - &ctxt.position1, - polygon2, - &ctxt.position2, - ctxt.manifold, - ); - ctxt.manifold.update_warmstart_multiplier(); - } else { - unreachable!() - } - - ctxt.manifold.sort_contacts(ctxt.prediction_distance); + unimplemented!() + // if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { + // generate_contacts( + // polygon1, + // &ctxt.position1, + // polygon2, + // &ctxt.position2, + // ctxt.manifold, + // ); + // ctxt.manifold.update_warmstart_multiplier(); + // } else { + // unreachable!() + // } + // + // ctxt.manifold.sort_contacts(ctxt.prediction_distance); } fn generate_contacts<'a>( diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 52ba9b7..49e9b40 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -1,7 +1,7 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, Shape, Trimesh}; +use crate::geometry::{Collider, ContactManifold, Shape, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeContactGeneratorWorkspace { @@ -26,9 +26,9 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) { let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::Trimesh(trimesh1) = collider1.shape() { + if let Some(trimesh1) = collider1.shape().as_trimesh() { do_generate_contacts(trimesh1, collider1, collider2, ctxt, false) - } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + } else if let Some(trimesh2) = collider2.shape().as_trimesh() { do_generate_contacts(trimesh2, collider2, collider1, ctxt, true) } } @@ -121,6 +121,7 @@ fn do_generate_contacts( let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut old_manifolds_it = workspace.old_manifolds.drain(..); + let shape_type2 = collider2.shape().shape_type(); for (i, triangle_id) in new_interferences.iter().enumerate() { if *triangle_id >= trimesh1.num_triangles() { @@ -159,10 +160,10 @@ fn do_generate_contacts( } let manifold = &mut ctxt.pair.manifolds[i]; - let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let triangle1 = trimesh1.triangle(*triangle_id); let (generator, mut workspace2) = ctxt .dispatcher - .dispatch_primitives(&triangle1, collider2.shape()); + .dispatch_primitives(ShapeType::Triangle, shape_type2); let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 { PrimitiveContactGenerationContext { diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index efbb35c..71573ed 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -1,8 +1,7 @@ //! Structures related to geometry: colliders, shapes, etc. pub use self::broad_phase_multi_sap::BroadPhase; -pub use self::capsule::Capsule; -pub use self::collider::{Collider, ColliderBuilder, Shape}; +pub use self::collider::{Collider, ColliderBuilder, ColliderShape}; pub use self::collider_set::{ColliderHandle, ColliderSet}; pub use self::contact::{ Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, @@ -19,9 +18,12 @@ pub use self::narrow_phase::NarrowPhase; pub use self::polygon::Polygon; pub use self::proximity::ProximityPair; pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; +pub use self::rounded::Rounded; pub use self::trimesh::Trimesh; pub use ncollide::query::Proximity; +/// A capsule shape. +pub type Capsule = ncollide::shape::Capsule; /// A cuboid shape. pub type Cuboid = ncollide::shape::Cuboid; /// A triangle shape. @@ -43,6 +45,8 @@ pub type ProximityEvent = ncollide::pipeline::ProximityEvent; pub type Ray = ncollide::query::Ray; /// The intersection between a ray and a collider. pub type RayIntersection = ncollide::query::RayIntersection; +/// The the projection of a point on a collider. +pub type PointProjection = ncollide::query::PointProjection; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::ball::WBall; @@ -61,10 +65,10 @@ pub(crate) use self::polyhedron_feature3d::PolyhedronFace; pub(crate) use self::waabb::{WRay, WAABB}; pub(crate) use self::wquadtree::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; +pub use self::shape::{Shape, ShapeType}; mod ball; mod broad_phase_multi_sap; -mod capsule; mod collider; mod collider_set; mod contact; @@ -89,3 +93,5 @@ mod wquadtree; //mod z_order; #[cfg(feature = "dim3")] mod polygonal_feature_map; +mod rounded; +mod shape; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index ebe0a79..e95709c 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -197,7 +197,8 @@ impl NarrowPhase { if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() { let dispatcher = DefaultProximityDispatcher; - let generator = dispatcher.dispatch(co1.shape(), co2.shape()); + let generator = dispatcher + .dispatch(co1.shape().shape_type(), co2.shape().shape_type()); let interaction = ProximityPair::new(*pair, generator.0, generator.1); let _ = self.proximity_graph.add_edge( @@ -226,7 +227,8 @@ impl NarrowPhase { if self.contact_graph.graph.find_edge(gid1, gid2).is_none() { let dispatcher = DefaultContactDispatcher; - let generator = dispatcher.dispatch(co1.shape(), co2.shape()); + let generator = dispatcher + .dispatch(co1.shape().shape_type(), co2.shape().shape_type()); let interaction = ContactPair::new(*pair, generator.0, generator.1); let _ = self.contact_graph.add_edge( co1.contact_graph_index, @@ -308,7 +310,8 @@ impl NarrowPhase { if pair.detector.is_none() { // We need a redispatch for this detector. // This can happen, e.g., after restoring a snapshot of the narrow-phase. - let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape()); + let (detector, workspace) = + dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type()); pair.detector = Some(detector); pair.detector_workspace = workspace; } @@ -418,7 +421,8 @@ impl NarrowPhase { if pair.generator.is_none() { // We need a redispatch for this generator. // This can happen, e.g., after restoring a snapshot of the narrow-phase. - let (generator, workspace) = dispatcher.dispatch(co1.shape(), co2.shape()); + let (generator, workspace) = + dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type()); pair.generator = Some(generator); pair.generator_workspace = workspace; } diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs index b00337d..d7c5d02 100644 --- a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs @@ -4,24 +4,16 @@ use crate::math::Isometry; use ncollide::query::PointQuery; pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - if let Shape::Ball(ball1) = ctxt.shape1 { - match ctxt.shape2 { - Shape::Triangle(tri2) => do_detect_proximity(tri2, ball1, &ctxt), - Shape::Cuboid(cube2) => do_detect_proximity(cube2, ball1, &ctxt), - _ => unimplemented!(), - } - } else if let Shape::Ball(ball2) = ctxt.shape2 { - match ctxt.shape1 { - Shape::Triangle(tri1) => do_detect_proximity(tri1, ball2, &ctxt), - Shape::Cuboid(cube1) => do_detect_proximity(cube1, ball2, &ctxt), - _ => unimplemented!(), - } + if let Some(ball1) = ctxt.shape1.as_ball() { + do_detect_proximity(ctxt.shape2, ball1, &ctxt) + } else if let Some(ball2) = ctxt.shape2.as_ball() { + do_detect_proximity(ctxt.shape1, ball2, &ctxt) } else { panic!("Invalid shape types provide.") } } -fn do_detect_proximity>( +fn do_detect_proximity>( point_query1: &P, ball2: &Ball, ctxt: &PrimitiveProximityDetectionContext, diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs index b68ebf9..2462fc9 100644 --- a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs @@ -4,7 +4,7 @@ use crate::math::Isometry; use ncollide::shape::Cuboid; pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { - if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) { detect_proximity( ctxt.prediction_distance, cube1, diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs index 12f3b4a..45991c7 100644 --- a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs +++ b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs @@ -5,7 +5,7 @@ use crate::math::Isometry; pub fn detect_proximity_cuboid_triangle( ctxt: &mut PrimitiveProximityDetectionContext, ) -> Proximity { - if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) { detect_proximity( ctxt.prediction_distance, cube1, @@ -13,7 +13,9 @@ pub fn detect_proximity_cuboid_triangle( triangle2, ctxt.position2, ) - } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + } else if let (Some(triangle1), Some(cube2)) = + (ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid()) + { detect_proximity( ctxt.prediction_distance, cube2, diff --git a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs index f0e049f..5b89dc5 100644 --- a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs +++ b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs @@ -5,17 +5,18 @@ use crate::math::Isometry; pub fn detect_proximity_polygon_polygon( ctxt: &mut PrimitiveProximityDetectionContext, ) -> Proximity { - if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { - detect_proximity( - ctxt.prediction_distance, - polygon1, - &ctxt.position1, - polygon2, - &ctxt.position2, - ) - } else { - unreachable!() - } + unimplemented!() + // if let (Some(polygon1), Some(polygon2)) = (ctxt.shape1.as_polygon(), ctxt.shape2.as_polygon()) { + // detect_proximity( + // ctxt.prediction_distance, + // polygon1, + // &ctxt.position1, + // polygon2, + // &ctxt.position2, + // ) + // } else { + // unreachable!() + // } } fn detect_proximity<'a>( diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs index 6d6b4c5..62f50f7 100644 --- a/src/geometry/proximity_detector/proximity_dispatcher.rs +++ b/src/geometry/proximity_detector/proximity_dispatcher.rs @@ -2,7 +2,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetector, ProximityDetector, ProximityPhase, TrimeshShapeProximityDetectorWorkspace, }; -use crate::geometry::Shape; +use crate::geometry::{Shape, ShapeType}; use std::any::Any; /// Trait implemented by structures responsible for selecting a collision-detection algorithm @@ -11,8 +11,8 @@ pub trait ProximityDispatcher { /// Select the proximity detection algorithm for the given pair of primitive shapes. fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveProximityDetector, Option>, @@ -20,8 +20,8 @@ pub trait ProximityDispatcher { /// Select the proximity detection algorithm for the given pair of non-primitive shapes. fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ProximityPhase, Option>); } @@ -31,14 +31,14 @@ pub struct DefaultProximityDispatcher; impl ProximityDispatcher for DefaultProximityDispatcher { fn dispatch_primitives( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> ( PrimitiveProximityDetector, Option>, ) { match (shape1, shape2) { - (Shape::Ball(_), Shape::Ball(_)) => ( + (ShapeType::Ball, ShapeType::Ball) => ( PrimitiveProximityDetector { #[cfg(feature = "simd-is-enabled")] detect_proximity_simd: super::detect_proximity_ball_ball_simd, @@ -47,56 +47,56 @@ impl ProximityDispatcher for DefaultProximityDispatcher { }, None, ), - (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + (ShapeType::Cuboid, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_cuboid, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Polygon(_), Shape::Polygon(_)) => ( + (ShapeType::Polygon, ShapeType::Polygon) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_polygon_polygon, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Triangle(_), Shape::Ball(_)) => ( + (ShapeType::Triangle, ShapeType::Ball) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Ball(_), Shape::Triangle(_)) => ( + (ShapeType::Ball, ShapeType::Triangle) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Cuboid(_), Shape::Ball(_)) => ( + (ShapeType::Cuboid, ShapeType::Ball) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Ball(_), Shape::Cuboid(_)) => ( + (ShapeType::Ball, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_ball_convex, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Triangle(_), Shape::Cuboid(_)) => ( + (ShapeType::Triangle, ShapeType::Cuboid) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_triangle, ..PrimitiveProximityDetector::default() }, None, ), - (Shape::Cuboid(_), Shape::Triangle(_)) => ( + (ShapeType::Cuboid, ShapeType::Triangle) => ( PrimitiveProximityDetector { detect_proximity: super::detect_proximity_cuboid_triangle, ..PrimitiveProximityDetector::default() @@ -109,18 +109,18 @@ impl ProximityDispatcher for DefaultProximityDispatcher { fn dispatch( &self, - shape1: &Shape, - shape2: &Shape, + shape1: ShapeType, + shape2: ShapeType, ) -> (ProximityPhase, Option>) { match (shape1, shape2) { - (Shape::Trimesh(_), _) => ( + (ShapeType::Trimesh, _) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() }), Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), ), - (_, Shape::Trimesh(_)) => ( + (_, ShapeType::Trimesh) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index edf3085..cce46d2 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -1,7 +1,7 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; -use crate::geometry::{Collider, Proximity, Shape, Trimesh}; +use crate::geometry::{Collider, Proximity, Shape, ShapeType, Trimesh}; use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; pub struct TrimeshShapeProximityDetectorWorkspace { @@ -24,9 +24,9 @@ pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> P let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; - if let Shape::Trimesh(trimesh1) = collider1.shape() { + if let Some(trimesh1) = collider1.shape().as_trimesh() { do_detect_proximity(trimesh1, collider1, collider2, ctxt) - } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + } else if let Some(trimesh2) = collider2.shape().as_trimesh() { do_detect_proximity(trimesh2, collider2, collider1, ctxt) } else { panic!("Invalid shape types provided.") @@ -83,6 +83,7 @@ fn do_detect_proximity( let new_interferences = &workspace.interferences; let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); let mut best_proximity = Proximity::Disjoint; + let shape_type2 = collider2.shape().shape_type(); for triangle_id in new_interferences.iter() { if *triangle_id >= trimesh1.num_triangles() { @@ -107,10 +108,10 @@ fn do_detect_proximity( }; } - let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let triangle1 = trimesh1.triangle(*triangle_id); let (proximity_detector, mut workspace2) = ctxt .dispatcher - .dispatch_primitives(&triangle1, collider2.shape()); + .dispatch_primitives(ShapeType::Triangle, shape_type2); let mut ctxt2 = PrimitiveProximityDetectionContext { prediction_distance: ctxt.prediction_distance, diff --git a/src/geometry/rounded.rs b/src/geometry/rounded.rs new file mode 100644 index 0000000..615d408 --- /dev/null +++ b/src/geometry/rounded.rs @@ -0,0 +1,7 @@ +/// A rounded shape. +pub struct Rounded { + /// The shape being rounded. + pub shape: S, + /// The rounding radius. + pub radius: f32, +} diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs index 0666c04..e2548dd 100644 --- a/src/geometry/sat.rs +++ b/src/geometry/sat.rs @@ -58,8 +58,8 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( let y2 = pos12 * Vector::y(); let z2 = pos12 * Vector::z(); - // We have 3 * 3 = 9 axii to test. - let axii = [ + // We have 3 * 3 = 9 axes to test. + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), @@ -74,7 +74,7 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway( Vector::new(-z2.y, z2.x, 0.0), ]; - for axis1 in &axii { + for axis1 in &axes { let norm1 = axis1.norm(); if norm1 > f32::default_epsilon() { let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line( @@ -149,7 +149,7 @@ pub fn cube_support_map_compute_separation_wrt_local_line>( pub fn cube_support_map_find_local_separating_edge_twoway( cube1: &Cuboid, shape2: &impl SupportMap, - axii: &[Vector], + axes: &[Vector], pos12: &Isometry, pos21: &Isometry, ) -> (f32, Vector) { @@ -157,7 +157,7 @@ pub fn cube_support_map_find_local_separating_edge_twoway( let mut best_separation = -std::f32::MAX; let mut best_dir = Vector::zeros(); - for axis1 in axii { + for axis1 in axes { if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) { let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line( cube1, shape2, pos12, pos21, &axis1, @@ -184,8 +184,8 @@ pub fn cube_triangle_find_local_separating_edge_twoway( let y2 = pos12 * (triangle2.c - triangle2.b); let z2 = pos12 * (triangle2.a - triangle2.c); - // We have 3 * 3 = 3 axii to test. - let axii = [ + // We have 3 * 3 = 3 axes to test. + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), @@ -200,7 +200,7 @@ pub fn cube_triangle_find_local_separating_edge_twoway( Vector::new(-z2.y, z2.x, 0.0), ]; - cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axii, pos12, pos21) + cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axes, pos12, pos21) } #[cfg(feature = "dim3")] @@ -212,14 +212,14 @@ pub fn cube_segment_find_local_separating_edge_twoway( ) -> (f32, Vector) { let x2 = pos12 * (segment2.b - segment2.a); - let axii = [ + let axes = [ // Vector::{x, y ,z}().cross(y2) Vector::new(0.0, -x2.z, x2.y), Vector::new(x2.z, 0.0, -x2.x), Vector::new(-x2.y, x2.x, 0.0), ]; - cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axii, pos12, pos21) + cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axes, pos12, pos21) } pub fn cube_support_map_find_local_separating_normal_oneway>( diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs new file mode 100644 index 0000000..c9e0a3a --- /dev/null +++ b/src/geometry/shape.rs @@ -0,0 +1,275 @@ +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, +}; +use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; +use downcast_rs::{impl_downcast, DowncastSync}; +use erased_serde::Serialize; +use na::Point3; +use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use ncollide::query::{PointQuery, RayCast}; +use num::Zero; +use num_derive::FromPrimitive; +use std::any::Any; + +#[derive(Copy, Clone, Debug, FromPrimitive)] +/// Enum representing the type of a shape. +pub enum ShapeType { + /// A ball shape. + Ball = 1, + /// A convex polygon shape. + Polygon, + /// A cuboid shape. + Cuboid, + /// A capsule shape. + Capsule, + /// A triangle shape. + Triangle, + /// A triangle mesh shape. + Trimesh, + /// A heightfield shape. + HeightField, + #[cfg(feature = "dim3")] + /// A cylindrical shape. + Cylinder, + // /// A custom shape type. + // Custom(u8), +} + +/// Trait implemented by shapes usable by Rapier. +pub trait Shape: RayCast + PointQuery + DowncastSync { + /// Convert this shape as a serializable entity. + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + None + } + + /// Computes the AABB of this shape. + fn compute_aabb(&self, position: &Isometry) -> AABB; + + /// Compute the mass-properties of this shape given its uniform density. + fn mass_properties(&self, density: f32) -> MassProperties; + + /// Gets the type tag of this shape. + fn shape_type(&self) -> ShapeType; + + /// Converts this shape to a polygonal feature-map, if it is one. + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + None + } + + // fn as_rounded(&self) -> Option<&Rounded>> { + // None + // } +} + +impl_downcast!(sync Shape); + +impl dyn Shape { + /// Converts this abstract shape to a ball, if it is one. + pub fn as_ball(&self) -> Option<&Ball> { + self.downcast_ref() + } + + /// Converts this abstract shape to a cuboid, if it is one. + pub fn as_cuboid(&self) -> Option<&Cuboid> { + self.downcast_ref() + } + + /// Converts this abstract shape to a capsule, if it is one. + pub fn as_capsule(&self) -> Option<&Capsule> { + self.downcast_ref() + } + + /// Converts this abstract shape to a triangle, if it is one. + pub fn as_triangle(&self) -> Option<&Triangle> { + self.downcast_ref() + } + + /// Converts this abstract shape to a triangle mesh, if it is one. + pub fn as_trimesh(&self) -> Option<&Trimesh> { + self.downcast_ref() + } + + /// Converts this abstract shape to a heightfield, if it is one. + pub fn as_heightfield(&self) -> Option<&HeightField> { + self.downcast_ref() + } + + /// Converts this abstract shape to a cylinder, if it is one. + pub fn as_cylinder(&self) -> Option<&Cylinder> { + self.downcast_ref() + } +} + +impl Shape for Ball { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_ball(density, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Ball + } +} + +// impl Shape for Polygon { +// #[cfg(feature = "serde-serialize")] +// fn as_serialize(&self) -> Option<&dyn Serialize> { +// Some(self as &dyn Serialize) +// } +// +// fn compute_aabb(&self, position: &Isometry) -> AABB { +// self.aabb(position) +// } +// +// fn mass_properties(&self, _density: f32) -> MassProperties { +// unimplemented!() +// } +// +// fn shape_type(&self) -> ShapeType { +// ShapeType::Polygon +// } +// } + +impl Shape for Cuboid { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cuboid(density, self.half_extents) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cuboid + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + Some(self as &dyn PolygonalFeatureMap) + } +} + +impl Shape for Capsule { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_capsule(density, self.half_height, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Capsule + } +} + +impl Shape for Triangle { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Triangle + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + Some(self as &dyn PolygonalFeatureMap) + } +} + +impl Shape for Trimesh { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.aabb(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Trimesh + } +} + +impl Shape for HeightField { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, _density: f32) -> MassProperties { + MassProperties::zero() + } + + fn shape_type(&self) -> ShapeType { + ShapeType::HeightField + } +} + +#[cfg(feature = "dim3")] +impl Shape for Cylinder { + #[cfg(feature = "serde-serialize")] + fn as_serialize(&self) -> Option<&dyn Serialize> { + Some(self as &dyn Serialize) + } + + fn compute_aabb(&self, position: &Isometry) -> AABB { + self.bounding_volume(position) + } + + fn mass_properties(&self, density: f32) -> MassProperties { + MassProperties::from_cylinder(density, self.half_height, self.radius) + } + + fn shape_type(&self) -> ShapeType { + ShapeType::Cylinder + } + + #[cfg(feature = "dim3")] + fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> { + Some(self as &dyn PolygonalFeatureMap) + } +} diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs index b6e23e7..38ce0a3 100644 --- a/src/geometry/trimesh.rs +++ b/src/geometry/trimesh.rs @@ -1,13 +1,9 @@ -use crate::geometry::{Triangle, WQuadtree}; +use crate::geometry::{PointProjection, Ray, RayIntersection, Triangle, WQuadtree}; use crate::math::{Isometry, Point}; use na::Point3; use ncollide::bounding_volume::{HasBoundingVolume, AABB}; - -#[cfg(feature = "dim3")] -use { - crate::geometry::{Ray, RayIntersection}, - ncollide::query::RayCast, -}; +use ncollide::query::{PointQuery, RayCast}; +use ncollide::shape::FeatureId; #[derive(Clone)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -110,6 +106,41 @@ impl Trimesh { } } +impl PointQuery for Trimesh { + fn project_point(&self, m: &Isometry, pt: &Point, solid: bool) -> PointProjection { + // TODO + unimplemented!() + } + + fn project_point_with_feature( + &self, + m: &Isometry, + pt: &Point, + ) -> (PointProjection, FeatureId) { + // TODO + unimplemented!() + } +} + +#[cfg(feature = "dim2")] +impl RayCast for Trimesh { + fn toi_and_normal_with_ray( + &self, + m: &Isometry, + ray: &Ray, + max_toi: f32, + solid: bool, + ) -> Option { + // TODO + None + } + + fn intersects_ray(&self, m: &Isometry, ray: &Ray, max_toi: f32) -> bool { + // TODO + false + } +} + #[cfg(feature = "dim3")] impl RayCast for Trimesh { fn toi_and_normal_with_ray( diff --git a/src/pipeline/query_pipeline.rs b/src/pipeline/query_pipeline.rs index 32f59fc..03103be 100644 --- a/src/pipeline/query_pipeline.rs +++ b/src/pipeline/query_pipeline.rs @@ -69,7 +69,11 @@ impl QueryPipeline { for handle in inter { let collider = &colliders[handle]; - if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if let Some(inter) = + collider + .shape() + .toi_and_normal_with_ray(collider.position(), ray, max_toi, true) + { if inter.toi < best { best = inter.toi; result = Some((handle, collider, inter)); @@ -103,7 +107,11 @@ impl QueryPipeline { for handle in inter { let collider = &colliders[handle]; - if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) { + if let Some(inter) = + collider + .shape() + .toi_and_normal_with_ray(collider.position(), ray, max_toi, true) + { if !callback(handle, collider, inter) { return; } diff --git a/src/utils.rs b/src/utils.rs index bd972b8..48f4ef7 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -1,6 +1,7 @@ //! Miscellaneous utilities. use crate::dynamics::RigidBodyHandle; +use crate::math::{Isometry, Point, Rotation, Vector}; #[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))] use indexmap::IndexMap as HashMap; use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; @@ -1332,3 +1333,28 @@ pub(crate) fn other_handle( pair.0 } } + +/// Returns the rotation that aligns the y axis to the segment direction. +pub(crate) fn rotation_wrt_y(a: &Point, b: &Point) -> Rotation { + let mut dir = b - a; + + if dir.y < 0.0 { + dir = -dir; + } + + #[cfg(feature = "dim2")] + return Rotation::rotation_between(&Vector::y(), &dir); + + #[cfg(feature = "dim3")] + return Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()); +} + +// Return the transform that aligns the y axis to the segment and move the origin to the segment middle, +// and the capsule's half-height. +pub(crate) fn segment_to_capsule(a: &Point, b: &Point) -> (Isometry, f32) { + let rot = rotation_wrt_y(a, b); + let half_height = (b - a).norm() / 2.0; + let center = na::center(a, b); + let pos = Isometry::from_parts(center.coords.into(), rot); + (pos, half_height) +} diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs index ca2d71f..ca1a2b8 100644 --- a/src_testbed/engine.rs +++ b/src_testbed/engine.rs @@ -350,33 +350,44 @@ impl GraphicsManager { color: Point3, out: &mut Vec, ) { - match collider.shape() { - Shape::Ball(ball) => { - out.push(Node::Ball(Ball::new(handle, ball.radius, color, window))) - } - Shape::Polygon(poly) => out.push(Node::Convex(Convex::new( - handle, - poly.vertices().to_vec(), - color, - window, - ))), - Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new( + let shape = collider.shape(); + + if let Some(ball) = shape.as_ball() { + out.push(Node::Ball(Ball::new(handle, ball.radius, color, window))) + } + + // Shape::Polygon(poly) => out.push(Node::Convex(Convex::new( + // handle, + // poly.vertices().to_vec(), + // color, + // window, + // ))), + + if let Some(cuboid) = shape.as_cuboid() { + out.push(Node::Box(BoxNode::new( handle, cuboid.half_extents, color, window, - ))), - Shape::Capsule(capsule) => { - out.push(Node::Capsule(Capsule::new(handle, capsule, color, window))) - } - Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new( + ))) + } + + if let Some(capsule) = shape.as_capsule() { + out.push(Node::Capsule(Capsule::new(handle, capsule, color, window))) + } + + if let Some(triangle) = shape.as_triangle() { + out.push(Node::Mesh(Mesh::new( handle, vec![triangle.a, triangle.b, triangle.c], vec![Point3::new(0, 1, 2)], color, window, - ))), - Shape::Trimesh(trimesh) => out.push(Node::Mesh(Mesh::new( + ))) + } + + if let Some(trimesh) = shape.as_trimesh() { + out.push(Node::Mesh(Mesh::new( handle, trimesh.vertices().to_vec(), trimesh @@ -386,21 +397,27 @@ impl GraphicsManager { .collect(), color, window, - ))), - Shape::HeightField(heightfield) => out.push(Node::HeightField(HeightField::new( + ))) + } + + if let Some(heightfield) = shape.as_heightfield() { + out.push(Node::HeightField(HeightField::new( handle, heightfield, color, window, - ))), - #[cfg(feature = "dim3")] - Shape::Cylinder(cylinder) => out.push(Node::Cylinder(Cylinder::new( + ))) + } + + #[cfg(feature = "dim3")] + if let Some(cylinder) = shape.as_cylinder() { + out.push(Node::Cylinder(Cylinder::new( handle, cylinder.half_height, cylinder.radius, color, window, - ))), + ))) } }