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

Merge pull request #41 from dimforge/cylinder

Browse Source 
Add cylinder and cone support + use a trait-object for shapes.
This commit is contained in:
Sébastien Crozet
2020-10-27 09:57:26 +01:00
committed by GitHub
parent f8acf6a5e9 ffbc3c02c7
commit 93153f5d93
66 changed files with 2100 additions and 637 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.github/workflows/rapier-ci-bench.yml vendored
View File

@@ -18,15 +18,17 @@ jobs:
BENCHBOT_TARGET_COMMIT: ${{ github.event.pull_request.head.sha }}
BENCHBOT_SHA: ${{ github.sha }}
BENCHBOT_HEAD_REF: ${{github.head_ref}}
BENCHBOT_OTHER_BACKENDS: false
runs-on: ubuntu-latest
steps:
- name: Find commit SHA
if: github.ref == 'refs/heads/master'
run: |
echo "::set-env name=BENCHBOT_TARGET_COMMIT::$BENCHBOT_SHA"
echo "::set-env name=BENCHBOT_OTHER_BACKENDS::true"
- name: Send 3D bench message
shell: bash
run: curl -u $BENCHBOT_AMQP_USER:$BENCHBOT_AMQP_PASS
-i -H "content-type:application/json" -X POST
https://$BENCHBOT_AMQP_HOST/api/exchanges/$BENCHBOT_AMQP_VHOST//publish
-d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\" }","payload_encoding":"string"}'
-d'{"properties":{},"routing_key":"benchmark","payload":"{ \"repository\":\"https://github.com/'$BENCHBOT_TARGET_REPO'\", \"branch\":\"'$GITHUB_REF'\", \"commit\":\"'$BENCHBOT_TARGET_COMMIT'\", \"other_backends\":'$BENCHBOT_OTHER_BACKENDS' }","payload_encoding":"string"}'

5
Cargo.toml
View File

@@ -5,6 +5,11 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark
[patch.crates-io]
#wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" }
#simba = { path = "../simba" }
#ncollide2d = { path = "../ncollide/build/ncollide2d" }
#ncollide3d = { path = "../ncollide/build/ncollide3d" }
#nphysics2d = { path = "../nphysics/build/nphysics2d" }
#nphysics3d = { path = "../nphysics/build/nphysics3d" }
#kiss3d = { path = "../kiss3d" }
[profile.release]
#debug = true

2
benchmarks2d/Cargo.toml
View File

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ]
[dependencies]
rand = "0.7"
Inflector = "0.11"
nalgebra = "0.22"
nalgebra = "0.23"
[dependencies.rapier_testbed2d]
path = "../build/rapier_testbed2d"

2
benchmarks3d/Cargo.toml
View File

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
[dependencies]
rand = "0.7"
Inflector = "0.11"
nalgebra = "0.22"
nalgebra = "0.23"
[dependencies.rapier_testbed3d]
path = "../build/rapier_testbed3d"

16
build/rapier2d/Cargo.toml
View File

