The open source OpenXR runtime
mercury: Hopefully fix 1DOF joint limiting
+73
-3
+73
-3
src/xrt/tracking/hand/mercury/kine/kinematic_main.cpp
+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