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

Outsource the contact manifold, SAT, and some shapes.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-08 17:31:49 +01:00
parent fd3b4801b6
commit 9bf1321f8f
62 changed files with 552 additions and 2904 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Cargo.toml
View File

@@ -10,6 +10,8 @@ members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "benchmark
#nphysics2d = { path = "../nphysics/build/nphysics2d" }
#nphysics3d = { path = "../nphysics/build/nphysics3d" }
#kiss3d = { path = "../kiss3d" }
buckler2d = { path = "../buckler/build/buckler2d" }
buckler3d = { path = "../buckler/build/buckler3d" }
[profile.release]
#debug = true

8
benchmarks3d/heightfield3.rs
View File

@@ -1,4 +1,4 @@
use na::{DMatrix, Point3, Vector3};
use na::{ComplexField, DMatrix, Point3, Vector3};
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
@@ -23,7 +23,11 @@ pub fn init_world(testbed: &mut Testbed) {
} else {
let x = i as f32 * ground_size.x / (nsubdivs as f32);
let z = j as f32 * ground_size.z / (nsubdivs as f32);
x.sin() + z.cos()
// NOTE: make sure we use the sin/cos from simba to ensure
// cross-platform determinism of the example when the
// enhanced_determinism feature is enabled.
<f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
}
});

39
benchmarks3d/trimesh3.rs
View File

@@ -1,6 +1,6 @@
use na::Point3;
use na::{ComplexField, DMatrix, Point3, Vector3};
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField};
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
@@ -14,28 +14,27 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Ground
*/
let ground_size = 200.0f32;
let ground_height = 1.0;
let ground_size = Vector3::new(200.0, 1.0, 200.0);
let nsubdivs = 20;
let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs);
let indices = quad
.flat_indices()
.chunks(3)
.map(|is| Point3::new(is[0], is[2], is[1]))
.collect();
let mut vertices = quad.coords;
let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs {
10.0
} else {
let x = i as f32 * ground_size.x / (nsubdivs as f32);
let z = j as f32 * ground_size.z / (nsubdivs as f32);
// ncollide generates a quad with `z` as the normal.
// so we switch z and y here and set a random altitude at each point.
for p in vertices.iter_mut() {
p.z = p.y;
p.y = (p.x.sin() + p.z.cos()) * ground_height;
if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 {
p.y = 10.0;
// NOTE: make sure we use the sin/cos from simba to ensure
// cross-platform determinism of the example when the
// enhanced_determinism feature is enabled.
<f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
}
}
});
// Here we will build our trimesh from the mesh representation of an
// heightfield.
let heightfield = HeightField::new(heights, ground_size);
let (vertices, indices) = heightfield.to_trimesh();
let rigid_body = RigidBodyBuilder::new_static().build();
let handle = bodies.insert(rigid_body);

4
build/rapier2d/Cargo.toml
View File

@@ -21,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 = [ "erased-serde", "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ]
enhanced-determinism = [ "simba/libm_force", "indexmap" ]
[lib]
@@ -35,7 +35,7 @@ vec_map = "0.8"
instant = { version = "0.1", features = [ "now" ]}
num-traits = "0.2"
nalgebra = "0.23"
ncollide2d = "0.26"
buckler2d = "0.1"
simba = "0.3"
approx = "0.4"
rayon = { version = "1", optional = true }

4
build/rapier3d/Cargo.toml
View File

@@ -21,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 = [ "erased-serde", "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
serde-serialize = [ "erased-serde", "nalgebra/serde-serialize", "buckler3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ]
enhanced-determinism = [ "simba/libm_force", "indexmap" ]
[lib]
@@ -35,7 +35,7 @@ vec_map = "0.8"
instant = { version = "0.1", features = [ "now" ]}
num-traits = "0.2"
nalgebra = "0.23"
ncollide3d = "0.26"
buckler3d = "0.1"
simba = "0.3"
approx = "0.4"
rayon = { version = "1", optional = true }

1
build/rapier_testbed2d/Cargo.toml
View File

@@ -31,6 +31,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
num_cpus = { version = "1", optional = true }
wrapped2d = { version = "0.4", optional = true }
buckler2d = "0.1"
ncollide2d = "0.26"
nphysics2d = { version = "0.18", optional = true }
crossbeam = "0.8"

1
build/rapier_testbed3d/Cargo.toml
View File

@@ -30,6 +30,7 @@ instant = { version = "0.1", features = [ "web-sys", "now" ]}
bitflags = "1"
glam = { version = "0.10", optional = true }
num_cpus = { version = "1", optional = true }
buckler3d = "0.1"
ncollide3d = "0.26"
nphysics3d = { version = "0.18", optional = true }
physx = { version = "0.8", optional = true }

42
examples3d/trimesh3.rs
View File

@@ -1,6 +1,6 @@
use na::{ComplexField, Point3};
use na::{ComplexField, DMatrix, Point3, Vector3};
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet, HeightField};
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
@@ -14,31 +14,27 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Ground
*/
let ground_size = 100.0f32;
let ground_height = 1.0;
let ground_size = Vector3::new(100.0, 1.0, 100.0);
let nsubdivs = 20;
let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs);
let indices = quad
.flat_indices()
.chunks(3)
.map(|is| Point3::new(is[0], is[2], is[1]))
.collect();
let mut vertices = quad.coords;
let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| {
if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs {
10.0
} else {
let x = i as f32 * ground_size.x / (nsubdivs as f32);
let z = j as f32 * ground_size.z / (nsubdivs as f32);
// ncollide generates a quad with `z` as the normal.
// so we switch z and y here and set a random altitude at each point.
for p in vertices.iter_mut() {
p.z = p.y;
// NOTE: make sure we use the sin/cos from simba to ensure
// cross-platform determinism of the example when the
// enhanced_determinism feature is enabled.
p.y = (<f32 as ComplexField>::sin(p.x) + <f32 as ComplexField>::cos(p.z)) * ground_height;
if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 {
p.y = 10.0;
// NOTE: make sure we use the sin/cos from simba to ensure
// cross-platform determinism of the example when the
// enhanced_determinism feature is enabled.
<f32 as ComplexField>::sin(x) + <f32 as ComplexField>::cos(z)
}
}
});
// Here we will build our trimesh from the mesh representation of an
// heightfield.
let heightfield = HeightField::new(heights, ground_size);
let (vertices, indices) = heightfield.to_trimesh();
let rigid_body = RigidBodyBuilder::new_static().build();
let handle = bodies.insert(rigid_body);

2
src/dynamics/rigid_body_set.rs
View File

@@ -479,7 +479,7 @@ impl RigidBodySet {
if let Some(contacts) = narrow_phase.contacts_with(*collider_handle) {
for inter in contacts {
for manifold in &inter.2.manifolds {
if manifold.num_active_contacts() > 0 {
if manifold.num_active_contacts > 0 {
let other = crate::utils::other_handle(
(inter.0, inter.1),
*collider_handle,

8
src/dynamics/solver/categorization.rs
View File

@@ -12,8 +12,8 @@ pub(crate) fn categorize_position_contacts(
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
if !rb1.is_dynamic() || !rb2.is_dynamic() {
match manifold.kinematics.category {
@@ -38,8 +38,8 @@ pub(crate) fn categorize_velocity_contacts(
) {
for manifold_i in manifold_indices {
let manifold = &manifolds[*manifold_i];
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
if !rb1.is_dynamic() || !rb2.is_dynamic() {
out_ground.push(*manifold_i)

12
src/dynamics/solver/interaction_groups.rs
View File

@@ -12,7 +12,7 @@ pub(crate) trait PairInteraction {
impl<'a> PairInteraction for &'a mut ContactManifold {
fn body_pair(&self) -> BodyPair {
self.body_pair
self.data.body_pair
}
}
@@ -77,7 +77,7 @@ impl ParallelInteractionGroups {
.iter()
.zip(self.interaction_colors.iter_mut())
{
let body_pair = interactions[*interaction_id].body_pair();
let body_pair = interactions[*interaction_id].data.body_pair();
let rb1 = &bodies[body_pair.body1];
let rb2 = &bodies[body_pair.body2];
@@ -338,7 +338,7 @@ impl InteractionGroups {
let mut occupied_mask = 0u128;
let max_interaction_points = interaction_indices
.iter()
.map(|i| interactions[*i].num_active_contacts())
.map(|i| interactions[*i].num_active_contacts)
.max()
.unwrap_or(1);
@@ -351,12 +351,12 @@ impl InteractionGroups {
// FIXME: how could we avoid iterating
// on each interaction at every iteration on k?
if interaction.num_active_contacts() != k {
if interaction.num_active_contacts != k {
continue;
}
let body1 = &bodies[interaction.body_pair.body1];
let body2 = &bodies[interaction.body_pair.body2];
let body1 = &bodies[interaction.data.body_pair.body1];
let body2 = &bodies[interaction.data.body_pair.body2];
let is_static1 = !body1.is_dynamic();
let is_static2 = !body2.is_dynamic();

4
src/dynamics/solver/parallel_velocity_solver.rs
View File

@@ -247,13 +247,13 @@ impl ParallelVelocitySolverPart<AnyJointVelocityConstraint> {
VelocityConstraintDesc::NongroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies);
self.constraints[joints[0].constraint_index] = constraint;
self.constraints[joints[0].data.constraint_index] = constraint;
}
#[cfg(feature = "simd-is-enabled")]
VelocityConstraintDesc::GroundGrouped(joint_id) => {
let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH];
let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies);
self.constraints[joints[0].constraint_index] = constraint;
self.constraints[joints[0].data.constraint_index] = constraint;
}
}
}

12
src/dynamics/solver/position_constraint.rs
View File

@@ -88,8 +88,8 @@ impl PositionConstraint {
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
let shift1 = manifold.local_n1 * -manifold.kinematics.radius1;
let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
let radius =
@@ -104,8 +104,8 @@ impl PositionConstraint {
let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
for l in 0..manifold_points.len() {
local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1);
local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2);
local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1);
local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2);
}
let constraint = PositionConstraint {
@@ -132,10 +132,10 @@ impl PositionConstraint {
}
} else {
if manifold.kinematics.category == KinematicsCategory::PointPoint {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyPositionConstraint::NongroupedPointPoint(constraint);
} else {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyPositionConstraint::NongroupedPlanePoint(constraint);
}
}

12
src/dynamics/solver/position_constraint_wide.rs
View File

@@ -35,8 +35,8 @@ impl WPositionConstraint {
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
let rbs1 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let rbs2 = array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let sqrt_ii1: AngularInertia<SimdFloat> =
@@ -51,8 +51,8 @@ impl WPositionConstraint {
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]);
let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -100,10 +100,10 @@ impl WPositionConstraint {
}
} else {
if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyPositionConstraint::GroupedPointPoint(constraint);
} else {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyPositionConstraint::GroupedPlanePoint(constraint);
}
}

16
src/dynamics/solver/position_ground_constraint.rs
View File

@@ -28,8 +28,8 @@ impl PositionGroundConstraint {
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let mut rb1 = &bodies[manifold.body_pair.body1];
let mut rb2 = &bodies[manifold.body_pair.body2];
let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flip = !rb2.is_dynamic();
let local_n1;
@@ -41,13 +41,13 @@ impl PositionGroundConstraint {
std::mem::swap(&mut rb1, &mut rb2);
local_n1 = manifold.local_n2;
local_n2 = manifold.local_n1;
delta1 = &manifold.delta2;
delta2 = &manifold.delta1;
delta1 = &manifold.data.delta2;
delta2 = &manifold.data.delta1;
} else {
local_n1 = manifold.local_n1;
local_n2 = manifold.local_n2;
delta1 = &manifold.delta1;
delta2 = &manifold.delta2;
delta1 = &manifold.data.delta1;
delta2 = &manifold.data.delta2;
};
let coll_pos1 = rb1.position * delta1;
@@ -105,10 +105,10 @@ impl PositionGroundConstraint {
}
} else {
if manifold.kinematics.category == KinematicsCategory::PointPoint {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyPositionConstraint::NongroupedPointPointGround(constraint);
} else {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyPositionConstraint::NongroupedPlanePointGround(constraint);
}
}

14
src/dynamics/solver/position_ground_constraint_wide.rs
View File

@@ -32,8 +32,10 @@ impl WPositionGroundConstraint {
out_constraints: &mut Vec<AnyPositionConstraint>,
push: bool,
) {
let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH];
let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH];
let mut rbs1 =
array![|ii| bodies.get(manifolds[ii].data.body_pair.body1).unwrap(); SIMD_WIDTH];
let mut rbs2 =
array![|ii| bodies.get(manifolds[ii].data.body_pair.body2).unwrap(); SIMD_WIDTH];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
@@ -55,10 +57,10 @@ impl WPositionGroundConstraint {
);
let delta1 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH],
);
let delta2 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
);
let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
@@ -112,10 +114,10 @@ impl WPositionGroundConstraint {
}
} else {
if manifolds[0].kinematics.category == KinematicsCategory::PointPoint {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyPositionConstraint::GroupedPointPointGround(constraint);
} else {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyPositionConstraint::GroupedPlanePointGround(constraint);
}
}

30
src/dynamics/solver/velocity_constraint.rs
View File

@@ -144,14 +144,14 @@ impl VelocityConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
let mj_lambda1 = rb1.active_set_offset;
let mj_lambda2 = rb2.active_set_offset;
let pos_coll1 = rb1.position * manifold.delta1;
let pos_coll2 = rb2.position * manifold.delta2;
let pos_coll1 = rb1.position * manifold.data.delta1;
let pos_coll2 = rb2.position * manifold.data.delta2;
let force_dir1 = pos_coll1 * (-manifold.local_n1);
let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
for (l, manifold_points) in manifold
.active_contacts()
@@ -164,7 +164,7 @@ impl VelocityConstraint {
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: rb1.mass_properties.inv_mass,
im2: rb2.mass_properties.inv_mass,
limit: manifold.friction,
limit: manifold.data.friction,
mj_lambda1,
mj_lambda2,
manifold_id,
@@ -207,7 +207,7 @@ impl VelocityConstraint {
constraint.dir1 = force_dir1;
constraint.im1 = rb1.mass_properties.inv_mass;
constraint.im2 = rb2.mass_properties.inv_mass;
constraint.limit = manifold.friction;
constraint.limit = manifold.data.friction;
constraint.mj_lambda1 = mj_lambda1;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
@@ -241,12 +241,12 @@ impl VelocityConstraint {
let mut rhs = (vel1 - vel2).dot(&force_dir1);
if rhs <= -params.restitution_velocity_threshold {
rhs += manifold.restitution * rhs
rhs += manifold.data.restitution * rhs
}
rhs += manifold_point.dist.max(0.0) * params.inv_dt();
let impulse = manifold_points[k].impulse * warmstart_coeff;
let impulse = manifold_points[k].data.impulse * warmstart_coeff;
constraint.elements[k].normal_part = VelocityConstraintElementPart {
gcross1,
@@ -275,9 +275,9 @@ impl VelocityConstraint {
+ gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")]
let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff;
constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart {
gcross1,
@@ -294,7 +294,7 @@ impl VelocityConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint));
} else {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyVelocityConstraint::Nongrouped(constraint);
}
}
@@ -388,15 +388,15 @@ impl VelocityConstraint {
for k in 0..self.num_contacts as usize {
let active_contacts = manifold.active_contacts_mut();
active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].tangent_impulse =
active_contacts[k_base + k].data.tangent_impulse =
self.elements[k].tangent_part[0].impulse;
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].tangent_impulse = [
active_contacts[k_base + k].data.tangent_impulse = [
self.elements[k].tangent_part[0].impulse,
self.elements[k].tangent_part[1].impulse,
];

28
src/dynamics/solver/velocity_constraint_wide.rs
View File

@@ -69,11 +69,11 @@ impl WVelocityConstraint {
push: bool,
) {
let inv_dt = SimdFloat::splat(params.inv_dt());
let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]);
let delta1 = Isometry::from(array![|ii| manifolds[ii].data.delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].data.delta2; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdFloat> =
@@ -103,13 +103,13 @@ impl WVelocityConstraint {
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold =
SimdFloat::splat(params.restitution_velocity_threshold);
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
@@ -140,7 +140,7 @@ impl WVelocityConstraint {
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
@@ -176,11 +176,11 @@ impl WVelocityConstraint {
for j in 0..DIM - 1 {
#[cfg(feature = "dim2")]
let impulse = SimdFloat::from(
array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
);
#[cfg(feature = "dim3")]
let impulse = SimdFloat::from(
array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
);
let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j]));
@@ -202,7 +202,7 @@ impl WVelocityConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::Grouped(constraint));
} else {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::Grouped(constraint);
}
}
@@ -342,15 +342,15 @@ impl WVelocityConstraint {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id;
let active_contacts = manifold.active_contacts_mut();
active_contacts[k_base + k].impulse = impulses[ii];
active_contacts[k_base + k].data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].tangent_impulse =
active_contacts[k_base + k].data.tangent_impulse =
[tangent_impulses[ii], bitangent_impulses[ii]];
}
}

34
src/dynamics/solver/velocity_ground_constraint.rs
View File

@@ -63,26 +63,26 @@ impl VelocityGroundConstraint {
out_constraints: &mut Vec<AnyVelocityConstraint>,
push: bool,
) {
let mut rb1 = &bodies[manifold.body_pair.body1];
let mut rb2 = &bodies[manifold.body_pair.body2];
let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flipped = !rb2.is_dynamic();
let force_dir1;
let coll_pos1;
let coll_pos2;
if flipped {
coll_pos1 = rb2.position * manifold.delta2;
coll_pos2 = rb1.position * manifold.delta1;
coll_pos1 = rb2.position * manifold.data.delta2;
coll_pos2 = rb1.position * manifold.data.delta1;
force_dir1 = coll_pos1 * (-manifold.local_n2);
std::mem::swap(&mut rb1, &mut rb2);
} else {
coll_pos1 = rb1.position * manifold.delta1;
coll_pos2 = rb2.position * manifold.delta2;
coll_pos1 = rb1.position * manifold.data.delta1;
coll_pos2 = rb2.position * manifold.data.delta2;
force_dir1 = coll_pos1 * (-manifold.local_n1);
}
let mj_lambda2 = rb2.active_set_offset;
let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
for (l, manifold_points) in manifold
.active_contacts()
@@ -94,7 +94,7 @@ impl VelocityGroundConstraint {
dir1: force_dir1,
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb2.mass_properties.inv_mass,
limit: manifold.friction,
limit: manifold.data.friction,
mj_lambda2,
manifold_id,
manifold_contact_id: l * MAX_MANIFOLD_POINTS,
@@ -135,7 +135,7 @@ impl VelocityGroundConstraint {
{
constraint.dir1 = force_dir1;
constraint.im2 = rb2.mass_properties.inv_mass;
constraint.limit = manifold.friction;
constraint.limit = manifold.data.friction;
constraint.mj_lambda2 = mj_lambda2;
constraint.manifold_id = manifold_id;
constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS;
@@ -173,12 +173,12 @@ impl VelocityGroundConstraint {
let mut rhs = (vel1 - vel2).dot(&force_dir1);
if rhs <= -params.restitution_velocity_threshold {
rhs += manifold.restitution * rhs
rhs += manifold.data.restitution * rhs
}
rhs += manifold_point.dist.max(0.0) * params.inv_dt();
let impulse = manifold_points[k].impulse * warmstart_coeff;
let impulse = manifold_points[k].data.impulse * warmstart_coeff;
constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
gcross2,
@@ -199,9 +199,9 @@ impl VelocityGroundConstraint {
let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_points[k].tangent_impulse * warmstart_coeff;
let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")]
let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff;
let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff;
constraint.elements[k].tangent_part[j] =
VelocityGroundConstraintElementPart {
@@ -218,7 +218,7 @@ impl VelocityGroundConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint));
} else {
out_constraints[manifold.constraint_index + l] =
out_constraints[manifold.data.constraint_index + l] =
AnyVelocityConstraint::NongroupedGround(constraint);
}
}
@@ -290,15 +290,15 @@ impl VelocityGroundConstraint {
for k in 0..self.num_contacts as usize {
let active_contacts = manifold.active_contacts_mut();
active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse;
active_contacts[k_base + k].data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].tangent_impulse =
active_contacts[k_base + k].data.tangent_impulse =
self.elements[k].tangent_part[0].impulse;
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].tangent_impulse = [
active_contacts[k_base + k].data.tangent_impulse = [
self.elements[k].tangent_part[0].impulse,
self.elements[k].tangent_part[1].impulse,
];

28
src/dynamics/solver/velocity_ground_constraint_wide.rs
View File

@@ -65,8 +65,8 @@ impl WVelocityGroundConstraint {
push: bool,
) {
let inv_dt = SimdFloat::splat(params.inv_dt());
let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let mut flipped = [false; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
@@ -90,10 +90,10 @@ impl WVelocityGroundConstraint {
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let delta1 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].data.delta2 } else { manifolds[ii].data.delta1 }; SIMD_WIDTH],
);
let delta2 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
);
let coll_pos1 = pos1 * delta1;
@@ -109,13 +109,13 @@ impl WVelocityGroundConstraint {
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].restitution; SIMD_WIDTH]);
let friction = SimdFloat::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdFloat::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold =
SimdFloat::splat(params.restitution_velocity_threshold);
let warmstart_multiplier =
SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]);
SimdFloat::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff);
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
@@ -146,7 +146,7 @@ impl WVelocityGroundConstraint {
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
SimdFloat::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
@@ -178,11 +178,11 @@ impl WVelocityGroundConstraint {
for j in 0..DIM - 1 {
#[cfg(feature = "dim2")]
let impulse = SimdFloat::from(
array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH],
array![|ii| manifold_points[ii][k].data.tangent_impulse; SIMD_WIDTH],
);
#[cfg(feature = "dim3")]
let impulse = SimdFloat::from(
array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH],
array![|ii| manifold_points[ii][k].data.tangent_impulse[j]; SIMD_WIDTH],
);
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
@@ -202,7 +202,7 @@ impl WVelocityGroundConstraint {
if push {
out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint));
} else {
out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] =
out_constraints[manifolds[0].data.constraint_index + l / MAX_MANIFOLD_POINTS] =
AnyVelocityConstraint::GroupedGround(constraint);
}
}
@@ -302,15 +302,15 @@ impl WVelocityGroundConstraint {
let manifold = &mut manifolds_all[self.manifold_id[ii]];
let k_base = self.manifold_contact_id;
let active_contacts = manifold.active_contacts_mut();
active_contacts[k_base + k].impulse = impulses[ii];
active_contacts[k_base + k].data.impulse = impulses[ii];
#[cfg(feature = "dim2")]
{
active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii];
active_contacts[k_base + k].data.tangent_impulse = tangent_impulses[ii];
}
#[cfg(feature = "dim3")]
{
active_contacts[k_base + k].tangent_impulse =
active_contacts[k_base + k].data.tangent_impulse =
[tangent_impulses[ii], bitangent_impulses[ii]];
}
}