@@ -1,4 +1,3 @@
# Name idea: bident for 2D and trident for 3D
[package]
name = "rapier2d"
version = "0.2.1"
@@ -22,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
# enabled with the "simd-stable" or "simd-nightly" feature.
simd-is-enabled = [ ]
wasm-bindgen = [ "instant/wasm-bindgen" ]
serde-serialize = [ "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
enhanced-determinism = [ "simba/libm_force", "indexmap" ]
[lib]
@@ -35,18 +34,21 @@ required-features = [ "dim2" ]
vec_map = "0.8"
instant = { version = "0.1", features = [ "now" ]}
num-traits = "0.2"
nalgebra = "0.22"
ncollide2d = "0.24"
simba = "^0.2.1"
approx = "0.3"
nalgebra = "0.23"
ncollide2d = "0.26"
simba = "0.3"
approx = "0.4"
rayon = { version = "1", optional = true }
crossbeam = "0.7"
crossbeam = "0.8"
generational-arena = "0.2"
arrayvec = "0.5"
bit-vec = "0.6"
rustc-hash = "1"
serde = { version = "1", features = [ "derive" ], optional = true }
erased-serde = { version = "0.3", optional = true }
indexmap = { version = "1", features = [ "serde-1" ], optional = true }
downcast-rs = "1.2"
num-derive = "0.3"
[dev-dependencies]
bincode = "1"

17
build/rapier3d/Cargo.toml
View File

@@ -1,4 +1,3 @@
# Name idea: bident for 2D and trident for 3D
[package]
name = "rapier3d"
version = "0.2.1"
@@ -22,7 +21,7 @@ simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ]
# enabled with the "simd-stable" or "simd-nightly" feature.
simd-is-enabled = [ ]
wasm-bindgen = [ "instant/wasm-bindgen" ]
serde-serialize = [ "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
enhanced-determinism = [ "simba/libm_force", "indexmap" ]
[lib]
@@ -35,18 +34,22 @@ required-features = [ "dim3" ]
vec_map = "0.8"
instant = { version = "0.1", features = [ "now" ]}
num-traits = "0.2"
nalgebra = "0.22"
ncollide3d = "0.24"
simba = "^0.2.1"
approx = "0.3"
nalgebra = "0.23"
ncollide3d = "0.26"
simba = "0.3"
approx = "0.4"
rayon = { version = "1", optional = true }
crossbeam = "0.7"
crossbeam = "0.8"
generational-arena = "0.2"
arrayvec = "0.5"
bit-vec = "0.6"
rustc-hash = "1"
serde = { version = "1", features = [ "derive" ], optional = true }
erased-serde = { version = "0.3", optional = true }
indexmap = { version = "1", features = [ "serde-1" ], optional = true }
downcast-rs = "1.2"
num-derive = "0.3"
[dev-dependencies]
bincode = "1"

10
build/rapier_testbed2d/Cargo.toml
View File

@@ -23,17 +23,17 @@ other-backends = [ "wrapped2d", "nphysics2d" ]
[dependencies]
nalgebra = "0.22"
kiss3d = { version = "0.25", features = [ "conrod" ] }
nalgebra = "0.23"
kiss3d = { version = "0.27", features = [ "conrod" ] }
rand = "0.7"
rand_pcg = "0.2"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
ncollide2d = "0.24"
nphysics2d = { version = "0.17", optional = true }
crossbeam = "0.7"
ncollide2d = "0.26"
nphysics2d = { version = "0.18", optional = true }
crossbeam = "0.8"
bincode = "1"
flexbuffers = "0.1"
Inflector = "0.11"

14
build/rapier_testbed3d/Cargo.toml
View File

@@ -22,19 +22,19 @@ parallel = [ "rapier3d/parallel", "num_cpus" ]
other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ]
[dependencies]
nalgebra = "0.22"
kiss3d = { version = "0.25", features = [ "conrod" ] }
nalgebra = "0.23"
kiss3d = { version = "0.27", features = [ "conrod" ] }
rand = "0.7"
rand_pcg = "0.2"
instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
glam = { version = "0.8", optional = true }
glam = { version = "0.9", optional = true }
num_cpus = { version = "1", optional = true }
ncollide3d = "0.24"
nphysics3d = { version = "0.17", optional = true }
physx = { version = "0.6", optional = true }
ncollide3d = "0.26"
nphysics3d = { version = "0.18", optional = true }
physx = { version = "0.7", optional = true }
physx-sys = { version = "0.4", optional = true }
crossbeam = "0.7"
crossbeam = "0.8"
bincode = "1"
flexbuffers = "0.1"
serde_cbor = "0.11"

2
examples2d/Cargo.toml
View File

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier2d/enhanced-determinism" ]
[dependencies]
rand = "0.7"
Inflector = "0.11"
nalgebra = "0.22"
nalgebra = "0.23"
[dependencies.rapier_testbed2d]
path = "../build/rapier_testbed2d"

2
examples3d/Cargo.toml
View File

@@ -14,7 +14,7 @@ enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
[dependencies]
rand = "0.7"
Inflector = "0.11"
nalgebra = "0.22"
nalgebra = "0.23"
[dependencies.rapier_testbed3d]
path = "../build/rapier_testbed3d"

2
examples3d/all_examples3.rs
View File

@@ -13,6 +13,7 @@ use std::cmp::Ordering;
mod add_remove3;
mod compound3;
mod debug_boxes3;
mod debug_cylinder3;
mod debug_triangle3;
mod debug_trimesh3;
mod domino3;
@@ -76,6 +77,7 @@ pub fn main() {
("(Debug) boxes", debug_boxes3::init_world),
("(Debug) triangle", debug_triangle3::init_world),
("(Debug) trimesh", debug_trimesh3::init_world),
("(Debug) cylinder", debug_cylinder3::init_world),
];
// Lexicographic sort, with stress tests moved at the end of the list.

65
examples3d/debug_cylinder3.rs Normal file
View File

@@ -0,0 +1,65 @@
use na::Point3;
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
// This shows a bug when a cylinder is in contact with a very large
// but very thin cuboid. In this case the EPA returns an incorrect
// contact normal, resulting in the cylinder falling through the floor.
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
/*
* Ground
*/
let ground_size = 100.1;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(0.0, -ground_height, 0.0)
.build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
colliders.insert(collider, handle, &mut bodies);
/*
* Create the cubes
*/
let num = 1;
let rad = 1.0;
let shiftx = rad * 2.0 + rad;
let shifty = rad * 2.0 + rad;
let shiftz = rad * 2.0 + rad;
let centerx = shiftx * (num / 2) as f32;
let centery = shifty / 2.0;
let centerz = shiftz * (num / 2) as f32;
let offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
let x = -centerx + offset;
let y = centery + 3.0;
let z = -centerz + offset;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cylinder(rad, rad).build();
colliders.insert(collider, handle, &mut bodies);
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

