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

Merge branch 'master' into infinite_fall_memory

Browse Source 
Fix merge conflict resulting from "axii" spelling correction
This commit is contained in:
Robert Hrusecky
2020-10-29 18:09:41 -05:00
parent dd8e25bc47 4b8242b9c2
commit bcec54ef31
76 changed files with 2756 additions and 819 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/dynamics/mass_properties.rs
View File

@@ -91,7 +91,7 @@ impl MassProperties {
}
#[cfg(feature = "dim3")]
/// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii.
/// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes.
pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> {
let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e);
self.principal_inertia_local_frame.to_rotation_matrix()
@@ -103,7 +103,7 @@ impl MassProperties {
}
#[cfg(feature = "dim3")]
/// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
/// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes.
pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
self.principal_inertia_local_frame.to_rotation_matrix()

23
src/dynamics/mass_properties_capsule.rs
View File

@@ -1,30 +1,9 @@
use crate::dynamics::MassProperties;
#[cfg(feature = "dim3")]
use crate::geometry::Capsule;
use crate::math::{Point, PrincipalAngularInertia, Vector};
use crate::math::Point;
impl MassProperties {
fn cylinder_y_volume_unit_inertia(
half_height: f32,
radius: f32,
) -> (f32, PrincipalAngularInertia<f32>) {
#[cfg(feature = "dim2")]
{
Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
}
#[cfg(feature = "dim3")]
{
let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
let sq_radius = radius * radius;
let sq_height = half_height * half_height * 4.0;
let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
(volume, inertia)
}
}
pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
let half_height = (b - a).norm() / 2.0;
let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);

29
src/dynamics/mass_properties_cone.rs Normal file
View File

@@ -0,0 +1,29 @@
use crate::dynamics::MassProperties;
use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
impl MassProperties {
pub(crate) fn cone_y_volume_unit_inertia(
half_height: f32,
radius: f32,
) -> (f32, PrincipalAngularInertia<f32>) {
let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0;
let sq_radius = radius * radius;
let sq_height = half_height * half_height * 4.0;
let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0;
let principal = sq_radius * 3.0 / 10.0;
(volume, Vector::new(off_principal, principal, off_principal))
}
pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self {
let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius);
let cyl_mass = cyl_vol * density;
Self::with_principal_inertia_frame(
Point::new(0.0, -half_height / 2.0, 0.0),
cyl_mass,
cyl_unit_i * cyl_mass,
Rotation::identity(),
)
}
}

40
src/dynamics/mass_properties_cylinder.rs Normal file
View File

@@ -0,0 +1,40 @@
use crate::dynamics::MassProperties;
#[cfg(feature = "dim3")]
use crate::math::{Point, Rotation};
use crate::math::{PrincipalAngularInertia, Vector};
impl MassProperties {
pub(crate) fn cylinder_y_volume_unit_inertia(
half_height: f32,
radius: f32,
) -> (f32, PrincipalAngularInertia<f32>) {
#[cfg(feature = "dim2")]
{
Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
}
#[cfg(feature = "dim3")]
{
let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
let sq_radius = radius * radius;
let sq_height = half_height * half_height * 4.0;
let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
(volume, inertia)
}
}
#[cfg(feature = "dim3")]
pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self {
let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
let cyl_mass = cyl_vol * density;
Self::with_principal_inertia_frame(
Point::origin(),
cyl_mass,
cyl_unit_i * cyl_mass,
Rotation::identity(),
)
}
}

2
src/dynamics/mass_properties_polygon.rs
View File

@@ -1,3 +1,5 @@
#![allow(dead_code)] // TODO: remove this
use crate::dynamics::MassProperties;
use crate::math::Point;

3
src/dynamics/mod.rs
View File

@@ -22,7 +22,10 @@ mod joint;
mod mass_properties;
mod mass_properties_ball;
mod mass_properties_capsule;
#[cfg(feature = "dim3")]
mod mass_properties_cone;
mod mass_properties_cuboid;
mod mass_properties_cylinder;
#[cfg(feature = "dim2")]
mod mass_properties_polygon;
mod rigid_body;

15
src/dynamics/rigid_body.rs
View File

