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

fix

Browse Source
This commit is contained in:
Emil Ernerfeldt
2021-02-19 14:41:11 +01:00
parent b94cdfa782
commit 59796e4767
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs
View File

@@ -113,18 +113,18 @@ impl FixedVelocityConstraint {
let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt();
if velocity_based_erp_inv_dt != 0.0 { if velocity_based_erp_inv_dt != 0.0 {
let error = anchor2 * anchor1.inverse(); let lin_err = anchor2.translation.vector - anchor1.translation.vector;
let lin_err = error.translation.vector; let ang_err = anchor2.rotation * anchor1.rotation.inverse();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
{ {
let ang_err = error.rotation.angle(); let ang_err = ang_err.angle();
rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt; rhs += Vector3::new(lin_err.x, lin_err.y, ang_err) * velocity_based_erp_inv_dt;
} }
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
{ {
let ang_err = error.rotation.scaled_axis(); let ang_err = ang_err.scaled_axis();
rhs += Vector6::new( rhs += Vector6::new(
lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z, lin_err.x, lin_err.y, lin_err.z, ang_err.x, ang_err.y, ang_err.z,
) * velocity_based_erp_inv_dt; ) * velocity_based_erp_inv_dt;