clear some c++ warnings.
[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         int a, flag, njoint;
864
865         njoint = 0;
866         for(a=0, ikchan = ikscene->channels; a<ikscene->numchan; ++a, ++ikchan) {
867                 pchan= tree->pchan[a];
868                 ikchan->pchan = pchan;
869                 ikchan->parent = (a>0) ? tree->parent[a] : -1;
870                 ikchan->owner = ikscene->blArmature;
871                 
872                 /* set DoF flag */
873                 flag = 0;
874                 if(!(pchan->ikflag & BONE_IK_NO_XDOF) && !(pchan->ikflag & BONE_IK_NO_XDOF_TEMP) && 
875                         (!(pchan->ikflag & BONE_IK_XLIMIT) || pchan->limitmin[0]<0.f || pchan->limitmax[0]>0.f))
876                         flag |= IK_XDOF;
877                 if(!(pchan->ikflag & BONE_IK_NO_YDOF) && !(pchan->ikflag & BONE_IK_NO_YDOF_TEMP) &&
878                         (!(pchan->ikflag & BONE_IK_YLIMIT) || pchan->limitmin[1]<0.f || pchan->limitmax[1]>0.f))
879                         flag |= IK_YDOF;
880                 if(!(pchan->ikflag & BONE_IK_NO_ZDOF) && !(pchan->ikflag & BONE_IK_NO_ZDOF_TEMP) &&
881                         (!(pchan->ikflag & BONE_IK_ZLIMIT) || pchan->limitmin[2]<0.f || pchan->limitmax[2]>0.f))
882                         flag |= IK_ZDOF;
883                 
884                 if(tree->stretch && (pchan->ikstretch > 0.0)) {
885                         flag |= IK_TRANSY;
886                 }
887                 /*
888                 Logic to create the segments:
889                 RX,RY,RZ = rotational joints with no length
890                 RY(tip) = rotational joints with a fixed length arm = (0,length,0)
891                 TY = translational joint on Y axis
892                 F(pos) = fixed joint with an arm at position pos 
893                 Conversion rule of the above flags:
894                 -   ==> F(tip)
895                 X   ==> RX(tip)
896                 Y   ==> RY(tip)
897                 Z   ==> RZ(tip)
898                 XY  ==> RX+RY(tip)
899                 XZ  ==> RX+RZ(tip)
900                 YZ  ==> RZ+RY(tip)
901                 XYZ ==> full spherical unless there are limits, in which case RX+RZ+RY(tip)
902                 In case of stretch, tip=(0,0,0) and there is an additional TY joint
903                 The frame at last of these joints represents the tail of the bone. 
904                 The head is computed by a reverse translation on Y axis of the bone length
905                 or in case of TY joint, by the frame at previous joint.
906                 In case of separation of bones, there is an additional F(head) joint
907
908                 Computing rest pose and length is complicated: the solver works in world space
909                 Here is the logic:
910                 rest position is computed only from bone->bone_mat.
911                 bone length is computed from bone->length multiplied by the scaling factor of
912                 the armature. Non-uniform scaling will give bad result!
913
914                 */
915                 switch (flag & (IK_XDOF|IK_YDOF|IK_ZDOF))
916                 {
917                 default:
918                         ikchan->jointType = 0;
919                         ikchan->ndof = 0;
920                         break;
921                 case IK_XDOF:
922                         // RX only, get the X rotation
923                         ikchan->jointType = IK_XDOF;
924                         ikchan->ndof = 1;
925                         break;
926                 case IK_YDOF:
927                         // RY only, get the Y rotation
928                         ikchan->jointType = IK_YDOF;
929                         ikchan->ndof = 1;
930                         break;
931                 case IK_ZDOF:
932                         // RZ only, get the Zz rotation
933                         ikchan->jointType = IK_ZDOF;
934                         ikchan->ndof = 1;
935                         break;
936                 case IK_XDOF|IK_YDOF:
937                         ikchan->jointType = IK_XDOF|IK_YDOF;
938                         ikchan->ndof = 2;
939                         break;
940                 case IK_XDOF|IK_ZDOF:
941                         // RX+RZ
942                         ikchan->jointType = IK_SWING;
943                         ikchan->ndof = 2;
944                         break;
945                 case IK_YDOF|IK_ZDOF:
946                         // RZ+RY
947                         ikchan->jointType = IK_ZDOF|IK_YDOF;
948                         ikchan->ndof = 2;
949                         break;
950                 case IK_XDOF|IK_YDOF|IK_ZDOF:
951                         // spherical joint
952                         if (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_YLIMIT|BONE_IK_ZLIMIT))
953                                 // decompose in a Swing+RotY joint
954                                 ikchan->jointType = IK_SWING|IK_YDOF;
955                         else
956                                 ikchan->jointType = IK_REVOLUTE;
957                         ikchan->ndof = 3;
958                         break;
959                 }
960                 if (flag & IK_TRANSY) {
961                         ikchan->jointType |= IK_TRANSY;
962                         ikchan->ndof++;
963                 }
964                 njoint += ikchan->ndof;
965         }
966         // njoint is the joint coordinate, create the Joint Array
967         ikscene->jointArray.resize(njoint);
968         ikscene->numjoint = njoint;
969         return njoint;
970 }
971
972 // compute array of joint value corresponding to current pose
973 static void convert_pose(IK_Scene *ikscene)
974 {
975         KDL::Rotation boneRot;
976         bPoseChannel *pchan;
977         IK_Channel *ikchan;
978         Bone *bone;
979         float rmat[4][4];       // rest pose of bone with parent taken into account
980         float bmat[4][4];       // difference
981         float scale;
982         double *rot;
983         int a, joint;
984
985         // assume uniform scaling and take Y scale as general scale for the armature
986         scale = len_v3(ikscene->blArmature->obmat[1]);
987         rot = &ikscene->jointArray(0);
988         for(joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) {
989                 pchan= ikchan->pchan;
990                 bone= pchan->bone;
991                 
992                 if (pchan->parent) {
993                         unit_m4(bmat);
994                         mul_m4_m4m3(bmat, pchan->parent->pose_mat, bone->bone_mat);
995                 } else {
996                         copy_m4_m4(bmat, bone->arm_mat);
997                 }
998                 invert_m4_m4(rmat, bmat);
999                 mul_m4_m4m4(bmat, pchan->pose_mat, rmat);
1000                 normalize_m4(bmat);
1001                 boneRot.setValue(bmat[0]);
1002                 GetJointRotation(boneRot, ikchan->jointType, rot);
1003                 if (ikchan->jointType & IK_TRANSY) {
1004                         // compute actual length 
1005                         rot[ikchan->ndof-1] = len_v3v3(pchan->pose_tail, pchan->pose_head) * scale;
1006                 } 
1007                 rot += ikchan->ndof;
1008                 joint += ikchan->ndof;
1009         }
1010 }
1011
1012 // compute array of joint value corresponding to current pose
1013 static void rest_pose(IK_Scene *ikscene)
1014 {
1015         bPoseChannel *pchan;
1016         IK_Channel *ikchan;
1017         Bone *bone;
1018         float scale;
1019         double *rot;
1020         int a, joint;
1021
1022         // assume uniform scaling and take Y scale as general scale for the armature
1023         scale = len_v3(ikscene->blArmature->obmat[1]);
1024         // rest pose is 0 
1025         KDL::SetToZero(ikscene->jointArray);
1026         // except for transY joints
1027         rot = &ikscene->jointArray(0);
1028         for(joint=a=0, ikchan = ikscene->channels; a<ikscene->numchan && joint<ikscene->numjoint; ++a, ++ikchan) {
1029                 pchan= ikchan->pchan;
1030                 bone= pchan->bone;
1031
1032                 if (ikchan->jointType & IK_TRANSY)
1033                         rot[ikchan->ndof-1] = bone->length*scale;
1034                 rot += ikchan->ndof;
1035                 joint += ikchan->ndof;
1036         }
1037 }
1038
1039 static IK_Scene* convert_tree(Scene *blscene, Object *ob, bPoseChannel *pchan)
1040 {
1041         PoseTree *tree = (PoseTree*)pchan->iktree.first;
1042         PoseTarget *target;
1043         bKinematicConstraint *condata;
1044         bConstraint *polarcon;
1045         bItasc *ikparam;
1046         iTaSC::Armature* arm;
1047         iTaSC::Scene* scene;
1048         IK_Scene* ikscene;
1049         IK_Channel* ikchan;
1050         KDL::Frame initPose;
1051         KDL::Rotation boneRot;
1052         Bone *bone;
1053         int a, numtarget;
1054         unsigned int t;
1055         float length;
1056         bool ret = true, ingame;
1057         double *rot;
1058
1059         if (tree->totchannel == 0)
1060                 return NULL;
1061
1062         ikscene = new IK_Scene;
1063         ikscene->blscene = blscene;
1064         arm = new iTaSC::Armature();
1065         scene = new iTaSC::Scene();
1066         ikscene->channels = new IK_Channel[tree->totchannel];
1067         ikscene->numchan = tree->totchannel;
1068         ikscene->armature = arm;
1069         ikscene->scene = scene;
1070         ikparam = (bItasc*)ob->pose->ikparam;
1071         ingame = (ob->pose->flag & POSE_GAME_ENGINE);
1072         if (!ikparam) {
1073                 // you must have our own copy
1074                 ikparam = &DefIKParam;
1075         } else if (ingame) {
1076                 // tweak the param when in game to have efficient stepping
1077                 // using fixed substep is not effecient since frames in the GE are often
1078                 // shorter than in animation => move to auto step automatically and set
1079                 // the target substep duration via min/max
1080                 if (!(ikparam->flag & ITASC_AUTO_STEP)) {
1081                         float timestep = blscene->r.frs_sec_base/blscene->r.frs_sec;
1082                         if (ikparam->numstep > 0)
1083                                 timestep /= ikparam->numstep;
1084                         // with equal min and max, the algorythm will take this step and the indicative substep most of the time
1085                         ikparam->minstep = ikparam->maxstep = timestep;
1086                         ikparam->flag |= ITASC_AUTO_STEP;
1087                 }
1088         }
1089         if ((ikparam->flag & ITASC_SIMULATION) && !ingame)
1090                 // no cache in animation mode
1091                 ikscene->cache = new iTaSC::Cache();
1092
1093         switch (ikparam->solver) {
1094         case ITASC_SOLVER_SDLS:
1095                 ikscene->solver = new iTaSC::WSDLSSolver();
1096                 break;
1097         case ITASC_SOLVER_DLS:
1098                 ikscene->solver = new iTaSC::WDLSSolver();
1099                 break;
1100         default:
1101                 delete ikscene;
1102                 return NULL;
1103         }
1104         ikscene->blArmature = ob;
1105
1106         std::string  joint;
1107         std::string  root("root");
1108         std::string  parent;
1109         std::vector<double> weights;
1110         double weight[3];
1111         // assume uniform scaling and take Y scale as general scale for the armature
1112         float scale = len_v3(ob->obmat[1]);
1113         // build the array of joints corresponding to the IK chain
1114         convert_channels(ikscene, tree);
1115         if (ingame) {
1116                 // in the GE, set the initial joint angle to match the current pose
1117                 // this will update the jointArray in ikscene
1118                 convert_pose(ikscene);
1119         } else {
1120                 // in Blender, the rest pose is always 0 for joints
1121                 rest_pose(ikscene);
1122         }
1123         rot = &ikscene->jointArray(0);
1124         for(a=0, ikchan = ikscene->channels; a<tree->totchannel; ++a, ++ikchan) {
1125                 pchan= ikchan->pchan;
1126                 bone= pchan->bone;
1127
1128                 KDL::Frame tip(iTaSC::F_identity);
1129                 Vector3 *fl = bone->bone_mat;
1130                 KDL::Rotation brot(
1131                                                    fl[0][0], fl[1][0], fl[2][0],
1132                                                    fl[0][1], fl[1][1], fl[2][1],
1133                                                    fl[0][2], fl[1][2], fl[2][2]);
1134                 KDL::Vector bpos(bone->head[0], bone->head[1], bone->head[2]);
1135                 bpos = bpos*scale;
1136                 KDL::Frame head(brot, bpos);
1137                 
1138                 // rest pose length of the bone taking scaling into account
1139                 length= bone->length*scale;
1140                 parent = (a > 0) ? ikscene->channels[tree->parent[a]].tail : root;
1141                 // first the fixed segment to the bone head
1142                 if (head.p.Norm() > KDL::epsilon || head.M.GetRot().Norm() > KDL::epsilon) {
1143                         joint = bone->name;
1144                         joint += ":H";
1145                         ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, head);
1146                         parent = joint;
1147                 }
1148                 if (!(ikchan->jointType & IK_TRANSY)) {
1149                         // fixed length, put it in tip
1150                         tip.p[1] = length;
1151                 }
1152                 joint = bone->name;
1153                 weight[0] = (1.0-pchan->stiffness[0]);
1154                 weight[1] = (1.0-pchan->stiffness[1]);
1155                 weight[2] = (1.0-pchan->stiffness[2]);
1156                 switch (ikchan->jointType & ~IK_TRANSY)
1157                 {
1158                 case 0:
1159                         // fixed bone
1160                         if (!(ikchan->jointType & IK_TRANSY)) {
1161                                 joint += ":F";
1162                                 ret = arm->addSegment(joint, parent, KDL::Joint::None, 0.0, tip);
1163                         }
1164                         break;
1165                 case IK_XDOF:
1166                         // RX only, get the X rotation
1167                         joint += ":RX";
1168                         ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0], tip);
1169                         weights.push_back(weight[0]);
1170                         break;
1171                 case IK_YDOF:
1172                         // RY only, get the Y rotation
1173                         joint += ":RY";
1174                         ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[0], tip);
1175                         weights.push_back(weight[1]);
1176                         break;
1177                 case IK_ZDOF:
1178                         // RZ only, get the Zz rotation
1179                         joint += ":RZ";
1180                         ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0], tip);
1181                         weights.push_back(weight[2]);
1182                         break;
1183                 case IK_XDOF|IK_YDOF:
1184                         joint += ":RX";
1185                         ret = arm->addSegment(joint, parent, KDL::Joint::RotX, rot[0]);
1186                         weights.push_back(weight[0]);
1187                         if (ret) {
1188                                 parent = joint;
1189                                 joint = bone->name;
1190                                 joint += ":RY";
1191                                 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
1192                                 weights.push_back(weight[1]);
1193                         }
1194                         break;
1195                 case IK_SWING:
1196                         joint += ":SW";
1197                         ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0], tip);
1198                         weights.push_back(weight[0]);
1199                         weights.push_back(weight[2]);
1200                         break;
1201                 case IK_YDOF|IK_ZDOF:
1202                         // RZ+RY
1203                         joint += ":RZ";
1204                         ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, rot[0]);
1205                         weights.push_back(weight[2]);
1206                         if (ret) {
1207                                 parent = joint;
1208                                 joint = bone->name;
1209                                 joint += ":RY";
1210                                 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[1], tip);
1211                                 weights.push_back(weight[1]);
1212                         }
1213                         break;
1214                 case IK_SWING|IK_YDOF:
1215                         // decompose in a Swing+RotY joint
1216                         joint += ":SW";
1217                         ret = arm->addSegment(joint, parent, KDL::Joint::Swing, rot[0]);
1218                         weights.push_back(weight[0]);
1219                         weights.push_back(weight[2]);
1220                         if (ret) {
1221                                 parent = joint;
1222                                 joint = bone->name;
1223                                 joint += ":RY";
1224                                 ret = arm->addSegment(joint, parent, KDL::Joint::RotY, rot[2], tip);
1225                                 weights.push_back(weight[1]);
1226                         }
1227                         break;
1228                 case IK_REVOLUTE:
1229                         joint += ":SJ";
1230                         ret = arm->addSegment(joint, parent, KDL::Joint::Sphere, rot[0], tip);
1231                         weights.push_back(weight[0]);
1232                         weights.push_back(weight[1]);
1233                         weights.push_back(weight[2]);
1234                         break;
1235                 }
1236                 if (ret && (ikchan->jointType & IK_TRANSY)) {
1237                         parent = joint;
1238                         joint = bone->name;
1239                         joint += ":TY";
1240                         ret = arm->addSegment(joint, parent, KDL::Joint::TransY, rot[ikchan->ndof-1]);
1241                         float ikstretch = pchan->ikstretch*pchan->ikstretch;
1242                         weight[1] = (1.0-MIN2(1.0-ikstretch, 0.99));
1243                         weights.push_back(weight[1]);
1244                 }
1245                 if (!ret)
1246                         // error making the armature??
1247                         break;
1248                 // joint points to the segment that correspond to the bone per say
1249                 ikchan->tail = joint;
1250                 ikchan->head = parent;
1251                 // in case of error
1252                 ret = false;
1253                 if ((ikchan->jointType & IK_XDOF) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ROTCTL))) {
1254                         joint = bone->name;
1255                         joint += ":RX";
1256                         if (pchan->ikflag & BONE_IK_XLIMIT) {
1257                                 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0)
1258                                         break;
1259                         }
1260                         if (pchan->ikflag & BONE_IK_ROTCTL) {
1261                                 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
1262                                         break;
1263                         }
1264                 }
1265                 if ((ikchan->jointType & IK_YDOF) && (pchan->ikflag & (BONE_IK_YLIMIT|BONE_IK_ROTCTL))) {
1266                         joint = bone->name;
1267                         joint += ":RY";
1268                         if (pchan->ikflag & BONE_IK_YLIMIT) {
1269                                 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[1], pchan->limitmax[1]) < 0)
1270                                         break;
1271                         }
1272                         if (pchan->ikflag & BONE_IK_ROTCTL) {
1273                                 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
1274                                         break;
1275                         }
1276                 }
1277                 if ((ikchan->jointType & IK_ZDOF) && (pchan->ikflag & (BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) {
1278                         joint = bone->name;
1279                         joint += ":RZ";
1280                         if (pchan->ikflag & BONE_IK_ZLIMIT) {
1281                                 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[2], pchan->limitmax[2]) < 0)
1282                                         break;
1283                         }
1284                         if (pchan->ikflag & BONE_IK_ROTCTL) {
1285                                 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
1286                                         break;
1287                         }
1288                 }
1289                 if ((ikchan->jointType & IK_SWING) && (pchan->ikflag & (BONE_IK_XLIMIT|BONE_IK_ZLIMIT|BONE_IK_ROTCTL))) {
1290                         joint = bone->name;
1291                         joint += ":SW";
1292                         if (pchan->ikflag & BONE_IK_XLIMIT) {
1293                                 if (arm->addLimitConstraint(joint, 0, pchan->limitmin[0], pchan->limitmax[0]) < 0)
1294                                         break;
1295                         }
1296                         if (pchan->ikflag & BONE_IK_ZLIMIT) {
1297                                 if (arm->addLimitConstraint(joint, 1, pchan->limitmin[2], pchan->limitmax[2]) < 0)
1298                                         break;
1299                         }
1300                         if (pchan->ikflag & BONE_IK_ROTCTL) {
1301                                 if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
1302                                         break;
1303                         }
1304                 }
1305                 if ((ikchan->jointType & IK_REVOLUTE) && (pchan->ikflag & BONE_IK_ROTCTL)) {
1306                         joint = bone->name;
1307                         joint += ":SJ";
1308                         if (arm->addConstraint(joint, joint_callback, ikchan, false, false) < 0)
1309                                 break;
1310                 }
1311                 //  no error, so restore
1312                 ret = true;
1313                 rot += ikchan->ndof;
1314         }
1315         if (!ret) {
1316                 delete ikscene;
1317                 return NULL;
1318         }
1319         // for each target, we need to add an end effector in the armature
1320         for (numtarget=0, polarcon=NULL, ret = true, target=(PoseTarget*)tree->targets.first; target; target=(PoseTarget*)target->next) {
1321                 condata= (bKinematicConstraint*)target->con->data;
1322                 pchan = tree->pchan[target->tip];
1323
1324                 if (is_cartesian_constraint(target->con)) {
1325                         // add the end effector
1326                         IK_Target* iktarget = new IK_Target();
1327                         ikscene->targets.push_back(iktarget);
1328                         iktarget->ee = arm->addEndEffector(ikscene->channels[target->tip].tail);
1329                         if (iktarget->ee == -1) {
1330                                 ret = false;
1331                                 break;
1332                         }
1333                         // initialize all the fields that we can set at this time
1334                         iktarget->blenderConstraint = target->con;
1335                         iktarget->channel = target->tip;
1336                         iktarget->simulation = (ikparam->flag & ITASC_SIMULATION);
1337                         iktarget->rootChannel = ikscene->channels[0].pchan;
1338                         iktarget->owner = ob;
1339                         iktarget->targetName = pchan->bone->name;
1340                         iktarget->targetName += ":T:";
1341                         iktarget->targetName += target->con->name;
1342                         iktarget->constraintName = pchan->bone->name;
1343                         iktarget->constraintName += ":C:";
1344                         iktarget->constraintName += target->con->name;
1345                         numtarget++;
1346                         if (condata->poletar)
1347                                 // this constraint has a polar target
1348                                 polarcon = target->con;
1349                 }
1350         }
1351         // deal with polar target if any
1352         if (numtarget == 1 && polarcon) {
1353                 ikscene->polarConstraint = polarcon;
1354         }
1355         // we can now add the armature
1356         // the armature is based on a moving frame. 
1357         // initialize with the correct position in case there is no cache
1358         base_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, ikscene);
1359         ikscene->base = new iTaSC::MovingFrame(initPose);
1360         ikscene->base->setCallback(base_callback, ikscene);
1361         std::string armname;
1362         armname = ob->id.name;
1363         armname += ":B";
1364         ret = scene->addObject(armname, ikscene->base);
1365         armname = ob->id.name;
1366         armname += ":AR";
1367         if (ret)
1368                 ret = scene->addObject(armname, ikscene->armature, ikscene->base);
1369         if (!ret) {
1370                 delete ikscene;
1371                 return NULL;
1372         }
1373         // set the weight
1374         e_matrix& Wq = arm->getWq();
1375         assert(Wq.cols() == (int)weights.size());
1376         for (int q=0; q<Wq.cols(); q++)
1377                 Wq(q,q)=weights[q];
1378         // get the inverse rest pose frame of the base to compute relative rest pose of end effectors
1379         // this is needed to handle the enforce parameter
1380         // ikscene->pchan[0] is the root channel of the tree
1381         // if it has no parent, then it's just the identify Frame
1382         float invBaseFrame[4][4];
1383         pchan = ikscene->channels[0].pchan;
1384         if (pchan->parent) {
1385                 // it has a parent, get the pose matrix from it 
1386                 float baseFrame[4][4];
1387                 pchan = pchan->parent;  
1388                 copy_m4_m4(baseFrame, pchan->bone->arm_mat);
1389                 // move to the tail and scale to get rest pose of armature base
1390                 copy_v3_v3(baseFrame[3], pchan->bone->arm_tail);
1391                 invert_m4_m4(invBaseFrame, baseFrame);
1392         } else {
1393                 unit_m4(invBaseFrame);
1394         }
1395         // finally add the constraint
1396         for (t=0; t<ikscene->targets.size(); t++) {
1397                 IK_Target* iktarget = ikscene->targets[t];
1398                 iktarget->blscene = blscene;
1399                 condata= (bKinematicConstraint*)iktarget->blenderConstraint->data;
1400                 pchan = tree->pchan[iktarget->channel];
1401                 unsigned int controltype, bonecnt;
1402                 double bonelen;
1403                 float mat[4][4];
1404
1405                 // add the end effector
1406                 // estimate the average bone length, used to clamp feedback error
1407                 for (bonecnt=0, bonelen=0.f, a=iktarget->channel; a>=0; a=tree->parent[a], bonecnt++)
1408                         bonelen += scale*tree->pchan[a]->bone->length;
1409                 bonelen /= bonecnt;             
1410
1411                 // store the rest pose of the end effector to compute enforce target
1412                 copy_m4_m4(mat, pchan->bone->arm_mat);
1413                 copy_v3_v3(mat[3], pchan->bone->arm_tail);
1414                 // get the rest pose relative to the armature base
1415                 mul_m4_m4m4(iktarget->eeRest, mat, invBaseFrame);
1416                 iktarget->eeBlend = (!ikscene->polarConstraint && condata->type==CONSTRAINT_IK_COPYPOSE) ? true : false;
1417                 // use target_callback to make sure the initPose includes enforce coefficient
1418                 target_callback(iTaSC::Timestamp(), iTaSC::F_identity, initPose, iktarget);
1419                 iktarget->target = new iTaSC::MovingFrame(initPose);
1420                 iktarget->target->setCallback(target_callback, iktarget);
1421                 ret = scene->addObject(iktarget->targetName, iktarget->target);
1422                 if (!ret)
1423                         break;
1424
1425                 switch (condata->type) {
1426                 case CONSTRAINT_IK_COPYPOSE:
1427                         controltype = 0;
1428                         if (condata->flag & CONSTRAINT_IK_ROT) {
1429                                 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_X))
1430                                         controltype |= iTaSC::CopyPose::CTL_ROTATIONX;
1431                                 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Y))
1432                                         controltype |= iTaSC::CopyPose::CTL_ROTATIONY;
1433                                 if (!(condata->flag & CONSTRAINT_IK_NO_ROT_Z))
1434                                         controltype |= iTaSC::CopyPose::CTL_ROTATIONZ;
1435                         }
1436                         if (condata->flag & CONSTRAINT_IK_POS) {
1437                                 if (!(condata->flag & CONSTRAINT_IK_NO_POS_X))
1438                                         controltype |= iTaSC::CopyPose::CTL_POSITIONX;
1439                                 if (!(condata->flag & CONSTRAINT_IK_NO_POS_Y))
1440                                         controltype |= iTaSC::CopyPose::CTL_POSITIONY;
1441                                 if (!(condata->flag & CONSTRAINT_IK_NO_POS_Z))
1442                                         controltype |= iTaSC::CopyPose::CTL_POSITIONZ;
1443                         }
1444                         if (controltype) {
1445                                 iktarget->constraint = new iTaSC::CopyPose(controltype, controltype, bonelen);
1446                                 // set the gain
1447                                 if (controltype & iTaSC::CopyPose::CTL_POSITION)
1448                                         iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_POSITION, iTaSC::ACT_ALPHA, condata->weight);
1449                                 if (controltype & iTaSC::CopyPose::CTL_ROTATION)
1450                                         iktarget->constraint->setControlParameter(iTaSC::CopyPose::ID_ROTATION, iTaSC::ACT_ALPHA, condata->orientweight);
1451                                 iktarget->constraint->registerCallback(copypose_callback, iktarget);
1452                                 iktarget->errorCallback = copypose_error;
1453                                 iktarget->controlType = controltype;
1454                                 // add the constraint
1455                                 if (condata->flag & CONSTRAINT_IK_TARGETAXIS)
1456                                         ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, iktarget->targetName, armname, "", ikscene->channels[iktarget->channel].tail);
1457                                 else
1458                                         ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
1459                         }
1460                         break;
1461                 case CONSTRAINT_IK_DISTANCE:
1462                         iktarget->constraint = new iTaSC::Distance(bonelen);
1463                         iktarget->constraint->setControlParameter(iTaSC::Distance::ID_DISTANCE, iTaSC::ACT_VALUE, condata->dist);
1464                         iktarget->constraint->registerCallback(distance_callback, iktarget);
1465                         iktarget->errorCallback = distance_error;
1466                         // we can update the weight on each substep
1467                         iktarget->constraint->substep(true);
1468                         // add the constraint
1469                         ret = scene->addConstraintSet(iktarget->constraintName, iktarget->constraint, armname, iktarget->targetName, ikscene->channels[iktarget->channel].tail);
1470                         break;
1471                 }
1472                 if (!ret)
1473                         break;
1474         }
1475         if (!ret ||
1476                 !scene->addCache(ikscene->cache) ||
1477                 !scene->addSolver(ikscene->solver) ||
1478                 !scene->initialize()) {
1479                 delete ikscene;
1480                 ikscene = NULL;
1481         }
1482         return ikscene;
1483 }
1484
1485 static void create_scene(Scene *scene, Object *ob)
1486 {
1487         bPoseChannel *pchan;
1488
1489         // create the IK scene
1490         for(pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) {
1491                 // by construction there is only one tree
1492                 PoseTree *tree = (PoseTree*)pchan->iktree.first;
1493                 if (tree) {
1494                         IK_Data* ikdata = get_ikdata(ob->pose);
1495                         // convert tree in iTaSC::Scene
1496                         IK_Scene* ikscene = convert_tree(scene, ob, pchan);
1497                         if (ikscene) {
1498                                 ikscene->next = ikdata->first;
1499                                 ikdata->first = ikscene;
1500                         }
1501                         // delete the trees once we are done
1502                         while(tree) {
1503                                 BLI_remlink(&pchan->iktree, tree);
1504                                 BLI_freelistN(&tree->targets);
1505                                 if(tree->pchan) MEM_freeN(tree->pchan);
1506                                 if(tree->parent) MEM_freeN(tree->parent);
1507                                 if(tree->basis_change) MEM_freeN(tree->basis_change);
1508                                 MEM_freeN(tree);
1509                                 tree = (PoseTree*)pchan->iktree.first;
1510                         }
1511                 }
1512         }
1513 }
1514
1515 static void init_scene(Object *ob)
1516 {
1517         if (ob->pose->ikdata) {
1518                 for(IK_Scene* scene = ((IK_Data*)ob->pose->ikdata)->first;
1519                         scene != NULL;
1520                         scene = scene->next) {
1521                         scene->channels[0].pchan->flag |= POSE_IKTREE;
1522                 }
1523         }
1524 }
1525
1526 static void execute_scene(Scene* blscene, IK_Scene* ikscene, bItasc* ikparam, float ctime, float frtime)
1527 {
1528         int i;
1529         IK_Channel* ikchan;
1530         if (ikparam->flag & ITASC_SIMULATION) {
1531                 for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) {
1532                         // In simulation mode we don't allow external contraint to change our bones, mark the channel done
1533                         // also tell Blender that this channel is part of IK tree (cleared on each where_is_pose()
1534                         ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN);
1535                         ikchan->jointValid = 0;
1536                 }
1537         } else {
1538                 // in animation mode, we must get the bone position from action and constraints
1539                 for(i=0, ikchan=ikscene->channels; i<ikscene->numchan; i++, ++ikchan) {
1540                         if (!(ikchan->pchan->flag & POSE_DONE))
1541                                 where_is_pose_bone(blscene, ikscene->blArmature, ikchan->pchan, ctime, 1);
1542                         // tell blender that this channel was controlled by IK, it's cleared on each where_is_pose()
1543                         ikchan->pchan->flag |= (POSE_DONE|POSE_CHAIN);
1544                         ikchan->jointValid = 0;
1545                 }
1546         }
1547         // only run execute the scene if at least one of our target is enabled
1548         for (i=ikscene->targets.size(); i > 0; --i) {
1549                 IK_Target* iktarget = ikscene->targets[i-1];
1550                 if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF))
1551                         break;
1552         }
1553         if (i == 0 && ikscene->armature->getNrOfConstraints() == 0)
1554                 // all constraint disabled
1555                 return;
1556
1557         // compute timestep
1558         double timestamp = ctime * frtime + 2147483.648;
1559         double timestep = frtime;
1560         bool reiterate = (ikparam->flag & ITASC_REITERATION) ? true : false;
1561         int numstep = (ikparam->flag & ITASC_AUTO_STEP) ? 0 : ikparam->numstep;
1562         bool simulation = true;
1563
1564         if (ikparam->flag & ITASC_SIMULATION) {
1565                 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
1566         } 
1567         else {
1568                 // in animation mode we start from the pose after action and constraint
1569                 convert_pose(ikscene);
1570                 ikscene->armature->setJointArray(ikscene->jointArray);
1571                 // and we don't handle velocity
1572                 reiterate = true;
1573                 simulation = false;
1574                 // time is virtual, so take fixed value for velocity parameters (see itasc_update_param)
1575                 // and choose 1s timestep to allow having velocity parameters in radiant 
1576                 timestep = 1.0;
1577                 // use auto setup to let the solver test the variation of the joints
1578                 numstep = 0;
1579         }
1580                 
1581         if (ikscene->cache && !reiterate && simulation) {
1582                 iTaSC::CacheTS sts, cts;
1583                 sts = cts = (iTaSC::CacheTS)(timestamp*1000.0+0.5);
1584                 if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || cts == 0) {
1585                         // the cache is empty before this time, reiterate
1586                         if (ikparam->flag & ITASC_INITIAL_REITERATION)
1587                                 reiterate = true;
1588                 } else {
1589                         // can take the cache as a start point.
1590                         sts -= cts;
1591                         timestep = sts/1000.0;
1592                 }
1593         }
1594         // don't cache if we are reiterating because we don't want to distroy the cache unnecessarily
1595         ikscene->scene->update(timestamp, timestep, numstep, false, !reiterate, simulation);
1596         if (reiterate) {
1597                 // how many times do we reiterate?
1598                 for (i=0; i<ikparam->numiter; i++) {
1599                         if (ikscene->armature->getMaxJointChange() < ikparam->precision ||
1600                                 ikscene->armature->getMaxEndEffectorChange() < ikparam->precision)
1601                                 break;
1602                         ikscene->scene->update(timestamp, timestep, numstep, true, false, simulation);
1603                 }
1604                 if (simulation) {
1605                         // one more fake iteration to cache
1606                         ikscene->scene->update(timestamp, 0.0, 1, true, true, true);
1607                 }
1608         }
1609         // compute constraint error
1610         for (i=ikscene->targets.size(); i > 0; --i) {
1611                 IK_Target* iktarget = ikscene->targets[i-1];
1612                 if (!(iktarget->blenderConstraint->flag & CONSTRAINT_OFF)) {
1613                         unsigned int nvalues;
1614                         const iTaSC::ConstraintValues* values;
1615                         values = iktarget->constraint->getControlParameters(&nvalues);
1616                         iktarget->errorCallback(values, nvalues, iktarget);
1617                 }
1618         }
1619         // Apply result to bone:
1620         // walk the ikscene->channels
1621         // for each, get the Frame of the joint corresponding to the bone relative to its parent
1622         // combine the parent and the joint frame to get the frame relative to armature
1623         // a backward translation of the bone length gives the head
1624         // if TY, compute the scale as the ratio of the joint length with rest pose length
1625         iTaSC::Armature* arm = ikscene->armature;
1626         KDL::Frame frame;
1627         double q_rest[3], q[3];
1628         const KDL::Joint* joint;
1629         const KDL::Frame* tip;
1630         bPoseChannel* pchan;
1631         float scale;
1632         float length;
1633         float yaxis[3];
1634         for (i=0, ikchan=ikscene->channels; i<ikscene->numchan; ++i, ++ikchan) {
1635                 if (i == 0) {
1636                         if (!arm->getRelativeFrame(frame, ikchan->tail))
1637                                 break;
1638                         // this frame is relative to base, make it relative to object
1639                         ikchan->frame = ikscene->baseFrame * frame;
1640                 } 
1641                 else {
1642                         if (!arm->getRelativeFrame(frame, ikchan->tail, ikscene->channels[ikchan->parent].tail))
1643                                 break;
1644                         // combine with parent frame to get frame relative to object
1645                         ikchan->frame = ikscene->channels[ikchan->parent].frame * frame;
1646                 }
1647                 // ikchan->frame is the tail frame relative to object
1648                 // get bone length
1649                 if (!arm->getSegment(ikchan->tail, 3, joint, q_rest[0], q[0], tip))
1650                         break;
1651                 if (joint->getType() == KDL::Joint::TransY) {
1652                         // stretch bones have a TY joint, compute the scale
1653                         scale = (float)(q[0]/q_rest[0]);
1654                         // the length is the joint itself
1655                         length = (float)q[0];
1656                 } 
1657                 else {
1658                         scale = 1.0f;
1659                         // for fixed bone, the length is in the tip (always along Y axis)
1660                         length = tip->p(1);
1661                 }
1662                 // ready to compute the pose mat
1663                 pchan = ikchan->pchan;
1664                 // tail mat
1665                 ikchan->frame.getValue(&pchan->pose_mat[0][0]);
1666                 VECCOPY(pchan->pose_tail, pchan->pose_mat[3]);
1667                 // shift to head
1668                 VECCOPY(yaxis, pchan->pose_mat[1]);
1669                 mul_v3_fl(yaxis, length);
1670                 sub_v3_v3v3(pchan->pose_mat[3], pchan->pose_mat[3], yaxis);
1671                 VECCOPY(pchan->pose_head, pchan->pose_mat[3]);
1672                 // add scale
1673                 mul_v3_fl(pchan->pose_mat[0], scale);
1674                 mul_v3_fl(pchan->pose_mat[1], scale);
1675                 mul_v3_fl(pchan->pose_mat[2], scale);
1676         }
1677         if (i<ikscene->numchan) {
1678                 // big problem
1679                 ;
1680         }
1681 }
1682
1683 //---------------------------------------------------
1684 // plugin interface
1685 //
1686 void itasc_initialize_tree(struct Scene *scene, Object *ob, float ctime)
1687 {
1688         bPoseChannel *pchan;
1689         int count = 0;
1690
1691         if (ob->pose->ikdata != NULL && !(ob->pose->flag & POSE_WAS_REBUILT)) {
1692                 init_scene(ob);
1693                 return;
1694         }
1695         // first remove old scene
1696         itasc_clear_data(ob->pose);
1697         // we should handle all the constraint and mark them all disabled
1698         // for blender but we'll start with the IK constraint alone
1699         for(pchan= (bPoseChannel *)ob->pose->chanbase.first; pchan; pchan= (bPoseChannel *)pchan->next) {
1700                 if(pchan->constflag & PCHAN_HAS_IK)
1701                         count += initialize_scene(ob, pchan);
1702         }
1703         // if at least one tree, create the scenes from the PoseTree stored in the channels 
1704         if (count)
1705                 create_scene(scene, ob);
1706         itasc_update_param(ob->pose);
1707         // make sure we don't rebuilt until the user changes something important
1708         ob->pose->flag &= ~POSE_WAS_REBUILT;
1709 }
1710
1711 void itasc_execute_tree(struct Scene *scene, struct Object *ob,  struct bPoseChannel *pchan, float ctime)
1712 {
1713         if (ob->pose->ikdata) {
1714                 IK_Data* ikdata = (IK_Data*)ob->pose->ikdata;
1715                 bItasc* ikparam = (bItasc*) ob->pose->ikparam;
1716                 // we need default parameters
1717                 if (!ikparam) ikparam = &DefIKParam;
1718
1719                 for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
1720                         if (ikscene->channels[0].pchan == pchan) {
1721                                 float timestep = scene->r.frs_sec_base/scene->r.frs_sec;
1722                                 if (ob->pose->flag & POSE_GAME_ENGINE) {
1723                                         timestep = ob->pose->ctime;
1724                                         // limit the timestep to avoid excessive number of iteration
1725                                         if (timestep > 0.2f)
1726                                                 timestep = 0.2f;
1727                                 }
1728                                 execute_scene(scene, ikscene, ikparam, ctime, timestep);
1729                                 break;
1730                         }
1731                 }
1732         }
1733 }
1734
1735 void itasc_release_tree(struct Scene *scene, struct Object *ob,  float ctime)
1736 {
1737         // not used for iTaSC
1738 }
1739
1740 void itasc_clear_data(struct bPose *pose)
1741 {
1742         if (pose->ikdata) {
1743                 IK_Data* ikdata = (IK_Data*)pose->ikdata;
1744                 for (IK_Scene* scene = ikdata->first; scene; scene = ikdata->first) {
1745                         ikdata->first = scene->next;
1746                         delete scene;
1747                 }
1748                 MEM_freeN(ikdata);
1749                 pose->ikdata = NULL;
1750         }
1751 }
1752
1753 void itasc_clear_cache(struct bPose *pose)
1754 {
1755         if (pose->ikdata) {
1756                 IK_Data* ikdata = (IK_Data*)pose->ikdata;
1757                 for (IK_Scene* scene = ikdata->first; scene; scene = scene->next) {
1758                         if (scene->cache)
1759                                 // clear all cache but leaving the timestamp 0 (=rest pose)
1760                                 scene->cache->clearCacheFrom(NULL, 1);
1761                 }
1762         }
1763 }
1764
1765 void itasc_update_param(struct bPose *pose)
1766 {
1767         if (pose->ikdata && pose->ikparam) {
1768                 IK_Data* ikdata = (IK_Data*)pose->ikdata;
1769                 bItasc* ikparam = (bItasc*)pose->ikparam;
1770                 for (IK_Scene* ikscene = ikdata->first; ikscene; ikscene = ikscene->next) {
1771                         double armlength = ikscene->armature->getArmLength();
1772                         ikscene->solver->setParam(iTaSC::Solver::DLS_LAMBDA_MAX, ikparam->dampmax*armlength);
1773                         ikscene->solver->setParam(iTaSC::Solver::DLS_EPSILON, ikparam->dampeps*armlength);
1774                         if (ikparam->flag & ITASC_SIMULATION) {
1775                                 ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, ikparam->minstep);
1776                                 ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, ikparam->maxstep);
1777                                 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, ikparam->maxvel);
1778                                 ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, ikparam->feedback);
1779                         } else {
1780                                 // in animation mode timestep is 1s by convention => 
1781                                 // qmax becomes radiant and feedback becomes fraction of error gap corrected in one iteration
1782                                 ikscene->scene->setParam(iTaSC::Scene::MIN_TIMESTEP, 1.0);
1783                                 ikscene->scene->setParam(iTaSC::Scene::MAX_TIMESTEP, 1.0);
1784                                 ikscene->solver->setParam(iTaSC::Solver::DLS_QMAX, 0.52);
1785                                 ikscene->armature->setControlParameter(CONSTRAINT_ID_ALL, iTaSC::Armature::ID_JOINT, iTaSC::ACT_FEEDBACK, 0.8);
1786                         }
1787                 }
1788         }
1789 }
1790
1791 void itasc_test_constraint(struct Object *ob, struct bConstraint *cons)
1792 {
1793         struct bKinematicConstraint *data = (struct bKinematicConstraint *)cons->data;
1794
1795         /* only for IK constraint */
1796         if (cons->type != CONSTRAINT_TYPE_KINEMATIC || data == NULL)
1797                 return;
1798
1799         switch (data->type) {
1800         case CONSTRAINT_IK_COPYPOSE:
1801         case CONSTRAINT_IK_DISTANCE:
1802                 /* cartesian space constraint */
1803                 break;
1804         }
1805 }
1806