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