Blender V5.3
itasc_plugin.cc
Go to the documentation of this file.
1/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
2 *
3 * SPDX-License-Identifier: GPL-2.0-or-later */
4
8
9#include <algorithm>
10#include <cmath>
11#include <cstdlib>
12#include <cstring>
13#include <vector>
14
15/* Already suppressed in `intern/itasc/CMakeLists.txt` however that doesn't apply here. */
16#if defined(__GNUC__) && !defined(__clang__)
17# pragma GCC diagnostic push
18# pragma GCC diagnostic ignored "-Wdeprecated-copy"
19#endif
20
21/* iTaSC headers */
22#ifdef WITH_IK_ITASC
23# include "Armature.hpp"
24# include "Cache.hpp"
25# include "CopyPose.hpp"
26# include "Distance.hpp"
27# include "MovingFrame.hpp"
28# include "Scene.hpp"
29# include "WDLSSolver.hpp"
30# include "WSDLSSolver.hpp"
31#endif
32
33#if defined(__GNUC__) && !defined(__clang__)
34# pragma GCC diagnostic pop
35#endif
36
37#include "MEM_guardedalloc.h"
38
39#include "BIK_api.h"
40#include "BLI_listbase.hh"
41#include "BLI_math_matrix_c.hh"
43#include "BLI_math_vector_c.hh"
44
45#include "BKE_action.hh"
46#include "BKE_armature.hh"
47#include "BKE_constraint.h"
48#include "DNA_action_types.h"
49#include "DNA_armature_types.h"
51#include "DNA_object_types.h"
52#include "DNA_scene_types.h"
53
54#include "itasc_plugin.h"
55
56namespace blender {
57
58/* default parameters */
60
61/* in case of animation mode, feedback and timestep is fixed */
62// #define ANIM_TIMESTEP 1.0
63#define ANIM_FEEDBACK 0.8
64// #define ANIM_QMAX 0.52
65
66/* Structure pointed by bPose.runtime->ikdata
67 * It contains everything needed to simulate the armatures
68 * There can be several simulation islands independent to each other */
69struct IK_Data {
70 struct IK_Scene *first;
71};
72
73using Vector3 = float[3];
74using Vector4 = float[4];
75struct IK_Target;
76using ErrorCallback = void (*)(const iTaSC::ConstraintValues *values,
77 uint nvalues,
78 IK_Target *iktarget);
79
80/* one structure for each target in the scene */
81struct IK_Target {
82 Depsgraph *bldepsgraph;
88 Object *owner; /* for auto IK */
90 std::string targetName;
91 std::string constraintName;
93 short channel; /* index in IK channel array of channel on which this target is defined */
94 short ee; /* end effector number */
95 bool simulation; /* true when simulation mode is used (update feedback) */
96 bool eeBlend; /* end effector affected by enforce blending */
97 float eeRest[4][4]; /* end effector initial pose relative to armature */
98
100 {
101 bldepsgraph = nullptr;
102 blscene = nullptr;
103 target = nullptr;
104 constraint = nullptr;
105 blenderConstraint = nullptr;
106 rootChannel = nullptr;
107 owner = nullptr;
108 controlType = 0;
109 channel = 0;
110 ee = 0;
111 eeBlend = true;
112 simulation = true;
113 targetName.reserve(32);
114 constraintName.reserve(32);
115 }
117 {
118 delete constraint;
119 delete target;
120 }
121};
122
124 bPoseChannel *pchan; /* channel where we must copy matrix back */
125 KDL::Frame frame; /* frame of the bone relative to object base, not armature base */
126 std::string tail; /* segment name of the joint from which we get the bone tail */
127 std::string head; /* segment name of the joint from which we get the bone head */
128 int parent; /* index in this array of the parent channel */
129 short jointType; /* type of joint, combination of IK_SegmentFlag */
130 char ndof; /* number of joint angles for this channel */
131 char jointValid; /* set to 1 when jointValue has been computed */
132 /* for joint constraint */
133 Object *owner; /* for pose and IK param */
134 double jointValue[4]; /* computed joint value */
135
137 {
138 pchan = nullptr;
139 parent = -1;
140 jointType = 0;
141 ndof = 0;
142 jointValid = 0;
143 owner = nullptr;
144 jointValue[0] = 0.0;
145 jointValue[1] = 0.0;
146 jointValue[2] = 0.0;
147 jointValue[3] = 0.0;
148 }
149};
150
151struct IK_Scene {
152 Depsgraph *bldepsgraph;
155 int numchan; /* number of channel in pchan */
156 int numjoint; /* number of joint in jointArray */
157 /* array of bone information, one per channel in the tree */
162 iTaSC::MovingFrame *base; /* armature base object */
163 KDL::Frame baseFrame; /* frame of armature base relative to blArmature */
164 KDL::JntArray jointArray; /* buffer for storing temporary joint array */
167 float blScale; /* scale of the Armature object (assume uniform scaling) */
168 float blInvScale; /* inverse of Armature object scale */
170 std::vector<IK_Target *> targets;
171
173 {
174 bldepsgraph = nullptr;
175 blscene = nullptr;
176 next = nullptr;
177 channels = nullptr;
178 armature = nullptr;
179 cache = nullptr;
180 scene = nullptr;
181 base = nullptr;
182 solver = nullptr;
183 blScale = blInvScale = 1.0f;
184 blArmature = nullptr;
185 numchan = 0;
186 numjoint = 0;
187 polarConstraint = nullptr;
188 }
189
191 {
192 /* delete scene first */
193 delete scene;
194 for (IK_Target *target : targets) {
195 delete target;
196 }
197 targets.clear();
198 delete[] channels;
199 delete solver;
200 delete armature;
201 delete base;
202 /* delete cache last */
203 delete cache;
204 }
205};
206
207/* type of IK joint, can be combined to list the joints corresponding to a bone */
216
225
226static int initialize_chain(Object * /*ob*/, bPoseChannel *pchan_tip, bConstraint *con)
227{
228 bPoseChannel *curchan, *pchan_root = nullptr, *chanlist[256], **oldchan;
229 PoseTree *tree;
230 PoseTarget *target;
232 int a, t, segcount = 0, size, newsize, *oldparent, parent, rootbone, treecount;
233
234 data = static_cast<bKinematicConstraint *>(con->data);
235
236 /* exclude tip from chain? */
237 if (!(data->flag & CONSTRAINT_IK_TIP)) {
238 pchan_tip = pchan_tip->parent;
239 }
240
241 rootbone = data->rootbone;
242 /* Find the chain's root & count the segments needed */
243 for (curchan = pchan_tip; curchan; curchan = curchan->parent) {
244 pchan_root = curchan;
245
246 if (++segcount > 255) { /* 255 is weak */
247 break;
248 }
249
250 if (segcount == rootbone) {
251 /* reached this end of the chain but if the chain is overlapping with a
252 * previous one, we must go back up to the root of the other chain */
253 if ((curchan->flag & POSE_CHAIN) && curchan->iktree.is_empty()) {
254 rootbone++;
255 continue;
256 }
257 break;
258 }
259
260 if (curchan->iktree.is_empty() == false) {
261 /* Oh, there is already a chain starting from this channel and our chain is longer.
262 * Should handle this by moving the previous chain up to the beginning of our chain
263 * For now we just stop here. */
264 break;
265 }
266 }
267 if (!segcount) {
268 return 0;
269 }
270 /* we reached a limit and still not the end of a previous chain, quit */
271 if ((pchan_root->flag & POSE_CHAIN) && pchan_root->iktree.is_empty()) {
272 return 0;
273 }
274
275 /* now that we know how many segment we have, set the flag */
276 for (rootbone = segcount, segcount = 0, curchan = pchan_tip; segcount < rootbone;
277 segcount++, curchan = curchan->parent)
278 {
279 chanlist[segcount] = curchan;
280 curchan->flag |= POSE_CHAIN;
281 }
282
283 /* setup the chain data */
284 /* create a target */
285 target = MEM_new_zeroed<PoseTarget>("posetarget");
286 target->con = con;
287 /* by construction there can be only one tree per channel
288 * and each channel can be part of at most one tree. */
289 tree = pchan_root->iktree.first();
290
291 if (tree == nullptr) {
292 /* make new tree */
293 tree = MEM_new_zeroed<PoseTree>("posetree");
294
295 tree->iterations = data->iterations;
296 tree->totchannel = segcount;
297 tree->stretch = (data->flag & CONSTRAINT_IK_STRETCH);
298
299 tree->pchan = MEM_new_array_zeroed<bPoseChannel *>(segcount, "ik tree pchan");
300 tree->parent = MEM_new_array_zeroed<int>(segcount, "ik tree parent");
301 for (a = 0; a < segcount; a++) {
302 tree->pchan[a] = chanlist[segcount - a - 1];
303 tree->parent[a] = a - 1;
304 }
305 target->tip = segcount - 1;
306
307 /* AND! link the tree to the root */
308 BLI_addtail(&pchan_root->iktree, tree);
309 /* new tree */
310 treecount = 1;
311 }
312 else {
313 tree->iterations = std::max<int>(data->iterations, tree->iterations);
314 tree->stretch = tree->stretch && !(data->flag & CONSTRAINT_IK_STRETCH);
315
316 /* Skip common pose channels and add remaining. */
317 size = std::min(segcount, tree->totchannel);
318 a = t = 0;
319 while (a < size && t < tree->totchannel) {
320 /* locate first matching channel */
321 for (; t < tree->totchannel && tree->pchan[t] != chanlist[segcount - a - 1]; t++) {
322 /* pass */
323 }
324 if (t >= tree->totchannel) {
325 break;
326 }
327 for (; a < size && t < tree->totchannel && tree->pchan[t] == chanlist[segcount - a - 1];
328 a++, t++)
329 {
330 /* pass */
331 }
332 }
333
334 segcount = segcount - a;
335 target->tip = tree->totchannel + segcount - 1;
336
337 if (segcount > 0) {
338 for (parent = a - 1; parent < tree->totchannel; parent++) {
339 if (tree->pchan[parent] == chanlist[segcount - 1]->parent) {
340 break;
341 }
342 }
343
344 /* shouldn't happen, but could with dependency cycles */
345 if (parent == tree->totchannel) {
346 parent = a - 1;
347 }
348
349 /* resize array */
350 newsize = tree->totchannel + segcount;
351 oldchan = tree->pchan;
352 oldparent = tree->parent;
353
354 tree->pchan = MEM_new_array_zeroed<bPoseChannel *>(newsize, "ik tree pchan");
355 tree->parent = MEM_new_array_zeroed<int>(newsize, "ik tree parent");
356 memcpy(tree->pchan, oldchan, sizeof(void *) * tree->totchannel);
357 memcpy(tree->parent, oldparent, sizeof(int) * tree->totchannel);
358 MEM_delete(oldchan);
359 MEM_delete(oldparent);
360
361 /* add new pose channels at the end, in reverse order */
362 for (a = 0; a < segcount; a++) {
363 tree->pchan[tree->totchannel + a] = chanlist[segcount - a - 1];
364 tree->parent[tree->totchannel + a] = tree->totchannel + a - 1;
365 }
366 tree->parent[tree->totchannel] = parent;
367
368 tree->totchannel = newsize;
369 }
370 /* reusing tree */
371 treecount = 0;
372 }
373
374 /* add target to the tree */
375 BLI_addtail(&tree->targets, target);
376 /* mark root channel having an IK tree */
377 pchan_root->flag |= POSE_IKTREE;
378 return treecount;
379}
380
382{
383 // bKinematicConstraint* data=(bKinematicConstraint *)con->data;
384
385 return true;
386}
387
389{
390 bKinematicConstraint *data = static_cast<bKinematicConstraint *>(con->data);
391
392 if (data->flag & CONSTRAINT_IK_AUTO) {
393 return true;
394 }
395 if (con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF)) {
396 return false;
397 }
398 if (is_cartesian_constraint(con)) {
399 /* cartesian space constraint */
400 if (data->tar == nullptr) {
401 return false;
402 }
403 if (data->tar->type == OB_ARMATURE && data->subtarget[0] == 0) {
404 return false;
405 }
406 }
407 return true;
408}
409
410static int initialize_scene(Object *ob, bPoseChannel *pchan_tip)
411{
412 /* Find all IK constraints and validate them. */
413 int treecount = 0;
414 for (bConstraint &con : pchan_tip->constraints) {
415 if (con.type == CONSTRAINT_TYPE_KINEMATIC) {
416 if (constraint_valid(&con)) {
417 treecount += initialize_chain(ob, pchan_tip, &con);
418 }
419 }
420 }
421 return treecount;
422}
423
425{
426 if (pose->runtime->ikdata) {
427 return static_cast<IK_Data *>(pose->runtime->ikdata);
428 }
429 pose->runtime->ikdata = MEM_new_zeroed<IK_Data>("iTaSC ikdata");
430 /* here init ikdata if needed
431 * now that we have scene, make sure the default param are initialized */
432 if (!DefIKParam.iksolver) {
434 }
435
436 return static_cast<IK_Data *>(pose->runtime->ikdata);
437}
438static double EulerAngleFromMatrix(const KDL::Rotation &R, int axis)
439{
440 double t = KDL::sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
441
442 if (t > 16.0 * KDL::epsilon) {
443 if (axis == 0) {
444 return -KDL::atan2(R(1, 2), R(2, 2));
445 }
446 if (axis == 1) {
447 return KDL::atan2(-R(0, 2), t);
448 }
449
450 return -KDL::atan2(R(0, 1), R(0, 0));
451 }
452
453 if (axis == 0) {
454 return -KDL::atan2(-R(2, 1), R(1, 1));
455 }
456 if (axis == 1) {
457 return KDL::atan2(-R(0, 2), t);
458 }
459
460 return 0.0f;
461}
462
463static double ComputeTwist(const KDL::Rotation &R)
464{
465 /* qy and qw are the y and w components of the quaternion from R */
466 double qy = R(0, 2) - R(2, 0);
467 double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
468
469 double tau = 2 * KDL::atan2(qy, qw);
470
471 return tau;
472}
473
474static void RemoveEulerAngleFromMatrix(KDL::Rotation &R, double angle, int axis)
475{
476 /* compute twist parameter */
478 switch (axis) {
479 case 0:
481 break;
482 case 1:
484 break;
485 case 2:
487 break;
488 default:
489 return;
490 }
491 /* remove angle */
492 R = R * T;
493}
494
495#if 0
496static void GetEulerXZY(const KDL::Rotation &R, double &X, double &Z, double &Y)
497{
498 if (fabs(R(0, 1)) > 1.0 - KDL::epsilon) {
499 X = -KDL::sign(R(0, 1)) * KDL::atan2(R(1, 2), R(1, 0));
500 Z = -KDL::sign(R(0, 1)) * KDL::PI / 2;
501 Y = 0.0;
502 }
503 else {
504 X = KDL::atan2(R(2, 1), R(1, 1));
505 Z = KDL::atan2(-R(0, 1), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 2))));
506 Y = KDL::atan2(R(0, 2), R(0, 0));
507 }
508}
509
510static void GetEulerXYZ(const KDL::Rotation &R, double &X, double &Y, double &Z)
511{
512 if (fabs(R(0, 2)) > 1.0 - KDL::epsilon) {
513 X = KDL::sign(R(0, 2)) * KDL::atan2(-R(1, 0), R(1, 1));
514 Y = KDL::sign(R(0, 2)) * KDL::PI / 2;
515 Z = 0.0;
516 }
517 else {
518 X = KDL::atan2(-R(1, 2), R(2, 2));
519 Y = KDL::atan2(R(0, 2), KDL::sqrt(KDL::sqr(R(0, 0)) + KDL::sqr(R(0, 1))));
520 Z = KDL::atan2(-R(0, 1), R(0, 0));
521 }
522}
523#endif
524
525static void GetJointRotation(KDL::Rotation &boneRot, int type, double *rot)
526{
527 switch (type & ~IK_TRANSY) {
528 default:
529 /* fixed bone, no joint */
530 break;
531 case IK_XDOF:
532 /* RX only, get the X rotation */
533 rot[0] = EulerAngleFromMatrix(boneRot, 0);
534 break;
535 case IK_YDOF:
536 /* RY only, get the Y rotation */
537 rot[0] = ComputeTwist(boneRot);
538 break;
539 case IK_ZDOF:
540 /* RZ only, get the Z rotation */
541 rot[0] = EulerAngleFromMatrix(boneRot, 2);
542 break;
543 case IK_XDOF | IK_YDOF:
544 rot[1] = ComputeTwist(boneRot);
545 RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
546 rot[0] = EulerAngleFromMatrix(boneRot, 0);
547 break;
548 case IK_SWING:
549 /* RX+RZ */
550 boneRot.GetXZRot().GetValue(rot);
551 break;
552 case IK_YDOF | IK_ZDOF:
553 /* RZ+RY */
554 rot[1] = ComputeTwist(boneRot);
555 RemoveEulerAngleFromMatrix(boneRot, rot[1], 1);
556 rot[0] = EulerAngleFromMatrix(boneRot, 2);
557 break;
558 case IK_SWING | IK_YDOF:
559 rot[2] = ComputeTwist(boneRot);
560 RemoveEulerAngleFromMatrix(boneRot, rot[2], 1);
561 boneRot.GetXZRot().GetValue(rot);
562 break;
563 case IK_REVOLUTE:
564 boneRot.GetRot().GetValue(rot);
565 break;
566 }
567}
568
569static bool target_callback(const iTaSC::Timestamp & /*timestamp*/,
570 const iTaSC::Frame & /*current*/,
571 iTaSC::Frame &next,
572 void *param)
573{
574 IK_Target *target = static_cast<IK_Target *>(param);
575 /* compute next target position
576 * get target matrix from constraint. */
577 bConstraint *constraint = target->blenderConstraint;
578 float tarmat[4][4];
579
581 target->blscene,
582 constraint,
583 0,
585 target->owner,
586 tarmat,
587 1.0);
588
589 /* rootmat contains the target pose in world coordinate
590 * if enforce is != 1.0, blend the target position with the end effector position
591 * if the armature was in rest position. This information is available in eeRest */
592 if (constraint->enforce != 1.0f && target->eeBlend) {
593 /* eeRest is relative to the reference frame of the IK root
594 * get this frame in world reference */
595 float restmat[4][4];
596 bPoseChannel *pchan = target->rootChannel;
597 if (pchan->parent) {
598 pchan = pchan->parent;
599 float chanmat[4][4];
600 copy_m4_m4(chanmat, pchan->pose_mat);
601 copy_v3_v3(chanmat[3], pchan->pose_tail);
602 mul_m4_series(restmat, target->owner->object_to_world().ptr(), chanmat, target->eeRest);
603 }
604 else {
605 mul_m4_m4m4(restmat, target->owner->object_to_world().ptr(), target->eeRest);
606 }
607 /* blend the target */
608 blend_m4_m4m4(tarmat, restmat, tarmat, constraint->enforce);
609 }
610 next.setValue(&tarmat[0][0]);
611 return true;
612}
613
614static bool base_callback(const iTaSC::Timestamp &timestamp,
615 const iTaSC::Frame & /*current*/,
616 iTaSC::Frame &next,
617 void *param)
618{
619 IK_Scene *ikscene = static_cast<IK_Scene *>(param);
620 /* compute next armature base pose
621 * algorithm:
622 * ikscene->pchan[0] is the root channel of the tree
623 * if it has a parent, get the pose matrix from it and replace [3] by parent pchan->tail
624 * then multiply by the armature matrix to get ikscene->armature base position */
625 bPoseChannel *pchan = ikscene->channels[0].pchan;
626 float rootmat[4][4];
627 if (pchan->parent) {
628 pchan = pchan->parent;
629 float chanmat[4][4];
630 copy_m4_m4(chanmat, pchan->pose_mat);
631 copy_v3_v3(chanmat[3], pchan->pose_tail);
632 /* save the base as a frame too so that we can compute deformation after simulation */
633 ikscene->baseFrame.setValue(&chanmat[0][0]);
634 /* iTaSC armature is scaled to object scale, scale the base frame too */
635 ikscene->baseFrame.p *= ikscene->blScale;
636 mul_m4_m4m4(rootmat, ikscene->blArmature->object_to_world().ptr(), chanmat);
637 }
638 else {
639 copy_m4_m4(rootmat, ikscene->blArmature->object_to_world().ptr());
640 ikscene->baseFrame = iTaSC::F_identity;
641 }
642 next.setValue(&rootmat[0][0]);
643 /* If there is a polar target (only during solving otherwise we don't have end effector). */
644 if (ikscene->polarConstraint && timestamp.update) {
645 /* compute additional rotation of base frame so that armature follows the polar target */
646 float imat[4][4]; /* IK tree base inverse matrix */
647 float polemat[4][4]; /* polar target in IK tree base frame */
648 float goalmat[4][4]; /* target in IK tree base frame */
649 float mat[4][4]; /* temp matrix */
650 bKinematicConstraint *poledata = static_cast<bKinematicConstraint *>(
651 ikscene->polarConstraint->data);
652
653 invert_m4_m4(imat, rootmat);
654 /* polar constraint imply only one target */
655 IK_Target *iktarget = ikscene->targets[0];
656 /* root channel from which we take the bone initial orientation */
657 IK_Channel &rootchan = ikscene->channels[0];
658
659 /* get polar target matrix in world space */
661 ikscene->blscene,
662 ikscene->polarConstraint,
663 1,
665 ikscene->blArmature,
666 mat,
667 1.0);
668 /* convert to armature space */
669 mul_m4_m4m4(polemat, imat, mat);
670 /* get the target in world space
671 * (was computed before as target object are defined before base object). */
672 iktarget->target->getPose().getValue(mat[0]);
673 /* convert to armature space */
674 mul_m4_m4m4(goalmat, imat, mat);
675 /* take position of target, polar target, end effector, in armature space */
676 KDL::Vector goalpos(goalmat[3]);
677 KDL::Vector polepos(polemat[3]);
678 KDL::Vector endpos = ikscene->armature->getPose(iktarget->ee).p;
679 /* get root bone orientation */
680 KDL::Frame rootframe;
681 ikscene->armature->getRelativeFrame(rootframe, rootchan.tail);
682 KDL::Vector rootx = rootframe.M.UnitX();
683 KDL::Vector rootz = rootframe.M.UnitZ();
684 /* and compute root bone head */
685 double q_rest[3], q[3], length;
686 const KDL::Joint *joint;
687 const KDL::Frame *tip;
688 ikscene->armature->getSegment(rootchan.tail, 3, joint, q_rest[0], q[0], tip);
689 length = (joint->getType() == KDL::Joint::TransY) ? q[0] : tip->p(1);
690 KDL::Vector rootpos = rootframe.p - length * rootframe.M.UnitY();
691
692 /* compute main directions */
693 KDL::Vector dir = KDL::Normalize(endpos - rootpos);
694 KDL::Vector poledir = KDL::Normalize(goalpos - rootpos);
695 /* compute up directions */
696 KDL::Vector poleup = KDL::Normalize(polepos - rootpos);
697 KDL::Vector up = rootx * KDL::cos(poledata->poleangle) + rootz * KDL::sin(poledata->poleangle);
698 /* from which we build rotation matrix */
699 KDL::Rotation endrot, polerot;
700 /* for the armature, using the root bone orientation */
701 KDL::Vector x = KDL::Normalize(dir * up);
702 endrot.UnitX(x);
703 endrot.UnitY(KDL::Normalize(x * dir));
704 endrot.UnitZ(-dir);
705 /* for the polar target */
706 x = KDL::Normalize(poledir * poleup);
707 polerot.UnitX(x);
708 polerot.UnitY(KDL::Normalize(x * poledir));
709 polerot.UnitZ(-poledir);
710 /* the difference between the two is the rotation we want to apply */
711 KDL::Rotation result(polerot * endrot.Inverse());
712 /* apply on base frame as this is an artificial additional rotation */
713 next.M = next.M * result;
714 ikscene->baseFrame.M = ikscene->baseFrame.M * result;
715 }
716 return true;
717}
718
719static bool copypose_callback(const iTaSC::Timestamp & /*timestamp*/,
720 iTaSC::ConstraintValues *const _values,
721 uint /*nvalues*/,
722 void *_param)
723{
724 IK_Target *iktarget = static_cast<IK_Target *>(_param);
725 bKinematicConstraint *condata = static_cast<bKinematicConstraint *>(
726 iktarget->blenderConstraint->data);
727 iTaSC::ConstraintValues *values = _values;
728 bItasc *ikparam = static_cast<bItasc *>(iktarget->owner->pose->ikparam);
729
730 /* we need default parameters */
731 if (!ikparam) {
732 ikparam = &DefIKParam;
733 }
734
735 if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
737 values->alpha = 0.0;
738 values->action = iTaSC::ACT_ALPHA;
739 values++;
740 }
742 values->alpha = 0.0;
743 values->action = iTaSC::ACT_ALPHA;
744 values++;
745 }
746 }
747 else {
749 /* update error */
750 values->alpha = condata->weight;
752 values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
753 values++;
754 }
756 /* update error */
757 values->alpha = condata->orientweight;
759 values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
760 values++;
761 }
762 }
763 return true;
764}
765
766static void copypose_error(const iTaSC::ConstraintValues *values,
767 uint /*nvalues*/,
768 IK_Target *iktarget)
769{
771 double error;
772 int i;
773
775 /* update error */
776 for (i = 0, error = 0.0, value = values->values; i < values->number; i++, value++) {
777 error += KDL::sqr(value->y - value->yd);
778 }
779 iktarget->blenderConstraint->lin_error = float(KDL::sqrt(error));
780 values++;
781 }
783 /* update error */
784 for (i = 0, error = 0.0, value = values->values; i < values->number; i++, value++) {
785 error += KDL::sqr(value->y - value->yd);
786 }
787 iktarget->blenderConstraint->rot_error = float(KDL::sqrt(error));
788 values++;
789 }
790}
791
792static bool distance_callback(const iTaSC::Timestamp &timestamp,
793 iTaSC::ConstraintValues *const _values,
794 uint /*nvalues*/,
795 void *_param)
796{
797 IK_Target *iktarget = static_cast<IK_Target *>(_param);
798 bKinematicConstraint *condata = static_cast<bKinematicConstraint *>(
799 iktarget->blenderConstraint->data);
800 iTaSC::ConstraintValues *values = _values;
801 bItasc *ikparam = static_cast<bItasc *>(iktarget->owner->pose->ikparam);
802 /* we need default parameters */
803 if (!ikparam) {
804 ikparam = &DefIKParam;
805 }
806
807 /* update weight according to mode */
808 if (iktarget->blenderConstraint->flag & CONSTRAINT_OFF) {
809 values->alpha = 0.0;
810 }
811 else {
812 switch (condata->mode) {
813 case LIMITDIST_INSIDE:
814 values->alpha = (values->values[0].y > condata->dist) ? condata->weight : 0.0;
815 break;
817 values->alpha = (values->values[0].y < condata->dist) ? condata->weight : 0.0;
818 break;
819 default:
820 values->alpha = condata->weight;
821 break;
822 }
823 if (!timestamp.substep) {
824 /* only update value on first timestep */
825 switch (condata->mode) {
826 case LIMITDIST_INSIDE:
827 values->values[0].yd = condata->dist * 0.95;
828 break;
830 values->values[0].yd = condata->dist * 1.05;
831 break;
832 default:
833 values->values[0].yd = condata->dist;
834 break;
835 }
837 values->feedback = (iktarget->simulation) ? ikparam->feedback : ANIM_FEEDBACK;
838 }
839 }
840 values->action |= iTaSC::ACT_ALPHA;
841 return true;
842}
843
844static void distance_error(const iTaSC::ConstraintValues *values,
845 uint /*nvalues*/,
846 IK_Target *iktarget)
847{
848 iktarget->blenderConstraint->lin_error = float(values->values[0].y - values->values[0].yd);
849}
850
851static bool joint_callback(const iTaSC::Timestamp & /*timestamp*/,
852 iTaSC::ConstraintValues *const _values,
853 uint _nvalues,
854 void *_param)
855{
856 IK_Channel *ikchan = static_cast<IK_Channel *>(_param);
857 bItasc *ikparam = static_cast<bItasc *>(ikchan->owner->pose->ikparam);
858 bPoseChannel *chan = ikchan->pchan;
859 int dof;
860
861 /* a channel can be split into multiple joints, so we get called multiple
862 * times for one channel (this callback is only for 1 joint in the armature)
863 * the IK_JointTarget structure is shared between multiple joint constraint
864 * and the target joint values is computed only once, remember this in jointValid
865 * Don't forget to reset it before each frame */
866 if (!ikchan->jointValid) {
867 float rmat[3][3];
868
869 if (chan->rotmode > 0) {
870 /* Euler rotations (will cause gimbal lock, but this can be alleviated a bit with rotation
871 * orders) */
872 eulO_to_mat3(rmat, chan->eul, chan->rotmode);
873 }
874 else if (chan->rotmode == ROT_MODE_AXISANGLE) {
875 /* axis-angle - stored in quaternion data,
876 * but not really that great for 3D-changing orientations */
877 axis_angle_to_mat3(rmat, &chan->quat[1], chan->quat[0]);
878 }
879 else {
880 /* Quaternions are normalized before use to eliminate scaling issues. */
881 normalize_qt(chan->quat);
882 quat_to_mat3(rmat, chan->quat);
883 }
884 KDL::Rotation jointRot(rmat[0][0],
885 rmat[1][0],
886 rmat[2][0],
887 rmat[0][1],
888 rmat[1][1],
889 rmat[2][1],
890 rmat[0][2],
891 rmat[1][2],
892 rmat[2][2]);
893 GetJointRotation(jointRot, ikchan->jointType, ikchan->jointValue);
894 ikchan->jointValid = 1;
895 }
896 /* determine which part of jointValue is used for this joint
897 * closely related to the way the joints are defined */
898 switch (ikchan->jointType & ~IK_TRANSY) {
899 case IK_XDOF:
900 case IK_YDOF:
901 case IK_ZDOF:
902 dof = 0;
903 break;
904 case IK_XDOF | IK_YDOF:
905 /* X + Y */
906 dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RX) ? 0 : 1;
907 break;
908 case IK_SWING:
909 /* XZ */
910 dof = 0;
911 break;
912 case IK_YDOF | IK_ZDOF:
913 /* Z + Y */
914 dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RZ) ? 0 : 1;
915 break;
916 case IK_SWING | IK_YDOF:
917 /* XZ + Y */
918 dof = (_values[0].id == iTaSC::Armature::ID_JOINT_RY) ? 2 : 0;
919 break;
920 case IK_REVOLUTE:
921 dof = 0;
922 break;
923 default:
924 dof = -1;
925 break;
926 }
927 if (dof >= 0) {
928 for (uint i = 0; i < _nvalues; i++, dof++) {
929 _values[i].values[0].yd = ikchan->jointValue[dof];
930 _values[i].alpha = chan->ikrotweight;
931 _values[i].feedback = ikparam->feedback;
932 }
933 }
934 return true;
935}
936
937/* build array of joint corresponding to IK chain */
938static int convert_channels(Depsgraph *depsgraph, IK_Scene *ikscene, PoseTree *tree, float ctime)
939{
940 IK_Channel *ikchan;
941 bPoseChannel *pchan;
942 int a, flag, njoint;
943
944 njoint = 0;
945 for (a = 0, ikchan = ikscene->channels; a < ikscene->numchan; a++, ikchan++) {
946 pchan = tree->pchan[a];
947 ikchan->pchan = pchan;
948 ikchan->parent = (a > 0) ? tree->parent[a] : -1;
949 ikchan->owner = ikscene->blArmature;
950
951 /* the constraint and channels must be applied before we build the iTaSC scene,
952 * this is because some of the pose data (e.g. pose head) don't have corresponding
953 * joint angles and can't be applied to the iTaSC armature dynamically */
954 if (!(pchan->flag & POSE_DONE)) {
955 BKE_pose_where_is_bone(depsgraph, ikscene->blscene, ikscene->blArmature, pchan, ctime, true);
956 }
957 /* tell blender that this channel was controlled by IK,
958 * it's cleared on each BKE_pose_where_is() */
959 pchan->flag |= (POSE_DONE | POSE_CHAIN);
960
961 /* set DoF flag */
962 flag = 0;
963 if (!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) &&
964 (!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0] < 0.0f ||
965 pchan->limitmax[0] > 0.0f))
966 {
967 flag |= IK_XDOF;
968 }
969 if (!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) &&
970 (!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1] < 0.0f ||
971 pchan->limitmax[1] > 0.0f))
972 {
973 flag |= IK_YDOF;
974 }
975 if (!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) &&
976 (!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2] < 0.0f ||
977 pchan->limitmax[2] > 0.0f))
978 {
979 flag |= IK_ZDOF;
980 }
981
982 if (tree->stretch && (pchan->ikstretch > 0.0)) {
983 flag |= IK_TRANSY;
984 }
985 /*
986 * Logic to create the segments:
987 * RX,RY,RZ = rotational joints with no length
988 * RY(tip) = rotational joints with a fixed length arm = (0,length,0)
989 * TY = translational joint on Y axis
990 * F(pos) = fixed joint with an arm at position pos
991 * Conversion rule of the above flags:
992 * - ==> F(tip)
993 * X ==> RX(tip)
994 * Y ==> RY(tip)
995 * Z ==> RZ(tip)
996 * XY ==> RX+RY(tip)
997 * XZ ==> RX+RZ(tip)
998 * YZ ==> RZ+RY(tip)
999 * XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip)
1000 * In case of stretch, tip=(0,0,0) and there is an additional TY joint
1001 * The frame at last of these joints represents the tail of the bone.
1002 * The head is computed by a reverse translation on Y axis of the bone length
1003 * or in case of TY joint, by the frame at previous joint.
1004 * In case of separation of bones, there is an additional F(head) joint
1005 *
1006 * Computing rest pose and length is complicated: the solver works in world space
1007 * Here is the logic:
1008 * rest position is computed only from bone->bone_mat.
1009 * bone length is computed from bone->length multiplied by the scaling factor of
1010 * the armature. Non-uniform scaling will give bad result!
1011 */
1012 switch (flag & (IK_XDOF | IK_YDOF | IK_ZDOF)) {
1013 default:
1014 ikchan->jointType = 0;
1015 ikchan->ndof = 0;
1016 break;
1017 case IK_XDOF:
1018 /* RX only, get the X rotation */
1019 ikchan->jointType = IK_XDOF;
1020 ikchan->ndof = 1;
1021 break;
1022 case IK_YDOF:
1023 /* RY only, get the Y rotation */
1024 ikchan->jointType = IK_YDOF;
1025 ikchan->ndof = 1;
1026 break;
1027 case IK_ZDOF:
1028 /* RZ only, get the Zz rotation */
1029 ikchan->jointType = IK_ZDOF;
1030 ikchan->ndof = 1;
1031 break;
1032 case IK_XDOF | IK_YDOF:
1033 ikchan->jointType = IK_XDOF | IK_YDOF;
1034 ikchan->ndof = 2;
1035 break;
1036 case IK_XDOF | IK_ZDOF:
1037 /* RX+RZ */
1038 ikchan->jointType = IK_SWING;
1039 ikchan->ndof = 2;
1040 break;
1041 case IK_YDOF | IK_ZDOF:
1042 /* RZ+RY */
1043 ikchan->jointType = IK_ZDOF | IK_YDOF;
1044 ikchan->ndof = 2;
1045 break;
1046 case IK_XDOF | IK_YDOF | IK_ZDOF:
1047 /* spherical joint */
1049 /* decompose in a Swing+RotY joint */
1050 ikchan->jointType = IK_SWING | IK_YDOF;
1051 }
1052 else {
1053 ikchan->jointType = IK_REVOLUTE;
1054 }
1055 ikchan->ndof = 3;
1056 break;
1057 }
1058 if (flag & IK_TRANSY) {
1059 ikchan->jointType |= IK_TRANSY;
1060 ikchan->ndof++;
1061 }
1062 njoint += ikchan->ndof;
1063 }
1064 /* njoint is the joint coordinate, create the Joint Array */
1065 ikscene->jointArray.resize(njoint);
1066 ikscene->numjoint = njoint;
1067 return njoint;
1068}
1069
1070/* compute array of joint value corresponding to current pose */
1071static void convert_pose(IK_Scene *ikscene)
1072{
1073 KDL::Rotation boneRot;
1074 bPoseChannel *pchan;
1075 IK_Channel *ikchan;
1076 Bone *bone;
1077 float rmat[4][4]; /* rest pose of bone with parent taken into account */
1078 float bmat[4][4]; /* difference */
1079 float scale;
1080 double *rot;
1081 int a, joint;
1082
1083 /* assume uniform scaling and take Y scale as general scale for the armature */
1084 scale = len_v3(ikscene->blArmature->object_to_world().ptr()[1]);
1085 rot = ikscene->jointArray(0);
1086 for (joint = a = 0, ikchan = ikscene->channels;
1087 a < ikscene->numchan && joint < ikscene->numjoint;
1088 a++, ikchan++)
1089 {
1090 pchan = ikchan->pchan;
1091 bone = pchan->bone_get(*ikscene->blArmature);
1092
1093 if (pchan->parent) {
1094 unit_m4(bmat);
1095 mul_m4_m4m3(bmat, pchan->parent->pose_mat, bone->bone_mat);
1096 }
1097 else {
1098 copy_m4_m4(bmat, bone->arm_mat);
1099 }
1100 invert_m4_m4(rmat, bmat);
1101 mul_m4_m4m4(bmat, rmat, pchan->pose_mat);
1102 normalize_m4(bmat);
1103 boneRot.setValue(bmat[0]);
1104 GetJointRotation(boneRot, ikchan->jointType, rot);
1105 if (ikchan->jointType & IK_TRANSY) {
1106 /* compute actual length */
1107 rot[ikchan->ndof - 1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale;
1108 }
1109 rot += ikchan->ndof;
1110 joint += ikchan->ndof;
1111 }
1112}
1113
1114/* compute array of joint value corresponding to current pose */
1115static void pose_rest(IK_Scene *ikscene)
1116{
1117 bPoseChannel *pchan;
1118 IK_Channel *ikchan;
1119 Bone *bone;
1120 float scale;
1121 double *rot;
1122 int a, joint;
1123
1124 /* assume uniform scaling and take Y scale as general scale for the armature */
1125 scale = len_v3(ikscene->blArmature->object_to_world().ptr()[1]);
1126 /* rest pose is 0 */
1127 SetToZero(ikscene->jointArray);
1128 /* except for transY joints */
1129 rot = ikscene->jointArray(0);
1130 for (joint = a = 0, ikchan = ikscene->channels;
1131 a < ikscene->numchan && joint < ikscene->numjoint;
1132 a++, ikchan++)
1133 {
1134 pchan = ikchan->pchan;
1135 bone = pchan->bone_get(*ikscene->blArmature);
1136
1137 if (ikchan->jointType & IK_TRANSY) {
1138 rot[ikchan->ndof - 1] = bone->length * scale;
1139 }
1140 rot += ikchan->ndof;
1141 joint += ikchan->ndof;
1142 }
1143}
1144
1146 Depsgraph *depsgraph, Scene *blscene, Object *ob, bPoseChannel *pchan, float ctime)
1147{
1148 PoseTree *tree = pchan->iktree.first();
1149 PoseTarget *target;
1150 bKinematicConstraint *condata;
1151 bConstraint *polarcon;
1152 bItasc *ikparam;
1153 iTaSC::Armature *arm;
1154 iTaSC::Scene *scene;
1155 IK_Scene *ikscene;
1156 IK_Channel *ikchan;
1157 KDL::Frame initPose;
1158 Bone *bone;
1159 int a, numtarget;
1160 uint t;
1161 float length;
1162 bool ret = true;
1163 double *rot;
1164 float start[3];
1165
1166 if (tree->totchannel == 0) {
1167 return nullptr;
1168 }
1169
1170 ikscene = new IK_Scene;
1171 ikscene->blscene = blscene;
1172 ikscene->bldepsgraph = depsgraph;
1173 arm = new iTaSC::Armature();
1174 scene = new iTaSC::Scene();
1175 ikscene->channels = new IK_Channel[tree->totchannel];
1176 ikscene->numchan = tree->totchannel;
1177 ikscene->armature = arm;
1178 ikscene->scene = scene;
1179 ikparam = static_cast<bItasc *>(ob->pose->ikparam);
1180
1181 if (!ikparam) {
1182 /* you must have our own copy */
1183 ikparam = &DefIKParam;
1184 }
1185
1186 if (ikparam->flag & ITASC_SIMULATION) {
1187 /* no cache in animation mode */
1188 ikscene->cache = new iTaSC::Cache();
1189 }
1190
1191 switch (ikparam->solver) {
1192 case ITASC_SOLVER_SDLS:
1193 ikscene->solver = new iTaSC::WSDLSSolver();
1194 break;
1195 case ITASC_SOLVER_DLS:
1196 ikscene->solver = new iTaSC::WDLSSolver();
1197 break;
1198 default:
1199 delete ikscene;
1200 return nullptr;
1201 }
1202 ikscene->blArmature = ob;
1203 /* assume uniform scaling and take Y scale as general scale for the armature */
1204 ikscene->blScale = len_v3(ob->object_to_world().ptr()[1]);
1205 ikscene->blInvScale = (ikscene->blScale < KDL::epsilon) ? 0.0f : 1.0f / ikscene->blScale;
1206
1207 std::string joint;
1208 std::string root("root");
1209 std::string parent;
1210 std::vector<double> weights;
1211 double weight[3];
1212 /* build the array of joints corresponding to the IK chain */
1213 convert_channels(depsgraph, ikscene, tree, ctime);
1214 /* in Blender, the rest pose is always 0 for joints */
1215 pose_rest(ikscene);
1216 rot = ikscene->jointArray(0);
1217
1218 for (a = 0, ikchan = ikscene->channels; a < tree->totchannel; a++, ikchan++) {
1219 pchan = ikchan->pchan;
1220 bone = pchan->bone_get(*ob);
1221
1223 /* compute the position and rotation of the head from previous segment */
1224 Vector3 *fl = bone->bone_mat;
1225 KDL::Rotation brot(
1226 fl[0][0], fl[1][0], fl[2][0], fl[0][1], fl[1][1], fl[2][1], fl[0][2], fl[1][2], fl[2][2]);
1227 /* if the bone is disconnected, the head is movable in pose mode
1228 * take that into account by using pose matrix instead of bone
1229 * Note that pose is expressed in armature space, convert to previous bone space */
1230 {
1231 float R_parmat[3][3];
1232 float iR_parmat[3][3];
1233 if (pchan->parent) {
1234 copy_m3_m4(R_parmat, pchan->parent->pose_mat);
1235 }
1236 else {
1237 unit_m3(R_parmat);
1238 }
1239 if (pchan->parent) {
1240 sub_v3_v3v3(start, pchan->pose_head, pchan->parent->pose_tail);
1241 }
1242 else if (ikparam->flag & ITASC_TRANSLATE_ROOT_BONES) {
1243 start[0] = start[1] = start[2] = 0.0f;
1244 }
1245 else {
1246 copy_v3_v3(start, pchan->pose_head);
1247 }
1248 invert_m3_m3(iR_parmat, R_parmat);
1249 normalize_m3(iR_parmat);
1250 mul_m3_v3(iR_parmat, start);
1251 }
1252 KDL::Vector bpos(start[0], start[1], start[2]);
1253 bpos *= ikscene->blScale;
1254 KDL::Frame head(brot, bpos);
1255
1256 /* rest pose length of the bone taking scaling into account */
1257 length = bone->length * ikscene->blScale;
1258 parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
1259 /* first the fixed segment to the bone head */
1260 if (!(bone->flag & BONE_CONNECTED) || head.M.GetRot().Norm() > KDL::epsilon) {
1261 joint = bone->name;
1262 joint += ":H";
1263 ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head);
1264 parent = joint;
1265 }
1266 if (!(ikchan->jointType & IK_TRANSY)) {
1267 /* fixed length, put it in tip */
1268 tip.p[1] = length;
1269 }
1270 joint = bone->name;
1271 weight[0] = (1.0 - pchan->stiffness[0]);
1272 weight[1] = (1.0 - pchan->stiffness[1]);
1273 weight[2] = (1.0 - pchan->stiffness[2]);
1274 switch (ikchan->jointType & ~IK_TRANSY) {
1275 case 0:
1276 /* fixed bone */
1277 joint += ":F";
1278 ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip);
1279 break;
1280 case IK_XDOF:
1281 /* RX only, get the X rotation */
1282 joint += ":RX";
1283 ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
1284 weights.push_back(weight[0]);
1285 break;
1286 case IK_YDOF:
1287 /* RY only, get the Y rotation */
1288 joint += ":RY";
1289 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
1290 weights.push_back(weight[1]);
1291 break;
1292 case IK_ZDOF:
1293 /* RZ only, get the Zz rotation */
1294 joint += ":RZ";
1295 ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
1296 weights.push_back(weight[2]);
1297 break;
1298 case IK_XDOF | IK_YDOF:
1299 joint += ":RX";
1300 ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
1301 weights.push_back(weight[0]);
1302 if (ret) {
1303 parent = joint;
1304 joint = bone->name;
1305 joint += ":RY";
1306 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
1307 weights.push_back(weight[1]);
1308 }
1309 break;
1310 case IK_SWING:
1311 joint += ":SW";
1312 ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
1313 weights.push_back(weight[0]);
1314 weights.push_back(weight[2]);
1315 break;
1316 case IK_YDOF | IK_ZDOF:
1317 /* RZ+RY */
1318 joint += ":RZ";
1319 ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
1320 weights.push_back(weight[2]);
1321 if (ret) {
1322 parent = joint;
1323 joint = bone->name;
1324 joint += ":RY";
1325 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
1326 weights.push_back(weight[1]);
1327 }
1328 break;
1329 case IK_SWING | IK_YDOF:
1330 /* decompose in a Swing+RotY joint */
1331 joint += ":SW";
1332 ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
1333 weights.push_back(weight[0]);
1334 weights.push_back(weight[2]);
1335 if (ret) {
1336 parent = joint;
1337 joint = bone->name;
1338 joint += ":RY";
1339 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
1340 weights.push_back(weight[1]);
1341 }
1342 break;
1343 case IK_REVOLUTE:
1344 joint += ":SJ";
1345 ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
1346 weights.push_back(weight[0]);
1347 weights.push_back(weight[1]);
1348 weights.push_back(weight[2]);
1349 break;
1350 }
1351 if (ret && (ikchan->jointType & IK_TRANSY)) {
1352 parent = joint;
1353 joint = bone->name;
1354 joint += ":TY";
1355 ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof - 1]);
1356 const float ikstretch = pchan->ikstretch * pchan->ikstretch;
1357 /* why invert twice here? */
1358 weight[1] = (1.0 - min_ff(1.0 - ikstretch, 1.0f - 0.001f));
1359 weights.push_back(weight[1]);
1360 }
1361 if (!ret) {
1362 /* error making the armature?? */
1363 break;
1364 }
1365 /* joint points to the segment that correspond to the bone per say */
1366 ikchan->tail = joint;
1367 ikchan->head = parent;
1368 /* in case of error */
1369 ret = false;
1370 if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT | BONE_IK_ROTCTL))) {
1371 joint = bone->name;
1372 joint += ":RX";
1373 if (pchan->ikflag & BONE_IK_XLIMIT) {
1374 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) {
1375 break;
1376 }
1377 }
1378 if (pchan->ikflag & BONE_IK_ROTCTL) {
1379 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
1380 break;
1381 }
1382 }
1383 }
1384 if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT | BONE_IK_ROTCTL))) {
1385 joint = bone->name;
1386 joint += ":RY";
1387 if (pchan->ikflag & BONE_IK_YLIMIT) {
1388 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[1], pchan->limitmax[1]) < 0) {
1389 break;
1390 }
1391 }
1392 if (pchan->ikflag & BONE_IK_ROTCTL) {
1393 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
1394 break;
1395 }
1396 }
1397 }
1398 if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT | BONE_IK_ROTCTL))) {
1399 joint = bone->name;
1400 joint += ":RZ";
1401 if (pchan->ikflag & BONE_IK_ZLIMIT) {
1402 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[2], pchan->limitmax[2]) < 0) {
1403 break;
1404 }
1405 }
1406 if (pchan->ikflag & BONE_IK_ROTCTL) {
1407 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
1408 break;
1409 }
1410 }
1411 }
1412 if ((ikchan->jointType & IK_SWING) &&
1414 {
1415 joint = bone->name;
1416 joint += ":SW";
1417 if (pchan->ikflag & BONE_IK_XLIMIT) {
1418 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0) {
1419 break;
1420 }
1421 }
1422 if (pchan->ikflag & BONE_IK_ZLIMIT) {
1423 if (arm->addLimitConstraint(joint, 1, pchan->limitmin[2], pchan->limitmax[2]) < 0) {
1424 break;
1425 }
1426 }
1427 if (pchan->ikflag & BONE_IK_ROTCTL) {
1428 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
1429 break;
1430 }
1431 }
1432 }
1433 if ((ikchan->jointType & IK_REVOLUTE) && (pchan->ikflag & BONE_IK_ROTCTL)) {
1434 joint = bone->name;
1435 joint += ":SJ";
1436 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0) {
1437 break;
1438 }
1439 }
1440 /* no error, so restore */
1441 ret = true;
1442 rot += ikchan->ndof;
1443 }
1444 if (!ret) {
1445 delete ikscene;
1446 return nullptr;
1447 }
1448 /* for each target, we need to add an end effector in the armature */
1449 for (numtarget = 0, polarcon = nullptr, ret = true, target = tree->targets.first(); target;
1450 target = target->next)
1451 {
1452 condata = static_cast<bKinematicConstraint *>(target->con->data);
1453 pchan = tree->pchan[target->tip];
1454
1455 if (is_cartesian_constraint(target->con)) {
1456 /* add the end effector */
1457 IK_Target *iktarget = new IK_Target();
1458 ikscene->targets.push_back(iktarget);
1459 iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail);
1460 if (iktarget->ee == -1) {
1461 ret = false;
1462 break;
1463 }
1464 /* initialize all the fields that we can set at this time */
1465 const Bone *bone = pchan->bone_get(*ob);
1466 iktarget->blenderConstraint = target->con;
1467 iktarget->channel = target->tip;
1468 iktarget->simulation = (ikparam->flag & ITASC_SIMULATION);
1469 iktarget->rootChannel = ikscene->channels[0].pchan;
1470 iktarget->owner = ob;
1471 iktarget->targetName = bone->name;
1472 iktarget->targetName += ":T:";
1473 iktarget->targetName += target->con->name;
1474 iktarget->constraintName = bone->name;
1475 iktarget->constraintName += ":C:";
1476 iktarget->constraintName += target->con->name;
1477 numtarget++;
1478 if (condata->poletar) {
1479 /* this constraint has a polar target */
1480 polarcon = target->con;
1481 }
1482 }
1483 }
1484 /* deal with polar target if any */
1485 if (numtarget == 1 && polarcon) {
1486 ikscene->polarConstraint = polarcon;
1487 }
1488 /* we can now add the armature
1489 * the armature is based on a moving frame.
1490 * initialize with the correct position in case there is no cache */
1491 base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene);
1492 ikscene->base = new iTaSC::MovingFrame(initPose);
1493 ikscene->base->setCallback(base_callback, ikscene);
1494 std::string armname;
1495 armname = ob->id.name;
1496 armname += ":B";
1497 ret = scene->addObject(armname, ikscene->base);
1498 armname = ob->id.name;
1499 armname += ":AR";
1500 if (ret) {
1501 ret = scene->addObject(armname, ikscene->armature, ikscene->base);
1502 }
1503 if (!ret) {
1504 delete ikscene;
1505 return nullptr;
1506 }
1507 /* set the weight */
1508 e_matrix &Wq = arm->getWq();
1509 assert(Wq.cols() == int(weights.size()));
1510 for (int q = 0; q < Wq.cols(); q++) {
1511 Wq(q, q) = weights[q];
1512 }
1513 /* get the inverse rest pose frame of the base to compute relative rest pose of end effectors
1514 * this is needed to handle the enforce parameter
1515 * ikscene->pchan[0] is the root channel of the tree
1516 * if it has no parent, then it's just the identify Frame */
1517 float invBaseFrame[4][4];
1518 pchan = ikscene->channels[0].pchan;
1519 if (pchan->parent) {
1520 /* it has a parent, get the pose matrix from it */
1521 float baseFrame[4][4];
1522 pchan = pchan->parent;
1523 const Bone *bone = pchan->bone_get(*ob);
1524 copy_m4_m4(baseFrame, bone->arm_mat);
1525 /* move to the tail and scale to get rest pose of armature base */
1526 copy_v3_v3(baseFrame[3], bone->arm_tail);
1527 invert_m4_m4(invBaseFrame, baseFrame);
1528 }
1529 else {
1530 unit_m4(invBaseFrame);
1531 }
1532 /* finally add the constraint */
1533 for (t = 0; t < ikscene->targets.size(); t++) {
1534 IK_Target *iktarget = ikscene->targets[t];
1535 iktarget->blscene = blscene;
1536 iktarget->bldepsgraph = depsgraph;
1537 condata = static_cast<bKinematicConstraint *>(iktarget->blenderConstraint->data);
1538 pchan = tree->pchan[iktarget->channel];
1539 uint controltype, bone_count;
1540 double bone_length;
1541 float mat[4][4];
1542
1543 /* add the end effector
1544 * estimate the average bone length, used to clamp feedback error */
1545 for (bone_count = 0, bone_length = 0.0f, a = iktarget->channel; a >= 0;
1546 a = tree->parent[a], bone_count++)
1547 {
1548 const bPoseChannel *pchan = tree->pchan[a];
1549 const Bone *bone = pchan->bone_get(*ob);
1550 bone_length += ikscene->blScale * bone->length;
1551 }
1552 bone_length /= bone_count;
1553
1554 /* store the rest pose of the end effector to compute enforce target */
1555 const Bone *bone = pchan->bone_get(*ob);
1556 copy_m4_m4(mat, bone->arm_mat);
1557 copy_v3_v3(mat[3], bone->arm_tail);
1558 /* get the rest pose relative to the armature base */
1559 mul_m4_m4m4(iktarget->eeRest, invBaseFrame, mat);
1560 iktarget->eeBlend = (!ikscene->polarConstraint && condata->type == CONSTRAINT_IK_COPYPOSE) ?
1561 true :
1562 false;
1563 /* use target_callback to make sure the initPose includes enforce coefficient */
1564 target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget);
1565 iktarget->target = new iTaSC::MovingFrame(initPose);
1566 iktarget->target->setCallback(target_callback, iktarget);
1567 ret = scene->addObject(iktarget->targetName, iktarget->target);
1568 if (!ret) {
1569 break;
1570 }
1571
1572 switch (condata->type) {
1574 controltype = 0;
1575 if (condata->flag & CONSTRAINT_IK_ROT) {
1576 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X)) {
1577 controltype |= iTaSC::CopyPose::CTL_ROTATIONX;
1578 }
1579 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y)) {
1580 controltype |= iTaSC::CopyPose::CTL_ROTATIONY;
1581 }
1582 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z)) {
1583 controltype |= iTaSC::CopyPose::CTL_ROTATIONZ;
1584 }
1585 }
1586 if (condata->flag & CONSTRAINT_IK_POS) {
1587 if (!(condata->flag & CONSTRAINT_IK_NO_POS_X)) {
1588 controltype |= iTaSC::CopyPose::CTL_POSITIONX;
1589 }
1590 if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y)) {
1591 controltype |= iTaSC::CopyPose::CTL_POSITIONY;
1592 }
1593 if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z)) {
1594 controltype |= iTaSC::CopyPose::CTL_POSITIONZ;
1595 }
1596 }
1597 if (controltype) {
1598 iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bone_length);
1599 /* set the gain */
1600 if (controltype & iTaSC::CopyPose::CTL_POSITION) {
1603 }
1604 if (controltype & iTaSC::CopyPose::CTL_ROTATION) {
1607 }
1608 iktarget->constraint->registerCallback(copypose_callback, iktarget);
1609 iktarget->errorCallback = copypose_error;
1610 iktarget->controlType = controltype;
1611 /* add the constraint */
1612 if (condata->flag & CONSTRAINT_IK_TARGETAXIS) {
1613 ret = scene->addConstraintSet(iktarget->constraintName,
1614 iktarget->constraint,
1615 iktarget->targetName,
1616 armname,
1617 "",
1618 ikscene->channels[iktarget->channel].tail);
1619 }
1620 else {
1621 ret = scene->addConstraintSet(iktarget->constraintName,
1622 iktarget->constraint,
1623 armname,
1624 iktarget->targetName,
1625 ikscene->channels[iktarget->channel].tail);
1626 }
1627 }
1628 break;
1630 iktarget->constraint = new iTaSC::Distance(bone_length);
1633 iktarget->constraint->registerCallback(distance_callback, iktarget);
1634 iktarget->errorCallback = distance_error;
1635 /* we can update the weight on each substep */
1636 iktarget->constraint->substep(true);
1637 /* add the constraint */
1638 ret = scene->addConstraintSet(iktarget->constraintName,
1639 iktarget->constraint,
1640 armname,
1641 iktarget->targetName,
1642 ikscene->channels[iktarget->channel].tail);
1643 break;
1644 }
1645 if (!ret) {
1646 break;
1647 }
1648 }
1649 if (!ret || !scene->addCache(ikscene->cache) || !scene->addSolver(ikscene->solver) ||
1650 !scene->initialize())
1651 {
1652 delete ikscene;
1653 ikscene = nullptr;
1654 }
1655 return ikscene;
1656}
1657
1658static void create_scene(Depsgraph *depsgraph, Scene *scene, Object *ob, float ctime)
1659{
1660 /* create the IK scene */
1661 for (bPoseChannel &pchan : ob->pose->chanbase) {
1662 /* by construction there is only one tree */
1663 PoseTree *tree = (PoseTree *)pchan.iktree.first_;
1664 if (tree) {
1665 IK_Data *ikdata = get_ikdata(ob->pose);
1666 /* convert tree in iTaSC::Scene */
1667 IK_Scene *ikscene = convert_tree(depsgraph, scene, ob, &pchan, ctime);
1668 if (ikscene) {
1669 ikscene->next = ikdata->first;
1670 ikdata->first = ikscene;
1671 }
1672 /* delete the trees once we are done */
1673 while (tree) {
1674 BLI_remlink(&pchan.iktree, tree);
1675 tree->targets.free_no_destruct();
1676 if (tree->pchan) {
1677 MEM_delete(tree->pchan);
1678 }
1679 if (tree->parent) {
1680 MEM_delete(tree->parent);
1681 }
1682 if (tree->basis_change) {
1683 MEM_delete(tree->basis_change);
1684 }
1685 MEM_delete(tree);
1686 tree = (PoseTree *)pchan.iktree.first_;
1687 }
1688 }
1689 }
1690}
1691
1692/* returns 1 if scaling has changed and tree must be reinitialized */
1693static int init_scene(Object *ob)
1694{
1695 /* check also if scaling has changed */
1696 float scale = len_v3(ob->object_to_world().ptr()[1]);
1697 IK_Scene *scene;
1698
1699 if (ob->pose->runtime->ikdata) {
1700 for (scene = (static_cast<IK_Data *>(ob->pose->runtime->ikdata))->first; scene != nullptr;
1701 scene = scene->next)
1702 {
1703 if (fabs(scene->blScale - scale) > KDL::epsilon) {
1704 return 1;
1705 }
1706 scene->channels[0].pchan->flag |= POSE_IKTREE;
1707 }
1708 }
1709 return 0;
1710}
1711
1712static void execute_scene(Depsgraph *depsgraph,
1713 Scene *blscene,
1714 IK_Scene *ikscene,
1715 bItasc *ikparam,
1716 float ctime,
1717 float frtime)
1718{
1719 int i;
1720 IK_Channel *ikchan;
1721 if (ikparam->flag & ITASC_SIMULATION) {
1722 for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ikchan++) {
1723 /* In simulation mode we don't allow external constraint to change our bones,
1724 * mark the channel done also tell Blender that this channel is part of IK tree.
1725 * Cleared on each BKE_pose_where_is() */
1726 ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
1727 ikchan->jointValid = 0;
1728 }
1729 }
1730 else {
1731 /* in animation mode, we must get the bone position from action and constraints */
1732 for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ikchan++) {
1733 if (!(ikchan->pchan->flag & POSE_DONE)) {
1735 depsgraph, blscene, ikscene->blArmature, ikchan->pchan, ctime, true);
1736 }
1737 /* tell blender that this channel was controlled by IK,
1738 * it's cleared on each BKE_pose_where_is() */
1739 ikchan->pchan->flag |= (POSE_DONE | POSE_CHAIN);
1740 ikchan->jointValid = 0;
1741 }
1742 }
1743 /* only run execute the scene if at least one of our target is enabled */
1744 for (i = ikscene->targets.size(); i > 0; i--) {
1745 IK_Target *iktarget = ikscene->targets[i - 1];
1746 if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) {
1747 break;
1748 }
1749 }
1750 if (i == 0 && ikscene->armature->getNrOfConstraints() == 0) {
1751 /* all constraint disabled */
1752 return;
1753 }
1754
1755 /* compute timestep */
1756 double timestamp = ctime * frtime + 2147483.648;
1757 double timestep = frtime;
1758 bool reiterate = (ikparam->flag & ITASC_REITERATION) ? true : false;
1759 int numstep = (ikparam->flag & ITASC_AUTO_STEP) ? 0 : ikparam->numstep;
1760 bool simulation = true;
1761
1762 if (ikparam->flag & ITASC_SIMULATION) {
1763 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
1764 }
1765 else {
1766 /* in animation mode we start from the pose after action and constraint */
1767 convert_pose(ikscene);
1768 ikscene->armature->setJointArray(ikscene->jointArray);
1769 /* and we don't handle velocity */
1770 reiterate = true;
1771 simulation = false;
1772 /* time is virtual, so take fixed value for velocity parameters (see itasc_update_param)
1773 * and choose 1s timestep to allow having velocity parameters in radiant */
1774 timestep = 1.0;
1775 /* use auto setup to let the solver test the variation of the joints */
1776 numstep = 0;
1777 }
1778
1779 if (ikscene->cache && !reiterate && simulation) {
1780 iTaSC::CacheTS sts, cts;
1781 sts = cts = (iTaSC::CacheTS)std::round(timestamp * 1000.0);
1782 if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == nullptr || cts == 0) {
1783 /* the cache is empty before this time, reiterate */
1784 if (ikparam->flag & ITASC_INITIAL_REITERATION) {
1785 reiterate = true;
1786 }
1787 }
1788 else {
1789 /* can take the cache as a start point. */
1790 sts -= cts;
1791 timestep = sts / 1000.0;
1792 }
1793 }
1794 /* don't cache if we are reiterating because we don't want to destroy the cache unnecessarily */
1795 ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation);
1796 if (reiterate) {
1797 /* how many times do we reiterate? */
1798 for (i = 0; i < ikparam->numiter; i++) {
1799 if (ikscene->armature->getMaxJointChange() < ikparam->precision ||
1800 ikscene->armature->getMaxEndEffectorChange() < ikparam->precision)
1801 {
1802 break;
1803 }
1804 ikscene->scene->update(timestamp, timestep, numstep, true, false, simulation);
1805 }
1806 if (simulation) {
1807 /* one more fake iteration to cache */
1808 ikscene->scene->update(timestamp, 0.0, 1, true, true, true);
1809 }
1810 }
1811 /* compute constraint error */
1812 for (i = ikscene->targets.size(); i > 0; i--) {
1813 IK_Target *iktarget = ikscene->targets[i - 1];
1814 if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF) && iktarget->constraint) {
1815 uint nvalues;
1816 const iTaSC::ConstraintValues *values;
1817 values = iktarget->constraint->getControlParameters(&nvalues);
1818 iktarget->errorCallback(values, nvalues, iktarget);
1819 }
1820 }
1821 /* Apply result to bone:
1822 * walk the ikscene->channels
1823 * for each, get the Frame of the joint corresponding to the bone relative to its parent
1824 * combine the parent and the joint frame to get the frame relative to armature
1825 * a backward translation of the bone length gives the head
1826 * if TY, compute the scale as the ratio of the joint length with rest pose length */
1827 iTaSC::Armature *arm = ikscene->armature;
1828 KDL::Frame frame;
1829 double q_rest[3], q[3];
1830 const KDL::Joint *joint;
1831 const KDL::Frame *tip;
1832 bPoseChannel *pchan;
1833 float scale;
1834 float length;
1835 float yaxis[3];
1836 for (i = 0, ikchan = ikscene->channels; i < ikscene->numchan; i++, ikchan++) {
1837 if (i == 0) {
1838 if (!arm->getRelativeFrame(frame, ikchan->tail)) {
1839 break;
1840 }
1841 /* this frame is relative to base, make it relative to object */
1842 ikchan->frame = ikscene->baseFrame * frame;
1843 }
1844 else {
1845 if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail)) {
1846 break;
1847 }
1848 /* combine with parent frame to get frame relative to object */
1849 ikchan->frame = ikscene->channels[ikchan->parent].frame * frame;
1850 }
1851 /* ikchan->frame is the tail frame relative to object
1852 * get bone length */
1853 if (!arm->getSegment(ikchan->tail, 3, joint, q_rest[0], q[0], tip)) {
1854 break;
1855 }
1856 if (joint->getType() == KDL::Joint::TransY) {
1857 /* stretch bones have a TY joint, compute the scale */
1858 scale = float(q[0] / q_rest[0]);
1859 /* the length is the joint itself */
1860 length = float(q[0]);
1861 }
1862 else {
1863 scale = 1.0f;
1864 /* for fixed bone, the length is in the tip (always along Y axis) */
1865 length = tip->p(1);
1866 }
1867 /* ready to compute the pose mat */
1868 pchan = ikchan->pchan;
1869 /* tail mat */
1870 ikchan->frame.getValue(&pchan->pose_mat[0][0]);
1871 /* the scale of the object was included in the ik scene, take it out now
1872 * because the pose channels are relative to the object */
1873 mul_v3_fl(pchan->pose_mat[3], ikscene->blInvScale);
1874 length *= ikscene->blInvScale;
1875 copy_v3_v3(pchan->pose_tail, pchan->pose_mat[3]);
1876 /* shift to head */
1877 copy_v3_v3(yaxis, pchan->pose_mat[1]);
1878 mul_v3_fl(yaxis, length);
1879 sub_v3_v3v3(pchan->pose_mat[3], pchan->pose_mat[3], yaxis);
1880 copy_v3_v3(pchan->pose_head, pchan->pose_mat[3]);
1881 /* add scale */
1882 mul_v3_fl(pchan->pose_mat[0], scale);
1883 mul_v3_fl(pchan->pose_mat[1], scale);
1884 mul_v3_fl(pchan->pose_mat[2], scale);
1885 }
1886 if (i < ikscene->numchan) {
1887 /* big problem */
1888 }
1889}
1890
1891/* -------------------------------------------------------------------- */
1894
1895void itasc_initialize_tree(Depsgraph *depsgraph, Scene *scene, Object *ob, float ctime)
1896{
1897 int count = 0;
1898
1899 if (ob->pose->runtime->ikdata != nullptr && !(ob->pose->flag & POSE_WAS_REBUILT)) {
1900 if (!init_scene(ob)) {
1901 return;
1902 }
1903 }
1904 /* first remove old scene */
1906 /* we should handle all the constraint and mark them all disabled
1907 * for blender but we'll start with the IK constraint alone */
1908 for (bPoseChannel &pchan : ob->pose->chanbase) {
1909 if (pchan.constflag & PCHAN_HAS_IK) {
1910 count += initialize_scene(ob, &pchan);
1911 }
1912 }
1913 /* if at least one tree, create the scenes from the PoseTree stored in the channels
1914 * postpone until execute_tree: this way the pose constraint are included */
1915 if (count) {
1916 create_scene(depsgraph, scene, ob, ctime);
1917 }
1919 /* make sure we don't rebuilt until the user changes something important */
1920 ob->pose->flag &= ~POSE_WAS_REBUILT;
1921}
1922
1924 Depsgraph *depsgraph, Scene *scene, Object *ob, bPoseChannel *pchan_root, float ctime)
1925{
1926 if (ob->pose->runtime->ikdata) {
1927 IK_Data *ikdata = static_cast<IK_Data *>(ob->pose->runtime->ikdata);
1928 bItasc *ikparam = static_cast<bItasc *>(ob->pose->ikparam);
1929 /* we need default parameters */
1930 if (!ikparam) {
1931 ikparam = &DefIKParam;
1932 }
1933
1934 for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
1935 if (ikscene->channels[0].pchan == pchan_root) {
1936 float timestep = scene->r.frs_sec_base / scene->r.frs_sec;
1937 execute_scene(depsgraph, scene, ikscene, ikparam, ctime, timestep);
1938 break;
1939 }
1940 }
1941 }
1942}
1943
1944void itasc_release_tree(Scene * /*scene*/, Object * /*ob*/, float /*ctime*/)
1945{
1946 /* not used for iTaSC */
1947}
1948
1950{
1951 if (pose->runtime->ikdata) {
1952 IK_Data *ikdata = static_cast<IK_Data *>(pose->runtime->ikdata);
1953 for (IK_Scene *scene = ikdata->first; scene; scene = ikdata->first) {
1954 ikdata->first = scene->next;
1955 delete scene;
1956 }
1957 MEM_delete(ikdata);
1958 pose->runtime->ikdata = nullptr;
1959 }
1960}
1961
1963{
1964 if (pose->runtime->ikdata) {
1965 IK_Data *ikdata = static_cast<IK_Data *>(pose->runtime->ikdata);
1966 for (IK_Scene *scene = ikdata->first; scene; scene = scene->next) {
1967 if (scene->cache) {
1968 /* clear all cache but leaving the timestamp 0 (=rest pose) */
1969 scene->cache->clearCacheFrom(nullptr, 1);
1970 }
1971 }
1972 }
1973}
1974
1976{
1977 if (pose->runtime->ikdata && pose->ikparam) {
1978 IK_Data *ikdata = static_cast<IK_Data *>(pose->runtime->ikdata);
1979 bItasc *ikparam = static_cast<bItasc *>(pose->ikparam);
1980 for (IK_Scene *ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
1981 double armlength = ikscene->armature->getArmLength();
1982 ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax * armlength);
1983 ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps * armlength);
1984 if (ikparam->flag & ITASC_SIMULATION) {
1985 ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep);
1986 ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep);
1987 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
1988 ikscene->armature->setControlParameter(
1990 }
1991 else {
1992 /* in animation mode timestep is 1s by convention => qmax becomes radiant and feedback
1993 * becomes fraction of error gap corrected in one iteration. */
1994 ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, 1.0);
1995 ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, 1.0);
1996 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, 0.52);
1997 ikscene->armature->setControlParameter(
1999 }
2000 }
2001 }
2002}
2003
2005{
2006 bKinematicConstraint *data = static_cast<bKinematicConstraint *>(cons->data);
2007
2008 /* only for IK constraint */
2009 if (cons->type != CONSTRAINT_TYPE_KINEMATIC || data == nullptr) {
2010 return;
2011 }
2012
2013 switch (data->type) {
2016 /* cartesian space constraint */
2017 break;
2018 }
2019}
2020
2022
2023} // namespace blender
Blender kernel action and pose functionality.
#define mul_m4_series(...)
#define CONSTRAINT_ID_ALL
Object is a sort of wrapper for general info.
#define Z
#define Y
Read Blender memory allocation and freeing.
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition btVector3.h:356
represents a frame transformation in 3D space (rotation + translation)
Definition frames.hpp:526
Rotation M
Orientation of the Frame.
Definition frames.hpp:529
void setValue(float *oglmat)
Definition frames.hpp:725
void getValue(float *oglmat) const
Definition frames.hpp:733
Vector p
origine of the Frame
Definition frames.hpp:528
void resize(unsigned int newSize)
Definition jntarray.cpp:66
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition joint.hpp:43
const JointType & getType() const
Definition joint.hpp:88
represents rotations in 3 dimensional space.
Definition frames.hpp:299
Vector GetRot() const
Returns a vector with the direction of the equiv. axis and its norm is angle.
Definition frames.cpp:297
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition frames.hpp:642
Vector UnitY() const
Access to the underlying unitvectors of the rotation matrix.
Definition frames.hpp:479
Vector UnitZ() const
Access to the underlying unitvectors of the rotation matrix.
Definition frames.hpp:491
Vector UnitX() const
Access to the underlying unitvectors of the rotation matrix.
Definition frames.hpp:467
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.hpp:610
Vector2 GetXZRot() const
Returns a 2D vector representing the equivalent rotation in the XZ plane that brings the Y axis onto ...
Definition frames.cpp:330
void setValue(float *oglmat)
Definition frames.hpp:657
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.hpp:615
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.hpp:620
void GetValue(double *xy) const
store vector components in array
Definition frames.hpp:787
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:143
bool addSegment(const std::string &segment_name, const std::string &hook_name, const Joint &joint, const double &q_rest, const Frame &f_tip=F_identity, const Inertia &M=Inertia::Zero())
Definition Armature.cpp:232
int addLimitConstraint(const std::string &segment_name, unsigned int dof, double _min, double _max)
Definition Armature.cpp:336
bool getRelativeFrame(Frame &result, const std::string &segment_name, const std::string &base_name=m_root)
Definition Armature.cpp:662
virtual const Frame & getPose(const unsigned int end_effector)
Definition Armature.cpp:655
int addConstraint(const std::string &segment_name, ConstraintCallback _function, void *_param=NULL, bool _freeParam=false, bool _substep=false)
Definition Armature.cpp:298
virtual int addEndEffector(const std::string &name)
Definition Armature.cpp:355
bool getSegment(const std::string &segment_name, const unsigned int q_size, const Joint *&p_joint, double &q_rest, double &q, const Frame *&p_tip)
Definition Armature.cpp:249
double getMaxEndEffectorChange()
Definition Armature.cpp:279
virtual bool setJointArray(const KDL::JntArray &joints)
Definition Armature.cpp:428
double getMaxJointChange()
Definition Armature.cpp:265
const void * getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp)
Definition Cache.cpp:546
bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0)
void substep(bool _substep)
virtual EIGEN_MAKE_ALIGNED_OPERATOR_NEW bool registerCallback(ConstraintCallback _function, void *_param)
virtual const ConstraintValues * getControlParameters(unsigned int *_nvalues)=0
virtual unsigned int getNrOfConstraints()
virtual e_matrix & getWq()
bool setCallback(MovingFrameCallback _function, void *_param)
virtual const KDL::Frame & getPose(const unsigned int end_effector=0)
Definition Object.hpp:38
bool addSolver(Solver *_solver)
Definition Scene.cpp:217
bool addConstraintSet(const std::string &name, ConstraintSet *task, const std::string &object1, const std::string &object2, const std::string &ee1="", const std::string &ee2="")
Definition Scene.cpp:182
bool update(double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true)
Definition Scene.cpp:317
bool addCache(Cache *_cache)
Definition Scene.cpp:227
bool initialize()
Definition Scene.cpp:237
bool addObject(const std::string &name, Object *object, UncontrolledObject *base=&Object::world, const std::string &baseFrame="")
Definition Scene.cpp:112
virtual void setParam(SolverParam param, double value)=0
#define rot(x, k)
#define e_matrix
IMETHOD void SetToZero(Vector &v)
Definition frames.inl:1104
float length(VecOp< float, D >) RET
float3x3 scale(float3x3 mat, float2 scale)
#define ANIM_FEEDBACK
ccl_device_inline dual3 fabs(const ccl_private dual3 &a)
Definition math_dual.h:241
#define T
#define R
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition rall1d.h:311
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Definition rall1d.h:367
double sign(double arg)
Definition utility.h:250
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition rall1d.h:429
INLINE Rall1d< T, V, S > sqr(const Rall1d< T, V, S > &arg)
Definition rall1d.h:351
const double PI
the value of pi
Definition utility.cpp:19
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition rall1d.h:319
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition utility.cpp:22
Vector Normalize(const Vector &, double eps=epsilon)
Definition frames.hpp:171
BMesh Iterators.
MINLINE float min_ff(float a, float b)
static int initialize_chain(Object *, bPoseChannel *pchan_tip, bConstraint *con)
void mul_m3_v3(const float M[3][3], float r[3])
static bool base_callback(const iTaSC::Timestamp &timestamp, const iTaSC::Frame &, iTaSC::Frame &next, void *param)
MINLINE void copy_v3_v3(float r[3], const float a[3])
unsigned int uint
Definition BKE_text.h:55
void copy_m3_m4(float m1[3][3], const float m2[4][4])
static IK_Scene * convert_tree(Depsgraph *depsgraph, Scene *blscene, Object *ob, bPoseChannel *pchan, float ctime)
static bool distance_callback(const iTaSC::Timestamp &timestamp, iTaSC::ConstraintValues *const _values, uint, void *_param)
bool invert_m3_m3(float inverse[3][3], const float mat[3][3])
static int init_scene(Object *ob)
void normalize_m3(float R[3][3]) ATTR_NONNULL()
float normalize_qt(float q[4])
bool invert_m4_m4(float inverse[4][4], const float mat[4][4])
void BKE_pose_where_is_bone(Depsgraph *depsgraph, Scene *scene, Object *ob, bPoseChannel *pchan, float ctime, bool do_extra)
unsigned short ushort
Definition BKE_text.h:56
static int initialize_scene(Object *ob, bPoseChannel *pchan_tip)
void axis_angle_to_mat3(float R[3][3], const float axis[3], float angle)
void blend_m4_m4m4(float out[4][4], const float dst[4][4], const float src[4][4], float srcweight)
static bool constraint_valid(bConstraint *con)
void mul_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4])
void itasc_initialize_tree(Depsgraph *depsgraph, Scene *scene, Object *ob, float ctime)
static bool target_callback(const iTaSC::Timestamp &, const iTaSC::Frame &, iTaSC::Frame &next, void *param)
static void execute_scene(Depsgraph *depsgraph, Scene *blscene, IK_Scene *ikscene, bItasc *ikparam, float ctime, float frtime)
void itasc_release_tree(Scene *, Object *, float)
static bool joint_callback(const iTaSC::Timestamp &, iTaSC::ConstraintValues *const _values, uint _nvalues, void *_param)
static void pose_rest(IK_Scene *ikscene)
MINLINE void mul_v3_fl(float r[3], float f)
void normalize_m4(float R[4][4]) ATTR_NONNULL()
static void create_scene(Depsgraph *depsgraph, Scene *scene, Object *ob, float ctime)
void eulO_to_mat3(float M[3][3], const float e[3], short order)
void quat_to_mat3(float m[3][3], const float q[4])
static bool is_cartesian_constraint(bConstraint *)
void itasc_clear_cache(bPose *pose)
static void GetJointRotation(KDL::Rotation &boneRot, int type, double *rot)
void * MEM_new_zeroed(size_t len, const char *str) ATTR_WARN_UNUSED_RESULT ATTR_ALLOC_SIZE(1) ATTR_NONNULL(2)
void BLI_remlink(ListBase *listbase, void *vlink) ATTR_NONNULL(1)
Definition listbase.cc:138
void mul_m4_m4m3(float R[4][4], const float A[4][4], const float B[3][3])
BMesh const char void * data
float[4] Vector4
void unit_m4(float m[4][4])
static double EulerAngleFromMatrix(const KDL::Rotation &R, int axis)
MINLINE float len_v3(const float a[3]) ATTR_WARN_UNUSED_RESULT
static void copypose_error(const iTaSC::ConstraintValues *values, uint, IK_Target *iktarget)
static bItasc DefIKParam
@ ITASC_INITIAL_REITERATION
@ ITASC_TRANSLATE_ROOT_BONES
void itasc_test_constraint(Object *, bConstraint *cons)
static bool copypose_callback(const iTaSC::Timestamp &, iTaSC::ConstraintValues *const _values, uint, void *_param)
MINLINE void sub_v3_v3v3(float r[3], const float a[3], const float b[3])
static void convert_pose(IK_Scene *ikscene)
void BLI_addtail(ListBase *listbase, void *vlink) ATTR_NONNULL(1)
Definition listbase.cc:118
void unit_m3(float m[3][3])
void(*)(const iTaSC::ConstraintValues *values, uint nvalues, IK_Target *iktarget) ErrorCallback
static int convert_channels(Depsgraph *depsgraph, IK_Scene *ikscene, PoseTree *tree, float ctime)
static void distance_error(const iTaSC::ConstraintValues *values, uint, IK_Target *iktarget)
void itasc_execute_tree(Depsgraph *depsgraph, Scene *scene, Object *ob, bPoseChannel *pchan_root, float ctime)
void BKE_constraint_target_matrix_get(struct Depsgraph *depsgraph, struct Scene *scene, struct bConstraint *con, int index, short ownertype, void *ownerdata, float mat[4][4], float ctime)
static double ComputeTwist(const KDL::Rotation &R)
void itasc_update_param(bPose *pose)
void copy_m4_m4(float m1[4][4], const float m2[4][4])
static void RemoveEulerAngleFromMatrix(KDL::Rotation &R, double angle, int axis)
MINLINE float len_v3v3(const float a[3], const float b[3]) ATTR_WARN_UNUSED_RESULT
static IK_Data * get_ikdata(bPose *pose)
void itasc_clear_data(bPose *pose)
void BKE_pose_itasc_init(bItasc *itasc)
void * MEM_new_array_zeroed(size_t len, size_t size, const char *str) ATTR_WARN_UNUSED_RESULT ATTR_ALLOC_SIZE(1
const Frame F_identity
unsigned int CacheTS
Definition Cache.hpp:33
#define X(TYPE)
KERNEL_FEATURE_LIGHT_PASSES KERNEL_FEATURE_PATH_GUIDING float
char name[258]
Definition DNA_ID.h:433
bPoseChannel * pchan
struct IK_Scene * first
iTaSC::Scene * scene
bConstraint * polarConstraint
std::vector< IK_Target * > targets
iTaSC::Solver * solver
Depsgraph * bldepsgraph
iTaSC::Armature * armature
IK_Channel * channels
KDL::JntArray jointArray
iTaSC::MovingFrame * base
iTaSC::Cache * cache
bPoseChannel * rootChannel
Depsgraph * bldepsgraph
std::string constraintName
bConstraint * blenderConstraint
iTaSC::MovingFrame * target
ErrorCallback errorCallback
std::string targetName
iTaSC::ConstraintSet * constraint
struct bPose * pose
float(* basis_change)[3][3]
ListBaseT< PoseTarget > targets
bPoseChannel ** pchan
struct RenderData r
ePose_IKSolverType iksolver
eItasc_Solver solver
struct bPoseChannel * parent
ListBaseT< struct PoseTree > iktree
ListBaseT< struct bConstraint > constraints
ePchan_ConstFlag constflag
bke::bPoseRuntime * runtime
ListBaseT< bPoseChannel > chanbase
struct ConstraintSingleValue * values
unsigned int substep
Definition Cache.hpp:41
unsigned int update
Definition Cache.hpp:44