nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Outsource the Shape trait, wquadtree, and shape types.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-14 15:51:43 +01:00
parent 9bf1321f8f
commit cc6d1b9730
47 changed files with 444 additions and 3363 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/geometry/ball.rs
View File

@@ -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<SimdFloat>,
pub radius: SimdFloat,
pub center: Point<SimdReal>,
pub radius: SimdReal,
}
#[cfg(feature = "simd-is-enabled")]
impl WBall {
pub fn new(center: Point<SimdFloat>, radius: SimdFloat) -> Self {
pub fn new(center: Point<SimdReal>, radius: SimdReal) -> Self {
WBall { center, radius }
}
}

18
src/geometry/collider.rs
View File

@@ -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<Point<f32>>, indices: Vec<Point3<u32>>) -> 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::<A, Capsule>(&mut seq)?,
Some(ShapeType::Triangle) => deser::<A, Triangle>(&mut seq)?,
Some(ShapeType::Segment) => deser::<A, Segment>(&mut seq)?,
Some(ShapeType::Trimesh) => deser::<A, Trimesh>(&mut seq)?,
Some(ShapeType::TriMesh) => deser::<A, TriMesh>(&mut seq)?,
Some(ShapeType::HeightField) => deser::<A, HeightField>(&mut seq)?,
Some(ShapeType::HalfSpace) => deser::<A, HalfSpace>(&mut seq)?,
#[cfg(feature = "dim3")]
Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&mut seq)?,
#[cfg(feature = "dim3")]

12
src/geometry/contact.rs
View File

@@ -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<SimdFloat>,
pub local_p2: Point<SimdFloat>,
pub local_n1: Vector<SimdFloat>,
pub local_n2: Vector<SimdFloat>,
pub dist: SimdFloat,
pub local_p1: Point<SimdReal>,
pub local_p2: Point<SimdReal>,
pub local_n1: Vector<SimdReal>,
pub local_n2: Vector<SimdReal>,
pub dist: SimdReal,
pub fid1: [u8; SIMD_WIDTH],
pub fid2: [u8; SIMD_WIDTH],
}

8
src/geometry/contact_generator/ball_ball_contact_generator.rs
View File

@@ -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<SimdFloat>) -> WContact {
fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry<SimdReal>) -> 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<SimdFlo
pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) {
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);

8
src/geometry/contact_generator/contact_dispatcher.rs
View File

@@ -3,9 +3,9 @@ use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
use crate::geometry::contact_generator::{
ContactGenerator, ContactGeneratorWorkspace, ContactPhase,
HeightFieldShapeContactGeneratorWorkspace, PrimitiveContactGenerator,
TrimeshShapeContactGeneratorWorkspace,
TriMeshShapeContactGeneratorWorkspace,
};
use crate::geometry::ShapeType;
use buckler::shape::ShapeType;
/// Trait implemented by structures responsible for selecting a collision-detection algorithm
/// for a given pair of shapes.
@@ -114,13 +114,13 @@ impl ContactDispatcher for DefaultContactDispatcher {
shape2: ShapeType,
) -> (ContactPhase, Option<ContactGeneratorWorkspace>) {
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) => (

8
src/geometry/contact_generator/contact_generator.rs
View File

@@ -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<SimdFloat>,
pub positions2: &'a Isometry<SimdFloat>,
pub positions1: &'a Isometry<SimdReal>,
pub positions2: &'a Isometry<SimdReal>,
pub manifolds: &'a mut [&'b mut ContactManifold],
pub workspaces: &'a mut [Option<&'b mut (dyn MaybeSerializableData)>],
}

6
src/geometry/contact_generator/contact_generator_workspace.rs
View File

@@ -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::<A, HeightFieldShapeContactGeneratorWorkspace>(&mut seq)?
}
Some(WorkspaceSerializationTag::TrimeshShapeContactGeneratorWorkspace) => {
deser::<A, TrimeshShapeContactGeneratorWorkspace>(&mut seq)?
Some(WorkspaceSerializationTag::TriMeshShapeContactGeneratorWorkspace) => {
deser::<A, TriMeshShapeContactGeneratorWorkspace>(&mut seq)?
}
#[cfg(feature = "dim3")]
Some(WorkspaceSerializationTag::PfmPfmContactGeneratorWorkspace) => {

9
src/geometry/contact_generator/heightfield_shape_contact_generator.rs
View File

@@ -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)
}

