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

Merge pull request #6 from dimforge/collider_removal

Browse Source 
Add collider removal + fix rigid-bodies with multiple colliders
This commit is contained in:
Sébastien Crozet
2020-09-01 18:21:11 +02:00
committed by GitHub
parent cc05bad041 763b909242
commit fef3a367d1
30 changed files with 634 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
.circleci/config.yml Normal file
View File

@@ -0,0 +1,82 @@
version: 2.1
executors:
rust-executor:
docker:
- image: rust:latest
jobs:
check-fmt:
executor: rust-executor
steps:
- checkout
- run:
name: install rustfmt
command: rustup component add rustfmt
- run:
name: check formatting
command: cargo fmt -- --check
build-native:
executor: rust-executor
steps:
- checkout
- run: apt-get update
- run: apt-get install -y cmake libxcb-composite0-dev
- run:
name: build rapier2d
command: cargo build --verbose -p rapier2d;
- run:
name: build rapier3d
command: cargo build --verbose -p rapier3d;
- run:
name: build rapier2d SIMD
command: cd build/rapier2d; cargo build --verbose --features simd-stable;
- run:
name: build rapier3d SIMD
command: cd build/rapier3d; cargo build --verbose --features simd-stable;
- run:
name: build rapier2d SIMD Parallel
command: cd build/rapier2d; cargo build --verbose --features simd-stable --features parallel;
- run:
name: build rapier3d SIMD Parallel
command: cd build/rapier3d; cargo build --verbose --features simd-stable --features parallel;
- run:
name: test
command: cargo test
- run:
name: check rapier_testbed2d
command: cargo check --verbose -p rapier_testbed2d;
- run:
name: check rapier_testbed3d
command: cargo check --verbose -p rapier_testbed3d;
- run:
name: check rapier-examples-2d
command: cargo check -j 1 --verbose -p rapier-examples-2d;
- run:
name: check rapier-examples-3d
command: cargo check -j 1 --verbose -p rapier-examples-3d;
build-wasm:
executor: rust-executor
steps:
- checkout
- run:
name: install cargo-web
command: cargo install -f cargo-web;
- run:
name: build rapier2d
command: cd build/rapier2d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown;
- run:
name: build rapier3d
command: cd build/rapier3d && cargo web build --verbose --features wasm-bindgen --target wasm32-unknown-unknown;
workflows:
version: 2
build:
jobs:
- check-fmt
- build-native:
requires:
- check-fmt
- build-wasm:
requires:
- check-fmt

7
Cargo.toml
View File

@@ -10,3 +10,10 @@ debug = false
codegen-units = 1
#opt-level = 1
#lto = true
#[profile.dev.package.rapier3d]
#opt-level = 3
#
#[profile.dev.package.kiss3d]
#opt-level = 3

2
examples2d/Cargo.toml
View File

@@ -1,5 +1,5 @@
[package]
name = "nphysics-examples-2d"
name = "rapier-examples-2d"
version = "0.1.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
edition = "2018"

4
examples3d/Cargo.toml
View File

@@ -1,5 +1,5 @@
[package]
name = "nphysics-examples-3d"
name = "rapier-examples-3d"
version = "0.1.0"
authors = [ "Sébastien Crozet <developer@crozet.re>" ]
edition = "2018"
@@ -24,4 +24,4 @@ path = "../build/rapier3d"
[[bin]]
name = "all_examples3"
path = "./all_examples3.rs"
path = "./all_examples3.rs"

2
examples3d/all_examples3.rs
View File

@@ -14,6 +14,7 @@ mod add_remove3;
mod balls3;
mod boxes3;
mod capsules3;
mod compound3;
mod debug_boxes3;
mod debug_triangle3;
mod domino3;
@@ -71,6 +72,7 @@ pub fn main() {
("Balls", balls3::init_world),
("Boxes", boxes3::init_world),
("Capsules", capsules3::init_world),
("Compound", compound3::init_world),
("Domino", domino3::init_world),
("Heightfield", heightfield3::init_world),
("Joints", joints3::init_world),

76
examples3d/compound3.rs Normal file
View File

@@ -0,0 +1,76 @@
use na::Point3;
use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let joints = JointSet::new();
/*
* Ground
*/
let ground_size = 200.1;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(0.0, -ground_height, 0.0)
.build();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
colliders.insert(collider, handle, &mut bodies);
/*
* Create the cubes
*/
let num = 8;
let rad = 0.2;
let shift = rad * 4.0 + rad;
let centerx = shift * (num / 2) as f32;
let centery = shift / 2.0;
let centerz = shift * (num / 2) as f32;
let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5;
for j in 0usize..25 {
for i in 0..num {
for k in 0usize..num {
let x = i as f32 * shift * 5.0 - centerx + offset;
let y = j as f32 * (shift * 5.0) + centery + 3.0;
let z = k as f32 * shift * 2.0 - centerz + offset;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build();
let handle = bodies.insert(rigid_body);
let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build();
let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
.translation(rad * 10.0, rad * 10.0, 0.0)
.build();
let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
.translation(-rad * 10.0, rad * 10.0, 0.0)
.build();
colliders.insert(collider1, handle, &mut bodies);
colliders.insert(collider2, handle, &mut bodies);
colliders.insert(collider3, handle, &mut bodies);
}
}
offset -= 0.05 * rad * (num as f32 - 1.0);
}
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin());
}
fn main() {
let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]);
testbed.run()
}

3
rustfmt.toml Normal file
View File

@@ -0,0 +1,3 @@
#indent_style = "Block"
#where_single_line = true
#brace_style = "PreferSameLine"

190
src/dynamics/mass_properties.rs
View File

@@ -1,7 +1,7 @@
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector};
use crate::utils;
use num::Zero;
use std::ops::{Add, AddAssign};
use std::ops::{Add, AddAssign, Sub, SubAssign};
#[cfg(feature = "dim3")]
use {na::Matrix3, std::ops::MulAssign};
@@ -90,6 +90,18 @@ impl MassProperties {
}
}
#[cfg(feature = "dim3")]
/// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii.
pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> {
let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e);
self.principal_inertia_local_frame.to_rotation_matrix()
* Matrix3::from_diagonal(&inv_principal_inertia)
* self
.principal_inertia_local_frame
.inverse()
.to_rotation_matrix()
}
#[cfg(feature = "dim3")]
/// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
@@ -125,6 +137,19 @@ impl MassProperties {
Matrix3::zeros()
}
}
/// Transform each element of the mass properties.
pub fn transform_by(&self, m: &Isometry<f32>) -> Self {
// NOTE: we don't apply the parallel axis theorem here
// because the center of mass is also transformed.
Self {
local_com: m * self.local_com,
inv_mass: self.inv_mass,
inv_principal_inertia_sqrt: self.inv_principal_inertia_sqrt,
#[cfg(feature = "dim3")]
principal_inertia_local_frame: m.rotation * self.principal_inertia_local_frame,
}
}
}
impl Zero for MassProperties {
@@ -143,6 +168,68 @@ impl Zero for MassProperties {
}
}
impl Sub<MassProperties> for MassProperties {
type Output = Self;
#[cfg(feature = "dim2")]
fn sub(self, other: MassProperties) -> Self {
if self.is_zero() || other.is_zero() {
return self;
}
let m1 = utils::inv(self.inv_mass);
let m2 = utils::inv(other.inv_mass);
let inv_mass = utils::inv(m1 - m2);
let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass;
let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 - i2;
// NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
let inv_principal_inertia_sqrt = utils::inv(inertia.max(0.0).sqrt());
Self {
local_com,
inv_mass,
inv_principal_inertia_sqrt,
}
}
#[cfg(feature = "dim3")]
fn sub(self, other: MassProperties) -> Self {
if self.is_zero() || other.is_zero() {
return self;
}
let m1 = utils::inv(self.inv_mass);
let m2 = utils::inv(other.inv_mass);
let inv_mass = utils::inv(m1 - m2);
let local_com = (self.local_com * m1 - other.local_com.coords * m2) * inv_mass;
let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com);
let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 - i2;
let eigen = inertia.symmetric_eigen();
let principal_inertia_local_frame =
Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one());
let principal_inertia = eigen.eigenvalues;
// NOTE: we drop the negative eigenvalues that may result from subtraction rounding errors.
let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.max(0.0).sqrt()));
Self {
local_com,
inv_mass,
inv_principal_inertia_sqrt,
principal_inertia_local_frame,
}
}
}
impl SubAssign<MassProperties> for MassProperties {
fn sub_assign(&mut self, rhs: MassProperties) {
*self = *self - rhs
}
}
impl Add<MassProperties> for MassProperties {
type Output = Self;
@@ -186,7 +273,8 @@ impl Add<MassProperties> for MassProperties {
let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com);
let inertia = i1 + i2;
let eigen = inertia.symmetric_eigen();
let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors);
let principal_inertia_local_frame =
Rotation::from_matrix_eps(&eigen.eigenvectors, 1.0e-6, 10, na::one());
let principal_inertia = eigen.eigenvalues;
let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt()));
@@ -204,3 +292,101 @@ impl AddAssign<MassProperties> for MassProperties {
*self = *self + rhs
}
}
impl approx::AbsDiffEq for MassProperties {
type Epsilon = f32;
fn default_epsilon() -> Self::Epsilon {
f32::default_epsilon()
}
fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool {
#[cfg(feature = "dim2")]
let inertia_is_ok = self
.inv_principal_inertia_sqrt
.abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon);
#[cfg(feature = "dim3")]
let inertia_is_ok = self
.reconstruct_inverse_inertia_matrix()
.abs_diff_eq(&other.reconstruct_inverse_inertia_matrix(), epsilon);
inertia_is_ok
&& self.local_com.abs_diff_eq(&other.local_com, epsilon)
&& self.inv_mass.abs_diff_eq(&other.inv_mass, epsilon)
&& self
.inv_principal_inertia_sqrt
.abs_diff_eq(&other.inv_principal_inertia_sqrt, epsilon)
}
}
impl approx::RelativeEq for MassProperties {
fn default_max_relative() -> Self::Epsilon {
f32::default_max_relative()
}
fn relative_eq(
&self,
other: &Self,
epsilon: Self::Epsilon,
max_relative: Self::Epsilon,
) -> bool {
#[cfg(feature = "dim2")]
let inertia_is_ok = self.inv_principal_inertia_sqrt.relative_eq(
&other.inv_principal_inertia_sqrt,
epsilon,
max_relative,
);
#[cfg(feature = "dim3")]
let inertia_is_ok = self.reconstruct_inverse_inertia_matrix().relative_eq(
&other.reconstruct_inverse_inertia_matrix(),
epsilon,
max_relative,
);
inertia_is_ok
&& self
.local_com
.relative_eq(&other.local_com, epsilon, max_relative)
&& self
.inv_mass
.relative_eq(&other.inv_mass, epsilon, max_relative)
}
}
#[cfg(test)]
mod test {
use super::MassProperties;
use crate::geometry::ColliderBuilder;
use approx::assert_relative_eq;
use num::Zero;
#[test]
fn mass_properties_add_sub() {
// Check that addition and subtraction of mass properties behave as expected.
let c1 = ColliderBuilder::capsule_x(1.0, 2.0).build();
let c2 = ColliderBuilder::capsule_y(3.0, 4.0).build();
let c3 = ColliderBuilder::ball(5.0).build();
let m1 = c1.mass_properties();
let m2 = c2.mass_properties();
let m3 = c3.mass_properties();
let m1m2m3 = m1 + m2 + m3;
assert_relative_eq!(m1 + m2, m2 + m1, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m1, m2 + m3, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m2, m1 + m3, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m3, m1 + m2, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - (m1 + m2), m3, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - (m1 + m3), m2, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - (m2 + m3), m1, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m1 - m2, m3, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m1 - m3, m2, epsilon = 1.0e-6);
assert_relative_eq!(m1m2m3 - m2 - m3, m1, epsilon = 1.0e-6);
assert_relative_eq!(
m1m2m3 - m1 - m2 - m3,
MassProperties::zero(),
epsilon = 1.0e-6
);
}
}