18
examples3d/heightfield3.rs
View File

@@ -54,13 +54,17 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body);
if j % 2 == 0 {
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
colliders.insert(collider, handle, &mut bodies);
} else {
let collider = ColliderBuilder::ball(rad).build();
colliders.insert(collider, handle, &mut bodies);
}
let collider = match j % 5 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
1 => ColliderBuilder::ball(rad).build(),
// Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
3 => ColliderBuilder::cone(rad, rad).build(),
_ => ColliderBuilder::capsule_y(rad, rad).build(),
};
colliders.insert(collider, handle, &mut bodies);
}
}
}

27
examples3d/primitives3.rs
View File

@@ -15,7 +15,7 @@ pub fn init_world(testbed: &mut Testbed) {
* Ground
*/
let ground_size = 100.1;
let ground_height = 0.1;
let ground_height = 2.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(0.0, -ground_height, 0.0)
@@ -30,29 +30,34 @@ pub fn init_world(testbed: &mut Testbed) {
let num = 8;
let rad = 1.0;
let shift = rad * 2.0 + rad;
let centerx = shift * (num / 2) as f32;
let centery = shift / 2.0;
let centerz = shift * (num / 2) as f32;
let shiftx = rad * 2.0 + rad;
let shifty = rad * 2.0 + rad;
let shiftz = rad * 2.0 + rad;
let centerx = shiftx * (num / 2) as f32;
let centery = shifty / 2.0;
let centerz = shiftz * (num / 2) as f32;
let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
for j in 0usize..20 {
for i in 0..num {
for k in 0usize..num {
let x = i as f32 * shift - centerx + offset;
let y = j as f32 * shift + centery + 3.0;
let z = k as f32 * shift - centerz + offset;
let x = i as f32 * shiftx - centerx + offset;
let y = j as f32 * shifty + centery + 3.0;
let z = k as f32 * shiftz - centerz + offset;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body);
let collider = match j % 2 {
let collider = match j % 5 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
1 => ColliderBuilder::ball(rad).build(),
// 2 => ColliderBuilder::capsule_y(rad, rad).build(),
_ => unreachable!(),
// Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
3 => ColliderBuilder::cone(rad, rad).build(),
_ => ColliderBuilder::capsule_y(rad, rad).build(),
};
colliders.insert(collider, handle, &mut bodies);

18
examples3d/trimesh3.rs
View File

@@ -64,13 +64,17 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body);
if j % 2 == 0 {
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
colliders.insert(collider, handle, &mut bodies);
} else {
let collider = ColliderBuilder::ball(rad).build();
colliders.insert(collider, handle, &mut bodies);
}
let collider = match j % 5 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(),
1 => ColliderBuilder::ball(rad).build(),
// Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(),
3 => ColliderBuilder::cone(rad, rad).build(),
_ => ColliderBuilder::capsule_y(rad, rad).build(),
};
colliders.insert(collider, handle, &mut bodies);
}
}
}

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;

3
src/dynamics/rigid_body.rs
View File