12
src/geometry/broad_phase_multi_sap.rs
View File

@@ -4,7 +4,7 @@ use crate::dynamics::RigidBodySet;
use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider};
use crate::math::{Point, Vector, DIM};
use bit_vec::BitVec;
use ncollide::bounding_volume::{BoundingVolume, AABB};
use buckler::bounding_volume::{BoundingVolume, AABB};
use std::cmp::Ordering;
use std::ops::{Index, IndexMut};
@@ -67,7 +67,7 @@ fn point_key(point: Point<f32>) -> Point<i32> {
(point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into()
}
fn region_aabb(index: Point<i32>) -> AABB<f32> {
fn region_aabb(index: Point<i32>) -> AABB {
let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into();
let maxs = mins + Vector::repeat(CELL_WIDTH);
AABB::new(mins, maxs)
@@ -345,7 +345,7 @@ struct SAPRegion {
}
impl SAPRegion {
pub fn new(bounds: AABB<f32>) -> Self {
pub fn new(bounds: AABB) -> Self {
let axes = [
SAPAxis::new(bounds.mins.x, bounds.maxs.x),
SAPAxis::new(bounds.mins.y, bounds.maxs.y),
@@ -361,7 +361,7 @@ impl SAPRegion {
}
}
pub fn recycle(bounds: AABB<f32>, mut old: Self) -> Self {
pub fn recycle(bounds: AABB, mut old: Self) -> Self {
// Correct the bounds
for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) {
axis.min_bound = bound;
@@ -381,7 +381,7 @@ impl SAPRegion {
old
}
pub fn recycle_or_new(bounds: AABB<f32>, pool: &mut Vec<Self>) -> Self {
pub fn recycle_or_new(bounds: AABB, pool: &mut Vec<Self>) -> Self {
if let Some(old) = pool.pop() {
Self::recycle(bounds, old)
} else {
@@ -488,7 +488,7 @@ pub struct BroadPhase {
#[derive(Clone)]
pub(crate) struct BroadPhaseProxy {
handle: ColliderHandle,
aabb: AABB<f32>,
aabb: AABB,
next_free: u32,
}

192
src/geometry/capsule.rs
View File

@@ -1,192 +0,0 @@
use crate::geometry::{Ray, RayIntersection, AABB};
use crate::math::{Isometry, Point, Rotation, Vector};
use approx::AbsDiffEq;
use na::Unit;
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 round segment.
pub struct Capsule {
/// The axis and endpoint of the capsule.
pub segment: Segment<f32>,
/// The radius of the capsule.
pub radius: f32,
}
impl Capsule {
/// Creates a new capsule aligned with the `x` axis and with the given half-height an radius.
pub fn new_x(half_height: f32, radius: f32) -> Self {
let b = Point::from(Vector::x() * half_height);
Self::new(-b, b, radius)
}
/// Creates a new capsule aligned with the `y` axis and with the given half-height an radius.
pub fn new_y(half_height: f32, radius: f32) -> Self {
let b = Point::from(Vector::y() * half_height);
Self::new(-b, b, radius)
}
/// Creates a new capsule aligned with the `z` axis and with the given half-height an radius.
#[cfg(feature = "dim3")]
pub fn new_z(half_height: f32, radius: f32) -> Self {
let b = Point::from(Vector::z() * half_height);
Self::new(-b, b, radius)
}
/// 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 {
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.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())
}
/// The height of this capsule.
pub fn height(&self) -> f32 {
(self.segment.b - self.segment.a).norm()
}
/// The half-height of this capsule.
pub fn half_height(&self) -> f32 {
self.height() / 2.0
}
/// The center of this capsule.
pub fn center(&self) -> Point<f32> {
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.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.segment.b - self.segment.a;
if dir.y < 0.0 {
dir = -dir;
}
#[cfg(feature = "dim2")]
{
Rotation::rotation_between(&Vector::y(), &dir)
}
#[cfg(feature = "dim3")]
{
Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity())
}
}
/// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`.
pub fn transform_wrt_y(&self) -> Isometry<f32> {
let rot = self.rotation_wrt_y();
Isometry::from_parts(self.center().coords.into(), rot)
}
}
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.
// We should find a way to avoid this code duplication.
impl PointQuery<f32> for Capsule {
#[inline]
fn project_point(
&self,
m: &Isometry<f32>,
pt: &Point<f32>,
solid: bool,
) -> PointProjection<f32> {
let seg = Segment::new(self.segment.a, self.segment.b);
let proj = seg.project_point(m, pt, solid);
let dproj = *pt - proj.point;
if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) {
let inside = dist <= self.radius;
if solid && inside {
return PointProjection::new(true, *pt);
} else {
return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius);
}
} else if solid {
return PointProjection::new(true, *pt);
}
#[cfg(feature = "dim2")]
if let Some(dir) = seg.normal() {
let dir = m * *dir;
PointProjection::new(true, proj.point + dir * self.radius)
} else {
// The segment has no normal, likely because it degenerates to a point.
PointProjection::new(true, proj.point + Vector::ith(1, self.radius))
}
#[cfg(feature = "dim3")]
if let Some(dir) = seg.direction() {
use crate::utils::WBasis;
let dir = m * dir.orthonormal_basis()[0];
PointProjection::new(true, proj.point + dir * self.radius)
} else {
// The segment has no normal, likely because it degenerates to a point.
PointProjection::new(true, proj.point + Vector::ith(1, self.radius))
}
}
#[inline]
fn project_point_with_feature(
&self,
m: &Isometry<f32>,
pt: &Point<f32>,
) -> (PointProjection<f32>, FeatureId) {
(self.project_point(m, pt, false), FeatureId::Face(0))
}
}

6
src/geometry/collider.rs
View File

@@ -6,8 +6,8 @@ use crate::geometry::{
#[cfg(feature = "dim3")]
use crate::geometry::{Cone, Cylinder, RoundCylinder};
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use buckler::bounding_volume::AABB;
use na::Point3;
use ncollide::bounding_volume::AABB;
use std::ops::Deref;
use std::sync::Arc;
@@ -270,11 +270,11 @@ impl Collider {
}
/// Compute the axis-aligned bounding box of this collider.
pub fn compute_aabb(&self) -> AABB<f32> {
pub fn compute_aabb(&self) -> AABB {
self.shape.compute_aabb(&self.position)
}
// pub(crate) fn compute_aabb_with_prediction(&self) -> AABB<f32> {
// pub(crate) fn compute_aabb_with_prediction(&self) -> AABB {
// let aabb1 = self.shape.compute_aabb(&self.position);
// let aabb2 = self.shape.compute_aabb(&self.predicted_position);
// aabb1.merged(&aabb2)

333
src/geometry/contact.rs
View File

@@ -1,7 +1,8 @@
use crate::buckler::query::TrackedData;
use crate::data::MaybeSerializableData;
use crate::dynamics::BodyPair;
use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet};
use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase};
use crate::geometry::{Collider, ColliderPair, ColliderSet};
use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold};
use crate::math::{Isometry, Point, Vector};
#[cfg(feature = "simd-is-enabled")]
use {
@@ -19,39 +20,6 @@ bitflags::bitflags! {
}
}
#[derive(Copy, Clone, Debug, PartialEq, Eq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// The type local linear approximation of the neighborhood of a pair contact points on two shapes
pub enum KinematicsCategory {
/// Both neighborhoods are assimilated to a single point.
PointPoint,
/// The first shape's neighborhood at the contact point is assimilated to a plane while
/// the second is assimilated to a point.
PlanePoint,
}
#[derive(Copy, Clone, Debug, PartialEq)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// Local contact geometry at the neighborhood of a pair of contact points.
pub struct ContactKinematics {
/// The local contact geometry.
pub category: KinematicsCategory,
/// The dilation applied to the first contact geometry.
pub radius1: f32,
/// The dilation applied to the second contact geometry.
pub radius2: f32,
}
impl Default for ContactKinematics {
fn default() -> Self {
ContactKinematics {
category: KinematicsCategory::PointPoint,
radius1: 0.0,
radius2: 0.0,
}
}
}
#[cfg(feature = "simd-is-enabled")]
pub(crate) struct WContact {
pub local_p1: Point<SimdFloat>,
@@ -70,10 +38,9 @@ impl WContact {
local_p1: self.local_p1.extract(i),
local_p2: self.local_p2.extract(i),
dist: self.dist.extract(i),
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: self.fid1[i],
fid2: self.fid2[i],
data: ContactData::default(),
};
(c, self.local_n1.extract(i), self.local_n2.extract(i))
@@ -83,11 +50,7 @@ impl WContact {
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A single contact between two collider.
pub struct Contact {
/// The contact point in the local-space of the first collider.
pub local_p1: Point<f32>,
/// The contact point in the local-space of the second collider.
pub local_p2: Point<f32>,
pub struct ContactData {
/// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body.
///
/// The impulse applied to the second collider's rigid-body is given by `-impulse`.
@@ -100,46 +63,9 @@ pub struct Contact {
/// collider's rigid-body.
#[cfg(feature = "dim3")]
pub tangent_impulse: [f32; 2],
/// The identifier of the subshape of the first collider involved in this contact.
///
/// For primitive shapes like cuboid, ball, etc., this is 0.
/// For shapes like trimesh and heightfield this identifies the specific triangle
/// involved in the contact.
pub fid1: u8,
/// The identifier of the subshape of the second collider involved in this contact.
///
/// For primitive shapes like cuboid, ball, etc., this is 0.
/// For shapes like trimesh and heightfield this identifies the specific triangle
/// involved in the contact.
pub fid2: u8,
/// The distance between the two colliders along the contact normal.
///
/// If this is negative, the colliders are penetrating.
pub dist: f32,
}
impl Contact {
pub(crate) fn new(
local_p1: Point<f32>,
local_p2: Point<f32>,
fid1: u8,
fid2: u8,
dist: f32,
) -> Self {
Self {
local_p1,
local_p2,
impulse: 0.0,
#[cfg(feature = "dim2")]
tangent_impulse: 0.0,
#[cfg(feature = "dim3")]
tangent_impulse: [0.0; 2],
fid1,
fid2,
dist,
}
}
impl ContactData {
#[cfg(feature = "dim2")]
pub(crate) fn zero_tangent_impulse() -> f32 {
0.0
@@ -149,26 +75,15 @@ impl Contact {
pub(crate) fn zero_tangent_impulse() -> [f32; 2] {
[0.0, 0.0]
}
}
pub(crate) fn copy_geometry_from(&mut self, contact: Contact) {
self.local_p1 = contact.local_p1;
self.local_p2 = contact.local_p2;
self.fid1 = contact.fid1;
self.fid2 = contact.fid2;
self.dist = contact.dist;
impl Default for ContactData {
fn default() -> Self {
Self {
impulse: 0.0,
tangent_impulse: Self::zero_tangent_impulse(),
}
}
// pub(crate) fn swap(self) -> Self {
// Self {
// local_p1: self.local_p2,
// local_p2: self.local_p1,
// impulse: self.impulse,
// tangent_impulse: self.tangent_impulse,
// fid1: self.fid2,
// fid2: self.fid1,
// dist: self.dist,
// }
// }
}
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -227,15 +142,16 @@ impl ContactPair {
let coll2 = &colliders[self.pair.collider2];
if self.manifolds.len() == 0 {
let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags);
self.manifolds.push(manifold);
let manifold_data = ContactManifoldData::from_colliders(self.pair, coll1, coll2, flags);
self.manifolds
.push(ContactManifold::with_data((0, 0), manifold_data));
}
// We have to make sure the order of the returned collider
// match the order of the pair stored inside of the manifold.
// (This order can be modified by the contact determination algorithm).
let manifold = &mut self.manifolds[0];
if manifold.pair.collider1 == self.pair.collider1 {
if manifold.data.pair.collider1 == self.pair.collider1 {
(
coll1,
coll2,
@@ -259,30 +175,12 @@ impl ContactPair {
///
/// A contact manifold describes a set of contacts between two colliders. All the contact
/// part of the same contact manifold share the same contact normal and contact kinematics.
pub struct ContactManifold {
// NOTE: use a SmallVec instead?
// And for 2D use an ArrayVec since there will never be more than 2 contacts anyways.
#[cfg(feature = "dim2")]
pub(super) points: arrayvec::ArrayVec<[Contact; 2]>,
#[cfg(feature = "dim3")]
pub(super) points: Vec<Contact>,
/// The number of active contacts on this contact manifold.
///
/// Active contacts are these that may result in contact forces.
pub num_active_contacts: usize,
/// The contact normal of all the contacts of this manifold, expressed in the local space of the first collider.
pub local_n1: Vector<f32>,
/// The contact normal of all the contacts of this manifold, expressed in the local space of the second collider.
pub local_n2: Vector<f32>,
/// The contact kinematics of all the contacts of this manifold.
pub kinematics: ContactKinematics,
pub struct ContactManifoldData {
// The following are set by the narrow-phase.
/// The pair of body involved in this contact manifold.
pub body_pair: BodyPair,
/// The pair of colliders involved in this contact manifold.
pub pair: ColliderPair,
/// The pair of subshapes involved in this contact manifold.
pub subshape_index_pair: (usize, usize),
/// The pair of body involved in this contact manifold.
pub body_pair: BodyPair,
pub(crate) warmstart_multiplier: f32,
// The two following are set by the constraints solver.
pub(crate) constraint_index: usize,
@@ -303,29 +201,44 @@ pub struct ContactManifold {
pub solver_flags: SolverFlags,
}
impl ContactManifold {
impl Default for ContactManifoldData {
fn default() -> Self {
Self::new(
ColliderPair::new(ColliderSet::invalid_handle(), ColliderSet::invalid_handle()),
BodyPair::new(
RigidBodySet::invalid_handle(),
RigidBodySet::invalid_handle(),
),
Isometry::identity(),
Isometry::identity(),
0.0,
0.0,
SolverFlags::empty(),
)
}
}
impl TrackedData for ContactManifoldData {
fn flip(&mut self) {
std::mem::swap(&mut self.pair.collider1, &mut self.pair.collider2);
std::mem::swap(&mut self.body_pair.body1, &mut self.body_pair.body2);
std::mem::swap(&mut self.delta1, &mut self.delta2);
}
}
impl ContactManifoldData {
pub(crate) fn new(
pair: ColliderPair,
subshapes: (usize, usize),
body_pair: BodyPair,
delta1: Isometry<f32>,
delta2: Isometry<f32>,
friction: f32,
restitution: f32,
solver_flags: SolverFlags,
) -> ContactManifold {
) -> ContactManifoldData {
Self {
#[cfg(feature = "dim2")]
points: arrayvec::ArrayVec::new(),
#[cfg(feature = "dim3")]
points: Vec::new(),
num_active_contacts: 0,
local_n1: Vector::zeros(),
local_n2: Vector::zeros(),
pair,
subshape_index_pair: subshapes,
body_pair,
kinematics: ContactKinematics::default(),
warmstart_multiplier: Self::min_warmstart_multiplier(),
friction,
restitution,
@@ -337,50 +250,23 @@ impl ContactManifold {
}
}
pub(crate) fn take(&mut self) -> Self {
ContactManifold {
#[cfg(feature = "dim2")]
points: self.points.clone(),
#[cfg(feature = "dim3")]
points: std::mem::replace(&mut self.points, Vec::new()),
num_active_contacts: self.num_active_contacts,
local_n1: self.local_n1,
local_n2: self.local_n2,
kinematics: self.kinematics,
body_pair: self.body_pair,
pair: self.pair,
subshape_index_pair: self.subshape_index_pair,
warmstart_multiplier: self.warmstart_multiplier,
friction: self.friction,
restitution: self.restitution,
delta1: self.delta1,
delta2: self.delta2,
constraint_index: self.constraint_index,
position_constraint_index: self.position_constraint_index,
solver_flags: self.solver_flags,
}
}
pub(crate) fn from_colliders(
pair: ColliderPair,
coll1: &Collider,
coll2: &Collider,
flags: SolverFlags,
) -> Self {
Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags)
Self::with_subshape_indices(pair, coll1, coll2, flags)
}
pub(crate) fn with_subshape_indices(
pair: ColliderPair,
coll1: &Collider,
coll2: &Collider,
subshape1: usize,
subshape2: usize,
solver_flags: SolverFlags,
) -> Self {
Self::new(
pair,
(subshape1, subshape2),
BodyPair::new(coll1.parent, coll2.parent),
*coll1.position_wrt_parent(),
*coll2.position_wrt_parent(),
@@ -398,134 +284,25 @@ impl ContactManifold {
0.01
}
/// Number of active contacts on this contact manifold.
#[inline]
pub fn num_active_contacts(&self) -> usize {
self.num_active_contacts
}
/// The slice of all the active contacts on this contact manifold.
///
/// Active contacts are contacts that may end up generating contact forces.
#[inline]
pub fn active_contacts(&self) -> &[Contact] {
&self.points[..self.num_active_contacts]
}
#[inline]
pub(crate) fn active_contacts_mut(&mut self) -> &mut [Contact] {
&mut self.points[..self.num_active_contacts]
}
/// The slice of all the contacts, active or not, on this contact manifold.
#[inline]
pub fn all_contacts(&self) -> &[Contact] {
&self.points
}
pub(crate) fn swap_identifiers(&mut self) {
self.pair = self.pair.swap();
self.body_pair = self.body_pair.swap();
self.subshape_index_pair = (self.subshape_index_pair.1, self.subshape_index_pair.0);
std::mem::swap(&mut self.delta1, &mut self.delta2);
}
pub(crate) fn update_warmstart_multiplier(&mut self) {
pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) {
// In 2D, tall stacks will actually suffer from this
// because oscillation due to inaccuracies in 2D often
// cause contacts to break, which would result in
// a reset of the warmstart multiplier.
if cfg!(feature = "dim2") {
self.warmstart_multiplier = 1.0;
manifold.data.warmstart_multiplier = 1.0;
return;
}
for pt in &self.points {
if pt.impulse != 0.0 {
self.warmstart_multiplier = (self.warmstart_multiplier * 2.0).min(1.0);
for pt in &manifold.points {
if pt.data.impulse != 0.0 {
manifold.data.warmstart_multiplier =
(manifold.data.warmstart_multiplier * 2.0).min(1.0);
return;
}
}
// Reset the multiplier.
self.warmstart_multiplier = Self::min_warmstart_multiplier()
}
#[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;
}
let local_n2 = pos12 * self.local_n2;
if -self.local_n1.dot(&local_n2) < angle_dot_threshold {
return false;
}
for pt in &mut self.points {
let local_p2 = pos12 * pt.local_p2;
let dpt = local_p2 - pt.local_p1;
let dist = dpt.dot(&self.local_n1);
if dist * pt.dist < 0.0 {
// We switched between penetrating/non-penetrating.
// The may result in other contacts to appear.
return false;
}
let new_local_p1 = local_p2 - self.local_n1 * dist;
if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold {
return false;
}
pt.dist = dist;
pt.local_p1 = new_local_p1;
}
true
}
/// Sort the contacts of this contact manifold such that the active contacts are in the first
/// positions of the array.
#[inline]
pub(crate) fn sort_contacts(&mut self, prediction_distance: f32) {
let num_contacts = self.points.len();
match num_contacts {
0 => {
self.num_active_contacts = 0;
}
1 => {
self.num_active_contacts = (self.points[0].dist < prediction_distance) as usize;
}
_ => {
let mut first_inactive_index = num_contacts;
self.num_active_contacts = 0;
while self.num_active_contacts != first_inactive_index {
if self.points[self.num_active_contacts].dist >= prediction_distance {
// Swap with the last contact.
self.points
.swap(self.num_active_contacts, first_inactive_index - 1);
first_inactive_index -= 1;
} else {
self.num_active_contacts += 1;
}
}
}
}
manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier()
}
}

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

@@ -1,5 +1,5 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Contact, KinematicsCategory};
use crate::geometry::{Contact, ContactManifoldData, KinematicsCategory};
use crate::math::{Point, Vector};
#[cfg(feature = "simd-is-enabled")]
use {
@@ -53,7 +53,7 @@ pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationCon
manifold.kinematics.category = KinematicsCategory::PointPoint;
manifold.kinematics.radius1 = radii_a.extract(i);
manifold.kinematics.radius2 = radii_b.extract(i);
manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(manifold);
} else {
manifold.points.clear();
}
@@ -94,7 +94,7 @@ pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext)
ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint;
ctxt.manifold.kinematics.radius1 = radius_a;
ctxt.manifold.kinematics.radius2 = radius_b;
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
} else {
ctxt.manifold.points.clear();
}

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

@@ -1,8 +1,8 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Ball, Contact, KinematicsCategory};
use crate::geometry::{Ball, Contact, ContactManifoldData, KinematicsCategory};
use crate::math::Isometry;
use buckler::query::PointQuery;
use na::Unit;
use ncollide::query::PointQuery;
pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) {
if let Some(ball1) = ctxt.shape1.as_ball() {
@@ -15,7 +15,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
fn do_generate_contacts<P: ?Sized + PointQuery<f32>>(
fn do_generate_contacts<P: ?Sized + PointQuery>(
point_query1: &P,
ball2: &Ball,
ctxt: &mut PrimitiveContactGenerationContext,
@@ -33,12 +33,8 @@ fn do_generate_contacts<P: ?Sized + PointQuery<f32>>(
}
let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into());
// TODO: add a `project_local_point` to the PointQuery trait to avoid
// the identity isometry.
let proj =
point_query1.project_point(&Isometry::identity(), &local_p2_1, cfg!(feature = "dim3"));
let dpos = local_p2_1 - proj.point;
let proj = point_query1.project_local_point(&local_p2_1, cfg!(feature = "dim3"));
let dpos = local_p2_1 - proj.local_point;
#[allow(unused_mut)] // Because `mut local_n1, mut dist` is needed in 2D but not in 3D.
if let Some((mut local_n1, mut dist)) = Unit::try_new_and_get(dpos, 0.0) {
@@ -51,8 +47,8 @@ fn do_generate_contacts<P: ?Sized + PointQuery<f32>>(
if dist <= ball2.radius + ctxt.prediction_distance {
let local_n2 = position2.inverse_transform_vector(&(position1 * -*local_n1));
let local_p2 = (local_n2 * ball2.radius).into();
let contact_point = Contact::new(proj.local_point, local_p2, 0, 0, dist - ball2.radius);
let contact_point = Contact::new(proj.point, local_p2, 0, 0, dist - ball2.radius);
if ctxt.manifold.points.len() != 1 {
ctxt.manifold.points.clear();
ctxt.manifold.points.push(contact_point);
@@ -66,7 +62,7 @@ fn do_generate_contacts<P: ?Sized + PointQuery<f32>>(
ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint;
ctxt.manifold.kinematics.radius1 = 0.0;
ctxt.manifold.kinematics.radius2 = ball2.radius;
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
} else {
ctxt.manifold.points.clear();
}

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

@@ -1,11 +1,11 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory};
use crate::geometry::{Capsule, Contact, ContactManifold, ContactManifoldData, KinematicsCategory};
use crate::math::Isometry;
use crate::math::Vector;
use approx::AbsDiffEq;
use na::Unit;
#[cfg(feature = "dim2")]
use ncollide::shape::SegmentPointLocation;
use buckler::shape::SegmentPointLocation;
use na::Unit;
pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Some(capsule1), Some(capsule2)) = (ctxt.shape1.as_capsule(), ctxt.shape2.as_capsule()) {
@@ -19,7 +19,7 @@ pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationCo
);
}
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
@@ -41,7 +41,7 @@ pub fn generate_contacts<'a>(
let seg1 = capsule1.segment;
let seg2_1 = capsule2.segment.transformed(&pos12);
let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
let (loc1, loc2) = buckler::query::details::closest_points_segment_segment_with_locations_nD(
(&seg1.a, &seg1.b),
(&seg2_1.a, &seg2_1.b),
);
@@ -94,11 +94,13 @@ pub fn generate_contacts<'a>(
{
// 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(
(seg1.a, seg1.b),
(seg2_1.a, seg2_1.b),
*local_n1,
) {
if let Some((clip_a, clip_b)) =
buckler::query::details::clip_segment_segment_with_normal(
(seg1.a, seg1.b),
(seg2_1.a, seg2_1.b),
*local_n1,
)
{
let contact =
if (clip_a.0 - local_p1).norm_squared() > f32::default_epsilon() * 100.0 {
// Use clip_a as the second contact.
@@ -139,7 +141,7 @@ pub fn generate_contacts<'a>(
}
}
super::match_contacts(manifold, &old_manifold_points, swapped);
manifold.match_contacts(&old_manifold_points, swapped);
}
#[cfg(feature = "dim3")]
@@ -156,10 +158,11 @@ pub fn generate_contacts<'a>(
let seg1 = capsule1.segment;
let seg2_1 = capsule2.segment.transformed(&pos12);
let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD(
(&seg1.a, &seg1.b),
(&seg2_1.a, &seg2_1.b),
);
let (loc1, loc2) =
buckler::query::closest_points::closest_points_segment_segment_with_locations_nD(
(&seg1.a, &seg1.b),
(&seg2_1.a, &seg2_1.b),
);
{
let bcoords1 = loc1.barycentric_coordinates();

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

@@ -1,11 +1,12 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
#[cfg(feature = "dim3")]
use crate::geometry::PolyhedronFace;
use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory};
#[cfg(feature = "dim2")]
use crate::geometry::{CuboidFeature, CuboidFeatureFace};
use crate::geometry::{Capsule, ContactManifold, ContactManifoldData, Cuboid, KinematicsCategory};
use crate::math::Isometry;
use crate::math::Vector;
use buckler::query::sat;
#[cfg(feature = "dim3")]
use buckler::shape::PolyhedronFeature;
#[cfg(feature = "dim2")]
use buckler::shape::{CuboidFeature, CuboidFeatureFace};
pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Some(cube1), Some(capsule2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_capsule()) {
@@ -18,7 +19,7 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon
ctxt.manifold,
false,
);
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
} else if let (Some(capsule1), Some(cube2)) =
(ctxt.shape1.as_capsule(), ctxt.shape2.as_cuboid())
{
@@ -31,7 +32,7 @@ pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationCon
ctxt.manifold,
true,
);
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
}
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
@@ -61,7 +62,8 @@ pub fn generate_contacts<'a>(
* Point-Face cases.
*
*/
let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, &segment2, &pos12);
let sep1 =
sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, &segment2, &pos12);
if sep1.0 > capsule2.radius + prediction_distance {
manifold.points.clear();
return;
@@ -84,8 +86,7 @@ pub fn generate_contacts<'a>(
#[cfg(feature = "dim2")]
let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D.
#[cfg(feature = "dim3")]
let sep3 =
sat::cube_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12, &pos21);
let sep3 = sat::cuboid_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12);
if sep3.0 > capsule2.radius + prediction_distance {
manifold.points.clear();
return;
@@ -118,20 +119,20 @@ pub fn generate_contacts<'a>(
{
if swapped_reference {
feature1 = CuboidFeatureFace::from(segment2);
feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1);
feature2 = cube1.support_face(pos21 * -best_sep.1);
} else {
feature1 = cuboid::support_face(cube1, best_sep.1);
feature1 = cube1.support_face(best_sep.1);
feature2 = CuboidFeatureFace::from(segment2);
}
}
#[cfg(feature = "dim3")]
{
if swapped_reference {
feature1 = PolyhedronFace::from(segment2);
feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1);
feature1 = PolyhedronFeature::from(segment2);
feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1);
} else {
feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1);
feature2 = PolyhedronFace::from(segment2);
feature1 = cube1.polyhedron_support_face(best_sep.1);
feature2 = PolyhedronFeature::from(segment2);
}
}
@@ -156,7 +157,7 @@ pub fn generate_contacts<'a>(
manifold,
);
#[cfg(feature = "dim3")]
PolyhedronFace::contacts(
PolyhedronFeature::contacts(
prediction_distance + capsule2.radius,
&feature1,
&best_sep.1,
@@ -185,5 +186,5 @@ pub fn generate_contacts<'a>(
}
// Transfer impulses.
super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference);
manifold.match_contacts(&old_manifold_points, swapped ^ swapped_reference);
}

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

@@ -1,9 +1,10 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory};
use crate::geometry::{ContactManifold, ContactManifoldData, KinematicsCategory};
use crate::math::Isometry;
#[cfg(feature = "dim2")]
use crate::math::Vector;
use ncollide::shape::Cuboid;
use buckler::query::sat;
use buckler::shape::{Cuboid, CuboidFeature};
pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) {
@@ -19,15 +20,15 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont
unreachable!()
}
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
pub fn generate_contacts<'a>(
prediction_distance: f32,
mut cube1: &'a Cuboid<f32>,
mut cube1: &'a Cuboid,
mut pos1: &'a Isometry<f32>,
mut cube2: &'a Cuboid<f32>,
mut cube2: &'a Cuboid,
mut pos2: &'a Isometry<f32>,
manifold: &mut ContactManifold,
) {
@@ -43,13 +44,13 @@ pub fn generate_contacts<'a>(
* Point-Face
*
*/
let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21);
let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12);
if sep1.0 > prediction_distance {
manifold.points.clear();
return;
}
let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12);
let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21);
if sep2.0 > prediction_distance {
manifold.points.clear();
return;
@@ -63,7 +64,7 @@ pub fn generate_contacts<'a>(
#[cfg(feature = "dim2")]
let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D.
#[cfg(feature = "dim3")]
let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21);
let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12);
if sep3.0 > prediction_distance {
manifold.points.clear();
return;
@@ -97,8 +98,8 @@ pub fn generate_contacts<'a>(
// Now the reference feature is from `cube1` and the best separation is `best_sep`.
// Everything must be expressed in the local-space of `cube1` for contact clipping.
let feature1 = cuboid::support_feature(cube1, best_sep.1);
let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1);
let feature1 = cube1.support_feature(best_sep.1);
let mut feature2 = cube2.support_feature(pos21 * -best_sep.1);
feature2.transform_by(&pos12);
match (&feature1, &feature2) {
@@ -151,5 +152,5 @@ pub fn generate_contacts<'a>(
manifold.kinematics.radius2 = 0.0;
// Transfer impulses.
super::match_contacts(manifold, &old_manifold_points, swapped);
manifold.match_contacts(&old_manifold_points, swapped);
}

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

@@ -1,13 +1,11 @@
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
#[cfg(feature = "dim3")]
use crate::geometry::PolyhedronFace;
use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Triangle};
use crate::geometry::{ContactManifold, ContactManifoldData, Cuboid, KinematicsCategory, Triangle};
use crate::math::Isometry;
#[cfg(feature = "dim2")]
use crate::{
geometry::{triangle, CuboidFeature},
math::Vector,
};
use crate::{buckler::shape::CuboidFeature, geometry::triangle, math::Vector};
use buckler::query::sat;
#[cfg(feature = "dim3")]
use buckler::shape::PolyhedronFeature;
pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) {
if let (Some(cube1), Some(triangle2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_triangle()) {
@@ -20,7 +18,7 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo
ctxt.manifold,
false,
);
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
} else if let (Some(triangle1), Some(cube2)) =
(ctxt.shape1.as_triangle(), ctxt.shape2.as_cuboid())
{
@@ -33,7 +31,7 @@ pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationCo
ctxt.manifold,
true,
);
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
}
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
@@ -61,7 +59,8 @@ pub fn generate_contacts<'a>(
* Point-Face cases.
*
*/
let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12);
let sep1 =
sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12);
if sep1.0 > prediction_distance {
manifold.points.clear();
return;
@@ -81,8 +80,7 @@ pub fn generate_contacts<'a>(
#[cfg(feature = "dim2")]
let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D.
#[cfg(feature = "dim3")]
let sep3 =
sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21);
let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12);
if sep3.0 > prediction_distance {
manifold.points.clear();
return;
@@ -115,20 +113,20 @@ pub fn generate_contacts<'a>(
{
if swapped_reference {
feature1 = triangle::support_face(triangle2, best_sep.1);
feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1);
feature2 = cube1.support_face(pos21 * -best_sep.1);
} else {
feature1 = cuboid::support_face(cube1, best_sep.1);
feature1 = cube1.support_face(best_sep.1);
feature2 = triangle::support_face(triangle2, pos21 * -best_sep.1);
}
}
#[cfg(feature = "dim3")]
{
if swapped_reference {
feature1 = PolyhedronFace::from(*triangle2);
feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1);
feature1 = PolyhedronFeature::from(*triangle2);
feature2 = cube1.polyhedron_support_face(pos21 * -best_sep.1);
} else {
feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1);
feature2 = PolyhedronFace::from(*triangle2);
feature1 = cube1.polyhedron_support_face(best_sep.1);
feature2 = PolyhedronFeature::from(*triangle2);
}
}
@@ -153,7 +151,7 @@ pub fn generate_contacts<'a>(
manifold,
);
#[cfg(feature = "dim3")]
PolyhedronFace::contacts(
PolyhedronFeature::contacts(
prediction_distance,
&feature1,
&best_sep.1,
@@ -169,5 +167,5 @@ pub fn generate_contacts<'a>(
manifold.kinematics.radius2 = 0.0;
// Transfer impulses.
super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference);
manifold.match_contacts(&old_manifold_points, swapped ^ swapped_reference);
}

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

