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

fix narrow revolute velocity

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-23 17:41:03 +01:00
parent 4ee09a8bc9
commit 9bbb081539
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs
View File

@@ -108,9 +108,8 @@ impl RevoluteVelocityConstraint {
let axis1 = rb1.position * joint.local_axis1; let axis1 = rb1.position * joint.local_axis1;
let axis2 = rb2.position * joint.local_axis2; let axis2 = rb2.position * joint.local_axis2;
let ang_err = let axis_error = axis1.cross(&axis2);
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error);
let ang_err = ang_err.scaled_axis();
rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
* velocity_based_erp_inv_dt; * velocity_based_erp_inv_dt;
@@ -416,9 +415,8 @@ impl RevoluteVelocityGroundConstraint {
axis1 = rb1.position * joint.local_axis1; axis1 = rb1.position * joint.local_axis1;
axis2 = rb2.position * joint.local_axis2; axis2 = rb2.position * joint.local_axis2;
} }
let ang_err = let axis_error = axis1.cross(&axis2);
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); let ang_err = basis2.tr_mul(&axis_error);
let ang_err = ang_err.scaled_axis();
rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y)
* velocity_based_erp_inv_dt; * velocity_based_erp_inv_dt;
@@ -521,6 +519,7 @@ impl RevoluteVelocityGroundConstraint {
.ii2_sqrt .ii2_sqrt
.transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); .transform_vector(ang_impulse + self.r2.gcross(lin_impulse));
} }
fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) { fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) {
if self.motor_inv_lhs != 0.0 { if self.motor_inv_lhs != 0.0 {
let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular);