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

Fix constraints resolution with non-identity relative collider position.

Browse Source
This commit is contained in:
Sébastien Crozet
2020-09-01 14:02:59 +02:00
parent 03b437f278
commit 9622827dc6
11 changed files with 118 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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,
};