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