source/blender/src/editarmature_retarget.c

   1 /**
   2  * $Id:
   3  *
   4  * ***** BEGIN GPL LICENSE BLOCK *****
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU General Public License
   8  * as published by the Free Software Foundation; either version 2
   9  * of the License, or (at your option) any later version.
  10  *
  11  * This program is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14  * GNU General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License
  17  * along with this program; if not, write to the Free Software Foundation,
  18  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  19  *
  20  * Contributor(s): Martin Poirier
  21  *
  22  * ***** END GPL LICENSE BLOCK *****
  23  * autoarmature.c: Interface for automagically manipulating armature (retarget, created, ...)
  24  */
  25
  26 #include <ctype.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29 #include <math.h>
  30
  31 #ifdef HAVE_CONFIG_H
  32 #include <config.h>
  33 #endif
  34
  35 #include "MEM_guardedalloc.h"
  36
  37 #include "PIL_time.h"
  38
  39 #include "DNA_ID.h"
  40 #include "DNA_action_types.h"
  41 #include "DNA_armature_types.h"
  42 #include "DNA_constraint_types.h"
  43 #include "DNA_mesh_types.h"
  44 #include "DNA_meshdata_types.h"
  45 #include "DNA_object_types.h"
  46 #include "DNA_scene_types.h"
  47 #include "DNA_view3d_types.h"
  48
  49 #include "BLI_blenlib.h"
  50 #include "BLI_arithb.h"
  51 #include "BLI_editVert.h"
  52 #include "BLI_ghash.h"
  53 #include "BLI_graph.h"
  54 #include "BLI_rand.h"
  55 #include "BLI_threads.h"
  56
  57 #include "BDR_editobject.h"
  58
  59 #include "BKE_global.h"
  60 #include "BKE_utildefines.h"
  61 #include "BKE_constraint.h"
  62 #include "BKE_armature.h"
  63
  64 #include "BIF_editarmature.h"
  65 #include "BIF_retarget.h"
  66 #include "BIF_space.h"
  67 #include "BIF_toolbox.h"
  68
  69 #include "PIL_time.h"
  70
  71 #include "mydevice.h"
  72 #include "reeb.h" // FIX ME
  73 #include "blendef.h"
  74
  75 /************ RIG RETARGET DATA STRUCTURES ***************/
  76
  77 typedef struct MemoNode {
  78         float   weight;
  79         int     next;
  80 } MemoNode;
  81
  82 typedef struct RetargetParam {
  83         RigGraph        *rigg;
  84         RigArc          *iarc;
  85         RigNode         *inode_start;
  86 } RetargetParam;
  87
  88 typedef enum
  89 {
  90         RETARGET_LENGTH,
  91         RETARGET_AGGRESSIVE
  92 } RetargetMode;
  93
  94 typedef enum
  95 {
  96         METHOD_BRUTE_FORCE = 0,
  97         METHOD_MEMOIZE = 1
  98 } RetargetMethod;
  99
 100 typedef enum
 101 {
 102         ARC_FREE = 0,
 103         ARC_TAKEN = 1,
 104         ARC_USED = 2
 105 } ArcUsageFlags;
 106
 107 RigGraph *GLOBAL_RIGG = NULL;
 108
 109 /*******************************************************************************************************/
 110
 111 void *exec_retargetArctoArc(void *param);
 112
 113 static void RIG_calculateEdgeAngle(RigEdge *edge_first, RigEdge *edge_second);
 114
 115 /* two levels */
 116 #define SHAPE_LEVELS (SHAPE_RADIX * SHAPE_RADIX)
 117
 118 /*********************************** EDITBONE UTILS ****************************************************/
 119
 120 int countEditBoneChildren(ListBase *list, EditBone *parent)
 121 {
 122         EditBone *ebone;
 123         int count = 0;
 124
 125         for (ebone = list->first; ebone; ebone = ebone->next)
 126         {
 127                 if (ebone->parent == parent)
 128                 {
 129                         count++;
 130                 }
 131         }
 132
 133         return count;
 134 }
 135
 136 EditBone* nextEditBoneChild(ListBase *list, EditBone *parent, int n)
 137 {
 138         EditBone *ebone;
 139
 140         for (ebone = list->first; ebone; ebone = ebone->next)
 141         {
 142                 if (ebone->parent == parent)
 143                 {
 144                         if (n == 0)
 145                         {
 146                                 return ebone;
 147                         }
 148                         n--;
 149                 }
 150         }
 151
 152         return NULL;
 153 }
 154
 155 void getEditBoneRollUpAxis(EditBone *bone, float roll, float up_axis[3])
 156 {
 157         float mat[3][3], nor[3];
 158
 159         VecSubf(nor, bone->tail, bone->head);
 160
 161         vec_roll_to_mat3(nor, roll, mat);
 162         VECCOPY(up_axis, mat[2]);
 163 }
 164
 165 float rollBoneByQuatAligned(EditBone *bone, float old_up_axis[3], float quat[4], float qroll[4], float aligned_axis[3])
 166 {
 167         float nor[3], new_up_axis[3], x_axis[3], z_axis[3];
 168
 169         VECCOPY(new_up_axis, old_up_axis);
 170         QuatMulVecf(quat, new_up_axis);
 171
 172         VecSubf(nor, bone->tail, bone->head);
 173
 174         Crossf(x_axis, nor, aligned_axis);
 175         Crossf(z_axis, x_axis, nor);
 176
 177         Normalize(new_up_axis);
 178         Normalize(x_axis);
 179         Normalize(z_axis);
 180
 181         if (Inpf(new_up_axis, x_axis) < 0)
 182         {
 183                 VecMulf(x_axis, -1);
 184         }
 185
 186         if (Inpf(new_up_axis, z_axis) < 0)
 187         {
 188                 VecMulf(z_axis, -1);
 189         }
 190
 191         if (NormalizedVecAngle2(x_axis, new_up_axis) < NormalizedVecAngle2(z_axis, new_up_axis))
 192         {
 193                 RotationBetweenVectorsToQuat(qroll, new_up_axis, x_axis); /* set roll rotation quat */
 194                 return rollBoneToVector(bone, x_axis);
 195         }
 196         else
 197         {
 198                 RotationBetweenVectorsToQuat(qroll, new_up_axis, z_axis); /* set roll rotation quat */
 199                 return rollBoneToVector(bone, z_axis);
 200         }
 201 }
 202
 203 float rollBoneByQuat(EditBone *bone, float old_up_axis[3], float quat[4])
 204 {
 205         float new_up_axis[3];
 206
 207         VECCOPY(new_up_axis, old_up_axis);
 208         QuatMulVecf(quat, new_up_axis);
 209
 210         return rollBoneToVector(bone, new_up_axis);
 211 }
 212
 213 /************************************ DESTRUCTORS ******************************************************/
 214
 215 void RIG_freeRigArc(BArc *arc)
 216 {
 217         BLI_freelistN(&((RigArc*)arc)->edges);
 218 }
 219
 220 void RIG_freeRigGraph(BGraph *rg)
 221 {
 222         RigGraph *rigg = (RigGraph*)rg;
 223         BNode *node;
 224         BArc *arc;
 225
 226 #ifdef USE_THREADS
 227         BLI_destroy_worker(rigg->worker);
 228 #endif
 229
 230         if (rigg->link_mesh)
 231         {
 232                 REEB_freeGraph(rigg->link_mesh);
 233         }
 234
 235         for (arc = rg->arcs.first; arc; arc = arc->next)
 236         {
 237                 RIG_freeRigArc(arc);
 238         }
 239         BLI_freelistN(&rg->arcs);
 240
 241         for (node = rg->nodes.first; node; node = node->next)
 242         {
 243                 BLI_freeNode(rg, (BNode*)node);
 244         }
 245         BLI_freelistN(&rg->nodes);
 246
 247         BLI_freelistN(&rigg->controls);
 248
 249         BLI_ghash_free(rigg->bones_map, NULL, NULL);
 250         BLI_ghash_free(rigg->controls_map, NULL, NULL);
 251
 252         if (rigg->editbones != &G.edbo)
 253         {
 254                 BLI_freelistN(rigg->editbones);
 255                 MEM_freeN(rigg->editbones);
 256         }
 257
 258         MEM_freeN(rg);
 259 }
 260
 261 /************************************* ALLOCATORS ******************************************************/
 262
 263 static RigGraph *newRigGraph()
 264 {
 265         RigGraph *rg;
 266         int totthread;
 267
 268         rg = MEM_callocN(sizeof(RigGraph), "rig graph");
 269
 270         rg->head = NULL;
 271
 272         rg->bones_map = BLI_ghash_new(BLI_ghashutil_strhash, BLI_ghashutil_strcmp);
 273         rg->controls_map = BLI_ghash_new(BLI_ghashutil_strhash, BLI_ghashutil_strcmp);
 274
 275         rg->free_arc = RIG_freeRigArc;
 276         rg->free_node = NULL;
 277
 278 #ifdef USE_THREADS
 279         if(G.scene->r.mode & R_FIXED_THREADS)
 280         {
 281                 totthread = G.scene->r.threads;
 282         }
 283         else
 284         {
 285                 totthread = BLI_system_thread_count();
 286         }
 287
 288         rg->worker = BLI_create_worker(exec_retargetArctoArc, totthread, 20); /* fix number of threads */
 289 #endif
 290
 291         return rg;
 292 }
 293
 294 static RigArc *newRigArc(RigGraph *rg)
 295 {
 296         RigArc *arc;
 297
 298         arc = MEM_callocN(sizeof(RigArc), "rig arc");
 299         arc->count = 0;
 300         BLI_addtail(&rg->arcs, arc);
 301
 302         return arc;
 303 }
 304
 305 static RigControl *newRigControl(RigGraph *rg)
 306 {
 307         RigControl *ctrl;
 308
 309         ctrl = MEM_callocN(sizeof(RigControl), "rig control");
 310
 311         BLI_addtail(&rg->controls, ctrl);
 312
 313         return ctrl;
 314 }
 315
 316 static RigNode *newRigNodeHead(RigGraph *rg, RigArc *arc, float p[3])
 317 {
 318         RigNode *node;
 319         node = MEM_callocN(sizeof(RigNode), "rig node");
 320         BLI_addtail(&rg->nodes, node);
 321
 322         VECCOPY(node->p, p);
 323         node->degree = 1;
 324         node->arcs = NULL;
 325
 326         arc->head = node;
 327
 328         return node;
 329 }
 330
 331 static void addRigNodeHead(RigGraph *rg, RigArc *arc, RigNode *node)
 332 {
 333         node->degree++;
 334
 335         arc->head = node;
 336 }
 337
 338 static RigNode *newRigNode(RigGraph *rg, float p[3])
 339 {
 340         RigNode *node;
 341         node = MEM_callocN(sizeof(RigNode), "rig node");
 342         BLI_addtail(&rg->nodes, node);
 343
 344         VECCOPY(node->p, p);
 345         node->degree = 0;
 346         node->arcs = NULL;
 347
 348         return node;
 349 }
 350
 351 static RigNode *newRigNodeTail(RigGraph *rg, RigArc *arc, float p[3])
 352 {
 353         RigNode *node = newRigNode(rg, p);
 354
 355         node->degree = 1;
 356         arc->tail = node;
 357
 358         return node;
 359 }
 360
 361 static void RIG_appendEdgeToArc(RigArc *arc, RigEdge *edge)
 362 {
 363         BLI_addtail(&arc->edges, edge);
 364
 365         if (edge->prev == NULL)
 366         {
 367                 VECCOPY(edge->head, arc->head->p);
 368         }
 369         else
 370         {
 371                 RigEdge *last_edge = edge->prev;
 372                 VECCOPY(edge->head, last_edge->tail);
 373                 RIG_calculateEdgeAngle(last_edge, edge);
 374         }
 375
 376         edge->length = VecLenf(edge->head, edge->tail);
 377
 378         arc->length += edge->length;
 379
 380         arc->count += 1;
 381 }
 382
 383 static void RIG_addEdgeToArc(RigArc *arc, float tail[3], EditBone *bone)
 384 {
 385         RigEdge *edge;
 386
 387         edge = MEM_callocN(sizeof(RigEdge), "rig edge");
 388
 389         VECCOPY(edge->tail, tail);
 390         edge->bone = bone;
 391
 392         if (bone)
 393         {
 394                 getEditBoneRollUpAxis(bone, bone->roll, edge->up_axis);
 395         }
 396
 397         RIG_appendEdgeToArc(arc, edge);
 398 }
 399 /************************************** CLONING TEMPLATES **********************************************/
 400
 401 static RigControl *cloneControl(RigGraph *rg, RigControl *src_ctrl, GHash *ptr_hash)
 402 {
 403         RigControl *ctrl;
 404
 405         ctrl = newRigControl(rg);
 406
 407         VECCOPY(ctrl->head, src_ctrl->head);
 408         VECCOPY(ctrl->tail, src_ctrl->tail);
 409         VECCOPY(ctrl->up_axis, src_ctrl->up_axis);
 410         VECCOPY(ctrl->offset, src_ctrl->offset);
 411
 412         ctrl->flag = src_ctrl->flag;
 413
 414         ctrl->bone = duplicateEditBone(src_ctrl->bone, rg->editbones, rg->ob);
 415         ctrl->bone->flag &= ~(BONE_SELECTED|BONE_ROOTSEL|BONE_TIPSEL);
 416         BLI_ghash_insert(ptr_hash, src_ctrl->bone, ctrl->bone);
 417
 418         ctrl->link = src_ctrl->link;
 419
 420         return ctrl;
 421 }
 422
 423 static RigArc *cloneArc(RigGraph *rg, RigArc *src_arc, GHash *ptr_hash)
 424 {
 425         RigEdge *src_edge;
 426         RigArc  *arc;
 427
 428         arc = newRigArc(rg);
 429
 430         arc->head = BLI_ghash_lookup(ptr_hash, src_arc->head);
 431         arc->tail = BLI_ghash_lookup(ptr_hash, src_arc->tail);
 432
 433         arc->head->degree++;
 434         arc->tail->degree++;
 435
 436         arc->length = src_arc->length;
 437
 438         arc->count = src_arc->count;
 439
 440         for (src_edge = src_arc->edges.first; src_edge; src_edge = src_edge->next)
 441         {
 442                 RigEdge *edge;
 443
 444                 edge = MEM_callocN(sizeof(RigEdge), "rig edge");
 445
 446                 VECCOPY(edge->head, src_edge->head);
 447                 VECCOPY(edge->tail, src_edge->tail);
 448                 VECCOPY(edge->up_axis, src_edge->up_axis);
 449
 450                 edge->length = src_edge->length;
 451                 edge->angle = src_edge->angle;
 452
 453                 if (src_edge->bone != NULL)
 454                 {
 455                         edge->bone = duplicateEditBone(src_edge->bone, rg->editbones, rg->ob);
 456                         edge->bone->flag &= ~(BONE_SELECTED|BONE_ROOTSEL|BONE_TIPSEL);
 457                         BLI_ghash_insert(ptr_hash, src_edge->bone, edge->bone);
 458                 }
 459
 460                 BLI_addtail(&arc->edges, edge);
 461         }
 462
 463         return arc;
 464 }
 465
 466 static RigGraph *cloneRigGraph(RigGraph *src)
 467 {
 468         GHash   *ptr_hash;
 469         RigNode *node;
 470         RigArc  *arc;
 471         RigControl *ctrl;
 472         RigGraph *rg;
 473
 474         ptr_hash = BLI_ghash_new(BLI_ghashutil_ptrhash, BLI_ghashutil_ptrcmp);
 475
 476         rg = newRigGraph();
 477
 478         rg->ob = src->ob;
 479         rg->editbones = src->editbones;
 480
 481         preEditBoneDuplicate(rg->editbones); /* prime bones for duplication */
 482
 483         /* Clone nodes */
 484         for (node = src->nodes.first; node; node = node->next)
 485         {
 486                 RigNode *cloned_node = newRigNode(rg, node->p);
 487                 BLI_ghash_insert(ptr_hash, node, cloned_node);
 488         }
 489
 490         rg->head = BLI_ghash_lookup(ptr_hash, src->head);
 491
 492         /* Clone arcs */
 493         for (arc = src->arcs.first; arc; arc = arc->next)
 494         {
 495                 cloneArc(rg, arc, ptr_hash);
 496         }
 497
 498         /* Clone controls */
 499         for (ctrl = src->controls.first; ctrl; ctrl = ctrl->next)
 500         {
 501                 cloneControl(rg, ctrl, ptr_hash);
 502         }
 503
 504         /* Relink bones properly */
 505         for (arc = rg->arcs.first; arc; arc = arc->next)
 506         {
 507                 RigEdge *edge;
 508
 509                 for (edge = arc->edges.first; edge; edge = edge->next)
 510                 {
 511                         if (edge->bone != NULL)
 512                         {
 513                                 EditBone *bone;
 514
 515                                 updateDuplicateSubtarget(edge->bone, rg->ob);
 516
 517                                 bone = BLI_ghash_lookup(ptr_hash, edge->bone->parent);
 518
 519                                 if (bone != NULL)
 520                                 {
 521                                         edge->bone->parent = bone;
 522                                 }
 523                         }
 524                 }
 525         }
 526
 527         for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
 528         {
 529                 EditBone *bone;
 530
 531                 updateDuplicateSubtarget(ctrl->bone, rg->ob);
 532
 533                 bone = BLI_ghash_lookup(ptr_hash, ctrl->bone->parent);
 534
 535                 if (bone != NULL)
 536                 {
 537                         ctrl->bone->parent = bone;
 538                 }
 539
 540                 ctrl->link = BLI_ghash_lookup(ptr_hash, ctrl->link);
 541         }
 542
 543         BLI_ghash_free(ptr_hash, NULL, NULL);
 544
 545         return rg;
 546 }
 547
 548
 549 /*******************************************************************************************************/
 550
 551 static void RIG_calculateEdgeAngle(RigEdge *edge_first, RigEdge *edge_second)
 552 {
 553         float vec_first[3], vec_second[3];
 554
 555         VecSubf(vec_first, edge_first->tail, edge_first->head);
 556         VecSubf(vec_second, edge_second->tail, edge_second->head);
 557
 558         Normalize(vec_first);
 559         Normalize(vec_second);
 560
 561         edge_first->angle = saacos(Inpf(vec_first, vec_second));
 562 }
 563
 564 /************************************ CONTROL BONES ****************************************************/
 565
 566 static void RIG_addControlBone(RigGraph *rg, EditBone *bone)
 567 {
 568         RigControl *ctrl = newRigControl(rg);
 569         ctrl->bone = bone;
 570         VECCOPY(ctrl->head, bone->head);
 571         VECCOPY(ctrl->tail, bone->tail);
 572         getEditBoneRollUpAxis(bone, bone->roll, ctrl->up_axis);
 573
 574         BLI_ghash_insert(rg->controls_map, bone->name, ctrl);
 575 }
 576
 577 static int RIG_parentControl(RigControl *ctrl, EditBone *link)
 578 {
 579         if (link)
 580         {
 581                 float offset[3];
 582                 int flag = 0;
 583
 584                 VecSubf(offset, ctrl->bone->head, link->head);
 585
 586                 /* if root matches, check for direction too */
 587                 if (Inpf(offset, offset) < 0.0001)
 588                 {
 589                         float vbone[3], vparent[3];
 590
 591                         flag |= RIG_CTRL_FIT_ROOT;
 592
 593                         VecSubf(vbone, ctrl->bone->tail, ctrl->bone->head);
 594                         VecSubf(vparent, link->tail, link->head);
 595
 596                         /* test for opposite direction */
 597                         if (Inpf(vbone, vparent) > 0)
 598                         {
 599                                 float nor[3];
 600                                 float len;
 601
 602                                 Crossf(nor, vbone, vparent);
 603
 604                                 len = Inpf(nor, nor);
 605                                 if (len < 0.0001)
 606                                 {
 607                                         flag |= RIG_CTRL_FIT_BONE;
 608                                 }
 609                         }
 610                 }
 611
 612                 /* Bail out if old one is automatically better */
 613                 if (flag < ctrl->flag)
 614                 {
 615                         return 0;
 616                 }
 617
 618                 /* if there's already a link
 619                  *      overwrite only if new link is higher in the chain */
 620                 if (ctrl->link && flag == ctrl->flag)
 621                 {
 622                         EditBone *bone = NULL;
 623
 624                         for (bone = ctrl->link; bone; bone = bone->parent)
 625                         {
 626                                 /* if link is in the chain, break and use that one */
 627                                 if (bone == link)
 628                                 {
 629                                         break;
 630                                 }
 631                         }
 632
 633                         /* not in chain, don't update link */
 634                         if (bone == NULL)
 635                         {
 636                                 return 0;
 637                         }
 638                 }
 639
 640
 641                 ctrl->link = link;
 642                 ctrl->flag = flag;
 643
 644                 VECCOPY(ctrl->offset, offset);
 645
 646                 return 1;
 647         }
 648
 649         return 0;
 650 }
 651
 652 static void RIG_reconnectControlBones(RigGraph *rg)
 653 {
 654         RigControl *ctrl;
 655         int change = 1;
 656
 657         /* first pass, link to deform bones */
 658         for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
 659         {
 660                 bPoseChannel *pchan;
 661                 bConstraint *con;
 662                 int found = 0;
 663
 664                 /* DO SOME MAGIC HERE */
 665                 for (pchan= rg->ob->pose->chanbase.first; pchan; pchan= pchan->next)
 666                 {
 667                         for (con= pchan->constraints.first; con; con= con->next)
 668                         {
 669                                 bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
 670                                 ListBase targets = {NULL, NULL};
 671                                 bConstraintTarget *ct;
 672
 673                                 /* constraint targets */
 674                                 if (cti && cti->get_constraint_targets)
 675                                 {
 676                                         cti->get_constraint_targets(con, &targets);
 677
 678                                         for (ct= targets.first; ct; ct= ct->next)
 679                                         {
 680                                                 if ((ct->tar == rg->ob) && strcmp(ct->subtarget, ctrl->bone->name) == 0)
 681                                                 {
 682                                                         /* SET bone link to bone corresponding to pchan */
 683                                                         EditBone *link = BLI_ghash_lookup(rg->bones_map, pchan->name);
 684
 685                                                         found = RIG_parentControl(ctrl, link);
 686                                                 }
 687                                         }
 688
 689                                         if (cti->flush_constraint_targets)
 690                                                 cti->flush_constraint_targets(con, &targets, 0);
 691                                 }
 692                         }
 693                 }
 694
 695                 /* if not found yet, check parent */
 696                 if (found == 0)
 697                 {
 698                         if (ctrl->bone->parent)
 699                         {
 700                                 /* make sure parent is a deforming bone
 701                                  * NULL if not
 702                                  *  */
 703                                 EditBone *link = BLI_ghash_lookup(rg->bones_map, ctrl->bone->parent->name);
 704
 705                                 found = RIG_parentControl(ctrl, link);
 706                         }
 707
 708                         /* check if bone is not superposed on another one */
 709                         {
 710                                 RigArc *arc;
 711                                 RigArc *best_arc = NULL;
 712                                 EditBone *link = NULL;
 713
 714                                 for (arc = rg->arcs.first; arc; arc = arc->next)
 715                                 {
 716                                         RigEdge *edge;
 717                                         for (edge = arc->edges.first; edge; edge = edge->next)
 718                                         {
 719                                                 if (edge->bone)
 720                                                 {
 721                                                         int fit = 0;
 722
 723                                                         fit = VecLenf(ctrl->bone->head, edge->bone->head) < 0.0001;
 724                                                         fit = fit || VecLenf(ctrl->bone->tail, edge->bone->tail) < 0.0001;
 725
 726                                                         if (fit)
 727                                                         {
 728                                                                 /* pick the bone on the arc with the lowest symmetry level
 729                                                                  * means you connect control to the trunk of the skeleton */
 730                                                                 if (best_arc == NULL || arc->symmetry_level < best_arc->symmetry_level)
 731                                                                 {
 732                                                                         best_arc = arc;
 733                                                                         link = edge->bone;
 734                                                                 }
 735                                                         }
 736                                                 }
 737                                         }
 738                                 }
 739
 740                                 found = RIG_parentControl(ctrl, link);
 741                         }
 742                 }
 743
 744                 /* if not found yet, check child */
 745                 if (found == 0)
 746                 {
 747                         RigArc *arc;
 748                         RigArc *best_arc = NULL;
 749                         EditBone *link = NULL;
 750
 751                         for (arc = rg->arcs.first; arc; arc = arc->next)
 752                         {
 753                                 RigEdge *edge;
 754                                 for (edge = arc->edges.first; edge; edge = edge->next)
 755                                 {
 756                                         if (edge->bone && edge->bone->parent == ctrl->bone)
 757                                         {
 758                                                 /* pick the bone on the arc with the lowest symmetry level
 759                                                  * means you connect control to the trunk of the skeleton */
 760                                                 if (best_arc == NULL || arc->symmetry_level < best_arc->symmetry_level)
 761                                                 {
 762                                                         best_arc = arc;
 763                                                         link = edge->bone;
 764                                                 }
 765                                         }
 766                                 }
 767                         }
 768
 769                         found = RIG_parentControl(ctrl, link);
 770                 }
 771
 772         }
 773
 774
 775         /* second pass, make chains in control bones */
 776         while (change)
 777         {
 778                 change = 0;
 779
 780                 for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
 781                 {
 782                         /* if control is not linked yet */
 783                         if (ctrl->link == NULL)
 784                         {
 785                                 bPoseChannel *pchan;
 786                                 bConstraint *con;
 787                                 RigControl *ctrl_parent = NULL;
 788                                 RigControl *ctrl_child;
 789                                 int found = 0;
 790
 791                                 if (ctrl->bone->parent)
 792                                 {
 793                                         ctrl_parent = BLI_ghash_lookup(rg->controls_map, ctrl->bone->parent->name);
 794                                 }
 795
 796                                 /* check constraints first */
 797
 798                                 /* DO SOME MAGIC HERE */
 799                                 for (pchan= rg->ob->pose->chanbase.first; pchan; pchan= pchan->next)
 800                                 {
 801                                         for (con= pchan->constraints.first; con; con= con->next)
 802                                         {
 803                                                 bConstraintTypeInfo *cti= constraint_get_typeinfo(con);
 804                                                 ListBase targets = {NULL, NULL};
 805                                                 bConstraintTarget *ct;
 806
 807                                                 /* constraint targets */
 808                                                 if (cti && cti->get_constraint_targets)
 809                                                 {
 810                                                         cti->get_constraint_targets(con, &targets);
 811
 812                                                         for (ct= targets.first; ct; ct= ct->next)
 813                                                         {
 814                                                                 if ((ct->tar == rg->ob) && strcmp(ct->subtarget, ctrl->bone->name) == 0)
 815                                                                 {
 816                                                                         /* SET bone link to ctrl corresponding to pchan */
 817                                                                         RigControl *link = BLI_ghash_lookup(rg->controls_map, pchan->name);
 818
 819                                                                         /* if owner is a control bone, link with it */
 820                                                                         if (link && link->link)
 821                                                                         {
 822                                                                                 printf("%s -constraint- %s\n", ctrl->bone->name, link->bone->name);
 823                                                                                 RIG_parentControl(ctrl, link->bone);
 824                                                                                 found = 1;
 825                                                                                 break;
 826                                                                         }
 827                                                                 }
 828                                                         }
 829
 830                                                         if (cti->flush_constraint_targets)
 831                                                                 cti->flush_constraint_targets(con, &targets, 0);
 832                                                 }
 833                                         }
 834                                 }
 835
 836                                 if (found == 0)
 837                                 {
 838                                         /* check if parent is already linked */
 839                                         if (ctrl_parent && ctrl_parent->link)
 840                                         {
 841                                                 printf("%s -parent- %s\n", ctrl->bone->name, ctrl_parent->bone->name);
 842                                                 RIG_parentControl(ctrl, ctrl_parent->bone);
 843                                                 change = 1;
 844                                         }
 845                                         else
 846                                         {
 847                                                 /* check childs */
 848                                                 for (ctrl_child = rg->controls.first; ctrl_child; ctrl_child = ctrl_child->next)
 849                                                 {
 850                                                         /* if a child is linked, link to that one */
 851                                                         if (ctrl_child->link && ctrl_child->bone->parent == ctrl->bone)
 852                                                         {
 853                                                                 printf("%s -child- %s\n", ctrl->bone->name, ctrl_child->bone->name);
 854                                                                 RIG_parentControl(ctrl, ctrl_child->bone);
 855                                                                 change = 1;
 856                                                                 break;
 857                                                         }
 858                                                 }
 859                                         }
 860                                 }
 861                         }
 862                 }
 863
 864         }
 865 }
 866
 867 /*******************************************************************************************************/
 868
 869 static void RIG_joinArcs(RigGraph *rg, RigNode *node, RigArc *joined_arc1, RigArc *joined_arc2)
 870 {
 871         RigEdge *edge, *next_edge;
 872
 873         /* ignore cases where joint is at start or end */
 874         if (joined_arc1->head == joined_arc2->head || joined_arc1->tail == joined_arc2->tail)
 875         {
 876                 return;
 877         }
 878
 879         /* swap arcs to make sure arc1 is before arc2 */
 880         if (joined_arc1->head == joined_arc2->tail)
 881         {
 882                 RigArc *tmp = joined_arc1;
 883                 joined_arc1 = joined_arc2;
 884                 joined_arc2 = tmp;
 885         }
 886
 887         for (edge = joined_arc2->edges.first; edge; edge = next_edge)
 888         {
 889                 next_edge = edge->next;
 890
 891                 RIG_appendEdgeToArc(joined_arc1, edge);
 892         }
 893
 894         joined_arc1->tail = joined_arc2->tail;
 895
 896         joined_arc2->edges.first = joined_arc2->edges.last = NULL;
 897
 898         BLI_removeArc((BGraph*)rg, (BArc*)joined_arc2);
 899
 900         BLI_removeNode((BGraph*)rg, (BNode*)node);
 901 }
 902
 903 static void RIG_removeNormalNodes(RigGraph *rg)
 904 {
 905         RigNode *node, *next_node;
 906
 907         for (node = rg->nodes.first; node; node = next_node)
 908         {
 909                 next_node = node->next;
 910
 911                 if (node->degree == 2)
 912                 {
 913                         RigArc *arc, *joined_arc1 = NULL, *joined_arc2 = NULL;
 914
 915                         for (arc = rg->arcs.first; arc; arc = arc->next)
 916                         {
 917                                 if (arc->head == node || arc->tail == node)
 918                                 {
 919                                         if (joined_arc1 == NULL)
 920                                         {
 921                                                 joined_arc1 = arc;
 922                                         }
 923                                         else
 924                                         {
 925                                                 joined_arc2 = arc;
 926                                                 break;
 927                                         }
 928                                 }
 929                         }
 930
 931                         RIG_joinArcs(rg, node, joined_arc1, joined_arc2);
 932                 }
 933         }
 934 }
 935
 936 static void RIG_removeUneededOffsets(RigGraph *rg)
 937 {
 938         RigArc *arc;
 939
 940         for (arc = rg->arcs.first; arc; arc = arc->next)
 941         {
 942                 RigEdge *first_edge, *last_edge;
 943
 944                 first_edge = arc->edges.first;
 945                 last_edge = arc->edges.last;
 946
 947                 if (first_edge->bone == NULL)
 948                 {
 949                         if (first_edge->bone == NULL && VecLenf(first_edge->tail, arc->head->p) <= 0.001)
 950                         {
 951                                 BLI_remlink(&arc->edges, first_edge);
 952                                 MEM_freeN(first_edge);
 953                         }
 954                         else if (arc->head->degree == 1)
 955                         {
 956                                 RigNode *new_node = (RigNode*)BLI_FindNodeByPosition((BGraph*)rg, first_edge->tail, 0.001);
 957
 958                                 if (new_node)
 959                                 {
 960                                         BLI_remlink(&arc->edges, first_edge);
 961                                         MEM_freeN(first_edge);
 962                                         BLI_replaceNodeInArc((BGraph*)rg, (BArc*)arc, (BNode*)new_node, (BNode*)arc->head);
 963                                 }
 964                                 else
 965                                 {
 966                                         RigEdge *next_edge = first_edge->next;
 967
 968                                         if (next_edge)
 969                                         {
 970                                                 BLI_remlink(&arc->edges, first_edge);
 971                                                 MEM_freeN(first_edge);
 972
 973                                                 VECCOPY(arc->head->p, next_edge->head);
 974                                         }
 975                                 }
 976                         }
 977                         else
 978                         {
 979                                 /* check if all arc connected start with a null edge */
 980                                 RigArc *other_arc;
 981                                 for (other_arc = rg->arcs.first; other_arc; other_arc = other_arc->next)
 982                                 {
 983                                         if (other_arc != arc)
 984                                         {
 985                                                 RigEdge *test_edge;
 986                                                 if (other_arc->head == arc->head)
 987                                                 {
 988                                                         test_edge = other_arc->edges.first;
 989
 990                                                         if (test_edge->bone != NULL)
 991                                                         {
 992                                                                 break;
 993                                                         }
 994                                                 }
 995                                                 else if (other_arc->tail == arc->head)
 996                                                 {
 997                                                         test_edge = other_arc->edges.last;
 998
 999                                                         if (test_edge->bone != NULL)
1000                                                         {
1001                                                                 break;
1002                                                         }
1003                                                 }
1004                                         }
1005                                 }
1006
1007                                 if (other_arc == NULL)
1008                                 {
1009                                         RigNode *new_node = (RigNode*)BLI_FindNodeByPosition((BGraph*)rg, first_edge->tail, 0.001);
1010
1011                                         if (new_node)
1012                                         {
1013                                                 /* remove null edge in other arcs too */
1014                                                 for (other_arc = rg->arcs.first; other_arc; other_arc = other_arc->next)
1015                                                 {
1016                                                         if (other_arc != arc)
1017                                                         {
1018                                                                 RigEdge *test_edge;
1019                                                                 if (other_arc->head == arc->head)
1020                                                                 {
1021                                                                         BLI_replaceNodeInArc((BGraph*)rg, (BArc*)other_arc, (BNode*)new_node, (BNode*)other_arc->head);
1022                                                                         test_edge = other_arc->edges.first;
1023                                                                         BLI_remlink(&other_arc->edges, test_edge);
1024                                                                         MEM_freeN(test_edge);
1025                                                                 }
1026                                                                 else if (other_arc->tail == arc->head)
1027                                                                 {
1028                                                                         BLI_replaceNodeInArc((BGraph*)rg, (BArc*)other_arc, (BNode*)new_node, (BNode*)other_arc->tail);
1029                                                                         test_edge = other_arc->edges.last;
1030                                                                         BLI_remlink(&other_arc->edges, test_edge);
1031                                                                         MEM_freeN(test_edge);
1032                                                                 }
1033                                                         }
1034                                                 }
1035
1036                                                 BLI_remlink(&arc->edges, first_edge);
1037                                                 MEM_freeN(first_edge);
1038                                                 BLI_replaceNodeInArc((BGraph*)rg, (BArc*)arc, (BNode*)new_node, (BNode*)arc->head);
1039                                         }
1040                                         else
1041                                         {
1042                                                 RigEdge *next_edge = first_edge->next;
1043
1044                                                 if (next_edge)
1045                                                 {
1046                                                         BLI_remlink(&arc->edges, first_edge);
1047                                                         MEM_freeN(first_edge);
1048
1049                                                         VECCOPY(arc->head->p, next_edge->head);
1050
1051                                                         /* remove null edge in other arcs too */
1052                                                         for (other_arc = rg->arcs.first; other_arc; other_arc = other_arc->next)
1053                                                         {
1054                                                                 if (other_arc != arc)
1055                                                                 {
1056                                                                         RigEdge *test_edge;
1057                                                                         if (other_arc->head == arc->head)
1058                                                                         {
1059                                                                                 test_edge = other_arc->edges.first;
1060                                                                                 BLI_remlink(&other_arc->edges, test_edge);
1061                                                                                 MEM_freeN(test_edge);
1062                                                                         }
1063                                                                         else if (other_arc->tail == arc->head)
1064                                                                         {
1065                                                                                 test_edge = other_arc->edges.last;
1066                                                                                 BLI_remlink(&other_arc->edges, test_edge);
1067                                                                                 MEM_freeN(test_edge);
1068                                                                         }
1069                                                                 }
1070                                                         }
1071                                                 }
1072                                         }
1073                                 }
1074                         }
1075                 }
1076
1077                 if (last_edge->bone == NULL)
1078                 {
1079                         if (VecLenf(last_edge->head, arc->tail->p) <= 0.001)
1080                         {
1081                                 BLI_remlink(&arc->edges, last_edge);
1082                                 MEM_freeN(last_edge);
1083                         }
1084                         else if (arc->tail->degree == 1)
1085                         {
1086                                 RigNode *new_node = (RigNode*)BLI_FindNodeByPosition((BGraph*)rg, last_edge->head, 0.001);
1087
1088                                 if (new_node)
1089                                 {
1090                                         RigEdge *previous_edge = last_edge->prev;
1091
1092                                         BLI_remlink(&arc->edges, last_edge);
1093                                         MEM_freeN(last_edge);
1094                                         BLI_replaceNodeInArc((BGraph*)rg, (BArc*)arc, (BNode*)new_node, (BNode*)arc->tail);
1095
1096                                         /* set previous angle to 0, since there's no following edges */
1097                                         if (previous_edge)
1098                                         {
1099                                                 previous_edge->angle = 0;
1100                                         }
1101                                 }
1102                                 else
1103                                 {
1104                                         RigEdge *previous_edge = last_edge->prev;
1105
1106                                         if (previous_edge)
1107                                         {
1108                                                 BLI_remlink(&arc->edges, last_edge);
1109                                                 MEM_freeN(last_edge);
1110
1111                                                 VECCOPY(arc->tail->p, previous_edge->tail);
1112                                                 previous_edge->angle = 0;
1113                                         }
1114                                 }
1115                         }
1116                 }
1117         }
1118 }
1119
1120 static void RIG_arcFromBoneChain(RigGraph *rg, ListBase *list, EditBone *root_bone, RigNode *starting_node, int selected)
1121 {
1122         EditBone *bone, *last_bone = root_bone;
1123         RigArc *arc = NULL;
1124         int contain_head = 0;
1125
1126         for(bone = root_bone; bone; bone = nextEditBoneChild(list, bone, 0))
1127         {
1128                 int nb_children;
1129
1130                 if (selected == 0 || (bone->flag & BONE_SELECTED))
1131                 {
1132                         if ((bone->flag & BONE_NO_DEFORM) == 0)
1133                         {
1134                                 BLI_ghash_insert(rg->bones_map, bone->name, bone);
1135
1136                                 if (arc == NULL)
1137                                 {
1138                                         arc = newRigArc(rg);
1139
1140                                         if (starting_node == NULL)
1141                                         {
1142                                                 starting_node = newRigNodeHead(rg, arc, root_bone->head);
1143                                         }
1144                                         else
1145                                         {
1146                                                 addRigNodeHead(rg, arc, starting_node);
1147                                         }
1148                                 }
1149
1150                                 if (bone->parent && (bone->flag & BONE_CONNECTED) == 0)
1151                                 {
1152                                         RIG_addEdgeToArc(arc, bone->head, NULL);
1153                                 }
1154
1155                                 RIG_addEdgeToArc(arc, bone->tail, bone);
1156
1157                                 last_bone = bone;
1158
1159                                 if (strcmp(bone->name, "head") == 0)
1160                                 {
1161                                         contain_head = 1;
1162                                 }
1163                         }
1164                         else if ((bone->flag & BONE_EDITMODE_LOCKED) == 0) /* ignore locked bones */
1165                         {
1166                                 RIG_addControlBone(rg, bone);
1167                         }
1168                 }
1169
1170                 nb_children = countEditBoneChildren(list, bone);
1171                 if (nb_children > 1)
1172                 {
1173                         RigNode *end_node = NULL;
1174                         int i;
1175
1176                         if (arc != NULL)
1177                         {
1178                                 end_node = newRigNodeTail(rg, arc, bone->tail);
1179                         }
1180                         else
1181                         {
1182                                 end_node = newRigNode(rg, bone->tail);
1183                         }
1184
1185                         for (i = 0; i < nb_children; i++)
1186                         {
1187                                 root_bone = nextEditBoneChild(list, bone, i);
1188                                 RIG_arcFromBoneChain(rg, list, root_bone, end_node, selected);
1189                         }
1190
1191                         /* arc ends here, break */
1192                         break;
1193                 }
1194         }
1195
1196         /* If the loop exited without forking */
1197         if (arc != NULL && bone == NULL)
1198         {
1199                 newRigNodeTail(rg, arc, last_bone->tail);
1200         }
1201
1202         if (contain_head)
1203         {
1204                 rg->head = arc->tail;
1205         }
1206 }
1207
1208 /*******************************************************************************************************/
1209 static void RIG_findHead(RigGraph *rg)
1210 {
1211         if (rg->head == NULL)
1212         {
1213                 if (BLI_countlist(&rg->arcs) == 1)
1214                 {
1215                         RigArc *arc = rg->arcs.first;
1216
1217                         rg->head = (RigNode*)arc->head;
1218                 }
1219                 else
1220                 {
1221                         RigArc *arc;
1222
1223                         for (arc = rg->arcs.first; arc; arc = arc->next)
1224                         {
1225                                 RigEdge *edge = arc->edges.last;
1226
1227                                 if (edge->bone->flag & (BONE_TIPSEL|BONE_SELECTED))
1228                                 {
1229                                         rg->head = arc->tail;
1230                                         break;
1231                                 }
1232                         }
1233                 }
1234
1235                 if (rg->head == NULL)
1236                 {
1237                         rg->head = rg->nodes.first;
1238                 }
1239         }
1240 }
1241
1242 /*******************************************************************************************************/
1243
1244 void RIG_printNode(RigNode *node, char name[])
1245 {
1246         printf("%s %p %i <%0.3f, %0.3f, %0.3f>\n", name, node, node->degree, node->p[0], node->p[1], node->p[2]);
1247
1248         if (node->symmetry_flag & SYM_TOPOLOGICAL)
1249         {
1250                 if (node->symmetry_flag & SYM_AXIAL)
1251                         printf("Symmetry AXIAL\n");
1252                 else if (node->symmetry_flag & SYM_RADIAL)
1253                         printf("Symmetry RADIAL\n");
1254
1255                 printvecf("symmetry axis", node->symmetry_axis);
1256         }
1257 }
1258
1259 void RIG_printArcBones(RigArc *arc)
1260 {
1261         RigEdge *edge;
1262
1263         for (edge = arc->edges.first; edge; edge = edge->next)
1264         {
1265                 if (edge->bone)
1266                         printf("%s ", edge->bone->name);
1267                 else
1268                         printf("---- ");
1269         }
1270         printf("\n");
1271 }
1272
1273 void RIG_printCtrl(RigControl *ctrl, char *indent)
1274 {
1275         char text[128];
1276
1277         printf("%sBone: %s\n", indent, ctrl->bone->name);
1278         printf("%sLink: %s\n", indent, ctrl->link ? ctrl->link->name : "!NONE!");
1279
1280         sprintf(text, "%soffset", indent);
1281         printvecf(text, ctrl->offset);
1282
1283         printf("%sFlag: %i\n", indent, ctrl->flag);
1284 }
1285
1286 void RIG_printLinkedCtrl(RigGraph *rg, EditBone *bone, int tabs)
1287 {
1288         RigControl *ctrl;
1289         char indent[64];
1290         char *s = indent;
1291         int i;
1292
1293         for (i = 0; i < tabs; i++)
1294         {
1295                 s[0] = '\t';
1296                 s++;
1297         }
1298         s[0] = 0;
1299
1300         for (ctrl = rg->controls.first; ctrl; ctrl = ctrl->next)
1301         {
1302                 if (ctrl->link == bone)
1303                 {
1304                         RIG_printCtrl(ctrl, indent);
1305                         RIG_printLinkedCtrl(rg, ctrl->bone, tabs + 1);
1306                 }
1307         }
1308 }
1309
1310 void RIG_printArc(RigGraph *rg, RigArc *arc)
1311 {
1312         RigEdge *edge;
1313
1314         RIG_printNode((RigNode*)arc->head, "head");
1315
1316         for (edge = arc->edges.first; edge; edge = edge->next)
1317         {
1318                 printf("\tinner joints %0.3f %0.3f %0.3f\n", edge->tail[0], edge->tail[1], edge->tail[2]);
1319                 printf("\t\tlength %f\n", edge->length);
1320                 printf("\t\tangle %f\n", edge->angle * 180 / M_PI);
1321                 if (edge->bone)
1322                 {
1323                         printf("\t\t%s\n", edge->bone->name);
1324                         RIG_printLinkedCtrl(rg, edge->bone, 3);
1325                 }
1326         }
1327         printf("symmetry level: %i flag: %i group %i\n", arc->symmetry_level, arc->symmetry_flag, arc->symmetry_group);
1328
1329         RIG_printNode((RigNode*)arc->tail, "tail");
1330 }
1331
1332 void RIG_printGraph(RigGraph *rg)
1333 {
1334         RigArc *arc;
1335
1336         printf("---- ARCS ----\n");
1337         for (arc = rg->arcs.first; arc; arc = arc->next)
1338         {
1339                 RIG_printArc(rg, arc);
1340                 printf("\n");
1341         }
1342
1343         if (rg->head)
1344         {
1345                 RIG_printNode(rg->head, "HEAD NODE:");
1346         }
1347         else
1348         {
1349                 printf("HEAD NODE: NONE\n");
1350         }
1351 }
1352
1353 /*******************************************************************************************************/
1354
1355 RigGraph *armatureToGraph(Object *ob, bArmature *arm)
1356 {
1357         EditBone *ebone;
1358         RigGraph *rg;
1359
1360         rg = newRigGraph();
1361
1362         if (G.obedit == ob)
1363         {
1364                 rg->editbones = &G.edbo;
1365         }
1366         else
1367         {
1368                 rg->editbones = MEM_callocN(sizeof(ListBase), "EditBones");
1369                 make_boneList(rg->editbones, &arm->bonebase, NULL);
1370         }
1371
1372         rg->ob = ob;
1373
1374         /* Do the rotations */
1375         for (ebone = rg->editbones->first; ebone; ebone=ebone->next){
1376                 if (ebone->parent == NULL)
1377                 {
1378                         RIG_arcFromBoneChain(rg, rg->editbones, ebone, NULL, 0);
1379                 }
1380         }
1381
1382         BLI_removeDoubleNodes((BGraph*)rg, 0.001);
1383
1384         RIG_removeNormalNodes(rg);
1385
1386         RIG_removeUneededOffsets(rg);
1387
1388         BLI_buildAdjacencyList((BGraph*)rg);
1389
1390         RIG_findHead(rg);
1391
1392         BLI_markdownSymmetry((BGraph*)rg, (BNode*)rg->head, G.scene->toolsettings->skgen_symmetry_limit);
1393
1394         RIG_reconnectControlBones(rg); /* after symmetry, because we use levels to find best match */
1395
1396         if (BLI_isGraphCyclic((BGraph*)rg))
1397         {
1398                 printf("armature cyclic\n");
1399         }
1400
1401         return rg;
1402 }
1403
1404 RigGraph *armatureSelectedToGraph(Object *ob, bArmature *arm)
1405 {
1406         EditBone *ebone;
1407         RigGraph *rg;
1408
1409         rg = newRigGraph();
1410
1411         if (G.obedit == ob)
1412         {
1413                 rg->editbones = &G.edbo;
1414         }
1415         else
1416         {
1417                 rg->editbones = MEM_callocN(sizeof(ListBase), "EditBones");
1418                 make_boneList(rg->editbones, &arm->bonebase, NULL);
1419         }
1420
1421         rg->ob = ob;
1422
1423         /* Do the rotations */
1424         for (ebone = rg->editbones->first; ebone; ebone=ebone->next){
1425                 if (ebone->parent == NULL)
1426                 {
1427                         RIG_arcFromBoneChain(rg, rg->editbones, ebone, NULL, 1);
1428                 }
1429         }
1430
1431         BLI_removeDoubleNodes((BGraph*)rg, 0.001);
1432
1433         RIG_removeNormalNodes(rg);
1434
1435         RIG_removeUneededOffsets(rg);
1436
1437         BLI_buildAdjacencyList((BGraph*)rg);
1438
1439         RIG_findHead(rg);
1440
1441         BLI_markdownSymmetry((BGraph*)rg, (BNode*)rg->head, G.scene->toolsettings->skgen_symmetry_limit);
1442
1443         RIG_reconnectControlBones(rg); /* after symmetry, because we use levels to find best match */
1444
1445         if (BLI_isGraphCyclic((BGraph*)rg))
1446         {
1447                 printf("armature cyclic\n");
1448         }
1449
1450         return rg;
1451 }
1452 /************************************ GENERATING *****************************************************/
1453
1454 static EditBone *add_editbonetolist(char *name, ListBase *list)
1455 {
1456         EditBone *bone= MEM_callocN(sizeof(EditBone), "eBone");
1457
1458         BLI_strncpy(bone->name, name, 32);
1459         unique_editbone_name(list, bone->name);
1460
1461         BLI_addtail(list, bone);
1462
1463         bone->flag |= BONE_TIPSEL;
1464         bone->weight= 1.0F;
1465         bone->dist= 0.25F;
1466         bone->xwidth= 0.1;
1467         bone->zwidth= 0.1;
1468         bone->ease1= 1.0;
1469         bone->ease2= 1.0;
1470         bone->rad_head= 0.10;
1471         bone->rad_tail= 0.05;
1472         bone->segments= 1;
1473         bone->layer=  1;//arm->layer;
1474
1475         return bone;
1476 }
1477
1478 EditBone * generateBonesForArc(RigGraph *rigg, ReebArc *arc, ReebNode *head, ReebNode *tail)
1479 {
1480         ReebArcIterator iter;
1481         float n[3];
1482         float ADAPTIVE_THRESHOLD = G.scene->toolsettings->skgen_correlation_limit;
1483         EditBone *lastBone = NULL;
1484
1485         /* init iterator to get start and end from head */
1486         initArcIterator(&iter, arc, head);
1487
1488         /* Calculate overall */
1489         VecSubf(n, arc->buckets[iter.end].p, head->p);
1490
1491         if (1 /* G.scene->toolsettings->skgen_options & SKGEN_CUT_CORRELATION */ )
1492         {
1493                 EmbedBucket *bucket = NULL;
1494                 EmbedBucket *previous = NULL;
1495                 EditBone *child = NULL;
1496                 EditBone *parent = NULL;
1497                 float normal[3] = {0, 0, 0};
1498                 float avg_normal[3];
1499                 int total = 0;
1500                 int boneStart = iter.start;
1501
1502                 parent = add_editbonetolist("Bone", rigg->editbones);
1503                 parent->flag = BONE_SELECTED|BONE_TIPSEL|BONE_ROOTSEL;
1504                 VECCOPY(parent->head, head->p);
1505
1506                 for (previous = nextBucket(&iter), bucket = nextBucket(&iter);
1507                         bucket;
1508                         previous = bucket, bucket = nextBucket(&iter))
1509                 {
1510                         float btail[3];
1511                         float value = 0;
1512
1513                         if (G.scene->toolsettings->skgen_options & SKGEN_STICK_TO_EMBEDDING)
1514                         {
1515                                 VECCOPY(btail, bucket->p);
1516                         }
1517                         else
1518                         {
1519                                 float length;
1520
1521                                 /* Calculate normal */
1522                                 VecSubf(n, bucket->p, parent->head);
1523                                 length = Normalize(n);
1524
1525                                 total += 1;
1526                                 VecAddf(normal, normal, n);
1527                                 VECCOPY(avg_normal, normal);
1528                                 VecMulf(avg_normal, 1.0f / total);
1529
1530                                 VECCOPY(btail, avg_normal);
1531                                 VecMulf(btail, length);
1532                                 VecAddf(btail, btail, parent->head);
1533                         }
1534
1535                         if (G.scene->toolsettings->skgen_options & SKGEN_ADAPTIVE_DISTANCE)
1536                         {
1537                                 value = calcDistance(arc, boneStart, iter.index, parent->head, btail);
1538                         }
1539                         else
1540                         {
1541                                 float n[3];
1542
1543                                 VecSubf(n, btail, parent->head);
1544                                 value = calcVariance(arc, boneStart, iter.index, parent->head, n);
1545                         }
1546
1547                         if (value > ADAPTIVE_THRESHOLD)
1548                         {
1549                                 VECCOPY(parent->tail, btail);
1550
1551                                 child = add_editbonetolist("Bone", rigg->editbones);
1552                                 VECCOPY(child->head, parent->tail);
1553                                 child->parent = parent;
1554                                 child->flag |= BONE_CONNECTED|BONE_SELECTED|BONE_TIPSEL|BONE_ROOTSEL;
1555
1556                                 parent = child; // new child is next parent
1557                                 boneStart = iter.index; // start from end
1558
1559                                 normal[0] = normal[1] = normal[2] = 0;
1560                                 total = 0;
1561                         }
1562                 }
1563
1564                 VECCOPY(parent->tail, tail->p);
1565
1566                 lastBone = parent; /* set last bone in the chain */
1567         }
1568
1569         return lastBone;
1570 }
1571
1572 void generateMissingArcsFromNode(RigGraph *rigg, ReebNode *node, int multi_level_limit)
1573 {
1574         while (node->multi_level > multi_level_limit && node->link_up)
1575         {
1576                 node = node->link_up;
1577         }
1578
1579         while (node->multi_level < multi_level_limit && node->link_down)
1580         {
1581                 node = node->link_down;
1582         }
1583
1584         if (node->multi_level == multi_level_limit)
1585         {
1586                 int i;
1587
1588                 for (i = 0; i < node->degree; i++)
1589                 {
1590                         ReebArc *earc = node->arcs[i];
1591
1592                         if (earc->flag == ARC_FREE && earc->head == node)
1593                         {
1594                                 ReebNode *other = BIF_otherNodeFromIndex(earc, node);
1595
1596                                 earc->flag = ARC_USED;
1597
1598                                 generateBonesForArc(rigg, earc, node, other);
1599                                 generateMissingArcsFromNode(rigg, other, multi_level_limit);
1600                         }
1601                 }
1602         }
1603 }
1604
1605 void generateMissingArcs(RigGraph *rigg)
1606 {
1607         ReebGraph *reebg = rigg->link_mesh;
1608         int multi_level_limit = 5;
1609
1610         for (reebg = rigg->link_mesh; reebg; reebg = reebg->link_up)
1611         {
1612                 ReebArc *earc;
1613
1614                 for (earc = reebg->arcs.first; earc; earc = earc->next)
1615                 {
1616                         if (earc->flag == ARC_USED)
1617                         {
1618                                 generateMissingArcsFromNode(rigg, earc->head, multi_level_limit);
1619                                 generateMissingArcsFromNode(rigg, earc->tail, multi_level_limit);
1620                         }
1621                 }
1622         }
1623 }
1624
1625 /************************************ RETARGETTING *****************************************************/
1626
1627 static void repositionControl(RigGraph *rigg, RigControl *ctrl, float head[3], float tail[3], float qrot[4], float resize)
1628 {
1629         RigControl *ctrl_child;
1630         float parent_offset[3], tail_offset[3];
1631
1632         VecSubf(tail_offset, ctrl->tail, ctrl->head);
1633         VecMulf(tail_offset, resize);
1634
1635         VECCOPY(parent_offset, ctrl->offset);
1636         VecMulf(parent_offset, resize);
1637
1638         QuatMulVecf(qrot, parent_offset);
1639         QuatMulVecf(qrot, tail_offset);
1640
1641         VecAddf(ctrl->bone->head, head, parent_offset);
1642         VecAddf(ctrl->bone->tail, ctrl->bone->head, tail_offset);
1643         ctrl->bone->roll = rollBoneByQuat(ctrl->bone, ctrl->up_axis, qrot);
1644
1645         ctrl->flag |= RIG_CTRL_DONE;
1646
1647         /* Cascade to connected control bones */
1648         for (ctrl_child = rigg->controls.first; ctrl_child; ctrl_child = ctrl_child->next)
1649         {
1650                 if (ctrl_child->link == ctrl->bone)
1651                 {
1652                         repositionControl(rigg, ctrl_child, ctrl->bone->head, ctrl->bone->tail, qrot, resize);
1653                 }
1654         }
1655
1656 }
1657
1658 static void repositionBone(RigGraph *rigg, RigEdge *edge, float vec0[3], float vec1[3], float *up_axis)
1659 {
1660         EditBone *bone;
1661         RigControl *ctrl;
1662         float qrot[4], resize;
1663         float v1[3], v2[3];
1664         float l1, l2;
1665
1666         bone = edge->bone;
1667
1668         VecSubf(v1, edge->tail, edge->head);
1669         VecSubf(v2, vec1, vec0);
1670
1671         l1 = Normalize(v1);
1672         l2 = Normalize(v2);
1673
1674         resize = l2 / l1;
1675
1676         RotationBetweenVectorsToQuat(qrot, v1, v2);
1677
1678         VECCOPY(bone->head, vec0);
1679         VECCOPY(bone->tail, vec1);
1680
1681         if (up_axis != NULL)
1682         {
1683                 float qroll[4];
1684
1685                 bone->roll = rollBoneByQuatAligned(bone, edge->up_axis, qrot, qroll, up_axis);
1686
1687                 QuatMul(qrot, qrot, qroll);
1688         }
1689         else
1690         {
1691                 bone->roll = rollBoneByQuat(bone, edge->up_axis, qrot);
1692         }
1693
1694         for (ctrl = rigg->controls.first; ctrl; ctrl = ctrl->next)
1695         {
1696                 if (ctrl->link == bone)
1697                 {
1698                         repositionControl(rigg, ctrl, vec0, vec1, qrot, resize);
1699                 }
1700         }
1701 }
1702
1703 static RetargetMode detectArcRetargetMode(RigArc *arc);
1704 static void retargetArctoArcLength(RigGraph *rigg, RigArc *iarc, RigNode *inode_start);
1705
1706
1707 static RetargetMode detectArcRetargetMode(RigArc *iarc)
1708 {
1709         RetargetMode mode = RETARGET_AGGRESSIVE;
1710         ReebArc *earc = iarc->link_mesh;
1711         RigEdge *edge;
1712         int large_angle = 0;
1713         float avg_angle = 0;
1714         float avg_length = 0;
1715         int nb_edges = 0;
1716
1717
1718         for (edge = iarc->edges.first; edge; edge = edge->next)
1719         {
1720                 avg_angle += edge->angle;
1721                 nb_edges++;
1722         }
1723
1724         avg_angle /= nb_edges - 1; /* -1 because last edge doesn't have an angle */
1725
1726         avg_length = iarc->length / nb_edges;
1727
1728
1729         if (nb_edges > 2)
1730         {
1731                 for (edge = iarc->edges.first; edge; edge = edge->next)
1732                 {
1733                         if (fabs(edge->angle - avg_angle) > M_PI / 6)
1734                         {
1735                                 large_angle = 1;
1736                         }
1737                 }
1738         }
1739         else if (nb_edges == 2 && avg_angle > 0)
1740         {
1741                 large_angle = 1;
1742         }
1743
1744
1745         if (large_angle == 0)
1746         {
1747                 mode = RETARGET_LENGTH;
1748         }
1749
1750         if (earc->bcount <= (iarc->count - 1))
1751         {
1752                 mode = RETARGET_LENGTH;
1753         }
1754
1755         mode = RETARGET_AGGRESSIVE;
1756
1757         return mode;
1758 }
1759
1760 #ifndef USE_THREADS
1761 static void printMovesNeeded(int *positions, int nb_positions)
1762 {
1763         int moves = 0;
1764         int i;
1765
1766         for (i = 0; i < nb_positions; i++)
1767         {
1768                 moves += positions[i] - (i + 1);
1769         }
1770
1771         printf("%i moves needed\n", moves);
1772 }
1773
1774 static void printPositions(int *positions, int nb_positions)
1775 {
1776         int i;
1777
1778         for (i = 0; i < nb_positions; i++)
1779         {
1780                 printf("%i ", positions[i]);
1781         }
1782         printf("\n");
1783 }
1784 #endif
1785
1786 #define MAX_COST 100 /* FIX ME */
1787
1788 static float costDistance(ReebArcIterator *iter, float *vec0, float *vec1, int i0, int i1)
1789 {
1790         EmbedBucket *bucket = NULL;
1791         float max_dist = 0;
1792         float v1[3], v2[3], c[3];
1793         float v1_inpf;
1794
1795         if (G.scene->toolsettings->skgen_retarget_distance_weight > 0)
1796         {
1797                 VecSubf(v1, vec0, vec1);
1798
1799                 v1_inpf = Inpf(v1, v1);
1800
1801                 if (v1_inpf > 0)
1802                 {
1803                         int j;
1804                         for (j = i0 + 1; j < i1 - 1; j++)
1805                         {
1806                                 float dist;
1807
1808                                 bucket = peekBucket(iter, j);
1809
1810                                 VecSubf(v2, bucket->p, vec1);
1811
1812                                 Crossf(c, v1, v2);
1813
1814                                 dist = Inpf(c, c) / v1_inpf;
1815
1816                                 max_dist = dist > max_dist ? dist : max_dist;
1817                         }
1818
1819                         return G.scene->toolsettings->skgen_retarget_distance_weight * max_dist;
1820                 }
1821                 else
1822                 {
1823                         return MAX_COST;
1824                 }
1825         }
1826         else
1827         {
1828                 return 0;
1829         }
1830 }
1831
1832 static float costAngle(float original_angle, float vec_first[3], float vec_second[3])
1833 {
1834         if (G.scene->toolsettings->skgen_retarget_angle_weight > 0)
1835         {
1836                 float current_angle;
1837
1838                 if (!VecIsNull(vec_first) && !VecIsNull(vec_second))
1839                 {
1840                         current_angle = saacos(Inpf(vec_first, vec_second));
1841
1842                         return G.scene->toolsettings->skgen_retarget_angle_weight * fabs(current_angle - original_angle);
1843                 }
1844                 else
1845                 {
1846                         return G.scene->toolsettings->skgen_retarget_angle_weight * M_PI;
1847                 }
1848         }
1849         else
1850         {
1851                 return 0;
1852         }
1853 }
1854
1855 static float costLength(float original_length, float current_length)
1856 {
1857         if (current_length == 0)
1858         {
1859                 return MAX_COST;
1860         }
1861         else
1862         {
1863                 float length_ratio = fabs((current_length - original_length) / original_length);
1864                 return G.scene->toolsettings->skgen_retarget_length_weight * length_ratio * length_ratio;
1865         }
1866 }
1867
1868 static float calcCostLengthDistance(ReebArcIterator *iter, float **vec_cache, RigEdge *edge, float *vec1, float *vec2, int i1, int i2)
1869 {
1870         float vec[3];
1871         float length;
1872
1873         VecSubf(vec, vec2, vec1);
1874         length = Normalize(vec);
1875
1876         return costLength(edge->length, length) + costDistance(iter, vec1, vec2, i1, i2);
1877 }
1878
1879 static float calcCostAngleLengthDistance(ReebArcIterator *iter, float **vec_cache, RigEdge *edge, float *vec0, float *vec1, float *vec2, int i1, int i2)
1880 {
1881         float vec_second[3], vec_first[3];
1882         float length2;
1883         float new_cost = 0;
1884
1885         VecSubf(vec_second, vec2, vec1);
1886         length2 = Normalize(vec_second);
1887
1888
1889         /* Angle cost */
1890         if (edge->prev)
1891         {
1892                 VecSubf(vec_first, vec1, vec0);
1893                 Normalize(vec_first);
1894
1895                 new_cost += costAngle(edge->prev->angle, vec_first, vec_second);
1896         }
1897
1898         /* Length cost */
1899         new_cost += costLength(edge->length, length2);
1900
1901         /* Distance cost */
1902         new_cost += costDistance(iter, vec1, vec2, i1, i2);
1903
1904         return new_cost;
1905 }
1906
1907 static int indexMemoNode(int nb_positions, int previous, int current, int joints_left)
1908 {
1909         return joints_left * nb_positions * nb_positions + current * nb_positions + previous;
1910 }
1911
1912 static void copyMemoPositions(int *positions, MemoNode *table, int nb_positions, int joints_left)
1913 {
1914         int previous = 0, current = 0;
1915         int i = 0;
1916
1917         for (i = 0; joints_left > 0; joints_left--, i++)
1918         {
1919                 MemoNode *node;
1920                 node = table + indexMemoNode(nb_positions, previous, current, joints_left);
1921
1922                 positions[i] = node->next;
1923
1924                 previous = current;
1925                 current = node->next;
1926         }
1927 }
1928
1929 static MemoNode * solveJoints(MemoNode *table, ReebArcIterator *iter, float **vec_cache, int nb_joints, int nb_positions, int previous, int current, RigEdge *edge, int joints_left)
1930 {
1931         MemoNode *node;
1932         int index = indexMemoNode(nb_positions, previous, current, joints_left);
1933
1934         node = table + index;
1935
1936         if (node->weight != 0)
1937         {
1938                 return node;
1939         }
1940         else if (joints_left == 0)
1941         {
1942                 float *vec1 = vec_cache[current];
1943                 float *vec2 = vec_cache[nb_positions + 1];
1944
1945                 node->weight = calcCostLengthDistance(iter, vec_cache, edge, vec1, vec2, current, iter->length);
1946
1947                 return node;
1948         }
1949         else
1950         {
1951                 MemoNode *min_node = NULL;
1952                 float *vec0 = vec_cache[previous];
1953                 float *vec1 = vec_cache[current];
1954                 float min_weight;
1955                 int min_next;
1956                 int next;
1957
1958                 for (next = current + 1; next <= nb_positions - (joints_left - 1); next++)
1959                 {
1960                         MemoNode *next_node;
1961                         float *vec2 = vec_cache[next];
1962                         float weight = 0;
1963
1964                         /* ADD WEIGHT OF PREVIOUS - CURRENT - NEXT triple */
1965                         weight = calcCostAngleLengthDistance(iter, vec_cache, edge, vec0, vec1, vec2, current, next);
1966
1967                         if (weight >= MAX_COST)
1968                         {
1969                                 continue;
1970                         }
1971
1972                         /* add node weight */
1973                         next_node = solveJoints(table, iter, vec_cache, nb_joints, nb_positions, current, next, edge->next, joints_left - 1);
1974                         weight += next_node->weight;
1975
1976                         if (min_node == NULL || weight < min_weight)
1977                         {
1978                                 min_weight = weight;
1979                                 min_node = next_node;
1980                                 min_next = next;
1981                         }
1982                 }
1983
1984                 if (min_node)
1985                 {
1986                         node->weight = min_weight;
1987                         node->next = min_next;
1988                         return node;
1989                 }
1990                 else
1991                 {
1992                         node->weight = MAX_COST;
1993                         return node;
1994                 }
1995         }
1996
1997 }
1998
1999 static int testFlipArc(RigArc *iarc, RigNode *inode_start)
2000 {
2001         ReebArc *earc = iarc->link_mesh;
2002         ReebNode *enode_start = BIF_NodeFromIndex(earc, inode_start->link_mesh);
2003
2004         /* no flip needed if both nodes are the same */
2005         if ((enode_start == earc->head && inode_start == iarc->head) || (enode_start == earc->tail && inode_start == iarc->tail))
2006         {
2007                 return 0;
2008         }
2009         else
2010         {
2011                 return 1;
2012         }
2013 }
2014
2015 static void retargetArctoArcAggresive(RigGraph *rigg, RigArc *iarc, RigNode *inode_start)
2016 {
2017         ReebArcIterator iter;
2018         RigEdge *edge;
2019         EmbedBucket *bucket = NULL;
2020         ReebNode *node_start, *node_end;
2021         ReebArc *earc = iarc->link_mesh;
2022         float min_cost = FLT_MAX;
2023         float *vec0, *vec1, *vec2;
2024         float **vec_cache;
2025         float *cost_cache;
2026         int *best_positions;
2027         int *positions;
2028         int nb_edges = BLI_countlist(&iarc->edges);
2029         int nb_joints = nb_edges - 1;
2030         RetargetMethod method = METHOD_MEMOIZE;
2031         int i;
2032
2033         if (nb_joints > earc->bcount)
2034         {
2035                 printf("NOT ENOUGH BUCKETS!\n");
2036                 return;
2037         }
2038
2039         positions = MEM_callocN(sizeof(int) * nb_joints, "Aggresive positions");
2040         best_positions = MEM_callocN(sizeof(int) * nb_joints, "Best Aggresive positions");
2041         cost_cache = MEM_callocN(sizeof(float) * nb_edges, "Cost cache");
2042         vec_cache = MEM_callocN(sizeof(float*) * (nb_edges + 1), "Vec cache");
2043
2044         if (testFlipArc(iarc, inode_start))
2045         {
2046                 node_start = earc->tail;
2047                 node_end = earc->head;
2048         }
2049         else
2050         {
2051                 node_start = earc->head;
2052                 node_end = earc->tail;
2053         }
2054
2055         /* init with first values */
2056         for (i = 0; i < nb_joints; i++)
2057         {
2058                 positions[i] = i + 1;
2059                 //positions[i] = (earc->bcount / nb_edges) * (i + 1);
2060         }
2061
2062         /* init cost cache */
2063         for (i = 0; i < nb_edges; i++)
2064         {
2065                 cost_cache[i] = 0;
2066         }
2067
2068         vec_cache[0] = node_start->p;
2069         vec_cache[nb_edges] = node_end->p;
2070
2071         if (method == METHOD_MEMOIZE)
2072         {
2073                 int nb_positions = earc->bcount;
2074                 int nb_memo_nodes = nb_positions * nb_positions * (nb_joints + 1);
2075                 MemoNode *table = MEM_callocN(nb_memo_nodes * sizeof(MemoNode), "memoization table");
2076                 MemoNode *result;
2077                 float **positions_cache = MEM_callocN(sizeof(float*) * (nb_positions + 2), "positions cache");
2078                 int i;
2079
2080                 positions_cache[0] = node_start->p;
2081                 positions_cache[nb_positions + 1] = node_end->p;
2082
2083                 initArcIterator(&iter, earc, node_start);
2084
2085                 for (i = 1; i <= nb_positions; i++)
2086                 {
2087                         EmbedBucket *bucket = peekBucket(&iter, i);
2088                         positions_cache[i] = bucket->p;
2089                 }
2090
2091                 result = solveJoints(table, &iter, positions_cache, nb_joints, earc->bcount, 0, 0, iarc->edges.first, nb_joints);
2092
2093                 min_cost = result->weight;
2094                 copyMemoPositions(best_positions, table, earc->bcount, nb_joints);
2095
2096                 MEM_freeN(table);
2097                 MEM_freeN(positions_cache);
2098         }
2099         /* BRUTE FORCE */
2100         else if (method == METHOD_BRUTE_FORCE)
2101         {
2102                 int last_index = 0;
2103                 int first_pass = 1;
2104                 int must_move = nb_joints - 1;
2105
2106                 while(1)
2107                 {
2108                         float cost = 0;
2109                         int need_calc = 0;
2110
2111                         /* increment to next possible solution */
2112
2113                         i = nb_joints - 1;
2114
2115                         if (first_pass)
2116                         {
2117                                 need_calc = 0;
2118                                 first_pass = 0;
2119                         }
2120                         else
2121                         {
2122                                 /* increment positions, starting from the last one
2123                                  * until a valid increment is found
2124                                  * */
2125                                 for (i = must_move; i >= 0; i--)
2126                                 {
2127                                         int remaining_joints = nb_joints - (i + 1);
2128
2129                                         positions[i] += 1;
2130                                         need_calc = i;
2131
2132                                         if (positions[i] + remaining_joints <= earc->bcount)
2133                                         {
2134                                                 break;
2135                                         }
2136                                 }
2137                         }
2138
2139                         if (i == -1)
2140                         {
2141                                 break;
2142                         }
2143
2144                         /* reset joints following the last increment*/
2145                         for (i = i + 1; i < nb_joints; i++)
2146                         {
2147                                 positions[i] = positions[i - 1] + 1;
2148                         }
2149
2150                         /* calculating cost */
2151                         initArcIterator(&iter, earc, node_start);
2152
2153                         vec0 = NULL;
2154                         vec1 = node_start->p;
2155                         vec2 = NULL;
2156
2157                         for (edge = iarc->edges.first, i = 0, last_index = 0;
2158                                  edge;
2159                                  edge = edge->next, i += 1)
2160                         {
2161
2162                                 if (i >= need_calc)
2163                                 {
2164                                         float vec_first[3], vec_second[3];
2165                                         float length1, length2;
2166                                         float new_cost = 0;
2167                                         int i1, i2;
2168
2169                                         if (i < nb_joints)
2170                                         {
2171                                                 i2 = positions[i];
2172                                                 bucket = peekBucket(&iter, positions[i]);
2173                                                 vec2 = bucket->p;
2174                                                 vec_cache[i + 1] = vec2; /* update cache for updated position */
2175                                         }
2176                                         else
2177                                         {
2178                                                 i2 = iter.length;
2179                                                 vec2 = node_end->p;
2180                                         }
2181
2182                                         if (i > 0)
2183                                         {
2184                                                 i1 = positions[i - 1];
2185                                         }
2186                                         else
2187                                         {
2188                                                 i1 = 1;
2189                                         }
2190
2191                                         vec1 = vec_cache[i];
2192
2193
2194                                         VecSubf(vec_second, vec2, vec1);
2195                                         length2 = Normalize(vec_second);
2196
2197                                         /* check angle */
2198                                         if (i != 0 && G.scene->toolsettings->skgen_retarget_angle_weight > 0)
2199                                         {
2200                                                 RigEdge *previous = edge->prev;
2201
2202                                                 vec0 = vec_cache[i - 1];
2203                                                 VecSubf(vec_first, vec1, vec0);
2204                                                 length1 = Normalize(vec_first);
2205
2206                                                 /* Angle cost */
2207                                                 new_cost += costAngle(previous->angle, vec_first, vec_second);
2208                                         }
2209
2210                                         /* Length Cost */
2211                                         new_cost += costLength(edge->length, length2);
2212
2213                                         /* Distance Cost */
2214                                         new_cost += costDistance(&iter, vec1, vec2, i1, i2);
2215
2216                                         cost_cache[i] = new_cost;
2217                                 }
2218
2219                                 cost += cost_cache[i];
2220
2221                                 if (cost > min_cost)
2222                                 {
2223                                         must_move = i;
2224                                         break;
2225                                 }
2226                         }
2227
2228                         if (must_move != i || must_move > nb_joints - 1)
2229                         {
2230                                 must_move = nb_joints - 1;
2231                         }
2232
2233                         /* cost optimizing */
2234                         if (cost < min_cost)
2235                         {
2236                                 min_cost = cost;
2237                                 memcpy(best_positions, positions, sizeof(int) * nb_joints);
2238                         }
2239                 }
2240         }
2241
2242         vec0 = node_start->p;
2243         initArcIterator(&iter, earc, node_start);
2244
2245 #ifndef USE_THREADS
2246         printPositions(best_positions, nb_joints);
2247         printMovesNeeded(best_positions, nb_joints);
2248         printf("min_cost %f\n", min_cost);
2249         printf("buckets: %i\n", earc->bcount);
2250 #endif
2251
2252         /* set joints to best position */
2253         for (edge = iarc->edges.first, i = 0;
2254                  edge;
2255                  edge = edge->next, i++)
2256         {
2257                 float *no = NULL;
2258                 if (i < nb_joints)
2259                 {
2260                         bucket = peekBucket(&iter, best_positions[i]);
2261                         vec1 = bucket->p;
2262                         no = bucket->no;
2263                 }
2264                 else
2265                 {
2266                         vec1 = node_end->p;
2267                 }
2268
2269                 if (edge->bone)
2270                 {
2271                         repositionBone(rigg, edge, vec0, vec1, no);
2272                 }
2273
2274                 vec0 = vec1;
2275         }
2276
2277         MEM_freeN(positions);
2278         MEM_freeN(best_positions);
2279         MEM_freeN(cost_cache);
2280         MEM_freeN(vec_cache);
2281 }
2282
2283 static void retargetArctoArcLength(RigGraph *rigg, RigArc *iarc, RigNode *inode_start)
2284 {
2285         ReebArcIterator iter;
2286         ReebArc *earc = iarc->link_mesh;
2287         ReebNode *node_start, *node_end;
2288         RigEdge *edge;
2289         EmbedBucket *bucket = NULL;
2290         float embedding_length = 0;
2291         float *vec0 = NULL;
2292         float *vec1 = NULL;
2293         float *previous_vec = NULL;
2294
2295
2296         if (testFlipArc(iarc, inode_start))
2297         {
2298                 node_start = (ReebNode*)earc->tail;
2299                 node_end = (ReebNode*)earc->head;
2300         }
2301         else
2302         {
2303                 node_start = (ReebNode*)earc->head;
2304                 node_end = (ReebNode*)earc->tail;
2305         }
2306
2307         initArcIterator(&iter, earc, node_start);
2308
2309         bucket = nextBucket(&iter);
2310
2311         vec0 = node_start->p;
2312
2313         while (bucket != NULL)
2314         {
2315                 vec1 = bucket->p;
2316
2317                 embedding_length += VecLenf(vec0, vec1);
2318
2319                 vec0 = vec1;
2320                 bucket = nextBucket(&iter);
2321         }
2322
2323         embedding_length += VecLenf(node_end->p, vec1);
2324
2325         /* fit bones */
2326         initArcIterator(&iter, earc, node_start);
2327
2328         bucket = nextBucket(&iter);
2329
2330         vec0 = node_start->p;
2331         previous_vec = vec0;
2332         vec1 = bucket->p;
2333
2334         for (edge = iarc->edges.first; edge; edge = edge->next)
2335         {
2336                 float new_bone_length = edge->length / iarc->length * embedding_length;
2337                 float *no = NULL;
2338                 float length = 0;
2339
2340                 while (bucket && new_bone_length > length)
2341                 {
2342                         length += VecLenf(previous_vec, vec1);
2343                         bucket = nextBucket(&iter);
2344                         previous_vec = vec1;
2345                         vec1 = bucket->p;
2346                         no = bucket->no;
2347                 }
2348
2349                 if (bucket == NULL)
2350                 {
2351                         vec1 = node_end->p;
2352                         no = NULL;
2353                 }
2354
2355                 /* no need to move virtual edges (space between unconnected bones) */
2356                 if (edge->bone)
2357                 {
2358                         repositionBone(rigg, edge, vec0, vec1, no);
2359                 }
2360
2361                 vec0 = vec1;
2362                 previous_vec = vec1;
2363         }
2364 }
2365
2366 static void retargetArctoArc(RigGraph *rigg, RigArc *iarc, RigNode *inode_start)
2367 {
2368 #ifdef USE_THREADS
2369         RetargetParam *p = MEM_callocN(sizeof(RetargetParam), "RetargetParam");
2370
2371         p->rigg = rigg;
2372         p->iarc = iarc;
2373         p->inode_start = inode_start;
2374
2375         BLI_insert_work(rigg->worker, p);
2376 #else
2377         RetargetParam p;
2378
2379         p.rigg = rigg;
2380         p.iarc = iarc;
2381         p.inode_start = inode_start;
2382
2383         exec_retargetArctoArc(&p);
2384 #endif
2385 }
2386
2387 void *exec_retargetArctoArc(void *param)
2388 {
2389         RetargetParam *p = (RetargetParam*)param;
2390         RigGraph *rigg = p->rigg;
2391         RigArc *iarc = p->iarc;
2392         RigNode *inode_start = p->inode_start;
2393         ReebArc *earc = iarc->link_mesh;
2394
2395         if (BLI_countlist(&iarc->edges) == 1)
2396         {
2397                 RigEdge *edge = iarc->edges.first;
2398
2399                 if (testFlipArc(iarc, inode_start))
2400                 {
2401                         repositionBone(rigg, edge, earc->tail->p, earc->head->p, NULL);
2402                 }
2403                 else
2404                 {
2405                         repositionBone(rigg, edge, earc->head->p, earc->tail->p, NULL);
2406                 }
2407         }
2408         else
2409         {
2410                 RetargetMode mode = detectArcRetargetMode(iarc);
2411
2412                 if (mode == RETARGET_AGGRESSIVE)
2413                 {
2414                         retargetArctoArcAggresive(rigg, iarc, inode_start);
2415                 }
2416                 else
2417                 {
2418                         retargetArctoArcLength(rigg, iarc, inode_start);
2419                 }
2420         }
2421
2422 #ifdef USE_THREADS
2423         MEM_freeN(p);
2424 #endif
2425
2426         return NULL;
2427 }
2428
2429 static void matchMultiResolutionNode(RigGraph *rigg, RigNode *inode, ReebNode *top_node)
2430 {
2431         ReebNode *enode = top_node;
2432         ReebGraph *reebg = BIF_graphForMultiNode(rigg->link_mesh, enode);
2433         int ishape, eshape;
2434
2435         ishape = BLI_subtreeShape((BGraph*)rigg, (BNode*)inode, NULL, 0) % SHAPE_LEVELS;
2436         eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, NULL, 0) % SHAPE_LEVELS;
2437
2438         inode->link_mesh = enode;
2439
2440         while (ishape == eshape && enode->link_down)
2441         {
2442                 inode->link_mesh = enode;
2443
2444                 enode = enode->link_down;
2445                 reebg = BIF_graphForMultiNode(rigg->link_mesh, enode); /* replace with call to link_down once that exists */
2446                 eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, NULL, 0) % SHAPE_LEVELS;
2447         }
2448 }
2449
2450 static void markMultiResolutionChildArc(ReebNode *end_enode, ReebNode *enode)
2451 {
2452         int i;
2453
2454         for(i = 0; i < enode->degree; i++)
2455         {
2456                 ReebArc *earc = (ReebArc*)enode->arcs[i];
2457
2458                 if (earc->flag == ARC_FREE)
2459                 {
2460                         earc->flag = ARC_TAKEN;
2461
2462                         if (earc->tail->degree > 1 && earc->tail != end_enode)
2463                         {
2464                                 markMultiResolutionChildArc(end_enode, earc->tail);
2465                         }
2466                         break;
2467                 }
2468         }
2469 }
2470
2471 static void markMultiResolutionArc(ReebArc *start_earc)
2472 {
2473         if (start_earc->link_up)
2474         {
2475                 ReebArc *earc;
2476                 for (earc = start_earc->link_up ; earc; earc = earc->link_up)
2477                 {
2478                         earc->flag = ARC_TAKEN;
2479
2480                         if (earc->tail->index != start_earc->tail->index)
2481                         {
2482                                 markMultiResolutionChildArc(earc->tail, earc->tail);
2483                         }
2484                 }
2485         }
2486 }
2487
2488 static void matchMultiResolutionArc(RigGraph *rigg, RigNode *start_node, RigArc *next_iarc, ReebArc *next_earc)
2489 {
2490         ReebNode *enode = next_earc->head;
2491         ReebGraph *reebg = BIF_graphForMultiNode(rigg->link_mesh, enode);
2492         int ishape, eshape;
2493
2494         ishape = BLI_subtreeShape((BGraph*)rigg, (BNode*)start_node, (BArc*)next_iarc, 1) % SHAPE_LEVELS;
2495         eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, (BArc*)next_earc, 1) % SHAPE_LEVELS;
2496
2497         while (ishape != eshape && next_earc->link_up)
2498         {
2499                 next_earc->flag = ARC_TAKEN; // mark previous as taken, to prevent backtrack on lower levels
2500
2501                 next_earc = next_earc->link_up;
2502                 reebg = reebg->link_up;
2503                 enode = next_earc->head;
2504                 eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, (BArc*)next_earc, 1) % SHAPE_LEVELS;
2505         }
2506
2507         next_earc->flag = ARC_USED;
2508         next_iarc->link_mesh = next_earc;
2509
2510         /* mark all higher levels as taken too */
2511         markMultiResolutionArc(next_earc);
2512 //      while (next_earc->link_up)
2513 //      {
2514 //              next_earc = next_earc->link_up;
2515 //              next_earc->flag = ARC_TAKEN;
2516 //      }
2517 }
2518
2519 static void matchMultiResolutionStartingNode(RigGraph *rigg, ReebGraph *reebg, RigNode *inode)
2520 {
2521         ReebNode *enode;
2522         int ishape, eshape;
2523
2524         enode = reebg->nodes.first;
2525
2526         ishape = BLI_subtreeShape((BGraph*)rigg, (BNode*)inode, NULL, 0) % SHAPE_LEVELS;
2527         eshape = BLI_subtreeShape((BGraph*)rigg->link_mesh, (BNode*)enode, NULL, 0) % SHAPE_LEVELS;
2528
2529         while (ishape != eshape && reebg->link_up)
2530         {
2531                 reebg = reebg->link_up;
2532
2533                 enode = reebg->nodes.first;
2534
2535                 eshape = BLI_subtreeShape((BGraph*)reebg, (BNode*)enode, NULL, 0) % SHAPE_LEVELS;
2536         }
2537
2538         inode->link_mesh = enode;
2539 }
2540
2541 static void findCorrespondingArc(RigGraph *rigg, RigArc *start_arc, RigNode *start_node, RigArc *next_iarc, int root)
2542 {
2543         ReebNode *enode = start_node->link_mesh;
2544         ReebArc *next_earc;
2545         int symmetry_level = next_iarc->symmetry_level;
2546         int symmetry_group = next_iarc->symmetry_group;
2547         int symmetry_flag = next_iarc->symmetry_flag;
2548         int i;
2549
2550         next_iarc->link_mesh = NULL;
2551
2552 //      if (root)
2553 //      {
2554 //              printf("-----------------------\n");
2555 //              printf("MATCHING LIMB\n");
2556 //              RIG_printArcBones(next_iarc);
2557 //      }
2558
2559         for(i = 0; i < enode->degree; i++)
2560         {
2561                 next_earc = (ReebArc*)enode->arcs[i];
2562
2563 //              if (next_earc->flag == ARC_FREE)
2564 //              {
2565 //                      printf("candidate (level %i ?= %i) (flag %i ?= %i) (group %i ?= %i)\n",
2566 //                      symmetry_level, next_earc->symmetry_level,
2567 //                      symmetry_flag, next_earc->symmetry_flag,
2568 //                      symmetry_group, next_earc->symmetry_flag);
2569 //              }
2570
2571                 if (next_earc->flag == ARC_FREE &&
2572                         next_earc->symmetry_flag == symmetry_flag &&
2573                         next_earc->symmetry_group == symmetry_group &&
2574                         next_earc->symmetry_level == symmetry_level)
2575                 {
2576 //                      printf("CORRESPONDING ARC FOUND\n");
2577 //                      printf("flag %i -- level %i -- flag %i -- group %i\n", next_earc->flag, next_earc->symmetry_level, next_earc->symmetry_flag, next_earc->symmetry_group);
2578
2579                         matchMultiResolutionArc(rigg, start_node, next_iarc, next_earc);
2580                         break;
2581                 }
2582         }
2583
2584         /* not found, try at higher nodes (lower node might have filtered internal arcs, messing shape of tree */
2585         if (next_iarc->link_mesh == NULL)
2586         {
2587 //              printf("NO CORRESPONDING ARC FOUND - GOING TO HIGHER LEVELS\n");
2588
2589                 if (enode->link_up)
2590                 {
2591                         start_node->link_mesh = enode->link_up;
2592                         findCorrespondingArc(rigg, start_arc, start_node, next_iarc, 0);
2593                 }
2594         }
2595
2596         /* still not found, print debug info */
2597         if (root && next_iarc->link_mesh == NULL)
2598         {
2599                 start_node->link_mesh = enode; /* linking back with root node */
2600
2601 //              printf("NO CORRESPONDING ARC FOUND\n");
2602 //              RIG_printArcBones(next_iarc);
2603 //
2604 //              printf("ON NODE %i, multilevel %i\n", enode->index, enode->multi_level);
2605 //
2606 //              printf("LOOKING FOR\n");
2607 //              printf("flag %i -- level %i -- flag %i -- group %i\n", ARC_FREE, symmetry_level, symmetry_flag, symmetry_group);
2608 //
2609 //              printf("CANDIDATES\n");
2610 //              for(i = 0; i < enode->degree; i++)
2611 //              {
2612 //                      next_earc = (ReebArc*)enode->arcs[i];
2613 //                      printf("flag %i -- level %i -- flag %i -- group %i\n", next_earc->flag, next_earc->symmetry_level, next_earc->symmetry_flag, next_earc->symmetry_group);
2614 //              }
2615
2616                 /* Emergency matching */
2617                 for(i = 0; i < enode->degree; i++)
2618                 {
2619                         next_earc = (ReebArc*)enode->arcs[i];
2620
2621                         if (next_earc->flag == ARC_FREE && next_earc->symmetry_level == symmetry_level)
2622                         {
2623 //                              printf("USING: \n");
2624 //                              printf("flag %i -- level %i -- flag %i -- group %i\n", next_earc->flag, next_earc->symmetry_level, next_earc->symmetry_flag, next_earc->symmetry_group);
2625                                 matchMultiResolutionArc(rigg, start_node, next_iarc, next_earc);
2626                                 break;
2627                         }
2628                 }
2629         }
2630
2631 }
2632
2633 static void retargetSubgraph(RigGraph *rigg, RigArc *start_arc, RigNode *start_node)
2634 {
2635         RigNode *inode = start_node;
2636         int i;
2637
2638         /* no start arc on first node */
2639         if (start_arc)
2640         {
2641                 ReebNode *enode = start_node->link_mesh;
2642                 ReebArc *earc = start_arc->link_mesh;
2643
2644                 retargetArctoArc(rigg, start_arc, start_node);
2645
2646                 enode = BIF_otherNodeFromIndex(earc, enode);
2647                 inode = (RigNode*)BLI_otherNode((BArc*)start_arc, (BNode*)inode);
2648
2649                 /* match with lowest node with correct shape */
2650                 matchMultiResolutionNode(rigg, inode, enode);
2651         }
2652
2653         for(i = 0; i < inode->degree; i++)
2654         {
2655                 RigArc *next_iarc = (RigArc*)inode->arcs[i];
2656
2657                 /* no back tracking */
2658                 if (next_iarc != start_arc)
2659                 {
2660                         findCorrespondingArc(rigg, start_arc, inode, next_iarc, 1);
2661                         if (next_iarc->link_mesh)
2662                         {
2663                                 retargetSubgraph(rigg, next_iarc, inode);
2664                         }
2665                 }
2666         }
2667 }
2668
2669 static void finishRetarget(RigGraph *rigg)
2670 {
2671 #ifdef USE_THREADS
2672         BLI_end_worker(rigg->worker);
2673 #endif
2674 }
2675
2676 static void adjustGraphs(RigGraph *rigg)
2677 {
2678         RigArc *arc;
2679
2680         for (arc = rigg->arcs.first; arc; arc = arc->next)
2681         {
2682                 if (arc->link_mesh)
2683                 {
2684                         retargetArctoArc(rigg, arc, arc->head);
2685                 }
2686         }
2687
2688         finishRetarget(rigg);
2689
2690         /* Turn the list into an armature */
2691         editbones_to_armature(rigg->editbones, rigg->ob);
2692
2693         BIF_undo_push("Retarget Skeleton");
2694 }
2695
2696 static void retargetGraphs(RigGraph *rigg)
2697 {
2698         ReebGraph *reebg = rigg->link_mesh;
2699         RigNode *inode;
2700
2701         /* flag all ReebArcs as free */
2702         BIF_flagMultiArcs(reebg, ARC_FREE);
2703
2704         /* return to first level */
2705         reebg = rigg->link_mesh;
2706
2707         inode = rigg->head;
2708
2709         matchMultiResolutionStartingNode(rigg, reebg, inode);
2710
2711         retargetSubgraph(rigg, NULL, inode);
2712
2713         //generateMissingArcs(rigg);
2714
2715         finishRetarget(rigg);
2716
2717         /* Turn the list into an armature */
2718         editbones_to_armature(rigg->editbones, rigg->ob);
2719 }
2720
2721
2722 void BIF_retargetArmature()
2723 {
2724         Object *ob;
2725         Base *base;
2726         ReebGraph *reebg;
2727         double start_time, end_time;
2728         double gstart_time, gend_time;
2729         double reeb_time, rig_time, retarget_time, total_time;
2730
2731         gstart_time = start_time = PIL_check_seconds_timer();
2732
2733         reebg = BIF_ReebGraphMultiFromEditMesh();
2734
2735         end_time = PIL_check_seconds_timer();
2736         reeb_time = end_time - start_time;
2737
2738         printf("Reeb Graph created\n");
2739
2740         base= FIRSTBASE;
2741         for (base = FIRSTBASE; base; base = base->next)
2742         {
2743                 if TESTBASELIB(base) {
2744                         ob = base->object;
2745
2746                         if (ob->type==OB_ARMATURE)
2747                         {
2748                                 RigGraph *rigg;
2749                                 bArmature *arm;
2750
2751                                 arm = ob->data;
2752
2753                                 /* Put the armature into editmode */
2754
2755
2756                                 start_time = PIL_check_seconds_timer();
2757
2758                                 rigg = armatureToGraph(ob, arm);
2759
2760                                 end_time = PIL_check_seconds_timer();
2761                                 rig_time = end_time - start_time;
2762
2763                                 printf("Armature graph created\n");
2764
2765                                 //RIG_printGraph(rigg);
2766
2767                                 rigg->link_mesh = reebg;
2768
2769                                 printf("retargetting %s\n", ob->id.name);
2770
2771                                 start_time = PIL_check_seconds_timer();
2772
2773                                 retargetGraphs(rigg);
2774
2775                                 end_time = PIL_check_seconds_timer();
2776                                 retarget_time = end_time - start_time;
2777
2778                                 BIF_freeRetarget();
2779
2780                                 GLOBAL_RIGG = rigg;
2781
2782                                 break; /* only one armature at a time */
2783                         }
2784                 }
2785         }
2786
2787         gend_time = PIL_check_seconds_timer();
2788
2789         total_time = gend_time - gstart_time;
2790
2791         printf("-----------\n");
2792         printf("runtime: \t%.3f\n", total_time);
2793         printf("reeb: \t\t%.3f (%.1f%%)\n", reeb_time, reeb_time / total_time * 100);
2794         printf("rig: \t\t%.3f (%.1f%%)\n", rig_time, rig_time / total_time * 100);
2795         printf("retarget: \t%.3f (%.1f%%)\n", retarget_time, retarget_time / total_time * 100);
2796         printf("-----------\n");
2797
2798         BIF_undo_push("Retarget Skeleton");
2799
2800         allqueue(REDRAWVIEW3D, 0);
2801 }
2802
2803 void BIF_retargetArc(ReebArc *earc)
2804 {
2805         Object *ob;
2806         RigGraph *template;
2807         RigGraph *rigg;
2808         RigArc *iarc;
2809         bArmature *arm;
2810
2811         ob = G.obedit;
2812         arm = ob->data;
2813
2814         template = armatureSelectedToGraph(ob, arm);
2815
2816         if (template->arcs.first == NULL)
2817         {
2818                 error("No deforming bones selected");
2819                 return;
2820         }
2821
2822         rigg = cloneRigGraph(template);
2823
2824         iarc = rigg->arcs.first;
2825
2826         iarc->link_mesh = earc;
2827         iarc->head->link_mesh = earc->head;
2828         iarc->tail->link_mesh = earc->tail;
2829
2830         retargetArctoArc(rigg, iarc, iarc->head);
2831
2832         finishRetarget(rigg);
2833
2834         RIG_freeRigGraph((BGraph*)template);
2835         RIG_freeRigGraph((BGraph*)rigg);
2836
2837         BIF_undo_push("Retarget Arc");
2838
2839         allqueue(REDRAWVIEW3D, 0);
2840 }
2841
2842 void BIF_adjustRetarget()
2843 {
2844         if (GLOBAL_RIGG)
2845         {
2846                 adjustGraphs(GLOBAL_RIGG);
2847         }
2848 }
2849
2850 void BIF_freeRetarget()
2851 {
2852         if (GLOBAL_RIGG)
2853         {
2854                 RIG_freeRigGraph((BGraph*)GLOBAL_RIGG);
2855                 GLOBAL_RIGG = NULL;
2856         }
2857 }