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