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

Properly take the tangent_velocity into account in the velocity solver.

Browse Source
This commit is contained in:
Crozet Sébastien
2021-02-23 15:47:24 +01:00
parent 4ca32a9546
commit 3fdb4cd6d3
4 changed files with 22 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -273,7 +273,8 @@ impl VelocityConstraint {
+ rb2.effective_inv_mass
+ gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
let rhs =
(vel1 - vel2 + manifold_point.tangent_velocity).dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_point.data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")]
@@ -391,7 +392,7 @@ impl VelocityConstraint {
active_contact.data.impulse = self.elements[k].normal_part.impulse;
#[cfg(feature = "dim2")]
{
active_contacts.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
active_contact.data.tangent_impulse = self.elements[k].tangent_part[0].impulse;
}
#[cfg(feature = "dim3")]
{

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

@@ -130,6 +130,8 @@ impl WVelocityConstraint {
);
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 tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
let impulse =
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
@@ -181,7 +183,7 @@ impl WVelocityConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdReal::splat(1.0)
/ (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
let rhs = (vel1 - vel2).dot(&tangents1[j]);
let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]);
constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart {
gcross1,

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

@@ -68,11 +68,11 @@ impl VelocityGroundConstraint {
let mut rb2 = &bodies[manifold.data.body_pair.body2];
let flipped = !rb2.is_dynamic();
let force_dir1 = if flipped {
let (force_dir1, flipped_multiplier) = if flipped {
std::mem::swap(&mut rb1, &mut rb2);
manifold.data.normal
(manifold.data.normal, -1.0)
} else {
-manifold.data.normal
(-manifold.data.normal, 1.0)
};
let mj_lambda2 = rb2.active_set_offset;
@@ -123,7 +123,7 @@ impl VelocityGroundConstraint {
.as_nongrouped_ground_mut()
.unwrap()
} else {
unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable.
};
#[cfg(target_arch = "wasm32")]
@@ -179,7 +179,9 @@ impl VelocityGroundConstraint {
.effective_world_inv_inertia_sqrt
.transform_vector(dp2.gcross(-tangents1[j]));
let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
let rhs = (vel1 - vel2
+ flipped_multiplier * manifold_point.tangent_velocity)
.dot(&tangents1[j]);
#[cfg(feature = "dim2")]
let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff;
#[cfg(feature = "dim3")]

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

@@ -66,15 +66,17 @@ impl WVelocityGroundConstraint {
let inv_dt = SimdReal::splat(params.inv_dt());
let mut rbs1 = array![|ii| &bodies[manifolds[ii].data.body_pair.body1]; SIMD_WIDTH];
let mut rbs2 = array![|ii| &bodies[manifolds[ii].data.body_pair.body2]; SIMD_WIDTH];
let mut flipped = [false; SIMD_WIDTH];
let mut flipped = [1.0; SIMD_WIDTH];
for ii in 0..SIMD_WIDTH {
if !rbs2[ii].is_dynamic() {
std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]);
flipped[ii] = true;
flipped[ii] = -1.0;
}
}
let flipped_sign = SimdReal::from(flipped);
let im2 = SimdReal::from(array![|ii| rbs2[ii].effective_inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdReal> = AngularInertia::from(
array![|ii| rbs2[ii].effective_world_inv_inertia_sqrt; SIMD_WIDTH],
@@ -89,9 +91,8 @@ impl WVelocityGroundConstraint {
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 = Vector::from(
array![|ii| if flipped[ii] { manifolds[ii].data.normal } else { -manifolds[ii].data.normal }; SIMD_WIDTH],
);
let normal1 = Vector::from(array![|ii| manifolds[ii].data.normal; SIMD_WIDTH]);
let force_dir1 = normal1 * -flipped_sign;
let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
@@ -125,6 +126,8 @@ impl WVelocityGroundConstraint {
);
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 tangent_velocity =
Vector::from(array![|ii| manifold_points[ii][k].tangent_velocity; SIMD_WIDTH]);
let impulse =
SimdReal::from(array![|ii| manifold_points[ii][k].data.impulse; SIMD_WIDTH]);
@@ -170,7 +173,7 @@ impl WVelocityGroundConstraint {
let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j]));
let r = SimdReal::splat(1.0) / (im2 + gcross2.gdot(gcross2));
let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]);
let rhs = (vel1 - vel2 + tangent_velocity * flipped_sign).dot(&tangents1[j]);
constraint.elements[k].tangent_parts[j] =
WVelocityGroundConstraintElementPart {