diff options
| author | Emil Ernerfeldt <emil.ernerfeldt@gmail.com> | 2021-02-25 10:16:10 +0100 |
|---|---|---|
| committer | Emil Ernerfeldt <emil.ernerfeldt@gmail.com> | 2021-02-26 11:06:29 +0100 |
| commit | 115bae172d1f6bc8f26f6e499100ca6b437f3c87 (patch) | |
| tree | 43daf2eabaeecf2998d677185c08c7d8b18a260e /src | |
| parent | f517601e17089ba2af3a4909acc217076c3dcb11 (diff) | |
| download | rapier-115bae172d1f6bc8f26f6e499100ca6b437f3c87.tar.gz rapier-115bae172d1f6bc8f26f6e499100ca6b437f3c87.tar.bz2 rapier-115bae172d1f6bc8f26f6e499100ca6b437f3c87.zip | |
fix the body-body revolute angle velocity erp
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs | 2 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs | 3 |
2 files changed, 3 insertions, 2 deletions
diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs index 839cddc..23cd6b0 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -109,7 +109,7 @@ impl RevoluteVelocityConstraint { let axis2 = rb2.position * joint.local_axis2; let axis_error = axis1.cross(&axis2); - let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error); + let ang_err = (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * 0.5; rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs index ad25101..81e41dc 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -133,7 +133,8 @@ impl WRevoluteVelocityConstraint { let axis2 = position2 * local_axis2; let axis_error = axis1.cross(&axis2); - let ang_err = basis2.tr_mul(&axis_error) - basis1.tr_mul(&axis_error); + let ang_err = + (basis2.tr_mul(&axis_error) + basis1.tr_mul(&axis_error)) * SimdReal::splat(0.5); rhs += Vector5::new(lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y) * velocity_based_erp_inv_dt; |