2
src/geometry/contact_generator/mod.rs
View File

@@ -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;

2
src/geometry/contact_generator/serializable_workspace_tag.rs
View File

@@ -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,

32
src/geometry/contact_generator/trimesh_shape_contact_generator.rs
View File

@@ -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<usize>,
pub struct TriMeshShapeContactGeneratorWorkspace {
interferences: Vec<u32>,
local_aabb2: AABB,
old_interferences: Vec<usize>,
old_interferences: Vec<u32>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
old_manifolds: Vec<ContactManifold>,
}
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,
))
}

14
src/geometry/mod.rs
View File

@@ -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;

2
src/geometry/narrow_phase.rs
View File

@@ -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;

8
src/geometry/proximity_detector/ball_ball_proximity_detector.rs
View File

@@ -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);

2
src/geometry/proximity_detector/mod.rs
View File

@@ -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;

6
src/geometry/proximity_detector/proximity_detector.rs
View File

@@ -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<SimdFloat>,
pub positions2: &'a Isometry<SimdFloat>,
pub positions1: &'a Isometry<SimdReal>,
pub positions2: &'a Isometry<SimdReal>,
pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>],
}

10
src/geometry/proximity_detector/proximity_dispatcher.rs
View File

@@ -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<Box<dyn Any + Send + Sync>>) {
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);

22
src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs
View File

@@ -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<usize>,
pub struct TriMeshShapeProximityDetectorWorkspace {
interferences: Vec<u32>,
local_aabb2: AABB,
old_interferences: Vec<usize>,
old_interferences: Vec<u32>,
}
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;

92
src/geometry/round_cylinder.rs
View File

@@ -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<f32>) -> Point<f32> {
self.local_support_point_toward(&Unit::new_normalize(*dir))
}
fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
self.cylinder.local_support_point_toward(dir) + **dir * self.border_radius
}
fn support_point(&self, transform: &Isometry<f32>, dir: &Vector<f32>) -> Point<f32> {
let local_dir = transform.inverse_transform_vector(dir);
transform * self.local_support_point(&local_dir)
}
fn support_point_toward(
&self,
transform: &Isometry<f32>,
dir: &Unit<Vector<f32>>,
) -> Point<f32> {
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<RayIntersection> {
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<f32>, 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<f32>,
) -> (PointProjection, FeatureId) {
(self.project_local_point(point, false), FeatureId::Unknown)
}
}

393
src/geometry/shape.rs
View File

@@ -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<f32>) -> 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<Box<AnyShape>>> {
// 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<f32>) -> 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<f32>) -> 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<f32>) -> 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<f32>) -> 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<f32>) -> 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<f32>) -> 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<f32>) -> 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<f32>) -> 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<f32>) -> 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<f32>) -> 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<f32>) -> 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,
))
}
}

203
src/geometry/trimesh.rs
View File

@@ -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<usize>,
aabb: AABB,
vertices: Vec<Point<f32>>,
indices: Vec<Point3<u32>>,
}
impl Trimesh {
/// Creates a new triangle mesh from a vertex buffer and an index buffer.
pub fn new(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> 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<f32>) -> AABB {
self.aabb.transform_by(pos)
}
pub(crate) fn waabbs(&self) -> &WQuadtree<usize> {
&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<Item = Triangle> + '_ {
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<f32>] {
&self.vertices[..]
}
/// The index buffer of this mesh.
pub fn indices(&self) -> &[Point3<u32>] {
&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<f32>, _solid: bool) -> PointProjection {
// TODO
unimplemented!()
}
fn project_local_point_and_get_feature(
&self,
_pt: &Point<f32>,
) -> (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<RayIntersection> {
// TODO
None
}
fn intersects_ray(&self, _m: &Isometry<f32>, _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<RayIntersection> {
// FIXME: do a best-first search.
let mut intersections = Vec::new();
self.wquadtree.cast_ray(&ray, max_toi, &mut intersections);
let mut best: Option<RayIntersection> = 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<HeightField> for Trimesh {
fn from(heightfield: HeightField) -> Self {
let (vtx, idx) = heightfield.to_trimesh();
Trimesh::new(vtx, idx)
}
}
#[cfg(feature = "dim3")]
impl From<Cuboid> for Trimesh {
fn from(cuboid: Cuboid) -> Self {
let (vtx, idx) = cuboid.to_trimesh();
Trimesh::new(vtx, idx)
}
}

217
src/geometry/waabb.rs
View File

@@ -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<SimdFloat>,
pub dir: Vector<SimdFloat>,
}
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<SimdFloat>,
pub maxs: Point<SimdFloat>,
}
#[cfg(feature = "serde-serialize")]
impl serde::Serialize for WAABB {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
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<D>(deserializer: D) -> Result<Self, D::Error>
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<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
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),
}
}
}

