diff options
| author | Sébastien Crozet <sebcrozet@dimforge.com> | 2024-03-23 10:15:07 +0100 |
|---|---|---|
| committer | Sébastien Crozet <sebastien@crozet.re> | 2024-03-23 10:34:01 +0100 |
| commit | b00113ed2f9e4824a254027b57c9b4a07c4c2307 (patch) | |
| tree | 1ef49e974804229992bab553e16c71e314305c04 /src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs | |
| parent | da92e5c2837b27433286cf0dd9d887fd44dda254 (diff) | |
| download | rapier-b00113ed2f9e4824a254027b57c9b4a07c4c2307.tar.gz rapier-b00113ed2f9e4824a254027b57c9b4a07c4c2307.tar.bz2 rapier-b00113ed2f9e4824a254027b57c9b4a07c4c2307.zip | |
fix: implement linear-coupled-motor constraint between two dynamic bodies
Fix #602
Diffstat (limited to 'src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs')
| -rw-r--r-- | src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs | 43 |
1 files changed, 21 insertions, 22 deletions
diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index e184e3d..60c42d3 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -217,28 +217,26 @@ impl JointTwoBodyConstraint<Real, 1> { } if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { - // if (motor_axes & !coupled_axes) & (1 << first_coupled_lin_axis_id) != 0 { - // let limits = if limit_axes & (1 << first_coupled_lin_axis_id) != 0 { - // Some([ - // joint.limits[first_coupled_lin_axis_id].min, - // joint.limits[first_coupled_lin_axis_id].max, - // ]) - // } else { - // None - // }; - // - // out[len] = builder.motor_linear_coupled - // params, - // [joint_id], - // body1, - // body2, - // coupled_axes, - // &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), - // limits, - // WritebackId::Motor(first_coupled_lin_axis_id), - // ); - // len += 1; - // } + let limits = if (limit_axes & (1 << first_coupled_lin_axis_id)) != 0 { + Some([ + joint.limits[first_coupled_lin_axis_id].min, + joint.limits[first_coupled_lin_axis_id].max, + ]) + } else { + None + }; + + out[len] = builder.motor_linear_coupled( + params, + [joint_id], + body1, + body2, + coupled_axes, + &joint.motors[first_coupled_lin_axis_id].motor_params(params.dt), + limits, + WritebackId::Motor(first_coupled_lin_axis_id), + ); + len += 1; } JointTwoBodyConstraintHelper::finalize_constraints(&mut out[start..len]); @@ -350,6 +348,7 @@ impl JointTwoBodyConstraint<Real, 1> { } } } + #[cfg(feature = "simd-is-enabled")] impl JointTwoBodyConstraint<SimdReal, SIMD_WIDTH> { pub fn lock_axes( |