From cc6d1b973002b4d366bc81ec6bf9e8240ad7b404 Mon Sep 17 00:00:00 2001 From: Crozet Sébastien Date: Mon, 14 Dec 2020 15:51:43 +0100 Subject: Outsource the Shape trait, wquadtree, and shape types. --- src/geometry/ball.rs | 8 +- src/geometry/collider.rs | 18 +- src/geometry/contact.rs | 12 +- .../ball_ball_contact_generator.rs | 8 +- .../contact_generator/contact_dispatcher.rs | 8 +- .../contact_generator/contact_generator.rs | 8 +- .../contact_generator_workspace.rs | 6 +- .../heightfield_shape_contact_generator.rs | 9 +- src/geometry/contact_generator/mod.rs | 2 +- .../serializable_workspace_tag.rs | 2 +- .../trimesh_shape_contact_generator.rs | 32 +- src/geometry/mod.rs | 14 +- src/geometry/narrow_phase.rs | 2 +- .../ball_ball_proximity_detector.rs | 8 +- src/geometry/proximity_detector/mod.rs | 2 +- .../proximity_detector/proximity_detector.rs | 6 +- .../proximity_detector/proximity_dispatcher.rs | 10 +- .../trimesh_shape_proximity_detector.rs | 22 +- src/geometry/round_cylinder.rs | 92 ---- src/geometry/shape.rs | 393 -------------- src/geometry/trimesh.rs | 203 ------- src/geometry/waabb.rs | 217 -------- src/geometry/wquadtree.rs | 587 --------------------- 23 files changed, 85 insertions(+), 1584 deletions(-) delete mode 100644 src/geometry/round_cylinder.rs delete mode 100644 src/geometry/shape.rs delete mode 100644 src/geometry/trimesh.rs delete mode 100644 src/geometry/waabb.rs delete mode 100644 src/geometry/wquadtree.rs (limited to 'src/geometry') diff --git a/src/geometry/ball.rs b/src/geometry/ball.rs index 7f4ad03..2d9f7be 100644 --- a/src/geometry/ball.rs +++ b/src/geometry/ball.rs @@ -1,16 +1,16 @@ #[cfg(feature = "simd-is-enabled")] -use crate::math::{Point, SimdFloat}; +use crate::math::{Point, SimdReal}; #[cfg(feature = "simd-is-enabled")] #[derive(Copy, Clone, Debug)] pub(crate) struct WBall { - pub center: Point, - pub radius: SimdFloat, + pub center: Point, + pub radius: SimdReal, } #[cfg(feature = "simd-is-enabled")] impl WBall { - pub fn new(center: Point, radius: SimdFloat) -> Self { + pub fn new(center: Point, radius: SimdReal) -> Self { WBall { center, radius } } } diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index 8154554..1275358 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,12 +1,13 @@ +use crate::buckler::shape::HalfSpace; use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; -use crate::geometry::{ - Ball, Capsule, Cuboid, HeightField, InteractionGroups, Segment, Shape, ShapeType, Triangle, - Trimesh, -}; -#[cfg(feature = "dim3")] -use crate::geometry::{Cone, Cylinder, RoundCylinder}; +use crate::geometry::InteractionGroups; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; use buckler::bounding_volume::AABB; +use buckler::shape::{ + Ball, Capsule, Cuboid, HeightField, Segment, Shape, ShapeType, TriMesh, Triangle, +}; +#[cfg(feature = "dim3")] +use buckler::shape::{Cone, Cylinder, RoundCylinder}; use na::Point3; use std::ops::Deref; use std::sync::Arc; @@ -77,7 +78,7 @@ impl ColliderShape { /// 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))) + ColliderShape(Arc::new(TriMesh::new(vertices, indices))) } /// Initializes an heightfield shape defined by its set of height and a scale @@ -165,8 +166,9 @@ impl<'de> serde::Deserialize<'de> for ColliderShape { Some(ShapeType::Capsule) => deser::(&mut seq)?, Some(ShapeType::Triangle) => deser::(&mut seq)?, Some(ShapeType::Segment) => deser::(&mut seq)?, - Some(ShapeType::Trimesh) => deser::(&mut seq)?, + Some(ShapeType::TriMesh) => deser::(&mut seq)?, Some(ShapeType::HeightField) => deser::(&mut seq)?, + Some(ShapeType::HalfSpace) => deser::(&mut seq)?, #[cfg(feature = "dim3")] Some(ShapeType::Cylinder) => deser::(&mut seq)?, #[cfg(feature = "dim3")] diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 1f0a902..14646d9 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -6,7 +6,7 @@ use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManif use crate::math::{Isometry, Point, Vector}; #[cfg(feature = "simd-is-enabled")] use { - crate::math::{SimdFloat, SIMD_WIDTH}, + crate::math::{SimdReal, SIMD_WIDTH}, simba::simd::SimdValue, }; @@ -22,11 +22,11 @@ bitflags::bitflags! { #[cfg(feature = "simd-is-enabled")] pub(crate) struct WContact { - pub local_p1: Point, - pub local_p2: Point, - pub local_n1: Vector, - pub local_n2: Vector, - pub dist: SimdFloat, + pub local_p1: Point, + pub local_p2: Point, + pub local_n1: Vector, + pub local_n2: Vector, + pub dist: SimdReal, pub fid1: [u8; SIMD_WIDTH], pub fid2: [u8; SIMD_WIDTH], } diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs index 7122998..f2ca7af 100644 --- a/src/geometry/contact_generator/ball_ball_contact_generator.rs +++ b/src/geometry/contact_generator/ball_ball_contact_generator.rs @@ -5,12 +5,12 @@ use crate::math::{Point, Vector}; use { crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd, crate::geometry::{WBall, WContact}, - crate::math::{Isometry, SimdFloat, SIMD_WIDTH}, + crate::math::{Isometry, SimdReal, SIMD_WIDTH}, simba::simd::SimdValue, }; #[cfg(feature = "simd-is-enabled")] -fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { +fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { let dcenter = ball2.center - ball1.center; let center_dist = dcenter.magnitude(); let normal = dcenter / center_dist; @@ -30,9 +30,9 @@ fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry (ContactPhase, Option) { match (shape1, shape2) { - (ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => ( + (ShapeType::TriMesh, _) | (_, ShapeType::TriMesh) => ( ContactPhase::NearPhase(ContactGenerator { generate_contacts: super::generate_contacts_trimesh_shape, ..ContactGenerator::default() }), Some(ContactGeneratorWorkspace::from( - TrimeshShapeContactGeneratorWorkspace::new(), + TriMeshShapeContactGeneratorWorkspace::new(), )), ), (ShapeType::HeightField, _) | (_, ShapeType::HeightField) => ( diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs index b1b1be6..06ab265 100644 --- a/src/geometry/contact_generator/contact_generator.rs +++ b/src/geometry/contact_generator/contact_generator.rs @@ -1,11 +1,11 @@ use crate::data::MaybeSerializableData; use crate::geometry::{ Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape, - SolverFlags, + ShapeType, SolverFlags, }; use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] -use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::math::{SimdReal, SIMD_WIDTH}; use crate::pipeline::EventHandler; #[derive(Copy, Clone)] @@ -158,8 +158,8 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> { pub colliders2: [&'a Collider; SIMD_WIDTH], pub shapes1: [&'a dyn Shape; SIMD_WIDTH], pub shapes2: [&'a dyn Shape; SIMD_WIDTH], - pub positions1: &'a Isometry, - pub positions2: &'a Isometry, + pub positions1: &'a Isometry, + pub positions2: &'a Isometry, pub manifolds: &'a mut [&'b mut ContactManifold], pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>], } diff --git a/src/geometry/contact_generator/contact_generator_workspace.rs b/src/geometry/contact_generator/contact_generator_workspace.rs index e89395f..7aac592 100644 --- a/src/geometry/contact_generator/contact_generator_workspace.rs +++ b/src/geometry/contact_generator/contact_generator_workspace.rs @@ -2,7 +2,7 @@ use crate::data::MaybeSerializableData; #[cfg(feature = "dim3")] use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace; use crate::geometry::contact_generator::{ - HeightFieldShapeContactGeneratorWorkspace, TrimeshShapeContactGeneratorWorkspace, + HeightFieldShapeContactGeneratorWorkspace, TriMeshShapeContactGeneratorWorkspace, WorkspaceSerializationTag, }; @@ -81,8 +81,8 @@ impl<'de> serde::Deserialize<'de> for ContactGeneratorWorkspace { Some(WorkspaceSerializationTag::HeightfieldShapeContactGeneratorWorkspace) => { deser::(&mut seq)? } - Some(WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace) => { - deser::(&mut seq)? + Some(WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace) => { + deser::(&mut seq)? } #[cfg(feature = "dim3")] Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => { diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs index 358ac84..125ac34 100644 --- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -5,9 +5,10 @@ use crate::geometry::contact_generator::{ ContactGenerationContext, ContactGeneratorWorkspace, PrimitiveContactGenerationContext, PrimitiveContactGenerator, }; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData}; #[cfg(feature = "dim2")] -use crate::geometry::Capsule; -use crate::geometry::{Collider, ContactManifold, ContactManifoldData, HeightField, Shape}; +use buckler::shape::Capsule; +use buckler::shape::{HeightField, Shape}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; @@ -95,7 +96,7 @@ fn do_generate_contacts( #[cfg(feature = "dim3")] let sub_shape1 = *part1; - let sub_detector = match workspace.sub_detectors.entry(i) { + let sub_detector = match workspace.sub_detectors.entry(i as usize) { Entry::Occupied(entry) => { let sub_detector = entry.into_mut(); let manifold = workspace.old_manifolds[sub_detector.manifold_id].take(); @@ -119,7 +120,7 @@ fn do_generate_contacts( collider2, solver_flags, ); - manifolds.push(ContactManifold::with_data((i, 0), manifold_data)); + manifolds.push(ContactManifold::with_data((i as usize, 0), manifold_data)); entry.insert(sub_detector) } diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs index f539d7a..4c0716a 100644 --- a/src/geometry/contact_generator/mod.rs +++ b/src/geometry/contact_generator/mod.rs @@ -25,7 +25,7 @@ pub use self::pfm_pfm_contact_generator::{ // pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; pub use self::contact_generator_workspace::ContactGeneratorWorkspace; pub use self::trimesh_shape_contact_generator::{ - generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, + generate_contacts_trimesh_shape, TriMeshShapeContactGeneratorWorkspace, }; pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag; diff --git a/src/geometry/contact_generator/serializable_workspace_tag.rs b/src/geometry/contact_generator/serializable_workspace_tag.rs index 2488c1e..4bdb902 100644 --- a/src/geometry/contact_generator/serializable_workspace_tag.rs +++ b/src/geometry/contact_generator/serializable_workspace_tag.rs @@ -2,7 +2,7 @@ use num_derive::FromPrimitive; #[derive(Copy, Clone, Debug, FromPrimitive)] pub(super) enum WorkspaceSerializationTag { - TrimeshShapeContactGeneratorWorkspace = 0, + TriMeshShapeContactGeneratorWorkspace = 0, #[cfg(feature = "dim3")] PfmPfmContactGeneratorWorkspace, HeightfieldShapeContactGeneratorWorkspace, diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs index 172069d..209ac42 100644 --- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -3,21 +3,21 @@ use crate::data::MaybeSerializableData; use crate::geometry::contact_generator::{ ContactGenerationContext, PrimitiveContactGenerationContext, }; -use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, Trimesh}; +use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, TriMesh}; #[cfg(feature = "serde-serialize")] use erased_serde::Serialize; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] -pub struct TrimeshShapeContactGeneratorWorkspace { - interferences: Vec, +pub struct TriMeshShapeContactGeneratorWorkspace { + interferences: Vec, local_aabb2: AABB, - old_interferences: Vec, + old_interferences: Vec, #[cfg_attr(feature = "serde-serialize", serde(skip))] old_manifolds: Vec, } -impl TrimeshShapeContactGeneratorWorkspace { +impl TriMeshShapeContactGeneratorWorkspace { pub fn new() -> Self { Self { interferences: Vec::new(), @@ -40,7 +40,7 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) { } fn do_generate_contacts( - trimesh1: &Trimesh, + trimesh1: &TriMesh, collider1: &Collider, collider2: &Collider, ctxt: &mut ContactGenerationContext, @@ -52,14 +52,14 @@ fn do_generate_contacts( ctxt.pair.pair }; - let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt + let workspace: &mut TriMeshShapeContactGeneratorWorkspace = ctxt .pair .generator_workspace .as_mut() - .expect("The TrimeshShapeContactGeneratorWorkspace is missing.") + .expect("The TriMeshShapeContactGeneratorWorkspace is missing.") .0 .downcast_mut() - .expect("Invalid workspace type, expected a TrimeshShapeContactGeneratorWorkspace."); + .expect("Invalid workspace type, expected a TriMeshShapeContactGeneratorWorkspace."); /* * Compute interferences. @@ -97,9 +97,9 @@ fn do_generate_contacts( .iter() .map(|manifold| { if manifold.data.pair.collider1 == ctxt_collider1 { - manifold.subshape_index_pair.0 + manifold.subshape_index_pair.0 as u32 } else { - manifold.subshape_index_pair.1 + manifold.subshape_index_pair.1 as u32 } }) .collect(); @@ -118,7 +118,7 @@ fn do_generate_contacts( workspace.interferences.clear(); trimesh1 - .waabbs() + .quadtree() .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } @@ -134,7 +134,7 @@ fn do_generate_contacts( // TODO: don't redispatch at each frame (we should probably do the same as // the heightfield). for (i, triangle_id) in new_interferences.iter().enumerate() { - if *triangle_id >= trimesh1.num_triangles() { + if *triangle_id >= trimesh1.num_triangles() as u32 { // Because of SIMD padding, the broad-phase may return tiangle indices greater // than the max. continue; @@ -160,7 +160,7 @@ fn do_generate_contacts( ctxt.solver_flags, ); - ContactManifold::with_data((*triangle_id, 0), data) + ContactManifold::with_data((*triangle_id as usize, 0), data) } else { // We already have a manifold for this triangle. old_inter_it.next(); @@ -206,11 +206,11 @@ fn do_generate_contacts( } } -impl MaybeSerializableData for TrimeshShapeContactGeneratorWorkspace { +impl MaybeSerializableData for TriMeshShapeContactGeneratorWorkspace { #[cfg(feature = "serde-serialize")] fn as_serialize(&self) -> Option<(u32, &dyn Serialize)> { Some(( - super::WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace as u32, + super::WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace as u32, self, )) } diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index caa229a..46fe360 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -13,9 +13,6 @@ pub use self::narrow_phase::NarrowPhase; pub use self::polygon::Polygon; pub use self::proximity::ProximityPair; pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; -#[cfg(feature = "dim3")] -pub use self::round_cylinder::RoundCylinder; -pub use self::trimesh::Trimesh; pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; pub use buckler::query::Proximity; @@ -106,11 +103,10 @@ pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact::WContact; pub(crate) use self::narrow_phase::ContactManifoldIndex; -pub(crate) use self::waabb::{WRay, WAABB}; -pub(crate) use self::wquadtree::WQuadtree; +pub(crate) use buckler::partitioning::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; pub use self::interaction_groups::InteractionGroups; -pub use self::shape::{Shape, ShapeType}; +pub use buckler::shape::*; mod ball; mod broad_phase_multi_sap; @@ -125,12 +121,6 @@ mod proximity; mod proximity_detector; pub(crate) mod sat; pub(crate) mod triangle; -mod trimesh; -mod waabb; -mod wquadtree; //mod z_order; mod interaction_groups; -#[cfg(feature = "dim3")] -mod round_cylinder; -mod shape; mod user_callbacks; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 463dd99..ad2d514 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -20,7 +20,7 @@ use crate::geometry::{ }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; //#[cfg(feature = "simd-is-enabled")] -//use crate::math::{SimdFloat, SIMD_WIDTH}; +//use crate::math::{SimdReal, SIMD_WIDTH}; use crate::buckler::query::Proximity; use crate::data::pubsub::Subscription; use crate::data::Coarena; diff --git a/src/geometry/proximity_detector/ball_ball_proximity_detector.rs b/src/geometry/proximity_detector/ball_ball_proximity_detector.rs index 2106c9f..65c141c 100644 --- a/src/geometry/proximity_detector/ball_ball_proximity_detector.rs +++ b/src/geometry/proximity_detector/ball_ball_proximity_detector.rs @@ -5,12 +5,12 @@ use crate::math::Point; #[cfg(feature = "simd-is-enabled")] use { crate::geometry::{proximity_detector::PrimitiveProximityDetectionContextSimd, WBall}, - crate::math::{SimdFloat, SIMD_WIDTH}, + crate::math::{SimdReal, SIMD_WIDTH}, simba::simd::SimdValue, }; #[cfg(feature = "simd-is-enabled")] -fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdFloat { +fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdReal { let dcenter = ball2.center - ball1.center; let center_dist = dcenter.magnitude(); center_dist - ball1.radius - ball2.radius @@ -22,9 +22,9 @@ pub fn detect_proximity_ball_ball_simd( ) -> [Proximity; SIMD_WIDTH] { let pos_ba = ctxt.positions2.inverse() * ctxt.positions1; let radii_a = - SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); + SimdReal::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); let radii_b = - SimdFloat::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); + SimdReal::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); let wball_a = WBall::new(Point::origin(), radii_a); let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b); diff --git a/src/geometry/proximity_detector/mod.rs b/src/geometry/proximity_detector/mod.rs index a99372f..fc904da 100644 --- a/src/geometry/proximity_detector/mod.rs +++ b/src/geometry/proximity_detector/mod.rs @@ -15,7 +15,7 @@ pub use self::proximity_detector::{ }; pub use self::proximity_dispatcher::{DefaultProximityDispatcher, ProximityDispatcher}; pub use self::trimesh_shape_proximity_detector::{ - detect_proximity_trimesh_shape, TrimeshShapeProximityDetectorWorkspace, + detect_proximity_trimesh_shape, TriMeshShapeProximityDetectorWorkspace, }; mod ball_ball_proximity_detector; diff --git a/src/geometry/proximity_detector/proximity_detector.rs b/src/geometry/proximity_detector/proximity_detector.rs index 7c8ad20..ea362de 100644 --- a/src/geometry/proximity_detector/proximity_detector.rs +++ b/src/geometry/proximity_detector/proximity_detector.rs @@ -3,7 +3,7 @@ use crate::geometry::{ }; use crate::math::Isometry; #[cfg(feature = "simd-is-enabled")] -use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::math::{SimdReal, SIMD_WIDTH}; use crate::pipeline::EventHandler; use std::any::Any; @@ -134,8 +134,8 @@ pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> { pub colliders2: [&'a Collider; SIMD_WIDTH], pub shapes1: [&'a dyn Shape; SIMD_WIDTH], pub shapes2: [&'a dyn Shape; SIMD_WIDTH], - pub positions1: &'a Isometry, - pub positions2: &'a Isometry, + pub positions1: &'a Isometry, + pub positions2: &'a Isometry, pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>], } diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs index 768aca6..709521e 100644 --- a/src/geometry/proximity_detector/proximity_dispatcher.rs +++ b/src/geometry/proximity_detector/proximity_dispatcher.rs @@ -1,6 +1,6 @@ use crate::geometry::proximity_detector::{ PrimitiveProximityDetector, ProximityDetector, ProximityPhase, - TrimeshShapeProximityDetectorWorkspace, + TriMeshShapeProximityDetectorWorkspace, }; use crate::geometry::ShapeType; use std::any::Any; @@ -113,19 +113,19 @@ impl ProximityDispatcher for DefaultProximityDispatcher { shape2: ShapeType, ) -> (ProximityPhase, Option>) { match (shape1, shape2) { - (ShapeType::Trimesh, _) => ( + (ShapeType::TriMesh, _) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() }), - Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), + Some(Box::new(TriMeshShapeProximityDetectorWorkspace::new())), ), - (_, ShapeType::Trimesh) => ( + (_, ShapeType::TriMesh) => ( ProximityPhase::NearPhase(ProximityDetector { detect_proximity: super::detect_proximity_trimesh_shape, ..ProximityDetector::default() }), - Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), + Some(Box::new(TriMeshShapeProximityDetectorWorkspace::new())), ), _ => { let (gen, workspace) = self.dispatch_primitives(shape1, shape2); diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs index 5366b39..dbb20f5 100644 --- a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -2,15 +2,15 @@ use crate::buckler::bounding_volume::{BoundingVolume, AABB}; use crate::geometry::proximity_detector::{ PrimitiveProximityDetectionContext, ProximityDetectionContext, }; -use crate::geometry::{Collider, Proximity, ShapeType, Trimesh}; +use crate::geometry::{Collider, Proximity, ShapeType, TriMesh}; -pub struct TrimeshShapeProximityDetectorWorkspace { - interferences: Vec, +pub struct TriMeshShapeProximityDetectorWorkspace { + interferences: Vec, local_aabb2: AABB, - old_interferences: Vec, + old_interferences: Vec, } -impl TrimeshShapeProximityDetectorWorkspace { +impl TriMeshShapeProximityDetectorWorkspace { pub fn new() -> Self { Self { interferences: Vec::new(), @@ -34,18 +34,18 @@ pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> P } fn do_detect_proximity( - trimesh1: &Trimesh, + trimesh1: &TriMesh, collider1: &Collider, collider2: &Collider, ctxt: &mut ProximityDetectionContext, ) -> Proximity { - let workspace: &mut TrimeshShapeProximityDetectorWorkspace = ctxt + let workspace: &mut TriMeshShapeProximityDetectorWorkspace = ctxt .pair .detector_workspace .as_mut() - .expect("The TrimeshShapeProximityDetectorWorkspace is missing.") + .expect("The TriMeshShapeProximityDetectorWorkspace is missing.") .downcast_mut() - .expect("Invalid workspace type, expected a TrimeshShapeProximityDetectorWorkspace."); + .expect("Invalid workspace type, expected a TriMeshShapeProximityDetectorWorkspace."); /* * Compute interferences. @@ -72,7 +72,7 @@ fn do_detect_proximity( workspace.interferences.clear(); trimesh1 - .waabbs() + .quadtree() .intersect_aabb(&local_aabb2, &mut workspace.interferences); workspace.local_aabb2 = local_aabb2; } @@ -86,7 +86,7 @@ fn do_detect_proximity( let shape_type2 = collider2.shape().shape_type(); for triangle_id in new_interferences.iter() { - if *triangle_id >= trimesh1.num_triangles() { + if *triangle_id >= trimesh1.num_triangles() as u32 { // Because of SIMD padding, the broad-phase may return tiangle indices greater // than the max. continue; diff --git a/src/geometry/round_cylinder.rs b/src/geometry/round_cylinder.rs deleted file mode 100644 index 450a9fc..0000000 --- a/src/geometry/round_cylinder.rs +++ /dev/null @@ -1,92 +0,0 @@ -use crate::geometry::Cylinder; -use crate::math::{Isometry, Point, Vector}; -use buckler::query::{ - gjk::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection, -}; -use buckler::shape::{FeatureId, SupportMap}; -use na::Unit; - -/// A rounded cylinder. -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug)] -pub struct RoundCylinder { - /// The cylinder being rounded. - pub cylinder: Cylinder, - /// The rounding radius. - pub border_radius: f32, -} - -impl RoundCylinder { - /// Create sa new cylinder where all its edges and vertices are rounded by a radius of `radius`. - /// - /// This is done by applying a dilation of the given radius to the cylinder. - pub fn new(half_height: f32, radius: f32, border_radius: f32) -> Self { - Self { - cylinder: Cylinder::new(half_height, radius), - border_radius, - } - } -} - -impl SupportMap for RoundCylinder { - fn local_support_point(&self, dir: &Vector) -> Point { - self.local_support_point_toward(&Unit::new_normalize(*dir)) - } - - fn local_support_point_toward(&self, dir: &Unit>) -> Point { - self.cylinder.local_support_point_toward(dir) + **dir * self.border_radius - } - - fn support_point(&self, transform: &Isometry, dir: &Vector) -> Point { - let local_dir = transform.inverse_transform_vector(dir); - transform * self.local_support_point(&local_dir) - } - - fn support_point_toward( - &self, - transform: &Isometry, - dir: &Unit>, - ) -> Point { - let local_dir = Unit::new_unchecked(transform.inverse_transform_vector(dir)); - transform * self.local_support_point_toward(&local_dir) - } -} - -impl RayCast for RoundCylinder { - fn cast_local_ray_and_get_normal( - &self, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option { - buckler::query::details::local_ray_intersection_with_support_map_with_params( - self, - &mut VoronoiSimplex::new(), - ray, - max_toi, - solid, - ) - } -} - -// TODO: if PointQuery had a `project_point_with_normal` method, we could just -// call this and adjust the projected point accordingly. -impl PointQuery for RoundCylinder { - #[inline] - fn project_local_point(&self, point: &Point, solid: bool) -> PointProjection { - buckler::query::details::local_point_projection_on_support_map( - self, - &mut VoronoiSimplex::new(), - point, - solid, - ) - } - - #[inline] - fn project_local_point_and_get_feature( - &self, - point: &Point, - ) -> (PointProjection, FeatureId) { - (self.project_local_point(point, false), FeatureId::Unknown) - } -} diff --git a/src/geometry/shape.rs b/src/geometry/shape.rs deleted file mode 100644 index 80b24c5..0000000 --- a/src/geometry/shape.rs +++ /dev/null @@ -1,393 +0,0 @@ -use crate::dynamics::MassProperties; -use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh}; -use crate::math::Isometry; -use buckler::bounding_volume::AABB; -use buckler::query::{PointQuery, RayCast}; -use downcast_rs::{impl_downcast, DowncastSync}; -#[cfg(feature = "serde-serialize")] -use erased_serde::Serialize; -use num::Zero; -use num_derive::FromPrimitive; -#[cfg(feature = "dim3")] -use { - crate::geometry::{Cone, Cylinder, RoundCylinder}, - buckler::bounding_volume::BoundingVolume, - buckler::shape::PolygonalFeatureMap, -}; - -#[derive(Copy, Clone, Debug, FromPrimitive)] -/// Enum representing the type of a shape. -pub enum ShapeType { - /// A ball shape. - Ball = 0, - /// A convex polygon shape. - Polygon, - /// A cuboid shape. - Cuboid, - /// A capsule shape. - Capsule, - /// A segment shape. - Segment, - /// A triangle shape. - Triangle, - /// A triangle mesh shape. - Trimesh, - /// A heightfield shape. - HeightField, - #[cfg(feature = "dim3")] - /// A cylindrical shape. - Cylinder, - #[cfg(feature = "dim3")] - /// A cylindrical shape. - Cone, - // /// A custom shape type. - // Custom(u8), - // /// A cuboid with rounded corners. - // RoundedCuboid, - // /// A triangle with rounded corners. - // RoundedTriangle, - // /// A triangle-mesh with rounded corners. - // RoundedTrimesh, - // /// An heightfield with rounded corners. - // RoundedHeightField, - /// A cylinder with rounded corners. - #[cfg(feature = "dim3")] - RoundCylinder, - // /// A cone with rounded corners. - // RoundedCone, -} - -/// 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 - } - - // TODO: add a compute_local_aabb method? - - /// 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, f32)> { - 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. - #[cfg(feature = "dim3")] - pub fn as_cylinder(&self) -> Option<&Cylinder> { - self.downcast_ref() - } - - /// Converts this abstract shape to a cone, if it is one. - #[cfg(feature = "dim3")] - pub fn as_cone(&self) -> Option<&Cone> { - self.downcast_ref() - } - - /// Converts this abstract shape to a cone, if it is one. - #[cfg(feature = "dim3")] - pub fn as_round_cylinder(&self) -> Option<&RoundCylinder> { - 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.aabb(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.aabb(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, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -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.aabb(position) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - MassProperties::from_capsule(density, self.segment.a, self.segment.b, self.radius) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Capsule - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((&self.segment as &dyn PolygonalFeatureMap, self.radius)) - } -} - -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.aabb(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, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -impl Shape for Segment { - #[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::Segment - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -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.aabb(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.aabb(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, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -#[cfg(feature = "dim3")] -impl Shape for Cone { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.aabb(position) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - MassProperties::from_cone(density, self.half_height, self.radius) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::Cone - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some((self as &dyn PolygonalFeatureMap, 0.0)) - } -} - -#[cfg(feature = "dim3")] -impl Shape for RoundCylinder { - #[cfg(feature = "serde-serialize")] - fn as_serialize(&self) -> Option<&dyn Serialize> { - Some(self as &dyn Serialize) - } - - fn compute_aabb(&self, position: &Isometry) -> AABB { - self.cylinder - .compute_aabb(position) - .loosened(self.border_radius) - } - - fn mass_properties(&self, density: f32) -> MassProperties { - // We ignore the margin here. - self.cylinder.mass_properties(density) - } - - fn shape_type(&self) -> ShapeType { - ShapeType::RoundCylinder - } - - #[cfg(feature = "dim3")] - fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, f32)> { - Some(( - &self.cylinder as &dyn PolygonalFeatureMap, - self.border_radius, - )) - } -} diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs deleted file mode 100644 index 02a3b1c..0000000 --- a/src/geometry/trimesh.rs +++ /dev/null @@ -1,203 +0,0 @@ -use crate::geometry::{ - Cuboid, HeightField, PointProjection, Ray, RayIntersection, Triangle, WQuadtree, -}; -use crate::math::{Isometry, Point}; -use buckler::bounding_volume::AABB; -use buckler::query::{PointQuery, RayCast}; -use buckler::shape::FeatureId; -use na::Point3; - -#[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A triangle mesh. -pub struct Trimesh { - wquadtree: WQuadtree, - aabb: AABB, - vertices: Vec>, - indices: Vec>, -} - -impl Trimesh { - /// Creates a new triangle mesh from a vertex buffer and an index buffer. - pub fn new(vertices: Vec>, indices: Vec>) -> Self { - assert!( - vertices.len() > 1, - "A triangle mesh must contain at least one point." - ); - assert!( - indices.len() > 1, - "A triangle mesh must contain at least one triangle." - ); - - let aabb = AABB::from_points(&vertices); - let data = indices.iter().enumerate().map(|(i, idx)| { - let aabb = Triangle::new( - vertices[idx[0] as usize], - vertices[idx[1] as usize], - vertices[idx[2] as usize], - ) - .local_aabb(); - (i, aabb) - }); - - let mut wquadtree = WQuadtree::new(); - // NOTE: we apply no dilation factor because we won't - // update this tree dynamically. - wquadtree.clear_and_rebuild(data, 0.0); - - Self { - wquadtree, - aabb, - vertices, - indices, - } - } - - /// Compute the axis-aligned bounding box of this triangle mesh. - pub fn aabb(&self, pos: &Isometry) -> AABB { - self.aabb.transform_by(pos) - } - - pub(crate) fn waabbs(&self) -> &WQuadtree { - &self.wquadtree - } - - /// The number of triangles forming this mesh. - pub fn num_triangles(&self) -> usize { - self.indices.len() - } - - /// An iterator through all the triangles of this mesh. - pub fn triangles(&self) -> impl Iterator + '_ { - self.indices.iter().map(move |ids| { - Triangle::new( - self.vertices[ids.x as usize], - self.vertices[ids.y as usize], - self.vertices[ids.z as usize], - ) - }) - } - - /// Get the `i`-th triangle of this mesh. - pub fn triangle(&self, i: usize) -> Triangle { - let idx = self.indices[i]; - Triangle::new( - self.vertices[idx.x as usize], - self.vertices[idx.y as usize], - self.vertices[idx.z as usize], - ) - } - - /// The vertex buffer of this mesh. - pub fn vertices(&self) -> &[Point] { - &self.vertices[..] - } - - /// The index buffer of this mesh. - pub fn indices(&self) -> &[Point3] { - &self.indices - } - - /// A flat view of the index buffer of this mesh. - pub fn flat_indices(&self) -> &[u32] { - unsafe { - let len = self.indices.len() * 3; - let data = self.indices.as_ptr() as *const u32; - std::slice::from_raw_parts(data, len) - } - } -} - -impl PointQuery for Trimesh { - fn project_local_point(&self, _pt: &Point, _solid: bool) -> PointProjection { - // TODO - unimplemented!() - } - - fn project_local_point_and_get_feature( - &self, - _pt: &Point, - ) -> (PointProjection, FeatureId) { - // TODO - unimplemented!() - } -} - -#[cfg(feature = "dim2")] -impl RayCast for Trimesh { - fn cast_local_ray_and_get_normal( - &self, - _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 cast_local_ray_and_get_normal( - &self, - ray: &Ray, - max_toi: f32, - solid: bool, - ) -> Option { - // FIXME: do a best-first search. - let mut intersections = Vec::new(); - self.wquadtree.cast_ray(&ray, max_toi, &mut intersections); - let mut best: Option = None; - - for inter in intersections { - let tri = self.triangle(inter); - if let Some(inter) = tri.cast_local_ray_and_get_normal(ray, max_toi, solid) { - if let Some(curr) = &mut best { - if curr.toi > inter.toi { - *curr = inter; - } - } else { - best = Some(inter); - } - } - } - - best - } - - fn intersects_local_ray(&self, ray: &Ray, max_toi: f32) -> bool { - // FIXME: do a best-first search. - let mut intersections = Vec::new(); - self.wquadtree.cast_ray(&ray, max_toi, &mut intersections); - - for inter in intersections { - let tri = self.triangle(inter); - if tri.intersects_local_ray(ray, max_toi) { - return true; - } - } - - false - } -} - -#[cfg(feature = "dim3")] -impl From for Trimesh { - fn from(heightfield: HeightField) -> Self { - let (vtx, idx) = heightfield.to_trimesh(); - Trimesh::new(vtx, idx) - } -} - -#[cfg(feature = "dim3")] -impl From for Trimesh { - fn from(cuboid: Cuboid) -> Self { - let (vtx, idx) = cuboid.to_trimesh(); - Trimesh::new(vtx, idx) - } -} diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs deleted file mode 100644 index 08f1df1..0000000 --- a/src/geometry/waabb.rs +++ /dev/null @@ -1,217 +0,0 @@ -use crate::geometry::Ray; -use crate::math::{Point, Vector, DIM, SIMD_WIDTH}; -use crate::utils; -use buckler::bounding_volume::AABB; -use num::{One, Zero}; -use { - crate::math::{SimdBool, SimdFloat}, - simba::simd::{SimdPartialOrd, SimdValue}, -}; - -#[derive(Debug, Copy, Clone)] -pub(crate) struct WRay { - pub origin: Point, - pub dir: Vector, -} - -impl WRay { - pub fn splat(ray: Ray) -> Self { - Self { - origin: Point::splat(ray.origin), - dir: Vector::splat(ray.dir), - } - } -} - -#[derive(Debug, Copy, Clone)] -pub(crate) struct WAABB { - pub mins: Point, - pub maxs: Point, -} - -#[cfg(feature = "serde-serialize")] -impl serde::Serialize for WAABB { - fn serialize(&self, serializer: S) -> Result - where - S: serde::Serializer, - { - use serde::ser::SerializeStruct; - - let mins: Point<[f32; SIMD_WIDTH]> = Point::from( - self.mins - .coords - .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), - ); - let maxs: Point<[f32; SIMD_WIDTH]> = Point::from( - self.maxs - .coords - .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), - ); - - let mut waabb = serializer.serialize_struct("WAABB", 2)?; - waabb.serialize_field("mins", &mins)?; - waabb.serialize_field("maxs", &maxs)?; - waabb.end() - } -} - -#[cfg(feature = "serde-serialize")] -impl<'de> serde::Deserialize<'de> for WAABB { - fn deserialize(deserializer: D) -> Result - where - D: serde::Deserializer<'de>, - { - struct Visitor {}; - impl<'de> serde::de::Visitor<'de> for Visitor { - type Value = WAABB; - fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { - write!( - formatter, - "two arrays containing at least {} floats", - SIMD_WIDTH * DIM * 2 - ) - } - - fn visit_seq(self, mut seq: A) -> Result - where - A: serde::de::SeqAccess<'de>, - { - let mins: Point<[f32; SIMD_WIDTH]> = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; - let maxs: Point<[f32; SIMD_WIDTH]> = seq - .next_element()? - .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?; - let mins = Point::from(mins.coords.map(|e| SimdFloat::from(e))); - let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e))); - Ok(WAABB { mins, maxs }) - } - } - - deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {}) - } -} - -impl WAABB { - pub fn new_invalid() -> Self { - Self::splat(AABB::new_invalid()) - } - - pub fn splat(aabb: AABB) -> Self { - Self { - mins: Point::splat(aabb.mins), - maxs: Point::splat(aabb.maxs), - } - } - - pub fn dilate_by_factor(&mut self, factor: SimdFloat) { - // If some of the AABBs on this WAABB are invalid, - // don't, dilate them. - let is_valid = self.mins.x.simd_le(self.maxs.x); - let factor = factor.select(is_valid, SimdFloat::zero()); - - // NOTE: we multiply each by factor instead of doing - // (maxs - mins) * factor. That's to avoid overflows (and - // therefore NaNs if this WAABB contains some invalid - // AABBs initialised with f32::MAX - let dilation = self.maxs * factor - self.mins * factor; - self.mins -= dilation; - self.maxs += dilation; - } - - pub fn replace(&mut self, i: usize, aabb: AABB) { - self.mins.replace(i, aabb.mins); - self.maxs.replace(i, aabb.maxs); - } - - pub fn intersects_ray(&self, ray: &WRay, max_toi: SimdFloat) -> SimdBool { - let _0 = SimdFloat::zero(); - let _1 = SimdFloat::one(); - let _infinity = SimdFloat::splat(f32::MAX); - - let mut hit = SimdBool::splat(true); - let mut tmin = SimdFloat::zero(); - let mut tmax = max_toi; - - // TODO: could this be optimized more considering we really just need a boolean answer? - for i in 0usize..DIM { - let is_not_zero = ray.dir[i].simd_ne(_0); - let is_zero_test = - ray.origin[i].simd_ge(self.mins[i]) & ray.origin[i].simd_le(self.maxs[i]); - let is_not_zero_test = { - let denom = _1 / ray.dir[i]; - let mut inter_with_near_plane = - ((self.mins[i] - ray.origin[i]) * denom).select(is_not_zero, -_infinity); - let mut inter_with_far_plane = - ((self.maxs[i] - ray.origin[i]) * denom).select(is_not_zero, _infinity); - - let gt = inter_with_near_plane.simd_gt(inter_with_far_plane); - utils::simd_swap(gt, &mut inter_with_near_plane, &mut inter_with_far_plane); - - tmin = tmin.simd_max(inter_with_near_plane); - tmax = tmax.simd_min(inter_with_far_plane); - - tmin.simd_le(tmax) - }; - - hit = hit & is_not_zero_test.select(is_not_zero, is_zero_test); - } - - hit - } - - #[cfg(feature = "dim2")] - pub fn contains(&self, other: &WAABB) -> SimdBool { - self.mins.x.simd_le(other.mins.x) - & self.mins.y.simd_le(other.mins.y) - & self.maxs.x.simd_ge(other.maxs.x) - & self.maxs.y.simd_ge(other.maxs.y) - } - - #[cfg(feature = "dim3")] - pub fn contains(&self, other: &WAABB) -> SimdBool { - self.mins.x.simd_le(other.mins.x) - & self.mins.y.simd_le(other.mins.y) - & self.mins.z.simd_le(other.mins.z) - & self.maxs.x.simd_ge(other.maxs.x) - & self.maxs.y.simd_ge(other.maxs.y) - & self.maxs.z.simd_ge(other.maxs.z) - } - - #[cfg(feature = "dim2")] - pub fn intersects(&self, other: &WAABB) -> SimdBool { - self.mins.x.simd_le(other.maxs.x) - & other.mins.x.simd_le(self.maxs.x) - & self.mins.y.simd_le(other.maxs.y) - & other.mins.y.simd_le(self.maxs.y) - } - - #[cfg(feature = "dim3")] - pub fn intersects(&self, other: &WAABB) -> SimdBool { - self.mins.x.simd_le(other.maxs.x) - & other.mins.x.simd_le(self.maxs.x) - & self.mins.y.simd_le(other.maxs.y) - & other.mins.y.simd_le(self.maxs.y) - & self.mins.z.simd_le(other.maxs.z) - & other.mins.z.simd_le(self.maxs.z) - } - - pub fn to_merged_aabb(&self) -> AABB { - AABB::new( - self.mins.coords.map(|e| e.simd_horizontal_min()).into(), - self.maxs.coords.map(|e| e.simd_horizontal_max()).into(), - ) - } -} - -impl From<[AABB; SIMD_WIDTH]> for WAABB { - fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { - let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; - let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH]; - - WAABB { - mins: Point::from(mins), - maxs: Point::from(maxs), - } - } -} diff --git a/src/geometry/wquadtree.rs b/src/geometry/wquadtree.rs deleted file mode 100644 index 6f57ed3..0000000 --- a/src/geometry/wquadtree.rs +++ /dev/null @@ -1,587 +0,0 @@ -use crate::geometry::{ColliderHandle, ColliderSet, Ray, AABB}; -use crate::geometry::{WRay, WAABB}; -use crate::math::Point; -#[cfg(feature = "dim3")] -use crate::math::Vector; -use crate::simd::{SimdFloat, SIMD_WIDTH}; -use buckler::bounding_volume::BoundingVolume; -use simba::simd::{SimdBool, SimdValue}; -use std::collections::VecDeque; -use std::ops::Range; - -pub trait IndexedData: Copy { - fn default() -> Self; - fn index(&self) -> usize; -} - -impl IndexedData for usize { - fn default() -> Self { - u32::MAX as usize - } - - fn index(&self) -> usize { - *self - } -} - -impl IndexedData for ColliderHandle { - fn default() -> Self { - ColliderSet::invalid_handle() - } - - fn index(&self) -> usize { - self.into_raw_parts().0 - } -} - -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct NodeIndex { - index: u32, // Index of the addressed node in the `nodes` array. - lane: u8, // SIMD lane of the addressed node. -} - -impl NodeIndex { - fn new(index: u32, lane: u8) -> Self { - Self { index, lane } - } - - fn invalid() -> Self { - Self { - index: u32::MAX, - lane: 0, - } - } -} - -#[derive(Copy, Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct WQuadtreeNode { - waabb: WAABB, - // Index of the nodes of the 4 nodes represented by self. - // If this is a leaf, it contains the proxy ids instead. - children: [u32; 4], - parent: NodeIndex, - leaf: bool, // TODO: pack this with the NodexIndex.lane? - dirty: bool, // TODO: move this to a separate bitvec? -} - -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -struct WQuadtreeProxy { - node: NodeIndex, - data: T, // The collider data. TODO: only set the collider generation here? -} - -impl WQuadtreeProxy { - fn invalid() -> Self { - Self { - node: NodeIndex::invalid(), - data: T::default(), - } - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Clone, Debug)] -pub struct WQuadtree { - nodes: Vec, - dirty_nodes: VecDeque, - proxies: Vec>, -} - -// FIXME: this should be generic too. -impl WQuadtree { - pub fn pre_update(&mut self, data: ColliderHandle) { - let id = data.into_raw_parts().0; - let node_id = self.proxies[id].node.index; - let node = &mut self.nodes[node_id as usize]; - if !node.dirty { - node.dirty = true; - self.dirty_nodes.push_back(node_id); - } - } - - pub fn update(&mut self, colliders: &ColliderSet, dilation_factor: f32) { - // Loop on the dirty leaves. - let dilation_factor = SimdFloat::splat(dilation_factor); - - while let Some(id) = self.dirty_nodes.pop_front() { - // NOTE: this will data the case where we reach the root of the tree. - if let Some(node) = self.nodes.get(id as usize) { - // Compute the new WAABB. - let mut new_aabbs = [AABB::new_invalid(); SIMD_WIDTH]; - for (child_id, new_aabb) in node.children.iter().zip(new_aabbs.iter_mut()) { - if node.leaf { - // We are in a leaf: compute the colliders' AABBs. - if let Some(proxy) = self.proxies.get(*child_id as usize) { - let collider = &colliders[proxy.data]; - *new_aabb = collider.compute_aabb(); - } - } else { - // We are in an internal node: compute the children's AABBs. - if let Some(node) = self.nodes.get(*child_id as usize) { - *new_aabb = node.waabb.to_merged_aabb(); - } - } - } - - let node = &mut self.nodes[id as usize]; - let new_waabb = WAABB::from(new_aabbs); - if !node.waabb.contains(&new_waabb).all() { - node.waabb = new_waabb; - node.waabb.dilate_by_factor(dilation_factor); - self.dirty_nodes.push_back(node.parent.index); - } - node.dirty = false; - } - } - } -} - -impl WQuadtree { - pub fn new() -> Self { - WQuadtree { - nodes: Vec::new(), - dirty_nodes: VecDeque::new(), - proxies: Vec::new(), - } - } - - pub fn clear_and_rebuild( - &mut self, - data: impl ExactSizeIterator, - dilation_factor: f32, - ) { - self.nodes.clear(); - self.proxies.clear(); - - // Create proxies. - let mut indices = Vec::with_capacity(data.len()); - let mut aabbs = vec![AABB::new_invalid(); data.len()]; - self.proxies = vec![WQuadtreeProxy::invalid(); data.len()]; - - for (data, aabb) in data { - let index = data.index(); - if index >= self.proxies.len() { - self.proxies.resize(index + 1, WQuadtreeProxy::invalid()); - aabbs.resize(index + 1, AABB::new_invalid()); - } - - self.proxies[index].data = data; - aabbs[index] = aabb; - indices.push(index); - } - - // Build the tree recursively. - let root_node = WQuadtreeNode { - waabb: WAABB::new_invalid(), - children: [1, u32::MAX, u32::MAX, u32::MAX], - parent: NodeIndex::invalid(), - leaf: false, - dirty: false, - }; - - self.nodes.push(root_node); - let root_id = NodeIndex::new(0, 0); - let (_, aabb) = self.do_recurse_build(&mut indices, &aabbs, root_id, dilation_factor); - self.nodes[0].waabb = WAABB::from([ - aabb, - AABB::new_invalid(), - AABB::new_invalid(), - AABB::new_invalid(), - ]); - } - - fn do_recurse_build( - &mut self, - indices: &mut [usize], - aabbs: &[AABB], - parent: NodeIndex, - dilation_factor: f32, - ) -> (u32, AABB) { - if indices.len() <= 4 { - // Leaf case. - let my_id = self.nodes.len(); - let mut my_aabb = AABB::new_invalid(); - let mut leaf_aabbs = [AABB::new_invalid(); 4]; - let mut proxy_ids = [u32::MAX; 4]; - - for (k, id) in indices.iter().enumerate() { - my_aabb.merge(&aabbs[*id]); - leaf_aabbs[k] = aabbs[*id]; - proxy_ids[k] = *id as u32; - self.proxies[*id].node = NodeIndex::new(my_id as u32, k as u8); - } - - let mut node = WQuadtreeNode { - waabb: WAABB::from(leaf_aabbs), - children: proxy_ids, - parent, - leaf: true, - dirty: false, - }; - - node.waabb - .dilate_by_factor(SimdFloat::splat(dilation_factor)); - self.nodes.push(node); - return (my_id as u32, my_aabb); - } - - // Compute the center and variance along each dimension. - // In 3D we compute the variance to not-subdivide the dimension with lowe