587
src/geometry/wquadtree.rs
View File

@@ -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<T> {
node: NodeIndex,
data: T, // The collider data. TODO: only set the collider generation here?
}
impl<T: IndexedData> WQuadtreeProxy<T> {
fn invalid() -> Self {
Self {
node: NodeIndex::invalid(),
data: T::default(),
}
}
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone, Debug)]
pub struct WQuadtree<T> {
nodes: Vec<WQuadtreeNode>,
dirty_nodes: VecDeque<u32>,
proxies: Vec<WQuadtreeProxy<T>>,
}
// FIXME: this should be generic too.
impl WQuadtree<ColliderHandle> {
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<T: IndexedData> WQuadtree<T> {
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<Item = (T, AABB)>,
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 lowest variance.
// Therefore variance computation is not needed in 2D because we only have 2 dimension
// to split in the first place.
let mut center = Point::origin();
#[cfg(feature = "dim3")]
let mut variance = Vector::zeros();
let denom = 1.0 / (indices.len() as f32);
for i in &*indices {
let coords = aabbs[*i].center().coords;
center += coords * denom;
#[cfg(feature = "dim3")]
{
variance += coords.component_mul(&coords) * denom;
}
}
#[cfg(feature = "dim3")]
{
variance = variance - center.coords.component_mul(&center.coords);
}
// Find the axis with minimum variance. This is the axis along
// which we are **not** subdividing our set.
#[allow(unused_mut)] // Does not need to be mutable in 2D.
let mut subdiv_dims = [0, 1];
#[cfg(feature = "dim3")]
{
let min = variance.imin();
subdiv_dims[0] = (min + 1) % 3;
subdiv_dims[1] = (min + 2) % 3;
}
// Split the set along the two subdiv_dims dimensions.
// TODO: should we split wrt. the median instead of the average?
// TODO: we should ensure each subslice contains at least 4 elements each (or less if
// indices has less than 16 elements in the first place.
let (left, right) = split_indices_wrt_dim(indices, &aabbs, &center, subdiv_dims[0]);
let (left_bottom, left_top) = split_indices_wrt_dim(left, &aabbs, &center, subdiv_dims[1]);
let (right_bottom, right_top) =
split_indices_wrt_dim(right, &aabbs, &center, subdiv_dims[1]);
// println!(
// "Recursing on children: {}, {}, {}, {}",
// left_bottom.len(),
// left_top.len(),
// right_bottom.len(),
// right_top.len()
// );
let node = WQuadtreeNode {
waabb: WAABB::new_invalid(),
children: [0; 4], // Will be set after the recursive call
parent,
leaf: false,
dirty: false,
};
let id = self.nodes.len() as u32;
self.nodes.push(node);
// Recurse!
let a = self.do_recurse_build(left_bottom, aabbs, NodeIndex::new(id, 0), dilation_factor);
let b = self.do_recurse_build(left_top, aabbs, NodeIndex::new(id, 1), dilation_factor);
let c = self.do_recurse_build(right_bottom, aabbs, NodeIndex::new(id, 2), dilation_factor);
let d = self.do_recurse_build(right_top, aabbs, NodeIndex::new(id, 3), dilation_factor);
// Now we know the indices of the grand-nodes.
self.nodes[id as usize].children = [a.0, b.0, c.0, d.0];
self.nodes[id as usize].waabb = WAABB::from([a.1, b.1, c.1, d.1]);
self.nodes[id as usize]
.waabb
.dilate_by_factor(SimdFloat::splat(dilation_factor));
// TODO: will this chain of .merged be properly optimized?
let my_aabb = a.1.merged(&b.1).merged(&c.1).merged(&d.1);
(id, my_aabb)
}
// FIXME: implement a visitor pattern to merge intersect_aabb
// and intersect_ray into a single method.
pub fn intersect_aabb(&self, aabb: &AABB, out: &mut Vec<T>) {
if self.nodes.is_empty() {
return;
}
// Special case for the root.
let mut stack = vec![0u32];
let waabb = WAABB::splat(*aabb);
while let Some(inode) = stack.pop() {
let node = self.nodes[inode as usize];
let intersections = node.waabb.intersects(&waabb);
let bitmask = intersections.bitmask();
for ii in 0..SIMD_WIDTH {
if (bitmask & (1 << ii)) != 0 {
if node.leaf {
// We found a leaf!
// Unfortunately, invalid AABBs return a intersection as well.
if let Some(proxy) = self.proxies.get(node.children[ii] as usize) {
out.push(proxy.data);
}
} else {
// Internal node, visit the child.
// Unfortunately, we have this check because invalid AABBs
// return a intersection as well.
if node.children[ii] as usize <= self.nodes.len() {
stack.push(node.children[ii]);
}
}
}
}
}
}
pub fn cast_ray(&self, ray: &Ray, max_toi: f32, out: &mut Vec<T>) {
if self.nodes.is_empty() {
return;
}
// Special case for the root.
let mut stack = vec![0u32];
let wray = WRay::splat(*ray);
let wmax_toi = SimdFloat::splat(max_toi);
while let Some(inode) = stack.pop() {
let node = self.nodes[inode as usize];
let hits = node.waabb.intersects_ray(&wray, wmax_toi);
let bitmask = hits.bitmask();
for ii in 0..SIMD_WIDTH {
if (bitmask & (1 << ii)) != 0 {
if node.leaf {
// We found a leaf!
// Unfortunately, invalid AABBs return a hit as well.
if let Some(proxy) = self.proxies.get(node.children[ii] as usize) {
out.push(proxy.data);
}
} else {
// Internal node, visit the child.
// Un fortunately, we have this check because invalid AABBs
// return a hit as well.
if node.children[ii] as usize <= self.nodes.len() {
stack.push(node.children[ii]);
}
}
}
}
}
}
}
#[allow(dead_code)]
struct WQuadtreeIncrementalBuilderStep {
range: Range<usize>,
parent: NodeIndex,
}
#[allow(dead_code)]
struct WQuadtreeIncrementalBuilder<T> {
quadtree: WQuadtree<T>,
to_insert: Vec<WQuadtreeIncrementalBuilderStep>,
aabbs: Vec<AABB>,
indices: Vec<usize>,
}
#[allow(dead_code)]
impl<T: IndexedData> WQuadtreeIncrementalBuilder<T> {
pub fn new() -> Self {
Self {
quadtree: WQuadtree::new(),
to_insert: Vec::new(),
aabbs: Vec::new(),
indices: Vec::new(),
}
}
pub fn update_single_depth(&mut self) {
if let Some(to_insert) = self.to_insert.pop() {
let indices = &mut self.indices[to_insert.range];
// Leaf case.
if indices.len() <= 4 {
let id = self.quadtree.nodes.len();
let mut 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() {
aabb.merge(&self.aabbs[*id]);
leaf_aabbs[k] = self.aabbs[*id];
proxy_ids[k] = *id as u32;
}
let node = WQuadtreeNode {
waabb: WAABB::from(leaf_aabbs),
children: proxy_ids,
parent: to_insert.parent,
leaf: true,
dirty: false,
};
self.quadtree.nodes[to_insert.parent.index as usize].children
[to_insert.parent.lane as usize] = id as u32;
self.quadtree.nodes[to_insert.parent.index as usize]
.waabb
.replace(to_insert.parent.lane as usize, aabb);
self.quadtree.nodes.push(node);
return;
}
// Compute the center and variance along each dimension.
// In 3D we compute the variance to not-subdivide the dimension with lowest variance.
// Therefore variance computation is not needed in 2D because we only have 2 dimension
// to split in the first place.
let mut center = Point::origin();
#[cfg(feature = "dim3")]
let mut variance = Vector::zeros();
let denom = 1.0 / (indices.len() as f32);
let mut aabb = AABB::new_invalid();
for i in &*indices {
let coords = self.aabbs[*i].center().coords;
aabb.merge(&self.aabbs[*i]);
center += coords * denom;
#[cfg(feature = "dim3")]
{
variance += coords.component_mul(&coords) * denom;
}
}
#[cfg(feature = "dim3")]
{
variance = variance - center.coords.component_mul(&center.coords);
}
// Find the axis with minimum variance. This is the axis along
// which we are **not** subdividing our set.
#[allow(unused_mut)] // Does not need to be mutable in 2D.
let mut subdiv_dims = [0, 1];
#[cfg(feature = "dim3")]
{
let min = variance.imin();
subdiv_dims[0] = (min + 1) % 3;
subdiv_dims[1] = (min + 2) % 3;
}
// Split the set along the two subdiv_dims dimensions.
// TODO: should we split wrt. the median instead of the average?
// TODO: we should ensure each subslice contains at least 4 elements each (or less if
// indices has less than 16 elements in the first place.
let (left, right) =
split_indices_wrt_dim(indices, &self.aabbs, &center, subdiv_dims[0]);
let (left_bottom, left_top) =
split_indices_wrt_dim(left, &self.aabbs, &center, subdiv_dims[1]);
let (right_bottom, right_top) =
split_indices_wrt_dim(right, &self.aabbs, &center, subdiv_dims[1]);
let node = WQuadtreeNode {
waabb: WAABB::new_invalid(),
children: [0; 4], // Will be set after the recursive call
parent: to_insert.parent,
leaf: false,
dirty: false,
};
let id = self.quadtree.nodes.len() as u32;
self.quadtree.nodes.push(node);
// Recurse!
let a = left_bottom.len();
let b = a + left_top.len();
let c = b + right_bottom.len();
let d = c + right_top.len();
self.to_insert.push(WQuadtreeIncrementalBuilderStep {
range: 0..a,
parent: NodeIndex::new(id, 0),
});
self.to_insert.push(WQuadtreeIncrementalBuilderStep {
range: a..b,
parent: NodeIndex::new(id, 1),
});
self.to_insert.push(WQuadtreeIncrementalBuilderStep {
range: b..c,
parent: NodeIndex::new(id, 2),
});
self.to_insert.push(WQuadtreeIncrementalBuilderStep {
range: c..d,
parent: NodeIndex::new(id, 3),
});
self.quadtree.nodes[to_insert.parent.index as usize].children
[to_insert.parent.lane as usize] = id as u32;
self.quadtree.nodes[to_insert.parent.index as usize]
.waabb
.replace(to_insert.parent.lane as usize, aabb);
}
}
}
fn split_indices_wrt_dim<'a>(
indices: &'a mut [usize],
aabbs: &[AABB],
split_point: &Point<f32>,
dim: usize,
) -> (&'a mut [usize], &'a mut [usize]) {
let mut icurr = 0;
let mut ilast = indices.len();
// The loop condition we can just do 0..indices.len()
// instead of the test icurr < ilast because we know
// we will iterate exactly once per index.
for _ in 0..indices.len() {
let i = indices[icurr];
let center = aabbs[i].center();
if center[dim] > split_point[dim] {
ilast -= 1;
indices.swap(icurr, ilast);
} else {
icurr += 1;
}
}
if icurr == 0 || icurr == indices.len() {
// We don't want to return one empty set. But
// this can happen if all the coordinates along the
// given dimension are equal.
// In this is the case, we just split in the middle.
let half = indices.len() / 2;
indices.split_at_mut(half)
} else {
indices.split_at_mut(icurr)
}
}
#[cfg(test)]
mod test {
use crate::geometry::{WQuadtree, AABB};
use crate::math::{Point, Vector};
#[test]
fn multiple_identical_AABB_stack_overflow() {
// A stack overflow was caused during the construction of the
// WAABB tree with more than four AABB with the same center.
let aabb = AABB::new(Point::origin(), Vector::repeat(1.0).into());
for k in 0..20 {
let mut tree = WQuadtree::new();
tree.clear_and_rebuild((0..k).map(|i| (i, aabb)), 0.0);
}
}
}