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