diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs | 33 |
1 files changed, 22 insertions, 11 deletions
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 4404dd1..ad25101 100644 --- a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -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; |