diff options
| author | Emil Ernerfeldt <emil.ernerfeldt@gmail.com> | 2021-02-23 17:41:03 +0100 |
|---|---|---|
| committer | Emil Ernerfeldt <emil.ernerfeldt@gmail.com> | 2021-02-26 11:06:29 +0100 |
| commit | 9bbb0815397c48f6fa19421a127a77447f279a4b (patch) | |
| tree | 1247f0e35887346291362711546fb3327625d12e /src | |
| parent | 4ee09a8bc9e17d43e1dae75fbd02b069c7211637 (diff) | |
| download | rapier-9bbb0815397c48f6fa19421a127a77447f279a4b.tar.gz rapier-9bbb0815397c48f6fa19421a127a77447f279a4b.tar.bz2 rapier-9bbb0815397c48f6fa19421a127a77447f279a4b.zip | |
fix narrow revolute velocity
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs | 11 |
1 files changed, 5 insertions, 6 deletions
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 15869e2..839cddc 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -108,9 +108,8 @@ impl RevoluteVelocityConstraint { let axis1 = rb1.position * joint.local_axis1; let axis2 = rb2.position * joint.local_axis2; - let ang_err = - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); - let ang_err = ang_err.scaled_axis(); + let axis_error = axis1.cross(&axis2); + let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error); rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; @@ -416,9 +415,8 @@ impl RevoluteVelocityGroundConstraint { axis1 = rb1.position * joint.local_axis1; axis2 = rb2.position * joint.local_axis2; } - let ang_err = - Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity); - let ang_err = ang_err.scaled_axis(); + let axis_error = axis1.cross(&axis2); + let ang_err = basis2.tr_mul(&axis_error); rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; @@ -521,6 +519,7 @@ impl RevoluteVelocityGroundConstraint { .ii2_sqrt .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); } + fn solve_motors(&mut self, mj_lambda2: &mut DeltaVel<Real>) { if self.motor_inv_lhs != 0.0 { let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); |