26
src/dynamics/rigid_body.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::MassProperties;
use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
use crate::geometry::{Collider, ColliderHandle, InteractionGraph, RigidBodyGraphIndex};
use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector};
use crate::utils::{WCross, WDot};
use num::Zero;
@@ -137,6 +137,28 @@ impl RigidBody {
crate::utils::inv(self.mass_properties.inv_mass)
}
/// Adds a collider to this rigid-body.
pub(crate) fn add_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.colliders.push(handle);
self.mass_properties += mass_properties;
self.update_world_mass_properties();
}
/// Removes a collider from this rigid-body.
pub(crate) fn remove_collider_internal(&mut self, handle: ColliderHandle, coll: &Collider) {
if let Some(i) = self.colliders.iter().position(|e| *e == handle) {
self.colliders.swap_remove(i);
let mass_properties = coll
.mass_properties()
.transform_by(coll.position_wrt_parent());
self.mass_properties -= mass_properties;
self.update_world_mass_properties();
}
}
/// Put this rigid body to sleep.
///
/// A sleeping body no longer moves and is no longer simulated by the physics engine unless
@@ -171,7 +193,7 @@ impl RigidBody {
}
fn integrate_velocity(&self, dt: f32) -> Isometry<f32> {
let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses.
let com = &self.position * self.mass_properties.local_com;
let shift = Translation::from(com.coords);
shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
}

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

