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