@@ -1,3 +1,4 @@
use crate::buckler::bounding_volume::BoundingVolume;
use crate::data::hashmap::{Entry, HashMap};
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::{
@@ -6,8 +7,7 @@ use crate::geometry::contact_generator::{
};
#[cfg(feature = "dim2")]
use crate::geometry::Capsule;
use crate::geometry::{Collider, ContactManifold, HeightField, Shape};
use crate::ncollide::bounding_volume::BoundingVolume;
use crate::geometry::{Collider, ContactManifold, ContactManifoldData, HeightField, Shape};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
@@ -88,7 +88,7 @@ fn do_generate_contacts(
let solver_flags = ctxt.solver_flags;
let shape_type2 = collider2.shape().shape_type();
heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| {
heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1| {
let position1 = collider1.position();
#[cfg(feature = "dim2")]
let sub_shape1 = Capsule::new(part1.a, part1.b, 0.0); // TODO: use a segment instead.
@@ -113,15 +113,13 @@ fn do_generate_contacts(
timestamp: new_timestamp,
workspace: workspace2,
};
let manifold = ContactManifold::with_subshape_indices(
let manifold_data = ContactManifoldData::from_colliders(
coll_pair,
collider1,
collider2,
i,
0,
solver_flags,
);
manifolds.push(manifold);
manifolds.push(ContactManifold::with_data((i, 0), manifold_data));
entry.insert(sub_detector)
}
@@ -142,7 +140,7 @@ fn do_generate_contacts(
let manifold = &mut manifolds[sub_detector.manifold_id];
let mut ctxt2 = if coll_pair.collider1 != manifold.pair.collider1 {
let mut ctxt2 = if coll_pair.collider1 != manifold.data.pair.collider1 {
PrimitiveContactGenerationContext {
prediction_distance,
collider1: collider2,

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

@@ -28,10 +28,6 @@ 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_with_normal;
pub(self) use self::serializable_workspace_tag::WorkspaceSerializationTag;
mod ball_ball_contact_generator;
@@ -53,29 +49,3 @@ mod serializable_workspace_tag;
mod trimesh_shape_contact_generator;
use crate::geometry::{Contact, ContactManifold};
pub(crate) fn match_contacts(
manifold: &mut ContactManifold,
old_contacts: &[Contact],
swapped: bool,
) {
for contact in &mut manifold.points {
if !swapped {
for old_contact in old_contacts {
if contact.fid1 == old_contact.fid1 && contact.fid2 == old_contact.fid2 {
// Transfer impulse cache.
contact.impulse = old_contact.impulse;
contact.tangent_impulse = old_contact.tangent_impulse;
}
}
} else {
for old_contact in old_contacts {
if contact.fid1 == old_contact.fid2 && contact.fid2 == old_contact.fid1 {
// Transfer impulse cache.
contact.impulse = old_contact.impulse;
contact.tangent_impulse = old_contact.tangent_impulse;
}
}
}
}
}

30
src/geometry/contact_generator/pfm_pfm_contact_generator.rs
View File

@@ -1,12 +1,15 @@
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{KinematicsCategory, PolygonalFeatureMap, PolyhedronFace};
use crate::geometry::{ContactManifoldData, KinematicsCategory};
use crate::math::{Isometry, Vector};
use buckler::query::{
self,
gjk::{GJKResult, VoronoiSimplex},
};
use buckler::shape::{PolygonalFeatureMap, PolyhedronFeature};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
use na::Unit;
use ncollide::query;
use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
@@ -15,12 +18,12 @@ pub struct PfmPfmContactManifoldGeneratorWorkspace {
feature = "serde-serialize",
serde(skip, default = "VoronoiSimplex::new")
)]
simplex: VoronoiSimplex<f32>,
simplex: VoronoiSimplex,
last_gjk_dir: Option<Unit<Vector<f32>>>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
feature1: PolyhedronFace,
feature1: PolyhedronFeature,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
feature2: PolyhedronFace,
feature2: PolyhedronFeature,
}
impl Default for PfmPfmContactManifoldGeneratorWorkspace {
@@ -28,8 +31,8 @@ impl Default for PfmPfmContactManifoldGeneratorWorkspace {
Self {
simplex: VoronoiSimplex::new(),
last_gjk_dir: None,
feature1: PolyhedronFace::new(),
feature2: PolyhedronFace::new(),
feature1: PolyhedronFeature::new(),
feature2: PolyhedronFeature::new(),
}
}
}
@@ -40,7 +43,7 @@ pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
ctxt.shape2.as_polygonal_feature_map(),
) {
do_generate_contacts(pfm1, border_radius1, pfm2, border_radius2, ctxt);
ctxt.manifold.update_warmstart_multiplier();
ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
ctxt.manifold.sort_contacts(ctxt.prediction_distance);
}
}
@@ -73,10 +76,9 @@ fn do_generate_contacts(
.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,
let contact = query::contact::contact_support_map_support_map_with_params(
&pos12,
pfm1,
pfm2,
total_prediction,
&mut workspace.simplex,
@@ -95,7 +97,7 @@ fn do_generate_contacts(
pfm2.local_support_feature(&normal2, &mut workspace.feature2);
workspace.feature2.transform_by(&pos12);
PolyhedronFace::contacts(
PolyhedronFeature::contacts(
total_prediction,
&workspace.feature1,
&normal1,
@@ -126,7 +128,7 @@ fn do_generate_contacts(
}
// Transfer impulses.
super::match_contacts(&mut ctxt.manifold, &old_manifold_points, false);
ctxt.manifold.match_contacts(&old_manifold_points, false);
}
impl MaybeSerializableData for PfmPfmContactManifoldGeneratorWorkspace {

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

@@ -1,10 +1,13 @@
#![allow(dead_code)] // TODO: remove this once we support polygons.
use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon};
use crate::geometry::{
sat, Contact, ContactData, ContactManifold, ContactManifoldData, KinematicsCategory, Polygon,
};
use crate::math::{Isometry, Point};
#[cfg(feature = "dim2")]
use crate::{math::Vector, utils};
use buckler::query;
pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationContext) {
unimplemented!()
@@ -16,7 +19,7 @@ pub fn generate_contacts_polygon_polygon(_ctxt: &mut PrimitiveContactGenerationC
// &ctxt.position2,
// ctxt.manifold,
// );
// ctxt.manifold.update_warmstart_multiplier();
// ContactManifoldData::update_warmstart_multiplier(ctxt.manifold);
// } else {
// unreachable!()
// }
@@ -75,12 +78,12 @@ fn generate_contacts<'a>(
m12 * p2.vertices[support_face2],
m12 * p2.vertices[(support_face2 + 1) % len2],
);
if let Some((clip_a, clip_b)) = clip_segments(seg1, seg2) {
if let Some((clip_a, clip_b)) = query::details::clip_segment_segment(seg1, seg2) {
let dist_a = (clip_a.1 - clip_a.0).dot(&local_n1);
let dist_b = (clip_b.1 - clip_b.0).dot(&local_n1);
let mut impulses_a = (0.0, Contact::zero_tangent_impulse());
let mut impulses_b = (0.0, Contact::zero_tangent_impulse());
let mut data_a = ContactData::default();
let mut data_b = ContactData::default();
let fids_a = (
((support_face1 * 2 + clip_a.2) % (len1 * 2)) as u8,
@@ -111,26 +114,15 @@ fn generate_contacts<'a>(
}
if fids_a == original_fids_a {
impulses_a = (
manifold.points[0].impulse,
manifold.points[0].tangent_impulse,
);
data_a = manifold.points[0].data;
} else if fids_a == original_fids_b {
impulses_a = (
manifold.points[1].impulse,
manifold.points[1].tangent_impulse,
);
data_a = manifold.points[1].data;
}
if fids_b == original_fids_a {
impulses_b = (
manifold.points[0].impulse,
manifold.points[0].tangent_impulse,
);
data_b = manifold.points[0].data;
} else if fids_b == original_fids_b {
impulses_b = (
manifold.points[1].impulse,
manifold.points[1].tangent_impulse,
);
data_b = manifold.points[1].data;
}
}
@@ -138,21 +130,19 @@ fn generate_contacts<'a>(
manifold.points.push(Contact {
local_p1: clip_a.0,
local_p2: m21 * clip_a.1,
impulse: impulses_a.0,
tangent_impulse: impulses_a.1,
fid1: fids_a.0,
fid2: fids_a.1,
dist: dist_a,
data: data_a,
});
manifold.points.push(Contact {
local_p1: clip_b.0,
local_p2: m21 * clip_b.1,
impulse: impulses_b.0,
tangent_impulse: impulses_b.1,
fid1: fids_b.0,
fid2: fids_b.1,
dist: dist_b,
data: data_b,
});
manifold.local_n1 = local_n1;
@@ -164,138 +154,3 @@ fn generate_contacts<'a>(
manifold.points.clear();
}
}
// Features in clipping points are:
// 0 = First vertex.
// 1 = On the face.
// 2 = Second vertex.
pub(crate) type ClippingPoints = (Point<f32>, Point<f32>, usize, usize);
#[cfg(feature = "dim2")]
pub(crate) fn clip_segments_with_normal(
mut seg1: (Point<f32>, Point<f32>),
mut seg2: (Point<f32>, Point<f32>),
normal: Vector<f32>,
) -> Option<(ClippingPoints, ClippingPoints)> {
use crate::utils::WBasis;
let tangent = normal.orthonormal_basis()[0];
let mut range1 = [seg1.0.coords.dot(&tangent), seg1.1.coords.dot(&tangent)];
let mut range2 = [seg2.0.coords.dot(&tangent), seg2.1.coords.dot(&tangent)];
let mut features1 = [0, 2];
let mut features2 = [0, 2];
if range1[1] < range1[0] {
range1.swap(0, 1);
features1.swap(0, 1);
std::mem::swap(&mut seg1.0, &mut seg1.1);
}
if range2[1] < range2[0] {
range2.swap(0, 1);
features2.swap(0, 1);
std::mem::swap(&mut seg2.0, &mut seg2.1);
}
if range2[0] > range1[1] || range1[0] > range2[1] {
// No clip point.
return None;
}
let ca = if range2[0] > range1[0] {
let bcoord = (range2[0] - range1[0]) * utils::inv(range1[1] - range1[0]);
let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord;
let p2 = seg2.0;
(p1, p2, 1, features2[0])
} else {
let bcoord = (range1[0] - range2[0]) * utils::inv(range2[1] - range2[0]);
let p1 = seg1.0;
let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
(p1, p2, features1[0], 1)
};
let cb = if range2[1] < range1[1] {
let bcoord = (range2[1] - range1[0]) * utils::inv(range1[1] - range1[0]);
let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord;
let p2 = seg2.1;
(p1, p2, 1, features2[1])
} else {
let bcoord = (range1[1] - range2[0]) * utils::inv(range2[1] - range2[0]);
let p1 = seg1.1;
let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
(p1, p2, features1[1], 1)
};
Some((ca, cb))
}
pub(crate) fn clip_segments(
mut seg1: (Point<f32>, Point<f32>),
mut seg2: (Point<f32>, Point<f32>),
) -> Option<(ClippingPoints, ClippingPoints)> {
// NOTE: no need to normalize the tangent.
let tangent1 = seg1.1 - seg1.0;
let sqnorm_tangent1 = tangent1.norm_squared();
let mut range1 = [0.0, sqnorm_tangent1];
let mut range2 = [
(seg2.0 - seg1.0).dot(&tangent1),
(seg2.1 - seg1.0).dot(&tangent1),
];
let mut features1 = [0, 2];
let mut features2 = [0, 2];
if range1[1] < range1[0] {
range1.swap(0, 1);
features1.swap(0, 1);
std::mem::swap(&mut seg1.0, &mut seg1.1);
}
if range2[1] < range2[0] {
range2.swap(0, 1);
features2.swap(0, 1);
std::mem::swap(&mut seg2.0, &mut seg2.1);
}
if range2[0] > range1[1] || range1[0] > range2[1] {
// No clip point.
return None;
}
let length1 = range1[1] - range1[0];
let length2 = range2[1] - range2[0];
let ca = if range2[0] > range1[0] {
let bcoord = (range2[0] - range1[0]) / length1;
let p1 = seg1.0 + tangent1 * bcoord;
let p2 = seg2.0;
(p1, p2, 1, features2[0])
} else {
let bcoord = (range1[0] - range2[0]) / length2;
let p1 = seg1.0;
let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
(p1, p2, features1[0], 1)
};
let cb = if range2[1] < range1[1] {
let bcoord = (range2[1] - range1[0]) / length1;
let p1 = seg1.0 + tangent1 * bcoord;
let p2 = seg2.1;
(p1, p2, 1, features2[1])
} else {
let bcoord = (range1[1] - range2[0]) / length2;
let p1 = seg1.1;
let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord;
(p1, p2, features1[1], 1)
};
Some((ca, cb))
}

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

@@ -1,9 +1,9 @@
use crate::buckler::bounding_volume::{BoundingVolume, AABB};
use crate::data::MaybeSerializableData;
use crate::geometry::contact_generator::{
ContactGenerationContext, PrimitiveContactGenerationContext,
};
use crate::geometry::{Collider, ContactManifold, ShapeType, Trimesh};
use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
use crate::geometry::{Collider, ContactManifold, ContactManifoldData, ShapeType, Trimesh};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
@@ -11,7 +11,7 @@ use erased_serde::Serialize;
#[derive(Clone)]
pub struct TrimeshShapeContactGeneratorWorkspace {
interferences: Vec<usize>,
local_aabb2: AABB<f32>,
local_aabb2: AABB,
old_interferences: Vec<usize>,
#[cfg_attr(feature = "serde-serialize", serde(skip))]
old_manifolds: Vec<ContactManifold>,
@@ -96,7 +96,7 @@ fn do_generate_contacts(
.old_manifolds
.iter()
.map(|manifold| {
if manifold.pair.collider1 == ctxt_collider1 {
if manifold.data.pair.collider1 == ctxt_collider1 {
manifold.subshape_index_pair.0
} else {
manifold.subshape_index_pair.1
@@ -153,14 +153,14 @@ fn do_generate_contacts(
let manifold = if old_inter_it.peek() != Some(triangle_id) {
// We don't have a manifold for this triangle yet.
ContactManifold::with_subshape_indices(
let data = ContactManifoldData::from_colliders(
ctxt_pair_pair,
collider1,
collider2,
*triangle_id,
0,
ctxt.solver_flags,
)
);
ContactManifold::with_data((*triangle_id, 0), data)
} else {
// We already have a manifold for this triangle.
old_inter_it.next();
@@ -176,7 +176,7 @@ fn do_generate_contacts(
.dispatcher
.dispatch_primitives(ShapeType::Triangle, shape_type2);
let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 {
let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.data.pair.collider1 {
PrimitiveContactGenerationContext {
prediction_distance: ctxt.prediction_distance,
collider1: collider2,

234
src/geometry/cuboid.rs
View File

@@ -1,234 +0,0 @@
#[cfg(feature = "dim3")]
use crate::geometry::PolyhedronFace;
use crate::geometry::{Cuboid, CuboidFeature, CuboidFeatureFace};
use crate::math::{Point, Vector};
use crate::utils::WSign;
pub fn local_support_point(cube: &Cuboid, local_dir: Vector<f32>) -> Point<f32> {
local_dir.copy_sign_to(cube.half_extents).into()
}
// #[cfg(feature = "dim2")]
// pub fn polygon_ref(
// cuboid: Cuboid,
// out_vertices: &mut [Point<f32>; 4],
// out_normals: &mut [Vector<f32>; 4],
// ) -> PolygonRef {
// *out_vertices = [
// Point::new(cuboid.half_extents.x, -cuboid.half_extents.y),
// Point::new(cuboid.half_extents.x, cuboid.half_extents.y),
// Point::new(-cuboid.half_extents.x, cuboid.half_extents.y),
// Point::new(-cuboid.half_extents.x, -cuboid.half_extents.y),
// ];
// *out_normals = [Vector::x(), Vector::y(), -Vector::x(), -Vector::y()];
//
// PolygonRef {
// vertices: &out_vertices[..],
// normals: &out_normals[..],
// }
// }
#[cfg(feature = "dim2")]
pub fn vertex_feature_id(vertex: Point<f32>) -> u8 {
((vertex.x.to_bits() >> 31) & 0b001 | (vertex.y.to_bits() >> 30) & 0b010) as u8
}
// #[cfg(feature = "dim3")]
// pub fn vertex_feature_id(vertex: Point<f32>) -> u8 {
// ((vertex.x.to_bits() >> 31) & 0b001
// | (vertex.y.to_bits() >> 30) & 0b010
// | (vertex.z.to_bits() >> 29) & 0b100) as u8
// }
#[cfg(feature = "dim3")]
pub fn polyhedron_support_face(cube: &Cuboid, local_dir: Vector<f32>) -> PolyhedronFace {
support_face(cube, local_dir).into()
}
#[cfg(feature = "dim2")]
pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeature {
// In 2D, it is best for stability to always return a face.
// It won't have any notable impact on performances anyway.
CuboidFeature::Face(support_face(cube, local_dir))
/*
let amax = local_dir.amax();
const MAX_DOT_THRESHOLD: f32 = 0.98480775301; // 10 degrees.
if amax > MAX_DOT_THRESHOLD {
// Support face.
CuboidFeature::Face(cube.support_face(local_dir))
} else {
// Support vertex
CuboidFeature::Vertex(cube.support_vertex(local_dir))
}
*/
}
#[cfg(feature = "dim3")]
pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeature {
CuboidFeature::Face(support_face(cube, local_dir))
/*
const MAX_DOT_THRESHOLD: f32 = crate::utils::COS_10_DEGREES;
const MIN_DOT_THRESHOLD: f32 = 1.0 - MAX_DOT_THRESHOLD;
let amax = local_dir.amax();
let amin = local_dir.amin();
if amax > MAX_DOT_THRESHOLD {
// Support face.
CuboidFeature::Face(support_face(cube, local_dir))
} else if amin < MIN_DOT_THRESHOLD {
// Support edge.
CuboidFeature::Edge(support_edge(cube, local_dir))
} else {
// Support vertex.
CuboidFeature::Vertex(support_vertex(cube, local_dir))
}
*/
}
// #[cfg(feature = "dim3")]
// pub(crate) fn support_vertex(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureVertex {
// let vertex = local_support_point(cube, local_dir);
// let vid = vertex_feature_id(vertex);
//
// CuboidFeatureVertex { vertex, vid }
// }
// #[cfg(feature = "dim3")]
// pub(crate) fn support_edge(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureEdge {
// let he = cube.half_extents;
// let i = local_dir.iamin();
// let j = (i + 1) % 3;
// let k = (i + 2) % 3;
// let mut a = Point::origin();
// a[i] = he[i];
// a[j] = local_dir[j].copy_sign_to(he[j]);
// a[k] = local_dir[k].copy_sign_to(he[k]);
//
// let mut b = a;
// b[i] = -he[i];
//
// let vid1 = vertex_feature_id(a);
// let vid2 = vertex_feature_id(b);
// let eid = (vid1.max(vid2) << 3) | vid1.min(vid2) | 0b11_000_000;
//
// CuboidFeatureEdge {
// vertices: [a, b],
// vids: [vid1, vid2],
// eid,
// }
// }
#[cfg(feature = "dim2")]
pub fn support_face(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureFace {
let he = cube.half_extents;
let i = local_dir.iamin();
let j = (i + 1) % 2;
let mut a = Point::origin();
a[i] = he[i];
a[j] = local_dir[j].copy_sign_to(he[j]);
let mut b = a;
b[i] = -he[i];
let vid1 = vertex_feature_id(a);
let vid2 = vertex_feature_id(b);
let fid = (vid1.max(vid2) << 2) | vid1.min(vid2) | 0b11_00_00;
CuboidFeatureFace {
vertices: [a, b],
vids: [vid1, vid2],
fid,
}
}
#[cfg(feature = "dim3")]
pub(crate) fn support_face(cube: &Cuboid, local_dir: Vector<f32>) -> CuboidFeatureFace {
// NOTE: can we use the orthonormal basis of local_dir
// to make this AoSoA friendly?
let he = cube.half_extents;
let iamax = local_dir.iamax();
let sign = local_dir[iamax].copy_sign_to(1.0);
let vertices = match iamax {
0 => [
Point::new(he.x * sign, he.y, he.z),
Point::new(he.x * sign, -he.y, he.z),
Point::new(he.x * sign, -he.y, -he.z),
Point::new(he.x * sign, he.y, -he.z),
],
1 => [
Point::new(he.x, he.y * sign, he.z),
Point::new(-he.x, he.y * sign, he.z),
Point::new(-he.x, he.y * sign, -he.z),
Point::new(he.x, he.y * sign, -he.z),
],
2 => [
Point::new(he.x, he.y, he.z * sign),
Point::new(he.x, -he.y, he.z * sign),
Point::new(-he.x, -he.y, he.z * sign),
Point::new(-he.x, he.y, he.z * sign),
],
_ => unreachable!(),
};
pub fn vid(i: u8) -> u8 {
// Each vertex has an even feature id.
i * 2
}
let sign_index = ((sign as i8 + 1) / 2) as usize;
// The vertex id as numbered depending on the sign of the vertex
// component. A + sign means the corresponding bit is 0 while a -
// sign means the corresponding bit is 1.
// For exampl the vertex [2.0, -1.0, -3.0] has the id 0b011
let vids = match iamax {
0 => [
[vid(0b000), vid(0b010), vid(0b011), vid(0b001)],
[vid(0b100), vid(0b110), vid(0b111), vid(0b101)],
][sign_index],
1 => [
[vid(0b000), vid(0b100), vid(0b101), vid(0b001)],
[vid(0b010), vid(0b110), vid(0b111), vid(0b011)],
][sign_index],
2 => [
[vid(0b000), vid(0b010), vid(0b110), vid(0b100)],
[vid(0b001), vid(0b011), vid(0b111), vid(0b101)],
][sign_index],
_ => unreachable!(),
};
// The feature ids of edges is obtained from the vertex ids
// of their endpoints.
// Assuming vid1 > vid2, we do: (vid1 << 3) | vid2 | 0b11000000
//
let eids = match iamax {
0 => [
[0b11_010_000, 0b11_011_010, 0b11_011_001, 0b11_001_000],
[0b11_110_100, 0b11_111_110, 0b11_111_101, 0b11_101_100],
][sign_index],
1 => [
[0b11_100_000, 0b11_101_100, 0b11_101_001, 0b11_001_000],
[0b11_110_010, 0b11_111_110, 0b11_111_011, 0b11_011_010],
][sign_index],
2 => [
[0b11_010_000, 0b11_110_010, 0b11_110_100, 0b11_100_000],
[0b11_011_001, 0b11_111_011, 0b11_111_101, 0b11_101_001],
][sign_index],
_ => unreachable!(),
};
// The face with normals [x, y, z] are numbered [10, 11, 12].
// The face with negated normals are numbered [13, 14, 15].
let fid = iamax + sign_index * 3 + 10;
CuboidFeatureFace {
vertices,
vids,
eids,
fid: fid as u8,
}
}

128
src/geometry/cuboid_feature2d.rs
View File

@@ -1,128 +0,0 @@
use crate::geometry::{self, Contact, ContactManifold};
use crate::math::{Isometry, Point, Vector};
use ncollide::shape::Segment;
#[derive(Debug)]
#[allow(dead_code)]
pub enum CuboidFeature {
Face(CuboidFeatureFace),
Vertex(CuboidFeatureVertex),
}
#[derive(Debug)]
pub struct CuboidFeatureVertex {
pub vertex: Point<f32>,
pub vid: u8,
}
impl CuboidFeatureVertex {
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
self.vertex = iso * self.vertex;
}
}
#[derive(Debug)]
pub struct CuboidFeatureFace {
pub vertices: [Point<f32>; 2],
pub vids: [u8; 2],
pub fid: u8,
}
impl From<Segment<f32>> for CuboidFeatureFace {
fn from(seg: Segment<f32>) -> Self {
CuboidFeatureFace {
vertices: [seg.a, seg.b],
vids: [0, 2],
fid: 1,
}
}
}
impl CuboidFeatureFace {
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
self.vertices[0] = iso * self.vertices[0];
self.vertices[1] = iso * self.vertices[1];
}
}
impl CuboidFeature {
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
match self {
CuboidFeature::Face(face) => face.transform_by(iso),
CuboidFeature::Vertex(vertex) => vertex.transform_by(iso),
}
}
/// Compute contacts points between a face and a vertex.
///
/// This method assume we already know that at least one contact exists.
pub fn face_vertex_contacts(
face1: &CuboidFeatureFace,
sep_axis1: &Vector<f32>,
vertex2: &CuboidFeatureVertex,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
) {
let tangent1 = face1.vertices[1] - face1.vertices[0];
let normal1 = Vector::new(-tangent1.y, tangent1.x);
let denom = -normal1.dot(&sep_axis1);
let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom;
let local_p2 = vertex2.vertex;
let local_p1 = vertex2.vertex - dist * sep_axis1;
manifold.points.push(Contact {
local_p1,
local_p2: pos21 * local_p2,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: face1.fid,
fid2: vertex2.vid,
dist,
});
}
pub fn face_face_contacts(
_prediction_distance: f32,
face1: &CuboidFeatureFace,
normal1: &Vector<f32>,
face2: &CuboidFeatureFace,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
) {
if let Some((clip_a, clip_b)) = geometry::clip_segments(
(face1.vertices[0], face1.vertices[1]),
(face2.vertices[0], face2.vertices[1]),
) {
let fids1 = [face1.vids[0], face1.fid, face1.vids[1]];
let fids2 = [face2.vids[0], face2.fid, face2.vids[1]];
let dist = (clip_a.1 - clip_a.0).dot(normal1);
if true {
// dist < prediction_distance {
manifold.points.push(Contact {
local_p1: clip_a.0,
local_p2: pos21 * clip_a.1,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: fids1[clip_a.2],
fid2: fids2[clip_a.3],
dist,
});
}
let dist = (clip_b.1 - clip_b.0).dot(normal1);
if true {
// dist < prediction_distance {
manifold.points.push(Contact {
local_p1: clip_b.0,
local_p2: pos21 * clip_b.1,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: fids1[clip_b.2],
fid2: fids2[clip_b.3],
dist,
});
}
}
}
}

516
src/geometry/cuboid_feature3d.rs
View File

@@ -1,516 +0,0 @@
use crate::geometry::{Contact, ContactManifold};
use crate::math::{Isometry, Point, Vector};
use crate::utils::WBasis;
use na::Point2;
#[derive(Debug)]
#[allow(dead_code)]
pub(crate) enum CuboidFeature {
Face(CuboidFeatureFace),
Edge(CuboidFeatureEdge),
Vertex(CuboidFeatureVertex),
}
#[derive(Debug)]
pub(crate) struct CuboidFeatureVertex {
pub vertex: Point<f32>,
pub vid: u8,
}
impl CuboidFeatureVertex {
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
self.vertex = iso * self.vertex;
}
}
#[derive(Debug)]
pub(crate) struct CuboidFeatureEdge {
pub vertices: [Point<f32>; 2],
pub vids: [u8; 2],
pub eid: u8,
}
impl CuboidFeatureEdge {
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
self.vertices[0] = iso * self.vertices[0];
self.vertices[1] = iso * self.vertices[1];
}
}
#[derive(Debug)]
pub(crate) struct CuboidFeatureFace {
pub vertices: [Point<f32>; 4],
pub vids: [u8; 4], // Feature ID of the vertices.
pub eids: [u8; 4], // Feature ID of the edges.
pub fid: u8, // Feature ID of the face.
}
impl CuboidFeatureFace {
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
self.vertices[0] = iso * self.vertices[0];
self.vertices[1] = iso * self.vertices[1];
self.vertices[2] = iso * self.vertices[2];
self.vertices[3] = iso * self.vertices[3];
}
}
impl CuboidFeature {
pub fn transform_by(&mut self, iso: &Isometry<f32>) {
match self {
CuboidFeature::Face(face) => face.transform_by(iso),
CuboidFeature::Edge(edge) => edge.transform_by(iso),
CuboidFeature::Vertex(vertex) => vertex.transform_by(iso),
}
}
/// Compute contacts points between a face and a vertex.
///
/// This method assume we already know that at least one contact exists.
pub fn face_vertex_contacts(
face1: &CuboidFeatureFace,
sep_axis1: &Vector<f32>,
vertex2: &CuboidFeatureVertex,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
) {
let normal1 =
(face1.vertices[0] - face1.vertices[1]).cross(&(face1.vertices[2] - face1.vertices[1]));
let denom = -normal1.dot(&sep_axis1);
let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom;
let local_p2 = vertex2.vertex;
let local_p1 = vertex2.vertex - dist * sep_axis1;
manifold.points.push(Contact {
local_p1,
local_p2: pos21 * local_p2,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: face1.fid,
fid2: vertex2.vid,
dist,
});
}
/// Compute contacts points between a face and an edge.
///
/// This method assume we already know that at least one contact exists.
pub fn face_edge_contacts(
prediction_distance: f32,
face1: &CuboidFeatureFace,
sep_axis1: &Vector<f32>,
edge2: &CuboidFeatureEdge,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
flipped: bool,
) {
// 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_face1 = [
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]),
),
Point2::new(
face1.vertices[2].coords.dot(&basis[0]),
face1.vertices[2].coords.dot(&basis[1]),
),
Point2::new(
face1.vertices[3].coords.dot(&basis[0]),
face1.vertices[3].coords.dot(&basis[1]),
),
];
let projected_edge2 = [
Point2::new(
edge2.vertices[0].coords.dot(&basis[0]),
edge2.vertices[0].coords.dot(&basis[1]),
),
Point2::new(
edge2.vertices[1].coords.dot(&basis[0]),
edge2.vertices[1].coords.dot(&basis[1]),
),
];
// Now we have to compute the intersection between all pairs of
// edges from the face 1 with the edge 2
for i in 0..4 {
let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]];
if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 {
// Found a contact between the two edges.
let edge1 = [face1.vertices[i], face1.vertices[(i + 1) % 4]];
let local_p1 = edge1[0] * (1.0 - bcoords.0) + edge1[1].coords * bcoords.0;
let local_p2 = edge2.vertices[0] * (1.0 - bcoords.1)
+ edge2.vertices[1].coords * bcoords.1;
let dist = (local_p2 - local_p1).dot(&sep_axis1);
if dist < prediction_distance {
if flipped {
manifold.points.push(Contact {
local_p1: local_p2,
// All points are expressed in the locale space of the first shape
// (even if there was a flip). So the point we need to transform by
// pos21 is the one that will go into local_p2.
local_p2: pos21 * local_p1,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: edge2.eid,
fid2: face1.eids[i],
dist,
});
} else {
manifold.points.push(Contact {
local_p1,
local_p2: pos21 * local_p2,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: face1.eids[i],
fid2: edge2.eid,
dist,
});
}
}
}
}
}
// Project the two points from the edge into the face.
let normal1 =
(face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1]));
'point_loop2: for i in 0..2 {
let p2 = projected_edge2[i];
let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3]));
for j in 0..3 {
let new_sign =
(projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j]));
if new_sign * sign < 0.0 {
// The point lies outside.
continue 'point_loop2;
}
}
// All the perp had the same sign: the point is inside of the other shapes projection.
// Output the contact.
let denom = -normal1.dot(&sep_axis1);
let dist = (face1.vertices[0] - edge2.vertices[i]).dot(&normal1) / denom;
let local_p2 = edge2.vertices[i];
let local_p1 = edge2.vertices[i] - dist * sep_axis1;
if dist < prediction_distance {
if flipped {
manifold.points.push(Contact {
local_p1: local_p2,
// All points are expressed in the locale space of the first shape
// (even if there was a flip). So the point we need to transform by
// pos21 is the one that will go into local_p2.
local_p2: pos21 * local_p1,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: edge2.vids[i],
fid2: face1.fid,
dist,
});
} else {
manifold.points.push(Contact {
local_p1,
local_p2: pos21 * local_p2,
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: face1.fid,
fid2: edge2.vids[i],
dist,
});
}
}
}
}
/// Compute contacts points between two edges.
///
/// This method assume we already know that at least one contact exists.
pub fn edge_edge_contacts(
edge1: &CuboidFeatureEdge,
sep_axis1: &Vector<f32>,
edge2: &CuboidFeatureEdge,
pos21: &Isometry<f32>,
manifold: &mut ContactManifold,
) {
let basis = sep_axis1.orthonormal_basis();
let projected_edge1 = [
Point2::new(
edge1.vertices[0].coords.dot(&basis[0]),
edge1.vertices[0].coords.dot(&basis[1]),
),
Point2::new(
edge1.vertices[1].coords.dot(&basis[0]),
edge1.vertices[1].coords.dot(&basis[1]),
),
];
let projected_edge2 = [
Point2::new(
edge2.vertices[0].coords.dot(&basis[0]),
edge2.vertices[0].coords.dot(&basis[1]),
),
Point2::new(
edge2.vertices[1].coords.dot(&basis[0]),
edge2.vertices[1].coords.dot(&basis[1]),
),
];
if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
let local_p1 =
edge1.vertices[0] * (1.0 - bcoords.0) + edge1.vertices[1].coords * bcoords.0;
let local_p2 =
edge2.vertices[0] * (1.0 - bcoords.1) + edge2.vertices[1].coords * bcoords.1;
let dist = (local_p2 - local_p1).dot(&sep_axis1);
manifold.points.push(Contact {
local_p1,
local_p2: pos21 * local_p2, // NOTE: local_p2 is expressed in the local space of cube1.
impulse: 0.0,
tangent_impulse: Contact::zero_tangent_impulse(),
fid1: edge1.eid,
fid2: edge2.eid,
dist,
});
}
}
pub fn face_face_contacts(
_prediction_distance: f32,
face1: &CuboidFeatureFace,
sep_axis1: &Vector<f32>,
face2: &CuboidFeatureFace,
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_face1 = [
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]),
),
Point2::new(
face1.vertices[2].coords.dot(&basis[0]),
face1.vertices[2].coords.dot(&basis[1]),
),
Point2::new(
face1.vertices[3].coords.dot(&basis[0]),
face1.vertices[3].coords.dot(&basis[1]),
),
];
let projected_face2 = [
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]),
),
Point2::new(
face2.vertices[2].coords.dot(&basis[0]),
face2.vertices[2].coords.dot(&basis[1]),
),
Point2::new(
face2.vertices[3].coords.dot(&basis[0]),
face2.vertices[3].coords.dot(&basis[1]),
),
];
// Also find all the vertices located inside of the other projected face.
let normal1 =
(face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1]));
let normal2 =
(face2.vertices[2] - face2.vertices[1]).cross(&(face2.vertices[0] - face2.vertices[1]));
// NOTE: The loop iterating on all the vertices for face1 is different from
// the one iterating on all the vertices of face2. In the second loop, we
// classify every point wrt. every edge on the other face. This will give
// us bit masks to filter out several edge-edge intersections.
'point_loop1: for i in 0..4 {
let p1 = projected_face1[i];
let sign = (projected_face2[0] - projected_face2[3]).perp(&(p1 - projected_face2[3]));
for j in 0..3 {
let new_sign =
(projected_face2[j + 1] - projected_face2[j]).perp(&(p1 - projected_face2[j]));
if new_sign * sign < 0.0 {
// The point lies outside.
continue 'point_loop1;
}
}
// All the perp had the same sign: the point is inside of the other shapes projection.
// Output the contact.
let denom = normal2.dot(&sep_axis1);
let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
let local_p1 = face1.vertices[i];
let local_p2 = face1.vertices[i] + dist * sep_axis1;
if true {
// 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.vids[i],
fid2: face2.fid,
dist,
});
}
}
let is_clockwise1 = (projected_face1[0] - projected_face1[1])
.perp(&(projected_face1[2] - projected_face1[1]))
>= 0.0;
let mut vertex_class2 = [0u8; 4];
for i in 0..4 {
let p2 = projected_face2[i];
let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3]));
vertex_class2[i] |= ((sign >= 0.0) as u8) << 3;
for j in 0..3 {
let sign =
(projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j]));
vertex_class2[i] |= ((sign >= 0.0) as u8) << j;
}
if !is_clockwise1 {
vertex_class2[i] = (!vertex_class2[i]) & 0b01111;
}
if vertex_class2[i] == 0 {
// All the perp had the same sign: the point is inside of the other shapes projection.
// Output the contact.
let denom = -normal1.dot(&sep_axis1);
let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
let local_p2 = face2.vertices[i];
let local_p1 = face2.vertices[i] - dist * sep_axis1;
if true {
// 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.fid,
fid2: face2.vids[i],
dist,
});
}
}
}
// Now we have to compute the intersection between all pairs of
// edges from the face 1 and from the face2.
for j in 0..4 {
let projected_edge2 = [projected_face2[j], projected_face2[(j + 1) % 4]];
if (vertex_class2[j] & vertex_class2[(j + 1) % 4]) != 0 {
continue;
}
let edge_class2 = vertex_class2[j] | vertex_class2[(j + 1) % 4];
for i in 0..4 {
if (edge_class2 & (1 << i)) != 0 {
let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]];
if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0
{
// Found a contact between the two edges.
let edge1 = (face1.vertices[i], face1.vertices[(i + 1) % 4]);
let edge2 = (face2.vertices[j], face2.vertices[(j + 1) % 4]);
let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1;
let dist = (local_p2 - local_p1).dot(&sep_axis1);
if true {
// 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[i],
fid2: face2.eids[j],
dist,
});
}
}
}
}
}
}
}
}
/// 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];
let r = edge1[0] - edge2[0];
let a = dir1.norm_squared();
let e = dir2.norm_squared();
let f = dir2.dot(&r);
let eps = f32::default_epsilon();
if a <= eps && e <= eps {
Some((0.0, 0.0))
} else if a <= eps {
Some((0.0, f / e))
} else {
let c = dir1.dot(&r);
if e <= eps {
Some((-c / a, 0.0))
} else {
let b = dir1.dot(&dir2);
let ae = a * e;
let bb = b * b;
let denom = ae - bb;
// Use absolute and ulps error to test collinearity.
let parallel = denom <= eps || approx::ulps_eq!(ae, bb);
let s = if !parallel {
(b * f - c * e) / denom
} else {
0.0
};
if parallel {
None
} else {
Some((s, (b * s + f) / e))
}
}
}
}