@@ -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_points[l].local_p1 + shift1;
local_p2[l] = manifold_points[l].local_p2 + shift2;
local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1);
local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2);
}
let constraint = PositionConstraint {

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

@@ -51,6 +51,9 @@ 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 rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -85,8 +88,8 @@ impl WPositionConstraint {
let local_p2 =
Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]);
constraint.local_p1[i] = local_p1 + shift1;
constraint.local_p2[i] = local_p2 + shift2;
constraint.local_p1[i] = delta1 * (local_p1 + shift1);
constraint.local_p2[i] = delta2 * (local_p2 + shift2);
}
if push {

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

@@ -34,22 +34,30 @@ impl PositionGroundConstraint {
let local_n1;
let local_n2;
let delta1;
let delta2;
if flip {
std::mem::swap(&mut rb1, &mut rb2);
local_n1 = manifold.local_n2;
local_n2 = manifold.local_n1;
delta1 = &manifold.delta2;
delta2 = &manifold.delta1;
} else {
local_n1 = manifold.local_n1;
local_n2 = manifold.local_n2;
delta1 = &manifold.delta1;
delta2 = &manifold.delta2;
};
let coll_pos1 = rb1.position * delta1;
let shift1 = local_n1 * -manifold.kinematics.radius1;
let shift2 = local_n2 * -manifold.kinematics.radius2;
let n1 = coll_pos1 * local_n1;
let radius =
manifold.kinematics.radius1 + manifold.kinematics.radius2 /* - params.allowed_linear_error */;
for (l, manifold_points) in manifold
for (l, manifold_contacts) in manifold
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
@@ -59,16 +67,16 @@ impl PositionGroundConstraint {
if flip {
// Don't forget that we already swapped rb1 and rb2 above.
// So if we flip, only manifold_points[k].{local_p1,local_p2} have to
// So if we flip, only manifold_contacts[k].{local_p1,local_p2} have to
// be swapped.
for k in 0..manifold_points.len() {
p1[k] = rb1.predicted_position * (manifold_points[k].local_p2 + shift1);
local_p2[k] = manifold_points[k].local_p1 + shift2;
for k in 0..manifold_contacts.len() {
p1[k] = coll_pos1 * (manifold_contacts[k].local_p2 + shift1);
local_p2[k] = delta2 * (manifold_contacts[k].local_p1 + shift2);
}
} else {
for k in 0..manifold_points.len() {
p1[k] = rb1.predicted_position * (manifold_points[k].local_p1 + shift1);
local_p2[k] = manifold_points[k].local_p2 + shift2;
for k in 0..manifold_contacts.len() {
p1[k] = coll_pos1 * (manifold_contacts[k].local_p1 + shift1);
local_p2[k] = delta2 * (manifold_contacts[k].local_p2 + shift2);
}
}
@@ -76,11 +84,11 @@ impl PositionGroundConstraint {
rb2: rb2.active_set_offset,
p1,
local_p2,
n1: rb1.predicted_position * local_n1,
n1,
radius,
im2: rb2.mass_properties.inv_mass,
ii2: rb2.world_inv_inertia_sqrt.squared(),
num_contacts: manifold_points.len() as u8,
num_contacts: manifold_contacts.len() as u8,
erp: params.erp,
max_linear_correction: params.max_linear_correction,
};

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

@@ -54,16 +54,24 @@ impl WPositionGroundConstraint {
array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH],
);
let delta1 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
);
let delta2 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
);
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 position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
let coll_pos1 =
delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/;
let n1 = position1 * local_n1;
let n1 = coll_pos1 * local_n1;
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
@@ -90,8 +98,8 @@ impl WPositionGroundConstraint {
array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH],
);
constraint.p1[i] = position1 * local_p1 - n1 * radius1;
constraint.local_p2[i] = local_p2 - local_n2 * radius2;
constraint.p1[i] = coll_pos1 * local_p1 - n1 * radius1;
constraint.local_p2[i] = delta2 * (local_p2 - local_n2 * radius2);
}
if push {

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

@@ -148,7 +148,9 @@ impl VelocityConstraint {
let rb2 = &bodies[manifold.body_pair.body2];
let mj_lambda1 = rb1.active_set_offset;
let mj_lambda2 = rb2.active_set_offset;
let force_dir1 = rb1.position * (-manifold.local_n1);
let pos_coll1 = rb1.position * manifold.delta1;
let pos_coll2 = rb2.position * manifold.delta2;
let force_dir1 = pos_coll1 * (-manifold.local_n1);
let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
for (l, manifold_points) in manifold
@@ -215,10 +217,8 @@ impl VelocityConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = (rb1.position * manifold_point.local_p1).coords
- rb1.position.translation.vector;
let dp2 = (rb2.position * manifold_point.local_p2).coords
- rb2.position.translation.vector;
let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com;
let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
@@ -355,7 +355,7 @@ impl VelocityConstraint {
}
}
// Solve penetration.
// Solve non-penetration.
for i in 0..self.num_contacts as usize {
let elt = &mut self.elements[i].normal_part;
let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)

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

@@ -72,6 +72,9 @@ impl WVelocityConstraint {
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 delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]);
let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdFloat> =
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
@@ -79,7 +82,8 @@ impl WVelocityConstraint {
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdFloat> =
@@ -88,9 +92,13 @@ impl WVelocityConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
let coll_pos1 = pos1 * delta1;
let coll_pos2 = pos2 * delta2;
let force_dir1 = coll_pos1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -118,11 +126,11 @@ impl WVelocityConstraint {
};
for k in 0..num_points {
// FIXME: can we avoid the multiplications by position1/position2 here?
// FIXME: can we avoid the multiplications by coll_pos1/coll_pos2 here?
// By working as much as possible in local-space.
let p1 = position1
let p1 = coll_pos1
* Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]);
let p2 = position2
let p2 = coll_pos2
* Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
@@ -130,8 +138,8 @@ impl WVelocityConstraint {
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
let dp1 = p1.coords - position1.translation.vector;
let dp2 = p2.coords - position2.translation.vector;
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2);

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

