1 //
2 // Copyright (c) 2008-2016 the Urho3D project.
3 //
4 // Permission is hereby granted, free of charge, to any person obtaining a copy
5 // of this software and associated documentation files (the "Software"), to deal
6 // in the Software without restriction, including without limitation the rights
7 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
8 // copies of the Software, and to permit persons to whom the Software is
9 // furnished to do so, subject to the following conditions:
10 //
11 // The above copyright notice and this permission notice shall be included in
12 // all copies or substantial portions of the Software.
13 //
14 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
17 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
18 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
19 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
20 // THE SOFTWARE.
21 //
22
23 #include "../IK/IKSolver.h"
24 #include "../IK/IKConstraint.h"
25 #include "../IK/IKEvents.h"
26 #include "../IK/IKEffector.h"
27 #include "../IK/IKConverters.h"
28
29 #include "../Core/Context.h"
30 #include "../Core/Profiler.h"
31 #include "../Graphics/Animation.h"
32 #include "../Graphics/AnimationState.h"
33 #include "../Graphics/DebugRenderer.h"
34 #include "../IO/Log.h"
35 #include "../Scene/SceneEvents.h"
36
37 #include <ik/effector.h>
38 #include <ik/node.h>
39 #include <ik/solver.h>
40 #include <ik/util.h>
41
42 namespace Urho3D
43 {
44
45 extern const char* IK_CATEGORY;
46
47 // ----------------------------------------------------------------------------
IKSolver(Context * context)48 IKSolver::IKSolver(Context* context) :
49 Component(context),
50 solver_(NULL),
51 algorithm_(FABRIK),
52 features_(AUTO_SOLVE | JOINT_ROTATIONS | UPDATE_ACTIVE_POSE),
53 chainTreesNeedUpdating_(false),
54 treeNeedsRebuild(true),
55 solverTreeValid_(false)
56 {
57 context_->RequireIK();
58
59 SetAlgorithm(FABRIK);
60
61 SubscribeToEvent(E_COMPONENTADDED, URHO3D_HANDLER(IKSolver, HandleComponentAdded));
62 SubscribeToEvent(E_COMPONENTREMOVED, URHO3D_HANDLER(IKSolver, HandleComponentRemoved));
63 SubscribeToEvent(E_NODEADDED, URHO3D_HANDLER(IKSolver, HandleNodeAdded));
64 SubscribeToEvent(E_NODEREMOVED, URHO3D_HANDLER(IKSolver, HandleNodeRemoved));
65 }
66
67 // ----------------------------------------------------------------------------
~IKSolver()68 IKSolver::~IKSolver()
69 {
70 // Destroying the solver tree will destroy the effector objects, so remove
71 // any references any of the IKEffector objects could be holding
72 for (PODVector<IKEffector*>::ConstIterator it = effectorList_.Begin(); it != effectorList_.End(); ++it)
73 (*it)->SetIKEffectorNode(NULL);
74
75 ik_solver_destroy(solver_);
76 context_->ReleaseIK();
77 }
78
79 // ----------------------------------------------------------------------------
RegisterObject(Context * context)80 void IKSolver::RegisterObject(Context* context)
81 {
82 context->RegisterFactory<IKSolver>(IK_CATEGORY);
83
84 static const char* algorithmNames[] = {
85 "1 Bone",
86 "2 Bone",
87 "FABRIK",
88 /* not implemented,
89 "MSD (Mass/Spring/Damper)",
90 "Jacobian Inverse",
91 "Jacobian Transpose",*/
92 NULL
93 };
94
95 URHO3D_ENUM_ACCESSOR_ATTRIBUTE("Algorithm", GetAlgorithm, SetAlgorithm, Algorithm, algorithmNames, FABRIK, AM_DEFAULT);
96 URHO3D_ACCESSOR_ATTRIBUTE("Max Iterations", GetMaximumIterations, SetMaximumIterations, unsigned, 20, AM_DEFAULT);
97 URHO3D_ACCESSOR_ATTRIBUTE("Convergence Tolerance", GetTolerance, SetTolerance, float, 0.001, AM_DEFAULT);
98 URHO3D_ACCESSOR_ATTRIBUTE("Joint Rotations", GetJOINT_ROTATIONS, SetJOINT_ROTATIONS, bool, true, AM_DEFAULT);
99 URHO3D_ACCESSOR_ATTRIBUTE("Target Rotations", GetTARGET_ROTATIONS, SetTARGET_ROTATIONS, bool, false, AM_DEFAULT);
100 URHO3D_ACCESSOR_ATTRIBUTE("Update Original Pose", GetUPDATE_ORIGINAL_POSE, SetUPDATE_ORIGINAL_POSE, bool, false, AM_DEFAULT);
101 URHO3D_ACCESSOR_ATTRIBUTE("Update Active Pose", GetUPDATE_ACTIVE_POSE, SetUPDATE_ACTIVE_POSE, bool, true, AM_DEFAULT);
102 URHO3D_ACCESSOR_ATTRIBUTE("Use Original Pose", GetUSE_ORIGINAL_POSE, SetUSE_ORIGINAL_POSE, bool, false, AM_DEFAULT);
103 URHO3D_ACCESSOR_ATTRIBUTE("Enable Constraints", GetCONSTRAINTS, SetCONSTRAINTS, bool, false, AM_DEFAULT);
104 URHO3D_ACCESSOR_ATTRIBUTE("Auto Solve", GetAUTO_SOLVE, SetAUTO_SOLVE, bool, true, AM_DEFAULT);
105 }
106
107 // ----------------------------------------------------------------------------
GetAlgorithm() const108 IKSolver::Algorithm IKSolver::GetAlgorithm() const
109 {
110 return algorithm_;
111 }
112
113 // ----------------------------------------------------------------------------
SetAlgorithm(IKSolver::Algorithm algorithm)114 void IKSolver::SetAlgorithm(IKSolver::Algorithm algorithm)
115 {
116 algorithm_ = algorithm;
117
118 /* We need to rebuild the tree so make sure that the scene is in the
119 * initial pose when this occurs.*/
120 if (node_ != NULL)
121 ApplyOriginalPoseToScene();
122
123 // Initial flags for when there is no solver to destroy
124 uint8_t initialFlags = 0;
125
126 // Destroys the tree and the solver
127 if (solver_ != NULL)
128 {
129 initialFlags = solver_->flags;
130 DestroyTree();
131 ik_solver_destroy(solver_);
132 }
133
134 switch (algorithm_)
135 {
136 case ONE_BONE : solver_ = ik_solver_create(SOLVER_ONE_BONE); break;
137 case TWO_BONE : solver_ = ik_solver_create(SOLVER_TWO_BONE); break;
138 case FABRIK : solver_ = ik_solver_create(SOLVER_FABRIK); break;
139 /*case MSD : solver_ = ik_solver_create(SOLVER_MSD); break;*/
140 }
141
142 solver_->flags = initialFlags;
143
144 if (node_ != NULL)
145 RebuildTree();
146 }
147
148 // ----------------------------------------------------------------------------
GetFeature(Feature feature) const149 bool IKSolver::GetFeature(Feature feature) const
150 {
151 return (features_ & feature) != 0;
152 }
153
154 // ----------------------------------------------------------------------------
SetFeature(Feature feature,bool enable)155 void IKSolver::SetFeature(Feature feature, bool enable)
156 {
157 switch (feature)
158 {
159 case CONSTRAINTS:
160 {
161 solver_->flags &= ~SOLVER_ENABLE_CONSTRAINTS;
162 if (enable)
163 solver_->flags |= SOLVER_ENABLE_CONSTRAINTS;
164 } break;
165
166 case TARGET_ROTATIONS:
167 {
168 solver_->flags &= ~SOLVER_CALCULATE_TARGET_ROTATIONS;
169 if (enable)
170 solver_->flags |= SOLVER_CALCULATE_TARGET_ROTATIONS;
171 } break;
172
173 case AUTO_SOLVE:
174 {
175 if (((features_ & AUTO_SOLVE) != 0) == enable)
176 break;
177
178 if (enable)
179 SubscribeToEvent(GetScene(), E_SCENEDRAWABLEUPDATEFINISHED, URHO3D_HANDLER(IKSolver, HandleSceneDrawableUpdateFinished));
180 else
181 UnsubscribeFromEvent(GetScene(), E_SCENEDRAWABLEUPDATEFINISHED);
182 } break;
183
184 default: break;
185 }
186
187 features_ &= ~feature;
188 if (enable)
189 features_ |= feature;
190 }
191
192 // ----------------------------------------------------------------------------
GetMaximumIterations() const193 unsigned IKSolver::GetMaximumIterations() const
194 {
195 return solver_->max_iterations;
196 }
197
198 // ----------------------------------------------------------------------------
SetMaximumIterations(unsigned iterations)199 void IKSolver::SetMaximumIterations(unsigned iterations)
200 {
201 solver_->max_iterations = iterations;
202 }
203
204 // ----------------------------------------------------------------------------
GetTolerance() const205 float IKSolver::GetTolerance() const
206 {
207 return solver_->tolerance;
208 }
209
210 // ----------------------------------------------------------------------------
SetTolerance(float tolerance)211 void IKSolver::SetTolerance(float tolerance)
212 {
213 if (tolerance < M_EPSILON)
214 tolerance = M_EPSILON;
215 solver_->tolerance = tolerance;
216 }
217
218 // ----------------------------------------------------------------------------
CreateIKNodeFromUrhoNode(const Node * node)219 ik_node_t* IKSolver::CreateIKNodeFromUrhoNode(const Node* node)
220 {
221 ik_node_t* ikNode = ik_node_create(node->GetID());
222
223 // Set initial position/rotation and pass in Node* as user data for later
224 ikNode->original_position = Vec3Urho2IK(node->GetWorldPosition());
225 ikNode->original_rotation = QuatUrho2IK(node->GetWorldRotation());
226 ikNode->user_data = (void*)node;
227
228 /*
229 * If Urho's node has an effector, also create and attach one to the
230 * library's node. At this point, the IKEffector component shouldn't be
231 * holding a reference to any internal effector. Check this for debugging
232 * purposes and log if it does.
233 */
234 IKEffector* effector = node->GetComponent<IKEffector>();
235 if (effector != NULL)
236 {
237 #ifdef DEBUG
238 if (effector->ikEffectorNode_ != NULL)
239 URHO3D_LOGWARNINGF("[ik] IKEffector (attached to node \"%s\") has a reference to a possibly invalid internal effector. Should be NULL.", effector->GetNode()->GetName().CString());
240 #endif
241 ik_effector_t* ikEffector = ik_effector_create();
242 ik_node_attach_effector(ikNode, ikEffector); // ownership of effector
243
244 effector->SetIKSolver(this);
245 effector->SetIKEffectorNode(ikNode);
246 }
247
248 // Exact same deal with the constraint
249 IKConstraint* constraint = node->GetComponent<IKConstraint>();
250 if (constraint != NULL)
251 {
252 #ifdef DEBUG
253 if (constraint->ikConstraintNode_ != NULL)
254 URHO3D_LOGWARNINGF("[ik] IKConstraint (attached to node \"%s\") has a reference to a possibly invalid internal constraint. Should be NULL.", constraint->GetNode()->GetName().CString());
255 #endif
256
257 constraint->SetIKConstraintNode(ikNode);
258 }
259
260 return ikNode;
261 }
262
263 // ----------------------------------------------------------------------------
DestroyTree()264 void IKSolver::DestroyTree()
265 {
266 ik_solver_destroy_tree(solver_);
267 effectorList_.Clear();
268 constraintList_.Clear();
269 }
270
271 // ----------------------------------------------------------------------------
RebuildTree()272 void IKSolver::RebuildTree()
273 {
274 assert (node_ != NULL);
275
276 // Destroy current tree and set a new root node
277 DestroyTree();
278 ik_node_t* ikRoot = CreateIKNodeFromUrhoNode(node_);
279 ik_solver_set_tree(solver_, ikRoot);
280
281 /*
282 * Collect all effectors and constraints from children, and filter them to
283 * make sure they are in our subtree.
284 */
285 node_->GetComponents<IKEffector>(effectorList_, true);
286 node_->GetComponents<IKConstraint>(constraintList_, true);
287 for (PODVector<IKEffector*>::Iterator it = effectorList_.Begin(); it != effectorList_.End();)
288 {
289 if (ComponentIsInOurSubtree(*it))
290 {
291 BuildTreeToEffector((*it));
292 ++it;
293 }
294 else
295 {
296 it = effectorList_.Erase(it);
297 }
298 }
299 for (PODVector<IKConstraint*>::Iterator it = constraintList_.Begin(); it != constraintList_.End();)
300 {
301 if (ComponentIsInOurSubtree(*it))
302 ++it;
303 else
304 it = constraintList_.Erase(it);
305 }
306
307 treeNeedsRebuild = false;
308 MarkChainsNeedUpdating();
309 }
310
311 // ----------------------------------------------------------------------------
BuildTreeToEffector(IKEffector * effector)312 bool IKSolver::BuildTreeToEffector(IKEffector* effector)
313 {
314 /*
315 * NOTE: This function makes the assumption that the node the effector is
316 * attached to is -- without a doubt -- in our subtree (by using
317 * ComponentIsInOurSubtree() first). If this is not the case, the program
318 * will abort.
319 */
320
321 /*
322 * we need to build tree up to the node where this effector was added. Do
323 * this by following the chain of parent nodes until we hit a node that
324 * exists in the solver's subtree. Then iterate backwards again and add each
325 * missing node to the solver's tree.
326 */
327 const Node* iterNode = effector->GetNode();
328 ik_node_t* ikNode;
329 PODVector<const Node*> missingNodes;
330 while ((ikNode = ik_node_find_child(solver_->tree, iterNode->GetID())) == NULL)
331 {
332 missingNodes.Push(iterNode);
333 iterNode = iterNode->GetParent();
334
335 // Assert the assumptions made (described in the beginning of this function)
336 assert(iterNode != NULL);
337 assert (iterNode->HasComponent<IKSolver>() == false || iterNode == node_);
338 }
339
340 while (missingNodes.Size() > 0)
341 {
342 iterNode = missingNodes.Back();
343 missingNodes.Pop();
344
345 ik_node_t* ikChildNode = CreateIKNodeFromUrhoNode(iterNode);
346 ik_node_add_child(ikNode, ikChildNode);
347
348 ikNode = ikChildNode;
349 }
350
351 return true;
352 }
353
354 // ----------------------------------------------------------------------------
ComponentIsInOurSubtree(Component * component) const355 bool IKSolver::ComponentIsInOurSubtree(Component* component) const
356 {
357 const Node* iterNode = component->GetNode();
358 while (true)
359 {
360 // Note part of our subtree
361 if (iterNode == NULL)
362 return false;
363 // Reached the root node, it's part of our subtree!
364 if (iterNode == node_)
365 return true;
366 // Path to us is being blocked by another solver
367 Component* otherSolver = iterNode->GetComponent<IKSolver>();
368 if (otherSolver != NULL && otherSolver != component)
369 return false;
370
371 iterNode = iterNode->GetParent();
372 }
373
374 return true;
375 }
376
377 // ----------------------------------------------------------------------------
RebuildChainTrees()378 void IKSolver::RebuildChainTrees()
379 {
380 solverTreeValid_ = (ik_solver_rebuild_chain_trees(solver_) == 0);
381 ik_calculate_rotation_weight_decays(&solver_->chain_tree);
382
383 chainTreesNeedUpdating_ = false;
384 }
385
386 // ----------------------------------------------------------------------------
RecalculateSegmentLengths()387 void IKSolver::RecalculateSegmentLengths()
388 {
389 ik_solver_recalculate_segment_lengths(solver_);
390 }
391
392 // ----------------------------------------------------------------------------
CalculateJointRotations()393 void IKSolver::CalculateJointRotations()
394 {
395 ik_solver_calculate_joint_rotations(solver_);
396 }
397
398 // ----------------------------------------------------------------------------
Solve()399 void IKSolver::Solve()
400 {
401 URHO3D_PROFILE(IKSolve);
402
403 if (treeNeedsRebuild)
404 RebuildTree();
405
406 if (chainTreesNeedUpdating_)
407 RebuildChainTrees();
408
409 if (IsSolverTreeValid() == false)
410 return;
411
412 if (features_ & UPDATE_ORIGINAL_POSE)
413 ApplySceneToOriginalPose();
414
415 if (features_ & UPDATE_ACTIVE_POSE)
416 ApplySceneToActivePose();
417
418 if (features_ & USE_ORIGINAL_POSE)
419 ApplyOriginalPoseToActivePose();
420
421 for (PODVector<IKEffector*>::ConstIterator it = effectorList_.Begin(); it != effectorList_.End(); ++it)
422 {
423 (*it)->UpdateTargetNodePosition();
424 }
425
426 ik_solver_solve(solver_);
427
428 if (features_ & JOINT_ROTATIONS)
429 ik_solver_calculate_joint_rotations(solver_);
430
431 ApplyActivePoseToScene();
432 }
433
434 // ----------------------------------------------------------------------------
ApplyInitialPoseToSceneCallback(ik_node_t * ikNode)435 static void ApplyInitialPoseToSceneCallback(ik_node_t* ikNode)
436 {
437 Node* node = (Node*)ikNode->user_data;
438 node->SetWorldRotation(QuatIK2Urho(&ikNode->original_rotation));
439 node->SetWorldPosition(Vec3IK2Urho(&ikNode->original_position));
440 }
ApplyOriginalPoseToScene()441 void IKSolver::ApplyOriginalPoseToScene()
442 {
443 ik_solver_iterate_tree(solver_, ApplyInitialPoseToSceneCallback);
444 }
445
446 // ----------------------------------------------------------------------------
ApplySceneToInitialPoseCallback(ik_node_t * ikNode)447 static void ApplySceneToInitialPoseCallback(ik_node_t* ikNode)
448 {
449 Node* node = (Node*)ikNode->user_data;
450 ikNode->original_rotation = QuatUrho2IK(node->GetWorldRotation());
451 ikNode->original_position = Vec3Urho2IK(node->GetWorldPosition());
452 }
ApplySceneToOriginalPose()453 void IKSolver::ApplySceneToOriginalPose()
454 {
455 ik_solver_iterate_tree(solver_, ApplySceneToInitialPoseCallback);
456 }
457
458 // ----------------------------------------------------------------------------
ApplyActivePoseToSceneCallback(ik_node_t * ikNode)459 static void ApplyActivePoseToSceneCallback(ik_node_t* ikNode)
460 {
461 Node* node = (Node*)ikNode->user_data;
462 node->SetWorldRotation(QuatIK2Urho(&ikNode->rotation));
463 node->SetWorldPosition(Vec3IK2Urho(&ikNode->position));
464 }
ApplyActivePoseToScene()465 void IKSolver::ApplyActivePoseToScene()
466 {
467 ik_solver_iterate_tree(solver_, ApplyActivePoseToSceneCallback);
468 }
469
470 // ----------------------------------------------------------------------------
ApplySceneToActivePoseCallback(ik_node_t * ikNode)471 static void ApplySceneToActivePoseCallback(ik_node_t* ikNode)
472 {
473 Node* node = (Node*)ikNode->user_data;
474 ikNode->rotation = QuatUrho2IK(node->GetWorldRotation());
475 ikNode->position = Vec3Urho2IK(node->GetWorldPosition());
476 }
ApplySceneToActivePose()477 void IKSolver::ApplySceneToActivePose()
478 {
479 ik_solver_iterate_tree(solver_, ApplySceneToActivePoseCallback);
480 }
481
482 // ----------------------------------------------------------------------------
ApplyOriginalPoseToActivePose()483 void IKSolver::ApplyOriginalPoseToActivePose()
484 {
485 ik_solver_reset_to_original_pose(solver_);
486 }
487
488 // ----------------------------------------------------------------------------
MarkChainsNeedUpdating()489 void IKSolver::MarkChainsNeedUpdating()
490 {
491 chainTreesNeedUpdating_ = true;
492 }
493
494 // ----------------------------------------------------------------------------
MarkTreeNeedsRebuild()495 void IKSolver::MarkTreeNeedsRebuild()
496 {
497 treeNeedsRebuild = true;
498 }
499
500 // ----------------------------------------------------------------------------
IsSolverTreeValid() const501 bool IKSolver::IsSolverTreeValid() const
502 {
503 return solverTreeValid_;
504 }
505
506 // ----------------------------------------------------------------------------
507 /*
508 * This next section maintains the internal list of effector nodes. Whenever
509 * nodes are deleted or added to the scene, or whenever components are added
510 * or removed from nodes, we must check to see which of those nodes are/were
511 * IK effector nodes and update our internal list accordingly.
512 *
513 * Unfortunately, E_COMPONENTREMOVED and E_COMPONENTADDED do not fire when a
514 * parent node is removed/added containing child effector nodes, so we must
515 * also monitor E_NODEREMOVED AND E_NODEADDED.
516 */
517
518 // ----------------------------------------------------------------------------
OnSceneSet(Scene * scene)519 void IKSolver::OnSceneSet(Scene* scene)
520 {
521 if (features_ & AUTO_SOLVE)
522 SubscribeToEvent(scene, E_SCENEDRAWABLEUPDATEFINISHED, URHO3D_HANDLER(IKSolver, HandleSceneDrawableUpdateFinished));
523 }
524
525 // ----------------------------------------------------------------------------
OnNodeSet(Node * node)526 void IKSolver::OnNodeSet(Node* node)
527 {
528 ApplyOriginalPoseToScene();
529 DestroyTree();
530
531 if (node != NULL)
532 RebuildTree();
533 }
534
535 // ----------------------------------------------------------------------------
HandleComponentAdded(StringHash eventType,VariantMap & eventData)536 void IKSolver::HandleComponentAdded(StringHash eventType, VariantMap& eventData)
537 {
538 using namespace ComponentAdded;
539 (void)eventType;
540
541 Node* node = static_cast<Node*>(eventData[P_NODE].GetPtr());
542 Component* component = static_cast<Component*>(eventData[P_COMPONENT].GetPtr());
543
544 /*
545 * When a solver gets added into the scene, any parent solver's tree will
546 * be invalidated. We need to find all parent solvers (by iterating up the
547 * tree) and mark them as such.
548 */
549 if (component->GetType() == IKSolver::GetTypeStatic())
550 {
551 for (Node* iterNode = node; iterNode != NULL; iterNode = iterNode->GetParent())
552 {
553 IKSolver* parentSolver = iterNode->GetComponent<IKSolver>();
554 if (parentSolver != NULL)
555 parentSolver->MarkTreeNeedsRebuild();
556
557 }
558
559 return; // No need to continue processing effectors or constraints
560 }
561
562 if (solver_->tree == NULL)
563 return;
564
565 /*
566 * Update tree if component is an effector and is part of our subtree.
567 */
568 if (component->GetType() == IKEffector::GetTypeStatic())
569 {
570 // Not interested in components that won't be part of our
571 if (ComponentIsInOurSubtree(component) == false)
572 return;
573
574 BuildTreeToEffector(static_cast<IKEffector*>(component));
575 effectorList_.Push(static_cast<IKEffector*>(component));
576 return;
577 }
578
579 if (component->GetType() == IKConstraint::GetTypeStatic())
580 {
581 if (ComponentIsInOurSubtree(component) == false)
582 return;
583
584 constraintList_.Push(static_cast<IKConstraint*>(component));
585 }
586 }
587
588 // ----------------------------------------------------------------------------
HandleComponentRemoved(StringHash eventType,VariantMap & eventData)589 void IKSolver::HandleComponentRemoved(StringHash eventType, VariantMap& eventData)
590 {
591 using namespace ComponentRemoved;
592
593 if (solver_->tree == NULL)
594 return;
595
596 Node* node = static_cast<Node*>(eventData[P_NODE].GetPtr());
597 Component* component = static_cast<Component*>(eventData[P_COMPONENT].GetPtr());
598
599 /*
600 * When a solver gets added into the scene, any parent solver's tree will
601 * be invalidated. We need to find all parent solvers (by iterating up the
602 * tree) and mark them as such.
603 */
604 if (component->GetType() == IKSolver::GetTypeStatic())
605 {
606 for (Node* iterNode = node; iterNode != NULL; iterNode = iterNode->GetParent())
607 {
608 IKSolver* parentSolver = iterNode->GetComponent<IKSolver>();
609 if (parentSolver != NULL)
610 parentSolver->MarkTreeNeedsRebuild();
611
612 }
613
614 return; // No need to continue processing effectors or constraints
615 }
616
617 // If an effector was removed, the tree will have to be rebuilt.
618 if (component->GetType() == IKEffector::GetTypeStatic())
619 {
620 if (ComponentIsInOurSubtree(component) == false)
621 return;
622
623 ik_node_t* ikNode = ik_node_find_child(solver_->tree, node->GetID());
624 assert(ikNode != NULL);
625
626 ik_node_destroy_effector(ikNode);
627 static_cast<IKEffector*>(component)->SetIKEffectorNode(NULL);
628 effectorList_.RemoveSwap(static_cast<IKEffector*>(component));
629
630 ApplyOriginalPoseToScene();
631 MarkTreeNeedsRebuild();
632 return;
633 }
634
635 // Remove the ikNode* reference the IKConstraint was holding
636 if (component->GetType() == IKConstraint::GetTypeStatic())
637 {
638 if (ComponentIsInOurSubtree(component) == false)
639 return;
640
641 ik_node_t* ikNode = ik_node_find_child(solver_->tree, node->GetID());
642 assert(ikNode != NULL);
643
644 static_cast<IKConstraint*>(component)->SetIKConstraintNode(NULL);
645 constraintList_.RemoveSwap(static_cast<IKConstraint*>(component));
646 }
647 }
648
649 // ----------------------------------------------------------------------------
HandleNodeAdded(StringHash eventType,VariantMap & eventData)650 void IKSolver::HandleNodeAdded(StringHash eventType, VariantMap& eventData)
651 {
652 using namespace NodeAdded;
653
654 if (solver_->tree == NULL)
655 return;
656
657 Node* node = static_cast<Node*>(eventData[P_NODE].GetPtr());
658
659 PODVector<IKEffector*> effectors;
660 node->GetComponents<IKEffector>(effectors, true);
661 for (PODVector<IKEffector*>::ConstIterator it = effectors.Begin(); it != effectors.End(); ++it)
662 {
663 if (ComponentIsInOurSubtree(*it) == false)
664 continue;
665
666 BuildTreeToEffector(*it);
667 effectorList_.Push(*it);
668 }
669
670 PODVector<IKConstraint*> constraints;
671 node->GetComponents<IKConstraint>(constraints, true);
672 for (PODVector<IKConstraint*>::ConstIterator it = constraints.Begin(); it != constraints.End(); ++it)
673 {
674 if (ComponentIsInOurSubtree(*it) == false)
675 continue;
676
677 constraintList_.Push(*it);
678 }
679 }
680
681 // ----------------------------------------------------------------------------
HandleNodeRemoved(StringHash eventType,VariantMap & eventData)682 void IKSolver::HandleNodeRemoved(StringHash eventType, VariantMap& eventData)
683 {
684 using namespace NodeRemoved;
685
686 if (solver_->tree == NULL)
687 return;
688
689 Node* node = static_cast<Node*>(eventData[P_NODE].GetPtr());
690
691 // Remove cached IKEffectors from our list
692 PODVector<IKEffector*> effectors;
693 node->GetComponents<IKEffector>(effectors, true);
694 for (PODVector<IKEffector*>::ConstIterator it = effectors.Begin(); it != effectors.End(); ++it)
695 {
696 (*it)->SetIKEffectorNode(NULL);
697 effectorList_.RemoveSwap(*it);
698 }
699
700 PODVector<IKConstraint*> constraints;
701 node->GetComponents<IKConstraint>(constraints, true);
702 for (PODVector<IKConstraint*>::ConstIterator it = constraints.Begin(); it != constraints.End(); ++it)
703 {
704 constraintList_.RemoveSwap(*it);
705 }
706
707 // Special case, if the node being destroyed is the root node, destroy the
708 // solver's tree instead of destroying the single node. Calling
709 // ik_node_destroy() on the solver's root node will cause segfaults.
710 ik_node_t* ikNode = ik_node_find_child(solver_->tree, node->GetID());
711 if (ikNode != NULL)
712 {
713 if (ikNode == solver_->tree)
714 ik_solver_destroy_tree(solver_);
715 else
716 ik_node_destroy(ikNode);
717
718 MarkChainsNeedUpdating();
719 }
720 }
721
722 // ----------------------------------------------------------------------------
HandleSceneDrawableUpdateFinished(StringHash eventType,VariantMap & eventData)723 void IKSolver::HandleSceneDrawableUpdateFinished(StringHash eventType, VariantMap& eventData)
724 {
725 Solve();
726 }
727
728 // ----------------------------------------------------------------------------
DrawDebugGeometry(bool depthTest)729 void IKSolver::DrawDebugGeometry(bool depthTest)
730 {
731 DebugRenderer* debug = GetScene()->GetComponent<DebugRenderer>();
732 if (debug)
733 DrawDebugGeometry(debug, depthTest);
734 }
735
736 // ----------------------------------------------------------------------------
DrawDebugGeometry(DebugRenderer * debug,bool depthTest)737 void IKSolver::DrawDebugGeometry(DebugRenderer* debug, bool depthTest)
738 {
739 // Draws all scene segments
740 for (PODVector<IKEffector*>::ConstIterator it = effectorList_.Begin(); it != effectorList_.End(); ++it)
741 (*it)->DrawDebugGeometry(debug, depthTest);
742
743 ORDERED_VECTOR_FOR_EACH(&solver_->effector_nodes_list, ik_node_t*, pnode)
744 ik_effector_t* effector = (*pnode)->effector;
745
746 // Calculate average length of all segments so we can determine the radius
747 // of the debug spheres to draw
748 int chainLength = effector->chain_length == 0 ? -1 : effector->chain_length;
749 ik_node_t* a = *pnode;
750 ik_node_t* b = a->parent;
751 float averageLength = 0.0f;
752 unsigned numberOfSegments = 0;
753 while (b && chainLength-- != 0)
754 {
755 vec3_t v = a->original_position;
756 vec3_sub_vec3(v.f, b->original_position.f);
757 averageLength += vec3_length(v.f);
758 ++numberOfSegments;
759 a = b;
760 b = b->parent;
761 }
762 averageLength /= numberOfSegments;
763
764 // connect all chained nodes together with lines
765 chainLength = effector->chain_length == 0 ? -1 : effector->chain_length;
766 a = *pnode;
767 b = a->parent;
768 debug->AddSphere(
769 Sphere(Vec3IK2Urho(&a->original_position), averageLength * 0.1f),
770 Color(0, 0, 255),
771 depthTest
772 );
773 debug->AddSphere(
774 Sphere(Vec3IK2Urho(&a->position), averageLength * 0.1f),
775 Color(255, 128, 0),
776 depthTest
777 );
778 while (b && chainLength-- != 0)
779 {
780 debug->AddLine(
781 Vec3IK2Urho(&a->original_position),
782 Vec3IK2Urho(&b->original_position),
783 Color(0, 255, 255),
784 depthTest
785 );
786 debug->AddSphere(
787 Sphere(Vec3IK2Urho(&b->original_position), averageLength * 0.1f),
788 Color(0, 0, 255),
789 depthTest
790 );
791 debug->AddLine(
792 Vec3IK2Urho(&a->position),
793 Vec3IK2Urho(&b->position),
794 Color(255, 0, 0),
795 depthTest
796 );
797 debug->AddSphere(
798 Sphere(Vec3IK2Urho(&b->position), averageLength * 0.1f),
799 Color(255, 128, 0),
800 depthTest
801 );
802 a = b;
803 b = b->parent;
804 }
805 ORDERED_VECTOR_END_EACH
806 }
807
808 // ----------------------------------------------------------------------------
809 // Need these wrapper functions flags of GetFeature/SetFeature can be correctly
810 // exposed to the editor
811 // ----------------------------------------------------------------------------
812
813 #define DEF_FEATURE_GETTER(feature_name) \
814 bool IKSolver::Get##feature_name() const \
815 { \
816 return GetFeature(feature_name); \
817 }
818
819 #define DEF_FEATURE_SETTER(feature_name) \
820 void IKSolver::Set##feature_name(bool enable) \
821 { \
822 SetFeature(feature_name, enable); \
823 }
824
825 DEF_FEATURE_GETTER(JOINT_ROTATIONS)
826 DEF_FEATURE_GETTER(TARGET_ROTATIONS)
827 DEF_FEATURE_GETTER(UPDATE_ORIGINAL_POSE)
828 DEF_FEATURE_GETTER(UPDATE_ACTIVE_POSE)
829 DEF_FEATURE_GETTER(USE_ORIGINAL_POSE)
830 DEF_FEATURE_GETTER(CONSTRAINTS)
831 DEF_FEATURE_GETTER(AUTO_SOLVE)
832
833 DEF_FEATURE_SETTER(JOINT_ROTATIONS)
834 DEF_FEATURE_SETTER(TARGET_ROTATIONS)
835 DEF_FEATURE_SETTER(UPDATE_ORIGINAL_POSE)
836 DEF_FEATURE_SETTER(UPDATE_ACTIVE_POSE)
837 DEF_FEATURE_SETTER(USE_ORIGINAL_POSE)
838 DEF_FEATURE_SETTER(CONSTRAINTS)
839 DEF_FEATURE_SETTER(AUTO_SOLVE)
840
841 } // namespace Urho3D
842