110
src/geometry/mod.rs
View File

@@ -1,17 +1,11 @@
//! Structures related to geometry: colliders, shapes, etc.
pub use self::broad_phase_multi_sap::BroadPhase;
pub use self::capsule::Capsule;
pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
pub use self::collider_set::{ColliderHandle, ColliderSet};
pub use self::contact::{
Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, SolverFlags,
};
pub use self::contact::{ContactData, ContactManifoldData};
pub use self::contact::{ContactPair, SolverFlags};
pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher};
#[cfg(feature = "dim2")]
pub(crate) use self::cuboid_feature2d::{CuboidFeature, CuboidFeatureFace};
#[cfg(feature = "dim3")]
pub(crate) use self::cuboid_feature3d::{CuboidFeature, CuboidFeatureFace};
pub use self::interaction_graph::{
ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex,
};
@@ -23,36 +17,87 @@ pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatch
pub use self::round_cylinder::RoundCylinder;
pub use self::trimesh::Trimesh;
pub use self::user_callbacks::{ContactPairFilter, PairFilterContext, ProximityPairFilter};
pub use ncollide::query::Proximity;
pub use buckler::query::Proximity;
pub use buckler::query::{KinematicsCategory, TrackedContact};
pub type Contact = buckler::query::TrackedContact<ContactData>;
pub type ContactManifold = buckler::query::ContactManifold<ContactManifoldData, ContactData>;
/// A segment shape.
pub type Segment = ncollide::shape::Segment<f32>;
pub type Segment = buckler::shape::Segment;
/// A cuboid shape.
pub type Cuboid = ncollide::shape::Cuboid<f32>;
pub type Cuboid = buckler::shape::Cuboid;
/// A triangle shape.
pub type Triangle = ncollide::shape::Triangle<f32>;
pub type Triangle = buckler::shape::Triangle;
/// A ball shape.
pub type Ball = ncollide::shape::Ball<f32>;
pub type Ball = buckler::shape::Ball;
/// A capsule shape.
pub type Capsule = buckler::shape::Capsule;
/// A heightfield shape.
pub type HeightField = ncollide::shape::HeightField<f32>;
pub type HeightField = buckler::shape::HeightField;
/// A cylindrical shape.
#[cfg(feature = "dim3")]
pub type Cylinder = ncollide::shape::Cylinder<f32>;
pub type Cylinder = buckler::shape::Cylinder;
/// A cone shape.
#[cfg(feature = "dim3")]
pub type Cone = ncollide::shape::Cone<f32>;
pub type Cone = buckler::shape::Cone;
/// 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.
pub type ContactEvent = ncollide::pipeline::ContactEvent<ColliderHandle>;
/// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not).
pub type ProximityEvent = ncollide::pipeline::ProximityEvent<ColliderHandle>;
pub type AABB = buckler::bounding_volume::AABB;
/// A ray that can be cast against colliders.
pub type Ray = ncollide::query::Ray<f32>;
pub type Ray = buckler::query::Ray;
/// The intersection between a ray and a collider.
pub type RayIntersection = ncollide::query::RayIntersection<f32>;
pub type RayIntersection = buckler::query::RayIntersection;
/// The the projection of a point on a collider.
pub type PointProjection = ncollide::query::PointProjection<f32>;
pub type PointProjection = buckler::query::PointProjection;
#[derive(Copy, Clone, Hash, Debug)]
/// Events occurring when two collision objects start or stop being in contact (or penetration).
pub enum ContactEvent {
/// Event occurring when two collision objects start being in contact.
///
/// This event is generated whenever the narrow-phase finds a contact between two collision objects that did not have any contact at the last update.
Started(ColliderHandle, ColliderHandle),
/// Event occurring when two collision objects stop being in contact.
///
/// This event is generated whenever the narrow-phase fails to find any contact between two collision objects that did have at least one contact at the last update.
Stopped(ColliderHandle, ColliderHandle),
}
#[derive(Copy, Clone, Debug)]
/// Events occurring when two collision objects start or stop being in close proximity, contact, or disjoint.
pub struct ProximityEvent {
/// The first collider to which the proximity event applies.
pub collider1: ColliderHandle,
/// The second collider to which the proximity event applies.
pub collider2: ColliderHandle,
/// The previous state of proximity between the two collision objects.
pub prev_status: Proximity,
/// The new state of proximity between the two collision objects.
pub new_status: Proximity,
}
impl ProximityEvent {
/// Instantiates a new proximity event.
///
/// Panics if `prev_status` is equal to `new_status`.
pub fn new(
collider1: ColliderHandle,
collider2: ColliderHandle,
prev_status: Proximity,
new_status: Proximity,
) -> Self {
assert_ne!(
prev_status, new_status,
"The previous and new status of a proximity event must not be the same."
);
Self {
collider1,
collider2,
prev_status,
new_status,
}
}
}
#[cfg(feature = "simd-is-enabled")]
pub(crate) use self::ball::WBall;
@@ -60,14 +105,7 @@ 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_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;
@@ -80,16 +118,9 @@ mod collider;
mod collider_set;
mod contact;
mod contact_generator;
pub(crate) mod cuboid;
#[cfg(feature = "dim2")]
mod cuboid_feature2d;
#[cfg(feature = "dim3")]
mod cuboid_feature3d;
mod interaction_graph;
mod narrow_phase;
mod polygon;
#[cfg(feature = "dim3")]
mod polyhedron_feature3d;
mod proximity;
mod proximity_detector;
pub(crate) mod sat;
@@ -98,11 +129,8 @@ mod trimesh;
mod waabb;
mod wquadtree;
//mod z_order;
mod capsule;
mod interaction_groups;
#[cfg(feature = "dim3")]
mod polygonal_feature_map;
#[cfg(feature = "dim3")]
mod round_cylinder;
mod shape;
mod user_callbacks;

