diff options
| author | Crozet Sébastien <developer@crozet.re> | 2020-12-17 18:37:16 +0100 |
|---|---|---|
| committer | Crozet Sébastien <developer@crozet.re> | 2020-12-29 11:31:59 +0100 |
| commit | 8fe2df126a279a435cc544b150aadf8f7b757868 (patch) | |
| tree | 5e574a98190b393d3d54af8922146d5078058824 | |
| parent | 29717c2887b2db39faf9c25053730b661dc5da2b (diff) | |
| download | rapier-8fe2df126a279a435cc544b150aadf8f7b757868.tar.gz rapier-8fe2df126a279a435cc544b150aadf8f7b757868.tar.bz2 rapier-8fe2df126a279a435cc544b150aadf8f7b757868.zip | |
Remove some irrelevant code.
26 files changed, 87 insertions, 472 deletions
@@ -10,8 +10,10 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark #nphysics2d = { path = "../nphysics/build/nphysics2d" } #nphysics3d = { path = "../nphysics/build/nphysics3d" } #kiss3d = { path = "../kiss3d" } -eagl2d = { path = "../eagl/build/eagl2d" } -eagl3d = { path = "../eagl/build/eagl3d" } +#cdl2d = { path = "../cdl/build/cdl2d" } +#cdl3d = { path = "../cdl/build/cdl3d" } +cdl2d = { git = "https://github.com/sebcrozet/cdl.git" } +cdl3d = { git = "https://github.com/sebcrozet/cdl.git" } [profile.release] #debug = true diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml index c20833c..3bbb8f0 100644 --- a/build/rapier2d/Cargo.toml +++ b/build/rapier2d/Cargo.toml @@ -21,8 +21,8 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "eagl2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] -enhanced-determinism = [ "simba/libm_force", "eagl2d/enhanced-determinism", "indexmap" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "cdl2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +enhanced-determinism = [ "simba/libm_force", "cdl2d/enhanced-determinism", "indexmap" ] [lib] name = "rapier2d" @@ -35,7 +35,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.23" -eagl2d = "0.1" +cdl2d = "0.1" simba = "0.3" approx = "0.4" rayon = { version = "1", optional = true } diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml index a9abef8..d2a1fae 100644 --- a/build/rapier3d/Cargo.toml +++ b/build/rapier3d/Cargo.toml @@ -15,14 +15,14 @@ edition = "2018" default = [ "dim3" ] dim3 = [ ] parallel = [ "rayon" ] -simd-stable = [ "eagl3d/simd-stable", "simba/wide", "simd-is-enabled" ] -simd-nightly = [ "eagl3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] +simd-stable = [ "cdl3d/simd-stable", "simba/wide", "simd-is-enabled" ] +simd-nightly = [ "cdl3d/simd-nightly", "simba/packed_simd", "simd-is-enabled" ] # Do not enable this feature directly. It is automatically # enabled with the "simd-stable" or "simd-nightly" feature. simd-is-enabled = [ ] wasm-bindgen = [ "instant/wasm-bindgen" ] -serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "eagl3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] -enhanced-determinism = [ "simba/libm_force", "eagl3d/enhanced-determinism" ] +serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "cdl3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +enhanced-determinism = [ "simba/libm_force", "cdl3d/enhanced-determinism" ] [lib] name = "rapier3d" @@ -35,7 +35,7 @@ vec_map = "0.8" instant = { version = "0.1", features = [ "now" ]} num-traits = "0.2" nalgebra = "0.23" -eagl3d = "0.1" +cdl3d = "0.1" simba = "0.3" approx = "0.4" rayon = { version = "1", optional = true } diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml index 23be92e..63d1136 100644 --- a/build/rapier_testbed2d/Cargo.toml +++ b/build/rapier_testbed2d/Cargo.toml @@ -31,7 +31,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" num_cpus = { version = "1", optional = true } wrapped2d = { version = "0.4", optional = true } -eagl2d = "0.1" +cdl2d = "0.1" ncollide2d = "0.26" nphysics2d = { version = "0.18", optional = true } crossbeam = "0.8" diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml index 2820a7c..830ceab 100644 --- a/build/rapier_testbed3d/Cargo.toml +++ b/build/rapier_testbed3d/Cargo.toml @@ -30,7 +30,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]} bitflags = "1" glam = { version = "0.10", optional = true } num_cpus = { version = "1", optional = true } -eagl3d = "0.1" +cdl3d = "0.1" ncollide3d = "0.26" nphysics3d = { version = "0.18", optional = true } physx = { version = "0.8", optional = true } diff --git a/src/data/arena.rs b/src/data/arena.rs index 3a24cd1..8a694dc 100644 --- a/src/data/arena.rs +++ b/src/data/arena.rs @@ -3,7 +3,7 @@ //! See https://github.com/fitzgen/generational-arena/blob/master/src/lib.rs. //! This has been modified to have a fully deterministic deserialization (including for the order of //! Index attribution after a deserialization of the arena. -use eagl::partitioning::IndexedData; +use cdl::partitioning::IndexedData; use std::cmp; use std::iter::{self, Extend, FromIterator, FusedIterator}; use std::mem; diff --git a/src/data/mod.rs b/src/data/mod.rs index eb0a229..1c53db4 100644 --- a/src/data/mod.rs +++ b/src/data/mod.rs @@ -1,7 +1,7 @@ //! Data structures modified with guaranteed deterministic behavior after deserialization. pub use self::coarena::Coarena; -pub use eagl::utils::MaybeSerializableData; +pub use cdl::utils::MaybeSerializableData; pub mod arena; mod coarena; diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs index 76e9de2..c2c36ef 100644 --- a/src/dynamics/mod.rs +++ b/src/dynamics/mod.rs @@ -9,7 +9,7 @@ pub use self::joint::{ }; pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodySet}; -pub use eagl::shape::MassProperties; +pub use cdl::shape::MassProperties; // #[cfg(not(feature = "parallel"))] pub(crate) use self::joint::JointGraphEdge; pub(crate) use self::rigid_body::RigidBodyChanges; diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs index ce42da8..6584ea2 100644 --- a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -13,7 +13,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix6, Vector6, U3}; #[cfg(feature = "dim2")] use { - eagl::utils::SdpMatrix3, + cdl::utils::SdpMatrix3, na::{Matrix3, Vector3}, }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index a2c7c2c..9e39bd6 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -8,7 +8,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] use { - eagl::utils::SdpMatrix2, + cdl::utils::SdpMatrix2, na::{Matrix2, Vector2}, }; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs index 86e0c78..dadf5da 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -12,7 +12,7 @@ use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; #[cfg(feature = "dim2")] use { - eagl::utils::SdpMatrix2, + cdl::utils::SdpMatrix2, na::{Matrix2, Vector2}, }; diff --git a/src/geometry/ball.rs b/src/geometry/ball.rs deleted file mode 100644 index 2d9f7be..0000000 --- a/src/geometry/ball.rs +++ /dev/null @@ -1,16 +0,0 @@ -#[cfg(feature = "simd-is-enabled")] -use crate::math::{Point, SimdReal}; - -#[cfg(feature = "simd-is-enabled")] -#[derive(Copy, Clone, Debug)] -pub(crate) struct WBall { - pub center: Point<SimdReal>, - pub radius: SimdReal, -} - -#[cfg(feature = "simd-is-enabled")] -impl WBall { - pub fn new(center: Point<SimdReal>, radius: SimdReal) -> Self { - WBall { center, radius } - } -} diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs index 4242d77..dbcc271 100644 --- a/src/geometry/broad_phase_multi_sap.rs +++ b/src/geometry/broad_phase_multi_sap.rs @@ -3,8 +3,8 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider}; use crate::math::{Point, Vector, DIM}; use bit_vec::BitVec; -use eagl::bounding_volume::{BoundingVolume, AABB}; -use eagl::utils::hashmap::HashMap; +use cdl::bounding_volume::{BoundingVolume, AABB}; +use cdl::utils::hashmap::HashMap; use std::cmp::Ordering; use std::ops::{Index, IndexMut}; @@ -477,8 +477,8 @@ pub struct BroadPhase { #[cfg_attr( feature = "serde-serialize", serde( - serialize_with = "eagl::utils::hashmap::serialize_hashmap_capacity", - deserialize_with = "eagl::utils::hashmap::deserialize_hashmap_capacity" + serialize_with = "cdl::utils::hashmap::serialize_hashmap_capacity", + deserialize_with = "cdl::utils::hashmap::deserialize_hashmap_capacity" ) )] reporting: HashMap<(u32, u32), bool>, // Workspace diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index db418a3..46153ac 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -1,13 +1,13 @@ +use crate::cdl::shape::HalfSpace; use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; -use crate::eagl::shape::HalfSpace; use crate::geometry::InteractionGroups; use crate::math::{AngVector, Isometry, Point, Rotation, Vector}; -use eagl::bounding_volume::AABB; -use eagl::shape::{ +use cdl::bounding_volume::AABB; +use cdl::shape::{ Ball, Capsule, Cuboid, HeightField, Segment, Shape, ShapeType, TriMesh, Triangle, }; #[cfg(feature = "dim3")] -use eagl::shape::{Cone, Cylinder, RoundCylinder}; +use cdl::shape::{Cone, Cylinder, RoundCylinder}; use na::Point3; use std::ops::Deref; use std::sync::Arc; diff --git a/src/geometry/contact_pair.rs b/src/geometry/contact_pair.rs index ba5da2a..990e61c 100644 --- a/src/geometry/contact_pair.rs +++ b/src/geometry/contact_pair.rs @@ -1,8 +1,8 @@ use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet}; use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold}; use crate::math::{Isometry, Point, Vector}; -use eagl::query::ContactManifoldsWorkspace; -use eagl::utils::MaybeSerializableData; +use cdl::query::ContactManifoldsWorkspace; +use cdl::utils::MaybeSerializableData; #[cfg(feature = "simd-is-enabled")] use { crate::math::{SimdReal, SIMD_WIDTH}, diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs index efb0cd4..2d0b5d9 100644 --- a/src/geometry/mod.rs +++ b/src/geometry/mod.rs @@ -10,39 +10,38 @@ pub use self::interaction_graph::{ ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex, }; pub use self::narrow_phase::NarrowPhase; -pub use self::polygon::Polygon; -pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; +pub use self::pair_filter::{ContactPairFilter, PairFilterContext, ProximityPairFilter}; -pub use eagl::query::{KinematicsCategory, TrackedContact}; +pub use cdl::query::{KinematicsCategory, TrackedContact}; -pub type Contact = eagl::query::TrackedContact<ContactData>; -pub type ContactManifold = eagl::query::ContactManifold<ContactManifoldData, ContactData>; +pub type Contact = cdl::query::TrackedContact<ContactData>; +pub type ContactManifold = cdl::query::ContactManifold<ContactManifoldData, ContactData>; /// A segment shape. -pub type Segment = eagl::shape::Segment; +pub type Segment = cdl::shape::Segment; /// A cuboid shape. -pub type Cuboid = eagl::shape::Cuboid; +pub type Cuboid = cdl::shape::Cuboid; /// A triangle shape. -pub type Triangle = eagl::shape::Triangle; +pub type Triangle = cdl::shape::Triangle; /// A ball shape. -pub type Ball = eagl::shape::Ball; +pub type Ball = cdl::shape::Ball; /// A capsule shape. -pub type Capsule = eagl::shape::Capsule; +pub type Capsule = cdl::shape::Capsule; /// A heightfield shape. -pub type HeightField = eagl::shape::HeightField; +pub type HeightField = cdl::shape::HeightField; /// A cylindrical shape. #[cfg(feature = "dim3")] -pub type Cylinder = eagl::shape::Cylinder; +pub type Cylinder = cdl::shape::Cylinder; /// A cone shape. #[cfg(feature = "dim3")] -pub type Cone = eagl::shape::Cone; +pub type Cone = cdl::shape::Cone; /// An axis-aligned bounding box. -pub type AABB = eagl::bounding_volume::AABB; +pub type AABB = cdl::bounding_volume::AABB; /// A ray that can be cast against colliders. -pub type Ray = eagl::query::Ray; +pub type Ray = cdl::query::Ray; /// The intersection between a ray and a collider. -pub type RayIntersection = eagl::query::RayIntersection; +pub type RayIntersection = cdl::query::RayIntersection; /// The the projection of a point on a collider. -pub type PointProjection = eagl::query::PointProjection; +pub type PointProjection = cdl::query::PointProjection; #[derive(Copy, Clone, Hash, Debug)] /// Events occurring when two collision objects start or stop being in contact (or penetration). @@ -81,19 +80,16 @@ impl IntersectionEvent { } } -#[cfg(feature = "simd-is-enabled")] -pub(crate) use self::ball::WBall; pub(crate) use self::broad_phase_multi_sap::{BroadPhasePairEvent, ColliderPair}; pub(crate) use self::collider_set::RemovedCollider; #[cfg(feature = "simd-is-enabled")] pub(crate) use self::contact_pair::WContact; pub(crate) use self::narrow_phase::ContactManifoldIndex; -pub(crate) use eagl::partitioning::WQuadtree; +pub(crate) use cdl::partitioning::WQuadtree; //pub(crate) use self::z_order::z_cmp_floats; pub use self::interaction_groups::InteractionGroups; -pub use eagl::shape::*; +pub use cdl::shape::*; -mod ball; mod broad_phase_multi_sap; mod collider; mod collider_set; @@ -101,8 +97,6 @@ mod collider_set; mod contact_pair; mod interaction_graph; mod narrow_phase; -mod polygon; -pub(crate) mod sat; //mod z_order; mod interaction_groups; -mod user_callbacks; +mod pair_filter; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index d9a4f31..ba60ef4 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -1,26 +1,19 @@ #[cfg(feature = "parallel")] use rayon::prelude::*; +use crate::data::pubsub::Subscription; +use crate::data::Coarena; use crate::dynamics::RigidBodySet; -use eagl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher}; -//#[cfg(feature = "simd-is-enabled")] -//use crate::geometry::{ -// contact_generator::ContactGenerationContextSimd, -// intersection_detector::ProximityDetectionContextSimd, WBall, -//}; use crate::geometry::{ - BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactManifoldData, - ContactPairFilter, IntersectionEvent, PairFilterContext, ProximityPairFilter, RemovedCollider, - SolverFlags, + BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent, + ContactManifoldData, ContactPairFilter, IntersectionEvent, PairFilterContext, + ProximityPairFilter, RemovedCollider, SolverFlags, }; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; -//#[cfg(feature = "simd-is-enabled")] -//use crate::math::{SimdReal, SIMD_WIDTH}; -use crate::data::pubsub::Subscription; -use crate::data::Coarena; use crate::pipeline::EventHandler; +use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher}; use std::collections::HashMap; -//use simba::simd::SimdValue; +use std::sync::Arc; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq, Eq)] @@ -42,14 +35,17 @@ impl ColliderGraphIndices { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] pub struct NarrowPhase { + #[serde(skip, default = "default_query_dispatcher")] + query_dispatcher: Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>>, contact_graph: InteractionGraph<ContactPair>, intersection_graph: InteractionGraph<bool>, graph_indices: Coarena<ColliderGraphIndices>, removed_colliders: Option<Subscription<RemovedCollider>>, - // ball_ball: Vec<usize>, // Workspace: Vec<*mut ContactPair>, - // shape_shape: Vec<usize>, // Workspace: Vec<*mut ContactPair>, - // ball_ball_prox: Vec<usize>, // Workspace: Vec<*mut bool>, - // shape_shape_prox: Vec<usize>, // Workspace: Vec<*mut bool>, +} + +fn default_query_dispatcher() -> Arc<dyn PersistentQueryDispatcher<ContactManifoldData, ContactData>> +{ + Arc::new(DefaultQueryDispatcher) } pub(crate) type ContactManifoldIndex = usize; @@ -57,15 +53,20 @@ pub(crate) type ContactManifoldIndex = usize; impl NarrowPhase { /// Creates a new empty narrow-phase. pub fn new() -> Self { + Self::with_query_dispatcher(DefaultQueryDispatcher) + } + + /// Creates a new empty narrow-phase with a custom query dispatcher. + pub fn with_query_dispatcher<D>(d: D) -> Self + where + D: 'static + PersistentQueryDispatcher<ContactManifoldData, ContactData>, + { Self { + query_dispatcher: Arc::new(d), contact_graph: InteractionGraph::new(), intersection_graph: InteractionGraph::new(), graph_indices: Coarena::new(), removed_colliders: None, - // ball_ball: Vec::new(), - // shape_shape: Vec::new(), - // ball_ball_prox: Vec::new(), - // shape_shape_prox: Vec::new(), } } @@ -391,6 +392,7 @@ impl NarrowPhase { events: &dyn EventHandler, ) { let nodes = &self.intersection_graph.graph.nodes; + let query_dispatcher = &*self.query_dispatcher; par_iter_mut!(&mut self.intersection_graph.graph.edges).for_each(|edge| { let handle1 = nodes[edge.source().index()].weight; let handle2 = nodes[edge.target().index()].weight; @@ -434,9 +436,9 @@ impl NarrowPhase { } let pos12 = co1.position().inverse() * co2.position(); - let dispatcher = DefaultQueryDispatcher; - if let Ok(intersection) = dispatcher.intersection_test(&pos12, co1.shape(), co2.shape()) + if let Ok(intersection) = + query_dispatcher.intersection_test(&pos12, co1.shape(), co2.shape()) { if intersection != edge.weight { edge.weight = intersection; @@ -458,6 +460,8 @@ impl NarrowPhase { pair_filter: Option<&dyn ContactPairFilter>, events: &dyn EventHandler, ) { + let query_dispatcher = &*self.query_dispatcher; + par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { let pair = &mut edge.weight; let co1 = &colliders[pair.pair.collider1]; @@ -507,9 +511,8 @@ impl NarrowPhase { solver_flags.remove(SolverFlags::COMPUTE_IMPULSES); } - let dispatcher = DefaultQueryDispatcher; let pos12 = co1.position().inverse() * co2.position(); - dispatcher.contact_manifolds( + query_dispatcher.contact_manifolds( &pos12, co1.shape(), co2.shape(), diff --git a/src/geometry/user_callbacks.rs b/src/geometry/pair_filter.rs index 629f707..629f707 100644 --- a/src/geometry/user_callbacks.rs +++ b/src/geometry/pair_filter.rs diff --git a/src/geometry/polygon.rs b/src/geometry/polygon.rs deleted file mode 100644 index 15cf005..0000000 --- a/src/geometry/polygon.rs +++ /dev/null @@ -1,78 +0,0 @@ -#![allow(dead_code)] // TODO: remove this once we support polygons. - -use crate::math::{Isometry, Point, Vector}; -use eagl::bounding_volume::AABB; - -#[derive(Clone)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A convex planar polygon. -pub struct Polygon { - pub(crate) vertices: Vec<Point<f32>>, - pub(crate) normals: Vec<Vector<f32>>, -} - -impl Polygon { - /// Builds a new polygon from a set of vertices and normals. - /// - /// The vertices must be ordered in such a way that two consecutive - /// vertices determines an edge of the polygon. For example `vertices[0], vertices[1]` - /// is an edge, `vertices[1], vertices[2]` is the next edge, etc. The last edge will - /// be `vertices[vertices.len() - 1], vertices[0]`. - /// The vertices must be given in counter-clockwise order. - /// The vertices must form a convex polygon. - /// - /// One normal must be provided per edge and mut point towards the outside of the polygon. - pub fn new(vertices: Vec<Point<f32>>, normals: Vec<Vector<f32>>) -> Self { - Self { vertices, normals } - } - - /// Compute the axis-aligned bounding box of the polygon. - pub fn aabb(&self, pos: &Isometry<f32>) -> AABB { - let p0 = pos * self.vertices[0]; - let mut mins = p0; - let mut maxs = p0; - - for pt in &self.vertices[1..] { - let pt = pos * pt; - mins = mins.inf(&pt); - maxs = maxs.sup(&pt); - } - - AABB::new(mins.into(), maxs.into()) - } - - /// The vertices of this polygon. - pub fn vertices(&self) -> &[Point<f32>] { - &self.vertices - } - - pub(crate) fn support_point(&self, dir: &Vector<f32>) -> usize { - let mut best_dot = -f32::MAX; - let mut best_i = 0; - - for (i, pt) in self.vertices.iter().enumerate() { - let dot = pt.coords.dot(&dir); - if dot > best_dot { - best_dot = dot; - best_i = i; - } - } - - best_i - } - - pub(crate) fn support_face(&self, dir: &Vector<f32>) -> usize { - let mut max_dot = -f32::MAX; - let mut max_dot_i = 0; - - for (i, normal) in self.normals.iter().enumerate() { - let dot = normal.dot(dir); - if dot > max_dot { - max_dot = dot; - max_dot_i = i; - } - } - - max_dot_i - } -} diff --git a/src/geometry/polygon_intersection.rs b/src/geometry/polygon_intersection.rs deleted file mode 100644 index a2186df..0000000 --- a/src/geometry/polygon_intersection.rs +++ /dev/null @@ -1,263 +0,0 @@ -use na::{Point2, Real}; - -use shape::SegmentPointLocation; -use utils::{self, SegmentsIntersection, TriangleOrientation}; - -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -enum InFlag { - PIn, - QIn, - Unknown, -} - -/// Location of a point on a polyline. -pub enum PolylinePointLocation<N> { - /// Point on a vertex. - OnVertex(usize), - /// Point on an edge. - OnEdge(usize, usize, [N; 2]), -} - -impl<N: Real> PolylinePointLocation<N> { - /// Computes the point corresponding to this location. - pub fn to_point(&self, pts: &[Point2<N>]) -> Point2<N> { - match self { - PolylinePointLocation::OnVertex(i) => pts[*i], - PolylinePointLocation::OnEdge(i1, i2, bcoords) => { - pts[*i1] * bcoords[0] + pts[*i2].coords * bcoords[1] - } - } - } - - fn from_segment_point_location(a: usize, b: usize, loc: SegmentPointLocation<N>) -> Self { - match loc { - SegmentPointLocation::OnVertex(0) => PolylinePointLocation::OnVertex(a), - SegmentPointLocation::OnVertex(1) => PolylinePointLocation::OnVertex(b), - SegmentPointLocation::OnVertex(_) => unreachable!(), - SegmentPointLocation::OnEdge(bcoords) => PolylinePointLocation::OnEdge(a, b, bcoords), - } - } -} - -/// Computes the intersection points of two convex polygons. -/// -/// The resulting polygon is output vertex-by-vertex to the `out` closure. -pub fn convex_polygons_intersection_points<N: Real>( - poly1: &[Point2<N>], - poly2: &[Point2<N>], - out: &mut Vec<Point2<N>>, -) { - convex_polygons_intersection(poly1, poly2, |loc1, loc2| { - if let Some(loc1) = loc1 { - out.push(loc1.to_point(poly1)) - } else if let Some(loc2) = loc2 { - out.push(loc2.to_point(poly2)) - } - }) -} - -/// Computes the intersection of two convex polygons. -/// -/// The resulting polygon is output vertex-by-vertex to the `out` closure. -pub fn convex_polygons_intersection<N: Real>( - poly1: &[Point2<N>], - poly2: &[Point2<N>], - mut out: impl FnMut(Option<PolylinePointLocation<N>>, Option<PolylinePointLocation<N>>), -) { - // FIXME: this does not handle correctly the case where the - // first triangle of the polygon is degenerate. - let rev1 = poly1.len() > 2 - && utils::triangle_orientation(&poly1[0], &poly1[1], &poly1[2]) - == TriangleOrientation::Clockwise; - let rev2 = poly2.len() > 2 - && utils::triangle_orientation(&poly2[0], &poly2[1], &poly2[2]) - == TriangleOrientation::Clockwise; - - // println!("rev1: {}, rev2: {}", rev1, rev2); - - let n = poly1.len(); - let m = poly2.len(); - - let mut a = 0; - let mut b = 0; - let mut aa = 0; - let mut ba = 0; - let mut inflag = InFlag::Unknown; - let mut first_point_found = false; - - // Quit when both adv. indices have cycled, or one has cycled twice. - while (aa < n || ba < m) && aa < 2 * n && ba < 2 * m { - let (a1, a2) = if rev1 { - ((n - a) % n, n - a - 1) - } else { - ((a + n - 1) % n, a) - }; - - let (b1, b2) = if rev2 { - ((m - b) % m, m - b - 1) - } else { - ((b + m - 1) % m, b) - }; - - // println!("Current indices: ({}, {}), ({}, {})", a1, a2, b1, b2); - - let dir_edge1 = poly1[a2] - poly1[a1]; - let dir_edge2 = poly2[b2] - poly2[b1]; - - let cross = utils::triangle_orientation( - &Point2::origin(), - &Point2::from_coordinates(dir_edge1), - &Point2::from_coordinates(dir_edge2), - ); - let aHB = utils::triangle_orientation(&poly2[b1], &poly2[b2], &poly1[a2]); - let bHA = utils::triangle_orientation(&poly1[a1], &poly1[a2], &poly2[b2]); - - // If edge1 & edge2 intersect, update inflag. - if let Some(inter) = - utils::segments_intersection(&poly1[a1], &poly1[a2], &poly2[b1], &poly2[b2]) - { - match inter { - SegmentsIntersection::Point { loc1, loc2 } => { - let loc1 = PolylinePointLocation::from_segment_point_location(a1, a2, loc1); - let loc2 = PolylinePointLocation::from_segment_point_location(b1, b2, loc2); - out(Some(loc1), Some(loc2)); - - if inflag == InFlag::Unknown && !first_point_found { - // This is the first point. - aa = 0; - ba = 0; - first_point_found = true; - } - - // Update inflag. - if aHB == TriangleOrientation::Counterclockwise { - inflag = InFlag::PIn; - } else if bHA == TriangleOrientation::Counterclockwise { - inflag = InFlag::QIn; - } - } - SegmentsIntersection::Segment { - first_loc1, - first_loc2, - second_loc1, - second_loc2, - } => { - // Special case: edge1 & edge2 overlap and oppositely oriented. - if dir_edge1.dot(&dir_edge2) < N::zero() { - let loc1 = - PolylinePointLocation::from_segment_point_location(a1, a2, first_loc1); - let loc2 = - PolylinePointLoca |
