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