source/blender/ikplugin/intern/itasc_plugin.cpp

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