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