1/**************************************************************************/
2/* skeleton_ik_3d.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
10/* */
11/* Permission is hereby granted, free of charge, to any person obtaining */
12/* a copy of this software and associated documentation files (the */
13/* "Software"), to deal in the Software without restriction, including */
14/* without limitation the rights to use, copy, modify, merge, publish, */
15/* distribute, sublicense, and/or sell copies of the Software, and to */
16/* permit persons to whom the Software is furnished to do so, subject to */
17/* the following conditions: */
18/* */
19/* The above copyright notice and this permission notice shall be */
20/* included in all copies or substantial portions of the Software. */
21/* */
22/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
25/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29/**************************************************************************/
30
31#include "skeleton_ik_3d.h"
32
33#ifndef _3D_DISABLED
34
35FabrikInverseKinematic::ChainItem *FabrikInverseKinematic::ChainItem::find_child(const BoneId p_bone_id) {
36 for (int i = children.size() - 1; 0 <= i; --i) {
37 if (p_bone_id == children[i].bone) {
38 return &children.write[i];
39 }
40 }
41 return nullptr;
42}
43
44FabrikInverseKinematic::ChainItem *FabrikInverseKinematic::ChainItem::add_child(const BoneId p_bone_id) {
45 const int infant_child_id = children.size();
46 children.resize(infant_child_id + 1);
47 children.write[infant_child_id].bone = p_bone_id;
48 children.write[infant_child_id].parent_item = this;
49 return &children.write[infant_child_id];
50}
51
52/// Build a chain that starts from the root to tip
53bool FabrikInverseKinematic::build_chain(Task *p_task, bool p_force_simple_chain) {
54 ERR_FAIL_COND_V(-1 == p_task->root_bone, false);
55
56 Chain &chain(p_task->chain);
57
58 chain.tips.resize(p_task->end_effectors.size());
59 chain.chain_root.bone = p_task->root_bone;
60 chain.chain_root.initial_transform = p_task->skeleton->get_bone_global_pose(chain.chain_root.bone);
61 chain.chain_root.current_pos = chain.chain_root.initial_transform.origin;
62 chain.middle_chain_item = nullptr;
63
64 // Holds all IDs that are composing a single chain in reverse order
65 Vector<BoneId> chain_ids;
66 // This is used to know the chain size
67 int sub_chain_size;
68 // Resize only one time in order to fit all joints for performance reason
69 chain_ids.resize(p_task->skeleton->get_bone_count());
70
71 for (int x = p_task->end_effectors.size() - 1; 0 <= x; --x) {
72 const EndEffector *ee(&p_task->end_effectors[x]);
73 ERR_FAIL_COND_V(p_task->root_bone >= ee->tip_bone, false);
74 ERR_FAIL_INDEX_V(ee->tip_bone, p_task->skeleton->get_bone_count(), false);
75
76 sub_chain_size = 0;
77 // Picks all IDs that composing a single chain in reverse order (except the root)
78 BoneId chain_sub_tip(ee->tip_bone);
79 while (chain_sub_tip > p_task->root_bone) {
80 chain_ids.write[sub_chain_size++] = chain_sub_tip;
81 chain_sub_tip = p_task->skeleton->get_bone_parent(chain_sub_tip);
82 }
83
84 BoneId middle_chain_item_id = (BoneId)(sub_chain_size * 0.5);
85
86 // Build chain by reading chain ids in reverse order
87 // For each chain item id will be created a ChainItem if doesn't exists
88 ChainItem *sub_chain(&chain.chain_root);
89 for (int i = sub_chain_size - 1; 0 <= i; --i) {
90 ChainItem *child_ci(sub_chain->find_child(chain_ids[i]));
91 if (!child_ci) {
92 child_ci = sub_chain->add_child(chain_ids[i]);
93
94 child_ci->initial_transform = p_task->skeleton->get_bone_global_pose(child_ci->bone);
95 child_ci->current_pos = child_ci->initial_transform.origin;
96
97 if (child_ci->parent_item) {
98 child_ci->length = child_ci->parent_item->current_pos.distance_to(child_ci->current_pos);
99 }
100 }
101
102 sub_chain = child_ci;
103
104 if (middle_chain_item_id == i) {
105 chain.middle_chain_item = child_ci;
106 }
107 }
108
109 if (!middle_chain_item_id) {
110 chain.middle_chain_item = nullptr;
111 }
112
113 // Initialize current tip
114 chain.tips.write[x].chain_item = sub_chain;
115 chain.tips.write[x].end_effector = ee;
116
117 if (p_force_simple_chain) {
118 // NOTE:
119 // This is a "hack" that force to create only one tip per chain since the solver of multi tip (end effector)
120 // is not yet created.
121 // Remove this code when this is done
122 break;
123 }
124 }
125 return true;
126}
127
128void FabrikInverseKinematic::solve_simple(Task *p_task, bool p_solve_magnet, Vector3 p_origin_pos) {
129 real_t distance_to_goal(1e4);
130 real_t previous_distance_to_goal(0);
131 int can_solve(p_task->max_iterations);
132 while (distance_to_goal > p_task->min_distance && Math::abs(previous_distance_to_goal - distance_to_goal) > 0.005 && can_solve) {
133 previous_distance_to_goal = distance_to_goal;
134 --can_solve;
135
136 solve_simple_backwards(p_task->chain, p_solve_magnet);
137 solve_simple_forwards(p_task->chain, p_solve_magnet, p_origin_pos);
138
139 distance_to_goal = p_task->chain.tips[0].end_effector->goal_transform.origin.distance_to(p_task->chain.tips[0].chain_item->current_pos);
140 }
141}
142
143void FabrikInverseKinematic::solve_simple_backwards(const Chain &r_chain, bool p_solve_magnet) {
144 if (p_solve_magnet && !r_chain.middle_chain_item) {
145 return;
146 }
147
148 Vector3 goal;
149 ChainItem *sub_chain_tip;
150 if (p_solve_magnet) {
151 goal = r_chain.magnet_position;
152 sub_chain_tip = r_chain.middle_chain_item;
153 } else {
154 goal = r_chain.tips[0].end_effector->goal_transform.origin;
155 sub_chain_tip = r_chain.tips[0].chain_item;
156 }
157
158 while (sub_chain_tip) {
159 sub_chain_tip->current_pos = goal;
160
161 if (sub_chain_tip->parent_item) {
162 // Not yet in the chain root
163 // So calculate next goal location
164
165 const Vector3 look_parent((sub_chain_tip->parent_item->current_pos - sub_chain_tip->current_pos).normalized());
166 goal = sub_chain_tip->current_pos + (look_parent * sub_chain_tip->length);
167
168 // [TODO] Constraints goes here
169 }
170
171 sub_chain_tip = sub_chain_tip->parent_item;
172 }
173}
174
175void FabrikInverseKinematic::solve_simple_forwards(Chain &r_chain, bool p_solve_magnet, Vector3 p_origin_pos) {
176 if (p_solve_magnet && !r_chain.middle_chain_item) {
177 return;
178 }
179
180 ChainItem *sub_chain_root(&r_chain.chain_root);
181 Vector3 origin = p_origin_pos;
182
183 while (sub_chain_root) { // Reach the tip
184 sub_chain_root->current_pos = origin;
185
186 if (!sub_chain_root->children.is_empty()) {
187 ChainItem &child(sub_chain_root->children.write[0]);
188
189 // Is not tip
190 // So calculate next origin location
191
192 // Look child
193 sub_chain_root->current_ori = (child.current_pos - sub_chain_root->current_pos).normalized();
194 origin = sub_chain_root->current_pos + (sub_chain_root->current_ori * child.length);
195
196 // [TODO] Constraints goes here
197
198 if (p_solve_magnet && sub_chain_root == r_chain.middle_chain_item) {
199 // In case of magnet solving this is the tip
200 sub_chain_root = nullptr;
201 } else {
202 sub_chain_root = &child;
203 }
204 } else {
205 // Is tip
206 sub_chain_root = nullptr;
207 }
208 }
209}
210
211FabrikInverseKinematic::Task *FabrikInverseKinematic::create_simple_task(Skeleton3D *p_sk, BoneId root_bone, BoneId tip_bone, const Transform3D &goal_transform) {
212 FabrikInverseKinematic::EndEffector ee;
213 ee.tip_bone = tip_bone;
214
215 Task *task(memnew(Task));
216 task->skeleton = p_sk;
217 task->root_bone = root_bone;
218 task->end_effectors.push_back(ee);
219 task->goal_global_transform = goal_transform;
220
221 if (!build_chain(task)) {
222 free_task(task);
223 return nullptr;
224 }
225
226 return task;
227}
228
229void FabrikInverseKinematic::free_task(Task *p_task) {
230 if (p_task) {
231 memdelete(p_task);
232 }
233}
234
235void FabrikInverseKinematic::set_goal(Task *p_task, const Transform3D &p_goal) {
236 p_task->goal_global_transform = p_goal;
237}
238
239void FabrikInverseKinematic::make_goal(Task *p_task, const Transform3D &p_inverse_transf, real_t blending_delta) {
240 if (blending_delta >= 0.99f) {
241 // Update the end_effector (local transform) without blending
242 p_task->end_effectors.write[0].goal_transform = p_inverse_transf * p_task->goal_global_transform;
243 } else {
244 // End effector in local transform
245 const Transform3D end_effector_pose(p_task->skeleton->get_bone_global_pose_no_override(p_task->end_effectors[0].tip_bone));
246
247 // Update the end_effector (local transform) by blending with current pose
248 p_task->end_effectors.write[0].goal_transform = end_effector_pose.interpolate_with(p_inverse_transf * p_task->goal_global_transform, blending_delta);
249 }
250}
251
252void FabrikInverseKinematic::solve(Task *p_task, real_t blending_delta, bool override_tip_basis, bool p_use_magnet, const Vector3 &p_magnet_position) {
253 if (blending_delta <= 0.01f) {
254 // Before skipping, make sure we undo the global pose overrides
255 ChainItem *ci(&p_task->chain.chain_root);
256 while (ci) {
257 p_task->skeleton->set_bone_global_pose_override(ci->bone, ci->initial_transform, 0.0, false);
258
259 if (!ci->children.is_empty()) {
260 ci = &ci->children.write[0];
261 } else {
262 ci = nullptr;
263 }
264 }
265
266 return; // Skip solving
267 }
268
269 // Update the initial root transform so its synced with any animation changes
270 _update_chain(p_task->skeleton, &p_task->chain.chain_root);
271
272 p_task->skeleton->set_bone_global_pose_override(p_task->chain.chain_root.bone, Transform3D(), 0.0, false);
273 Vector3 origin_pos = p_task->skeleton->get_bone_global_pose(p_task->chain.chain_root.bone).origin;
274
275 make_goal(p_task, p_task->skeleton->get_global_transform().affine_inverse(), blending_delta);
276
277 if (p_use_magnet && p_task->chain.middle_chain_item) {
278 p_task->chain.magnet_position = p_task->chain.middle_chain_item->initial_transform.origin.lerp(p_magnet_position, blending_delta);
279 solve_simple(p_task, true, origin_pos);
280 }
281 solve_simple(p_task, false, origin_pos);
282
283 // Assign new bone position.
284 ChainItem *ci(&p_task->chain.chain_root);
285 while (ci) {
286 Transform3D new_bone_pose(ci->initial_transform);
287 new_bone_pose.origin = ci->current_pos;
288
289 if (!ci->children.is_empty()) {
290 Vector3 forward_vector = (ci->children[0].initial_transform.origin - ci->initial_transform.origin).normalized();
291 // Rotate the bone towards the next bone in the chain:
292 new_bone_pose.basis.rotate_to_align(forward_vector, new_bone_pose.origin.direction_to(ci->children[0].current_pos));
293
294 } else {
295 // Set target orientation to tip
296 if (override_tip_basis) {
297 new_bone_pose.basis = p_task->chain.tips[0].end_effector->goal_transform.basis;
298 } else {
299 new_bone_pose.basis = new_bone_pose.basis * p_task->chain.tips[0].end_effector->goal_transform.basis;
300 }
301 }
302
303 // IK should not affect scale, so undo any scaling
304 new_bone_pose.basis.orthonormalize();
305 new_bone_pose.basis.scale(p_task->skeleton->get_bone_global_pose(ci->bone).basis.get_scale());
306
307 p_task->skeleton->set_bone_global_pose_override(ci->bone, new_bone_pose, 1.0, true);
308
309 if (!ci->children.is_empty()) {
310 ci = &ci->children.write[0];
311 } else {
312 ci = nullptr;
313 }
314 }
315}
316
317void FabrikInverseKinematic::_update_chain(const Skeleton3D *p_sk, ChainItem *p_chain_item) {
318 if (!p_chain_item) {
319 return;
320 }
321
322 p_chain_item->initial_transform = p_sk->get_bone_global_pose_no_override(p_chain_item->bone);
323 p_chain_item->current_pos = p_chain_item->initial_transform.origin;
324
325 ChainItem *items = p_chain_item->children.ptrw();
326 for (int i = 0; i < p_chain_item->children.size(); i += 1) {
327 _update_chain(p_sk, items + i);
328 }
329}
330
331void SkeletonIK3D::_validate_property(PropertyInfo &p_property) const {
332 if (p_property.name == "root_bone" || p_property.name == "tip_bone") {
333 Skeleton3D *skeleton = get_parent_skeleton();
334 if (skeleton) {
335 String names("--,");
336 for (int i = 0; i < skeleton->get_bone_count(); i++) {
337 if (i > 0) {
338 names += ",";
339 }
340 names += skeleton->get_bone_name(i);
341 }
342
343 p_property.hint = PROPERTY_HINT_ENUM;
344 p_property.hint_string = names;
345 } else {
346 p_property.hint = PROPERTY_HINT_NONE;
347 p_property.hint_string = "";
348 }
349 }
350}
351
352void SkeletonIK3D::_bind_methods() {
353 ClassDB::bind_method(D_METHOD("set_root_bone", "root_bone"), &SkeletonIK3D::set_root_bone);
354 ClassDB::bind_method(D_METHOD("get_root_bone"), &SkeletonIK3D::get_root_bone);
355
356 ClassDB::bind_method(D_METHOD("set_tip_bone", "tip_bone"), &SkeletonIK3D::set_tip_bone);
357 ClassDB::bind_method(D_METHOD("get_tip_bone"), &SkeletonIK3D::get_tip_bone);
358
359 ClassDB::bind_method(D_METHOD("set_interpolation", "interpolation"), &SkeletonIK3D::set_interpolation);
360 ClassDB::bind_method(D_METHOD("get_interpolation"), &SkeletonIK3D::get_interpolation);
361
362 ClassDB::bind_method(D_METHOD("set_target_transform", "target"), &SkeletonIK3D::set_target_transform);
363 ClassDB::bind_method(D_METHOD("get_target_transform"), &SkeletonIK3D::get_target_transform);
364
365 ClassDB::bind_method(D_METHOD("set_target_node", "node"), &SkeletonIK3D::set_target_node);
366 ClassDB::bind_method(D_METHOD("get_target_node"), &SkeletonIK3D::get_target_node);
367
368 ClassDB::bind_method(D_METHOD("set_override_tip_basis", "override"), &SkeletonIK3D::set_override_tip_basis);
369 ClassDB::bind_method(D_METHOD("is_override_tip_basis"), &SkeletonIK3D::is_override_tip_basis);
370
371 ClassDB::bind_method(D_METHOD("set_use_magnet", "use"), &SkeletonIK3D::set_use_magnet);
372 ClassDB::bind_method(D_METHOD("is_using_magnet"), &SkeletonIK3D::is_using_magnet);
373
374 ClassDB::bind_method(D_METHOD("set_magnet_position", "local_position"), &SkeletonIK3D::set_magnet_position);
375 ClassDB::bind_method(D_METHOD("get_magnet_position"), &SkeletonIK3D::get_magnet_position);
376
377 ClassDB::bind_method(D_METHOD("get_parent_skeleton"), &SkeletonIK3D::get_parent_skeleton);
378 ClassDB::bind_method(D_METHOD("is_running"), &SkeletonIK3D::is_running);
379
380 ClassDB::bind_method(D_METHOD("set_min_distance", "min_distance"), &SkeletonIK3D::set_min_distance);
381 ClassDB::bind_method(D_METHOD("get_min_distance"), &SkeletonIK3D::get_min_distance);
382
383 ClassDB::bind_method(D_METHOD("set_max_iterations", "iterations"), &SkeletonIK3D::set_max_iterations);
384 ClassDB::bind_method(D_METHOD("get_max_iterations"), &SkeletonIK3D::get_max_iterations);
385
386 ClassDB::bind_method(D_METHOD("start", "one_time"), &SkeletonIK3D::start, DEFVAL(false));
387 ClassDB::bind_method(D_METHOD("stop"), &SkeletonIK3D::stop);
388
389 ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "root_bone"), "set_root_bone", "get_root_bone");
390 ADD_PROPERTY(PropertyInfo(Variant::STRING_NAME, "tip_bone"), "set_tip_bone", "get_tip_bone");
391 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "interpolation", PROPERTY_HINT_RANGE, "0,1,0.001"), "set_interpolation", "get_interpolation");
392 ADD_PROPERTY(PropertyInfo(Variant::TRANSFORM3D, "target", PROPERTY_HINT_NONE, "suffix:m"), "set_target_transform", "get_target_transform");
393 ADD_PROPERTY(PropertyInfo(Variant::BOOL, "override_tip_basis"), "set_override_tip_basis", "is_override_tip_basis");
394 ADD_PROPERTY(PropertyInfo(Variant::BOOL, "use_magnet"), "set_use_magnet", "is_using_magnet");
395 ADD_PROPERTY(PropertyInfo(Variant::VECTOR3, "magnet", PROPERTY_HINT_NONE, "suffix:m"), "set_magnet_position", "get_magnet_position");
396 ADD_PROPERTY(PropertyInfo(Variant::NODE_PATH, "target_node"), "set_target_node", "get_target_node");
397 ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "min_distance", PROPERTY_HINT_NONE, "suffix:m"), "set_min_distance", "get_min_distance");
398 ADD_PROPERTY(PropertyInfo(Variant::INT, "max_iterations"), "set_max_iterations", "get_max_iterations");
399}
400
401void SkeletonIK3D::_notification(int p_what) {
402 switch (p_what) {
403 case NOTIFICATION_ENTER_TREE: {
404 skeleton_ref = Object::cast_to<Skeleton3D>(get_parent());
405 set_process_priority(1);
406 reload_chain();
407 } break;
408
409 case NOTIFICATION_INTERNAL_PROCESS: {
410 if (target_node_override_ref) {
411 reload_goal();
412 }
413 _solve_chain();
414 } break;
415
416 case NOTIFICATION_EXIT_TREE: {
417 reload_chain();
418 } break;
419 }
420}
421
422SkeletonIK3D::SkeletonIK3D() {
423}
424
425SkeletonIK3D::~SkeletonIK3D() {
426 FabrikInverseKinematic::free_task(task);
427 task = nullptr;
428}
429
430void SkeletonIK3D::set_root_bone(const StringName &p_root_bone) {
431 root_bone = p_root_bone;
432 reload_chain();
433}
434
435StringName SkeletonIK3D::get_root_bone() const {
436 return root_bone;
437}
438
439void SkeletonIK3D::set_tip_bone(const StringName &p_tip_bone) {
440 tip_bone = p_tip_bone;
441 reload_chain();
442}
443
444StringName SkeletonIK3D::get_tip_bone() const {
445 return tip_bone;
446}
447
448void SkeletonIK3D::set_interpolation(real_t p_interpolation) {
449 interpolation = p_interpolation;
450}
451
452real_t SkeletonIK3D::get_interpolation() const {
453 return interpolation;
454}
455
456void SkeletonIK3D::set_target_transform(const Transform3D &p_target) {
457 target = p_target;
458 reload_goal();
459}
460
461const Transform3D &SkeletonIK3D::get_target_transform() const {
462 return target;
463}
464
465void SkeletonIK3D::set_target_node(const NodePath &p_node) {
466 target_node_path_override = p_node;
467 target_node_override_ref = Variant();
468 reload_goal();
469}
470
471NodePath SkeletonIK3D::get_target_node() {
472 return target_node_path_override;
473}
474
475void SkeletonIK3D::set_override_tip_basis(bool p_override) {
476 override_tip_basis = p_override;
477}
478
479bool SkeletonIK3D::is_override_tip_basis() const {
480 return override_tip_basis;
481}
482
483void SkeletonIK3D::set_use_magnet(bool p_use) {
484 use_magnet = p_use;
485}
486
487bool SkeletonIK3D::is_using_magnet() const {
488 return use_magnet;
489}
490
491void SkeletonIK3D::set_magnet_position(const Vector3 &p_local_position) {
492 magnet_position = p_local_position;
493}
494
495const Vector3 &SkeletonIK3D::get_magnet_position() const {
496 return magnet_position;
497}
498
499void SkeletonIK3D::set_min_distance(real_t p_min_distance) {
500 min_distance = p_min_distance;
501}
502
503void SkeletonIK3D::set_max_iterations(int p_iterations) {
504 max_iterations = p_iterations;
505}
506
507Skeleton3D *SkeletonIK3D::get_parent_skeleton() const {
508 return cast_to<Skeleton3D>(skeleton_ref.get_validated_object());
509}
510
511bool SkeletonIK3D::is_running() {
512 return is_processing_internal();
513}
514
515void SkeletonIK3D::start(bool p_one_time) {
516 if (p_one_time) {
517 set_process_internal(false);
518
519 if (target_node_override_ref) {
520 reload_goal();
521 }
522
523 _solve_chain();
524 } else {
525 set_process_internal(true);
526 }
527}
528
529void SkeletonIK3D::stop() {
530 set_process_internal(false);
531 Skeleton3D *skeleton = get_parent_skeleton();
532 if (skeleton) {
533 skeleton->clear_bones_global_pose_override();
534 }
535}
536
537Transform3D SkeletonIK3D::_get_target_transform() {
538 if (!target_node_override_ref && !target_node_path_override.is_empty()) {
539 target_node_override_ref = Object::cast_to<Node3D>(get_node(target_node_path_override));
540 }
541
542 Node3D *target_node_override = cast_to<Node3D>(target_node_override_ref.get_validated_object());
543 if (target_node_override && target_node_override->is_inside_tree()) {
544 return target_node_override->get_global_transform();
545 } else {
546 return target;
547 }
548}
549
550void SkeletonIK3D::reload_chain() {
551 FabrikInverseKinematic::free_task(task);
552 task = nullptr;
553
554 Skeleton3D *skeleton = get_parent_skeleton();
555 if (!skeleton) {
556 return;
557 }
558
559 task = FabrikInverseKinematic::create_simple_task(skeleton, skeleton->find_bone(root_bone), skeleton->find_bone(tip_bone), _get_target_transform());
560 if (task) {
561 task->max_iterations = max_iterations;
562 task->min_distance = min_distance;
563 }
564}
565
566void SkeletonIK3D::reload_goal() {
567 if (!task) {
568 return;
569 }
570
571 FabrikInverseKinematic::set_goal(task, _get_target_transform());
572}
573
574void SkeletonIK3D::_solve_chain() {
575 if (!task) {
576 return;
577 }
578 FabrikInverseKinematic::solve(task, interpolation, override_tip_basis, use_magnet, magnet_position);
579}
580
581#endif // _3D_DISABLED
582