From 865ce8a8e5301b23ca474adaaffe8b43e725803e Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Tue, 20 Oct 2020 11:55:33 +0200 Subject: 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 | 367 ++++++++++----------- .../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_detector/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, 768 insertions(+), 424 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 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 Deref for ColliderShape { + type Target = Shape; + fn deref(&self) -> &Shape { + &*self.0 } +} - /// 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, - } +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 cuboid shape, if `self` is one. - pub fn as_cuboid(&self) -> Option<&Cuboid> { - match self { - Shape::Cuboid(c) => Some(c), - _ => None, - } + /// Initialize 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 { + ColliderShape(Arc::new(Cylinder::new(half_height, radius))) } - /// 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, - } + /// Initialize a cuboid shape defined by its half-extents. + pub fn cuboid(half_extents: Vector) -> Self { + ColliderShape(Arc::new(Cuboid::new(half_extents))) } - /// 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, - } + /// 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))) } - /// 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, - } + /// Initializes a triangle shape. + pub fn triangle(a: Point, b: Point, c: Point) -> Self { + ColliderShape(Arc::new(Triangle::new(a, b, c))) } - /// 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, - } + /// 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))) } - /// 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, - } + /// 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))) } - /// gets a reference to this shape seen as a PolygonalFeatureMap. + /// 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 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 heightfield(heights: na::DMatrix, scale: Vector) -> Self { + ColliderShape(Arc::new(HeightField::new(heights, scale))) } +} - /// 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), +#[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.", + )) } } +} - /// 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) +#[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, Pro