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

Constraint solver: properly take non-zero center of masses into account.

Browse Source
This commit is contained in:
Sébastien Crozet
2020-08-31 19:02:37 +02:00
parent df2156ffd0
commit c286f44c4e
4 changed files with 14 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -215,10 +215,8 @@ 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 = (rb1.position * manifold_point.local_p1).coords let dp1 = (rb1.position * manifold_point.local_p1) - rb1.world_com;
- rb1.position.translation.vector; let dp2 = (rb2.position * manifold_point.local_p2) - rb2.world_com;
let dp2 = (rb2.position * manifold_point.local_p2).coords
- rb2.position.translation.vector;
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);
@@ -355,7 +353,7 @@ impl VelocityConstraint {
} }
} }
// Solve penetration. // Solve non-penetration.
for i in 0..self.num_contacts as usize { for i in 0..self.num_contacts as usize {
let elt = &mut self.elements[i].normal_part; let elt = &mut self.elements[i].normal_part;
let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular) let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)

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

@@ -80,6 +80,7 @@ impl WVelocityConstraint {
let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
let ii2: AngularInertia<SimdFloat> = let ii2: AngularInertia<SimdFloat> =
@@ -89,6 +90,7 @@ impl WVelocityConstraint {
let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
@@ -130,8 +132,8 @@ impl WVelocityConstraint {
let impulse = let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
let dp1 = p1.coords - position1.translation.vector; let dp1 = p1 - world_com1;
let dp2 = p2.coords - position2.translation.vector; let dp2 = p2 - 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);

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

@@ -155,8 +155,8 @@ impl VelocityGroundConstraint {
rb2.position * manifold_point.local_p2, rb2.position * manifold_point.local_p2,
) )
}; };
let dp2 = p2.coords - rb2.position.translation.vector; let dp2 = p2 - rb2.world_com;
let dp1 = p1.coords - rb1.position.translation.vector; 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);

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

@@ -89,6 +89,9 @@ impl WVelocityGroundConstraint {
let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); let position2 = Isometry::from(array![|ii| rbs2[ii].position; 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 force_dir1 = position1 let force_dir1 = position1
* -Vector::from( * -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].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
@@ -130,8 +133,8 @@ impl WVelocityGroundConstraint {
let impulse = let impulse =
SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
let dp1 = p1.coords - position1.translation.vector; let dp1 = p1 - world_com1;
let dp2 = p2.coords - position2.translation.vector; let dp2 = p2 - 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);