911e8aae3407fdbdf3257abef5565c9b86334b0e
[blender.git] / source / blender / blenlib / intern / graph.c
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * Contributor(s): Martin Poirier
19  *
20  * ***** END GPL LICENSE BLOCK *****
21  * graph.c: Common graph interface and methods
22  */
23
24 /** \file blender/blenlib/intern/graph.c
25  *  \ingroup bli
26  */
27
28 #include <float.h>
29 #include <math.h>
30
31 #include "MEM_guardedalloc.h"
32
33 #include "BLI_utildefines.h"
34 #include "BLI_listbase.h"
35 #include "BLI_graph.h"
36 #include "BLI_math.h"
37
38
39 static void testRadialSymmetry(BGraph *graph, BNode *root_node, RadialArc *ring, int total, float axis[3], float limit, int group);
40
41 static void handleAxialSymmetry(BGraph *graph, BNode *root_node, int depth, float axis[3], float limit);
42 static void testAxialSymmetry(BGraph *graph, BNode *root_node, BNode *node1, BNode *node2, BArc *arc1, BArc *arc2, float axis[3], float limit, int group);
43 static void flagAxialSymmetry(BNode *root_node, BNode *end_node, BArc *arc, int group);
44
45 void BLI_freeNode(BGraph *graph, BNode *node)
46 {
47         if (node->arcs) {
48                 MEM_freeN(node->arcs);
49         }
50         
51         if (graph->free_node) {
52                 graph->free_node(node);
53         }
54 }
55
56 void BLI_removeNode(BGraph *graph, BNode *node)
57 {
58         BLI_freeNode(graph, node);
59         BLI_freelinkN(&graph->nodes, node);
60 }
61
62 BNode *BLI_otherNode(BArc *arc, BNode *node)
63 {
64         return (arc->head == node) ? arc->tail : arc->head;
65 }
66
67 void BLI_removeArc(BGraph *graph, BArc *arc)
68 {
69         if (graph->free_arc) {
70                 graph->free_arc(arc);
71         }
72
73         BLI_freelinkN(&graph->arcs, arc);
74 }
75
76 void BLI_flagNodes(BGraph *graph, int flag)
77 {
78         BNode *node;
79         
80         for (node = graph->nodes.first; node; node = node->next) {
81                 node->flag = flag;
82         }
83 }
84
85 void BLI_flagArcs(BGraph *graph, int flag)
86 {
87         BArc *arc;
88         
89         for (arc = graph->arcs.first; arc; arc = arc->next) {
90                 arc->flag = flag;
91         }
92 }
93
94 static void addArcToNodeAdjacencyList(BNode *node, BArc *arc)
95 {
96         node->arcs[node->flag] = arc;
97         node->flag++;
98 }
99
100 void BLI_buildAdjacencyList(BGraph *graph)
101 {
102         BNode *node;
103         BArc *arc;
104
105         for (node = graph->nodes.first; node; node = node->next) {
106                 if (node->arcs != NULL) {
107                         MEM_freeN(node->arcs);
108                 }
109                 
110                 node->arcs = MEM_callocN((node->degree) * sizeof(BArc *), "adjacency list");
111                 
112                 /* temporary use to indicate the first index available in the lists */
113                 node->flag = 0;
114         }
115
116         for (arc = graph->arcs.first; arc; arc = arc->next) {
117                 addArcToNodeAdjacencyList(arc->head, arc);
118                 addArcToNodeAdjacencyList(arc->tail, arc);
119         }
120
121         for (node = graph->nodes.first; node; node = node->next) {
122                 if (node->degree != node->flag) {
123                         printf("error in node [%p]. Added only %i arcs out of %i\n", (void *)node, node->flag, node->degree);
124                 }
125         }
126 }
127
128 void BLI_rebuildAdjacencyListForNode(BGraph *graph, BNode *node)
129 {
130         BArc *arc;
131
132         if (node->arcs != NULL) {
133                 MEM_freeN(node->arcs);
134         }
135         
136         node->arcs = MEM_callocN((node->degree) * sizeof(BArc *), "adjacency list");
137         
138         /* temporary use to indicate the first index available in the lists */
139         node->flag = 0;
140
141         for (arc = graph->arcs.first; arc; arc = arc->next) {
142                 if (arc->head == node) {
143                         addArcToNodeAdjacencyList(arc->head, arc);
144                 }
145                 else if (arc->tail == node) {
146                         addArcToNodeAdjacencyList(arc->tail, arc);
147                 }
148         }
149
150         if (node->degree != node->flag) {
151                 printf("error in node [%p]. Added only %i arcs out of %i\n", (void *)node, node->flag, node->degree);
152         }
153 }
154
155 void BLI_freeAdjacencyList(BGraph *graph)
156 {
157         BNode *node;
158
159         for (node = graph->nodes.first; node; node = node->next) {
160                 if (node->arcs != NULL) {
161                         MEM_freeN(node->arcs);
162                         node->arcs = NULL;
163                 }
164         }
165 }
166
167 bool BLI_hasAdjacencyList(BGraph *graph)
168 {
169         BNode *node;
170         
171         for (node = graph->nodes.first; node; node = node->next) {
172                 if (node->arcs == NULL) {
173                         return false;
174                 }
175         }
176         
177         return true;
178 }
179
180 void BLI_replaceNodeInArc(BGraph *graph, BArc *arc, BNode *node_src, BNode *node_replaced)
181 {
182         if (arc->head == node_replaced) {
183                 arc->head = node_src;
184                 node_src->degree++;
185         }
186
187         if (arc->tail == node_replaced) {
188                 arc->tail = node_src;
189                 node_src->degree++;
190         }
191         
192         if (arc->head == arc->tail) {
193                 node_src->degree -= 2;
194                 
195                 graph->free_arc(arc);
196                 BLI_freelinkN(&graph->arcs, arc);
197         }
198
199         if (node_replaced->degree == 0) {
200                 BLI_removeNode(graph, node_replaced);
201         }
202 }
203
204 void BLI_replaceNode(BGraph *graph, BNode *node_src, BNode *node_replaced)
205 {
206         BArc *arc, *next_arc;
207         
208         for (arc = graph->arcs.first; arc; arc = next_arc) {
209                 next_arc = arc->next;
210                 
211                 if (arc->head == node_replaced) {
212                         arc->head = node_src;
213                         node_replaced->degree--;
214                         node_src->degree++;
215                 }
216
217                 if (arc->tail == node_replaced) {
218                         arc->tail = node_src;
219                         node_replaced->degree--;
220                         node_src->degree++;
221                 }
222                 
223                 if (arc->head == arc->tail) {
224                         node_src->degree -= 2;
225                         
226                         graph->free_arc(arc);
227                         BLI_freelinkN(&graph->arcs, arc);
228                 }
229         }
230         
231         if (node_replaced->degree == 0) {
232                 BLI_removeNode(graph, node_replaced);
233         }
234 }
235
236 void BLI_removeDoubleNodes(BGraph *graph, float limit)
237 {
238         const float limit_sq = limit * limit;
239         BNode *node_src, *node_replaced;
240         
241         for (node_src = graph->nodes.first; node_src; node_src = node_src->next) {
242                 for (node_replaced = graph->nodes.first; node_replaced; node_replaced = node_replaced->next) {
243                         if (node_replaced != node_src && len_squared_v3v3(node_replaced->p, node_src->p) <= limit_sq) {
244                                 BLI_replaceNode(graph, node_src, node_replaced);
245                         }
246                 }
247         }
248         
249 }
250
251 BNode *BLI_FindNodeByPosition(BGraph *graph, const float p[3], const float limit)
252 {
253         const float limit_sq = limit * limit;
254         BNode *closest_node = NULL, *node;
255         float min_distance = 0.0f;
256         
257         for (node = graph->nodes.first; node; node = node->next) {
258                 float distance = len_squared_v3v3(p, node->p);
259                 if (distance <= limit_sq && (closest_node == NULL || distance < min_distance)) {
260                         closest_node = node;
261                         min_distance = distance;
262                 }
263         }
264         
265         return closest_node;
266 }
267 /************************************* SUBGRAPH DETECTION **********************************************/
268
269 static void flagSubgraph(BNode *node, int subgraph)
270 {
271         if (node->subgraph_index == 0) {
272                 BArc *arc;
273                 int i;
274                 
275                 node->subgraph_index = subgraph;
276                 
277                 for (i = 0; i < node->degree; i++) {
278                         arc = node->arcs[i];
279                         flagSubgraph(BLI_otherNode(arc, node), subgraph);
280                 }
281         }
282
283
284 int BLI_FlagSubgraphs(BGraph *graph)
285 {
286         BNode *node;
287         int subgraph = 0;
288
289         if (BLI_hasAdjacencyList(graph) == 0) {
290                 BLI_buildAdjacencyList(graph);
291         }
292         
293         for (node = graph->nodes.first; node; node = node->next) {
294                 node->subgraph_index = 0;
295         }
296         
297         for (node = graph->nodes.first; node; node = node->next) {
298                 if (node->subgraph_index == 0) {
299                         subgraph++;
300                         flagSubgraph(node, subgraph);
301                 }
302         }
303         
304         return subgraph;
305 }
306
307 void BLI_ReflagSubgraph(BGraph *graph, int old_subgraph, int new_subgraph)
308 {
309         BNode *node;
310
311         for (node = graph->nodes.first; node; node = node->next) {
312                 if (node->flag == old_subgraph) {
313                         node->flag = new_subgraph;
314                 }
315         }
316 }
317
318 /*************************************** CYCLE DETECTION ***********************************************/
319
320 static bool detectCycle(BNode *node, BArc *src_arc)
321 {
322         bool value = false;
323         
324         if (node->flag == 0) {
325                 int i;
326
327                 /* mark node as visited */
328                 node->flag = 1;
329
330                 for (i = 0; i < node->degree && value == 0; i++) {
331                         BArc *arc = node->arcs[i];
332                         
333                         /* don't go back on the source arc */
334                         if (arc != src_arc) {
335                                 value = detectCycle(BLI_otherNode(arc, node), arc);
336                         }
337                 }
338         }
339         else {
340                 value = true;
341         }
342         
343         return value;
344 }
345
346 bool BLI_isGraphCyclic(BGraph *graph)
347 {
348         BNode *node;
349         bool value = false;
350         
351         /* NEED TO CHECK IF ADJACENCY LIST EXIST */
352         
353         /* Mark all nodes as not visited */
354         BLI_flagNodes(graph, 0);
355
356         /* detectCycles in subgraphs */
357         for (node = graph->nodes.first; node && value == false; node = node->next) {
358                 /* only for nodes in subgraphs that haven't been visited yet */
359                 if (node->flag == 0) {
360                         value = value || detectCycle(node, NULL);
361                 }
362         }
363         
364         return value;
365 }
366
367 BArc *BLI_findConnectedArc(BGraph *graph, BArc *arc, BNode *v)
368 {
369         BArc *nextArc;
370         
371         for (nextArc = graph->arcs.first; nextArc; nextArc = nextArc->next) {
372                 if (arc != nextArc && (nextArc->head == v || nextArc->tail == v)) {
373                         break;
374                 }
375         }
376         
377         return nextArc;
378 }
379
380 /*********************************** GRAPH AS TREE FUNCTIONS *******************************************/
381
382 static int subtreeShape(BNode *node, BArc *rootArc, int include_root)
383 {
384         int depth = 0;
385         
386         node->flag = 1;
387         
388         if (include_root) {
389                 BNode *newNode = BLI_otherNode(rootArc, node);
390                 return subtreeShape(newNode, rootArc, 0);
391         }
392         else {
393                 /* Base case, no arcs leading away */
394                 if (node->arcs == NULL || *(node->arcs) == NULL) {
395                         return 0;
396                 }
397                 else {
398                         int i;
399         
400                         for (i = 0; i < node->degree; i++) {
401                                 BArc *arc = node->arcs[i];
402                                 BNode *newNode = BLI_otherNode(arc, node);
403                                 
404                                 /* stop immediate and cyclic backtracking */
405                                 if (arc != rootArc && newNode->flag == 0) {
406                                         depth += subtreeShape(newNode, arc, 0);
407                                 }
408                         }
409                 }
410                 
411                 return SHAPE_RADIX * depth + 1;
412         }
413 }
414
415 int BLI_subtreeShape(BGraph *graph, BNode *node, BArc *rootArc, int include_root)
416 {
417         BLI_flagNodes(graph, 0);
418         return subtreeShape(node, rootArc, include_root);
419 }
420
421 float BLI_subtreeLength(BNode *node)
422 {
423         float length = 0;
424         int i;
425
426         node->flag = 0; /* flag node as visited */
427
428         for (i = 0; i < node->degree; i++) {
429                 BArc *arc = node->arcs[i];
430                 BNode *other_node = BLI_otherNode(arc, node);
431                 
432                 if (other_node->flag != 0) {
433                         float subgraph_length = arc->length + BLI_subtreeLength(other_node); 
434                         length = MAX2(length, subgraph_length);
435                 }
436         }
437         
438         return length;
439 }
440
441 void BLI_calcGraphLength(BGraph *graph)
442 {
443         float length = 0;
444         int nb_subgraphs;
445         int i;
446         
447         nb_subgraphs = BLI_FlagSubgraphs(graph);
448         
449         for (i = 1; i <= nb_subgraphs; i++) {
450                 BNode *node;
451                 
452                 for (node = graph->nodes.first; node; node = node->next) {
453                         /* start on an external node  of the subgraph */
454                         if (node->subgraph_index == i && node->degree == 1) {
455                                 float subgraph_length = BLI_subtreeLength(node);
456                                 length = MAX2(length, subgraph_length);
457                                 break;
458                         }
459                 }
460         }
461         
462         graph->length = length;
463 }
464
465 /********************************* SYMMETRY DETECTION **************************************************/
466
467 static void markdownSymmetryArc(BGraph *graph, BArc *arc, BNode *node, int level, float limit);
468
469 void BLI_mirrorAlongAxis(float v[3], float center[3], float axis[3])
470 {
471         float dv[3], pv[3];
472         
473         sub_v3_v3v3(dv, v, center);
474         project_v3_v3v3(pv, dv, axis);
475         mul_v3_fl(pv, -2);
476         add_v3_v3(v, pv);
477 }
478
479 static void testRadialSymmetry(BGraph *graph, BNode *root_node, RadialArc *ring, int total, float axis[3], float limit, int group)
480 {
481         const float limit_sq = limit * limit;
482         int symmetric = 1;
483         int i;
484         
485         /* sort ring by angle */
486         for (i = 0; i < total - 1; i++) {
487                 float minAngle = FLT_MAX;
488                 int minIndex = -1;
489                 int j;
490
491                 for (j = i + 1; j < total; j++) {
492                         float angle = dot_v3v3(ring[i].n, ring[j].n);
493
494                         /* map negative values to 1..2 */
495                         if (angle < 0) {
496                                 angle = 1 - angle;
497                         }
498
499                         if (angle < minAngle) {
500                                 minIndex = j;
501                                 minAngle = angle;
502                         }
503                 }
504
505                 /* swap if needed */
506                 if (minIndex != i + 1) {
507                         RadialArc tmp;
508                         tmp = ring[i + 1];
509                         ring[i + 1] = ring[minIndex];
510                         ring[minIndex] = tmp;
511                 }
512         }
513
514         for (i = 0; i < total && symmetric; i++) {
515                 BNode *node1, *node2;
516                 float tangent[3];
517                 float normal[3];
518                 float p[3];
519                 int j = (i + 1) % total; /* next arc in the circular list */
520
521                 add_v3_v3v3(tangent, ring[i].n, ring[j].n);
522                 cross_v3_v3v3(normal, tangent, axis);
523                 
524                 node1 = BLI_otherNode(ring[i].arc, root_node);
525                 node2 = BLI_otherNode(ring[j].arc, root_node);
526
527                 copy_v3_v3(p, node2->p);
528                 BLI_mirrorAlongAxis(p, root_node->p, normal);
529                 
530                 /* check if it's within limit before continuing */
531                 if (len_squared_v3v3(node1->p, p) > limit_sq) {
532                         symmetric = 0;
533                 }
534
535         }
536
537         if (symmetric) {
538                 /* mark node as symmetric physically */
539                 copy_v3_v3(root_node->symmetry_axis, axis);
540                 root_node->symmetry_flag |= SYM_PHYSICAL;
541                 root_node->symmetry_flag |= SYM_RADIAL;
542                 
543                 /* FLAG SYMMETRY GROUP */
544                 for (i = 0; i < total; i++) {
545                         ring[i].arc->symmetry_group = group;
546                         ring[i].arc->symmetry_flag = SYM_SIDE_RADIAL + i;
547                 }
548
549                 if (graph->radial_symmetry) {
550                         graph->radial_symmetry(root_node, ring, total);
551                 }
552         }
553 }
554
555 static void handleRadialSymmetry(BGraph *graph, BNode *root_node, int depth, float axis[3], float limit)
556 {
557         RadialArc *ring = NULL;
558         RadialArc *unit;
559         int total = 0;
560         int group;
561         int first;
562         int i;
563
564         /* mark topological symmetry */
565         root_node->symmetry_flag |= SYM_TOPOLOGICAL;
566
567         /* total the number of arcs in the symmetry ring */
568         for (i = 0; i < root_node->degree; i++) {
569                 BArc *connectedArc = root_node->arcs[i];
570                 
571                 /* depth is store as a negative in flag. symmetry level is positive */
572                 if (connectedArc->symmetry_level == -depth) {
573                         total++;
574                 }
575         }
576
577         ring = MEM_callocN(sizeof(RadialArc) * total, "radial symmetry ring");
578         unit = ring;
579
580         /* fill in the ring */
581         for (i = 0; i < root_node->degree; i++) {
582                 BArc *connectedArc = root_node->arcs[i];
583                 
584                 /* depth is store as a negative in flag. symmetry level is positive */
585                 if (connectedArc->symmetry_level == -depth) {
586                         BNode *otherNode = BLI_otherNode(connectedArc, root_node);
587                         float vec[3];
588
589                         unit->arc = connectedArc;
590
591                         /* project the node to node vector on the symmetry plane */
592                         sub_v3_v3v3(unit->n, otherNode->p, root_node->p);
593                         project_v3_v3v3(vec, unit->n, axis);
594                         sub_v3_v3v3(unit->n, unit->n, vec);
595
596                         normalize_v3(unit->n);
597
598                         unit++;
599                 }
600         }
601
602         /* sort ring by arc length
603          * using a rather bogus insertion sort
604          * but rings will never get too big to matter
605          * */
606         for (i = 0; i < total; i++) {
607                 int j;
608
609                 for (j = i - 1; j >= 0; j--) {
610                         BArc *arc1, *arc2;
611                         
612                         arc1 = ring[j].arc;
613                         arc2 = ring[j + 1].arc;
614                         
615                         if (arc1->length > arc2->length) {
616                                 /* swap with smaller */
617                                 RadialArc tmp;
618                                 
619                                 tmp = ring[j + 1];
620                                 ring[j + 1] = ring[j];
621                                 ring[j] = tmp;
622                         }
623                         else {
624                                 break;
625                         }
626                 }
627         }
628
629         /* Dispatch to specific symmetry tests */
630         first = 0;
631         group = 0;
632         
633         for (i = 1; i < total; i++) {
634                 int dispatch = 0;
635                 int last = i - 1;
636                 
637                 if (fabsf(ring[first].arc->length - ring[i].arc->length) > limit) {
638                         dispatch = 1;
639                 }
640
641                 /* if not dispatching already and on last arc
642                  * Dispatch using current arc as last
643                  */
644                 if (dispatch == 0 && i == total - 1) {
645                         last = i;
646                         dispatch = 1;
647                 }
648                 
649                 if (dispatch) {
650                         int sub_total = last - first + 1; 
651
652                         group += 1;
653
654                         if (sub_total == 1) {
655                                 group -= 1; /* not really a group so decrement */
656                                 /* NOTHING TO DO */
657                         }
658                         else if (sub_total == 2) {
659                                 BArc *arc1, *arc2;
660                                 BNode *node1, *node2;
661                                 
662                                 arc1 = ring[first].arc;
663                                 arc2 = ring[last].arc;
664                                 
665                                 node1 = BLI_otherNode(arc1, root_node);
666                                 node2 = BLI_otherNode(arc2, root_node);
667                                 
668                                 testAxialSymmetry(graph, root_node, node1, node2, arc1, arc2, axis, limit, group);
669                         }
670                         else if (sub_total != total) /* allocate a new sub ring if needed */ {
671                                 RadialArc *sub_ring = MEM_callocN(sizeof(RadialArc) * sub_total, "radial symmetry ring");
672                                 int sub_i;
673                                 
674                                 /* fill in the sub ring */
675                                 for (sub_i = 0; sub_i < sub_total; sub_i++) {
676                                         sub_ring[sub_i] = ring[first + sub_i];
677                                 }
678                                 
679                                 testRadialSymmetry(graph, root_node, sub_ring, sub_total, axis, limit, group);
680                         
681                                 MEM_freeN(sub_ring);
682                         }
683                         else if (sub_total == total) {
684                                 testRadialSymmetry(graph, root_node, ring, total, axis, limit, group);
685                         }
686                         
687                         first = i;
688                 }
689         }
690
691
692         MEM_freeN(ring);
693 }
694
695 static void flagAxialSymmetry(BNode *root_node, BNode *end_node, BArc *arc, int group)
696 {
697         float vec[3];
698         
699         arc->symmetry_group = group;
700         
701         sub_v3_v3v3(vec, end_node->p, root_node->p);
702         
703         if (dot_v3v3(vec, root_node->symmetry_axis) < 0) {
704                 arc->symmetry_flag |= SYM_SIDE_NEGATIVE;
705         }
706         else {
707                 arc->symmetry_flag |= SYM_SIDE_POSITIVE;
708         }
709 }
710
711 static void testAxialSymmetry(BGraph *graph, BNode *root_node, BNode *node1, BNode *node2, BArc *arc1, BArc *arc2, float axis[3], float limit, int group)
712 {
713         const float limit_sq = limit * limit;
714         float nor[3], vec[3], p[3];
715
716         sub_v3_v3v3(p, node1->p, root_node->p);
717         cross_v3_v3v3(nor, p, axis);
718
719         sub_v3_v3v3(p, root_node->p, node2->p);
720         cross_v3_v3v3(vec, p, axis);
721         add_v3_v3(vec, nor);
722         
723         cross_v3_v3v3(nor, vec, axis);
724         
725         if (fabsf(nor[0]) > fabsf(nor[1]) && fabsf(nor[0]) > fabsf(nor[2]) && nor[0] < 0) {
726                 negate_v3(nor);
727         }
728         else if (fabsf(nor[1]) > fabsf(nor[0]) && fabsf(nor[1]) > fabsf(nor[2]) && nor[1] < 0) {
729                 negate_v3(nor);
730         }
731         else if (fabsf(nor[2]) > fabsf(nor[1]) && fabsf(nor[2]) > fabsf(nor[0]) && nor[2] < 0) {
732                 negate_v3(nor);
733         }
734         
735         /* mirror node2 along axis */
736         copy_v3_v3(p, node2->p);
737         BLI_mirrorAlongAxis(p, root_node->p, nor);
738         
739         /* check if it's within limit before continuing */
740         if (len_squared_v3v3(node1->p, p) <= limit_sq) {
741                 /* mark node as symmetric physically */
742                 copy_v3_v3(root_node->symmetry_axis, nor);
743                 root_node->symmetry_flag |= SYM_PHYSICAL;
744                 root_node->symmetry_flag |= SYM_AXIAL;
745
746                 /* flag side on arcs */
747                 flagAxialSymmetry(root_node, node1, arc1, group);
748                 flagAxialSymmetry(root_node, node2, arc2, group);
749                 
750                 if (graph->axial_symmetry) {
751                         graph->axial_symmetry(root_node, node1, node2, arc1, arc2);
752                 }
753         }
754         else {
755                 /* NOT SYMMETRIC */
756         }
757 }
758
759 static void handleAxialSymmetry(BGraph *graph, BNode *root_node, int depth, float axis[3], float limit)
760 {
761         BArc *arc1 = NULL, *arc2 = NULL;
762         BNode *node1 = NULL, *node2 = NULL;
763         int i;
764         
765         /* mark topological symmetry */
766         root_node->symmetry_flag |= SYM_TOPOLOGICAL;
767
768         for (i = 0; i < root_node->degree; i++) {
769                 BArc *connectedArc = root_node->arcs[i];
770                 
771                 /* depth is store as a negative in flag. symmetry level is positive */
772                 if (connectedArc->symmetry_level == -depth) {
773                         if (arc1 == NULL) {
774                                 arc1 = connectedArc;
775                                 node1 = BLI_otherNode(arc1, root_node);
776                         }
777                         else {
778                                 arc2 = connectedArc;
779                                 node2 = BLI_otherNode(arc2, root_node);
780                                 break; /* Can stop now, the two arcs have been found */
781                         }
782                 }
783         }
784         
785         /* shouldn't happen, but just to be sure */
786         if (node1 == NULL || node2 == NULL) {
787                 return;
788         }
789         
790         testAxialSymmetry(graph, root_node, node1, node2, arc1, arc2, axis, limit, 1);
791 }
792
793 static void markdownSecondarySymmetry(BGraph *graph, BNode *node, int depth, int level, float limit)
794 {
795         float axis[3] = {0, 0, 0};
796         int count = 0;
797         int i;
798         
799         /* count the number of branches in this symmetry group
800          * and determinate the axis of symmetry
801          */
802         for (i = 0; i < node->degree; i++) {
803                 BArc *connectedArc = node->arcs[i];
804                 
805                 /* depth is store as a negative in flag. symmetry level is positive */
806                 if (connectedArc->symmetry_level == -depth) {
807                         count++;
808                 }
809                 /* If arc is on the axis */
810                 else if (connectedArc->symmetry_level == level) {
811                         add_v3_v3(axis, connectedArc->head->p);
812                         sub_v3_v3v3(axis, axis, connectedArc->tail->p);
813                 }
814         }
815
816         normalize_v3(axis);
817
818         /* Split between axial and radial symmetry */
819         if (count == 2) {
820                 handleAxialSymmetry(graph, node, depth, axis, limit);
821         }
822         else {
823                 handleRadialSymmetry(graph, node, depth, axis, limit);
824         }
825                 
826         /* markdown secondary symetries */
827         for (i = 0; i < node->degree; i++) {
828                 BArc *connectedArc = node->arcs[i];
829                 
830                 if (connectedArc->symmetry_level == -depth) {
831                         /* markdown symmetry for branches corresponding to the depth */
832                         markdownSymmetryArc(graph, connectedArc, node, level + 1, limit);
833                 }
834         }
835 }
836
837 static void markdownSymmetryArc(BGraph *graph, BArc *arc, BNode *node, int level, float limit)
838 {
839         int i;
840
841         /* if arc is null, we start straight from a node */
842         if (arc) {
843                 arc->symmetry_level = level;
844                 
845                 node = BLI_otherNode(arc, node);
846         }
847         
848         for (i = 0; i < node->degree; i++) {
849                 BArc *connectedArc = node->arcs[i];
850                 
851                 if (connectedArc != arc) {
852                         BNode *connectedNode = BLI_otherNode(connectedArc, node);
853                         
854                         /* symmetry level is positive value, negative values is subtree depth */
855                         connectedArc->symmetry_level = -BLI_subtreeShape(graph, connectedNode, connectedArc, 0);
856                 }
857         }
858
859         arc = NULL;
860
861         for (i = 0; i < node->degree; i++) {
862                 int issymmetryAxis = 0;
863                 BArc *connectedArc = node->arcs[i];
864                 
865                 /* only arcs not already marked as symetric */
866                 if (connectedArc->symmetry_level < 0) {
867                         int j;
868                         
869                         /* true by default */
870                         issymmetryAxis = 1;
871                         
872                         for (j = 0; j < node->degree; j++) {
873                                 BArc *otherArc = node->arcs[j];
874                                 
875                                 /* different arc, same depth */
876                                 if (otherArc != connectedArc && otherArc->symmetry_level == connectedArc->symmetry_level) {
877                                         /* not on the symmetry axis */
878                                         issymmetryAxis = 0;
879                                         break;
880                                 }
881                         }
882                 }
883                 
884                 /* arc could be on the symmetry axis */
885                 if (issymmetryAxis == 1) {
886                         /* no arc as been marked previously, keep this one */
887                         if (arc == NULL) {
888                                 arc = connectedArc;
889                         }
890                         else if (connectedArc->symmetry_level < arc->symmetry_level) {
891                                 /* go with more complex subtree as symmetry arc */
892                                 arc = connectedArc;
893                         }
894                 }
895         }
896         
897         /* go down the arc continuing the symmetry axis */
898         if (arc) {
899                 markdownSymmetryArc(graph, arc, node, level, limit);
900         }
901
902         
903         /* secondary symmetry */
904         for (i = 0; i < node->degree; i++) {
905                 BArc *connectedArc = node->arcs[i];
906                 
907                 /* only arcs not already marked as symetric and is not the next arc on the symmetry axis */
908                 if (connectedArc->symmetry_level < 0) {
909                         /* subtree depth is store as a negative value in the symmetry */
910                         markdownSecondarySymmetry(graph, node, -connectedArc->symmetry_level, level, limit);
911                 }
912         }
913 }
914
915 void BLI_markdownSymmetry(BGraph *graph, BNode *root_node, float limit)
916 {
917         BNode *node;
918         BArc *arc;
919         
920         if (root_node == NULL) {
921                 return;
922         }
923         
924         if (BLI_isGraphCyclic(graph)) {
925                 return;
926         }
927         
928         /* mark down all arcs as non-symetric */
929         BLI_flagArcs(graph, 0);
930         
931         /* mark down all nodes as not on the symmetry axis */
932         BLI_flagNodes(graph, 0);
933
934         node = root_node;
935         
936         /* sanity check REMOVE ME */
937         if (node->degree > 0) {
938                 arc = node->arcs[0];
939                 
940                 if (node->degree == 1) {
941                         markdownSymmetryArc(graph, arc, node, 1, limit);
942                 }
943                 else {
944                         markdownSymmetryArc(graph, NULL, node, 1, limit);
945                 }
946                 
947
948
949                 /* mark down non-symetric arcs */
950                 for (arc = graph->arcs.first; arc; arc = arc->next) {
951                         if (arc->symmetry_level < 0) {
952                                 arc->symmetry_level = 0;
953                         }
954                         else {
955                                 /* mark down nodes with the lowest level symmetry axis */
956                                 if (arc->head->symmetry_level == 0 || arc->head->symmetry_level > arc->symmetry_level) {
957                                         arc->head->symmetry_level = arc->symmetry_level;
958                                 }
959                                 if (arc->tail->symmetry_level == 0 || arc->tail->symmetry_level > arc->symmetry_level) {
960                                         arc->tail->symmetry_level = arc->symmetry_level;
961                                 }
962                         }
963                 }
964         }
965 }
966
967 void *IT_head(void *arg)
968 {
969         BArcIterator *iter = (BArcIterator *)arg;
970         return iter->head(iter);
971 }
972
973 void *IT_tail(void *arg)
974 {
975         BArcIterator *iter = (BArcIterator *)arg;
976         return iter->tail(iter); 
977 }
978
979 void *IT_peek(void *arg, int n)
980 {
981         BArcIterator *iter = (BArcIterator *)arg;
982         
983         if (iter->index + n < 0) {
984                 return iter->head(iter);
985         }
986         else if (iter->index + n >= iter->length) {
987                 return iter->tail(iter);
988         }
989         else {
990                 return iter->peek(iter, n);
991         }
992 }
993
994 void *IT_next(void *arg)
995 {
996         BArcIterator *iter = (BArcIterator *)arg;
997         return iter->next(iter);
998 }
999
1000 void *IT_nextN(void *arg, int n)
1001 {
1002         BArcIterator *iter = (BArcIterator *)arg;
1003         return iter->nextN(iter, n);
1004 }
1005
1006 void *IT_previous(void *arg)
1007 {
1008         BArcIterator *iter = (BArcIterator *)arg;
1009         return iter->previous(iter);
1010 }
1011
1012 int   IT_stopped(void *arg)
1013 {
1014         BArcIterator *iter = (BArcIterator *)arg;
1015         return iter->stopped(iter);
1016 }