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