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