@@ -66,20 +66,22 @@ impl VelocityGroundConstraint {
let mut rb1 = &bodies[manifold.body_pair.body1];
let mut rb2 = &bodies[manifold.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;
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;
force_dir1 = coll_pos1 * (-manifold.local_n1);
}
let mj_lambda2 = rb2.active_set_offset;
let force_dir1 = if flipped {
// NOTE: we already swapped rb1 and rb2
// so we multiply by rb1.position.
rb1.position * (-manifold.local_n2)
} else {
rb1.position * (-manifold.local_n1)
};
let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
for (l, manifold_points) in manifold
@@ -144,19 +146,19 @@ impl VelocityGroundConstraint {
let manifold_point = &manifold_points[k];
let (p1, p2) = if flipped {
// NOTE: we already swapped rb1 and rb2
// so we multiply by rb2.position.
// so we multiply by coll_pos1/coll_pos2.
(
rb1.position * manifold_point.local_p2,
rb2.position * manifold_point.local_p1,
coll_pos1 * manifold_point.local_p2,
coll_pos2 * manifold_point.local_p1,
)
} else {
(
rb1.position * manifold_point.local_p1,
rb2.position * manifold_point.local_p2,
coll_pos1 * manifold_point.local_p1,
coll_pos2 * manifold_point.local_p2,
)
};
let dp2 = p2.coords - rb2.position.translation.vector;
let dp1 = p1.coords - rb1.position.translation.vector;
let dp2 = p2 - rb2.world_com;
let dp1 = p1 - rb1.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);

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

@@ -86,10 +86,23 @@ impl WVelocityGroundConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let force_dir1 = position1
let delta1 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
);
let delta2 = Isometry::from(
array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
);
let coll_pos1 = pos1 * delta1;
let coll_pos2 = pos2 * delta2;
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let force_dir1 = coll_pos1
* -Vector::from(
array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
);
@@ -117,11 +130,11 @@ impl WVelocityGroundConstraint {
};
for k in 0..num_points {
let p1 = position1
let p1 = coll_pos1
* Point::from(
array![|ii| if flipped[ii] { manifold_points[ii][k].local_p2 } else { manifold_points[ii][k].local_p1 }; SIMD_WIDTH],
);
let p2 = position2
let p2 = coll_pos2
* Point::from(
array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH],
);
@@ -130,8 +143,8 @@ impl WVelocityGroundConstraint {
let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
let dp1 = p1.coords - position1.translation.vector;
let dp2 = p2.coords - position2.translation.vector;
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2);

4
src/geometry/broad_phase_multi_sap.rs
View File

