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

Try using solver contacts again, but in a more cache-coherent way.

Browse Source
This commit is contained in:
Crozet Sébastien
2020-12-30 17:30:07 +01:00
parent 7545e06cb1
commit 43628c8846
11 changed files with 165 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -63,9 +63,9 @@ pub(crate) struct PositionConstraint {
// NOTE: the points are relative to the center of masses. // NOTE: the points are relative to the center of masses.
pub local_p1: [Point<f32>; MAX_MANIFOLD_POINTS], pub local_p1: [Point<f32>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS], pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
pub dists: [f32; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<f32>, pub local_n1: Vector<f32>,
pub num_contacts: u8, pub num_contacts: u8,
pub radius: f32,
pub im1: f32, pub im1: f32,
pub im2: f32, pub im2: f32,
pub ii1: AngularInertia<f32>, pub ii1: AngularInertia<f32>,
@@ -90,22 +90,21 @@ impl PositionConstraint {
) { ) {
let rb1 = &bodies[manifold.data.body_pair.body1]; let rb1 = &bodies[manifold.data.body_pair.body1];
let rb2 = &bodies[manifold.data.body_pair.body2]; let rb2 = &bodies[manifold.data.body_pair.body2];
let shift1 = manifold.local_n1 * -manifold.kinematics.radius1; let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
let radius =
manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/;
for (l, manifold_points) in manifold for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
let mut dists = [0.0; MAX_MANIFOLD_POINTS];
for l in 0..manifold_points.len() { for l in 0..manifold_points.len() {
local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1); local_p1[l] = rb1
local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2); .position
.inverse_transform_point(&manifold_points[l].point);
local_p2[l] = rb2
.position
.inverse_transform_point(&manifold_points[l].point);
dists[l] = manifold_points[l].dist;
} }
let constraint = PositionConstraint { let constraint = PositionConstraint {
@@ -113,8 +112,8 @@ impl PositionConstraint {
rb2: rb2.active_set_offset, rb2: rb2.active_set_offset,
local_p1, local_p1,
local_p2, local_p2,
local_n1: manifold.local_n1, local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal),
radius, dists,
im1: rb1.mass_properties.inv_mass, im1: rb1.mass_properties.inv_mass,
im2: rb2.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass,
ii1: rb1.world_inv_inertia_sqrt.squared(), ii1: rb1.world_inv_inertia_sqrt.squared(),
@@ -152,9 +151,9 @@ impl PositionConstraint {
let mut pos1 = positions[self.rb1]; let mut pos1 = positions[self.rb1];
let mut pos2 = positions[self.rb2]; let mut pos2 = positions[self.rb2];
let allowed_err = params.allowed_linear_error; let allowed_err = params.allowed_linear_error;
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let p1 = pos1 * self.local_p1[k]; let p1 = pos1 * self.local_p1[k];
let p2 = pos2 * self.local_p2[k]; let p2 = pos2 * self.local_p2[k];
let dpos = p2 - p1; let dpos = p2 - p1;
@@ -204,9 +203,9 @@ impl PositionConstraint {
let mut pos1 = positions[self.rb1]; let mut pos1 = positions[self.rb1];
let mut pos2 = positions[self.rb2]; let mut pos2 = positions[self.rb2];
let allowed_err = params.allowed_linear_error; let allowed_err = params.allowed_linear_error;
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let n1 = pos1 * self.local_n1; let n1 = pos1 * self.local_n1;
let p1 = pos1 * self.local_p1[k]; let p1 = pos1 * self.local_p1[k];
let p2 = pos2 * self.local_p2[k]; let p2 = pos2 * self.local_p2[k];

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

@@ -16,8 +16,8 @@ pub(crate) struct WPositionConstraint {
// NOTE: the points are relative to the center of masses. // NOTE: the points are relative to the center of masses.
pub local_p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS], pub local_p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS], pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub dists: [SimdReal; MAX_MANIFOLD_POINTS],
pub local_n1: Vector<SimdReal>, pub local_n1: Vector<SimdReal>,
pub radius: SimdReal,
pub im1: SimdReal, pub im1: SimdReal,
pub im2: SimdReal, pub im2: SimdReal,
pub ii1: AngularInertia<SimdReal>, pub ii1: AngularInertia<SimdReal>,
@@ -45,22 +45,20 @@ impl WPositionConstraint {
let sqrt_ii2: AngularInertia<SimdReal> = let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]); let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); let local_n1 = pos1.inverse_transform_vector(&Vector::from(
let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); array![|ii| manifolds[ii].local_n1; 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 rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/; let num_active_contacts = manifolds[0].num_active_contacts();
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WPositionConstraint { let mut constraint = WPositionConstraint {
@@ -69,7 +67,7 @@ impl WPositionConstraint {
local_p1: [Point::origin(); MAX_MANIFOLD_POINTS], local_p1: [Point::origin(); MAX_MANIFOLD_POINTS],
local_p2: [Point::origin(); MAX_MANIFOLD_POINTS], local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
local_n1, local_n1,
radius, dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
im1, im1,
im2, im2,
ii1: sqrt_ii1.squared(), ii1: sqrt_ii1.squared(),
@@ -79,17 +77,12 @@ impl WPositionConstraint {
num_contacts: num_points as u8, num_contacts: num_points as u8,
}; };
let shift1 = local_n1 * -radius1;
let shift2 = local_n2 * -radius2;
for i in 0..num_points { for i in 0..num_points {
let local_p1 = let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
Point::from(array![|ii| manifold_points[ii][i].local_p1; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
let local_p2 = constraint.local_p1[i] = pos1.inverse_transform_point(&point);
Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]); constraint.local_p2[i] = pos2.inverse_transform_point(&point);
constraint.dists[i] = dist;
constraint.local_p1[i] = delta1 * (local_p1 + shift1);
constraint.local_p2[i] = delta2 * (local_p2 + shift2);
} }
if push { if push {
@@ -120,9 +113,9 @@ impl WPositionConstraint {
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdReal::splat(params.allowed_linear_error); let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let p1 = pos1 * self.local_p1[k]; let p1 = pos1 * self.local_p1[k];
let p2 = pos2 * self.local_p2[k]; let p2 = pos2 * self.local_p2[k];
@@ -174,9 +167,9 @@ impl WPositionConstraint {
let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]);
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdReal::splat(params.allowed_linear_error); let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let n1 = pos1 * self.local_n1; let n1 = pos1 * self.local_n1;
let p1 = pos1 * self.local_p1[k]; let p1 = pos1 * self.local_p1[k];
let p2 = pos2 * self.local_p2[k]; let p2 = pos2 * self.local_p2[k];

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

@@ -11,9 +11,9 @@ pub(crate) struct PositionGroundConstraint {
// NOTE: the points are relative to the center of masses. // NOTE: the points are relative to the center of masses.
pub p1: [Point<f32>; MAX_MANIFOLD_POINTS], pub p1: [Point<f32>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS], pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
pub dists: [f32; MAX_MANIFOLD_POINTS],
pub n1: Vector<f32>, pub n1: Vector<f32>,
pub num_contacts: u8, pub num_contacts: u8,
pub radius: f32,
pub im2: f32, pub im2: f32,
pub ii2: AngularInertia<f32>, pub ii2: AngularInertia<f32>,
pub erp: f32, pub erp: f32,
@@ -32,52 +32,26 @@ impl PositionGroundConstraint {
let mut rb2 = &bodies[manifold.data.body_pair.body2]; let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flip = !rb2.is_dynamic(); let flip = !rb2.is_dynamic();
let local_n1; let n1 = if flip {
let local_n2;
let delta1;
let delta2;
if flip {
std::mem::swap(&mut rb1, &mut rb2); std::mem::swap(&mut rb1, &mut rb2);
local_n1 = manifold.local_n2; -manifold.data.normal
local_n2 = manifold.local_n1;
delta1 = &manifold.data.delta2;
delta2 = &manifold.data.delta1;
} else { } else {
local_n1 = manifold.local_n1; manifold.data.normal
local_n2 = manifold.local_n2;
delta1 = &manifold.data.delta1;
delta2 = &manifold.data.delta2;
}; };
let coll_pos1 = rb1.position * delta1; let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
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_contacts) in manifold for (l, manifold_contacts) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
let mut dists = [0.0; MAX_MANIFOLD_POINTS];
if flip { for k in 0..manifold_contacts.len() {
// Don't forget that we already swapped rb1 and rb2 above. p1[k] = manifold_contacts[k].point;
// So if we flip, only manifold_contacts[k].{local_p1,local_p2} have to local_p2[k] = rb2
// be swapped. .position
for k in 0..manifold_contacts.len() { .inverse_transform_point(&manifold_contacts[k].point);
p1[k] = coll_pos1 * (manifold_contacts[k].local_p2 + shift1); dists[k] = manifold_contacts[k].dist;
local_p2[k] = delta2 * (manifold_contacts[k].local_p1 + shift2);
}
} else {
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);
}
} }
let constraint = PositionGroundConstraint { let constraint = PositionGroundConstraint {
@@ -85,7 +59,7 @@ impl PositionGroundConstraint {
p1, p1,
local_p2, local_p2,
n1, n1,
radius, dists,
im2: rb2.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass,
ii2: rb2.world_inv_inertia_sqrt.squared(), ii2: rb2.world_inv_inertia_sqrt.squared(),
num_contacts: manifold_contacts.len() as u8, num_contacts: manifold_contacts.len() as u8,
@@ -123,9 +97,9 @@ impl PositionGroundConstraint {
// Compute jacobians. // Compute jacobians.
let mut pos2 = positions[self.rb2]; let mut pos2 = positions[self.rb2];
let allowed_err = params.allowed_linear_error; let allowed_err = params.allowed_linear_error;
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let p1 = self.p1[k]; let p1 = self.p1[k];
let p2 = pos2 * self.local_p2[k]; let p2 = pos2 * self.local_p2[k];
let dpos = p2 - p1; let dpos = p2 - p1;
@@ -165,9 +139,9 @@ impl PositionGroundConstraint {
// Compute jacobians. // Compute jacobians.
let mut pos2 = positions[self.rb2]; let mut pos2 = positions[self.rb2];
let allowed_err = params.allowed_linear_error; let allowed_err = params.allowed_linear_error;
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let n1 = self.n1; let n1 = self.n1;
let p1 = self.p1[k]; let p1 = self.p1[k];
let p2 = pos2 * self.local_p2[k]; let p2 = pos2 * self.local_p2[k];

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

@@ -15,8 +15,8 @@ pub(crate) struct WPositionGroundConstraint {
// NOTE: the points are relative to the center of masses. // NOTE: the points are relative to the center of masses.
pub p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS], pub p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS], pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub dists: [SimdReal; MAX_MANIFOLD_POINTS],
pub n1: Vector<SimdReal>, pub n1: Vector<SimdReal>,
pub radius: SimdReal,
pub im2: SimdReal, pub im2: SimdReal,
pub ii2: AngularInertia<SimdReal>, pub ii2: AngularInertia<SimdReal>,
pub erp: SimdReal, pub erp: SimdReal,
@@ -49,34 +49,17 @@ impl WPositionGroundConstraint {
let sqrt_ii2: AngularInertia<SimdReal> = let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let local_n1 = Vector::from( let n1 = Vector::from(
array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; SIMD_WIDTH],
);
let local_n2 = Vector::from(
array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH],
); );
let delta1 = Isometry::from( let pos2 = Isometry::from(array![|ii| rbs2[ii].position; 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].data.delta1 } else { manifolds[ii].data.delta2 }; SIMD_WIDTH],
);
let radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; 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 rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let radius = radius1 + radius2 /*- SimdReal::splat(params.allowed_linear_error)*/; let num_active_contacts = manifolds[0].num_active_contacts();
let n1 = coll_pos1 * local_n1; for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH];
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];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WPositionGroundConstraint { let mut constraint = WPositionGroundConstraint {
@@ -84,7 +67,7 @@ impl WPositionGroundConstraint {
p1: [Point::origin(); MAX_MANIFOLD_POINTS], p1: [Point::origin(); MAX_MANIFOLD_POINTS],
local_p2: [Point::origin(); MAX_MANIFOLD_POINTS], local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
n1, n1,
radius, dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
im2, im2,
ii2: sqrt_ii2.squared(), ii2: sqrt_ii2.squared(),
erp: SimdReal::splat(params.erp), erp: SimdReal::splat(params.erp),
@@ -93,15 +76,11 @@ impl WPositionGroundConstraint {
}; };
for i in 0..num_points { for i in 0..num_points {
let local_p1 = Point::from( let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
array![|ii| if flipped[ii] { manifold_points[ii][i].local_p2 } else { manifold_points[ii][i].local_p1 }; SIMD_WIDTH], let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
); constraint.p1[i] = point;
let local_p2 = Point::from( constraint.local_p2[i] = pos2.inverse_transform_point(&point);
array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH], constraint.dists[i] = dist;
);
constraint.p1[i] = coll_pos1 * local_p1 - n1 * radius1;
constraint.local_p2[i] = delta2 * (local_p2 - local_n2 * radius2);
} }
if push { if push {
@@ -132,9 +111,9 @@ impl WPositionGroundConstraint {
// Compute jacobians. // Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdReal::splat(params.allowed_linear_error); let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let p1 = self.p1[k]; let p1 = self.p1[k];
let p2 = pos2 * self.local_p2[k]; let p2 = pos2 * self.local_p2[k];
@@ -174,9 +153,9 @@ impl WPositionGroundConstraint {
// Compute jacobians. // Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
let allowed_err = SimdReal::splat(params.allowed_linear_error); let allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize { for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let n1 = self.n1; let n1 = self.n1;
let p1 = self.p1[k]; let p1 = self.p1[k];
let p2 = pos2 * self.local_p2[k]; let p2 = pos2 * self.local_p2[k];

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

@@ -148,23 +148,18 @@ impl VelocityConstraint {
let rb2 = &bodies[manifold.data.body_pair.body2]; let rb2 = &bodies[manifold.data.body_pair.body2];
let mj_lambda1 = rb1.active_set_offset; let mj_lambda1 = rb1.active_set_offset;
let mj_lambda2 = rb2.active_set_offset; let mj_lambda2 = rb2.active_set_offset;
let pos_coll1 = rb1.position * manifold.data.delta1; let force_dir1 = -manifold.data.normal;
let pos_coll2 = rb2.position * manifold.data.delta2;
let force_dir1 = pos_coll1 * (-manifold.local_n1);
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
for (l, manifold_points) in manifold for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
#[cfg(not(target_arch = "wasm32"))] #[cfg(not(target_arch = "wasm32"))]
let mut constraint = VelocityConstraint { let mut constraint = VelocityConstraint {
dir1: force_dir1, dir1: force_dir1,
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: rb1.mass_properties.inv_mass, im1: rb1.mass_properties.inv_mass,
im2: rb2.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass,
limit: manifold.data.friction, limit: 0.0,
mj_lambda1, mj_lambda1,
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
@@ -217,12 +212,13 @@ impl VelocityConstraint {
for k in 0..manifold_points.len() { for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k]; let manifold_point = &manifold_points[k];
let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com; let dp1 = manifold_point.point - rb1.world_com;
let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com; let dp2 = manifold_point.point - rb2.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
// Normal part. // Normal part.
{ {
let gcross1 = rb1 let gcross1 = rb1
@@ -241,12 +237,12 @@ impl VelocityConstraint {
let mut rhs = (vel1 - vel2).dot(&force_dir1); let mut rhs = (vel1 - vel2).dot(&force_dir1);
if rhs <= -params.restitution_velocity_threshold { if rhs <= -params.restitution_velocity_threshold {
rhs += manifold.data.restitution * rhs rhs += manifold_point.restitution * rhs
} }
rhs += manifold_point.dist.max(0.0) * params.inv_dt(); rhs += manifold_point.dist.max(0.0) * params.inv_dt();
let impulse = manifold_points[k].data.impulse * warmstart_coeff; let impulse = manifold_point.data.impulse * warmstart_coeff;
constraint.elements[k].normal_part = VelocityConstraintElementPart { constraint.elements[k].normal_part = VelocityConstraintElementPart {
gcross1, gcross1,
@@ -275,9 +271,9 @@ impl VelocityConstraint {
+ gcross2.gdot(gcross2)); + gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]); let rhs = (vel1 - vel2).dot(&tangents1[j]);
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; let impulse = manifold_point.data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let impulse = manifold_points[k].data.tangent_impulse[j] * warmstart_coeff; let impulse = manifold_point.data.tangent_impulse[j] * warmstart_coeff;
constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart { constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart {
gcross1, gcross1,

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

@@ -72,9 +72,6 @@ impl WVelocityConstraint {
let rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; 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 rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; 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 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); let im1 = SimdReal::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii1: AngularInertia<SimdReal> = let ii1: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
@@ -82,7 +79,6 @@ impl WVelocityConstraint {
let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let angvel1 = AngVector::<SimdReal>::from(array![|ii| rbs1[ii].angvel; 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 world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); let im2 = SimdReal::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
@@ -92,27 +88,24 @@ impl WVelocityConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; 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 world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let coll_pos1 = pos1 * delta1; let force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
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_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
let warmstart_multiplier = let warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
let num_active_contacts = manifolds[0].num_active_contacts();
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; let manifold_points = array![|ii|
&manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH
];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WVelocityConstraint { let mut constraint = WVelocityConstraint {
@@ -120,7 +113,7 @@ impl WVelocityConstraint {
elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1, im1,
im2, im2,
limit: friction, limit: SimdReal::splat(0.0),
mj_lambda1, mj_lambda1,
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
@@ -129,24 +122,24 @@ impl WVelocityConstraint {
}; };
for k in 0..num_points { for k in 0..num_points {
// FIXME: can we avoid the multiplications by coll_pos1/coll_pos2 here? let friction =
// By working as much as possible in local-space. SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
let p1 = coll_pos1 let restitution =
* Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]); SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
let p2 = coll_pos2 let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
* Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse = let impulse =
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1; let dp1 = point - world_com1;
let dp2 = p2 - world_com2; let dp2 = point - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1); let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2); let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
// Normal part. // Normal part.
{ {
let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1));

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

@@ -66,35 +66,25 @@ impl VelocityGroundConstraint {
let mut rb1 = &bodies[manifold.data.body_pair.body1]; let mut rb1 = &bodies[manifold.data.body_pair.body1];
let mut rb2 = &bodies[manifold.data.body_pair.body2]; let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flipped = !rb2.is_dynamic(); let flipped = !rb2.is_dynamic();
let force_dir1;
let coll_pos1;
let coll_pos2;
if flipped { let force_dir1 = if flipped {
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); std::mem::swap(&mut rb1, &mut rb2);
manifold.data.normal
} else { } else {
coll_pos1 = rb1.position * manifold.data.delta1; -manifold.data.normal
coll_pos2 = rb2.position * manifold.data.delta2; };
force_dir1 = coll_pos1 * (-manifold.local_n1);
}
let mj_lambda2 = rb2.active_set_offset; let mj_lambda2 = rb2.active_set_offset;
let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff; let warmstart_coeff = manifold.data.warmstart_multiplier * params.warmstart_coeff;
let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
for (l, manifold_points) in manifold for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
#[cfg(not(target_arch = "wasm32"))] #[cfg(not(target_arch = "wasm32"))]
let mut constraint = VelocityGroundConstraint { let mut constraint = VelocityGroundConstraint {
dir1: force_dir1, dir1: force_dir1,
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb2.mass_properties.inv_mass, im2: rb2.mass_properties.inv_mass,
limit: manifold.data.friction, limit: 0.0,
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
manifold_contact_id: l * MAX_MANIFOLD_POINTS, manifold_contact_id: l * MAX_MANIFOLD_POINTS,
@@ -144,24 +134,13 @@ impl VelocityGroundConstraint {
for k in 0..manifold_points.len() { for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k]; let manifold_point = &manifold_points[k];
let (p1, p2) = if flipped { let dp2 = manifold_point.point - rb2.world_com;
// NOTE: we already swapped rb1 and rb2 let dp1 = manifold_point.point - rb1.world_com;
// so we multiply by coll_pos1/coll_pos2.
(
coll_pos1 * manifold_point.local_p2,
coll_pos2 * manifold_point.local_p1,
)
} else {
(
coll_pos1 * manifold_point.local_p1,
coll_pos2 * manifold_point.local_p2,
)
};
let dp2 = p2 - rb2.world_com;
let dp1 = p1 - rb1.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
// Normal part. // Normal part.
{ {
let gcross2 = rb2 let gcross2 = rb2
@@ -173,7 +152,7 @@ impl VelocityGroundConstraint {
let mut rhs = (vel1 - vel2).dot(&force_dir1); let mut rhs = (vel1 - vel2).dot(&force_dir1);
if rhs <= -params.restitution_velocity_threshold { if rhs <= -params.restitution_velocity_threshold {
rhs += manifold.data.restitution * rhs rhs += manifold_point.restitution * rhs
} }
rhs += manifold_point.dist.max(0.0) * params.inv_dt(); rhs += manifold_point.dist.max(0.0) * params.inv_dt();

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

@@ -86,46 +86,31 @@ impl WVelocityGroundConstraint {
let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let angvel2 = AngVector::<SimdReal>::from(array![|ii| rbs2[ii].angvel; 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 delta1 = Isometry::from(
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].data.delta1 } else { manifolds[ii].data.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_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 world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let force_dir1 = coll_pos1 let force_dir1 = Vector::from(
* -Vector::from( array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH],
array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], );
);
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
let friction = SimdReal::from(array![|ii| manifolds[ii].data.friction; SIMD_WIDTH]);
let restitution = SimdReal::from(array![|ii| manifolds[ii].data.restitution; SIMD_WIDTH]);
let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold); let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
let warmstart_multiplier = let warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]); SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff); let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
let num_active_contacts = manifolds[0].num_active_contacts();
for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { for l in (0..num_active_contacts).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; let manifold_points = array![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]; SIMD_WIDTH];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WVelocityGroundConstraint { let mut constraint = WVelocityGroundConstraint {
dir1: force_dir1, dir1: force_dir1,
elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2, im2,
limit: friction, limit: SimdReal::splat(0.0),
mj_lambda2, mj_lambda2,
manifold_id, manifold_id,
manifold_contact_id: l, manifold_contact_id: l,
@@ -133,25 +118,23 @@ impl WVelocityGroundConstraint {
}; };
for k in 0..num_points { for k in 0..num_points {
let p1 = coll_pos1 let friction =
* Point::from( SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
array![|ii| if flipped[ii] { manifold_points[ii][k].local_p2 } else { manifold_points[ii][k].local_p1 }; SIMD_WIDTH], let restitution =
); SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
let p2 = coll_pos2 let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
* Point::from(
array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH],
);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse = let impulse =
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]); SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1; let dp1 = point - world_com1;
let dp2 = p2 - world_com2; let dp2 = point - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1); let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2); let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
// Normal part. // Normal part.
{ {
let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1));

65
src/geometry/contact_pair.rs
View File

@@ -1,6 +1,6 @@
use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet}; use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet};
use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold}; use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold};
use crate::math::{Isometry, Point, Vector}; use crate::math::{Isometry, Point, Real, Vector};
use cdl::query::ContactManifoldsWorkspace; use cdl::query::ContactManifoldsWorkspace;
use cdl::utils::MaybeSerializableData; use cdl::utils::MaybeSerializableData;
#[cfg(feature = "simd-is-enabled")] #[cfg(feature = "simd-is-enabled")]
@@ -134,7 +134,7 @@ impl ContactPair {
let coll2 = &colliders[self.pair.collider2]; let coll2 = &colliders[self.pair.collider2];
if self.manifolds.len() == 0 { if self.manifolds.len() == 0 {
let manifold_data = ContactManifoldData::from_colliders(coll1, coll2, flags); let manifold_data = ContactManifoldData::with_subshape_indices(coll1, coll2, flags);
self.manifolds self.manifolds
.push(ContactManifold::with_data((0, 0), manifold_data)); .push(ContactManifold::with_data((0, 0), manifold_data));
} }
@@ -164,18 +164,21 @@ pub struct ContactManifoldData {
pub(crate) position_constraint_index: usize, pub(crate) position_constraint_index: usize,
// We put the following fields here to avoids reading the colliders inside of the // We put the following fields here to avoids reading the colliders inside of the
// contact preparation method. // 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 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>,
/// Flags used to control some aspects of the constraints solver for this contact manifold. /// Flags used to control some aspects of the constraints solver for this contact manifold.
pub solver_flags: SolverFlags, pub solver_flags: SolverFlags,
pub normal: Vector<Real>,
pub solver_contacts: Vec<SolverContact>,
}
#[derive(Copy, Clone, Debug)]
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub struct SolverContact {
pub point: Point<Real>,
pub dist: Real,
pub friction: Real,
pub restitution: Real,
pub surface_velocity: Vector<Real>,
pub data: ContactData,
} }
impl Default for ContactManifoldData { impl Default for ContactManifoldData {
@@ -185,39 +188,32 @@ impl Default for ContactManifoldData {
RigidBodySet::invalid_handle(), RigidBodySet::invalid_handle(),
RigidBodySet::invalid_handle(), RigidBodySet::invalid_handle(),
), ),
Isometry::identity(),
Isometry::identity(),
0.0,
0.0,
SolverFlags::empty(), SolverFlags::empty(),
) )
} }
} }
impl ContactManifoldData { impl ContactManifoldData {
pub(crate) fn new( pub(crate) fn new(body_pair: BodyPair, solver_flags: SolverFlags) -> ContactManifoldData {
body_pair: BodyPair,
delta1: Isometry<f32>,
delta2: Isometry<f32>,
friction: f32,
restitution: f32,
solver_flags: SolverFlags,
) -> ContactManifoldData {
Self { Self {
body_pair, body_pair,
warmstart_multiplier: Self::min_warmstart_multiplier(), warmstart_multiplier: Self::min_warmstart_multiplier(),
friction,
restitution,
delta1,
delta2,
constraint_index: 0, constraint_index: 0,
position_constraint_index: 0, position_constraint_index: 0,
solver_flags, solver_flags,
normal: Vector::zeros(),
solver_contacts: Vec::new(),
} }
} }
pub(crate) fn from_colliders(coll1: &Collider, coll2: &Collider, flags: SolverFlags) -> Self { pub(crate) fn set_from_colliders(
Self::with_subshape_indices(coll1, coll2, flags) &mut self,
coll1: &Collider,
coll2: &Collider,
flags: SolverFlags,
) {
self.body_pair = BodyPair::new(coll1.parent, coll2.parent);
self.solver_flags = flags;
} }
pub(crate) fn with_subshape_indices( pub(crate) fn with_subshape_indices(
@@ -225,14 +221,7 @@ impl ContactManifoldData {
coll2: &Collider, coll2: &Collider,
solver_flags: SolverFlags, solver_flags: SolverFlags,
) -> Self { ) -> Self {
Self::new( Self::new(BodyPair::new(coll1.parent, coll2.parent), solver_flags)
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,
solver_flags,
)
} }
pub(crate) fn min_warmstart_multiplier() -> f32 { pub(crate) fn min_warmstart_multiplier() -> f32 {

2
src/geometry/mod.rs
View File

@@ -4,7 +4,7 @@ pub use self::broad_phase_multi_sap::BroadPhase;
pub use self::collider::{Collider, ColliderBuilder, ColliderShape}; pub use self::collider::{Collider, ColliderBuilder, ColliderShape};
pub use self::collider_set::{ColliderHandle, ColliderSet}; pub use self::collider_set::{ColliderHandle, ColliderSet};
pub use self::contact_pair::{ContactData, ContactManifoldData}; pub use self::contact_pair::{ContactData, ContactManifoldData};
pub use self::contact_pair::{ContactPair, SolverFlags}; pub use self::contact_pair::{ContactPair, SolverContact, SolverFlags};
pub use self::interaction_graph::{ pub use self::interaction_graph::{
ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex, ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex,
}; };

21
src/geometry/narrow_phase.rs
View File

@@ -7,9 +7,10 @@ use crate::dynamics::RigidBodySet;
use crate::geometry::{ use crate::geometry::{
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent, BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent,
ContactManifoldData, ContactPairFilter, IntersectionEvent, PairFilterContext, ContactManifoldData, ContactPairFilter, IntersectionEvent, PairFilterContext,
ProximityPairFilter, RemovedCollider, SolverFlags, ProximityPairFilter, RemovedCollider, SolverContact, SolverFlags,
}; };
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
use crate::math::Vector;
use crate::pipeline::EventHandler; use crate::pipeline::EventHandler;
use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher}; use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher};
use std::collections::HashMap; use std::collections::HashMap;
@@ -526,7 +527,23 @@ impl NarrowPhase {
// TODO: don't write this everytime? // TODO: don't write this everytime?
for manifold in &mut pair.manifolds { for manifold in &mut pair.manifolds {
manifold.data = ContactManifoldData::from_colliders(co1, co2, solver_flags); manifold.data.solver_contacts.clear();
manifold.data.set_from_colliders(co1, co2, solver_flags);
manifold.data.normal = co1.position() * manifold.local_n1;
for contact in &manifold.points[..manifold.num_active_contacts] {
let solver_contact = SolverContact {
point: co1.position() * contact.local_p1
+ manifold.data.normal * contact.dist / 2.0,
dist: contact.dist,
friction: (co1.friction + co2.friction) / 2.0,
restitution: (co1.restitution + co2.restitution) / 2.0,
surface_velocity: Vector::zeros(),
data: contact.data,
};
manifold.data.solver_contacts.push(solver_contact);
}
} }
}); });
} }