From e4bf6914c788f02f47fd6a5f6d581df92f9c6b43 Mon Sep 17 00:00:00 2001
From: Dzmitry Malyshau <kvark@fastmail.com>
Date: Fri, 1 Dec 2023 23:34:10 -0800
Subject: Fix check for motor constraints on angular DoF

---
 .../solver/joint_constraint/joint_generic_velocity_constraint.rs      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
index 8c83f58..de5bbe6 100644
--- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
+++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
@@ -89,7 +89,7 @@ impl JointGenericVelocityConstraint {
 
         let start = len;
         for i in DIM..SPATIAL_DIM {
-            if (motor_axes >> DIM) & (1 << i) != 0 {
+            if motor_axes & (1 << i) != 0 {
                 out[len] = builder.motor_angular_generic(
                     params,
                     jacobians,
@@ -385,7 +385,7 @@ impl JointGenericVelocityGroundConstraint {
 
         let start = len;
         for i in DIM..SPATIAL_DIM {
-            if (motor_axes >> DIM) & (1 << i) != 0 {
+            if motor_axes & (1 << i) != 0 {
                 out[len] = builder.motor_angular_generic_ground(
                     params,
                     jacobians,
-- 
cgit