7
src/geometry/narrow_phase.rs
View File

@@ -21,9 +21,9 @@ use crate::geometry::{
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
//#[cfg(feature = "simd-is-enabled")]
//use crate::math::{SimdFloat, SIMD_WIDTH};
use crate::buckler::query::Proximity;
use crate::data::pubsub::Subscription;
use crate::data::Coarena;
use crate::ncollide::query::Proximity;
use crate::pipeline::EventHandler;
use std::collections::HashMap;
//use simba::simd::SimdValue;
@@ -568,9 +568,10 @@ impl NarrowPhase {
// FIXME: don't iterate through all the interactions.
for inter in self.contact_graph.graph.edges.iter_mut() {
for manifold in &mut inter.weight.manifolds {
let rb1 = &bodies[manifold.body_pair.body1];
let rb2 = &bodies[manifold.body_pair.body2];
let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2];
if manifold
.data
.solver_flags
.contains(SolverFlags::COMPUTE_IMPULSES)
&& manifold.num_active_contacts() != 0

4
src/geometry/polygon.rs
View File

@@ -1,7 +1,7 @@
#![allow(dead_code)] // TODO: remove this once we support polygons.
use crate::math::{Isometry, Point, Vector};
use ncollide::bounding_volume::AABB;
use buckler::bounding_volume::AABB;
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -27,7 +27,7 @@ impl Polygon {
}
/// Compute the axis-aligned bounding box of the polygon.
pub fn aabb(&self, pos: &Isometry<f32>) -> AABB<f32> {
pub fn aabb(&self, pos: &Isometry<f32>) -> AABB {
let p0 = pos * self.vertices[0];
let mut mins = p0;
let mut maxs = p0;

132
src/geometry/polygonal_feature_map.rs
View File

@@ -1,132 +0,0 @@
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];
}
}
}

445
src/geometry/polyhedron_feature3d.rs
View File

@@ -1,445 +0,0 @@
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;
use ncollide::shape::Segment;
#[derive(Debug, Clone)]
pub struct PolyhedronFace {
pub vertices: [Point<f32>; 4],
pub vids: [u8; 4], // Feature ID of the vertices.
pub eids: [u8; 4], // Feature ID of the edges.
pub fid: u8, // Feature ID of the face.
pub num_vertices: usize,
}
impl Default for PolyhedronFace {
fn default() -> Self {
Self {
vertices: [Point::origin(); 4],
vids: [0; 4],
eids: [0; 4],
fid: 0,
num_vertices: 0,
}
}
}
impl From<CuboidFeatureFace> for PolyhedronFace {
fn from(face: CuboidFeatureFace) -> Self {
Self {
vertices: face.vertices,
vids: face.vids,
eids: face.eids,
fid: face.fid,
num_vertices: 4,
}
}
}
impl From<Triangle> for PolyhedronFace {
fn from(tri: Triangle) -> Self {
Self {
vertices: [tri.a, tri.b, tri.c, tri.c],
vids: [0, 2, 4, 4],
eids: [1, 3, 5, 5],
fid: 0,
num_vertices: 3,
}
}
}
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],
eids: [1, 1, 1, 1],
fid: 0,
num_vertices: 2,
}
}
}
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 p in &mut self.vertices[0..self.num_vertices] {
*p = iso * *p;
}
}
pub fn contacts(
prediction_distance: f32,
face1: &PolyhedronFace,
sep_axis1: &Vector<f32>,
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
// 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_face1 = [
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]),
),
Point2::new(
face1.vertices[2].coords.dot(&basis[0]),
face1.vertices[2].coords.dot(&basis[1]),
),
Point2::new(
face1.vertices[3].coords.dot(&basis[0]),
face1.vertices[3].coords.dot(&basis[1]),
),
];
let projected_face2 = [
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]),
),
Point2::new(
face2.vertices[2].coords.dot(&basis[0]),
face2.vertices[2].coords.dot(&basis[1]),
),
Point2::new(
face2.vertices[3].coords.dot(&basis[0]),
face2.vertices[3].coords.dot(&basis[1]),
),
];
// Also find all the vertices located inside of the other projected face.
if face2.num_vertices > 2 {
let normal2 = (face2.vertices[2] - face2.vertices[1])
.cross(&(face2.vertices[0] - face2.vertices[1]));
let denom = normal2.dot(&sep_axis1);
if !relative_eq!(denom, 0.0) {
let last_index2 = face2.num_vertices as usize - 1;
'point_loop1: for i in 0..face1.num_vertices as usize {
let p1 = projected_face1[i];
let sign = (projected_face2[0] - projected_face2[last_index2])
.perp(&(p1 - projected_face2[last_index2]));
for j in 0..last_index2 {
let new_sign = (projected_face2[j + 1] - projected_face2[j])
.perp(&(p1 - projected_face2[j]));
if new_sign * sign < 0.0 {
// The point lies outside.
continue 'point_loop1;
}
}
// All the perp had the same sign: the point is inside of the other shapes projection.
// Output the contact.
let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom;
let local_p1 = face1.vertices[i];
let local_p2 = face1.vertices[i] + dist * 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.vids[i],
fid2: face2.fid,
dist,
});
}
}
}
}
if face1.num_vertices > 2 {
let normal1 = (face1.vertices[2] - face1.vertices[1])
.cross(&(face1.vertices[0] - face1.vertices[1]));
let denom = -normal1.dot(&sep_axis1);
if !relative_eq!(denom, 0.0) {
let last_index1 = face1.num_vertices as usize - 1;
'point_loop2: for i in 0..face2.num_vertices as usize {
let p2 = projected_face2[i];
let sign = (projected_face1[0] - projected_face1[last_index1])
.perp(&(p2 - projected_face1[last_index1]));
for j in 0..last_index1 {
let new_sign = (projected_face1[j + 1] - projected_face1[j])
.perp(&(p2 - projected_face1[j]));
if new_sign * sign < 0.0 {
// The point lies outside.
continue 'point_loop2;
}
}
// All the perp had the same sign: the point is inside of the other shapes projection.
// Output the contact.
let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom;
let local_p2 = face2.vertices[i];
let local_p1 = face2.vertices[i] - dist * sep_axis1;
if true {
// 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.fid,
fid2: face2.vids[i],
dist,
});
}
}
}
}
// Now we have to compute the intersection between all pairs of
// edges from the face 1 and from the face2.
for j in 0..face2.num_vertices {
let projected_edge2 = [
projected_face2[j],
projected_face2[(j + 1) % face2.num_vertices],
];
for i in 0..face1.num_vertices {
let projected_edge1 = [
projected_face1[i],
projected_face1[(i + 1) % face1.num_vertices],
];
if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) {
if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 {
// Found a contact between the two edges.
let edge1 = (
face1.vertices[i],
face1.vertices[(i + 1) % face1.num_vertices],
);
let edge2 = (
face2.vertices[j],
face2.vertices[(j + 1) % face2.num_vertices],
);
let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0;
let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.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[i],
fid2: face2.eids[j],
dist,
});
}
}
}
}
}
}
}
/// 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)> {
// Inspired by Real-time collision detection by Christer Ericson.
let dir1 = edge1[1] - edge1[0];
let dir2 = edge2[1] - edge2[0];
let r = edge1[0] - edge2[0];
let a = dir1.norm_squared();
let e = dir2.norm_squared();
let f = dir2.dot(&r);
let eps = f32::default_epsilon();
if a <= eps && e <= eps {
Some((0.0, 0.0))
} else if a <= eps {
Some((0.0, f / e))
} else {
let c = dir1.dot(&r);
if e <= eps {
Some((-c / a, 0.0))
} else {
let b = dir1.dot(&dir2);
let ae = a * e;
let bb = b * b;
let denom = ae - bb;
// Use absolute and ulps error to test collinearity.
let parallel = denom <= eps || approx::ulps_eq!(ae, bb);
let s = if !parallel {
(b * f - c * e) / denom
} else {
0.0
};
if parallel {
None
} else {
Some((s, (b * s + f) / e))
}
}
}
}

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