@@ -662,7 +662,7 @@ mod test {
let rb = RigidBodyBuilder::new_dynamic().build();
let co = ColliderBuilder::ball(0.5).build();
let hrb = bodies.insert(rb);
let hco = colliders.insert(co, hrb, &mut bodies);
colliders.insert(co, hrb, &mut bodies);
broad_phase.update_aabbs(0.0, &bodies, &mut colliders);
@@ -681,7 +681,7 @@ mod test {
let rb = RigidBodyBuilder::new_dynamic().build();
let co = ColliderBuilder::ball(0.5).build();
let hrb = bodies.insert(rb);
let hco = colliders.insert(co, hrb, &mut bodies);
colliders.insert(co, hrb, &mut bodies);
// Make sure the proxy handles is recycled properly.
broad_phase.update_aabbs(0.0, &bodies, &mut colliders);

42
src/geometry/collider.rs
View File

@@ -3,7 +3,7 @@ use crate::geometry::{
Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
Proximity, Triangle, Trimesh,
};
use crate::math::{Isometry, Point, Vector};
use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
use na::Point3;
use ncollide::bounding_volume::{HasBoundingVolume, AABB};
use num::Zero;
@@ -150,6 +150,7 @@ impl Collider {
}
/// The position of this collider expressed in the local-space of the rigid-body it is attached to.
#[deprecated(note = "use `.position_wrt_parent()` instead.")]
pub fn delta(&self) -> &Isometry<f32> {
&self.delta
}
@@ -159,6 +160,11 @@ impl Collider {
&self.position
}
/// The position of this collider wrt the body it is attached to.
pub fn position_wrt_parent(&self) -> &Isometry<f32> {
&self.delta
}
/// The density of this collider.
pub fn density(&self) -> f32 {
self.density
@@ -347,7 +353,41 @@ impl ColliderBuilder {
self
}
/// Sets the initial translation of the collider to be created,
/// relative to the rigid-body it is attached to.
#[cfg(feature = "dim2")]
pub fn translation(mut self, x: f32, y: f32) -> Self {
self.delta.translation.x = x;
self.delta.translation.y = y;
self
}
/// Sets the initial translation of the collider to be created,
/// relative to the rigid-body it is attached to.
#[cfg(feature = "dim3")]
pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self {
self.delta.translation.x = x;
self.delta.translation.y = y;
self.delta.translation.z = z;
self
}
/// Sets the initial orientation of the collider to be created,
/// relative to the rigid-body it is attached to.
pub fn rotation(mut self, angle: AngVector<f32>) -> Self {
self.delta.rotation = Rotation::new(angle);
self
}
/// Sets the initial position (translation and orientation) of the collider to be created,
/// relative to the rigid-body it is attached to.
pub fn position(mut self, pos: Isometry<f32>) -> Self {
self.delta = pos;
self
}
/// Set the position of this collider in the local-space of the rigid-body it is attached to.
#[deprecated(note = "Use `.position` instead.")]
pub fn delta(mut self, delta: Isometry<f32>) -> Self {
self.delta = delta;
self

6
src/geometry/collider_set.rs
View File

@@ -47,7 +47,6 @@ impl ColliderSet {
parent_handle: RigidBodyHandle,
bodies: &mut RigidBodySet,
) -> ColliderHandle {
let mass_properties = coll.mass_properties();
coll.parent = parent_handle;
let parent = bodies
.get_mut_internal(parent_handle)
@@ -55,9 +54,8 @@ impl ColliderSet {
coll.position = parent.position * coll.delta;
coll.predicted_position = parent.predicted_position * coll.delta;
let handle = self.colliders.insert(coll);
parent.colliders.push(handle);
parent.mass_properties += mass_properties;
parent.update_world_mass_properties();
let coll = self.colliders.get(handle).unwrap();
parent.add_collider_internal(handle, &coll);
bodies.activate(parent_handle);
handle
}

24
src/geometry/contact.rs
View File

@@ -273,16 +273,21 @@ pub struct ContactManifold {
/// The pair of subshapes involved in this contact manifold.
pub subshape_index_pair: (usize, usize),
pub(crate) warmstart_multiplier: f32,
// We put the friction and restitution here because
// this avoids reading the colliders inside of the
// The two following are set by the constraints solver.
pub(crate) constraint_index: usize,
pub(crate) position_constraint_index: usize,
// We put the following fields here to avoids reading the colliders inside of the
// contact preparation method.
/// The friction coefficient for of all the contacts on this contact manifold.
pub friction: f32,
/// The restitution coefficient for all the contacts on this contact manifold.
pub restitution: f32,
// The following are set by the constraints solver.
pub(crate) constraint_index: usize,
pub(crate) position_constraint_index: usize,
/// The relative position between the first collider and its parent at the time the
/// contact points were generated.
pub delta1: Isometry<f32>,
/// The relative position between the second collider and its parent at the time the
/// contact points were generated.
pub delta2: Isometry<f32>,
}
impl ContactManifold {
@@ -290,6 +295,8 @@ impl ContactManifold {
pair: ColliderPair,
subshapes: (usize, usize),
body_pair: BodyPair,
delta1: Isometry<f32>,
delta2: Isometry<f32>,
friction: f32,
restitution: f32,
) -> ContactManifold {
@@ -308,6 +315,8 @@ impl ContactManifold {
warmstart_multiplier: Self::min_warmstart_multiplier(),
friction,
restitution,
delta1,
delta2,
constraint_index: 0,
position_constraint_index: 0,
}
@@ -329,6 +338,8 @@ impl ContactManifold {
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,
}
@@ -349,6 +360,8 @@ impl ContactManifold {
pair,
(subshape1, subshape2),
BodyPair::new(coll1.parent, coll2.parent),
*coll1.position_wrt_parent(),
*coll2.position_wrt_parent(),
(coll1.friction + coll2.friction) * 0.5,
(coll1.restitution + coll2.restitution) * 0.5,
)
@@ -391,6 +404,7 @@ impl ContactManifold {
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) {

1
src/geometry/contact_generator/ball_polygon_contact_generator.rs
View File

@@ -0,0 +1 @@

1
src/geometry/contact_generator/cuboid_polygon_contact_generator.rs
View File

@@ -0,0 +1 @@

7
src/geometry/narrow_phase.rs
View File

@@ -96,7 +96,7 @@ impl NarrowPhase {
}
// We have to manage the fact that one other collider will
// hive its graph index changed because of the node's swap-remove.
// have its graph index changed because of the node's swap-remove.
if let Some(replacement) = self
.proximity_graph
.remove_node(proximity_graph_id)
@@ -129,6 +129,11 @@ impl NarrowPhase {
if let (Some(co1), Some(co2)) =
colliders.get2_mut_internal(pair.collider1, pair.collider2)
{
if co1.parent == co2.parent {
// Same parents. Ignore collisions.
continue;
}
if co1.is_sensor() || co2.is_sensor() {
let gid1 = co1.proximity_graph_index;
let gid2 = co2.proximity_graph_index;

1
src/geometry/proximity_detector/ball_polygon_proximity_detector.rs
View File

@@ -0,0 +1 @@

1
src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs
View File

@@ -0,0 +1 @@

24
src/pipeline/physics_pipeline.rs
View File

@@ -7,7 +7,8 @@ use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandl
#[cfg(feature = "parallel")]
use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver};
use crate::geometry::{
BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase,
BroadPhase, BroadPhasePairEvent, Collider, ColliderHandle, ColliderPair, ColliderSet,
ContactManifoldIndex, NarrowPhase,
};
use crate::math::Vector;
use crate::pipeline::EventHandler;
@@ -245,6 +246,27 @@ impl PhysicsPipeline {
self.counters.step_completed();
}
/// Remove a collider and all its associated data.
pub fn remove_collider(
&mut self,
handle: ColliderHandle,
broad_phase: &mut BroadPhase,
narrow_phase: &mut NarrowPhase,
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
) -> Option<Collider> {
broad_phase.remove_colliders(&[handle], colliders);
narrow_phase.remove_colliders(&[handle], colliders, bodies);
let collider = colliders.remove_internal(handle)?;
if let Some(parent) = bodies.get_mut_internal(collider.parent) {
parent.remove_collider_internal(handle, &collider);
bodies.wake_up(collider.parent);
}
Some(collider)
}
/// Remove a rigid-body and all its associated data.
pub fn remove_rigid_body(
&mut self,

8
src_testbed/nphysics_backend.rs
View File

@@ -13,7 +13,7 @@ use rapier::dynamics::{
IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet,
};
use rapier::geometry::{Collider, ColliderSet, Shape};
use rapier::math::{Isometry, Vector};
use rapier::math::Vector;
use std::collections::HashMap;
#[cfg(feature = "dim3")]
use {ncollide::shape::TriMesh, nphysics::joint::BallConstraint};
@@ -165,7 +165,7 @@ impl NPhysicsWorld {
for coll_handle in rb.colliders() {
let collider = &mut colliders[*coll_handle];
collider.set_position_debug(pos * collider.delta());
collider.set_position_debug(pos * collider.position_wrt_parent());
}
}
}
@@ -176,7 +176,7 @@ fn nphysics_collider_from_rapier_collider(
is_dynamic: bool,
) -> Option<ColliderDesc<f32>> {
let margin = ColliderDesc::<f32>::default_margin();
let mut pos = Isometry::identity();
let mut pos = *collider.position_wrt_parent();
let shape = match collider.shape() {
Shape::Cuboid(cuboid) => {
@@ -184,7 +184,7 @@ fn nphysics_collider_from_rapier_collider(
}
Shape::Ball(ball) => ShapeHandle::new(Ball::new(ball.radius - margin)),
Shape::Capsule(capsule) => {
pos = capsule.transform_wrt_y();
pos *= capsule.transform_wrt_y();
ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius))
}
Shape::HeightField(heightfield) => ShapeHandle::new(heightfield.clone()),

28
src_testbed/physx_backend.rs
View File

@@ -225,6 +225,28 @@ impl PhysxWorld {
}
}
// Update mass properties.
for (rapier_handle, physx_handle) in rapier2physx.iter() {
let rb = &bodies[*rapier_handle];
if let Some(rp) = scene.get_dynamic_mut(*physx_handle) {
let densities: Vec<_> = rb
.colliders()
.iter()
.map(|h| colliders[*h].density())
.collect();
unsafe {
physx_sys::PxRigidBodyExt_updateMassAndInertia_mut(
rp.as_ptr_mut().ptr as *mut physx_sys::PxRigidBody,
densities.as_ptr(),
densities.len() as u32,
std::ptr::null(),
false,
);
}
}
}
let mut res = Self {
physics,
cooking,
@@ -390,7 +412,7 @@ impl PhysxWorld {
for coll_handle in rb.colliders() {
let collider = &mut colliders[*coll_handle];
collider.set_position_debug(iso * collider.delta());
collider.set_position_debug(iso * collider.position_wrt_parent());
}
}
}
@@ -399,7 +421,7 @@ impl PhysxWorld {
fn physx_collider_from_rapier_collider(
collider: &Collider,
) -> Option<(ColliderDesc, Isometry3<f32>)> {
let mut local_pose = Isometry3::identity();
let mut local_pose = *collider.position_wrt_parent();
let desc = match collider.shape() {
Shape::Cuboid(cuboid) => ColliderDesc::Box(
cuboid.half_extents.x,
@@ -416,7 +438,7 @@ fn physx_collider_from_rapier_collider(
}
let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir);
local_pose =
local_pose *=
Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity());
ColliderDesc::Capsule(capsule.radius, capsule.height())
}

31
src_testbed/testbed.rs
View File

@@ -791,6 +791,29 @@ impl Testbed {
.state
.action_flags
.set(TestbedActionFlags::EXAMPLE_CHANGED, true),
WindowEvent::Key(Key::C, Action::Release, _) => {
// Delete 1 collider of 10% of the remaining dynamic bodies.
let mut colliders: Vec<_> = self
.physics
.bodies
.iter()
.filter(|e| e.1.is_dynamic())
.filter(|e| !e.1.colliders().is_empty())
.map(|e| e.1.colliders().to_vec())
.collect();
colliders.sort_by_key(|co| -(co.len() as isize));
let num_to_delete = (colliders.len() / 10).max(1);
for to_delete in &colliders[..num_to_delete] {
self.physics.pipeline.remove_collider(
to_delete[0],
&mut self.physics.broad_phase,
&mut self.physics.narrow_phase,
&mut self.physics.bodies,
&mut self.physics.colliders,
);
}
}
WindowEvent::Key(Key::D, Action::Release, _) => {
// Delete 10% of the remaining dynamic bodies.
let dynamic_bodies: Vec<_> = self
@@ -1539,7 +1562,7 @@ impl State for Testbed {
}
if self.state.flags.contains(TestbedStateFlags::CONTACT_POINTS) {
draw_contacts(window, &self.physics.narrow_phase, &self.physics.bodies);
draw_contacts(window, &self.physics.narrow_phase, &self.physics.colliders);
}
if self.state.running == RunMode::Step {
@@ -1634,7 +1657,7 @@ Hashes at frame: {}
}
}
fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) {
fn draw_contacts(window: &mut Window, nf: &NarrowPhase, colliders: &ColliderSet) {
for (_, _, pair) in nf.contact_graph().interaction_pairs() {
for manifold in &pair.manifolds {
for pt in manifold.all_contacts() {
@@ -1643,8 +1666,8 @@ fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) {
} else {
Point3::new(1.0, 0.0, 0.0)
};
let pos1 = bodies[manifold.body_pair.body1].position;
let pos2 = bodies[manifold.body_pair.body2].position;
let pos1 = colliders[manifold.pair.collider1].position();
let pos2 = colliders[manifold.pair.collider2].position();
let start = pos1 * pt.local_p1;
let end = pos2 * pt.local_p2;
let n = pos1 * manifold.local_n1;