The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

mercury: Hopefully fix 1DOF joint limiting

+73 -3
+73 -3
src/xrt/tracking/hand/mercury/kine/kinematic_main.cpp
··· 135 135 bone->bone_relation.linear() = bone->bone_relation.linear() * rot; 136 136 } 137 137 138 + #if 1 139 + static void 140 + clamp_to_x_axis(struct kinematic_hand_4f *hand, 141 + int finger_idx, 142 + int bone_idx, 143 + bool clamp_angle = false, 144 + float min_angle = 0, 145 + float max_angle = 0) 146 + { 147 + bone_t &bone = hand->fingers[finger_idx].bones[bone_idx]; 148 + // U_LOG_E("before_anything"); 149 + 150 + // std::cout << bone->bone_relation.linear().matrix() << std::endl; 151 + int axis = 0; 152 + Eigen::Vector3f axes[3] = {Eigen::Vector3f::UnitX(), Eigen::Vector3f::UnitY(), Eigen::Vector3f::UnitZ()}; 153 + 154 + 155 + // The input rotation will very likely rotate a vector pointed in the direction of axis we want to lock to... 156 + // somewhere else. So, we find the new direction 157 + Eigen::Vector3f axis_rotated_by_input = bone.bone_relation.linear() * axes[axis]; 158 + 159 + // And find a correction rotation that rotates our input rotation such that it doesn't affect vectors pointing 160 + // in the direction of our axis anymore. 161 + Eigen::Matrix3f correction_rot = 162 + Eigen::Quaternionf().setFromTwoVectors(axis_rotated_by_input.normalized(), axes[axis]).matrix(); 163 + bone.bone_relation.linear() = correction_rot * bone.bone_relation.linear(); 164 + 165 + 166 + if (!clamp_angle) { 167 + return; 168 + } 169 + 170 + Eigen::Vector3f axis_to_clamp_rotation = axes[(axis + 1) % 3]; 171 + 172 + Eigen::Vector3f new_ortho_direction = bone.bone_relation.linear() * axis_to_clamp_rotation; 173 + float rotation_value = atan2(new_ortho_direction((axis + 2) % 3), new_ortho_direction((axis + 1) % 3)); 174 + 175 + 176 + 177 + if (rotation_value < max_angle && rotation_value > min_angle) { 178 + return; 179 + } 180 + 181 + float positive_rotation_value, negative_rotation_value; 182 + 183 + 184 + if (rotation_value < 0) { 185 + positive_rotation_value = (M_PI * 2) - rotation_value; 186 + negative_rotation_value = rotation_value; 187 + } else { 188 + negative_rotation_value = (-M_PI * 2) + rotation_value; 189 + positive_rotation_value = rotation_value; 190 + } 191 + 192 + if ((positive_rotation_value - max_angle) > (min_angle - negative_rotation_value)) { 193 + // Difference between max angle and positive value is higher, so we're closer to the minimum bound. 194 + rotation_value = min_angle; 195 + } else { 196 + rotation_value = max_angle; 197 + } 198 + 199 + bone.bone_relation.linear() = Eigen::AngleAxisf(rotation_value, axes[axis]).toRotationMatrix(); 200 + } 201 + 202 + #else 138 203 static void 139 204 clamp_to_x_axis(struct kinematic_hand_4f *hand, 140 205 int finger_idx, ··· 201 266 // No, the sine of the joint limit 202 267 float rotation_value = asin(cross(0)); 203 268 204 - //!@bug No. If the rotation value is outside of the allowed values, clamp it to the rotation it's *closest to*. 269 + //!@bug No. If the rotation value is outside of the allowed values, clamp it to the rotation it's 270 + //!*closest to*. 205 271 // That's different than using clamp, rotation formalisms are complicated. 206 272 clamp(&rotation_value, min_angle, max_angle); 207 273 ··· 216 282 // std::cout << n << "\n"; 217 283 } 218 284 } 285 + #endif 286 + 219 287 220 288 // Is this not just swing-twist about the Z axis? Dunnoooo... Find out later. 221 289 static void ··· 460 528 } 461 529 462 530 void 463 - alloc_kinematic_hand(kinematic_hand_4f **out_kinematic_hand) { 531 + alloc_kinematic_hand(kinematic_hand_4f **out_kinematic_hand) 532 + { 464 533 kinematic_hand_4f *h = new kinematic_hand_4f; 465 534 *out_kinematic_hand = h; 466 535 } 467 536 468 537 void 469 - free_kinematic_hand(kinematic_hand_4f **kinematic_hand) { 538 + free_kinematic_hand(kinematic_hand_4f **kinematic_hand) 539 + { 470 540 delete *kinematic_hand; 471 541 } 472 542