@@ -1,7 +1,7 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{Ball, Proximity};
use crate::math::Isometry;
use ncollide::query::PointQuery;
use buckler::query::PointQuery;
pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
if let Some(ball1) = ctxt.shape1.as_ball() {
@@ -13,7 +13,7 @@ pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContex
}
}
fn do_detect_proximity<P: ?Sized + PointQuery<f32>>(
fn do_detect_proximity<P: ?Sized + PointQuery>(
point_query1: &P,
ball2: &Ball,
ctxt: &PrimitiveProximityDetectionContext,
@@ -22,11 +22,8 @@ fn do_detect_proximity<P: ?Sized + PointQuery<f32>>(
.position1
.inverse_transform_point(&ctxt.position2.translation.vector.into());
// TODO: add a `project_local_point` to the PointQuery trait to avoid
// the identity isometry.
let proj =
point_query1.project_point(&Isometry::identity(), &local_p2_1, cfg!(feature = "dim3"));
let dpos = local_p2_1 - proj.point;
let proj = point_query1.project_local_point(&local_p2_1, cfg!(feature = "dim3"));
let dpos = local_p2_1 - proj.local_point;
let dist = dpos.norm();
if proj.is_inside {

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

@@ -1,7 +1,8 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{sat, Proximity};
use crate::geometry::Proximity;
use crate::math::Isometry;
use ncollide::shape::Cuboid;
use buckler::query::sat;
use buckler::shape::Cuboid;
pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity {
if let (Some(cube1), Some(cube2)) = (ctxt.shape1.as_cuboid(), ctxt.shape2.as_cuboid()) {
@@ -19,9 +20,9 @@ pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionCont
pub fn detect_proximity<'a>(
prediction_distance: f32,
cube1: &'a Cuboid<f32>,
cube1: &'a Cuboid,
pos1: &'a Isometry<f32>,
cube2: &'a Cuboid<f32>,
cube2: &'a Cuboid,
pos2: &'a Isometry<f32>,
) -> Proximity {
let pos12 = pos1.inverse() * pos2;
@@ -32,14 +33,12 @@ pub fn detect_proximity<'a>(
* Point-Face
*
*/
let sep1 =
sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21).0;
let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12).0;
if sep1 > prediction_distance {
return Proximity::Disjoint;
}
let sep2 =
sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12).0;
let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21).0;
if sep2 > prediction_distance {
return Proximity::Disjoint;
}
@@ -52,7 +51,7 @@ pub fn detect_proximity<'a>(
#[cfg(feature = "dim2")]
let sep3 = -f32::MAX; // This case does not exist in 2D.
#[cfg(feature = "dim3")]
let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21).0;
let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12).0;
if sep3 > prediction_distance {
return Proximity::Disjoint;
}

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

