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