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