@@ -1,6 +1,7 @@
use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext;
use crate::geometry::{sat, Cuboid, Proximity, Triangle};
use crate::geometry::{Cuboid, Proximity, Triangle};
use crate::math::Isometry;
use buckler::query::sat;
pub fn detect_proximity_cuboid_triangle(
ctxt: &mut PrimitiveProximityDetectionContext,
@@ -44,7 +45,7 @@ pub fn detect_proximity<'a>(
*
*/
let sep1 =
sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12).0;
sat::cuboid_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12).0;
if sep1 > prediction_distance {
return Proximity::Disjoint;
}
@@ -62,8 +63,7 @@ pub fn detect_proximity<'a>(
#[cfg(feature = "dim2")]
let sep3 = -f32::MAX; // This case does not exist in 2D.
#[cfg(feature = "dim3")]
let sep3 =
sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21).0;
let sep3 = sat::cuboid_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12).0;
if sep3 > prediction_distance {
return Proximity::Disjoint;
}

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

@@ -1,12 +1,12 @@
use crate::buckler::bounding_volume::{BoundingVolume, AABB};
use crate::geometry::proximity_detector::{
PrimitiveProximityDetectionContext, ProximityDetectionContext,
};
use crate::geometry::{Collider, Proximity, ShapeType, Trimesh};
use crate::ncollide::bounding_volume::{BoundingVolume, AABB};
pub struct TrimeshShapeProximityDetectorWorkspace {
interferences: Vec<usize>,
local_aabb2: AABB<f32>,
local_aabb2: AABB,
old_interferences: Vec<usize>,
}

49
src/geometry/round_cylinder.rs
View File

