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

fix the revolute wide

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-23 17:49:33 +01:00
parent 9bbb081539
commit a0824772c9
1 changed files with 22 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -8,7 +8,7 @@ use crate::math::{
AngVector, AngularInertia, Isometry, Point, Real, Rotation, SimdReal, Vector, SIMD_WIDTH,
};
use crate::utils::{WAngularInertia, WCross, WCrossMatrix};
use na::{Cholesky, Matrix3x2, Matrix5, Vector3, Vector5, U2, U3};
use na::{Cholesky, Matrix3x2, Matrix5, Unit, Vector5, U2, U3};
#[derive(Debug)]
pub(crate) struct WRevoluteVelocityConstraint {
@@ -124,11 +124,16 @@ impl WRevoluteVelocityConstraint {
let lin_err = anchor2 - anchor1;
let ang_err = Vector3::<SimdReal>::from(array![|ii| {
let axis1 = rbs1[ii].position * joints[ii].local_axis1;
let axis2 = rbs2[ii].position * joints[ii].local_axis2;
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity).scaled_axis()
}; SIMD_WIDTH]);
let local_axis1 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);
let local_axis2 =
Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis2; SIMD_WIDTH]);
let axis1 = position1 * local_axis1;
let axis2 = position2 * local_axis2;
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;
@@ -397,11 +402,17 @@ impl WRevoluteVelocityGroundConstraint {
let lin_err = anchor2 - anchor1;
let ang_err = Vector3::<SimdReal>::from(array![|ii| {
let axis1 = rbs1[ii].position * if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 };
let axis2 = rbs2[ii].position * if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 };
Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or_else(Rotation::identity).scaled_axis()
}; SIMD_WIDTH]);
let local_axis1 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis2 } else { joints[ii].local_axis1 }; SIMD_WIDTH],
);
let local_axis2 = Unit::<Vector<_>>::from(
array![|ii| if flipped[ii] { joints[ii].local_axis1 } else { joints[ii].local_axis2 }; SIMD_WIDTH],
);
let axis1 = position1 * local_axis1;
let axis2 = position2 * local_axis2;
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;