@@ -54,6 +54,8 @@ pub struct RigidBody {
pub(crate) active_set_timestamp: u32,
/// The status of the body, governing how it is affected by external forces.
pub body_status: BodyStatus,
/// User-defined data associated to this rigid-body.
pub user_data: u128,
}
impl Clone for RigidBody {
@@ -90,6 +92,7 @@ impl RigidBody {
active_set_offset: 0,
active_set_timestamp: 0,
body_status: BodyStatus::Dynamic,
user_data: 0,
}
}
@@ -218,6 +221,7 @@ impl RigidBody {
let shift = Translation::from(com.coords);
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
}
pub(crate) fn integrate(&mut self, dt: f32) {
self.position = self.integrate_velocity(dt) * self.position;
}
@@ -334,13 +338,14 @@ impl RigidBody {
}
}
/// A builder for rigid-bodies.
/// A builder for rigid-bodies.
pub struct RigidBodyBuilder {
position: Isometry<f32>,
linvel: Vector<f32>,
angvel: AngVector<f32>,
body_status: BodyStatus,
can_sleep: bool,
user_data: u128,
}
impl RigidBodyBuilder {
@@ -352,6 +357,7 @@ impl RigidBodyBuilder {
angvel: na::zero(),
body_status,
can_sleep: true,
user_data: 0,
}
}
@@ -399,6 +405,12 @@ impl RigidBodyBuilder {
self
}
/// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
pub fn user_data(mut self, data: u128) -> Self {
self.user_data = data;
self
}
/// Sets the initial linear velocity of the rigid-body to be created.
#[cfg(feature = "dim2")]
pub fn linvel(mut self, x: f32, y: f32) -> Self {
@@ -433,6 +445,7 @@ impl RigidBodyBuilder {
rb.linvel = self.linvel;
rb.angvel = self.angvel;
rb.body_status = self.body_status;
rb.user_data = self.user_data;
if !self.can_sleep {
rb.activation.threshold = -1.0;

16
src/geometry/broad_phase_multi_sap.rs
View File

@@ -337,7 +337,7 @@ impl SAPAxis {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
struct SAPRegion {
axii: [SAPAxis; DIM],
axes: [SAPAxis; DIM],
existing_proxies: BitVec,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
to_insert: Vec<usize>, // Workspace
@@ -347,14 +347,14 @@ struct SAPRegion {
impl SAPRegion {
pub fn new(bounds: AABB<f32>) -> Self {
let axii = [
let axes = [
SAPAxis::new(bounds.mins.x, bounds.maxs.x),
SAPAxis::new(bounds.mins.y, bounds.maxs.y),
#[cfg(feature = "dim3")]
SAPAxis::new(bounds.mins.z, bounds.maxs.z),
];
SAPRegion {
axii,
axes,
existing_proxies: BitVec::new(),
to_insert: Vec::new(),
update_count: 0,
@@ -394,14 +394,14 @@ impl SAPRegion {
// Update endpoints.
let mut deleted = 0;
for dim in 0..DIM {
self.axii[dim].update_endpoints(dim, proxies, reporting);
deleted += self.axii[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies);
self.axes[dim].update_endpoints(dim, proxies, reporting);
deleted += self.axes[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies);
}
if deleted > 0 {
self.proxy_count -= deleted;
for dim in 0..DIM {
self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
}
}
@@ -411,9 +411,9 @@ impl SAPRegion {
if !self.to_insert.is_empty() {
// Insert new proxies.
for dim in 1..DIM {
self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None);
self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None);
}
self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
self.to_insert.clear();
// In the rare event that all proxies leave this region in the next step, we need an

86
src/geometry/capsule.rs
View File

@@ -1,18 +1,16 @@
use crate::geometry::AABB;
use crate::geometry::{Ray, RayIntersection, AABB};
use crate::math::{Isometry, Point, Rotation, Vector};
use approx::AbsDiffEq;
use na::Unit;
use ncollide::query::{PointProjection, PointQuery};
use ncollide::shape::{FeatureId, Segment};
use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast};
use ncollide::shape::{FeatureId, Segment, SupportMap};
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A capsule shape defined as a segment with a radius.
/// A capsule shape defined as a round segment.
pub struct Capsule {
/// The first endpoint of the capsule.
pub a: Point<f32>,
/// The second enpdoint of the capsule.
pub b: Point<f32>,
/// The axis and endpoint of the capsule.
pub segment: Segment<f32>,
/// The radius of the capsule.
pub radius: f32,
}
@@ -39,13 +37,14 @@ impl Capsule {
/// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`.
pub fn new(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
Self { a, b, radius }
let segment = Segment::new(a, b);
Self { segment, radius }
}
/// The axis-aligned bounding box of this capsule.
pub fn aabb(&self, pos: &Isometry<f32>) -> AABB {
let a = pos * self.a;
let b = pos * self.b;
let a = pos * self.segment.a;
let b = pos * self.segment.b;
let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius);
let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius);
AABB::new(mins.into(), maxs.into())
@@ -53,7 +52,7 @@ impl Capsule {
/// The height of this capsule.
pub fn height(&self) -> f32 {
(self.b - self.a).norm()
(self.segment.b - self.segment.a).norm()
}
/// The half-height of this capsule.
@@ -63,17 +62,17 @@ impl Capsule {
/// The center of this capsule.
pub fn center(&self) -> Point<f32> {
na::center(&self.a, &self.b)
na::center(&self.segment.a, &self.segment.b)
}
/// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`.
pub fn transform_by(&self, pos: &Isometry<f32>) -> Self {
Self::new(pos * self.a, pos * self.b, self.radius)
Self::new(pos * self.segment.a, pos * self.segment.b, self.radius)
}
/// The rotation `r` such that `r * Y` is collinear with `b - a`.
pub fn rotation_wrt_y(&self) -> Rotation<f32> {
let mut dir = self.b - self.a;
let mut dir = self.segment.b - self.segment.a;
if dir.y < 0.0 {
dir = -dir;
}
@@ -96,24 +95,49 @@ impl Capsule {
}
}
// impl SupportMap<f32> for Capsule {
// fn local_support_point(&self, dir: &Vector) -> Point {
// let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis());
// self.local_support_point_toward(&dir)
// }
//
// fn local_support_point_toward(&self, dir: &Unit<Vector>) -> Point {
// if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) {
// self.a + **dir * self.radius
// } else {
// self.b + **dir * self.radius
// }
// }
// }
impl SupportMap<f32> for Capsule {
fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis());
self.local_support_point_toward(&dir)
}
fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) {
self.segment.a + **dir * self.radius
} else {
self.segment.b + **dir * self.radius
}
}
}
impl RayCast<f32> for Capsule {
fn toi_and_normal_with_ray(
&self,
m: &Isometry<f32>,
ray: &Ray,
max_toi: f32,
solid: bool,
) -> Option<RayIntersection> {
let ls_ray = ray.inverse_transform_by(m);
ncollide::query::ray_intersection_with_support_map_with_params(
&Isometry::identity(),
self,
&mut VoronoiSimplex::new(),
&ls_ray,
max_toi,
solid,
)
.map(|mut res| {
res.normal = m * res.normal;
res
})
}
}
// TODO: this code has been extracted from ncollide and added here
// so we can modify it to fit with our new definition of capsule.
// Wa should find a way to avoid this code duplication.
// We should find a way to avoid this code duplication.
impl PointQuery<f32> for Capsule {
#[inline]
fn project_point(
@@ -122,7 +146,7 @@ impl PointQuery<f32> for Capsule {
pt: &Point<f32>,
solid: bool,
) -> PointProjection<f32> {
let seg = Segment::new(self.a, self.b);
let seg = Segment::new(self.segment.a, self.segment.b);
let proj = seg.project_point(m, pt, solid);
let dproj = *pt - proj.point;

432
src/geometry/collider.rs
View File

@@ -1,145 +1,189 @@
use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
use crate::geometry::{
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
Proximity, Ray, RayIntersection, Triangle, Trimesh,
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph,
InteractionGroups, Proximity, Segment, Shape, ShapeType, Triangle, Trimesh,
};
#[cfg(feature = "dim3")]
use crate::geometry::{Cone, Cylinder, RoundCylinder};
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use na::Point3;
use ncollide::bounding_volume::{HasBoundingVolume, AABB};
use ncollide::query::RayCast;
use num::Zero;
use ncollide::bounding_volume::AABB;
use std::ops::Deref;
use std::sync::Arc;
/// The shape of a collider.
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// An enum grouping all the possible shape of a collider.
pub enum Shape {
/// A ball shape.
Ball(Ball),
/// A convex polygon shape.
Polygon(Polygon),
/// A cuboid shape.
Cuboid(Cuboid),
/// A capsule shape.
Capsule(Capsule),
/// A triangle shape.
Triangle(Triangle),
/// A triangle mesh shape.
Trimesh(Trimesh),
/// A heightfield shape.
HeightField(HeightField),
pub struct ColliderShape(pub Arc<dyn Shape>);
impl Deref for ColliderShape {
type Target = dyn Shape;
fn deref(&self) -> &dyn Shape {
&*self.0
}
}
impl Shape {
/// Gets a reference to the underlying ball shape, if `self` is one.
pub fn as_ball(&self) -> Option<&Ball> {
match self {
Shape::Ball(b) => Some(b),
_ => None,
}
impl ColliderShape {
/// Initialize a ball shape defined by its radius.
pub fn ball(radius: f32) -> Self {
ColliderShape(Arc::new(Ball::new(radius)))
}
/// Gets a reference to the underlying polygon shape, if `self` is one.
pub fn as_polygon(&self) -> Option<&Polygon> {
match self {
Shape::Polygon(p) => Some(p),
_ => None,
}
/// Initialize a cylindrical shape defined by its half-height
/// (along along the y axis) and its radius.
#[cfg(feature = "dim3")]
pub fn cylinder(half_height: f32, radius: f32) -> Self {
ColliderShape(Arc::new(Cylinder::new(half_height, radius)))
}
/// Gets a reference to the underlying cuboid shape, if `self` is one.
pub fn as_cuboid(&self) -> Option<&Cuboid> {
match self {
Shape::Cuboid(c) => Some(c),
_ => None,
}
/// Initialize a rounded cylindrical shape defined by its half-height
/// (along along the y axis), its radius, and its roundedness (the
/// radius of the sphere used for dilating the cylinder).
#[cfg(feature = "dim3")]
pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self {
ColliderShape(Arc::new(RoundCylinder::new(
half_height,
radius,
border_radius,
)))
}
/// Gets a reference to the underlying capsule shape, if `self` is one.
pub fn as_capsule(&self) -> Option<&Capsule> {
match self {
Shape::Capsule(c) => Some(c),
_ => None,
}
/// Initialize a cone shape defined by its half-height
/// (along along the y axis) and its basis radius.
#[cfg(feature = "dim3")]
pub fn cone(half_height: f32, radius: f32) -> Self {
ColliderShape(Arc::new(Cone::new(half_height, radius)))
}
/// Gets a reference to the underlying triangle mesh shape, if `self` is one.
pub fn as_trimesh(&self) -> Option<&Trimesh> {
match self {
Shape::Trimesh(c) => Some(c),
_ => None,
}
/// Initialize a cuboid shape defined by its half-extents.
pub fn cuboid(half_extents: Vector<f32>) -> Self {
ColliderShape(Arc::new(Cuboid::new(half_extents)))
}
/// Gets a reference to the underlying heightfield shape, if `self` is one.
pub fn as_heightfield(&self) -> Option<&HeightField> {
match self {
Shape::HeightField(h) => Some(h),
_ => None,
}
/// Initialize a capsule shape from its endpoints and radius.
pub fn capsule(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
ColliderShape(Arc::new(Capsule::new(a, b, radius)))
}
/// Gets a reference to the underlying triangle shape, if `self` is one.
pub fn as_triangle(&self) -> Option<&Triangle> {
match self {
Shape::Triangle(c) => Some(c),
_ => None,
}
/// Initialize a segment shape from its endpoints.
pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
ColliderShape(Arc::new(Segment::new(a, b)))
}
/// Computes the axis-aligned bounding box of this shape.
pub fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
match self {
Shape::Ball(ball) => ball.bounding_volume(position),
Shape::Polygon(poly) => poly.aabb(position),
Shape::Capsule(caps) => caps.aabb(position),
Shape::Cuboid(cuboid) => cuboid.bounding_volume(position),
Shape::Triangle(triangle) => triangle.bounding_volume(position),
Shape::Trimesh(trimesh) => trimesh.aabb(position),
Shape::HeightField(heightfield) => heightfield.bounding_volume(position),
}
/// Initializes a triangle shape.
pub fn triangle(a: Point<f32>, b: Point<f32>, c: Point<f32>) -> Self {
ColliderShape(Arc::new(Triangle::new(a, b, c)))
}
/// Computes the first intersection point between a ray in this collider.
///
/// Some shapes are not supported yet and will always return `None`.
///
/// # Parameters
/// - `position`: the position of this shape.
/// - `ray`: the ray to cast.
/// - `max_toi`: the maximum time-of-impact that can be reported by this cast. This effectively
/// limits the length of the ray to `ray.dir.norm() * max_toi`. Use `f32::MAX` for an unbounded ray.
pub fn cast_ray(
&self,
position: &Isometry<f32>,
ray: &Ray,
max_toi: f32,
) -> Option<RayIntersection> {
match self {
Shape::Ball(ball) => ball.toi_and_normal_with_ray(position, ray, max_toi, true),
Shape::Polygon(_poly) => None,
Shape::Capsule(caps) => {
let pos = position * caps.transform_wrt_y();
let caps = ncollide::shape::Capsule::new(caps.half_height(), caps.radius);
caps.toi_and_normal_with_ray(&pos, ray, max_toi, true)
/// 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)))
}
/// Initializes an heightfield shape defined by its set of height and a scale
/// factor along each coordinate axis.
#[cfg(feature = "dim2")]
pub fn heightfield(heights: na::DVector<f32>, scale: Vector<f32>) -> Self {
ColliderShape(Arc::new(HeightField::new(heights, scale)))
}
/// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale
/// factor along each coordinate axis.
#[cfg(feature = "dim3")]
pub fn heightfield(heights: na::DMatrix<f32>, scale: Vector<f32>) -> Self {
ColliderShape(Arc::new(HeightField::new(heights, scale)))
}
}
#[cfg(feature = "serde-serialize")]
impl serde::Serialize for ColliderShape {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: serde::Serializer,
{
use crate::serde::ser::SerializeStruct;
if let Some(ser) = self.0.as_serialize() {
let typ = self.0.shape_type();
let mut state = serializer.serialize_struct("ColliderShape", 2)?;
state.serialize_field("tag", &(typ as i32))?;
state.serialize_field("inner", ser)?;
state.end()
} else {
Err(serde::ser::Error::custom(
"Found a non-serializable custom shape.",
))
}
}
}
#[cfg(feature = "serde-serialize")]
impl<'de> serde::Deserialize<'de> for ColliderShape {
fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
where
D: serde::Deserializer<'de>,
{
struct Visitor {};
impl<'de> serde::de::Visitor<'de> for Visitor {
type Value = ColliderShape;
fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result {
write!(formatter, "one shape type tag and the inner shape data")
}
Shape::Cuboid(cuboid) => cuboid.toi_and_normal_with_ray(position, ray, max_toi, true),
#[cfg(feature = "dim2")]
Shape::Triangle(_) | Shape::Trimesh(_) => {
// This is not implemented yet in 2D.
None
}
#[cfg(feature = "dim3")]
Shape::Triangle(triangle) => {
triangle.toi_and_normal_with_ray(position, ray, max_toi, true)
}
#[cfg(feature = "dim3")]
Shape::Trimesh(trimesh) => {
trimesh.toi_and_normal_with_ray(position, ray, max_toi, true)
}
Shape::HeightField(heightfield) => {
heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
fn visit_seq<A>(self, mut seq: A) -> Result<Self::Value, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
use num::cast::FromPrimitive;
let tag: i32 = seq
.next_element()?
.ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
fn deser<'de, A, S: Shape + serde::Deserialize<'de>>(
seq: &mut A,
) -> Result<Arc<dyn Shape>, A::Error>
where
A: serde::de::SeqAccess<'de>,
{
let shape: S = seq.next_element()?.ok_or_else(|| {
serde::de::Error::custom("Failed to deserialize builtin shape.")
})?;
Ok(Arc::new(shape) as Arc<dyn Shape>)
}
let shape = match ShapeType::from_i32(tag) {
Some(ShapeType::Ball) => deser::<A, Ball>(&mut seq)?,
Some(ShapeType::Polygon) => {
unimplemented!()
// let shape: Polygon = seq
// .next_element()?
// .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?;
// Arc::new(shape) as Arc<dyn Shape>
}
Some(ShapeType::Cuboid) => deser::<A, Cuboid>(&mut seq)?,
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::HeightField) => deser::<A, HeightField>(&mut seq)?,
#[cfg(feature = "dim3")]
Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&mut seq)?,
#[cfg(feature = "dim3")]
Some(ShapeType::Cone) => deser::<A, Cone>(&mut seq)?,
#[cfg(feature = "dim3")]
Some(ShapeType::RoundCylinder) => deser::<A, RoundCylinder>(&mut seq)?,
None => {
return Err(serde::de::Error::custom(
"found invalid shape type to deserialize",
))
}
};
Ok(ColliderShape(shape))
}
}
deserializer.deserialize_struct("ColliderShape", &["tag", "inner"], Visitor {})
}
}
@@ -148,7 +192,7 @@ impl Shape {
///
/// To build a new collider, use the `ColliderBuilder` structure.
pub struct Collider {
shape: Shape,
shape: ColliderShape,
density: f32,
is_sensor: bool,
pub(crate) parent: RigidBodyHandle,
@@ -159,9 +203,13 @@ pub struct Collider {
pub friction: f32,
/// The restitution coefficient of this collider.
pub restitution: f32,
pub(crate) collision_groups: InteractionGroups,
pub(crate) solver_groups: InteractionGroups,
pub(crate) contact_graph_index: ColliderGraphIndex,
pub(crate) proximity_graph_index: ColliderGraphIndex,
pub(crate) proxy_index: usize,
/// User-defined data associated to this rigid-body.
pub user_data: u128,
}
impl Clone for Collider {
@@ -209,14 +257,24 @@ impl Collider {
&self.delta
}
/// The collision groups used by this collider.
pub fn collision_groups(&self) -> InteractionGroups {
self.collision_groups
}
/// The solver groups used by this collider.
pub fn solver_groups(&self) -> InteractionGroups {
self.solver_groups
}
/// The density of this collider.
pub fn density(&self) -> f32 {
self.density
}
/// The geometric shape of this collider.
pub fn shape(&self) -> &Shape {
&self.shape
pub fn shape(&self) -> &dyn Shape {
&*self.shape.0
}
/// Compute the axis-aligned bounding box of this collider.
@@ -232,20 +290,7 @@ impl Collider {
/// Compute the local-space mass properties of this collider.
pub fn mass_properties(&self) -> MassProperties {
match &self.shape {
Shape::Ball(ball) => MassProperties::from_ball(self.density, ball.radius),
#[cfg(feature = "dim2")]
Shape::Polygon(p) => MassProperties::from_polygon(self.density, p.vertices()),
#[cfg(feature = "dim3")]
Shape::Polygon(_p) => unimplemented!(),
Shape::Cuboid(c) => MassProperties::from_cuboid(self.density, c.half_extents),
Shape::Capsule(caps) => {
MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius)
}
Shape::Triangle(_) => MassProperties::zero(),
Shape::Trimesh(_) => MassProperties::zero(),
Shape::HeightField(_) => MassProperties::zero(),
}
self.shape.mass_properties(self.density)
}
}
@@ -254,7 +299,7 @@ impl Collider {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct ColliderBuilder {
/// The shape of the collider to be built.
pub shape: Shape,
pub shape: ColliderShape,
/// The density of the collider to be built.
density: Option<f32>,
/// The friction coefficient of the collider to be built.
@@ -265,11 +310,17 @@ pub struct ColliderBuilder {
pub delta: Isometry<f32>,
/// Is this collider a sensor?
pub is_sensor: bool,
/// The user-data of the collider being built.
pub user_data: u128,
/// The collision groups for the collider being built.
pub collision_groups: InteractionGroups,
/// The solver groups for the collider being built.
pub solver_groups: InteractionGroups,
}
impl ColliderBuilder {
/// Initialize a new collider builder with the given shape.
pub fn new(shape: Shape) -> Self {
pub fn new(shape: ColliderShape) -> Self {
Self {
shape,
density: None,
@@ -277,6 +328,9 @@ impl ColliderBuilder {
restitution: 0.0,
delta: Isometry::identity(),
is_sensor: false,
user_data: 0,
collision_groups: InteractionGroups::all(),
solver_groups: InteractionGroups::all(),
}
}
@@ -288,96 +342,93 @@ impl ColliderBuilder {
/// Initialize a new collider builder with a ball shape defined by its radius.
pub fn ball(radius: f32) -> Self {
Self::new(Shape::Ball(Ball::new(radius)))
Self::new(ColliderShape::ball(radius))
}
/// Initialize a new collider builder with a cylindrical shape defined by its half-height
/// (along along the y axis) and its radius.
#[cfg(feature = "dim3")]
pub fn cylinder(half_height: f32, radius: f32) -> Self {
Self::new(ColliderShape::cylinder(half_height, radius))
}
/// Initialize a new collider builder with a rounded cylindrical shape defined by its half-height
/// (along along the y axis), its radius, and its roundedness (the
/// radius of the sphere used for dilating the cylinder).
#[cfg(feature = "dim3")]
pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self {
Self::new(ColliderShape::round_cylinder(
half_height,
radius,
border_radius,
))
}
/// Initialize a new collider builder with a cone shape defined by its half-height
/// (along along the y axis) and its basis radius.
#[cfg(feature = "dim3")]
pub fn cone(half_height: f32, radius: f32) -> Self {
Self::new(ColliderShape::cone(half_height, radius))
}
/// Initialize a new collider builder with a cuboid shape defined by its half-extents.
#[cfg(feature = "dim2")]
pub fn cuboid(hx: f32, hy: f32) -> Self {
let cuboid = Cuboid {
half_extents: Vector::new(hx, hy),
};
Self::new(Shape::Cuboid(cuboid))
/*
use crate::math::Point;
let vertices = vec![
Point::new(hx, -hy),
Point::new(hx, hy),
Point::new(-hx, hy),
Point::new(-hx, -hy),
];
let normals = vec![Vector::x(), Vector::y(), -Vector::x(), -Vector::y()];
let polygon = Polygon::new(vertices, normals);
Self::new(Shape::Polygon(polygon))
*/
Self::new(ColliderShape::cuboid(Vector::new(hx, hy)))
}
/// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
pub fn capsule_x(half_height: f32, radius: f32) -> Self {
let capsule = Capsule::new_x(half_height, radius);
Self::new(Shape::Capsule(capsule))
let p = Point::from(Vector::x() * half_height);
Self::new(ColliderShape::capsule(-p, p, radius))
}
/// Initialize a new collider builder with a capsule shape aligned with the `y` axis.
pub fn capsule_y(half_height: f32, radius: f32) -> Self {
let capsule = Capsule::new_y(half_height, radius);
Self::new(Shape::Capsule(capsule))
let p = Point::from(Vector::y() * half_height);
Self::new(ColliderShape::capsule(-p, p, radius))
}
/// Initialize a new collider builder with a capsule shape aligned with the `z` axis.
#[cfg(feature = "dim3")]
pub fn capsule_z(half_height: f32, radius: f32) -> Self {
let capsule = Capsule::new_z(half_height, radius);
Self::new(Shape::Capsule(capsule))
let p = Point::from(Vector::z() * half_height);
Self::new(ColliderShape::capsule(-p, p, radius))
}
/// Initialize a new collider builder with a cuboid shape defined by its half-extents.
#[cfg(feature = "dim3")]
pub fn cuboid(hx: f32, hy: f32, hz: f32) -> Self {
let cuboid = Cuboid {
half_extents: Vector::new(hx, hy, hz),
};
Self::new(Shape::Cuboid(cuboid))
Self::new(ColliderShape::cuboid(Vector::new(hx, hy, hz)))
}
/// Initializes a collider builder with a segment shape.
///
/// A segment shape is modeled by a capsule with a 0 radius.
pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
let capsule = Capsule::new(a, b, 0.0);
Self::new(Shape::Capsule(capsule))
Self::new(ColliderShape::segment(a, b))
}
/// Initializes a collider builder with a triangle shape.
pub fn triangle(a: Point<f32>, b: Point<f32>, c: Point<f32>) -> Self {
let triangle = Triangle::new(a, b, c);
Self::new(Shape::Triangle(triangle))
Self::new(ColliderShape::triangle(a, b, c))
}
/// Initializes a collider builder with a triangle mesh shape defined by its vertex and index buffers.
pub fn trimesh(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
let trimesh = Trimesh::new(vertices, indices);
Self::new(Shape::Trimesh(trimesh))
Self::new(ColliderShape::trimesh(vertices, indices))
}
/// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
/// factor along each coordinate axis.
#[cfg(feature = "dim2")]
pub fn heightfield(heights: na::DVector<f32>, scale: Vector<f32>) -> Self {
let heightfield = HeightField::new(heights, scale);
Self::new(Shape::HeightField(heightfield))
Self::new(ColliderShape::heightfield(heights, scale))
}
/// Initializes a collider builder with a heightfield shape defined by its set of height and a scale
/// factor along each coordinate axis.
#[cfg(feature = "dim3")]
pub fn heightfield(heights: na::DMatrix<f32>, scale: Vector<f32>) -> Self {
let heightfield = HeightField::new(heights, scale);
Self::new(Shape::HeightField(heightfield))
Self::new(ColliderShape::heightfield(heights, scale))
}
/// The default friction coefficient used by the collider builder.
@@ -385,6 +436,30 @@ impl ColliderBuilder {
0.5
}
/// Sets an arbitrary user-defined 128-bit integer associated to the colliders built by this builder.
pub fn user_data(mut self, data: u128) -> Self {
self.user_data = data;
self
}
/// Sets the collision groups used by this collider.
///
/// Two colliders will interact iff. their collision groups are compatible.
/// See [InteractionGroups::test] for details.
pub fn collision_groups(mut self, groups: InteractionGroups) -> Self {
self.collision_groups = groups;
self
}
/// Sets the solver groups used by this collider.
///
/// Forces between two colliders in contact will be computed iff their solver groups are
/// compatible. See [InteractionGroups::test] for details.
pub fn solver_groups(mut self, groups: InteractionGroups) -> Self {
self.solver_groups = groups;
self
}
/// Sets whether or not the collider built by this builder is a sensor.
pub fn sensor(mut self, is_sensor: bool) -> Self {
self.is_sensor = is_sensor;
@@ -449,7 +524,7 @@ impl ColliderBuilder {
self
}
/// Buildes a new collider attached to the given rigid-body.
/// Builds a new collider attached to the given rigid-body.
pub fn build(&self) -> Collider {
let density = self.get_density();
@@ -466,6 +541,9 @@ impl ColliderBuilder {
contact_graph_index: InteractionGraph::<Contact>::invalid_graph_index(),
proximity_graph_index: InteractionGraph::<Proximity>::invalid_graph_index(),
proxy_index: crate::INVALID_USIZE,
collision_groups: self.collision_groups,
solver_groups: self.solver_groups,
user_data: self.user_data,
}
}
}

50
src/geometry/contact.rs
View File

@@ -9,6 +9,16 @@ use {
simba::simd::SimdValue,
};
bitflags::bitflags! {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Flags affecting the behavior of the constraints solver for a given contact manifold.
pub struct SolverFlags: u32 {
/// The constraint solver will take this contact manifold into
/// account for force computation.
const COMPUTE_IMPULSES = 0b01;
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// The type local linear approximation of the neighborhood of a pair contact points on two shapes
@@ -206,6 +216,7 @@ impl ContactPair {
pub(crate) fn single_manifold<'a, 'b>(
&'a mut self,
colliders: &'b ColliderSet,
flags: SolverFlags,
) -> (
&'b Collider,
&'b Collider,
@@ -216,7 +227,7 @@ impl ContactPair {
let coll2 = &colliders[self.pair.collider2];
if self.manifolds.len() == 0 {
let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2);
let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags);
self.manifolds.push(manifold);
}
@@ -288,6 +299,8 @@ pub struct ContactManifold {
/// The relative position between the second collider and its parent at the time the
/// contact points were generated.
pub delta2: Isometry<f32>,
/// Flags used to control some aspects of the constraints solver for this contact manifold.
pub solver_flags: SolverFlags,
}
impl ContactManifold {
@@ -299,6 +312,7 @@ impl ContactManifold {
delta2: Isometry<f32>,
friction: f32,
restitution: f32,
solver_flags: SolverFlags,
) -> ContactManifold {
Self {
#[cfg(feature = "dim2")]
@@ -319,6 +333,7 @@ impl ContactManifold {
delta2,
constraint_index: 0,
position_constraint_index: 0,
solver_flags,
}
}
@@ -342,11 +357,17 @@ impl ContactManifold {
delta2: self.delta2,
constraint_index: self.constraint_index,
position_constraint_index: self.position_constraint_index,
solver_flags: self.solver_flags,
}
}
pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self {
Self::with_subshape_indices(pair, coll1, coll2, 0, 0)
pub(crate) fn from_colliders(
pair: ColliderPair,
coll1: &Collider,
coll2: &Collider,
flags: SolverFlags,
) -> Self {
Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags)
}
pub(crate) fn with_subshape_indices(
@@ -355,6 +376,7 @@ impl ContactManifold {
coll2: &Collider,
subshape1: usize,
subshape2: usize,
solver_flags: SolverFlags,
) -> Self {
Self::new(
pair,
@@ -364,6 +386,7 @@ impl ContactManifold {
*coll2.position_wrt_parent(),
(coll1.friction + coll2.friction) * 0.5,
(coll1.restitution + coll2.restitution) * 0.5,
solver_flags,
)
}
@@ -430,16 +453,26 @@ impl ContactManifold {
#[inline]
pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>) -> bool {
// const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
const DIST_SQ_THRESHOLD: f32 = 0.001; // FIXME: this should not be hard-coded.
self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD)
}
#[inline]
pub(crate) fn try_update_contacts_eps(
&mut self,
pos12: &Isometry<f32>,
angle_dot_threshold: f32,
dist_sq_threshold: f32,
) -> bool {
if self.points.len() == 0 {
return false;
}
// const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
let local_n2 = pos12 * self.local_n2;
if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD {
if -self.local_n1.dot(&local_n2) < angle_dot_threshold {
return false;
}
@@ -455,8 +488,7 @@ impl ContactManifold {
}
let new_local_p1 = local_p2 - self.local_n1 * dist;
let dist_threshold = 0.001; // FIXME: this should not be hard-coded.
if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold {
if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold {
return false;
}

23
src/geometry/contact_generator/ball_convex_contact_generator.rs
View File

@@ -1,32 +1,21 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Ball, Contact, KinematicsCategory, Shape};
use crate::geometry::{Ball, Contact, KinematicsCategory};
use crate::math::Isometry;
use na::Unit;
use ncollide::query::PointQuery;
pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) {
if let Shape::Ball(ball1) = ctxt.shape1 {
if let Some(ball1) = ctxt.shape1.as_ball() {
ctxt.manifold.swap_identifiers();
match ctxt.shape2 {
Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true),
Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true),
Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true),
_ => unimplemented!(),
}
} else if let Shape::Ball(ball2) = ctxt.shape2 {
match ctxt.shape1 {
Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false),
Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false),
Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false),
_ => unimplemented!(),
}
do_generate_contacts(ctxt.shape2, ball1, ctxt, true);
} else if let Some(ball2) = ctxt.shape2.as_ball() {
do_generate_contacts(ctxt.shape1, ball2, ctxt, false);
}
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
fn do_generate_contacts<P: PointQuery<f32>>(
fn do_generate_contacts<P: ?Sized + PointQuery<f32>>(
point_query1: &P,
ball2: &Ball,
ctxt: &mut PrimitiveContactGenerationContext,

39
src/geometry/contact_generator/capsule_capsule_contact_generator.rs
View File

@@ -1,14 +1,14 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory, Shape};
use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory};
use crate::math::Isometry;
use crate::math::Vector;
use approx::AbsDiffEq;
use na::Unit;
#[cfg(feature = "dim2")]
use ncollide::shape::{Segment, SegmentPointLocation};
use ncollide::shape::SegmentPointLocation;
pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Shape::Capsule(capsule1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) {
if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) {
generate_contacts(
ctxt.prediction_distance,
capsule1,
@@ -39,10 +39,11 @@ pub fn generate_contacts<'a>(
let pos12 = pos1.inverse() * pos2;
let pos21 = pos12.inverse();
let capsule2_1 = capsule2.transform_by(&pos12);
let seg1 = capsule1.segment;
let seg2_1 = capsule2.segment.transformed(&pos12);
let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
(&capsule1.a, &capsule1.b),
(&capsule2_1.a, &capsule2_1.b),
(&seg1.a, &seg1.b),
(&seg2_1.a, &seg2_1.b),
);
// We do this clone to perform contact tracking and transfer impulses.
@@ -65,8 +66,8 @@ pub fn generate_contacts<'a>(
let bcoords1 = loc1.barycentric_coordinates();
let bcoords2 = loc2.barycentric_coordinates();
let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1];
let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1];
let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
let local_n1 =
Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());
@@ -87,18 +88,15 @@ pub fn generate_contacts<'a>(
return;
}
let seg1 = Segment::new(capsule1.a, capsule1.b);
let seg2 = Segment::new(capsule2_1.a, capsule2_1.b);
if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2.direction()) {
if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2_1.direction()) {
if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8
&& dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8
{
// Capsules axii are almost parallel and are almost perpendicular to the normal.
// Capsules axes are almost parallel and are almost perpendicular to the normal.
// Find a second contact point.
if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal(
(capsule1.a, capsule1.b),
(capsule2_1.a, capsule2_1.b),
(seg1.a, seg1.b),
(seg2_1.a, seg2_1.b),
*local_n1,
) {
let contact =
@@ -156,17 +154,18 @@ pub fn generate_contacts<'a>(
let pos12 = pos1.inverse() * pos2;
let pos21 = pos12.inverse();
let capsule2_1 = capsule1.transform_by(&pos12);
let seg1 = capsule1.segment;
let seg2_1 = capsule2.segment.transformed(&pos12);
let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
(&capsule1.a, &capsule1.b),
(&capsule2_1.a, &capsule2_1.b),
(&seg1.a, &seg1.b),
(&seg2_1.a, &seg2_1.b),
);
{
let bcoords1 = loc1.barycentric_coordinates();
let bcoords2 = loc2.barycentric_coordinates();
let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1];
let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1];
let local_p1 = seg1.a * bcoords1[0] + seg1.b.coords * bcoords1[1];
let local_p2 = seg2_1.a * bcoords2[0] + seg2_1.b.coords * bcoords2[1];
let local_n1 =
Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis());

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

@@ -1,8 +1,10 @@
#[cfg(feature = "dim3")]
use crate::geometry::contact_generator::PfmPfmContactManifoldGeneratorWorkspace;
use crate::geometry::contact_generator::{
ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace,
PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace,
};
use crate::geometry::Shape;
use crate::geometry::ShapeType;
use std::any::Any;
/// Trait implemented by structures responsible for selecting a collision-detection algorithm
@@ -11,8 +13,8 @@ pub trait ContactDispatcher {
/// Select the collision-detection algorithm for the given pair of primitive shapes.
fn dispatch_primitives(
&self,
shape1: &Shape,
shape2: &Shape,
shape1: ShapeType,
shape2: ShapeType,
) -> (
PrimitiveContactGenerator,
Option<Box<dyn Any + Send + Sync>>,
@@ -20,8 +22,8 @@ pub trait ContactDispatcher {
/// Select the collision-detection algorithm for the given pair of non-primitive shapes.
fn dispatch(
&self,
shape1: &Shape,
shape2: &Shape,
shape1: ShapeType,
shape2: ShapeType,
) -> (ContactPhase, Option<Box<dyn Any + Send + Sync>>);
}
@@ -31,14 +33,14 @@ pub struct DefaultContactDispatcher;
impl ContactDispatcher for DefaultContactDispatcher {
fn dispatch_primitives(
&self,
shape1: &Shape,
shape2: &Shape,
shape1: ShapeType,
shape2: ShapeType,
) -> (
PrimitiveContactGenerator,
Option<Box<dyn Any + Send + Sync>>,
) {
match (shape1, shape2) {
(Shape::Ball(_), Shape::Ball(_)) => (
(ShapeType::Ball, ShapeType::Ball) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_ball_ball,
#[cfg(feature = "simd-is-enabled")]
@@ -47,52 +49,64 @@ impl ContactDispatcher for DefaultContactDispatcher {
},
None,
),
(Shape::Cuboid(_), Shape::Cuboid(_)) => (
(ShapeType::Cuboid, ShapeType::Cuboid) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_cuboid_cuboid,
..PrimitiveContactGenerator::default()
},
None,
),
(Shape::Polygon(_), Shape::Polygon(_)) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_polygon_polygon,
..PrimitiveContactGenerator::default()
},
None,
),
(Shape::Capsule(_), Shape::Capsule(_)) => (
// (ShapeType::Polygon, ShapeType::Polygon) => (
// PrimitiveContactGenerator {
// generate_contacts: super::generate_contacts_polygon_polygon,
// ..PrimitiveContactGenerator::default()
// },
// None,
// ),
(ShapeType::Capsule, ShapeType::Capsule) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_capsule_capsule,
..PrimitiveContactGenerator::default()
},
None,
),
(Shape::Cuboid(_), Shape::Ball(_))
| (Shape::Ball(_), Shape::Cuboid(_))
| (Shape::Triangle(_), Shape::Ball(_))
| (Shape::Ball(_), Shape::Triangle(_))
| (Shape::Capsule(_), Shape::Ball(_))
| (Shape::Ball(_), Shape::Capsule(_)) => (
(_, ShapeType::Ball) | (ShapeType::Ball, _) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_ball_convex,
..PrimitiveContactGenerator::default()
},
None,
),
(Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => (
(ShapeType::Capsule, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Capsule) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_cuboid_capsule,
..PrimitiveContactGenerator::default()
},
None,
),
(Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => (
(ShapeType::Triangle, ShapeType::Cuboid) | (ShapeType::Cuboid, ShapeType::Triangle) => {
(
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_cuboid_triangle,
..PrimitiveContactGenerator::default()
},
None,
)
}
#[cfg(feature = "dim3")]
(ShapeType::Cylinder, _)
| (_, ShapeType::Cylinder)
| (ShapeType::Cone, _)
| (_, ShapeType::Cone)
| (ShapeType::RoundCylinder, _)
| (_, ShapeType::RoundCylinder)
| (ShapeType::Capsule, _)
| (_, ShapeType::Capsule) => (
PrimitiveContactGenerator {
generate_contacts: super::generate_contacts_cuboid_triangle,
generate_contacts: super::generate_contacts_pfm_pfm,
..PrimitiveContactGenerator::default()
},
None,
Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())),
),
_ => (PrimitiveContactGenerator::default(), None),
}
@@ -100,18 +114,18 @@ impl ContactDispatcher for DefaultContactDispatcher {
fn dispatch(
&self,
shape1: &Shape,
shape2: &Shape,
shape1: ShapeType,
shape2: ShapeType,
) -> (ContactPhase, Option<Box<dyn Any + Send + Sync>>) {
match (shape1, shape2) {
(Shape::Trimesh(_), _) | (_, Shape::Trimesh(_)) => (
(ShapeType::Trimesh, _) | (_, ShapeType::Trimesh) => (
ContactPhase::NearPhase(ContactGenerator {
generate_contacts: super::generate_contacts_trimesh_shape,
..ContactGenerator::default()
}),
Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())),
),
(Shape::HeightField(_), _) | (_, Shape::HeightField(_)) => (
(ShapeType::HeightField, _) | (_, ShapeType::HeightField) => (
ContactPhase::NearPhase(ContactGenerator {
generate_contacts: super::generate_contacts_heightfield_shape,
..ContactGenerator::default()

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

@@ -1,5 +1,6 @@
use crate::geometry::{
Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape,
SolverFlags,
};
use crate::math::Isometry;
#[cfg(feature = "simd-is-enabled")]
@@ -26,8 +27,9 @@ impl ContactPhase {
Self::NearPhase(gen) => (gen.generate_contacts)(&mut context),
Self::ExactPhase(gen) => {
// Build the primitive context from the non-primitive context and dispatch.
let (collider1, collider2, manifold, workspace) =
context.pair.single_manifold(context.colliders);
let (collider1, collider2, manifold, workspace) = context
.pair
.single_manifold(context.colliders, context.solver_flags);
let mut context2 = PrimitiveContactGenerationContext {
prediction_distance: context.prediction_distance,
collider1,
@@ -85,9 +87,11 @@ impl ContactPhase {
[Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH],
> = ArrayVec::new();
for pair in context.pairs.iter_mut() {
for (pair, solver_flags) in
context.pairs.iter_mut().zip(context.solver_flags.iter())
{
let (collider1, collider2, manifold, workspace) =
pair.single_manifold(context.colliders);
pair.single_manifold(context.colliders, *solver_flags);
colliders_arr.push((collider1, collider2));
manifold_arr.push(manifold);
workspace_arr.push(workspace);
@@ -139,8 +143,8 @@ pub struct PrimitiveContactGenerationContext<'a> {
pub prediction_distance: f32,
pub collider1: &'a Collider,
pub collider2: &'a Collider,
pub shape1: &'a Shape,
pub shape2: &'a Shape,
pub shape1: &'a dyn Shape,
pub shape2: &'a dyn Shape,
pub position1: &'a Isometry<f32>,
pub position2: &'a Isometry<f32>,
pub manifold: &'a mut ContactManifold,
@@ -152,8 +156,8 @@ pub struct PrimitiveContactGenerationContextSimd<'a, 'b> {
pub prediction_distance: f32,
pub colliders1: [&'a Collider; SIMD_WIDTH],
pub colliders2: [&'a Collider; SIMD_WIDTH],
pub shapes1: [&'a Shape; SIMD_WIDTH],
pub shapes2: [&'a Shape; 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 manifolds: &'a mut [&'b mut ContactManifold],
@@ -188,6 +192,7 @@ pub struct ContactGenerationContext<'a> {
pub prediction_distance: f32,
pub colliders: &'a ColliderSet,
pub pair: &'a mut ContactPair,
pub solver_flags: SolverFlags,
}
#[cfg(feature = "simd-is-enabled")]
@@ -196,6 +201,7 @@ pub struct ContactGenerationContextSimd<'a, 'b> {
pub prediction_distance: f32,
pub colliders: &'a ColliderSet,
pub pairs: &'a mut [&'b mut ContactPair],
pub solver_flags: &'a [SolverFlags],
}
#[derive(Copy, Clone)]

11
src/geometry/contact_generator/cuboid_capsule_contact_generator.rs
View File

@@ -1,15 +1,14 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
#[cfg(feature = "dim3")]
use crate::geometry::PolyhedronFace;
use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory, Shape};
use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory};
#[cfg(feature = "dim2")]
use crate::geometry::{CuboidFeature, CuboidFeatureFace};
use crate::math::Isometry;
use crate::math::Vector;
use ncollide::shape::Segment;
pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Shape::Cuboid(cube1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) {
if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) {
generate_contacts(
ctxt.prediction_distance,
cube1,
@@ -20,7 +19,9 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon
false,
);
ctxt.manifold.update_warmstart_multiplier();
} else if let (Shape::Capsule(capsule1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
} else if let (Some(capsule1), Some(cube2)) =
(ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid())
{
generate_contacts(
ctxt.prediction_distance,
cube2,
@@ -53,7 +54,7 @@ pub fn generate_contacts<'a>(
return;
}
let segment2 = Segment::new(capsule2.a, capsule2.b);
let segment2 = capsule2.segment;
/*
*

4
src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs
View File

@@ -1,12 +1,12 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory, Shape};
use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory};
use crate::math::Isometry;
#[cfg(feature = "dim2")]
use crate::math::Vector;
use ncollide::shape::Cuboid;
pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) {
generate_contacts(
ctxt.prediction_distance,
cube1,

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

@@ -1,7 +1,7 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
#[cfg(feature = "dim3")]
use crate::geometry::PolyhedronFace;
use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Shape, Triangle};
use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Triangle};
use crate::math::Isometry;
#[cfg(feature = "dim2")]
use crate::{
@@ -10,7 +10,7 @@ use crate::{
};
pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) {
if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) {
generate_contacts(
ctxt.prediction_distance,
cube1,
@@ -21,7 +21,9 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo
false,
);
ctxt.manifold.update_warmstart_multiplier();
} else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
} else if let (Some(triangle1), Some(cube2)) =
(ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid())
{
generate_contacts(
ctxt.prediction_distance,
cube2,

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

@@ -3,10 +3,8 @@ use crate::geometry::contact_generator::{
};
#[cfg(feature = "dim2")]
use crate::geometry::Capsule;
use crate::geometry::{Collider, ContactManifold, HeightField, Shape};
use crate::geometry::{Collider, ContactManifold, HeightField, Shape, ShapeType};
use crate::ncollide::bounding_volume::BoundingVolume;
#[cfg(feature = "dim3")]
use crate::{geometry::Triangle, math::Point};
use std::any::Any;
use std::collections::hash_map::Entry;
use std::collections::HashMap;
@@ -38,9 +36,9 @@ pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext)
let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
if let Shape::HeightField(heightfield1) = collider1.shape() {
if let Some(heightfield1) = collider1.shape().as_heightfield() {
do_generate_contacts(heightfield1, collider1, collider2, ctxt, false)
} else if let Shape::HeightField(heightfield2) = collider2.shape() {
} else if let Some(heightfield2) = collider2.shape().as_heightfield() {
do_generate_contacts(heightfield2, collider2, collider1, ctxt, true)
}
}
@@ -59,6 +57,7 @@ fn do_generate_contacts(
.expect("The HeightFieldShapeContactGeneratorWorkspace is missing.")
.downcast_mut()
.expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace.");
let shape_type2 = collider2.shape().shape_type();
/*
* Detect if the detector context has been reset.
@@ -71,24 +70,9 @@ fn do_generate_contacts(
} else {
manifold.subshape_index_pair.1
};
// println!(
// "Restoring for {} [chosen with {:?}]",
// subshape_id, manifold.subshape_index_pair
// );
// Use dummy shapes for the dispatch.
#[cfg(feature = "dim2")]
let sub_shape1 =
Shape::Capsule(Capsule::new(na::Point::origin(), na::Point::origin(), 0.0));
#[cfg(feature = "dim3")]
let sub_shape1 = Shape::Triangle(Triangle::new(
Point::origin(),
Point::origin(),
Point::origin(),
));
let (generator, workspace2) = ctxt
.dispatcher
.dispatch_primitives(&sub_shape1, collider2.shape());
.dispatch_primitives(ShapeType::Capsule, shape_type2);
let sub_detector = SubDetector {
generator,
@@ -120,12 +104,16 @@ fn do_generate_contacts(
let manifolds = &mut ctxt.pair.manifolds;
let prediction_distance = ctxt.prediction_distance;
let dispatcher = ctxt.dispatcher;
let solver_flags = ctxt.solver_flags;
let shape_type2 = collider2.shape().shape_type();
heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| {
let position1 = collider1.position();
#[cfg(feature = "dim2")]
let sub_shape1 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0));
let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead.
#[cfg(feature = "dim3")]
let sub_shape1 = Shape::Triangle(*part1);
let sub_shape1 = *part1;
let sub_detector = match workspace.sub_detectors.entry(i) {
Entry::Occupied(entry) => {
let sub_detector = entry.into_mut();
@@ -137,15 +125,21 @@ fn do_generate_contacts(
}
Entry::Vacant(entry) => {
let (generator, workspace2) =
dispatcher.dispatch_primitives(&sub_shape1, collider2.shape());
dispatcher.dispatch_primitives(sub_shape1.shape_type(), shape_type2);
let sub_detector = SubDetector {
generator,
manifold_id: manifolds.len(),
timestamp: new_timestamp,
workspace: workspace2,
};
let manifold =
ContactManifold::with_subshape_indices(coll_pair, collider1, collider2, i, 0);
let manifold = ContactManifold::with_subshape_indices(
coll_pair,
collider1,
collider2,
i,
0,
solver_flags,
);
manifolds.push(manifold);
entry.insert(sub_detector)
@@ -162,7 +156,7 @@ fn do_generate_contacts(
shape1: collider2.shape(),
shape2: &sub_shape1,
position1: collider2.position(),
position2: collider1.position(),
position2: position1,
manifold,
workspace: sub_detector.workspace.as_deref_mut(),
}
@@ -173,7 +167,7 @@ fn do_generate_contacts(
collider2,
shape1: &sub_shape1,
shape2: collider2.shape(),
position1: collider1.position(),
position1,
position2: collider2.position(),
manifold,
workspace: sub_detector.workspace.as_deref_mut(),

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

@@ -18,15 +18,18 @@ pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triang
pub use self::heightfield_shape_contact_generator::{
generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace,
};
pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
#[cfg(feature = "dim3")]
pub use self::pfm_pfm_contact_generator::{
generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace,
};
// pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
pub use self::trimesh_shape_contact_generator::{
generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace,
};
pub(crate) use self::polygon_polygon_contact_generator::clip_segments;
#[cfg(feature = "dim2")]
pub(crate) use self::polygon_polygon_contact_generator::{
clip_segments, clip_segments_with_normal,
};
pub(crate) use self::polygon_polygon_contact_generator::clip_segments_with_normal;
mod ball_ball_contact_generator;
mod ball_convex_contact_generator;
@@ -39,6 +42,8 @@ mod cuboid_cuboid_contact_generator;
mod cuboid_polygon_contact_generator;
mod cuboid_triangle_contact_generator;
mod heightfield_shape_contact_generator;
#[cfg(feature = "dim3")]
mod pfm_pfm_contact_generator;
mod polygon_polygon_contact_generator;
mod trimesh_shape_contact_generator;

119
src/geometry/contact_generator/pfm_pfm_contact_generator.rs Normal file
View File

@@ -0,0 +1,119 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{KinematicsCategory, PolygonalFeatureMap, PolyhedronFace};
use crate::math::{Isometry, Vector};
use na::Unit;
use ncollide::query;
use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex};
pub struct PfmPfmContactManifoldGeneratorWorkspace {
simplex: VoronoiSimplex<f32>,
last_gjk_dir: Option<Unit<Vector<f32>>>,
feature1: PolyhedronFace,
feature2: PolyhedronFace,
}
impl Default for PfmPfmContactManifoldGeneratorWorkspace {
fn default() -> Self {
Self {
simplex: VoronoiSimplex::new(),
last_gjk_dir: None,
feature1: PolyhedronFace::new(),
feature2: PolyhedronFace::new(),
}
}
}
pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Some((pfm1, border_radius1)), Some((pfm2, border_radius2))) = (
ctxt.shape1.as_polygonal_feature_map(),
ctxt.shape2.as_polygonal_feature_map(),
) {
do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt);
ctxt.manifold.update_warmstart_multiplier();
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
}
fn do_generate_contacts(
pfm1: &dyn PolygonalFeatureMap,
border_radius1: f32,
pfm2: &dyn PolygonalFeatureMap,
border_radius2: f32,
ctxt: &mut PrimitiveContactGenerationContext,
) {
let pos12 = ctxt.position1.inverse() * ctxt.position2;
let pos21 = pos12.inverse();
// We use very small thresholds for the manifold update because something to high would
// cause numerical drifts with the effect of introducing bumps in
// what should have been smooth rolling motions.
if ctxt
.manifold
.try_update_contacts_eps(&pos12, crate::utils::COS_1_DEGREES, 1.0e-6)
{
return;
}
let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt
.workspace
.as_mut()
.expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.")
.downcast_mut()
.expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
let total_prediction = ctxt.prediction_distance + border_radius1 + border_radius2;
let contact = query::contact_support_map_support_map_with_params(
&Isometry::identity(),
pfm1,
&pos12,
pfm2,
total_prediction,
&mut workspace.simplex,
workspace.last_gjk_dir,
);
let old_manifold_points = ctxt.manifold.points.clone();
ctxt.manifold.points.clear();
match contact {
GJKResult::ClosestPoints(_, _, dir) => {
workspace.last_gjk_dir = Some(dir);
let normal1 = dir;
let normal2 = pos21 * -dir;
pfm1.local_support_feature(&normal1, &mut workspace.feature1);
pfm2.local_support_feature(&normal2, &mut workspace.feature2);
workspace.feature2.transform_by(&pos12);
PolyhedronFace::contacts(
total_prediction,
&workspace.feature1,
&normal1,
&workspace.feature2,
&pos21,
ctxt.manifold,
);
if border_radius1 != 0.0 || border_radius2 != 0.0 {
for contact in &mut ctxt.manifold.points {
contact.local_p1 += *normal1 * border_radius1;
contact.local_p2 += *normal2 * border_radius2;
contact.dist -= border_radius1 + border_radius2;
}
}
// Adjust points to take the radius into account.
ctxt.manifold.local_n1 = *normal1;
ctxt.manifold.local_n2 = *normal2;
ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // TODO: is this the more appropriate?
ctxt.manifold.kinematics.radius1 = 0.0;
ctxt.manifold.kinematics.radius2 = 0.0;
}
GJKResult::NoIntersection(dir) => {
workspace.last_gjk_dir = Some(dir);
}
_ => {}
}
// Transfer impulses.
super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false);
}

35
src/geometry/contact_generator/polygon_polygon_contact_generator.rs
View File

@@ -1,24 +1,27 @@
#![allow(dead_code)] // TODO: remove this once we support polygons.
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon, Shape};
use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon};
use crate::math::{Isometry, Point};
#[cfg(feature = "dim2")]
use crate::{math::Vector, utils};
pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
generate_contacts(
polygon1,
&ctxt.position1,
polygon2,
&ctxt.position2,
ctxt.manifold,
);
ctxt.manifold.update_warmstart_multiplier();
} else {
unreachable!()
}
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationContext) {
unimplemented!()
// if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
// generate_contacts(
// polygon1,
// &ctxt.position1,
// polygon2,
// &ctxt.position2,
// ctxt.manifold,
// );
// ctxt.manifold.update_warmstart_multiplier();
// } else {
// unreachable!()
// }
//
// ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
fn generate_contacts<'a>(

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

@@ -1,7 +1,7 @@
use crate::geometry::contact_generator::{
ContactGenerationContext, PrimitiveContactGenerationContext,
};
use crate::geometry::{Collider, ContactManifold, Shape, Trimesh};
use crate::geometry::{Collider, ContactManifold, ShapeType, Trimesh};
use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
pub struct TrimeshShapeContactGeneratorWorkspace {
@@ -26,9 +26,9 @@ pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) {
let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
if let Shape::Trimesh(trimesh1) = collider1.shape() {
if let Some(trimesh1) = collider1.shape().as_trimesh() {
do_generate_contacts(trimesh1, collider1, collider2, ctxt, false)
} else if let Shape::Trimesh(trimesh2) = collider2.shape() {
} else if let Some(trimesh2) = collider2.shape().as_trimesh() {
do_generate_contacts(trimesh2, collider2, collider1, ctxt, true)
}
}
@@ -121,6 +121,7 @@ fn do_generate_contacts(
let new_interferences = &workspace.interferences;
let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
let mut old_manifolds_it = workspace.old_manifolds.drain(..);
let shape_type2 = collider2.shape().shape_type();
for (i, triangle_id) in new_interferences.iter().enumerate() {
if *triangle_id >= trimesh1.num_triangles() {
@@ -148,6 +149,7 @@ fn do_generate_contacts(
collider2,
*triangle_id,
0,
ctxt.solver_flags,
)
} else {
// We already have a manifold for this triangle.
@@ -159,10 +161,10 @@ fn do_generate_contacts(
}
let manifold = &mut ctxt.pair.manifolds[i];
let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id));
let triangle1 = trimesh1.triangle(*triangle_id);
let (generator, mut workspace2) = ctxt
.dispatcher
.dispatch_primitives(&triangle1, collider2.shape());
.dispatch_primitives(ShapeType::Triangle, shape_type2);
let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 {
PrimitiveContactGenerationContext {

60
src/geometry/interaction_groups.rs Normal file
View File

@@ -0,0 +1,60 @@
#[repr(transparent)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, Hash, PartialEq, Eq)]
/// Pairwise filtering using bit masks.
///
/// This filtering method is based on two 16-bit values:
/// - The interaction groups (the 16 left-most bits of `self.0`).
/// - The interaction mask (the 16 right-most bits of `self.0`).
///
/// An interaction is allowed between two filters `a` and `b` two conditions
/// are met simultaneously:
/// - The interaction groups of `a` has at least one bit set to `1` in common with the interaction mask of `b`.
/// - The interaction groups of `b` has at least one bit set to `1` in common with the interaction mask of `a`.
/// In other words, interactions are allowed between two filter iff. the following condition is met:
/// ```ignore
/// ((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0
/// ```
pub struct InteractionGroups(pub u32);
impl InteractionGroups {
/// Initializes with the given interaction groups and interaction mask.
pub const fn new(groups: u16, masks: u16) -> Self {
Self::none().with_groups(groups).with_mask(masks)
}
/// Allow interaction with everything.
pub const fn all() -> Self {
Self(u32::MAX)
}
/// Prevent all interactions.
pub const fn none() -> Self {
Self(0)
}
/// Sets the group this filter is part of.
pub const fn with_groups(self, groups: u16) -> Self {
Self((self.0 & 0x0000ffff) | ((groups as u32) << 16))
}
/// Sets the interaction mask of this filter.
pub const fn with_mask(self, mask: u16) -> Self {
Self((self.0 & 0xffff0000) | (mask as u32))
}
/// Check if interactions should be allowed based on the interaction groups and mask.
///
/// An interaction is allowed iff. the groups of `self` contain at least one bit set to 1 in common
/// with the mask of `rhs`, and vice-versa.
#[inline]
pub const fn test(self, rhs: Self) -> bool {
((self.0 >> 16) & rhs.0) != 0 && ((rhs.0 >> 16) & self.0) != 0
}
}
impl Default for InteractionGroups {
fn default() -> Self {
Self::all()
}
}

33
src/geometry/mod.rs
View File

@@ -2,10 +2,10 @@
pub use self::broad_phase_multi_sap::BroadPhase;
pub use self::capsule::Capsule;
pub use self::collider::{Collider, ColliderBuilder, Shape};
pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
pub use self::collider_set::{ColliderHandle, ColliderSet};
pub use self::contact::{
Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory,
Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, SolverFlags,
};
pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher};
#[cfg(feature = "dim2")]
@@ -19,9 +19,14 @@ 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 ncollide::query::Proximity;
/// A segment shape.
pub type Segment = ncollide::shape::Segment<f32>;
/// A cuboid shape.
pub type Cuboid = ncollide::shape::Cuboid<f32>;
/// A triangle shape.
@@ -30,6 +35,12 @@ pub type Triangle = ncollide::shape::Triangle<f32>;
pub type Ball = ncollide::shape::Ball<f32>;
/// A heightfield shape.
pub type HeightField = ncollide::shape::HeightField<f32>;
/// A cylindrical shape.
#[cfg(feature = "dim3")]
pub type Cylinder = ncollide::shape::Cylinder<f32>;
/// A cone shape.
#[cfg(feature = "dim3")]
pub type Cone = ncollide::shape::Cone<f32>;
/// An axis-aligned bounding box.
pub type AABB = ncollide::bounding_volume::AABB<f32>;
/// Event triggered when two non-sensor colliders start or stop being in contact.
@@ -40,6 +51,8 @@ pub type ProximityEvent = ncollide::pipeline::ProximityEvent<ColliderHandle>;
pub type Ray = ncollide::query::Ray<f32>;
/// The intersection between a ray and a collider.
pub type RayIntersection = ncollide::query::RayIntersection<f32>;
/// The the projection of a point on a collider.
pub type PointProjection = ncollide::query::PointProjection<f32>;
#[cfg(feature = "simd-is-enabled")]
pub(crate) use self::ball::WBall;
@@ -47,18 +60,22 @@ 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::WContact;
pub(crate) use self::contact_generator::clip_segments;
#[cfg(feature = "dim2")]
pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal};
pub(crate) use self::contact_generator::clip_segments_with_normal;
pub(crate) use self::narrow_phase::ContactManifoldIndex;
#[cfg(feature = "dim3")]
pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap;
#[cfg(feature = "dim3")]
pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
pub(crate) use self::waabb::{WRay, WAABB};
pub(crate) use self::wquadtree::WQuadtree;
//pub(crate) use self::z_order::z_cmp_floats;
pub use self::interaction_groups::InteractionGroups;
pub use self::shape::{Shape, ShapeType};
mod ball;
mod broad_phase_multi_sap;
mod capsule;
mod collider;
mod collider_set;
mod contact;
@@ -81,3 +98,11 @@ mod trimesh;
mod waabb;
mod wquadtree;
//mod z_order;
mod capsule;
mod interaction_groups;
#[cfg(feature = "dim3")]
mod polygonal_feature_map;
#[cfg(feature = "dim3")]
mod round_cylinder;
mod shape;
mod user_callbacks;

220
src/geometry/narrow_phase.rs
View File

@@ -14,8 +14,9 @@ use crate::geometry::proximity_detector::{
// proximity_detector::ProximityDetectionContextSimd, WBall,
//};
use crate::geometry::{
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ProximityEvent,
ProximityPair, RemovedCollider,
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactEvent, ContactPairFilter,
PairFilterContext, ProximityEvent, ProximityPair, ProximityPairFilter, RemovedCollider,
SolverFlags,
};
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
//#[cfg(feature = "simd-is-enabled")]
@@ -197,7 +198,8 @@ impl NarrowPhase {
if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() {
let dispatcher = DefaultProximityDispatcher;
let generator = dispatcher.dispatch(co1.shape(), co2.shape());
let generator = dispatcher
.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
let interaction =
ProximityPair::new(*pair, generator.0, generator.1);
let _ = self.proximity_graph.add_edge(
@@ -226,7 +228,8 @@ impl NarrowPhase {
if self.contact_graph.graph.find_edge(gid1, gid2).is_none() {
let dispatcher = DefaultContactDispatcher;
let generator = dispatcher.dispatch(co1.shape(), co2.shape());
let generator = dispatcher
.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
let interaction = ContactPair::new(*pair, generator.0, generator.1);
let _ = self.contact_graph.add_edge(
co1.contact_graph_index,
@@ -288,6 +291,7 @@ impl NarrowPhase {
prediction_distance: f32,
bodies: &RigidBodySet,
colliders: &ColliderSet,
pair_filter: Option<&dyn ProximityPairFilter>,
events: &dyn EventHandler,
) {
par_iter_mut!(&mut self.proximity_graph.graph.edges).for_each(|edge| {
@@ -299,16 +303,44 @@ impl NarrowPhase {
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()) {
// No need to update this contact because nothing moved.
if (rb1.is_sleeping() && rb2.is_static())
|| (rb2.is_sleeping() && rb1.is_static())
|| (rb1.is_sleeping() && rb2.is_sleeping())
{
// No need to update this proximity because nothing moved.
return;
}
if !co1.collision_groups.test(co2.collision_groups) {
// The proximity is not allowed.
return;
}
if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
// Default filtering rule: no proximity between two non-dynamic bodies.
return;
}
if let Some(filter) = pair_filter {
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider1: co1,
collider2: co2,
};
if !filter.filter_proximity_pair(&context) {
// No proximity allowed.
return;
}
}
let dispatcher = DefaultProximityDispatcher;
if pair.detector.is_none() {
// We need a redispatch for this detector.
// This can happen, e.g., after restoring a snapshot of the narrow-phase.
let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
let (detector, workspace) =
dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
pair.detector = Some(detector);
pair.detector_workspace = workspace;
}
@@ -326,69 +358,6 @@ impl NarrowPhase {
.unwrap()
.detect_proximity(context, events);
});
/*
// First, group pairs.
// NOTE: the transmutes here are OK because the Vec are all cleared
// before we leave this method.
// We do this in order to avoid reallocating those vecs each time
// we compute the contacts. Unsafe is necessary because we can't just
// store a Vec<&mut ProximityPair> into the NarrowPhase struct without
// polluting the World with lifetimes.
let ball_ball_prox: &mut Vec<&mut ProximityPair> =
unsafe { std::mem::transmute(&mut self.ball_ball_prox) };
let shape_shape_prox: &mut Vec<&mut ProximityPair> =
unsafe { std::mem::transmute(&mut self.shape_shape_prox) };
let bodies = &bodies.bodies;
// FIXME: don't iterate through all the interactions.
for pair in &mut self.proximity_graph.interactions {
let co1 = &colliders[pair.pair.collider1];
let co2 = &colliders[pair.pair.collider2];
// FIXME: avoid lookup into bodies.
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
{
// No need to update this proximity because nothing moved.
continue;
}
match (co1.shape(), co2.shape()) {
(Shape::Ball(_), Shape::Ball(_)) => ball_ball_prox.push(pair),
_ => shape_shape_prox.push(pair),
}
}
par_chunks_mut!(ball_ball_prox, SIMD_WIDTH).for_each(|pairs| {
let context = ProximityDetectionContextSimd {
dispatcher: &DefaultProximityDispatcher,
prediction_distance,
colliders,
pairs,
};
context.pairs[0]
.detector
.detect_proximity_simd(context, events);
});
par_iter_mut!(shape_shape_prox).for_each(|pair| {
let context = ProximityDetectionContext {
dispatcher: &DefaultProximityDispatcher,
prediction_distance,
colliders,
pair,
};
context.pair.detector.detect_proximity(context, events);
});
ball_ball_prox.clear();
shape_shape_prox.clear();
*/
}
pub(crate) fn compute_contacts(
@@ -396,6 +365,7 @@ impl NarrowPhase {
prediction_distance: f32,
bodies: &RigidBodySet,
colliders: &ColliderSet,
pair_filter: Option<&dyn ContactPairFilter>,
events: &dyn EventHandler,
) {
par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| {
@@ -407,18 +377,52 @@ impl NarrowPhase {
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if ((rb1.is_sleeping() || rb1.is_static()) && (rb2.is_sleeping() || rb2.is_static()))
|| (!rb1.is_dynamic() && !rb2.is_dynamic())
if (rb1.is_sleeping() && rb2.is_static())
|| (rb2.is_sleeping() && rb1.is_static())
|| (rb1.is_sleeping() && rb2.is_sleeping())
{
// No need to update this contact because nothing moved.
return;
}
if !co1.collision_groups.test(co2.collision_groups) {
// The collision is not allowed.
return;
}
if pair_filter.is_none() && !rb1.is_dynamic() && !rb2.is_dynamic() {
// Default filtering rule: no contact between two non-dynamic bodies.
return;
}
let mut solver_flags = if let Some(filter) = pair_filter {
let context = PairFilterContext {
rigid_body1: rb1,
rigid_body2: rb2,
collider1: co1,
collider2: co2,
};
if let Some(solver_flags) = filter.filter_contact_pair(&context) {
solver_flags
} else {
// No contact allowed.
return;
}
} else {
SolverFlags::COMPUTE_IMPULSES
};
if !co1.solver_groups.test(co2.solver_groups) {
solver_flags.remove(SolverFlags::COMPUTE_IMPULSES);
}
let dispatcher = DefaultContactDispatcher;
if pair.generator.is_none() {
// We need a redispatch for this generator.
// This can happen, e.g., after restoring a snapshot of the narrow-phase.
let (generator, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
let (generator, workspace) =
dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
pair.generator = Some(generator);
pair.generator_workspace = workspace;
}
@@ -428,6 +432,7 @@ impl NarrowPhase {
prediction_distance,
colliders,
pair,
solver_flags,
};
context
@@ -436,69 +441,6 @@ impl NarrowPhase {
.unwrap()
.generate_contacts(context, events);
});
/*
// First, group pairs.
// NOTE: the transmutes here are OK because the Vec are all cleared
// before we leave this method.
// We do this in order to avoid reallocating those vecs each time
// we compute the contacts. Unsafe is necessary because we can't just
// store a Vec<&mut ContactPair> into the NarrowPhase struct without
// polluting the World with lifetimes.
let ball_ball: &mut Vec<&mut ContactPair> =
unsafe { std::mem::transmute(&mut self.ball_ball) };
let shape_shape: &mut Vec<&mut ContactPair> =
unsafe { std::mem::transmute(&mut self.shape_shape) };
let bodies = &bodies.bodies;
// FIXME: don't iterate through all the interactions.
for pair in &mut self.contact_graph.interactions {
let co1 = &colliders[pair.pair.collider1];
let co2 = &colliders[pair.pair.collider2];
// FIXME: avoid lookup into bodies.
let rb1 = &bodies[co1.parent];
let rb2 = &bodies[co2.parent];
if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic())
{
// No need to update this contact because nothing moved.
continue;
}
match (co1.shape(), co2.shape()) {
(Shape::Ball(_), Shape::Ball(_)) => ball_ball.push(pair),
_ => shape_shape.push(pair),
}
}
par_chunks_mut!(ball_ball, SIMD_WIDTH).for_each(|pairs| {
let context = ContactGenerationContextSimd {
dispatcher: &DefaultContactDispatcher,
prediction_distance,
colliders,
pairs,
};
context.pairs[0]
.generator
.generate_contacts_simd(context, events);
});
par_iter_mut!(shape_shape).for_each(|pair| {
let context = ContactGenerationContext {
dispatcher: &DefaultContactDispatcher,
prediction_distance,
colliders,
pair,
};
context.pair.generator.generate_contacts(context, events);
});
ball_ball.clear();
shape_shape.clear();
*/
}
/// Retrieve all the interactions with at least one contact point, happening between two active bodies.
@@ -518,7 +460,11 @@ impl NarrowPhase {
for manifold in &mut inter.weight.manifolds {
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
if manifold.num_active_contacts() != 0
if manifold
.solver_flags
.contains(SolverFlags::COMPUTE_IMPULSES)
&& manifold.num_active_contacts() != 0
&& (rb1.is_dynamic() || rb2.is_dynamic())
&& (!rb1.is_dynamic() || !rb1.is_sleeping())
&& (!rb2.is_dynamic() || !rb2.is_sleeping())
{

2
src/geometry/polygon.rs
View File

@@ -1,3 +1,5 @@
#![allow(dead_code)] // TODO: remove this once we support polygons.
use crate::math::{Isometry, Point, Vector};
use ncollide::bounding_volume::AABB;

132
src/geometry/polygonal_feature_map.rs Normal file
View File

@@ -0,0 +1,132 @@
use crate::geometry::PolyhedronFace;
use crate::geometry::{cuboid, Cone, Cuboid, Cylinder, Segment, Triangle};
use crate::math::{Point, Vector};
use approx::AbsDiffEq;
use na::{Unit, Vector2};
use ncollide::shape::SupportMap;
/// Trait implemented by convex shapes with features with polyhedral approximations.
pub trait PolygonalFeatureMap: SupportMap<f32> {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace);
}
impl PolygonalFeatureMap for Segment {
fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
*out_feature = PolyhedronFace::from(*self);
}
}
impl PolygonalFeatureMap for Triangle {
fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
*out_feature = PolyhedronFace::from(*self);
}
}
impl PolygonalFeatureMap for Cuboid {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
let face = cuboid::support_face(self, **dir);
*out_feature = PolyhedronFace::from(face);
}
}
impl PolygonalFeatureMap for Cylinder {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
// About feature ids.
// At all times, we consider our cylinder to be approximated as follows:
// - The curved part is approximated by a single segment.
// - Each flat cap of the cylinder is approximated by a square.
// - The curved-part segment has a feature ID of 0, and its endpoint with negative
// `y` coordinate has an ID of 1.
// - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
// when looking at the cap with an eye looking towards +y).
// - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
// - The bottom cap has its face feature ID of 9.
// - The feature IDs of the top cap are the same as the bottom cap to which we add 10.
// So its vertices have IDs 11,13,15,17, its edges 12,14,16,18, and its face 19.
// - Note that at all times, one of each cap's vertices are the same as the curved-part
// segment endpoints.
let dir2 = Vector2::new(dir.x, dir.z)
.try_normalize(f32::default_epsilon())
.unwrap_or(Vector2::x());
if dir.y.abs() < 0.5 {
// We return a segment lying on the cylinder's curved part.
out_features.vertices[0] = Point::new(
dir2.x * self.radius,
-self.half_height,
dir2.y * self.radius,
);
out_features.vertices[1] =
Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
out_features.eids = [0, 0, 0, 0];
out_features.fid = 0;
out_features.num_vertices = 2;
out_features.vids = [1, 11, 11, 11];
} else {
// We return a square approximation of the cylinder cap.
let y = self.half_height.copysign(dir.y);
out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
if dir.y < 0.0 {
out_features.eids = [2, 4, 6, 8];
out_features.fid = 9;
out_features.num_vertices = 4;
out_features.vids = [1, 3, 5, 7];
} else {
out_features.eids = [12, 14, 16, 18];
out_features.fid = 19;
out_features.num_vertices = 4;
out_features.vids = [11, 13, 15, 17];
}
}
}
}
impl PolygonalFeatureMap for Cone {
fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
// About feature ids. It is very similar to the feature ids of cylinders.
// At all times, we consider our cone to be approximated as follows:
// - The curved part is approximated by a single segment.
// - The flat cap of the cone is approximated by a square.
// - The curved-part segment has a feature ID of 0, and its endpoint with negative
// `y` coordinate has an ID of 1.
// - The bottom cap has its vertices with feature ID of 1,3,5,7 (in counter-clockwise order
// when looking at the cap with an eye looking towards +y).
// - The bottom cap has its four edge feature IDs of 2,4,6,8, in counter-clockwise order.
// - The bottom cap has its face feature ID of 9.
// - Note that at all times, one of the cap's vertices are the same as the curved-part
// segment endpoints.
let dir2 = Vector2::new(dir.x, dir.z)
.try_normalize(f32::default_epsilon())
.unwrap_or(Vector2::x());
if dir.y > 0.0 {
// We return a segment lying on the cone's curved part.
out_features.vertices[0] = Point::new(
dir2.x * self.radius,
-self.half_height,
dir2.y * self.radius,
);
out_features.vertices[1] = Point::new(0.0, self.half_height, 0.0);
out_features.eids = [0, 0, 0, 0];
out_features.fid = 0;
out_features.num_vertices = 2;
out_features.vids = [1, 11, 11, 11];
} else {
// We return a square approximation of the cone cap.
let y = -self.half_height;
out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
out_features.eids = [2, 4, 6, 8];
out_features.fid = 9;
out_features.num_vertices = 4;
out_features.vids = [1, 3, 5, 7];
}
}
}

155
src/geometry/polyhedron_feature3d.rs
View File

@@ -1,4 +1,5 @@
use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle};
use crate::approx::AbsDiffEq;
use crate::geometry::{self, Contact, ContactManifold, CuboidFeatureFace, Triangle};
use crate::math::{Isometry, Point, Vector};
use crate::utils::WBasis;
use na::Point2;
@@ -39,6 +40,8 @@ impl From<Triangle> for PolyhedronFace {
impl From<Segment<f32>> for PolyhedronFace {
fn from(seg: Segment<f32>) -> Self {
// Vertices have feature ids 0 and 2.
// The segment interior has feature id 1.
Self {
vertices: [seg.a, seg.b, seg.b, seg.b],
vids: [0, 2, 2, 2],
@@ -50,9 +53,19 @@ impl From<Segment<f32>> for PolyhedronFace {
}
impl PolyhedronFace {
pub fn new() -> Self {
Self {
vertices: [Point::origin(); 4],
vids: [0; 4],
eids: [0; 4],
fid: 0,
num_vertices: 0,
}
}
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
for v in &mut self.vertices[0..self.num_vertices] {
*v = iso * *v;
for p in &mut self.vertices[0..self.num_vertices] {
*p = iso * *p;
}
}
@@ -63,6 +76,140 @@ impl PolyhedronFace {
face2: &PolyhedronFace,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
) {
match (face1.num_vertices, face2.num_vertices) {
(2, 2) => Self::contacts_edge_edge(
prediction_distance,
face1,
sep_axis1,
face2,
pos21,
manifold,
),
_ => Self::contacts_face_face(
prediction_distance,
face1,
sep_axis1,
face2,
pos21,
manifold,
),
}
}
fn contacts_edge_edge(
prediction_distance: f32,
face1: &PolyhedronFace,
sep_axis1: &Vector<f32>,
face2: &PolyhedronFace,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
) {
// Project the faces to a 2D plane for contact clipping.
// The plane they are projected onto has normal sep_axis1
// and contains the origin (this is numerically OK because
// we are not working in world-space here).
let basis = sep_axis1.orthonormal_basis();
let projected_edge1 = [
Point2::new(
face1.vertices[0].coords.dot(&basis[0]),
face1.vertices[0].coords.dot(&basis[1]),
),
Point2::new(
face1.vertices[1].coords.dot(&basis[0]),
face1.vertices[1].coords.dot(&basis[1]),
),
];
let projected_edge2 = [
Point2::new(
face2.vertices[0].coords.dot(&basis[0]),
face2.vertices[0].coords.dot(&basis[1]),
),
Point2::new(
face2.vertices[1].coords.dot(&basis[0]),
face2.vertices[1].coords.dot(&basis[1]),
),
];
let tangent1 =
(projected_edge1[1] - projected_edge1[0]).try_normalize(f32::default_epsilon());
let tangent2 =
(projected_edge2[1] - projected_edge2[0]).try_normalize(f32::default_epsilon());
// TODO: not sure what the best value for eps is.
// Empirically, it appears that an epsilon smaller than 1.0e-3 is too small.
if let (Some(tangent1), Some(tangent2)) = (tangent1, tangent2) {
let parallel = tangent1.dot(&tangent2) >= crate::utils::COS_FRAC_PI_8;
if !parallel {
let seg1 = (&projected_edge1[0], &projected_edge1[1]);
let seg2 = (&projected_edge2[0], &projected_edge2[1]);
let (loc1, loc2) =
ncollide::query::closest_points_segment_segment_with_locations_nD(seg1, seg2);
// Found a contact between the two edges.
let bcoords1 = loc1.barycentric_coordinates();
let bcoords2 = loc2.barycentric_coordinates();
let edge1 = (face1.vertices[0], face1.vertices[1]);
let edge2 = (face2.vertices[0], face2.vertices[1]);
let local_p1 = edge1.0 * bcoords1[0] + edge1.1.coords * bcoords1[1];
let local_p2 = edge2.0 * bcoords2[0] + edge2.1.coords * bcoords2[1];
let dist = (local_p2 - local_p1).dot(&sep_axis1);
if dist <= prediction_distance {
manifold.points.push(Contact {
local_p1,
local_p2: pos21 * local_p2,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: face1.eids[0],
fid2: face2.eids[0],
dist,
});
}
return;
}
}
// The lines are parallel so we are having a conformal contact.
// Let's use a range-based clipping to extract two contact points.
// TODO: would it be better and/or more efficient to do the
//clipping in 2D?
if let Some(clips) = geometry::clip_segments(
(face1.vertices[0], face1.vertices[1]),
(face2.vertices[0], face2.vertices[1]),
) {
manifold.points.push(Contact {
local_p1: (clips.0).0,
local_p2: pos21 * (clips.0).1,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: 0, // FIXME
fid2: 0, // FIXME
dist: ((clips.0).1 - (clips.0).0).dot(&sep_axis1),
});
manifold.points.push(Contact {
local_p1: (clips.1).0,
local_p2: pos21 * (clips.1).1,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: 0, // FIXME
fid2: 0, // FIXME
dist: ((clips.1).1 - (clips.1).0).dot(&sep_axis1),
});
}
}
fn contacts_face_face(
prediction_distance: f32,
face1: &PolyhedronFace,
sep_axis1: &Vector<f32>,
face2: &PolyhedronFace,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
) {
// Project the faces to a 2D plane for contact clipping.
// The plane they are projected onto has normal sep_axis1
@@ -242,8 +389,6 @@ impl PolyhedronFace {
/// Compute the barycentric coordinates of the intersection between the two given lines.
/// Returns `None` if the lines are parallel.
fn closest_points_line2d(edge1: [Point2<f32>; 2], edge2: [Point2<f32>; 2]) -> Option<(f32, f32)> {
use approx::AbsDiffEq;
// Inspired by Real-time collision detection by Christer Ericson.
let dir1 = edge1[1] - edge1[0];
let dir2 = edge2[1] - edge2[0];

20
src/geometry/proximity_detector/ball_convex_proximity_detector.rs
View File

@@ -1,27 +1,19 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{Ball, Proximity, Shape};
use crate::geometry::{Ball, Proximity};
use crate::math::Isometry;
use ncollide::query::PointQuery;
pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
if let Shape::Ball(ball1) = ctxt.shape1 {
match ctxt.shape2 {
Shape::Triangle(tri2) => do_detect_proximity(tri2, ball1, &ctxt),
Shape::Cuboid(cube2) => do_detect_proximity(cube2, ball1, &ctxt),
_ => unimplemented!(),
}
} else if let Shape::Ball(ball2) = ctxt.shape2 {
match ctxt.shape1 {
Shape::Triangle(tri1) => do_detect_proximity(tri1, ball2, &ctxt),
Shape::Cuboid(cube1) => do_detect_proximity(cube1, ball2, &ctxt),
_ => unimplemented!(),
}
if let Some(ball1) = ctxt.shape1.as_ball() {
do_detect_proximity(ctxt.shape2, ball1, &ctxt)
} else if let Some(ball2) = ctxt.shape2.as_ball() {
do_detect_proximity(ctxt.shape1, ball2, &ctxt)
} else {
panic!("Invalid shape types provide.")
}
}
fn do_detect_proximity<P: PointQuery<f32>>(
fn do_detect_proximity<P: ?Sized + PointQuery<f32>>(
point_query1: &P,
ball2: &Ball,
ctxt: &PrimitiveProximityDetectionContext,

4
src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs
View File

@@ -1,10 +1,10 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{sat, Proximity, Shape};
use crate::geometry::{sat, Proximity};
use crate::math::Isometry;
use ncollide::shape::Cuboid;
pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) {
detect_proximity(
ctxt.prediction_distance,
cube1,

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

@@ -1,11 +1,11 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{sat, Cuboid, Proximity, Shape, Triangle};
use crate::geometry::{sat, Cuboid, Proximity, Triangle};
use crate::math::Isometry;
pub fn detect_proximity_cuboid_triangle(
ctxt: &mut PrimitiveProximityDetectionContext,
) -> Proximity {
if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) {
if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) {
detect_proximity(
ctxt.prediction_distance,
cube1,
@@ -13,7 +13,9 @@ pub fn detect_proximity_cuboid_triangle(
triangle2,
ctxt.position2,
)
} else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) {
} else if let (Some(triangle1), Some(cube2)) =
(ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid())
{
detect_proximity(
ctxt.prediction_distance,
cube2,

29
src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs
View File

@@ -1,21 +1,24 @@
#![allow(dead_code)]
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{sat, Polygon, Proximity, Shape};
use crate::geometry::{sat, Polygon, Proximity};
use crate::math::Isometry;
pub fn detect_proximity_polygon_polygon(
ctxt: &mut PrimitiveProximityDetectionContext,
_ctxt: &mut PrimitiveProximityDetectionContext,
) -> Proximity {
if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) {
detect_proximity(
ctxt.prediction_distance,
polygon1,
&ctxt.position1,
polygon2,
&ctxt.position2,
)
} else {
unreachable!()
}
unimplemented!()
// if let (Some(polygon1), Some(polygon2)) = (ctxt.shape1.as_polygon(), ctxt.shape2.as_polygon()) {
// detect_proximity(
// ctxt.prediction_distance,
// polygon1,
// &ctxt.position1,
// polygon2,
// &ctxt.position2,
// )
// } else {
// unreachable!()
// }
}
fn detect_proximity<'a>(

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

@@ -120,8 +120,8 @@ pub struct PrimitiveProximityDetectionContext<'a> {
pub prediction_distance: f32,
pub collider1: &'a Collider,
pub collider2: &'a Collider,
pub shape1: &'a Shape,
pub shape2: &'a Shape,
pub shape1: &'a dyn Shape,
pub shape2: &'a dyn Shape,
pub position1: &'a Isometry<f32>,
pub position2: &'a Isometry<f32>,
pub workspace: Option<&'a mut (dyn Any + Send + Sync)>,
@@ -132,8 +132,8 @@ pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> {
pub prediction_distance: f32,
pub colliders1: [&'a Collider; SIMD_WIDTH],
pub colliders2: [&'a Collider; SIMD_WIDTH],
pub shapes1: [&'a Shape; SIMD_WIDTH],
pub shapes2: [&'a Shape; 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 workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>],

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

@@ -2,7 +2,7 @@ use crate::geometry::proximity_detector::{
PrimitiveProximityDetector, ProximityDetector, ProximityPhase,
TrimeshShapeProximityDetectorWorkspace,
};
use crate::geometry::Shape;
use crate::geometry::ShapeType;
use std::any::Any;
/// Trait implemented by structures responsible for selecting a collision-detection algorithm
@@ -11,8 +11,8 @@ pub trait ProximityDispatcher {
/// Select the proximity detection algorithm for the given pair of primitive shapes.
fn dispatch_primitives(
&self,
shape1: &Shape,
shape2: &Shape,
shape1: ShapeType,
shape2: ShapeType,
) -> (
PrimitiveProximityDetector,
Option<Box<dyn Any + Send + Sync>>,
@@ -20,8 +20,8 @@ pub trait ProximityDispatcher {
/// Select the proximity detection algorithm for the given pair of non-primitive shapes.
fn dispatch(
&self,
shape1: &Shape,
shape2: &Shape,
shape1: ShapeType,
shape2: ShapeType,
) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>);
}
@@ -31,14 +31,14 @@ pub struct DefaultProximityDispatcher;
impl ProximityDispatcher for DefaultProximityDispatcher {
fn dispatch_primitives(
&self,
shape1: &Shape,
shape2: &Shape,
shape1: ShapeType,
shape2: ShapeType,
) -> (
PrimitiveProximityDetector,
Option<Box<dyn Any + Send + Sync>>,
) {
match (shape1, shape2) {
(Shape::Ball(_), Shape::Ball(_)) => (
(ShapeType::Ball, ShapeType::Ball) => (
PrimitiveProximityDetector {
#[cfg(feature = "simd-is-enabled")]
detect_proximity_simd: super::detect_proximity_ball_ball_simd,
@@ -47,56 +47,56 @@ impl ProximityDispatcher for DefaultProximityDispatcher {
},
None,
),
(Shape::Cuboid(_), Shape::Cuboid(_)) => (
(ShapeType::Cuboid, ShapeType::Cuboid) => (
PrimitiveProximityDetector {
detect_proximity: super::detect_proximity_cuboid_cuboid,
..PrimitiveProximityDetector::default()
},
None,
),
(Shape::Polygon(_), Shape::Polygon(_)) => (
(ShapeType::Polygon, ShapeType::Polygon) => (
PrimitiveProximityDetector {
detect_proximity: super::detect_proximity_polygon_polygon,
..PrimitiveProximityDetector::default()
},
None,
),
(Shape::Triangle(_), Shape::Ball(_)) => (
(ShapeType::Triangle, ShapeType::Ball) => (
PrimitiveProximityDetector {
detect_proximity: super::detect_proximity_ball_convex,
..PrimitiveProximityDetector::default()
},
None,
),
(Shape::Ball(_), Shape::Triangle(_)) => (
(ShapeType::Ball, ShapeType::Triangle) => (
PrimitiveProximityDetector {
detect_proximity: super::detect_proximity_ball_convex,
..PrimitiveProximityDetector::default()
},
None,
),
(Shape::Cuboid(_), Shape::Ball(_)) => (
(ShapeType::Cuboid, ShapeType::Ball) => (
PrimitiveProximityDetector {
detect_proximity: super::detect_proximity_ball_convex,
..PrimitiveProximityDetector::default()
},
None,
),
(Shape::Ball(_), Shape::Cuboid(_)) => (
(ShapeType::Ball, ShapeType::Cuboid) => (
PrimitiveProximityDetector {
detect_proximity: super::detect_proximity_ball_convex,
..PrimitiveProximityDetector::default()
},
None,
),
(Shape::Triangle(_), Shape::Cuboid(_)) => (
(ShapeType::Triangle, ShapeType::Cuboid) => (
PrimitiveProximityDetector {
detect_proximity: super::detect_proximity_cuboid_triangle,
..PrimitiveProximityDetector::default()
},
None,
),
(Shape::Cuboid(_), Shape::Triangle(_)) => (
(ShapeType::Cuboid, ShapeType::Triangle) => (
PrimitiveProximityDetector {
detect_proximity: super::detect_proximity_cuboid_triangle,
..PrimitiveProximityDetector::default()
@@ -109,18 +109,18 @@ impl ProximityDispatcher for DefaultProximityDispatcher {
fn dispatch(
&self,
shape1: &Shape,
shape2: &Shape,
shape1: ShapeType,
shape2: ShapeType,
) -> (ProximityPhase, Option<Box<dyn Any + Send + Sync>>) {
match (shape1, shape2) {
(Shape::Trimesh(_), _) => (
(ShapeType::Trimesh, _) => (
ProximityPhase::NearPhase(ProximityDetector {
detect_proximity: super::detect_proximity_trimesh_shape,
..ProximityDetector::default()
}),
Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())),
),
(_, Shape::Trimesh(_)) => (
(_, ShapeType::Trimesh) => (
ProximityPhase::NearPhase(ProximityDetector {
detect_proximity: super::detect_proximity_trimesh_shape,
..ProximityDetector::default()

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

@@ -1,7 +1,7 @@
use crate::geometry::proximity_detector::{
PrimitiveProximityDetectionContext, ProximityDetectionContext,
};
use crate::geometry::{Collider, Proximity, Shape, Trimesh};
use crate::geometry::{Collider, Proximity, ShapeType, Trimesh};
use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
pub struct TrimeshShapeProximityDetectorWorkspace {
@@ -24,9 +24,9 @@ pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> P
let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1];
let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2];
if let Shape::Trimesh(trimesh1) = collider1.shape() {
if let Some(trimesh1) = collider1.shape().as_trimesh() {
do_detect_proximity(trimesh1, collider1, collider2, ctxt)
} else if let Shape::Trimesh(trimesh2) = collider2.shape() {
} else if let Some(trimesh2) = collider2.shape().as_trimesh() {
do_detect_proximity(trimesh2, collider2, collider1, ctxt)
} else {
panic!("Invalid shape types provided.")
@@ -83,6 +83,7 @@ fn do_detect_proximity(
let new_interferences = &workspace.interferences;
let mut old_inter_it = workspace.old_interferences.drain(..).peekable();
let mut best_proximity = Proximity::Disjoint;
let shape_type2 = collider2.shape().shape_type();
for triangle_id in new_interferences.iter() {
if *triangle_id >= trimesh1.num_triangles() {
@@ -107,10 +108,10 @@ fn do_detect_proximity(
};
}
let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id));
let triangle1 = trimesh1.triangle(*triangle_id);
let (proximity_detector, mut workspace2) = ctxt
.dispatcher
.dispatch_primitives(&triangle1, collider2.shape());
.dispatch_primitives(ShapeType::Triangle, shape_type2);
let mut ctxt2 = PrimitiveProximityDetectionContext {
prediction_distance: ctxt.prediction_distance,

107
src/geometry/round_cylinder.rs Normal file
View File

@@ -0,0 +1,107 @@
use crate::geometry::Cylinder;
use crate::math::{Isometry, Point, Vector};
use na::Unit;
use ncollide::query::{
algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection,
};
use ncollide::shape::{FeatureId, SupportMap};
/// 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<f32> 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<f32> for RoundCylinder {
fn toi_and_normal_with_ray(
&self,
m: &Isometry<f32>,
ray: &Ray<f32>,
max_toi: f32,
solid: bool,
) -> Option<RayIntersection<f32>> {
let ls_ray = ray.inverse_transform_by(m);
ncollide::query::ray_intersection_with_support_map_with_params(
&Isometry::identity(),
self,
&mut VoronoiSimplex::new(),
&ls_ray,
max_toi,
solid,
)
.map(|mut res| {
res.normal = m * res.normal;
res
})
}
}
// TODO: if PointQuery had a `project_point_with_normal` method, we could just
// call this and adjust the projected point accordingly.
impl PointQuery<f32> for RoundCylinder {
#[inline]
fn project_point(
&self,
m: &Isometry<f32>,
point: &Point<f32>,
solid: bool,
) -> PointProjection<f32> {
ncollide::query::point_projection_on_support_map(
m,
self,
&mut VoronoiSimplex::new(),
point,
solid,
)
}
#[inline]
fn project_point_with_feature(
&self,
m: &Isometry<f32>,
point: &Point<f32>,
) -> (PointProjection<f32>, FeatureId) {
(self.project_point(m, point, false), FeatureId::Unknown)
}
}

105
src/geometry/sat.rs
View File

@@ -1,9 +1,22 @@
use crate::geometry::{cuboid, Cuboid, Polygon, Triangle};
use crate::geometry::{cuboid, Cuboid, Polygon, Segment, Triangle};
use crate::math::{Isometry, Point, Vector, DIM};
use crate::utils::WSign;
use na::Unit;
use ncollide::shape::{Segment, SupportMap};
use ncollide::shape::SupportMap;
#[allow(dead_code)]
pub fn support_map_support_map_compute_separation(
sm1: &impl SupportMap<f32>,
sm2: &impl SupportMap<f32>,
m12: &Isometry<f32>,
dir1: &Unit<Vector<f32>>,
) -> f32 {
let p1 = sm1.local_support_point_toward(dir1);
let p2 = sm2.support_point_toward(m12, &-*dir1);
(p2 - p1).dot(dir1)
}
#[allow(dead_code)]
pub fn polygon_polygon_compute_separation_features(
p1: &Polygon,
p2: &Polygon,
@@ -58,8 +71,8 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway(
let y2 = pos12 * Vector::y();
let z2 = pos12 * Vector::z();
// We have 3 * 3 = 9 axii to test.
let axii = [
// We have 3 * 3 = 9 axes to test.
let axes = [
// Vector::{x, y ,z}().cross(y2)
Vector::new(0.0, -x2.z, x2.y),
Vector::new(x2.z, 0.0, -x2.x),
@@ -74,7 +87,7 @@ pub fn cuboid_cuboid_find_local_separating_edge_twoway(
Vector::new(-z2.y, z2.x, 0.0),
];
for axis1 in &axii {
for axis1 in &axes {
let norm1 = axis1.norm();
if norm1 > f32::default_epsilon() {
let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line(
@@ -127,7 +140,6 @@ pub fn cuboid_cuboid_find_local_separating_normal_oneway(
*
*
*/
#[cfg(feature = "dim3")]
pub fn cube_support_map_compute_separation_wrt_local_line<S: SupportMap<f32>>(
cube1: &Cuboid,
@@ -149,7 +161,7 @@ pub fn cube_support_map_compute_separation_wrt_local_line<S: SupportMap<f32>>(
pub fn cube_support_map_find_local_separating_edge_twoway(
cube1: &Cuboid,
shape2: &impl SupportMap<f32>,
axii: &[Vector<f32>],
axes: &[Vector<f32>],
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
) -> (f32, Vector<f32>) {
@@ -157,7 +169,7 @@ pub fn cube_support_map_find_local_separating_edge_twoway(
let mut best_separation = -std::f32::MAX;
let mut best_dir = Vector::zeros();
for axis1 in axii {
for axis1 in axes {
if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) {
let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line(
cube1, shape2, pos12, pos21, &axis1,
@@ -184,8 +196,8 @@ pub fn cube_triangle_find_local_separating_edge_twoway(
let y2 = pos12 * (triangle2.c - triangle2.b);
let z2 = pos12 * (triangle2.a - triangle2.c);
// We have 3 * 3 = 3 axii to test.
let axii = [
// We have 3 * 3 = 3 axes to test.
let axes = [
// Vector::{x, y ,z}().cross(y2)
Vector::new(0.0, -x2.z, x2.y),
Vector::new(x2.z, 0.0, -x2.x),
@@ -200,26 +212,26 @@ pub fn cube_triangle_find_local_separating_edge_twoway(
Vector::new(-z2.y, z2.x, 0.0),
];
cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axii, pos12, pos21)
cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axes, pos12, pos21)
}
#[cfg(feature = "dim3")]
pub fn cube_segment_find_local_separating_edge_twoway(
cube1: &Cuboid,
segment2: &Segment<f32>,
segment2: &Segment,
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
) -> (f32, Vector<f32>) {
let x2 = pos12 * (segment2.b - segment2.a);
let axii = [
let axes = [
// Vector::{x, y ,z}().cross(y2)
Vector::new(0.0, -x2.z, x2.y),
Vector::new(x2.z, 0.0, -x2.x),
Vector::new(-x2.y, x2.x, 0.0),
];
cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axii, pos12, pos21)
cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axes, pos12, pos21)
}
pub fn cube_support_map_find_local_separating_normal_oneway<S: SupportMap<f32>>(
@@ -286,9 +298,72 @@ pub fn triangle_cuboid_find_local_separating_normal_oneway(
#[cfg(feature = "dim2")]
pub fn segment_cuboid_find_local_separating_normal_oneway(
segment1: &Segment<f32>,
segment1: &Segment,
shape2: &Cuboid,
pos12: &Isometry<f32>,
) -> (f32, Vector<f32>) {
point_cuboid_find_local_separating_normal_oneway(segment1.a, segment1.normal(), shape2, pos12)
}
/*
* Capsules
*/
#[cfg(feature = "dim3")]
pub fn triangle_segment_find_local_separating_normal_oneway(
triangle1: &Triangle,
segment2: &Segment,
m12: &Isometry<f32>,
) -> (f32, Vector<f32>) {
if let Some(dir) = triangle1.normal() {
let p2a = segment2.support_point_toward(m12, &-dir);
let p2b = segment2.support_point_toward(m12, &dir);
let sep_a = (p2a - triangle1.a).dot(&dir);
let sep_b = -(p2b - triangle1.a).dot(&dir);
if sep_a >= sep_b {
(sep_a, *dir)
} else {
(sep_b, -*dir)
}
} else {
(-f32::MAX, Vector::zeros())
}
}
#[cfg(feature = "dim3")]
pub fn segment_triangle_find_local_separating_edge(
segment1: &Segment,
triangle2: &Triangle,
pos12: &Isometry<f32>,
) -> (f32, Vector<f32>) {
let x2 = pos12 * (triangle2.b - triangle2.a);
let y2 = pos12 * (triangle2.c - triangle2.b);
let z2 = pos12 * (triangle2.a - triangle2.c);
let dir1 = segment1.scaled_direction();
let crosses1 = [dir1.cross(&x2), dir1.cross(&y2), dir1.cross(&z2)];
let axes1 = [
crosses1[0],
crosses1[1],
crosses1[2],
-crosses1[0],
-crosses1[1],
-crosses1[2],
];
let mut max_separation = -f32::MAX;
let mut sep_dir = axes1[0];
for axis1 in &axes1 {
if let Some(axis1) = Unit::try_new(*axis1, 0.0) {
let sep =
support_map_support_map_compute_separation(segment1, triangle2, pos12, &axis1);
if sep > max_separation {
max_separation = sep;
sep_dir = *axis1;
}
}
}
(max_separation, sep_dir)
}

390
src/geometry/shape.rs Normal file
View File

@@ -0,0 +1,390 @@
use crate::dynamics::MassProperties;
use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh};
use crate::math::Isometry;
use downcast_rs::{impl_downcast, DowncastSync};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
use ncollide::bounding_volume::{HasBoundingVolume, AABB};
use ncollide::query::{PointQuery, RayCast};
use num::Zero;
use num_derive::FromPrimitive;
#[cfg(feature = "dim3")]
use {
crate::geometry::{Cone, Cylinder, PolygonalFeatureMap, RoundCylinder},
ncollide::bounding_volume::BoundingVolume,
};
#[derive(Copy, Clone, Debug, FromPrimitive)]
/// Enum representing the type of a shape.
pub enum ShapeType {
/// A ball shape.
Ball = 1,
/// A convex polygon shape.
Polygon,
/// A cuboid shape.
Cuboid,
/// A capsule shape.
Capsule,
/// A 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<f32> + PointQuery<f32> + DowncastSync {
/// Convert this shape as a serializable entity.
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
None
}
/// Computes the AABB of this shape.
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32>;
/// 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<f32> {
self.bounding_volume(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_ball(density, self.radius)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Ball
}
}
// impl Shape for Polygon {
// #[cfg(feature = "serde-serialize")]
// fn as_serialize(&self) -> Option<&dyn Serialize> {
// Some(self as &dyn Serialize)
// }
//
// fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
// 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<f32> {
self.bounding_volume(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_cuboid(density, self.half_extents)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Cuboid
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, 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<f32> {
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<f32> {
self.bounding_volume(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
MassProperties::zero()
}
fn shape_type(&self) -> ShapeType {
ShapeType::Triangle
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, 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<f32> {
self.bounding_volume(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<f32> {
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<f32> {
self.bounding_volume(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
MassProperties::zero()
}
fn shape_type(&self) -> ShapeType {
ShapeType::HeightField
}
}
#[cfg(feature = "dim3")]
impl Shape for Cylinder {
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
MassProperties::from_cylinder(density, self.half_height, self.radius)
}
fn shape_type(&self) -> ShapeType {
ShapeType::Cylinder
}
#[cfg(feature = "dim3")]
fn as_polygonal_feature_map(&self) -> Option<(&dyn PolygonalFeatureMap, 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<f32> {
self.bounding_volume(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<f32> {
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,
))
}
}

45
src/geometry/trimesh.rs
View File

@@ -1,13 +1,9 @@
use crate::geometry::{Triangle, WQuadtree};
use crate::geometry::{PointProjection, Ray, RayIntersection, Triangle, WQuadtree};
use crate::math::{Isometry, Point};
use na::Point3;
use ncollide::bounding_volume::{HasBoundingVolume, AABB};
#[cfg(feature = "dim3")]
use {
crate::geometry::{Ray, RayIntersection},
ncollide::query::RayCast,
};
use ncollide::query::{PointQuery, RayCast};
use ncollide::shape::FeatureId;
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -110,6 +106,41 @@ impl Trimesh {
}
}
impl PointQuery<f32> for Trimesh {
fn project_point(&self, _m: &Isometry<f32>, _pt: &Point<f32>, _solid: bool) -> PointProjection {
// TODO
unimplemented!()
}
fn project_point_with_feature(
&self,
_m: &Isometry<f32>,
_pt: &Point<f32>,
) -> (PointProjection, FeatureId) {
// TODO
unimplemented!()
}
}
#[cfg(feature = "dim2")]
impl RayCast<f32> for Trimesh {
fn toi_and_normal_with_ray(
&self,
_m: &Isometry<f32>,
_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<f32> for Trimesh {
fn toi_and_normal_with_ray(

57
src/geometry/user_callbacks.rs Normal file
View File

@@ -0,0 +1,57 @@
use crate::dynamics::RigidBody;
use crate::geometry::{Collider, SolverFlags};
/// Context given to custom collision filters to filter-out collisions.
pub struct PairFilterContext<'a> {
/// The first collider involved in the potential collision.
pub rigid_body1: &'a RigidBody,
/// The first collider involved in the potential collision.
pub rigid_body2: &'a RigidBody,
/// The first collider involved in the potential collision.
pub collider1: &'a Collider,
/// The first collider involved in the potential collision.
pub collider2: &'a Collider,
}
/// User-defined filter for potential contact pairs detected by the broad-phase.
///
/// This can be used to apply custom logic in order to decide whether two colliders
/// should have their contact computed by the narrow-phase, and if these contact
/// should be solved by the constraints solver
pub trait ContactPairFilter: Send + Sync {
/// Applies the contact pair filter.
///
/// Note that using a contact pair filter will replace the default contact filtering
/// which consists of preventing contact computation between two non-dynamic bodies.
///
/// This filtering method is called after taking into account the colliders collision groups.
///
/// If this returns `None`, then the narrow-phase will ignore this contact pair and
/// not compute any contact manifolds for it.
/// If this returns `Some`, then the narrow-phase will compute contact manifolds for
/// this pair of colliders, and configure them with the returned solver flags. For
/// example, if this returns `Some(SolverFlags::COMPUTE_IMPULSES)` then the contacts
/// will be taken into account by the constraints solver. If this returns
/// `Some(SolverFlags::empty())` then the constraints solver will ignore these
/// contacts.
fn filter_contact_pair(&self, context: &PairFilterContext) -> Option<SolverFlags>;
}
/// User-defined filter for potential proximity pairs detected by the broad-phase.
///
/// This can be used to apply custom logic in order to decide whether two colliders
/// should have their proximity computed by the narrow-phase.
pub trait ProximityPairFilter: Send + Sync {
/// Applies the proximity pair filter.
///
/// Note that using a proximity pair filter will replace the default proximity filtering
/// which consists of preventing proximity computation between two non-dynamic bodies.
///
/// This filtering method is called after taking into account the colliders collision groups.
///
/// If this returns `false`, then the narrow-phase will ignore this pair and
/// not compute any proximity information for it.
/// If this return `true` then the narrow-phase will compute proximity
/// information for this pair.
fn filter_proximity_pair(&self, context: &PairFilterContext) -> bool;
}

33
src/geometry/wquadtree.rs
View File

@@ -539,7 +539,7 @@ fn split_indices_wrt_dim<'a>(
dim: usize,
) -> (&'a mut [usize], &'a mut [usize]) {
let mut icurr = 0;
let mut ilast = indices.len() - 1;
let mut ilast = indices.len();
// The loop condition we can just do 0..indices.len()
// instead of the test icurr < ilast because we know
@@ -549,12 +549,39 @@ fn split_indices_wrt_dim<'a>(
let center = aabbs[i].center();
if center[dim] > split_point[dim] {
indices.swap(icurr, ilast);
ilast -= 1;
indices.swap(icurr, ilast);
} else {
icurr += 1;
}
}
indices.split_at_mut(icurr)
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);
}
}
}

1
src/lib.rs
View File

@@ -10,6 +10,7 @@
#![deny(missing_docs)]
pub extern crate crossbeam;
pub extern crate nalgebra as na;
#[cfg(feature = "dim2")]
pub extern crate ncollide2d as ncollide;

23
src/pipeline/collision_pipeline.rs
View File

@@ -1,7 +1,10 @@
//! Physics pipeline structures.
use crate::dynamics::{JointSet, RigidBodySet};
use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactPairFilter, NarrowPhase,
ProximityPairFilter,
};
use crate::pipeline::EventHandler;
/// The collision pipeline, responsible for performing collision detection between colliders.
@@ -40,6 +43,8 @@ impl CollisionPipeline {
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
contact_pair_filter: Option<&dyn ContactPairFilter>,
proximity_pair_filter: Option<&dyn ProximityPairFilter>,
events: &dyn EventHandler,
) {
bodies.maintain_active_set();
@@ -52,8 +57,20 @@ impl CollisionPipeline {
narrow_phase.register_pairs(colliders, bodies, &self.broad_phase_events, events);
narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events);
narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events);
narrow_phase.compute_contacts(
prediction_distance,
bodies,
colliders,
contact_pair_filter,
events,
);
narrow_phase.compute_proximities(
prediction_distance,
bodies,
colliders,
proximity_pair_filter,
events,
);
bodies.update_active_set_with_contacts(
colliders,

11
src/pipeline/physics_pipeline.rs
View File

@@ -7,7 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBodySet};
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex,
ContactPairFilter, NarrowPhase, ProximityPairFilter,
};
use crate::math::Vector;
use crate::pipeline::EventHandler;
@@ -68,6 +69,8 @@ impl PhysicsPipeline {
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
contact_pair_filter: Option<&dyn ContactPairFilter>,
proximity_pair_filter: Option<&dyn ProximityPairFilter>,
events: &dyn EventHandler,
) {
self.counters.step_started();
@@ -112,12 +115,14 @@ impl PhysicsPipeline {
integration_parameters.prediction_distance,
bodies,
colliders,
contact_pair_filter,
events,
);
narrow_phase.compute_proximities(
integration_parameters.prediction_distance,
bodies,
colliders,
proximity_pair_filter,
events,
);
// println!("Compute contact time: {}", instant::now() - t);
@@ -285,6 +290,8 @@ mod test {
&mut bodies,
&mut colliders,
&mut joints,
None,
None,
&(),
);
}
@@ -327,6 +334,8 @@ mod test {
&mut bodies,
&mut colliders,
&mut joints,
None,
None,
&(),
);
}

35
src/pipeline/query_pipeline.rs
View File

@@ -1,5 +1,7 @@
use crate::dynamics::RigidBodySet;
use crate::geometry::{Collider, ColliderHandle, ColliderSet, Ray, RayIntersection, WQuadtree};
use crate::geometry::{
Collider, ColliderHandle, ColliderSet, InteractionGroups, Ray, RayIntersection, WQuadtree,
};
/// A pipeline for performing queries on all the colliders of a scene.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -59,6 +61,7 @@ impl QueryPipeline {
colliders: &'a ColliderSet,
ray: &Ray,
max_toi: f32,
groups: InteractionGroups,
) -> Option<(ColliderHandle, &'a Collider, RayIntersection)> {
// TODO: avoid allocation?
let mut inter = Vec::new();
@@ -69,10 +72,17 @@ impl QueryPipeline {
for handle in inter {
let collider = &colliders[handle];
if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
if inter.toi < best {
best = inter.toi;
result = Some((handle, collider, inter));
if collider.collision_groups.test(groups) {
if let Some(inter) = collider.shape().toi_and_normal_with_ray(
collider.position(),
ray,
max_toi,
true,
) {
if inter.toi < best {
best = inter.toi;
result = Some((handle, collider, inter));
}
}
}
}
@@ -95,6 +105,7 @@ impl QueryPipeline {
colliders: &'a ColliderSet,
ray: &Ray,
max_toi: f32,
groups: InteractionGroups,
mut callback: impl FnMut(ColliderHandle, &'a Collider, RayIntersection) -> bool,
) {
// TODO: avoid allocation?
@@ -103,9 +114,17 @@ impl QueryPipeline {
for handle in inter {
let collider = &colliders[handle];
if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
if !callback(handle, collider, inter) {
return;
if collider.collision_groups.test(groups) {
if let Some(inter) = collider.shape().toi_and_normal_with_ray(
collider.position(),
ray,
max_toi,
true,
) {
if !callback(handle, collider, inter) {
return;
}
}
}
}

6
src/utils.rs
View File

@@ -19,8 +19,10 @@ use {
// pub(crate) const COS_10_DEGREES: f32 = 0.98480775301;
// pub(crate) const COS_45_DEGREES: f32 = 0.70710678118;
// pub(crate) const SIN_45_DEGREES: f32 = COS_45_DEGREES;
#[cfg(feature = "dim3")]
pub(crate) const COS_1_DEGREES: f32 = 0.99984769515;
pub(crate) const COS_5_DEGREES: f32 = 0.99619469809;
#[cfg(feature = "dim2")]
// #[cfg(feature = "dim2")]
pub(crate) const COS_FRAC_PI_8: f32 = 0.92387953251;
#[cfg(feature = "dim2")]
pub(crate) const SIN_FRAC_PI_8: f32 = 0.38268343236;
@@ -91,7 +93,7 @@ impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
impl WSign<SimdFloat> for SimdFloat {
fn copy_sign_to(self, to: SimdFloat) -> SimdFloat {
self.simd_copysign(to)
to.simd_copysign(self)
}
}