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

fix the body-body revolute angle velocity erp

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-25 10:16:10 +01:00
parent f517601e17
commit 115bae172d
2 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -109,7 +109,7 @@ impl RevoluteVelocityConstraint {
let axis2 = rb2.position * joint.local_axis2; let axis2 = rb2.position * joint.local_axis2;
let axis_error = axis1.cross(&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) 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;

3
src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs
View File

@@ -133,7 +133,8 @@ impl WRevoluteVelocityConstraint {
let axis2 = position2 * local_axis2; let axis2 = position2 * local_axis2;
let axis_error = axis1.cross(&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) 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;