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