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

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();