@@ -218,6 +218,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,7 +335,7 @@ impl RigidBody {
}
}
/// A builder for rigid-bodies.
/// A builder for rigid-bodies.
pub struct RigidBodyBuilder {
position: Isometry<f32>,
linvel: Vector<f32>,

16
src/geometry/broad_phase_multi_sap.rs
View File

@@ -335,7 +335,7 @@ impl SAPAxis {
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
struct SAPRegion {
axii: [SAPAxis; DIM],
axes: [SAPAxis; DIM],
existing_proxies: BitVec,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
to_insert: Vec<usize>, // Workspace
@@ -344,14 +344,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(),
need_update: false,
@@ -386,15 +386,15 @@ impl SAPRegion {
// Update endpoints.
let mut deleted_any = false;
for dim in 0..DIM {
self.axii[dim].update_endpoints(dim, proxies, reporting);
deleted_any = self.axii[dim]
self.axes[dim].update_endpoints(dim, proxies, reporting);
deleted_any = self.axes[dim]
.delete_out_of_bounds_proxies(&mut self.existing_proxies)
|| deleted_any;
}
if deleted_any {
for dim in 0..DIM {
self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
}
}
@@ -404,9 +404,9 @@ impl SAPRegion {
if !self.to_insert.is_empty() {
// Insert new proxies.
for dim in 1..DIM {
self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None);
self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None);
}
self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
self.to_insert.clear();
}
}

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;

380
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, 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,
@@ -215,8 +259,8 @@ impl Collider {
}
/// 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 +276,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 +285,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.
@@ -269,7 +300,7 @@ pub struct ColliderBuilder {
impl ColliderBuilder {
/// Initialize a new collider builder with the given shape.
pub fn new(shape: Shape) -> Self {
pub fn new(shape: ColliderShape) -> Self {
Self {
shape,
density: None,
@@ -288,96 +319,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.

21
src/geometry/contact.rs
View File

@@ -430,16 +430,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 +465,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()

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

@@ -139,8 +139,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 +152,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],

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,

39
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,15 @@ fn do_generate_contacts(
let manifolds = &mut ctxt.pair.manifolds;
let prediction_distance = ctxt.prediction_distance;
let dispatcher = ctxt.dispatcher;
let shape_type2 = collider2.shape().shape_type();
heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| {
let position1 = collider1.position();
#[cfg(feature = "dim2")]
let sub_shape1 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0));
let 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,7 +124,7 @@ 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(),
@@ -162,7 +149,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 +160,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>(

11
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() {
@@ -159,10 +160,10 @@ fn do_generate_contacts(
}
let manifold = &mut ctxt.pair.manifolds[i];
let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id));
let triangle1 = trimesh1.triangle(*triangle_id);
let (generator, mut workspace2) = ctxt
.dispatcher
.dispatch_primitives(&triangle1, collider2.shape());
.dispatch_primitives(ShapeType::Triangle, shape_type2);
let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 {
PrimitiveContactGenerationContext {

27
src/geometry/mod.rs
View File

@@ -2,7 +2,7 @@
pub use self::broad_phase_multi_sap::BroadPhase;
pub use self::capsule::Capsule;
pub use self::collider::{Collider, ColliderBuilder, Shape};
pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
pub use self::collider_set::{ColliderHandle, ColliderSet};
pub use self::contact::{
Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory,
@@ -19,9 +19,13 @@ 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 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 +34,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 +50,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 +59,21 @@ 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::shape::{Shape, ShapeType};
mod ball;
mod broad_phase_multi_sap;
mod capsule;
mod collider;
mod collider_set;
mod contact;
@@ -81,3 +96,9 @@ mod trimesh;
mod waabb;
mod wquadtree;
//mod z_order;
mod capsule;
#[cfg(feature = "dim3")]
mod polygonal_feature_map;
#[cfg(feature = "dim3")]
mod round_cylinder;
mod shape;

12
src/geometry/narrow_phase.rs
View File

@@ -197,7 +197,8 @@ impl NarrowPhase {
if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() {
let dispatcher = DefaultProximityDispatcher;
let generator = dispatcher.dispatch(co1.shape(), co2.shape());
let generator = dispatcher
.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
let interaction =
ProximityPair::new(*pair, generator.0, generator.1);
let _ = self.proximity_graph.add_edge(
@@ -226,7 +227,8 @@ impl NarrowPhase {
if self.contact_graph.graph.find_edge(gid1, gid2).is_none() {
let dispatcher = DefaultContactDispatcher;
let generator = dispatcher.dispatch(co1.shape(), co2.shape());
let generator = dispatcher
.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
let interaction = ContactPair::new(*pair, generator.0, generator.1);
let _ = self.contact_graph.add_edge(
co1.contact_graph_index,
@@ -308,7 +310,8 @@ impl NarrowPhase {
if pair.detector.is_none() {
// We need a redispatch for this detector.
// This can happen, e.g., after restoring a snapshot of the narrow-phase.
let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
let (detector, workspace) =
dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
pair.detector = Some(detector);
pair.detector_workspace = workspace;
}
@@ -418,7 +421,8 @@ impl NarrowPhase {
if pair.generator.is_none() {
// We need a redispatch for this generator.
// This can happen, e.g., after restoring a snapshot of the narrow-phase.
let (generator, workspace) = dispatcher.dispatch(co1.shape(), co2.shape());
let (generator, workspace) =
dispatcher.dispatch(co1.shape().shape_type(), co2.shape().shape_type());
pair.generator = Some(generator);
pair.generator_workspace = workspace;
}

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(

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;

12
src/pipeline/query_pipeline.rs
View File

@@ -69,7 +69,11 @@ impl QueryPipeline {
for handle in inter {
let collider = &colliders[handle];
if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
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));
@@ -103,7 +107,11 @@ impl QueryPipeline {
for handle in inter {
let collider = &colliders[handle];
if let Some(inter) = collider.shape().cast_ray(collider.position(), ray, max_toi) {
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)
}
}

72
src_testbed/box2d_backend.rs
View File

@@ -5,7 +5,7 @@ use rapier::counters::Counters;
use rapier::dynamics::{
IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
};
use rapier::geometry::{Collider, ColliderSet, Shape};
use rapier::geometry::{Collider, ColliderSet};
use std::f32;
use wrapped2d::b2;
@@ -167,44 +167,42 @@ impl Box2dWorld {
fixture_def.is_sensor = collider.is_sensor();
fixture_def.filter = b2::Filter::new();
match collider.shape() {
Shape::Ball(b) => {
let mut b2_shape = b2::CircleShape::new();
b2_shape.set_radius(b.radius);
b2_shape.set_position(center);
body.create_fixture(&b2_shape, &mut fixture_def);
}
Shape::Cuboid(c) => {
let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y);
body.create_fixture(&b2_shape, &mut fixture_def);
}
Shape::Polygon(poly) => {
let points: Vec<_> = poly
.vertices()
.iter()
.map(|p| collider.position_wrt_parent() * p)
.map(|p| na_vec_to_b2_vec(p.coords))
.collect();
let b2_shape = b2::PolygonShape::new_with(&points);
body.create_fixture(&b2_shape, &mut fixture_def);
}
Shape::HeightField(heightfield) => {
let mut segments = heightfield.segments();
let seg1 = segments.next().unwrap();
let mut vertices = vec![
na_vec_to_b2_vec(seg1.a.coords),
na_vec_to_b2_vec(seg1.b.coords),
];
let shape = collider.shape();
// TODO: this will not handle holes properly.
segments.for_each(|seg| {
vertices.push(na_vec_to_b2_vec(seg.b.coords));
});
if let Some(b) = shape.as_ball() {
let mut b2_shape = b2::CircleShape::new();
b2_shape.set_radius(b.radius);
b2_shape.set_position(center);
body.create_fixture(&b2_shape, &mut fixture_def);
} else if let Some(c) = shape.as_cuboid() {
let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y);
body.create_fixture(&b2_shape, &mut fixture_def);
// } else if let Some(polygon) = shape.as_polygon() {
// let points: Vec<_> = poly
// .vertices()
// .iter()
// .map(|p| collider.position_wrt_parent() * p)
// .map(|p| na_vec_to_b2_vec(p.coords))
// .collect();
// let b2_shape = b2::PolygonShape::new_with(&points);
// body.create_fixture(&b2_shape, &mut fixture_def);
} else if let Some(heightfield) = shape.as_heightfield() {
let mut segments = heightfield.segments();
let seg1 = segments.next().unwrap();
let mut vertices = vec![
na_vec_to_b2_vec(seg1.a.coords),
na_vec_to_b2_vec(seg1.b.coords),
];
let b2_shape = b2::ChainShape::new_chain(&vertices);
body.create_fixture(&b2_shape, &mut fixture_def);
}
_ => eprintln!("Creating a shape unknown to the Box2d backend."),
// TODO: this will not handle holes properly.
segments.for_each(|seg| {
vertices.push(na_vec_to_b2_vec(seg.b.coords));
});
let b2_shape = b2::ChainShape::new_chain(&vertices);
body.create_fixture(&b2_shape, &mut fixture_def);
} else {
eprintln!("Creating a shape unknown to the Box2d backend.");
}
}

88
src_testbed/engine.rs
View File

@@ -9,11 +9,10 @@ use na::Point3;
use crate::math::Point;
use crate::objects::ball::Ball;
use crate::objects::box_node::Box as BoxNode;
use crate::objects::convex::Convex;
use crate::objects::heightfield::HeightField;
use crate::objects::node::{GraphicsNode, Node};
use rapier::dynamics::{RigidBodyHandle, RigidBodySet};
use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
use rapier::geometry::{Collider, ColliderHandle, ColliderSet};
//use crate::objects::capsule::Capsule;
//use crate::objects::convex::Convex;
//#[cfg(feature = "fluids")]
@@ -26,6 +25,10 @@ use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape};
//#[cfg(feature = "fluids")]
//use crate::objects::FluidRenderingMode;
use crate::objects::capsule::Capsule;
#[cfg(feature = "dim3")]
use crate::objects::cone::Cone;
#[cfg(feature = "dim3")]
use crate::objects::cylinder::Cylinder;
use crate::objects::mesh::Mesh;
use rand::{Rng, SeedableRng};
use rand_pcg::Pcg32;
@@ -349,33 +352,44 @@ impl GraphicsManager {
color: Point3<f32>,
out: &mut Vec<Node>,
) {
match collider.shape() {
Shape::Ball(ball) => {
out.push(Node::Ball(Ball::new(handle, ball.radius, color, window)))
}
Shape::Polygon(poly) => out.push(Node::Convex(Convex::new(
handle,
poly.vertices().to_vec(),
color,
window,
))),
Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new(
let shape = collider.shape();
if let Some(ball) = shape.as_ball() {
out.push(Node::Ball(Ball::new(handle, ball.radius, color, window)))
}
// Shape::Polygon(poly) => out.push(Node::Convex(Convex::new(
// handle,
// poly.vertices().to_vec(),
// color,
// window,
// ))),
if let Some(cuboid) = shape.as_cuboid() {
out.push(Node::Box(BoxNode::new(
handle,
cuboid.half_extents,
color,
window,
))),
Shape::Capsule(capsule) => {
out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
}
Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new(
)))
}
if let Some(capsule) = shape.as_capsule() {
out.push(Node::Capsule(Capsule::new(handle, capsule, color, window)))
}
if let Some(triangle) = shape.as_triangle() {
out.push(Node::Mesh(Mesh::new(
handle,
vec![triangle.a, triangle.b, triangle.c],
vec![Point3::new(0, 1, 2)],
color,
window,
))),
Shape::Trimesh(trimesh) => out.push(Node::Mesh(Mesh::new(
)))
}
if let Some(trimesh) = shape.as_trimesh() {
out.push(Node::Mesh(Mesh::new(
handle,
trimesh.vertices().to_vec(),
trimesh
@@ -385,13 +399,41 @@ impl GraphicsManager {
.collect(),
color,
window,
))),
Shape::HeightField(heightfield) => out.push(Node::HeightField(HeightField::new(
)))
}
if let Some(heightfield) = shape.as_heightfield() {
out.push(Node::HeightField(HeightField::new(
handle,
heightfield,
color,
window,
))),
)))
}
#[cfg(feature = "dim3")]
if let Some(cylinder) = shape
.as_cylinder()
.or(shape.as_round_cylinder().map(|r| &r.cylinder))
{
out.push(Node::Cylinder(Cylinder::new(
handle,
cylinder.half_height,
cylinder.radius,
color,
window,
)))
}
#[cfg(feature = "dim3")]
if let Some(cone) = shape.as_cone() {
out.push(Node::Cone(Cone::new(
handle,
cone.half_height,
cone.radius,
color,
window,
)))
}
}

51
src_testbed/nphysics_backend.rs
View File

@@ -12,7 +12,7 @@ use rapier::counters::Counters;
use rapier::dynamics::{
IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
};
use rapier::geometry::{Collider, ColliderSet, Shape};
use rapier::geometry::{Collider, ColliderSet};
use rapier::math::Vector;
use std::collections::HashMap;
#[cfg(feature = "dim3")]
@@ -177,28 +177,37 @@ fn nphysics_collider_from_rapier_collider(
) -> Option<ColliderDesc<f32>> {
let margin = ColliderDesc::<f32>::default_margin();
let mut pos = *collider.position_wrt_parent();
let shape = collider.shape();
let shape = match collider.shape() {
Shape::Cuboid(cuboid) => {
ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin)))
}
Shape::Ball(ball) => ShapeHandle::new(Ball::new(ball.radius - margin)),
Shape::Capsule(capsule) => {
pos *= capsule.transform_wrt_y();
ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
}
Shape::HeightField(heightfield) => ShapeHandle::new(heightfield.clone()),
let shape = if let Some(cuboid) = shape.as_cuboid() {
ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin)))
} else if let Some(ball) = shape.as_ball() {
ShapeHandle::new(Ball::new(ball.radius - margin))
} else if let Some(capsule) = shape.as_capsule() {
pos *= capsule.transform_wrt_y();
ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
} else if let Some(heightfield) = shape.as_heightfield() {
ShapeHandle::new(heightfield.clone())
} else {
#[cfg(feature = "dim3")]
Shape::Trimesh(trimesh) => ShapeHandle::new(TriMesh::new(
trimesh.vertices().to_vec(),
trimesh
.indices()
.iter()
.map(|idx| na::convert(*idx))
.collect(),
None,
)),
_ => return None,
if let Some(trimesh) = shape.as_trimesh() {
ShapeHandle::new(TriMesh::new(
trimesh.vertices().to_vec(),
trimesh
.indices()
.iter()
.map(|idx| na::convert(*idx))
.collect(),
None,
))
} else {
return None;
}
#[cfg(feature = "dim2")]
{
return None;
}
};
let density = if is_dynamic { collider.density() } else { 0.0 };

74
src_testbed/objects/cone.rs Normal file
View File

@@ -0,0 +1,74 @@
use crate::objects::node::{self, GraphicsNode};
use kiss3d::window::Window;
use na::Point3;
use rapier::geometry::{ColliderHandle, ColliderSet};
use rapier::math::Isometry;
pub struct Cone {
color: Point3<f32>,
base_color: Point3<f32>,
gfx: GraphicsNode,
collider: ColliderHandle,
}
impl Cone {
pub fn new(
collider: ColliderHandle,
half_height: f32,
radius: f32,
color: Point3<f32>,
window: &mut Window,
) -> Cone {
#[cfg(feature = "dim2")]
let node = window.add_rectangle(radius, half_height);
#[cfg(feature = "dim3")]
let node = window.add_cone(radius, half_height * 2.0);
let mut res = Cone {
color,
base_color: color,
gfx: node,
collider,
};
// res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
res.gfx.set_color(color.x, color.y, color.z);
res
}
pub fn select(&mut self) {
self.color = Point3::new(1.0, 0.0, 0.0);
}
pub fn unselect(&mut self) {
self.color = self.base_color;
}
pub fn set_color(&mut self, color: Point3<f32>) {
self.gfx.set_color(color.x, color.y, color.z);
self.color = color;
self.base_color = color;
}
pub fn update(&mut self, colliders: &ColliderSet) {
node::update_scene_node(
&mut self.gfx,
colliders,
self.collider,
&self.color,
&Isometry::identity(),
);
}
pub fn scene_node(&self) -> &GraphicsNode {
&self.gfx
}
pub fn scene_node_mut(&mut self) -> &mut GraphicsNode {
&mut self.gfx
}
pub fn object(&self) -> ColliderHandle {
self.collider
}
}

74
src_testbed/objects/cylinder.rs Normal file
View File

@@ -0,0 +1,74 @@
use crate::objects::node::{self, GraphicsNode};
use kiss3d::window::Window;
use na::Point3;
use rapier::geometry::{ColliderHandle, ColliderSet};
use rapier::math::Isometry;
pub struct Cylinder {
color: Point3<f32>,
base_color: Point3<f32>,
gfx: GraphicsNode,
collider: ColliderHandle,
}
impl Cylinder {
pub fn new(
collider: ColliderHandle,
half_height: f32,
radius: f32,
color: Point3<f32>,
window: &mut Window,
) -> Cylinder {
#[cfg(feature = "dim2")]
let node = window.add_rectangle(radius, half_height);
#[cfg(feature = "dim3")]
let node = window.add_cylinder(radius, half_height * 2.0);
let mut res = Cylinder {
color,
base_color: color,
gfx: node,
collider,
};
// res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten");
res.gfx.set_color(color.x, color.y, color.z);
res
}
pub fn select(&mut self) {
self.color = Point3::new(1.0, 0.0, 0.0);
}
pub fn unselect(&mut self) {
self.color = self.base_color;
}
pub fn set_color(&mut self, color: Point3<f32>) {
self.gfx.set_color(color.x, color.y, color.z);
self.color = color;
self.base_color = color;
}
pub fn update(&mut self, colliders: &ColliderSet) {
node::update_scene_node(
&mut self.gfx,
colliders,
self.collider,
&self.color,
&Isometry::identity(),
);
}
pub fn scene_node(&self) -> &GraphicsNode {
&self.gfx
}
pub fn scene_node_mut(&mut self) -> &mut GraphicsNode {
&mut self.gfx
}
pub fn object(&self) -> ColliderHandle {
self.collider
}
}

2
src_testbed/objects/mod.rs
View File

@@ -1,7 +1,9 @@
pub mod ball;
pub mod box_node;
pub mod capsule;
pub mod cone;
pub mod convex;
pub mod cylinder;
pub mod heightfield;
pub mod mesh;
pub mod node;

18
src_testbed/objects/node.rs
View File

@@ -10,6 +10,8 @@ use crate::objects::mesh::Mesh;
use kiss3d::window::Window;
use na::Point3;
use crate::objects::cone::Cone;
use crate::objects::cylinder::Cylinder;
use rapier::geometry::{ColliderHandle, ColliderSet};
use rapier::math::Isometry;
@@ -28,6 +30,8 @@ pub enum Node {
// Polyline(Polyline),
Mesh(Mesh),
Convex(Convex),
Cylinder(Cylinder),
Cone(Cone),
}
impl Node {
@@ -42,6 +46,8 @@ impl Node {
// Node::Polyline(ref mut n) => n.select(),
Node::Mesh(ref mut n) => n.select(),
Node::Convex(ref mut n) => n.select(),
Node::Cylinder(ref mut n) => n.select(),
Node::Cone(ref mut n) => n.select(),
}
}
@@ -56,6 +62,8 @@ impl Node {
// Node::Polyline(ref mut n) => n.unselect(),
Node::Mesh(ref mut n) => n.unselect(),
Node::Convex(ref mut n) => n.unselect(),
Node::Cylinder(ref mut n) => n.unselect(),
Node::Cone(ref mut n) => n.unselect(),
}
}
@@ -70,6 +78,8 @@ impl Node {
// Node::Polyline(ref mut n) => n.update(colliders),
Node::Mesh(ref mut n) => n.update(colliders),
Node::Convex(ref mut n) => n.update(colliders),
Node::Cylinder(ref mut n) => n.update(colliders),
Node::Cone(ref mut n) => n.update(colliders),
}
}
@@ -97,6 +107,8 @@ impl Node {
Node::HeightField(ref n) => Some(n.scene_node()),
Node::Mesh(ref n) => Some(n.scene_node()),
Node::Convex(ref n) => Some(n.scene_node()),
Node::Cylinder(ref n) => Some(n.scene_node()),
Node::Cone(ref n) => Some(n.scene_node()),
#[cfg(feature = "dim2")]
_ => None,
}
@@ -113,6 +125,8 @@ impl Node {
Node::HeightField(ref mut n) => Some(n.scene_node_mut()),
Node::Mesh(ref mut n) => Some(n.scene_node_mut()),
Node::Convex(ref mut n) => Some(n.scene_node_mut()),
Node::Cylinder(ref mut n) => Some(n.scene_node_mut()),
Node::Cone(ref mut n) => Some(n.scene_node_mut()),
#[cfg(feature = "dim2")]
_ => None,
}
@@ -129,6 +143,8 @@ impl Node {
// Node::Polyline(ref n) => n.object(),
Node::Mesh(ref n) => n.object(),
Node::Convex(ref n) => n.object(),
Node::Cylinder(ref n) => n.object(),
Node::Cone(ref n) => n.object(),
}
}
@@ -143,6 +159,8 @@ impl Node {
// Node::Polyline(ref mut n) => n.set_color(color),
Node::Mesh(ref mut n) => n.set_color(color),
Node::Convex(ref mut n) => n.set_color(color),
Node::Cylinder(ref mut n) => n.set_color(color),
Node::Cone(ref mut n) => n.set_color(color),
}
}
}

43
src_testbed/physx_backend.rs
View File

@@ -6,7 +6,7 @@ use rapier::counters::Counters;
use rapier::dynamics::{
IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
};
use rapier::geometry::{Collider, ColliderSet, Shape};
use rapier::geometry::{Collider, ColliderSet};
use rapier::utils::WBasis;
use std::collections::HashMap;
@@ -422,27 +422,29 @@ fn physx_collider_from_rapier_collider(
collider: &Collider,
) -> Option<(ColliderDesc, Isometry3<f32>)> {
let mut local_pose = *collider.position_wrt_parent();
let desc = match collider.shape() {
Shape::Cuboid(cuboid) => ColliderDesc::Box(
let shape = collider.shape();
let desc = if let Some(cuboid) = shape.as_cuboid() {
ColliderDesc::Box(
cuboid.half_extents.x,
cuboid.half_extents.y,
cuboid.half_extents.z,
),
Shape::Ball(ball) => ColliderDesc::Sphere(ball.radius),
Shape::Capsule(capsule) => {
let center = capsule.center();
let mut dir = capsule.b - capsule.a;
)
} else if let Some(ball) = shape.as_ball() {
ColliderDesc::Sphere(ball.radius)
} else if let Some(capsule) = shape.as_capsule() {
let center = capsule.center();
let mut dir = capsule.segment.b - capsule.segment.a;
if dir.x < 0.0 {
dir = -dir;
}
let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
local_pose *=
Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
ColliderDesc::Capsule(capsule.radius, capsule.height())
if dir.x < 0.0 {
dir = -dir;
}
Shape::Trimesh(trimesh) => ColliderDesc::TriMesh {
let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
local_pose *= Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
ColliderDesc::Capsule(capsule.radius, capsule.height())
} else if let Some(trimesh) = shape.as_trimesh() {
ColliderDesc::TriMesh {
vertices: trimesh
.vertices()
.iter()
@@ -450,11 +452,10 @@ fn physx_collider_from_rapier_collider(
.collect(),
indices: trimesh.flat_indices().to_vec(),
mesh_scale: Vector3::repeat(1.0).into_glam(),
},
_ => {
eprintln!("Creating a shape unknown to the PhysX backend.");
return None;
}
} else {
eprintln!("Creating a shape unknown to the PhysX backend.");
return None;
};
Some((desc, local_pose))

4
src_testbed/testbed.rs
View File

@@ -21,7 +21,9 @@ use na::{self, Point2, Point3, Vector3};
use rapier::dynamics::{
ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet,
};
use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent, Ray};
#[cfg(feature = "dim3")]
use rapier::geometry::Ray;
use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent};
use rapier::math::Vector;
use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline, QueryPipeline};
#[cfg(feature = "fluids")]