@@ -1,10 +1,10 @@
use crate::geometry::Cylinder;
use crate::math::{Isometry, Point, Vector};
use na::Unit;
use ncollide::query::{
algorithms::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection,
use buckler::query::{
gjk::VoronoiSimplex, PointProjection, PointQuery, Ray, RayCast, RayIntersection,
};
use ncollide::shape::{FeatureId, SupportMap};
use buckler::shape::{FeatureId, SupportMap};
use na::Unit;
/// A rounded cylinder.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -28,7 +28,7 @@ impl RoundCylinder {
}
}
impl SupportMap<f32> for RoundCylinder {
impl SupportMap for RoundCylinder {
fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
self.local_support_point_toward(&Unit::new_normalize(*dir))
}
@@ -52,43 +52,29 @@ impl SupportMap<f32> for RoundCylinder {
}
}
impl RayCast<f32> for RoundCylinder {
fn toi_and_normal_with_ray(
impl RayCast for RoundCylinder {
fn cast_local_ray_and_get_normal(
&self,
m: &Isometry<f32>,
ray: &Ray<f32>,
ray: &Ray,
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(),
) -> Option<RayIntersection> {
buckler::query::details::local_ray_intersection_with_support_map_with_params(
self,
&mut VoronoiSimplex::new(),
&ls_ray,
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 {
impl PointQuery 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,
fn project_local_point(&self, point: &Point<f32>, solid: bool) -> PointProjection {
buckler::query::details::local_point_projection_on_support_map(
self,
&mut VoronoiSimplex::new(),
point,
@@ -97,11 +83,10 @@ impl PointQuery<f32> for RoundCylinder {
}
#[inline]
fn project_point_with_feature(
fn project_local_point_and_get_feature(
&self,
m: &Isometry<f32>,
point: &Point<f32>,
) -> (PointProjection<f32>, FeatureId) {
(self.project_point(m, point, false), FeatureId::Unknown)
) -> (PointProjection, FeatureId) {
(self.project_local_point(point, false), FeatureId::Unknown)
}
}

344
src/geometry/sat.rs
View File

@@ -1,20 +1,7 @@
use crate::geometry::{cuboid, Cuboid, Polygon, Segment, Triangle};
use crate::geometry::{Cuboid, Polygon, Segment, Triangle};
use crate::math::{Isometry, Point, Vector, DIM};
use crate::utils::WSign;
use na::Unit;
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(
@@ -38,332 +25,3 @@ pub fn polygon_polygon_compute_separation_features(
(max_separation, separation_features.0, separation_features.1)
}
#[cfg(feature = "dim3")]
pub fn cuboid_cuboid_compute_separation_wrt_local_line(
cube1: &Cuboid,
cube2: &Cuboid,
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
axis1: &Vector<f32>,
) -> (f32, Vector<f32>) {
let signum = pos12.translation.vector.dot(axis1).copy_sign_to(1.0);
let axis1 = axis1 * signum;
let local_pt1 = cuboid::local_support_point(cube1, axis1);
let local_pt2 = cuboid::local_support_point(cube2, pos21 * -axis1);
let pt2 = pos12 * local_pt2;
let separation = (pt2 - local_pt1).dot(&axis1);
(separation, axis1)
}
#[cfg(feature = "dim3")]
pub fn cuboid_cuboid_find_local_separating_edge_twoway(
cube1: &Cuboid,
cube2: &Cuboid,
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
) -> (f32, Vector<f32>) {
use approx::AbsDiffEq;
let mut best_separation = -std::f32::MAX;
let mut best_dir = Vector::zeros();
let x2 = pos12 * Vector::x();
let y2 = pos12 * Vector::y();
let z2 = pos12 * Vector::z();
// 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),
Vector::new(-x2.y, x2.x, 0.0),
// Vector::{x, y ,z}().cross(y2)
Vector::new(0.0, -y2.z, y2.y),
Vector::new(y2.z, 0.0, -y2.x),
Vector::new(-y2.y, y2.x, 0.0),
// Vector::{x, y ,z}().cross(y2)
Vector::new(0.0, -z2.z, z2.y),
Vector::new(z2.z, 0.0, -z2.x),
Vector::new(-z2.y, z2.x, 0.0),
];
for axis1 in &axes {
let norm1 = axis1.norm();
if norm1 > f32::default_epsilon() {
let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line(
cube1,
cube2,
pos12,
pos21,
&(axis1 / norm1),
);
if separation > best_separation {
best_separation = separation;
best_dir = axis1;
}
}
}
(best_separation, best_dir)
}
pub fn cuboid_cuboid_find_local_separating_normal_oneway(
cube1: &Cuboid,
cube2: &Cuboid,
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
) -> (f32, Vector<f32>) {
let mut best_separation = -std::f32::MAX;
let mut best_dir = Vector::zeros();
for i in 0..DIM {
let sign = pos12.translation.vector[i].copy_sign_to(1.0);
let axis1 = Vector::ith(i, sign);
let local_pt2 = cuboid::local_support_point(cube2, pos21 * -axis1);
let pt2 = pos12 * local_pt2;
let separation = pt2[i] * sign - cube1.half_extents[i];
if separation > best_separation {
best_separation = separation;
best_dir = axis1;
}
}
(best_separation, best_dir)
}
/*
*
*
* Triangles.
*
*
*/
#[cfg(feature = "dim3")]
pub fn cube_support_map_compute_separation_wrt_local_line<S: SupportMap<f32>>(
cube1: &Cuboid,
shape2: &S,
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
axis1: &Unit<Vector<f32>>,
) -> (f32, Unit<Vector<f32>>) {
let signum = pos12.translation.vector.dot(axis1).copy_sign_to(1.0);
let axis1 = Unit::new_unchecked(**axis1 * signum);
let local_pt1 = cuboid::local_support_point(cube1, *axis1);
let local_pt2 = shape2.local_support_point_toward(&(pos21 * -axis1));
let pt2 = pos12 * local_pt2;
let separation = (pt2 - local_pt1).dot(&axis1);
(separation, axis1)
}
#[cfg(feature = "dim3")]
pub fn cube_support_map_find_local_separating_edge_twoway(
cube1: &Cuboid,
shape2: &impl SupportMap<f32>,
axes: &[Vector<f32>],
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
) -> (f32, Vector<f32>) {
use approx::AbsDiffEq;
let mut best_separation = -std::f32::MAX;
let mut best_dir = Vector::zeros();
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,
);
if separation > best_separation {
best_separation = separation;
best_dir = *axis1;
}
}
}
(best_separation, best_dir)
}
#[cfg(feature = "dim3")]
pub fn cube_triangle_find_local_separating_edge_twoway(
cube1: &Cuboid,
triangle2: &Triangle,
pos12: &Isometry<f32>,
pos21: &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);
// 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),
Vector::new(-x2.y, x2.x, 0.0),
// Vector::{x, y ,z}().cross(y2)
Vector::new(0.0, -y2.z, y2.y),
Vector::new(y2.z, 0.0, -y2.x),
Vector::new(-y2.y, y2.x, 0.0),
// Vector::{x, y ,z}().cross(y2)
Vector::new(0.0, -z2.z, z2.y),
Vector::new(z2.z, 0.0, -z2.x),
Vector::new(-z2.y, z2.x, 0.0),
];
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,
pos12: &Isometry<f32>,
pos21: &Isometry<f32>,
) -> (f32, Vector<f32>) {
let x2 = pos12 * (segment2.b - segment2.a);
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, &axes, pos12, pos21)
}
pub fn cube_support_map_find_local_separating_normal_oneway<S: SupportMap<f32>>(
cube1: &Cuboid,
shape2: &S,
pos12: &Isometry<f32>,
) -> (f32, Vector<f32>) {
let mut best_separation = -std::f32::MAX;
let mut best_dir = Vector::zeros();
for i in 0..DIM {
for sign in &[-1.0, 1.0] {
let axis1 = Vector::ith(i, *sign);
let pt2 = shape2.support_point_toward(&pos12, &Unit::new_unchecked(-axis1));
let separation = pt2[i] * *sign - cube1.half_extents[i];
if separation > best_separation {
best_separation = separation;
best_dir = axis1;
}
}
}
(best_separation, best_dir)
}
// NOTE: this only works with cuboid on the rhs because it has its symmetry origin at zero
// (therefore we can check only one normal direction).
pub fn point_cuboid_find_local_separating_normal_oneway(
point1: Point<f32>,
normal1: Option<Unit<Vector<f32>>>,
shape2: &Cuboid,
pos12: &Isometry<f32>,
) -> (f32, Vector<f32>) {
let mut best_separation = -std::f32::MAX;
let mut best_dir = Vector::zeros();
if let Some(normal1) = normal1 {
let axis1 = if (pos12.translation.vector - point1.coords).dot(&normal1) >= 0.0 {
normal1
} else {
-normal1
};
let pt2 = shape2.support_point_toward(&pos12, &-axis1);
let separation = (pt2 - point1).dot(&axis1);
if separation > best_separation {
best_separation = separation;
best_dir = *axis1;
}
}
(best_separation, best_dir)
}
pub fn triangle_cuboid_find_local_separating_normal_oneway(
triangle1: &Triangle,
shape2: &Cuboid,
pos12: &Isometry<f32>,
) -> (f32, Vector<f32>) {
point_cuboid_find_local_separating_normal_oneway(triangle1.a, triangle1.normal(), shape2, pos12)
}
#[cfg(feature = "dim2")]
pub fn segment_cuboid_find_local_separating_normal_oneway(
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)
}

49
src/geometry/shape.rs
View File

@@ -1,17 +1,18 @@
use crate::dynamics::MassProperties;
use crate::geometry::{Ball, Capsule, Cuboid, HeightField, Segment, Triangle, Trimesh};
use crate::math::Isometry;
use buckler::bounding_volume::AABB;
use buckler::query::{PointQuery, RayCast};
use downcast_rs::{impl_downcast, DowncastSync};
#[cfg(feature = "serde-serialize")]
use erased_serde::Serialize;
use 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,
crate::geometry::{Cone, Cylinder, RoundCylinder},
buckler::bounding_volume::BoundingVolume,
buckler::shape::PolygonalFeatureMap,
};
#[derive(Copy, Clone, Debug, FromPrimitive)]
@@ -57,7 +58,7 @@ pub enum ShapeType {
}
/// Trait implemented by shapes usable by Rapier.
pub trait Shape: RayCast<f32> + PointQuery<f32> + DowncastSync {
pub trait Shape: RayCast + PointQuery + DowncastSync {
/// Convert this shape as a serializable entity.
#[cfg(feature = "serde-serialize")]
fn as_serialize(&self) -> Option<&dyn Serialize> {
@@ -67,7 +68,7 @@ pub trait Shape: RayCast<f32> + PointQuery<f32> + DowncastSync {
// TODO: add a compute_local_aabb method?
/// Computes the AABB of this shape.
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32>;
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB;
/// Compute the mass-properties of this shape given its uniform density.
fn mass_properties(&self, density: f32) -> MassProperties;
@@ -144,8 +145,8 @@ impl Shape for Ball {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
@@ -163,7 +164,7 @@ impl Shape for Ball {
// Some(self as &dyn Serialize)
// }
//
// fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
// fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
// self.aabb(position)
// }
//
@@ -182,8 +183,8 @@ impl Shape for Cuboid {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
@@ -206,7 +207,7 @@ impl Shape for Capsule {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
@@ -230,8 +231,8 @@ impl Shape for Triangle {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
@@ -254,8 +255,8 @@ impl Shape for Segment {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
@@ -278,7 +279,7 @@ impl Shape for Trimesh {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
@@ -297,8 +298,8 @@ impl Shape for HeightField {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, _density: f32) -> MassProperties {
@@ -317,8 +318,8 @@ impl Shape for Cylinder {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
@@ -342,8 +343,8 @@ impl Shape for Cone {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
self.bounding_volume(position)
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.aabb(position)
}
fn mass_properties(&self, density: f32) -> MassProperties {
@@ -367,7 +368,7 @@ impl Shape for RoundCylinder {
Some(self as &dyn Serialize)
}
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
fn compute_aabb(&self, position: &Isometry<f32>) -> AABB {
self.cylinder
.compute_aabb(position)
.loosened(self.border_radius)

4
src/geometry/triangle.rs
View File

@@ -1,7 +1,5 @@
#[cfg(feature = "dim2")]
use crate::geometry::{CuboidFeatureFace, Triangle};
#[cfg(feature = "dim2")]
use crate::math::Vector;
use crate::{buckler::shape::CuboidFeatureFace, geometry::Triangle, math::Vector};
#[cfg(feature = "dim2")]
pub fn support_face(_triangle: &Triangle, _local_dir: Vector<f32>) -> CuboidFeatureFace {

63
src/geometry/trimesh.rs
View File

@@ -1,16 +1,18 @@
use crate::geometry::{PointProjection, Ray, RayIntersection, Triangle, WQuadtree};
use crate::geometry::{
Cuboid, HeightField, PointProjection, Ray, RayIntersection, Triangle, WQuadtree,
};
use crate::math::{Isometry, Point};
use buckler::bounding_volume::AABB;
use buckler::query::{PointQuery, RayCast};
use buckler::shape::FeatureId;
use na::Point3;
use ncollide::bounding_volume::{HasBoundingVolume, AABB};
use ncollide::query::{PointQuery, RayCast};
use ncollide::shape::FeatureId;
#[derive(Clone)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
/// A triangle mesh.
pub struct Trimesh {
wquadtree: WQuadtree<usize>,
aabb: AABB<f32>,
aabb: AABB,
vertices: Vec<Point<f32>>,
indices: Vec<Point3<u32>>,
}
@@ -34,7 +36,7 @@ impl Trimesh {
vertices[idx[1] as usize],
vertices[idx[2] as usize],
)
.local_bounding_volume();
.local_aabb();
(i, aabb)
});
@@ -52,7 +54,7 @@ impl Trimesh {
}
/// Compute the axis-aligned bounding box of this triangle mesh.
pub fn aabb(&self, pos: &Isometry<f32>) -> AABB<f32> {
pub fn aabb(&self, pos: &Isometry<f32>) -> AABB {
self.aabb.transform_by(pos)
}
@@ -106,15 +108,14 @@ impl Trimesh {
}
}
impl PointQuery<f32> for Trimesh {
fn project_point(&self, _m: &Isometry<f32>, _pt: &Point<f32>, _solid: bool) -> PointProjection {
impl PointQuery for Trimesh {
fn project_local_point(&self, _pt: &Point<f32>, _solid: bool) -> PointProjection {
// TODO
unimplemented!()
}
fn project_point_with_feature(
fn project_local_point_and_get_feature(
&self,
_m: &Isometry<f32>,
_pt: &Point<f32>,
) -> (PointProjection, FeatureId) {
// TODO
@@ -123,10 +124,9 @@ impl PointQuery<f32> for Trimesh {
}
#[cfg(feature = "dim2")]
impl RayCast<f32> for Trimesh {
fn toi_and_normal_with_ray(
impl RayCast for Trimesh {
fn cast_local_ray_and_get_normal(
&self,
_m: &Isometry<f32>,
_ray: &Ray,
_max_toi: f32,
_solid: bool,
@@ -142,24 +142,21 @@ impl RayCast<f32> for Trimesh {
}
#[cfg(feature = "dim3")]
impl RayCast<f32> for Trimesh {
fn toi_and_normal_with_ray(
impl RayCast for Trimesh {
fn cast_local_ray_and_get_normal(
&self,
m: &Isometry<f32>,
ray: &Ray,
max_toi: f32,
solid: bool,
) -> Option<RayIntersection> {
// FIXME: do a best-first search.
let mut intersections = Vec::new();
let ls_ray = ray.inverse_transform_by(m);
self.wquadtree
.cast_ray(&ls_ray, max_toi, &mut intersections);
self.wquadtree.cast_ray(&ray, max_toi, &mut intersections);
let mut best: Option<RayIntersection> = None;
for inter in intersections {
let tri = self.triangle(inter);
if let Some(inter) = tri.toi_and_normal_with_ray(m, ray, max_toi, solid) {
if let Some(inter) = tri.cast_local_ray_and_get_normal(ray, max_toi, solid) {
if let Some(curr) = &mut best {
if curr.toi > inter.toi {
*curr = inter;
@@ -173,16 +170,14 @@ impl RayCast<f32> for Trimesh {
best
}
fn intersects_ray(&self, m: &Isometry<f32>, ray: &Ray, max_toi: f32) -> bool {
fn intersects_local_ray(&self, ray: &Ray, max_toi: f32) -> bool {
// FIXME: do a best-first search.
let mut intersections = Vec::new();
let ls_ray = ray.inverse_transform_by(m);
self.wquadtree
.cast_ray(&ls_ray, max_toi, &mut intersections);
self.wquadtree.cast_ray(&ray, max_toi, &mut intersections);
for inter in intersections {
let tri = self.triangle(inter);
if tri.intersects_ray(m, ray, max_toi) {
if tri.intersects_local_ray(ray, max_toi) {
return true;
}
}
@@ -190,3 +185,19 @@ impl RayCast<f32> for Trimesh {
false
}
}
#[cfg(feature = "dim3")]
impl From<HeightField> for Trimesh {
fn from(heightfield: HeightField) -> Self {
let (vtx, idx) = heightfield.to_trimesh();
Trimesh::new(vtx, idx)
}
}
#[cfg(feature = "dim3")]
impl From<Cuboid> for Trimesh {
fn from(cuboid: Cuboid) -> Self {
let (vtx, idx) = cuboid.to_trimesh();
Trimesh::new(vtx, idx)
}
}

12
src/geometry/waabb.rs
View File

@@ -1,7 +1,7 @@
use crate::geometry::Ray;
use crate::math::{Point, Vector, DIM, SIMD_WIDTH};
use crate::utils;
use ncollide::bounding_volume::AABB;
use buckler::bounding_volume::AABB;
use num::{One, Zero};
use {
crate::math::{SimdBool, SimdFloat},
@@ -97,7 +97,7 @@ impl WAABB {
Self::splat(AABB::new_invalid())
}
pub fn splat(aabb: AABB<f32>) -> Self {
pub fn splat(aabb: AABB) -> Self {
Self {
mins: Point::splat(aabb.mins),
maxs: Point::splat(aabb.maxs),
@@ -119,7 +119,7 @@ impl WAABB {
self.maxs += dilation;
}
pub fn replace(&mut self, i: usize, aabb: AABB<f32>) {
pub fn replace(&mut self, i: usize, aabb: AABB) {
self.mins.replace(i, aabb.mins);
self.maxs.replace(i, aabb.maxs);
}
@@ -196,7 +196,7 @@ impl WAABB {
& other.mins.z.simd_le(self.maxs.z)
}
pub fn to_merged_aabb(&self) -> AABB<f32> {
pub fn to_merged_aabb(&self) -> AABB {
AABB::new(
self.mins.coords.map(|e| e.simd_horizontal_min()).into(),
self.maxs.coords.map(|e| e.simd_horizontal_max()).into(),
@@ -204,8 +204,8 @@ impl WAABB {
}
}
impl From<[AABB<f32>; SIMD_WIDTH]> for WAABB {
fn from(aabbs: [AABB<f32>; SIMD_WIDTH]) -> Self {
impl From<[AABB; SIMD_WIDTH]> for WAABB {
fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self {
let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH];
let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH];

2
src/geometry/wquadtree.rs
View File

@@ -4,7 +4,7 @@ use crate::math::Point;
#[cfg(feature = "dim3")]
use crate::math::Vector;
use crate::simd::{SimdFloat, SIMD_WIDTH};
use ncollide::bounding_volume::BoundingVolume;
use buckler::bounding_volume::BoundingVolume;
use simba::simd::{SimdBool, SimdValue};
use std::collections::VecDeque;
use std::ops::Range;

11
src/lib.rs
View File

@@ -8,14 +8,15 @@
//! - The ability to run a perfectly deterministic simulation on different machine, as long as they
//! are compliant with the IEEE 754-2008 floating point standard.
#![deny(missing_docs)]
// FIXME: deny that
#![allow(missing_docs)]
#[cfg(feature = "dim2")]
pub extern crate buckler2d as buckler;
#[cfg(feature = "dim3")]
pub extern crate buckler3d as buckler;
pub extern crate crossbeam;
pub extern crate nalgebra as na;
#[cfg(feature = "dim2")]
pub extern crate ncollide2d as ncollide;
#[cfg(feature = "dim3")]
pub extern crate ncollide3d as ncollide;
#[cfg(feature = "serde")]
#[macro_use]
extern crate serde;

4
src/pipeline/query_pipeline.rs
View File

@@ -74,7 +74,7 @@ impl QueryPipeline {
for handle in inter {
if let Some(collider) = colliders.get(handle) {
if collider.collision_groups.test(groups) {
if let Some(inter) = collider.shape().toi_and_normal_with_ray(
if let Some(inter) = collider.shape().cast_ray_and_get_normal(
collider.position(),
ray,
max_toi,
@@ -118,7 +118,7 @@ impl QueryPipeline {
let collider = &colliders[handle];
if collider.collision_groups.test(groups) {
if let Some(inter) = collider.shape().toi_and_normal_with_ray(
if let Some(inter) = collider.shape().cast_ray_and_get_normal(
collider.position(),
ray,
max_toi,

2
src_testbed/engine.rs
View File

@@ -501,7 +501,7 @@ impl GraphicsManager {
object: DefaultColliderHandle,
colliders: &DefaultColliderSet<f32>,
delta: Isometry<f32>,
shape: &Cuboid<f32>,
shape: &Cuboid,
color: Point3<f32>,
out: &mut Vec<Node>,
) {

4
src_testbed/lib.rs
View File

@@ -1,5 +1,9 @@
#[macro_use]
extern crate kiss3d;
#[cfg(feature = "dim2")]
extern crate buckler2d as buckler;
#[cfg(feature = "dim3")]
extern crate buckler3d as buckler;
extern crate nalgebra as na;
#[cfg(feature = "dim2")]
extern crate ncollide2d as ncollide;

7
src_testbed/nphysics_backend.rs
View File

@@ -1,4 +1,4 @@
use ncollide::shape::{Ball, Capsule, Cuboid, ShapeHandle};
use ncollide::shape::{Ball, Capsule, Cuboid, HeightField, ShapeHandle};
use nphysics::force_generator::DefaultForceGeneratorSet;
use nphysics::joint::{
DefaultJointConstraintSet, FixedConstraint, PrismaticConstraint, RevoluteConstraint,
@@ -187,7 +187,10 @@ fn nphysics_collider_from_rapier_collider(
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())
let heights = heightfield.heights();
let scale = heightfield.scale();
let heightfield = HeightField::new(heights.clone(), *scale);
ShapeHandle::new(heightfield)
} else {
#[cfg(feature = "dim3")]
if let Some(trimesh) = shape.as_trimesh() {

17
src_testbed/objects/heightfield.rs
View File

@@ -1,15 +1,16 @@
#[cfg(feature = "dim3")]
use crate::objects::node::{self, GraphicsNode};
use buckler::shape;
use kiss3d::resource::Mesh;
use kiss3d::window::Window;
use na::{self, Point3};
use ncollide::shape;
#[cfg(feature = "dim3")]
use ncollide::transformation::ToTriMesh;
use rapier::geometry::{ColliderHandle, ColliderSet};
#[cfg(feature = "dim2")]
use rapier::math::Point;
#[cfg(feature = "dim3")]
use rapier::math::Vector;
use std::cell::RefCell;
use std::rc::Rc;
pub struct HeightField {
color: Point3<f32>,
@@ -25,7 +26,7 @@ impl HeightField {
#[cfg(feature = "dim2")]
pub fn new(
collider: ColliderHandle,
heightfield: &shape::HeightField<f32>,
heightfield: &shape::HeightField,
color: Point3<f32>,
_: &mut Window,
) -> HeightField {
@@ -47,16 +48,18 @@ impl HeightField {
#[cfg(feature = "dim3")]
pub fn new(
collider: ColliderHandle,
heightfield: &shape::HeightField<f32>,
heightfield: &shape::HeightField,
color: Point3<f32>,
window: &mut Window,
) -> HeightField {
let mesh = heightfield.to_trimesh(());
let (vertices, indices) = heightfield.to_trimesh();
let indices = indices.into_iter().map(|i| na::convert(i)).collect();
let mesh = Mesh::new(vertices, indices, None, None, false);
let mut res = HeightField {
color,
base_color: color,
gfx: window.add_trimesh(mesh, Vector::repeat(1.0)),
gfx: window.add_mesh(Rc::new(RefCell::new(mesh)), Vector::repeat(1.0)),
collider: collider,
};

2
src_testbed/objects/polyline.rs
View File

@@ -1,6 +1,6 @@
use buckler2d::shape;
use kiss3d::window::Window;
use na::{Isometry2, Point2, Point3};
use ncollide2d::shape;
use nphysics2d::object::{ColliderAnchor, DefaultColliderHandle, DefaultColliderSet};
pub struct Polyline {

4
src_testbed/testbed.rs
View File

@@ -1521,8 +1521,8 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet)
} else {
Point3::new(1.0, 0.0, 0.0)
};
let pos1 = colliders[manifold.pair.collider1].position();
let pos2 = colliders[manifold.pair.collider2].position();
let pos1 = colliders[manifold.data.pair.collider1].position();
let pos2 = colliders[manifold.data.pair.collider2].position();
let start = pos1 * pt.local_p1;
let end = pos2 * pt.local_p2;
let n = pos1 * manifold.local_n1;