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.
pub local_p1: [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 num_contacts: u8,
pub radius: f32,
pub im1: f32,
pub im2: f32,
pub ii1: AngularInertia<f32>,
@@ -90,22 +90,21 @@ impl PositionConstraint {
) {
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 =
manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/;
let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
for (l, manifold_points) in manifold
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
let mut local_p1 = [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() {
local_p1[l] = manifold.data.delta1 * (manifold_points[l].local_p1 + shift1);
local_p2[l] = manifold.data.delta2 * (manifold_points[l].local_p2 + shift2);
local_p1[l] = rb1
.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 {
@@ -113,8 +112,8 @@ impl PositionConstraint {
rb2: rb2.active_set_offset,
local_p1,
local_p2,
local_n1: manifold.local_n1,
radius,
local_n1: rb1.position.inverse_transform_vector(&manifold.data.normal),
dists,
im1: rb1.mass_properties.inv_mass,
im2: rb2.mass_properties.inv_mass,
ii1: rb1.world_inv_inertia_sqrt.squared(),
@@ -152,9 +151,9 @@ impl PositionConstraint {
let mut pos1 = positions[self.rb1];
let mut pos2 = positions[self.rb2];
let allowed_err = params.allowed_linear_error;
let target_dist = self.radius - allowed_err;
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 p2 = pos2 * self.local_p2[k];
let dpos = p2 - p1;
@@ -204,9 +203,9 @@ impl PositionConstraint {
let mut pos1 = positions[self.rb1];
let mut pos2 = positions[self.rb2];
let allowed_err = params.allowed_linear_error;
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let n1 = pos1 * self.local_n1;
let p1 = pos1 * self.local_p1[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.
pub local_p1: [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 radius: SimdReal,
pub im1: SimdReal,
pub im2: SimdReal,
pub ii1: AngularInertia<SimdReal>,
@@ -45,22 +45,20 @@ impl WPositionConstraint {
let sqrt_ii2: AngularInertia<SimdReal> =
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 local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; 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 radius1 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
let radius2 = SimdReal::from(array![|ii| manifolds[ii].kinematics.radius2; 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 local_n1 = pos1.inverse_transform_vector(&Vector::from(
array![|ii| manifolds[ii].local_n1; 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 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) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
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];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WPositionConstraint {
@@ -69,7 +67,7 @@ impl WPositionConstraint {
local_p1: [Point::origin(); MAX_MANIFOLD_POINTS],
local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
local_n1,
radius,
dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
im1,
im2,
ii1: sqrt_ii1.squared(),
@@ -79,17 +77,12 @@ impl WPositionConstraint {
num_contacts: num_points as u8,
};
let shift1 = local_n1 * -radius1;
let shift2 = local_n2 * -radius2;
for i in 0..num_points {
let local_p1 =
Point::from(array![|ii| manifold_points[ii][i].local_p1; SIMD_WIDTH]);
let local_p2 =
Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]);
constraint.local_p1[i] = delta1 * (local_p1 + shift1);
constraint.local_p2[i] = delta2 * (local_p2 + shift2);
let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
constraint.local_p1[i] = pos1.inverse_transform_point(&point);
constraint.local_p2[i] = pos2.inverse_transform_point(&point);
constraint.dists[i] = dist;
}
if push {
@@ -120,9 +113,9 @@ impl WPositionConstraint {
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 allowed_err = SimdReal::splat(params.allowed_linear_error);
let target_dist = self.radius - allowed_err;
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 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 pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
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 {
let target_dist = -self.dists[k] - allowed_err;
let n1 = pos1 * self.local_n1;
let p1 = pos1 * self.local_p1[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.
pub p1: [Point<f32>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<f32>; MAX_MANIFOLD_POINTS],
pub dists: [f32; MAX_MANIFOLD_POINTS],
pub n1: Vector<f32>,
pub num_contacts: u8,
pub radius: f32,
pub im2: f32,
pub ii2: AngularInertia<f32>,
pub erp: f32,
@@ -32,52 +32,26 @@ impl PositionGroundConstraint {
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flip = !rb2.is_dynamic();
let local_n1;
let local_n2;
let delta1;
let delta2;
if flip {
let n1 = if flip {
std::mem::swap(&mut rb1, &mut rb2);
local_n1 = manifold.local_n2;
local_n2 = manifold.local_n1;
delta1 = &manifold.data.delta2;
delta2 = &manifold.data.delta1;
-manifold.data.normal
} else {
local_n1 = manifold.local_n1;
local_n2 = manifold.local_n2;
delta1 = &manifold.data.delta1;
delta2 = &manifold.data.delta2;
manifold.data.normal
};
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 */;
let active_contacts = &manifold.data.solver_contacts[..manifold.num_active_contacts];
for (l, manifold_contacts) in manifold
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
for (l, manifold_contacts) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS];
let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
let mut dists = [0.0; MAX_MANIFOLD_POINTS];
if flip {
// Don't forget that we already swapped rb1 and rb2 above.
// So if we flip, only manifold_contacts[k].{local_p1,local_p2} have to
// be swapped.
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_contacts.len() {
p1[k] = coll_pos1 * (manifold_contacts[k].local_p1 + shift1);
local_p2[k] = delta2 * (manifold_contacts[k].local_p2 + shift2);
}
for k in 0..manifold_contacts.len() {
p1[k] = manifold_contacts[k].point;
local_p2[k] = rb2
.position
.inverse_transform_point(&manifold_contacts[k].point);
dists[k] = manifold_contacts[k].dist;
}
let constraint = PositionGroundConstraint {
@@ -85,7 +59,7 @@ impl PositionGroundConstraint {
p1,
local_p2,
n1,
radius,
dists,
im2: rb2.mass_properties.inv_mass,
ii2: rb2.world_inv_inertia_sqrt.squared(),
num_contacts: manifold_contacts.len() as u8,
@@ -123,9 +97,9 @@ impl PositionGroundConstraint {
// Compute jacobians.
let mut pos2 = positions[self.rb2];
let allowed_err = params.allowed_linear_error;
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let p1 = self.p1[k];
let p2 = pos2 * self.local_p2[k];
let dpos = p2 - p1;
@@ -165,9 +139,9 @@ impl PositionGroundConstraint {
// Compute jacobians.
let mut pos2 = positions[self.rb2];
let allowed_err = params.allowed_linear_error;
let target_dist = self.radius - allowed_err;
for k in 0..self.num_contacts as usize {
let target_dist = -self.dists[k] - allowed_err;
let n1 = self.n1;
let p1 = self.p1[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.
pub p1: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub local_p2: [Point<SimdReal>; MAX_MANIFOLD_POINTS],
pub dists: [SimdReal; MAX_MANIFOLD_POINTS],
pub n1: Vector<SimdReal>,
pub radius: SimdReal,
pub im2: SimdReal,
pub ii2: AngularInertia<SimdReal>,
pub erp: SimdReal,
@@ -49,34 +49,17 @@ impl WPositionGroundConstraint {
let sqrt_ii2: AngularInertia<SimdReal> =
AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
let local_n1 = Vector::from(
array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
);
let local_n2 = Vector::from(
array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH],
let n1 = Vector::from(
array![|ii| if flipped[ii] { -manifolds[ii].data.normal } else { manifolds[ii].data.normal }; 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 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 pos2 = Isometry::from(array![|ii| rbs2[ii].position; 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..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
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];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WPositionGroundConstraint {
@@ -84,7 +67,7 @@ impl WPositionGroundConstraint {
p1: [Point::origin(); MAX_MANIFOLD_POINTS],
local_p2: [Point::origin(); MAX_MANIFOLD_POINTS],
n1,
radius,
dists: [SimdReal::zero(); MAX_MANIFOLD_POINTS],
im2,
ii2: sqrt_ii2.squared(),
erp: SimdReal::splat(params.erp),
@@ -93,15 +76,11 @@ impl WPositionGroundConstraint {
};
for i in 0..num_points {
let local_p1 = Point::from(
array![|ii| if flipped[ii] { manifold_points[ii][i].local_p2 } else { manifold_points[ii][i].local_p1 }; SIMD_WIDTH],
);
let local_p2 = Point::from(
array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH],
);
constraint.p1[i] = coll_pos1 * local_p1 - n1 * radius1;
constraint.local_p2[i] = delta2 * (local_p2 - local_n2 * radius2);
let point = Point::from(array![|ii| manifold_points[ii][i].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][i].dist; SIMD_WIDTH]);
constraint.p1[i] = point;
constraint.local_p2[i] = pos2.inverse_transform_point(&point);
constraint.dists[i] = dist;
}
if push {
@@ -132,9 +111,9 @@ impl WPositionGroundConstraint {
// Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
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 {
let target_dist = -self.dists[k] - allowed_err;
let p1 = self.p1[k];
let p2 = pos2 * self.local_p2[k];
@@ -174,9 +153,9 @@ impl WPositionGroundConstraint {
// Compute jacobians.
let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]);
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 {
let target_dist = -self.dists[k] - allowed_err;
let n1 = self.n1;
let p1 = self.p1[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 mj_lambda1 = rb1.active_set_offset;
let mj_lambda2 = rb2.active_set_offset;
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 force_dir1 = -manifold.data.normal;
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
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
#[cfg(not(target_arch = "wasm32"))]
let mut constraint = VelocityConstraint {
dir1: force_dir1,
elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1: rb1.mass_properties.inv_mass,
im2: rb2.mass_properties.inv_mass,
limit: manifold.data.friction,
limit: 0.0,
mj_lambda1,
mj_lambda2,
manifold_id,
@@ -217,12 +212,13 @@ impl VelocityConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com;
let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com;
let dp1 = manifold_point.point - rb1.world_com;
let dp2 = manifold_point.point - rb2.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
// Normal part.
{
let gcross1 = rb1
@@ -241,12 +237,12 @@ impl VelocityConstraint {
let mut rhs = (vel1 - vel2).dot(&force_dir1);
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();
let impulse = manifold_points[k].data.impulse * warmstart_coeff;
let impulse = manifold_point.data.impulse * warmstart_coeff;
constraint.elements[k].normal_part = VelocityConstraintElementPart {
gcross1,
@@ -275,9 +271,9 @@ impl VelocityConstraint {
+ gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
#[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")]
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 {
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 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 ii1: AngularInertia<SimdReal> =
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 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 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 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 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 force_dir1 = -Vector::from(array![|ii| manifolds[ii].data.normal; 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 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 warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
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) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
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
];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WVelocityConstraint {
@@ -120,7 +113,7 @@ impl WVelocityConstraint {
elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im1,
im2,
limit: friction,
limit: SimdReal::splat(0.0),
mj_lambda1,
mj_lambda2,
manifold_id,
@@ -129,24 +122,24 @@ impl WVelocityConstraint {
};
for k in 0..num_points {
// FIXME: can we avoid the multiplications by coll_pos1/coll_pos2 here?
// By working as much as possible in local-space.
let p1 = coll_pos1
* Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]);
let p2 = coll_pos2
* Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]);
let friction =
SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
let restitution =
SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
let dp1 = point - world_com1;
let dp2 = point - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
// Normal part.
{
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 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.data.delta2;
coll_pos2 = rb1.position * manifold.data.delta1;
force_dir1 = coll_pos1 * (-manifold.local_n2);
let force_dir1 = if flipped {
std::mem::swap(&mut rb1, &mut rb2);
manifold.data.normal
} else {
coll_pos1 = rb1.position * manifold.data.delta1;
coll_pos2 = rb2.position * manifold.data.delta2;
force_dir1 = coll_pos1 * (-manifold.local_n1);
}
-manifold.data.normal
};
let mj_lambda2 = rb2.active_set_offset;
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
.active_contacts()
.chunks(MAX_MANIFOLD_POINTS)
.enumerate()
{
for (l, manifold_points) in active_contacts.chunks(MAX_MANIFOLD_POINTS).enumerate() {
#[cfg(not(target_arch = "wasm32"))]
let mut constraint = VelocityGroundConstraint {
dir1: force_dir1,
elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2: rb2.mass_properties.inv_mass,
limit: manifold.data.friction,
limit: 0.0,
mj_lambda2,
manifold_id,
manifold_contact_id: l * MAX_MANIFOLD_POINTS,
@@ -144,24 +134,13 @@ impl VelocityGroundConstraint {
for k in 0..manifold_points.len() {
let manifold_point = &manifold_points[k];
let (p1, p2) = if flipped {
// NOTE: we already swapped rb1 and rb2
// 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 dp2 = manifold_point.point - rb2.world_com;
let dp1 = manifold_point.point - rb1.world_com;
let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
constraint.limit = manifold_point.friction;
// Normal part.
{
let gcross2 = rb2
@@ -173,7 +152,7 @@ impl VelocityGroundConstraint {
let mut rhs = (vel1 - vel2).dot(&force_dir1);
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();

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 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_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],
);
let force_dir1 = Vector::from(
array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; 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 warmstart_multiplier =
SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
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) {
let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH];
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];
let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
let mut constraint = WVelocityGroundConstraint {
dir1: force_dir1,
elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
im2,
limit: friction,
limit: SimdReal::splat(0.0),
mj_lambda2,
manifold_id,
manifold_contact_id: l,
@@ -133,25 +118,23 @@ impl WVelocityGroundConstraint {
};
for k in 0..num_points {
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 = coll_pos2
* Point::from(
array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH],
);
let friction =
SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
let restitution =
SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
let impulse =
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
let dp1 = p1 - world_com1;
let dp2 = p2 - world_com2;
let dp1 = point - world_com1;
let dp2 = point - world_com2;
let vel1 = linvel1 + angvel1.gcross(dp1);
let vel2 = linvel2 + angvel2.gcross(dp2);
constraint.limit = friction;
// Normal part.
{
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::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::utils::MaybeSerializableData;
#[cfg(feature = "simd-is-enabled")]
@@ -134,7 +134,7 @@ impl ContactPair {
let coll2 = &colliders[self.pair.collider2];
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
.push(ContactManifold::with_data((0, 0), manifold_data));
}
@@ -164,18 +164,21 @@ pub struct ContactManifoldData {
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 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.
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 {
@@ -185,39 +188,32 @@ impl Default for ContactManifoldData {
RigidBodySet::invalid_handle(),
RigidBodySet::invalid_handle(),
),
Isometry::identity(),
Isometry::identity(),
0.0,
0.0,
SolverFlags::empty(),
)
}
}
impl ContactManifoldData {
pub(crate) fn new(
body_pair: BodyPair,
delta1: Isometry<f32>,
delta2: Isometry<f32>,
friction: f32,
restitution: f32,
solver_flags: SolverFlags,
) -> ContactManifoldData {
pub(crate) fn new(body_pair: BodyPair, solver_flags: SolverFlags) -> ContactManifoldData {
Self {
body_pair,
warmstart_multiplier: Self::min_warmstart_multiplier(),
friction,
restitution,
delta1,
delta2,
constraint_index: 0,
position_constraint_index: 0,
solver_flags,
normal: Vector::zeros(),
solver_contacts: Vec::new(),
}
}
pub(crate) fn from_colliders(coll1: &Collider, coll2: &Collider, flags: SolverFlags) -> Self {
Self::with_subshape_indices(coll1, coll2, flags)
pub(crate) fn set_from_colliders(
&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(
@@ -225,14 +221,7 @@ impl ContactManifoldData {
coll2: &Collider,
solver_flags: SolverFlags,
) -> Self {
Self::new(
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,
)
Self::new(BodyPair::new(coll1.parent, coll2.parent), solver_flags)
}
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_set::{ColliderHandle, ColliderSet};
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::{
ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex,
};

21
src/geometry/narrow_phase.rs
View File

@@ -7,9 +7,10 @@ use crate::dynamics::RigidBodySet;
use crate::geometry::{
BroadPhasePairEvent, ColliderGraphIndex, ColliderHandle, ContactData, ContactEvent,
ContactManifoldData, ContactPairFilter, IntersectionEvent, PairFilterContext,
ProximityPairFilter, RemovedCollider, SolverFlags,
ProximityPairFilter, RemovedCollider, SolverContact, SolverFlags,
};
use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph};
use crate::math::Vector;
use crate::pipeline::EventHandler;
use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher, QueryDispatcher};
use std::collections::HashMap;
@@ -526,7 +527,23 @@ impl NarrowPhase {
// TODO: don't write this everytime?
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);
}
}
});
}