Tomato: re-integrate ceres library with updates needed for tracking improvement
[blender.git] / intern / dualcon / intern / octree.cpp
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): Tao Ju, Nicholas Bishop
19  *
20  * ***** END GPL LICENSE BLOCK *****
21  */
22
23 #include "octree.h"
24 #include <Eigen/Dense>
25 #include <limits>
26 #include <time.h>
27
28 /**
29  * Implementations of Octree member functions.
30  *
31  * @author Tao Ju
32  */
33
34 /* set to non-zero value to enable debugging output */
35 #define DC_DEBUG 0
36
37 #if DC_DEBUG
38 /* enable debug printfs */
39 #define dc_printf printf
40 #else
41 /* disable debug printfs */
42 #define dc_printf(...) do {} while (0)
43 #endif
44
45 Octree::Octree(ModelReader *mr,
46                DualConAllocOutput alloc_output_func,
47                DualConAddVert add_vert_func,
48                DualConAddQuad add_quad_func,
49                DualConFlags flags, DualConMode dualcon_mode, int depth,
50                float threshold, float sharpness)
51         : use_flood_fill(flags & DUALCON_FLOOD_FILL),
52         /* note on `use_manifold':
53
54            After playing around with this option, the only case I could
55            find where this option gives different results is on
56            relatively thin corners. Sometimes along these corners two
57            vertices from seperate sides will be placed in the same
58            position, so hole gets filled with a 5-sided face, where two
59            of those vertices are in the same 3D location. If
60            `use_manifold' is disabled, then the modifier doesn't
61            generate two separate vertices so the results end up as all
62            quads.
63
64            Since the results are just as good with all quads, there
65            doesn't seem any reason to allow this to be toggled in
66            Blender. -nicholasbishop
67          */
68         use_manifold(false),
69         hermite_num(sharpness),
70         mode(dualcon_mode),
71         alloc_output(alloc_output_func),
72         add_vert(add_vert_func),
73         add_quad(add_quad_func)
74 {
75         thresh = threshold;
76         reader = mr;
77         dimen = 1 << GRID_DIMENSION;
78         range = reader->getBoundingBox(origin);
79         nodeCount = nodeSpace = 0;
80         maxDepth = depth;
81         mindimen = (dimen >> maxDepth);
82         minshift = (GRID_DIMENSION - maxDepth);
83         buildTable();
84
85         maxTrianglePerCell = 0;
86
87         // Initialize memory
88 #ifdef IN_VERBOSE_MODE
89         dc_printf("Range: %f origin: %f, %f,%f \n", range, origin[0], origin[1], origin[2]);
90         dc_printf("Initialize memory...\n");
91 #endif
92         initMemory();
93         root = (Node *)createInternal(0);
94
95         // Read MC table
96 #ifdef IN_VERBOSE_MODE
97         dc_printf("Reading contour table...\n");
98 #endif
99         cubes = new Cubes();
100
101 }
102
103 Octree::~Octree()
104 {
105         freeMemory();
106 }
107
108 void Octree::scanConvert()
109 {
110         // Scan triangles
111 #if DC_DEBUG
112         clock_t start, finish;
113         start = clock();
114 #endif
115
116         addTrian();
117         resetMinimalEdges();
118         preparePrimalEdgesMask(&root->internal);
119
120 #if DC_DEBUG
121         finish = clock();
122         dc_printf("Time taken: %f seconds                   \n",
123                   (double)(finish - start) / CLOCKS_PER_SEC);
124 #endif
125
126         // Generate signs
127         // Find holes
128 #if DC_DEBUG
129         dc_printf("Patching...\n");
130         start = clock();
131 #endif
132         trace();
133 #if DC_DEBUG
134         finish = clock();
135         dc_printf("Time taken: %f seconds \n",  (double)(finish - start) / CLOCKS_PER_SEC);
136 #ifdef IN_VERBOSE_MODE
137         dc_printf("Holes: %d Average Length: %f Max Length: %d \n", numRings, (float)totRingLengths / (float) numRings, maxRingLength);
138 #endif
139 #endif
140
141         // Check again
142         int tnumRings = numRings;
143         trace();
144 #ifdef IN_VERBOSE_MODE
145         dc_printf("Holes after patching: %d \n", numRings);
146 #endif
147         numRings = tnumRings;
148
149 #if DC_DEBUG
150         dc_printf("Building signs...\n");
151         start = clock();
152 #endif
153         buildSigns();
154 #if DC_DEBUG
155         finish = clock();
156         dc_printf("Time taken: %f seconds \n",  (double)(finish - start) / CLOCKS_PER_SEC);
157 #endif
158
159         if (use_flood_fill) {
160                 /*
161                    start = clock();
162                    floodFill();
163                    // Check again
164                    tnumRings = numRings;
165                    trace();
166                    dc_printf("Holes after filling: %d \n", numRings);
167                    numRings = tnumRings;
168                    buildSigns();
169                    finish = clock();
170                    dc_printf("Time taken: %f seconds \n",       (double)(finish - start) / CLOCKS_PER_SEC);
171                  */
172 #if DC_DEBUG
173                 start = clock();
174                 dc_printf("Removing components...\n");
175 #endif
176                 floodFill();
177                 buildSigns();
178                 //      dc_printf("Checking...\n");
179                 //      floodFill();
180 #if DC_DEBUG
181                 finish = clock();
182                 dc_printf("Time taken: %f seconds \n", (double)(finish - start) / CLOCKS_PER_SEC);
183 #endif
184         }
185
186         // Output
187 #if DC_DEBUG
188         start = clock();
189 #endif
190         writeOut();
191 #if DC_DEBUG
192         finish = clock();
193 #endif
194         // dc_printf("Time taken: %f seconds \n",       (double)(finish - start) / CLOCKS_PER_SEC);
195
196         // Print info
197 #ifdef IN_VERBOSE_MODE
198         printMemUsage();
199 #endif
200 }
201
202 void Octree::initMemory()
203 {
204         leafalloc[0] = new MemoryAllocator<sizeof(LeafNode)>();
205         leafalloc[1] = new MemoryAllocator<sizeof(LeafNode) + sizeof(float) *EDGE_FLOATS>();
206         leafalloc[2] = new MemoryAllocator<sizeof(LeafNode) + sizeof(float) *EDGE_FLOATS * 2>();
207         leafalloc[3] = new MemoryAllocator<sizeof(LeafNode) + sizeof(float) *EDGE_FLOATS * 3>();
208
209         alloc[0] = new MemoryAllocator<sizeof(InternalNode)>();
210         alloc[1] = new MemoryAllocator<sizeof(InternalNode) + sizeof(Node *)>();
211         alloc[2] = new MemoryAllocator<sizeof(InternalNode) + sizeof(Node *) * 2>();
212         alloc[3] = new MemoryAllocator<sizeof(InternalNode) + sizeof(Node *) * 3>();
213         alloc[4] = new MemoryAllocator<sizeof(InternalNode) + sizeof(Node *) * 4>();
214         alloc[5] = new MemoryAllocator<sizeof(InternalNode) + sizeof(Node *) * 5>();
215         alloc[6] = new MemoryAllocator<sizeof(InternalNode) + sizeof(Node *) * 6>();
216         alloc[7] = new MemoryAllocator<sizeof(InternalNode) + sizeof(Node *) * 7>();
217         alloc[8] = new MemoryAllocator<sizeof(InternalNode) + sizeof(Node *) * 8>();
218 }
219
220 void Octree::freeMemory()
221 {
222         for (int i = 0; i < 9; i++) {
223                 alloc[i]->destroy();
224                 delete alloc[i];
225         }
226
227         for (int i = 0; i < 4; i++) {
228                 leafalloc[i]->destroy();
229                 delete leafalloc[i];
230         }
231 }
232
233 void Octree::printMemUsage()
234 {
235         int totalbytes = 0;
236         dc_printf("********* Internal nodes: \n");
237         for (int i = 0; i < 9; i++) {
238                 alloc[i]->printInfo();
239
240                 totalbytes += alloc[i]->getAll() * alloc[i]->getBytes();
241         }
242         dc_printf("********* Leaf nodes: \n");
243         int totalLeafs = 0;
244         for (int i = 0; i < 4; i++) {
245                 leafalloc[i]->printInfo();
246
247                 totalbytes += leafalloc[i]->getAll() * leafalloc[i]->getBytes();
248                 totalLeafs += leafalloc[i]->getAllocated();
249         }
250
251         dc_printf("Total allocated bytes on disk: %d \n", totalbytes);
252         dc_printf("Total leaf nodes: %d\n", totalLeafs);
253 }
254
255 void Octree::resetMinimalEdges()
256 {
257         cellProcParity(root, 0, maxDepth);
258 }
259
260 void Octree::addTrian()
261 {
262         Triangle *trian;
263         int count = 0;
264
265 #if DC_DEBUG
266         int total = reader->getNumTriangles();
267         int unitcount = 1000;
268         dc_printf("\nScan converting to depth %d...\n", maxDepth);
269 #endif
270
271         srand(0);
272
273         while ((trian = reader->getNextTriangle()) != NULL) {
274                 // Drop triangles
275                 {
276                         addTrian(trian, count);
277                 }
278                 delete trian;
279
280                 count++;
281
282 #if DC_DEBUG
283                 if (count % unitcount == 0) {
284                         putchar(13);
285
286                         switch ((count / unitcount) % 4) {
287                                 case 0: dc_printf("-");
288                                         break;
289                                 case 1: dc_printf("/");
290                                         break;
291                                 case 2: dc_printf("|");
292                                         break;
293                                 case 3: dc_printf("\\");
294                                         break;
295                         }
296
297                         float percent = (float) count / total;
298
299                         /*
300                            int totbars = 50;
301                            int bars =(int)(percent * totbars);
302                            for(int i = 0; i < bars; i ++) {
303                             putchar(219);
304                            }
305                            for(i = bars; i < totbars; i ++) {
306                             putchar(176);
307                            }
308                          */
309
310                         dc_printf(" %d triangles: ", count);
311                         dc_printf(" %f%% complete.", 100 * percent);
312                 }
313 #endif
314
315         }
316         putchar(13);
317 }
318
319 void Octree::addTrian(Triangle *trian, int triind)
320 {
321         int i, j;
322
323         // Blowing up the triangle to the grid
324         float mid[3] = {0, 0, 0};
325         for (i = 0; i < 3; i++)
326                 for (j = 0; j < 3; j++) {
327                         trian->vt[i][j] = dimen * (trian->vt[i][j] - origin[j]) / range;
328                         mid[j] += trian->vt[i][j] / 3;
329                 }
330
331         // Generate projections
332         LONG cube[2][3] = {{0, 0, 0}, {dimen, dimen, dimen}};
333         LONG trig[3][3];
334
335         for (i = 0; i < 3; i++)
336                 for (j = 0; j < 3; j++) {
337                         trig[i][j] = (LONG)(trian->vt[i][j]);
338                         // Perturb end points, if set so
339                 }
340
341         // Add to the octree
342         // int start[3] = {0, 0, 0};
343         LONG errorvec = (LONG)(0);
344         Projections *proj = new Projections(cube, trig, errorvec, triind);
345         root = (Node *)addTrian(&root->internal, proj, maxDepth);
346
347         delete proj->inherit;
348         delete proj;
349 }
350
351
352 InternalNode *Octree::addTrian(InternalNode *node, Projections *p, int height)
353 {
354         int i;
355         int vertdiff[8][3] = {{0, 0, 0}, {0, 0, 1}, {0, 1, -1}, {0, 0, 1}, {1, -1, -1}, {0, 0, 1}, {0, 1, -1}, {0, 0, 1}};
356         UCHAR boxmask = p->getBoxMask();
357         Projections *subp = new Projections(p);
358
359         int count = 0;
360         int tempdiff[3] = {0, 0, 0};
361         for (i = 0; i < 8; i++) {
362                 tempdiff[0] += vertdiff[i][0];
363                 tempdiff[1] += vertdiff[i][1];
364                 tempdiff[2] += vertdiff[i][2];
365
366                 /* Quick pruning using bounding box */
367                 if (boxmask & (1 << i)) {
368                         subp->shift(tempdiff);
369                         tempdiff[0] = tempdiff[1] = tempdiff[2] = 0;
370
371                         /* Pruning using intersection test */
372                         if (subp->isIntersecting()) {
373                         // if(subp->getIntersectionMasks(cedgemask, edgemask))
374                                 if (!hasChild(node, i)) {
375                                         if (height == 1) {
376                                                 node = addLeafChild(node, i, count, createLeaf(0));
377                                         }
378                                         else {
379                                                 node = addInternalChild(node, i, count, createInternal(0));
380                                         }
381                                 }
382                                 Node *chd = getChild(node, count);
383
384                                 if (!isLeaf(node, i)) {
385                                         // setChild(node, count, addTrian(chd, subp, height - 1, vertmask[i], edgemask));
386                                         setChild(node, count, (Node *)addTrian(&chd->internal, subp, height - 1));
387                                 }
388                                 else {
389                                         setChild(node, count, (Node *)updateCell(&chd->leaf, subp));
390                                 }
391                         }
392                 }
393
394                 if (hasChild(node, i)) {
395                         count++;
396                 }
397         }
398
399         delete subp;
400
401         return node;
402 }
403
404 LeafNode *Octree::updateCell(LeafNode *node, Projections *p)
405 {
406         int i;
407
408         // Edge connectivity
409         int mask[3] = {0, 4, 8  };
410         int oldc = 0, newc = 0;
411         float offs[3];
412         float a[3], b[3], c[3];
413
414         for (i = 0; i < 3; i++) {
415                 if (!getEdgeParity(node, mask[i])) {
416                         if (p->isIntersectingPrimary(i)) {
417                                 // actualQuads ++;
418                                 setEdge(node, mask[i]);
419                                 offs[newc] = p->getIntersectionPrimary(i);
420                                 a[newc] = (float) p->inherit->norm[0];
421                                 b[newc] = (float) p->inherit->norm[1];
422                                 c[newc] = (float) p->inherit->norm[2];
423                                 newc++;
424                         }
425                 }
426                 else {
427                         offs[newc] = getEdgeOffsetNormal(node, oldc, a[newc], b[newc], c[newc]);
428
429 //                      if(p->isIntersectingPrimary(i))
430                         {
431                                 // dc_printf("Multiple intersections!\n");
432
433 //                              setPatchEdge(node, i);
434                         }
435
436                         oldc++;
437                         newc++;
438                 }
439         }
440
441         if (newc > oldc) {
442                 // New offsets added, update this node
443                 node = updateEdgeOffsetsNormals(node, oldc, newc, offs, a, b, c);
444         }
445
446         return node;
447 }
448
449 void Octree::preparePrimalEdgesMask(InternalNode *node)
450 {
451         int count = 0;
452         for (int i = 0; i < 8; i++) {
453                 if (hasChild(node, i)) {
454                         if (isLeaf(node, i))
455                                 createPrimalEdgesMask(&getChild(node, count)->leaf);
456                         else
457                                 preparePrimalEdgesMask(&getChild(node, count)->internal);
458
459                         count++;
460                 }
461         }
462 }
463
464 void Octree::trace()
465 {
466         int st[3] = {0, 0, 0, };
467         numRings = 0;
468         totRingLengths = 0;
469         maxRingLength = 0;
470
471         PathList *chdpath = NULL;
472         root = trace(root, st, dimen, maxDepth, chdpath);
473
474         if (chdpath != NULL) {
475                 dc_printf("there are incomplete rings.\n");
476                 printPaths(chdpath);
477         };
478 }
479
480 Node *Octree::trace(Node *newnode, int *st, int len, int depth, PathList *& paths)
481 {
482         len >>= 1;
483         PathList *chdpaths[8];
484         Node *chd[8];
485         int nst[8][3];
486         int i, j;
487
488         // Get children paths
489         int chdleaf[8];
490         fillChildren(&newnode->internal, chd, chdleaf);
491
492         // int count = 0;
493         for (i = 0; i < 8; i++) {
494                 for (j = 0; j < 3; j++) {
495                         nst[i][j] = st[j] + len * vertmap[i][j];
496                 }
497
498                 if (chd[i] == NULL || isLeaf(&newnode->internal, i)) {
499                         chdpaths[i] = NULL;
500                 }
501                 else {
502                         trace(chd[i], nst[i], len, depth - 1, chdpaths[i]);
503                 }
504         }
505
506         // Get connectors on the faces
507         PathList *conn[12];
508         Node *nf[2];
509         int lf[2];
510         int df[2] = {depth - 1, depth - 1};
511         int *nstf[2];
512
513         fillChildren(&newnode->internal, chd, chdleaf);
514
515         for (i = 0; i < 12; i++) {
516                 int c[2] = {cellProcFaceMask[i][0], cellProcFaceMask[i][1]};
517
518                 for (int j = 0; j < 2; j++) {
519                         lf[j] = chdleaf[c[j]];
520                         nf[j] = chd[c[j]];
521                         nstf[j] = nst[c[j]];
522                 }
523
524                 conn[i] = NULL;
525
526                 findPaths((Node **)nf, lf, df, nstf, depth - 1, cellProcFaceMask[i][2], conn[i]);
527
528                 //if(conn[i]) {
529                 //              printPath(conn[i]);
530                 //}
531         }
532
533         // Connect paths
534         PathList *rings = NULL;
535         combinePaths(chdpaths[0], chdpaths[1], conn[8], rings);
536         combinePaths(chdpaths[2], chdpaths[3], conn[9], rings);
537         combinePaths(chdpaths[4], chdpaths[5], conn[10], rings);
538         combinePaths(chdpaths[6], chdpaths[7], conn[11], rings);
539
540         combinePaths(chdpaths[0], chdpaths[2], conn[4], rings);
541         combinePaths(chdpaths[4], chdpaths[6], conn[5], rings);
542         combinePaths(chdpaths[0], NULL, conn[6], rings);
543         combinePaths(chdpaths[4], NULL, conn[7], rings);
544
545         combinePaths(chdpaths[0], chdpaths[4], conn[0], rings);
546         combinePaths(chdpaths[0], NULL, conn[1], rings);
547         combinePaths(chdpaths[0], NULL, conn[2], rings);
548         combinePaths(chdpaths[0], NULL, conn[3], rings);
549
550         // By now, only chdpaths[0] and rings have contents
551
552         // Process rings
553         if (rings) {
554                 // printPath(rings);
555
556                 /* Let's count first */
557                 PathList *trings = rings;
558                 while (trings) {
559                         numRings++;
560                         totRingLengths += trings->length;
561                         if (trings->length > maxRingLength) {
562                                 maxRingLength = trings->length;
563                         }
564                         trings = trings->next;
565                 }
566
567                 // printPath(rings);
568                 newnode = patch(newnode, st, (len << 1), rings);
569         }
570
571         // Return incomplete paths
572         paths = chdpaths[0];
573         return newnode;
574 }
575
576 void Octree::findPaths(Node *node[2], int leaf[2], int depth[2], int *st[2], int maxdep, int dir, PathList *& paths)
577 {
578         if (!(node[0] && node[1])) {
579                 return;
580         }
581
582         if (!(leaf[0] && leaf[1])) {
583                 // Not at the bottom, recur
584
585                 // Fill children nodes
586                 int i, j;
587                 Node *chd[2][8];
588                 int chdleaf[2][8];
589                 int nst[2][8][3];
590
591                 for (j = 0; j < 2; j++) {
592                         if (!leaf[j]) {
593                                 fillChildren(&node[j]->internal, chd[j], chdleaf[j]);
594
595                                 int len = (dimen >> (maxDepth - depth[j] + 1));
596                                 for (i = 0; i < 8; i++) {
597                                         for (int k = 0; k < 3; k++) {
598                                                 nst[j][i][k] = st[j][k] + len * vertmap[i][k];
599                                         }
600                                 }
601
602                         }
603                 }
604
605                 // 4 face calls
606                 Node *nf[2];
607                 int df[2];
608                 int lf[2];
609                 int *nstf[2];
610                 for (i = 0; i < 4; i++) {
611                         int c[2] = {faceProcFaceMask[dir][i][0], faceProcFaceMask[dir][i][1]};
612                         for (int j = 0; j < 2; j++) {
613                                 if (leaf[j]) {
614                                         lf[j] = leaf[j];
615                                         nf[j] = node[j];
616                                         df[j] = depth[j];
617                                         nstf[j] = st[j];
618                                 }
619                                 else {
620                                         lf[j] = chdleaf[j][c[j]];
621                                         nf[j] = chd[j][c[j]];
622                                         df[j] = depth[j] - 1;
623                                         nstf[j] = nst[j][c[j]];
624                                 }
625                         }
626                         findPaths(nf, lf, df, nstf, maxdep - 1, faceProcFaceMask[dir][i][2], paths);
627                 }
628
629         }
630         else {
631                 // At the bottom, check this face
632                 int ind = (depth[0] == maxdep ? 0 : 1);
633                 int fcind = 2 * dir + (1 - ind);
634                 if (getFaceParity((LeafNode *)node[ind], fcind)) {
635                         // Add into path
636                         PathElement *ele1 = new PathElement;
637                         PathElement *ele2 = new PathElement;
638
639                         ele1->pos[0] = st[0][0];
640                         ele1->pos[1] = st[0][1];
641                         ele1->pos[2] = st[0][2];
642
643                         ele2->pos[0] = st[1][0];
644                         ele2->pos[1] = st[1][1];
645                         ele2->pos[2] = st[1][2];
646
647                         ele1->next = ele2;
648                         ele2->next = NULL;
649
650                         PathList *lst = new PathList;
651                         lst->head = ele1;
652                         lst->tail = ele2;
653                         lst->length = 2;
654                         lst->next = paths;
655                         paths = lst;
656
657                         // int l =(dimen >> maxDepth);
658                 }
659         }
660
661 }
662
663 void Octree::combinePaths(PathList *& list1, PathList *list2, PathList *paths, PathList *& rings)
664 {
665         // Make new list of paths
666         PathList *nlist = NULL;
667
668         // Search for each connectors in paths
669         PathList *tpaths = paths;
670         PathList *tlist, *pre;
671         while (tpaths) {
672                 PathList *singlist = tpaths;
673                 PathList *templist;
674                 tpaths = tpaths->next;
675                 singlist->next = NULL;
676
677                 // Look for hookup in list1
678                 tlist = list1;
679                 pre = NULL;
680                 while (tlist) {
681                         if ((templist = combineSinglePath(list1, pre, tlist, singlist, NULL, singlist)) != NULL) {
682                                 singlist = templist;
683                                 continue;
684                         }
685                         pre = tlist;
686                         tlist = tlist->next;
687                 }
688
689                 // Look for hookup in list2
690                 tlist = list2;
691                 pre = NULL;
692                 while (tlist) {
693                         if ((templist = combineSinglePath(list2, pre, tlist, singlist, NULL, singlist)) != NULL) {
694                                 singlist = templist;
695                                 continue;
696                         }
697                         pre = tlist;
698                         tlist = tlist->next;
699                 }
700
701                 // Look for hookup in nlist
702                 tlist = nlist;
703                 pre = NULL;
704                 while (tlist) {
705                         if ((templist = combineSinglePath(nlist, pre, tlist, singlist, NULL, singlist)) != NULL) {
706                                 singlist = templist;
707                                 continue;
708                         }
709                         pre = tlist;
710                         tlist = tlist->next;
711                 }
712
713                 // Add to nlist or rings
714                 if (isEqual(singlist->head, singlist->tail)) {
715                         PathElement *temp = singlist->head;
716                         singlist->head = temp->next;
717                         delete temp;
718                         singlist->length--;
719                         singlist->tail->next = singlist->head;
720
721                         singlist->next = rings;
722                         rings = singlist;
723                 }
724                 else {
725                         singlist->next = nlist;
726                         nlist = singlist;
727                 }
728
729         }
730
731         // Append list2 and nlist to the end of list1
732         tlist = list1;
733         if (tlist != NULL) {
734                 while (tlist->next != NULL) {
735                         tlist = tlist->next;
736                 }
737                 tlist->next = list2;
738         }
739         else {
740                 tlist = list2;
741                 list1 = list2;
742         }
743
744         if (tlist != NULL) {
745                 while (tlist->next != NULL) {
746                         tlist = tlist->next;
747                 }
748                 tlist->next = nlist;
749         }
750         else {
751                 tlist = nlist;
752                 list1 = nlist;
753         }
754
755 }
756
757 PathList *Octree::combineSinglePath(PathList *& head1, PathList *pre1, PathList *& list1, PathList *& head2, PathList *pre2, PathList *& list2)
758 {
759         if (isEqual(list1->head, list2->head) || isEqual(list1->tail, list2->tail)) {
760                 // Reverse the list
761                 if (list1->length < list2->length) {
762                         // Reverse list1
763                         PathElement *prev = list1->head;
764                         PathElement *next = prev->next;
765                         prev->next = NULL;
766                         while (next != NULL) {
767                                 PathElement *tnext = next->next;
768                                 next->next = prev;
769
770                                 prev = next;
771                                 next = tnext;
772                         }
773
774                         list1->tail = list1->head;
775                         list1->head = prev;
776                 }
777                 else {
778                         // Reverse list2
779                         PathElement *prev = list2->head;
780                         PathElement *next = prev->next;
781                         prev->next = NULL;
782                         while (next != NULL) {
783                                 PathElement *tnext = next->next;
784                                 next->next = prev;
785
786                                 prev = next;
787                                 next = tnext;
788                         }
789
790                         list2->tail = list2->head;
791                         list2->head = prev;
792                 }
793         }
794
795         if (isEqual(list1->head, list2->tail)) {
796
797                 // Easy case
798                 PathElement *temp = list1->head->next;
799                 delete list1->head;
800                 list2->tail->next = temp;
801
802                 PathList *nlist = new PathList;
803                 nlist->length = list1->length + list2->length - 1;
804                 nlist->head = list2->head;
805                 nlist->tail = list1->tail;
806                 nlist->next = NULL;
807
808                 deletePath(head1, pre1, list1);
809                 deletePath(head2, pre2, list2);
810
811                 return nlist;
812         }
813         else if (isEqual(list1->tail, list2->head)) {
814                 // Easy case
815                 PathElement *temp = list2->head->next;
816                 delete list2->head;
817                 list1->tail->next = temp;
818
819                 PathList *nlist = new PathList;
820                 nlist->length = list1->length + list2->length - 1;
821                 nlist->head = list1->head;
822                 nlist->tail = list2->tail;
823                 nlist->next = NULL;
824
825                 deletePath(head1, pre1, list1);
826                 deletePath(head2, pre2, list2);
827
828                 return nlist;
829         }
830
831         return NULL;
832 }
833
834 void Octree::deletePath(PathList *& head, PathList *pre, PathList *& curr)
835 {
836         PathList *temp = curr;
837         curr = temp->next;
838         delete temp;
839
840         if (pre == NULL) {
841                 head = curr;
842         }
843         else {
844                 pre->next = curr;
845         }
846 }
847
848 void Octree::printElement(PathElement *ele)
849 {
850         if (ele != NULL) {
851                 dc_printf("(%d %d %d)", ele->pos[0], ele->pos[1], ele->pos[2]);
852         }
853 }
854
855 void Octree::printPath(PathList *path)
856 {
857         PathElement *n = path->head;
858         int same = 0;
859
860 #if DC_DEBUG
861         int len = (dimen >> maxDepth);
862 #endif
863         while (n && (same == 0 || n != path->head)) {
864                 same++;
865                 dc_printf("(%d %d %d)", n->pos[0] / len, n->pos[1] / len, n->pos[2] / len);
866                 n = n->next;
867         }
868
869         if (n == path->head) {
870                 dc_printf(" Ring!\n");
871         }
872         else {
873                 dc_printf(" %p end!\n", n);
874         }
875 }
876
877 void Octree::printPath(PathElement *path)
878 {
879         PathElement *n = path;
880         int same = 0;
881 #if DC_DEBUG
882         int len = (dimen >> maxDepth);
883 #endif
884         while (n && (same == 0 || n != path)) {
885                 same++;
886                 dc_printf("(%d %d %d)", n->pos[0] / len, n->pos[1] / len, n->pos[2] / len);
887                 n = n->next;
888         }
889
890         if (n == path) {
891                 dc_printf(" Ring!\n");
892         }
893         else {
894                 dc_printf(" %p end!\n", n);
895         }
896
897 }
898
899
900 void Octree::printPaths(PathList *path)
901 {
902         PathList *iter = path;
903         int i = 0;
904         while (iter != NULL) {
905                 dc_printf("Path %d:\n", i);
906                 printPath(iter);
907                 iter = iter->next;
908                 i++;
909         }
910 }
911
912 Node *Octree::patch(Node *newnode, int st[3], int len, PathList *rings)
913 {
914 #ifdef IN_DEBUG_MODE
915         dc_printf("Call to PATCH with rings: \n");
916         printPaths(rings);
917 #endif
918
919         /* Do nothing but couting
920            PathList* tlist = rings;
921            PathList* ttlist;
922            PathElement* telem, * ttelem;
923            while(tlist!= NULL) {
924             // printPath(tlist);
925             numRings ++;
926             totRingLengths += tlist->length;
927             if(tlist->length > maxRingLength) {
928                 maxRingLength = tlist->length;
929             }
930             ttlist = tlist;
931             tlist = tlist->next;
932            }
933            return node;
934          */
935
936
937         /* Pass onto separate calls in each direction */
938         if (len == mindimen) {
939                 dc_printf("Error! should have no list by now.\n");
940                 exit(0);
941         }
942
943         // YZ plane
944         PathList *xlists[2];
945         newnode = patchSplit(newnode, st, len, rings, 0, xlists[0], xlists[1]);
946
947         // XZ plane
948         PathList *ylists[4];
949         newnode = patchSplit(newnode, st, len, xlists[0], 1, ylists[0], ylists[1]);
950         newnode = patchSplit(newnode, st, len, xlists[1], 1, ylists[2], ylists[3]);
951
952         // XY plane
953         PathList *zlists[8];
954         newnode = patchSplit(newnode, st, len, ylists[0], 2, zlists[0], zlists[1]);
955         newnode = patchSplit(newnode, st, len, ylists[1], 2, zlists[2], zlists[3]);
956         newnode = patchSplit(newnode, st, len, ylists[2], 2, zlists[4], zlists[5]);
957         newnode = patchSplit(newnode, st, len, ylists[3], 2, zlists[6], zlists[7]);
958
959         // Recur
960         len >>= 1;
961         int count = 0;
962         for (int i = 0; i < 8; i++) {
963                 if (zlists[i] != NULL) {
964                         int nori[3] = {
965                                 st[0] + len * vertmap[i][0],
966                                 st[1] + len * vertmap[i][1],
967                                 st[2] + len * vertmap[i][2]
968                         };
969                         patch(getChild(&newnode->internal, count), nori, len, zlists[i]);
970                 }
971
972                 if (hasChild(&newnode->internal, i)) {
973                         count++;
974                 }
975         }
976 #ifdef IN_DEBUG_MODE
977         dc_printf("Return from PATCH\n");
978 #endif
979         return newnode;
980
981 }
982
983
984 Node *Octree::patchSplit(Node *newnode, int st[3], int len, PathList *rings,
985                          int dir, PathList *& nrings1, PathList *& nrings2)
986 {
987 #ifdef IN_DEBUG_MODE
988         dc_printf("Call to PATCHSPLIT with direction %d and rings: \n", dir);
989         printPaths(rings);
990 #endif
991
992         nrings1 = NULL;
993         nrings2 = NULL;
994         PathList *tmp;
995         while (rings != NULL) {
996                 // Process this ring
997                 newnode = patchSplitSingle(newnode, st, len, rings->head, dir, nrings1, nrings2);
998
999                 // Delete this ring from the group
1000                 tmp = rings;
1001                 rings = rings->next;
1002                 delete tmp;
1003         }
1004
1005 #ifdef IN_DEBUG_MODE
1006         dc_printf("Return from PATCHSPLIT with \n");
1007         dc_printf("Rings gourp 1:\n");
1008         printPaths(nrings1);
1009         dc_printf("Rings group 2:\n");
1010         printPaths(nrings2);
1011 #endif
1012
1013         return newnode;
1014 }
1015
1016 Node *Octree::patchSplitSingle(Node *newnode, int st[3], int len, PathElement *head, int dir, PathList *& nrings1, PathList *& nrings2)
1017 {
1018 #ifdef IN_DEBUG_MODE
1019         dc_printf("Call to PATCHSPLITSINGLE with direction %d and path: \n", dir);
1020         printPath(head);
1021 #endif
1022
1023         if (head == NULL) {
1024 #ifdef IN_DEBUG_MODE
1025                 dc_printf("Return from PATCHSPLITSINGLE with head==NULL.\n");
1026 #endif
1027                 return newnode;
1028         }
1029         else {
1030                 // printPath(head);
1031         }
1032
1033         // Walk along the ring to find pair of intersections
1034         PathElement *pre1 = NULL;
1035         PathElement *pre2 = NULL;
1036         int side = findPair(head, st[dir] + len / 2, dir, pre1, pre2);
1037
1038         /*
1039            if(pre1 == pre2) {
1040             int edgelen =(dimen >> maxDepth);
1041             dc_printf("Location: %d %d %d Direction: %d Reso: %d\n", st[0]/edgelen, st[1]/edgelen, st[2]/edgelen, dir, len/edgelen);
1042             printPath(head);
1043             exit(0);
1044            }
1045          */
1046
1047         if (side) {
1048                 // Entirely on one side
1049                 PathList *nring = new PathList();
1050                 nring->head = head;
1051
1052                 if (side == -1) {
1053                         nring->next = nrings1;
1054                         nrings1 = nring;
1055                 }
1056                 else {
1057                         nring->next = nrings2;
1058                         nrings2 = nring;
1059                 }
1060         }
1061         else {
1062                 // Break into two parts
1063                 PathElement *nxt1 = pre1->next;
1064                 PathElement *nxt2 = pre2->next;
1065                 pre1->next = nxt2;
1066                 pre2->next = nxt1;
1067
1068                 newnode = connectFace(newnode, st, len, dir, pre1, pre2);
1069
1070                 if (isEqual(pre1, pre1->next)) {
1071                         if (pre1 == pre1->next) {
1072                                 delete pre1;
1073                                 pre1 = NULL;
1074                         }
1075                         else {
1076                                 PathElement *temp = pre1->next;
1077                                 pre1->next = temp->next;
1078                                 delete temp;
1079                         }
1080                 }
1081                 if (isEqual(pre2, pre2->next)) {
1082                         if (pre2 == pre2->next) {
1083                                 delete pre2;
1084                                 pre2 = NULL;
1085                         }
1086                         else {
1087                                 PathElement *temp = pre2->next;
1088                                 pre2->next = temp->next;
1089                                 delete temp;
1090                         }
1091                 }
1092
1093                 compressRing(pre1);
1094                 compressRing(pre2);
1095
1096                 // Recur
1097                 newnode = patchSplitSingle(newnode, st, len, pre1, dir, nrings1, nrings2);
1098                 newnode = patchSplitSingle(newnode, st, len, pre2, dir, nrings1, nrings2);
1099
1100         }
1101
1102 #ifdef IN_DEBUG_MODE
1103         dc_printf("Return from PATCHSPLITSINGLE with \n");
1104         dc_printf("Rings gourp 1:\n");
1105         printPaths(nrings1);
1106         dc_printf("Rings group 2:\n");
1107         printPaths(nrings2);
1108 #endif
1109
1110         return newnode;
1111 }
1112
1113 Node *Octree::connectFace(Node *newnode, int st[3], int len, int dir,
1114                           PathElement *f1, PathElement *f2)
1115 {
1116 #ifdef IN_DEBUG_MODE
1117         dc_printf("Call to CONNECTFACE with direction %d and length %d path: \n", dir, len);
1118         dc_printf("Path(low side): \n");
1119         printPath(f1);
1120 //      checkPath(f1);
1121         dc_printf("Path(high side): \n");
1122         printPath(f2);
1123 //      checkPath(f2);
1124 #endif
1125
1126         // Setup 2D
1127         int pos = st[dir] + len / 2;
1128         int xdir = (dir + 1) % 3;
1129         int ydir = (dir + 2) % 3;
1130
1131         // Use existing intersections on f1 and f2
1132         int x1, y1, x2, y2;
1133         float p1, q1, p2, q2;
1134
1135         getFacePoint(f2->next, dir, x1, y1, p1, q1);
1136         getFacePoint(f2, dir, x2, y2, p2, q2);
1137
1138         float dx = x2 + p2 - x1 - p1;
1139         float dy = y2 + q2 - y1 - q1;
1140
1141         // Do adapted Bresenham line drawing
1142         float rx = p1, ry = q1;
1143         int incx = 1, incy = 1;
1144         int lx = x1, ly = y1;
1145         int hx = x2, hy = y2;
1146         int choice;
1147         if (x2 < x1) {
1148                 incx = -1;
1149                 rx = 1 - rx;
1150                 lx = x2;
1151                 hx = x1;
1152         }
1153         if (y2 < y1) {
1154                 incy = -1;
1155                 ry = 1 - ry;
1156                 ly = y2;
1157                 hy = y1;
1158         }
1159
1160         float sx = dx * incx;
1161         float sy = dy * incy;
1162
1163         int ori[3];
1164         ori[dir] = pos / mindimen;
1165         ori[xdir] = x1;
1166         ori[ydir] = y1;
1167         int walkdir;
1168         int inc;
1169         float alpha;
1170
1171         PathElement *curEleN = f1;
1172         PathElement *curEleP = f2->next;
1173         Node *nodeN = NULL, *nodeP = NULL;
1174         LeafNode *curN = locateLeaf(&newnode->internal, len, f1->pos);
1175         LeafNode *curP = locateLeaf(&newnode->internal, len, f2->next->pos);
1176         if (curN == NULL || curP == NULL) {
1177                 exit(0);
1178         }
1179         int stN[3], stP[3];
1180         int lenN, lenP;
1181
1182         /* Unused code, leaving for posterity
1183
1184            float stpt[3], edpt[3];
1185            stpt[dir] = edpt[dir] =(float) pos;
1186            stpt[xdir] =(x1 + p1) * mindimen;
1187            stpt[ydir] =(y1 + q1) * mindimen;
1188            edpt[xdir] =(x2 + p2) * mindimen;
1189            edpt[ydir] =(y2 + q2) * mindimen;
1190          */
1191         while (ori[xdir] != x2 || ori[ydir] != y2) {
1192                 int next;
1193                 if (sy * (1 - rx) > sx * (1 - ry)) {
1194                         choice = 1;
1195                         next = ori[ydir] + incy;
1196                         if (next < ly || next > hy) {
1197                                 choice = 4;
1198                                 next = ori[xdir] + incx;
1199                         }
1200                 }
1201                 else {
1202                         choice = 2;
1203                         next = ori[xdir] + incx;
1204                         if (next < lx || next > hx) {
1205                                 choice = 3;
1206                                 next = ori[ydir] + incy;
1207                         }
1208                 }
1209
1210                 if (choice & 1) {
1211                         ori[ydir] = next;
1212                         if (choice == 1) {
1213                                 rx += (sy == 0 ? 0 : (1 - ry) * sx / sy);
1214                                 ry = 0;
1215                         }
1216
1217                         walkdir = 2;
1218                         inc = incy;
1219                         alpha = x2 < x1 ? 1 - rx : rx;
1220                 }
1221                 else {
1222                         ori[xdir] = next;
1223                         if (choice == 2) {
1224                                 ry += (sx == 0 ? 0 : (1 - rx) * sy / sx);
1225                                 rx = 0;
1226                         }
1227
1228                         walkdir = 1;
1229                         inc = incx;
1230                         alpha = y2 < y1 ? 1 - ry : ry;
1231                 }
1232
1233
1234
1235                 // Get the exact location of the marcher
1236                 int nori[3] = {ori[0] * mindimen, ori[1] * mindimen, ori[2] * mindimen};
1237                 float spt[3] = {(float) nori[0], (float) nori[1], (float) nori[2]};
1238                 spt[(dir + (3 - walkdir)) % 3] += alpha * mindimen;
1239                 if (inc < 0) {
1240                         spt[(dir + walkdir) % 3] += mindimen;
1241                 }
1242
1243                 // dc_printf("new x,y: %d %d\n", ori[xdir] / edgelen, ori[ydir] / edgelen);
1244                 // dc_printf("nori: %d %d %d alpha: %f walkdir: %d\n", nori[0], nori[1], nori[2], alpha, walkdir);
1245                 // dc_printf("%f %f %f\n", spt[0], spt[1], spt[2]);
1246
1247                 // Locate the current cells on both sides
1248                 newnode = locateCell(&newnode->internal, st, len, nori, dir, 1, nodeN, stN, lenN);
1249                 newnode = locateCell(&newnode->internal, st, len, nori, dir, 0, nodeP, stP, lenP);
1250
1251                 updateParent(&newnode->internal, len, st);
1252
1253                 int flag = 0;
1254                 // Add the cells to the rings and fill in the patch
1255                 PathElement *newEleN;
1256                 if (curEleN->pos[0] != stN[0] || curEleN->pos[1] != stN[1] || curEleN->pos[2] != stN[2]) {
1257                         if (curEleN->next->pos[0] != stN[0] || curEleN->next->pos[1] != stN[1] || curEleN->next->pos[2] != stN[2]) {
1258                                 newEleN = new PathElement;
1259                                 newEleN->next = curEleN->next;
1260                                 newEleN->pos[0] = stN[0];
1261                                 newEleN->pos[1] = stN[1];
1262                                 newEleN->pos[2] = stN[2];
1263
1264                                 curEleN->next = newEleN;
1265                         }
1266                         else {
1267                                 newEleN = curEleN->next;
1268                         }
1269                         curN = patchAdjacent(&newnode->internal, len, curEleN->pos, curN,
1270                                              newEleN->pos, (LeafNode *)nodeN, walkdir,
1271                                              inc, dir, 1, alpha);
1272
1273                         curEleN = newEleN;
1274                         flag++;
1275                 }
1276
1277                 PathElement *newEleP;
1278                 if (curEleP->pos[0] != stP[0] || curEleP->pos[1] != stP[1] || curEleP->pos[2] != stP[2]) {
1279                         if (f2->pos[0] != stP[0] || f2->pos[1] != stP[1] || f2->pos[2] != stP[2]) {
1280                                 newEleP = new PathElement;
1281                                 newEleP->next = curEleP;
1282                                 newEleP->pos[0] = stP[0];
1283                                 newEleP->pos[1] = stP[1];
1284                                 newEleP->pos[2] = stP[2];
1285
1286                                 f2->next = newEleP;
1287                         }
1288                         else {
1289                                 newEleP = f2;
1290                         }
1291                         curP = patchAdjacent(&newnode->internal, len, curEleP->pos, curP,
1292                                              newEleP->pos, (LeafNode *)nodeP, walkdir,
1293                                              inc, dir, 0, alpha);
1294
1295
1296
1297                         curEleP = newEleP;
1298                         flag++;
1299                 }
1300
1301
1302                 /*
1303                    if(flag == 0) {
1304                     dc_printf("error: non-synchronized patching! at \n");
1305                    }
1306                  */
1307         }
1308
1309 #ifdef IN_DEBUG_MODE
1310         dc_printf("Return from CONNECTFACE with \n");
1311         dc_printf("Path(low side):\n");
1312         printPath(f1);
1313         checkPath(f1);
1314         dc_printf("Path(high side):\n");
1315         printPath(f2);
1316         checkPath(f2);
1317 #endif
1318
1319
1320         return newnode;
1321 }
1322
1323 LeafNode *Octree::patchAdjacent(InternalNode *node, int len, int st1[3],
1324                                 LeafNode *leaf1, int st2[3], LeafNode *leaf2,
1325                                 int walkdir, int inc, int dir, int side,
1326                                 float alpha)
1327 {
1328 #ifdef IN_DEBUG_MODE
1329         dc_printf("Before patching.\n");
1330         printInfo(st1);
1331         printInfo(st2);
1332         dc_printf("-----------------%d %d %d; %d %d %d\n", st1[0], st2[1], st1[2], st2[0], st2[1], st2[2]);
1333 #endif
1334
1335         // Get edge index on each leaf
1336         int edgedir = (dir + (3 - walkdir)) % 3;
1337         int incdir = (dir + walkdir) % 3;
1338         int ind1 = (edgedir == 1 ? (dir + 3 - edgedir) % 3 - 1 : 2 - (dir + 3 - edgedir) % 3);
1339         int ind2 = (edgedir == 1 ? (incdir + 3 - edgedir) % 3 - 1 : 2 - (incdir + 3 - edgedir) % 3);
1340
1341         int eind1 = ((edgedir << 2) | (side << ind1) | ((inc > 0 ? 1 : 0) << ind2));
1342         int eind2 = ((edgedir << 2) | (side << ind1) | ((inc > 0 ? 0 : 1) << ind2));
1343
1344 #ifdef IN_DEBUG_MODE
1345         dc_printf("Index 1: %d Alpha 1: %f Index 2: %d Alpha 2: %f\n", eind1, alpha, eind2, alpha);
1346         /*
1347            if(alpha < 0 || alpha > 1) {
1348             dc_printf("Index 1: %d Alpha 1: %f Index 2: %d Alpha 2: %f\n", eind1, alpha, eind2, alpha);
1349             printInfo(st1);
1350             printInfo(st2);
1351            }
1352          */
1353 #endif
1354
1355         // Flip edge parity
1356         LeafNode *nleaf1 = flipEdge(leaf1, eind1, alpha);
1357         LeafNode *nleaf2 = flipEdge(leaf2, eind2, alpha);
1358
1359         // Update parent link
1360         updateParent(node, len, st1, nleaf1);
1361         updateParent(node, len, st2, nleaf2);
1362         // updateParent(nleaf1, mindimen, st1);
1363         // updateParent(nleaf2, mindimen, st2);
1364
1365         /*
1366            float m[3];
1367            dc_printf("Adding new point: %f %f %f\n", spt[0], spt[1], spt[2]);
1368            getMinimizer(leaf1, m);
1369            dc_printf("Cell %d now has minimizer %f %f %f\n", leaf1, m[0], m[1], m[2]);
1370            getMinimizer(leaf2, m);
1371            dc_printf("Cell %d now has minimizer %f %f %f\n", leaf2, m[0], m[1], m[2]);
1372          */
1373
1374 #ifdef IN_DEBUG_MODE
1375         dc_printf("After patching.\n");
1376         printInfo(st1);
1377         printInfo(st2);
1378 #endif
1379         return nleaf2;
1380 }
1381
1382 Node *Octree::locateCell(InternalNode *node, int st[3], int len, int ori[3], int dir, int side, Node *& rleaf, int rst[3], int& rlen)
1383 {
1384 #ifdef IN_DEBUG_MODE
1385         // dc_printf("Call to LOCATECELL with node ");
1386         // printNode(node);
1387 #endif
1388
1389         int i;
1390         len >>= 1;
1391         int ind = 0;
1392         for (i = 0; i < 3; i++) {
1393                 ind <<= 1;
1394                 if (i == dir && side == 1) {
1395                         ind |= (ori[i] <= (st[i] + len) ? 0 : 1);
1396                 }
1397                 else {
1398                         ind |= (ori[i] < (st[i] + len) ? 0 : 1);
1399                 }
1400         }
1401
1402 #ifdef IN_DEBUG_MODE
1403         // dc_printf("In LOCATECELL index of ori(%d %d %d) with dir %d side %d in st(%d %d %d, %d) is: %d\n",
1404         //      ori[0], ori[1], ori[2], dir, side, st[0], st[1], st[2], len, ind);
1405 #endif
1406
1407         rst[0] = st[0] + vertmap[ind][0] * len;
1408         rst[1] = st[1] + vertmap[ind][1] * len;
1409         rst[2] = st[2] + vertmap[ind][2] * len;
1410
1411         if (hasChild(node, ind)) {
1412                 int count = getChildCount(node, ind);
1413                 Node *chd = getChild(node, count);
1414                 if (isLeaf(node, ind)) {
1415                         rleaf = chd;
1416                         rlen = len;
1417                 }
1418                 else {
1419                         // Recur
1420                         setChild(node, count, locateCell(&chd->internal, rst, len, ori, dir, side, rleaf, rst, rlen));
1421                 }
1422         }
1423         else {
1424                 // Create a new child here
1425                 if (len == mindimen) {
1426                         LeafNode *chd = createLeaf(0);
1427                         node = addChild(node, ind, (Node *)chd, 1);
1428                         rleaf = (Node *)chd;
1429                         rlen = len;
1430                 }
1431                 else {
1432                         // Subdivide the empty cube
1433                         InternalNode *chd = createInternal(0);
1434                         node = addChild(node, ind,
1435                                         locateCell(chd, rst, len, ori, dir, side, rleaf, rst, rlen), 0);
1436                 }
1437         }
1438
1439 #ifdef IN_DEBUG_MODE
1440         // dc_printf("Return from LOCATECELL with node ");
1441         // printNode(newnode);
1442 #endif
1443         return (Node *)node;
1444 }
1445
1446 void Octree::checkElement(PathElement *ele)
1447 {
1448         /*
1449            if(ele != NULL && locateLeafCheck(ele->pos) != ele->node) {
1450             dc_printf("Screwed! at pos: %d %d %d\n", ele->pos[0]>>minshift, ele->pos[1]>>minshift, ele->pos[2]>>minshift);
1451             exit(0);
1452            }
1453          */
1454 }
1455
1456 void Octree::checkPath(PathElement *path)
1457 {
1458         PathElement *n = path;
1459         int same = 0;
1460         while (n && (same == 0 || n != path)) {
1461                 same++;
1462                 checkElement(n);
1463                 n = n->next;
1464         }
1465
1466 }
1467
1468 void Octree::testFacePoint(PathElement *e1, PathElement *e2)
1469 {
1470         int i;
1471         PathElement *e = NULL;
1472         for (i = 0; i < 3; i++) {
1473                 if (e1->pos[i] != e2->pos[i]) {
1474                         if (e1->pos[i] < e2->pos[i]) {
1475                                 e = e2;
1476                         }
1477                         else {
1478                                 e = e1;
1479                         }
1480                         break;
1481                 }
1482         }
1483
1484         int x, y;
1485         float p, q;
1486         dc_printf("Test.");
1487         getFacePoint(e, i, x, y, p, q);
1488 }
1489
1490 void Octree::getFacePoint(PathElement *leaf, int dir, int& x, int& y, float& p, float& q)
1491 {
1492         // Find average intersections
1493         float avg[3] = {0, 0, 0};
1494         float off[3];
1495         int num = 0, num2 = 0;
1496
1497         LeafNode *leafnode = locateLeaf(leaf->pos);
1498         for (int i = 0; i < 4; i++) {
1499                 int edgeind = faceMap[dir * 2][i];
1500                 int nst[3];
1501                 for (int j = 0; j < 3; j++) {
1502                         nst[j] = leaf->pos[j] + mindimen * vertmap[edgemap[edgeind][0]][j];
1503                 }
1504
1505                 if (getEdgeIntersectionByIndex(nst, edgeind / 4, off, 1)) {
1506                         avg[0] += off[0];
1507                         avg[1] += off[1];
1508                         avg[2] += off[2];
1509                         num++;
1510                 }
1511                 if (getEdgeParity(leafnode, edgeind)) {
1512                         num2++;
1513                 }
1514         }
1515         if (num == 0) {
1516                 dc_printf("Wrong! dir: %d pos: %d %d %d num: %d\n", dir, leaf->pos[0] >> minshift, leaf->pos[1] >> minshift, leaf->pos[2] >> minshift, num2);
1517                 avg[0] = (float) leaf->pos[0];
1518                 avg[1] = (float) leaf->pos[1];
1519                 avg[2] = (float) leaf->pos[2];
1520         }
1521         else {
1522
1523                 avg[0] /= num;
1524                 avg[1] /= num;
1525                 avg[2] /= num;
1526
1527                 //avg[0] =(float) leaf->pos[0];
1528                 //avg[1] =(float) leaf->pos[1];
1529                 //avg[2] =(float) leaf->pos[2];
1530         }
1531
1532         int xdir = (dir + 1) % 3;
1533         int ydir = (dir + 2) % 3;
1534
1535         float xf = avg[xdir];
1536         float yf = avg[ydir];
1537
1538 #ifdef IN_DEBUG_MODE
1539         // Is it outside?
1540         // PathElement* leaf = leaf1->len < leaf2->len ? leaf1 : leaf2;
1541         /*
1542            float* m =(leaf == leaf1 ? m1 : m2);
1543            if(xf < leaf->pos[xdir] ||
1544              yf < leaf->pos[ydir] ||
1545              xf > leaf->pos[xdir] + leaf->len ||
1546              yf > leaf->pos[ydir] + leaf->len) {
1547             dc_printf("Outside cube(%d %d %d), %d : %d %d %f %f\n", leaf->pos[0], leaf->pos[1], leaf->pos[2], leaf->len,
1548                             pos, dir, xf, yf);
1549
1550             // For now, snap to cell
1551             xf = m[xdir];
1552             yf = m[ydir];
1553            }
1554          */
1555
1556         /*
1557            if(alpha < 0 || alpha > 1 ||
1558              xf < leaf->pos[xdir] || xf > leaf->pos[xdir] + leaf->len ||
1559              yf < leaf->pos[ydir] || yf > leaf->pos[ydir] + leaf->len) {
1560             dc_printf("Alpha: %f Address: %d and %d\n", alpha, leaf1->node, leaf2->node);
1561             dc_printf("GETFACEPOINT result:(%d %d %d) %d min:(%f %f %f);(%d %d %d) %d min:(%f %f %f).\n",
1562                 leaf1->pos[0], leaf1->pos[1], leaf1->pos[2], leaf1->len, m1[0], m1[1], m1[2],
1563                 leaf2->pos[0], leaf2->pos[1], leaf2->pos[2], leaf2->len, m2[0], m2[1], m2[2]);
1564             dc_printf("Face point at dir %d pos %d: %f %f\n", dir, pos, xf, yf);
1565            }
1566          */
1567 #endif
1568
1569
1570         // Get the integer and float part
1571         x = ((leaf->pos[xdir]) >> minshift);
1572         y = ((leaf->pos[ydir]) >> minshift);
1573
1574         p = (xf - leaf->pos[xdir]) / mindimen;
1575         q = (yf - leaf->pos[ydir]) / mindimen;
1576
1577
1578 #ifdef IN_DEBUG_MODE
1579         dc_printf("Face point at dir %d : %f %f\n", dir, xf, yf);
1580 #endif
1581 }
1582
1583 int Octree::findPair(PathElement *head, int pos, int dir, PathElement *& pre1, PathElement *& pre2)
1584 {
1585         int side = getSide(head, pos, dir);
1586         PathElement *cur = head;
1587         PathElement *anchor;
1588         PathElement *ppre1, *ppre2;
1589
1590         // Start from this face, find a pair
1591         anchor = cur;
1592         ppre1 = cur;
1593         cur = cur->next;
1594         while (cur != anchor && (getSide(cur, pos, dir) == side)) {
1595                 ppre1 = cur;
1596                 cur = cur->next;
1597         }
1598         if (cur == anchor) {
1599                 // No pair found
1600                 return side;
1601         }
1602
1603         side = getSide(cur, pos, dir);
1604         ppre2 = cur;
1605         cur = cur->next;
1606         while (getSide(cur, pos, dir) == side) {
1607                 ppre2 = cur;
1608                 cur = cur->next;
1609         }
1610
1611
1612         // Switch pre1 and pre2 if we start from the higher side
1613         if (side == -1) {
1614                 cur = ppre1;
1615                 ppre1 = ppre2;
1616                 ppre2 = cur;
1617         }
1618
1619         pre1 = ppre1;
1620         pre2 = ppre2;
1621
1622         return 0;
1623 }
1624
1625 int Octree::getSide(PathElement *e, int pos, int dir)
1626 {
1627         return (e->pos[dir] < pos ? -1 : 1);
1628 }
1629
1630 int Octree::isEqual(PathElement *e1, PathElement *e2)
1631 {
1632         return (e1->pos[0] == e2->pos[0] && e1->pos[1] == e2->pos[1] && e1->pos[2] == e2->pos[2]);
1633 }
1634
1635 void Octree::compressRing(PathElement *& ring)
1636 {
1637         if (ring == NULL) {
1638                 return;
1639         }
1640 #ifdef IN_DEBUG_MODE
1641         dc_printf("Call to COMPRESSRING with path: \n");
1642         printPath(ring);
1643 #endif
1644
1645         PathElement *cur = ring->next->next;
1646         PathElement *pre = ring->next;
1647         PathElement *prepre = ring;
1648         PathElement *anchor = prepre;
1649
1650         do {
1651                 while (isEqual(cur, prepre)) {
1652                         // Delete
1653                         if (cur == prepre) {
1654                                 // The ring has shrinked to a point
1655                                 delete pre;
1656                                 delete cur;
1657                                 anchor = NULL;
1658                                 break;
1659                         }
1660                         else {
1661                                 prepre->next = cur->next;
1662                                 delete pre;
1663                                 delete cur;
1664                                 pre = prepre->next;
1665                                 cur = pre->next;
1666                                 anchor = prepre;
1667                         }
1668                 }
1669
1670                 if (anchor == NULL) {
1671                         break;
1672                 }
1673
1674                 prepre = pre;
1675                 pre = cur;
1676                 cur = cur->next;
1677         } while (prepre != anchor);
1678
1679         ring = anchor;
1680
1681 #ifdef IN_DEBUG_MODE
1682         dc_printf("Return from COMPRESSRING with path: \n");
1683         printPath(ring);
1684 #endif
1685 }
1686
1687 void Octree::buildSigns()
1688 {
1689         // First build a lookup table
1690         // dc_printf("Building up look up table...\n");
1691         int size = 1 << 12;
1692         unsigned char table[1 << 12];
1693         for (int i = 0; i < size; i++) {
1694                 table[i] = 0;
1695         }
1696         for (int i = 0; i < 256; i++) {
1697                 int ind = 0;
1698                 for (int j = 11; j >= 0; j--) {
1699                         ind <<= 1;
1700                         if (((i >> edgemap[j][0]) & 1) ^ ((i >> edgemap[j][1]) & 1)) {
1701                                 ind |= 1;
1702                         }
1703                 }
1704
1705                 table[ind] = i;
1706         }
1707
1708         // Next, traverse the grid
1709         int sg = 1;
1710         int cube[8];
1711         buildSigns(table, root, 0, sg, cube);
1712 }
1713
1714 void Octree::buildSigns(unsigned char table[], Node *node, int isLeaf, int sg, int rvalue[8])
1715 {
1716         if (node == NULL) {
1717                 for (int i = 0; i < 8; i++) {
1718                         rvalue[i] = sg;
1719                 }
1720                 return;
1721         }
1722
1723         if (isLeaf == 0) {
1724                 // Internal node
1725                 Node *chd[8];
1726                 int leaf[8];
1727                 fillChildren(&node->internal, chd, leaf);
1728
1729                 // Get the signs at the corners of the first cube
1730                 rvalue[0] = sg;
1731                 int oris[8];
1732                 buildSigns(table, chd[0], leaf[0], sg, oris);
1733
1734                 // Get the rest
1735                 int cube[8];
1736                 for (int i = 1; i < 8; i++) {
1737                         buildSigns(table, chd[i], leaf[i], oris[i], cube);
1738                         rvalue[i] = cube[i];
1739                 }
1740
1741         }
1742         else {
1743                 // Leaf node
1744                 generateSigns(&node->leaf, table, sg);
1745
1746                 for (int i = 0; i < 8; i++) {
1747                         rvalue[i] = getSign(&node->leaf, i);
1748                 }
1749         }
1750 }
1751
1752 void Octree::floodFill()
1753 {
1754         // int threshold =(int)((dimen/mindimen) *(dimen/mindimen) * 0.5f);
1755         int st[3] = {0, 0, 0};
1756
1757         // First, check for largest component
1758         // size stored in -threshold
1759         clearProcessBits(root, maxDepth);
1760         int threshold = floodFill(root, st, dimen, maxDepth, 0);
1761
1762         // Next remove
1763         dc_printf("Largest component: %d\n", threshold);
1764         threshold *= thresh;
1765         dc_printf("Removing all components smaller than %d\n", threshold);
1766
1767         int st2[3] = {0, 0, 0};
1768         clearProcessBits(root, maxDepth);
1769         floodFill(root, st2, dimen, maxDepth, threshold);
1770
1771 }
1772
1773 void Octree::clearProcessBits(Node *node, int height)
1774 {
1775         int i;
1776
1777         if (height == 0) {
1778                 // Leaf cell,
1779                 for (i = 0; i < 12; i++) {
1780                         setOutProcess(&node->leaf, i);
1781                 }
1782         }
1783         else {
1784                 // Internal cell, recur
1785                 int count = 0;
1786                 for (i = 0; i < 8; i++) {
1787                         if (hasChild(&node->internal, i)) {
1788                                 clearProcessBits(getChild(&node->internal, count), height - 1);
1789                                 count++;
1790                         }
1791                 }
1792         }
1793 }
1794
1795 int Octree::floodFill(LeafNode *leaf, int st[3], int len, int height, int threshold)
1796 {
1797         int i, j;
1798         int maxtotal = 0;
1799
1800         // Leaf cell,
1801         int par, inp;
1802
1803         // Test if the leaf has intersection edges
1804         for (i = 0; i < 12; i++) {
1805                 par = getEdgeParity(leaf, i);
1806                 inp = isInProcess(leaf, i);
1807
1808                 if (par == 1 && inp == 0) {
1809                         // Intersection edge, hasn't been processed
1810                         // Let's start filling
1811                         GridQueue *queue = new GridQueue();
1812                         int total = 1;
1813
1814                         // Set to in process
1815                         int mst[3];
1816                         mst[0] = st[0] + vertmap[edgemap[i][0]][0] * len;
1817                         mst[1] = st[1] + vertmap[edgemap[i][0]][1] * len;
1818                         mst[2] = st[2] + vertmap[edgemap[i][0]][2] * len;
1819                         int mdir = i / 4;
1820                         setInProcessAll(mst, mdir);
1821
1822                         // Put this edge into queue
1823                         queue->pushQueue(mst, mdir);
1824
1825                         // Queue processing
1826                         int nst[3], dir;
1827                         while (queue->popQueue(nst, dir) == 1) {
1828                                 // dc_printf("nst: %d %d %d, dir: %d\n", nst[0]/mindimen, nst[1]/mindimen, nst[2]/mindimen, dir);
1829                                 // locations
1830                                 int stMask[3][3] = {
1831                                         {0, 0 - len, 0 - len},
1832                                         {0 - len, 0, 0 - len},
1833                                         {0 - len, 0 - len, 0}
1834                                 };
1835                                 int cst[2][3];
1836                                 for (j = 0; j < 3; j++) {
1837                                         cst[0][j] = nst[j];
1838                                         cst[1][j] = nst[j] + stMask[dir][j];
1839                                 }
1840
1841                                 // cells
1842                                 LeafNode *cs[2];
1843                                 for (j = 0; j < 2; j++) {
1844                                         cs[j] = locateLeaf(cst[j]);
1845                                 }
1846
1847                                 // Middle sign
1848                                 int s = getSign(cs[0], 0);
1849
1850                                 // Masks
1851                                 int fcCells[4] = {1, 0, 1, 0};
1852                                 int fcEdges[3][4][3] = {
1853                                         {{9, 2, 11}, {8, 1, 10}, {5, 1, 7}, {4, 2, 6}},
1854                                         {{10, 6, 11}, {8, 5, 9}, {1, 5, 3}, {0, 6, 2}},
1855                                         {{6, 10, 7}, {4, 9, 5}, {2, 9, 3}, {0, 10, 1}}
1856                                 };
1857
1858                                 // Search for neighboring connected intersection edges
1859                                 for (int find = 0; find < 4; find++) {
1860                                         int cind = fcCells[find];
1861                                         int eind, edge;
1862                                         if (s == 0) {
1863                                                 // Original order
1864                                                 for (eind = 0; eind < 3; eind++) {
1865                                                         edge = fcEdges[dir][find][eind];
1866                                                         if (getEdgeParity(cs[cind], edge) == 1) {
1867                                                                 break;
1868                                                         }
1869                                                 }
1870                                         }
1871                                         else {
1872                                                 // Inverse order
1873                                                 for (eind = 2; eind >= 0; eind--) {
1874                                                         edge = fcEdges[dir][find][eind];
1875                                                         if (getEdgeParity(cs[cind], edge) == 1) {
1876                                                                 break;
1877                                                         }
1878                                                 }
1879                                         }
1880
1881                                         if (eind == 3 || eind == -1) {
1882                                                 dc_printf("Wrong! this is not a consistent sign. %d\n", eind);
1883                                         }
1884                                         else {
1885                                                 int est[3];
1886                                                 est[0] = cst[cind][0] + vertmap[edgemap[edge][0]][0] * len;
1887                                                 est[1] = cst[cind][1] + vertmap[edgemap[edge][0]][1] * len;
1888                                                 est[2] = cst[cind][2] + vertmap[edgemap[edge][0]][2] * len;
1889                                                 int edir = edge / 4;
1890
1891                                                 if (isInProcess(cs[cind], edge) == 0) {
1892                                                         setInProcessAll(est, edir);
1893                                                         queue->pushQueue(est, edir);
1894                                                         // dc_printf("Pushed: est: %d %d %d, edir: %d\n", est[0]/len, est[1]/len, est[2]/len, edir);
1895                                                         total++;
1896                                                 }
1897                                         }
1898                                 }
1899                         }
1900
1901                         dc_printf("Size of component: %d ", total);
1902
1903                         if (threshold == 0) {
1904                                 // Measuring stage
1905                                 if (total > maxtotal) {
1906                                         maxtotal = total;
1907                                 }
1908                                 dc_printf(".\n");
1909                                 continue;
1910                         }
1911
1912                         if (total >= threshold) {
1913                                 dc_printf("Maintained.\n");
1914                                 continue;
1915                         }
1916                         dc_printf("Less then %d, removing...\n", threshold);
1917
1918                         // We have to remove this noise
1919
1920                         // Flip parity
1921                         // setOutProcessAll(mst, mdir);
1922                         flipParityAll(mst, mdir);
1923
1924                         // Put this edge into queue
1925                         queue->pushQueue(mst, mdir);
1926
1927                         // Queue processing
1928                         while (queue->popQueue(nst, dir) == 1) {
1929                                 // dc_printf("nst: %d %d %d, dir: %d\n", nst[0]/mindimen, nst[1]/mindimen, nst[2]/mindimen, dir);
1930                                 // locations
1931                                 int stMask[3][3] = {
1932                                         {0, 0 - len, 0 - len},
1933                                         {0 - len, 0, 0 - len},
1934                                         {0 - len, 0 - len, 0}
1935                                 };
1936                                 int cst[2][3];
1937                                 for (j = 0; j < 3; j++) {
1938                                         cst[0][j] = nst[j];
1939                                         cst[1][j] = nst[j] + stMask[dir][j];
1940                                 }
1941
1942                                 // cells
1943                                 LeafNode *cs[2];
1944                                 for (j = 0; j < 2; j++)
1945                                         cs[j] = locateLeaf(cst[j]);
1946
1947                                 // Middle sign
1948                                 int s = getSign(cs[0], 0);
1949
1950                                 // Masks
1951                                 int fcCells[4] = {1, 0, 1, 0};
1952                                 int fcEdges[3][4][3] = {
1953                                         {{9, 2, 11}, {8, 1, 10}, {5, 1, 7}, {4, 2, 6}},
1954                                         {{10, 6, 11}, {8, 5, 9}, {1, 5, 3}, {0, 6, 2}},
1955                                         {{6, 10, 7}, {4, 9, 5}, {2, 9, 3}, {0, 10, 1}}
1956                                 };
1957
1958                                 // Search for neighboring connected intersection edges
1959                                 for (int find = 0; find < 4; find++) {
1960                                         int cind = fcCells[find];
1961                                         int eind, edge;
1962                                         if (s == 0) {
1963                                                 // Original order
1964                                                 for (eind = 0; eind < 3; eind++) {
1965                                                         edge = fcEdges[dir][find][eind];
1966                                                         if (isInProcess(cs[cind], edge) == 1) {
1967                                                                 break;
1968                                                         }
1969                                                 }
1970                                         }
1971                                         else {
1972                                                 // Inverse order
1973                                                 for (eind = 2; eind >= 0; eind--) {
1974                                                         edge = fcEdges[dir][find][eind];
1975                                                         if (isInProcess(cs[cind], edge) == 1) {
1976                                                                 break;
1977                                                         }
1978                                                 }
1979                                         }
1980
1981                                         if (eind == 3 || eind == -1) {
1982                                                 dc_printf("Wrong! this is not a consistent sign. %d\n", eind);
1983                                         }
1984                                         else {
1985                                                 int est[3];
1986                                                 est[0] = cst[cind][0] + vertmap[edgemap[edge][0]][0] * len;
1987                                                 est[1] = cst[cind][1] + vertmap[edgemap[edge][0]][1] * len;
1988                                                 est[2] = cst[cind][2] + vertmap[edgemap[edge][0]][2] * len;
1989                                                 int edir = edge / 4;
1990
1991                                                 if (getEdgeParity(cs[cind], edge) == 1) {
1992                                                         flipParityAll(est, edir);
1993                                                         queue->pushQueue(est, edir);
1994                                                         // dc_printf("Pushed: est: %d %d %d, edir: %d\n", est[0]/len, est[1]/len, est[2]/len, edir);
1995                                                         total++;
1996                                                 }
1997                                         }
1998                                 }
1999                         }
2000                 }
2001         }
2002
2003         return maxtotal;
2004 }
2005
2006 int Octree::floodFill(Node *node, int st[3], int len, int height, int threshold)
2007 {
2008         int i;
2009         int maxtotal = 0;
2010
2011         if (height == 0) {
2012                 maxtotal = floodFill(&node->leaf, st, len, height, threshold);
2013         }
2014         else {
2015                 // Internal cell, recur
2016                 int count = 0;
2017                 len >>= 1;
2018                 for (i = 0; i < 8; i++) {
2019                         if (hasChild((InternalNode *)node, i)) {
2020                                 int nst[3];
2021                                 nst[0] = st[0] + vertmap[i][0] * len;
2022                                 nst[1] = st[1] + vertmap[i][1] * len;
2023                                 nst[2] = st[2] + vertmap[i][2] * len;
2024
2025                                 int d = floodFill(getChild((InternalNode *)node, count), nst, len, height - 1, threshold);
2026                                 if (d > maxtotal) {
2027                                         maxtotal = d;
2028                                 }
2029                                 count++;
2030                         }
2031                 }
2032         }
2033
2034
2035         return maxtotal;
2036
2037 }
2038
2039 void Octree::writeOut()
2040 {
2041         int numQuads = 0;
2042         int numVertices = 0;
2043         int numEdges = 0;
2044
2045         countIntersection(root, maxDepth, numQuads, numVertices, numEdges);
2046
2047         dc_printf("Vertices counted: %d Polys counted: %d \n", numVertices, numQuads);
2048         output_mesh = alloc_output(numVertices, numQuads);
2049         int offset = 0;
2050         int st[3] = {0, 0, 0};
2051
2052         // First, output vertices
2053         offset = 0;
2054         actualVerts = 0;
2055         actualQuads = 0;
2056
2057         generateMinimizer(root, st, dimen, maxDepth, offset);
2058         cellProcContour(root, 0, maxDepth);
2059         dc_printf("Vertices written: %d Quads written: %d \n", offset, actualQuads);
2060 }
2061
2062 void Octree::countIntersection(Node *node, int height, int& nedge, int& ncell, int& nface)
2063 {
2064         if (height > 0) {
2065                 int total = getNumChildren(&node->internal);
2066                 for (int i = 0; i < total; i++) {
2067                         countIntersection(getChild(&node->internal, i), height - 1, nedge, ncell, nface);
2068                 }
2069         }
2070         else {
2071                 nedge += getNumEdges2(&node->leaf);
2072
2073                 int smask = getSignMask(&node->leaf);
2074
2075                 if (use_manifold) {
2076                         int comps = manifold_table[smask].comps;
2077                         ncell += comps;
2078                 }
2079                 else {
2080                         if (smask > 0 && smask < 255) {
2081                                 ncell++;
2082                         }
2083                 }
2084
2085                 for (int i = 0; i < 3; i++) {
2086                         if (getFaceEdgeNum(&node->leaf, i * 2)) {
2087                                 nface++;
2088                         }
2089                 }
2090         }
2091 }
2092
2093 /* from http://eigen.tuxfamily.org/bz/show_bug.cgi?id=257 */
2094 template<typename _Matrix_Type_>
2095 void pseudoInverse(const _Matrix_Type_ &a,
2096                    _Matrix_Type_ &result,
2097                    double epsilon = std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon())
2098 {
2099         Eigen::JacobiSVD< _Matrix_Type_ > svd = a.jacobiSvd(Eigen::ComputeFullU |
2100                                                             Eigen::ComputeFullV);
2101
2102         typename _Matrix_Type_::Scalar tolerance = epsilon * std::max(a.cols(),
2103                                                                       a.rows()) *
2104                                                    svd.singularValues().array().abs().maxCoeff();
2105
2106         result = svd.matrixV() *
2107                  _Matrix_Type_((svd.singularValues().array().abs() >
2108                                 tolerance).select(svd.singularValues().
2109                                                   array().inverse(), 0)).asDiagonal() *
2110                  svd.matrixU().adjoint();
2111 }
2112
2113 void solve_least_squares(const float halfA[], const float b[],
2114                          const float midpoint[], float rvalue[])
2115 {
2116         /* calculate pseudo-inverse */
2117         Eigen::MatrixXf A(3, 3), pinv(3, 3);
2118         A << halfA[0], halfA[1], halfA[2],
2119             halfA[1], halfA[3], halfA[4],
2120             halfA[2], halfA[4], halfA[5];
2121         pseudoInverse(A, pinv);
2122
2123         Eigen::Vector3f b2(b), mp(midpoint), result;
2124         b2 = b2 + A * -mp;
2125         result = pinv * b2 + mp;
2126
2127         for (int i = 0; i < 3; i++)
2128                 rvalue[i] = result(i);
2129 }
2130
2131 void minimize(float rvalue[3], float mp[3], const float pts[12][3],
2132               const float norms[12][3], const int parity[12])
2133 {
2134         float ata[6] = {0, 0, 0, 0, 0, 0};
2135         float atb[3] = {0, 0, 0};
2136         int ec = 0;
2137
2138         for (int i = 0; i < 12; i++) {
2139                 // if(getEdgeParity(leaf, i))
2140                 if (parity[i]) {
2141                         const float *norm = norms[i];
2142                         const float *p = pts[i];
2143
2144                         // QEF
2145                         ata[0] += (float)(norm[0] * norm[0]);
2146                         ata[1] += (float)(norm[0] * norm[1]);
2147                         ata[2] += (float)(norm[0] * norm[2]);
2148                         ata[3] += (float)(norm[1] * norm[1]);
2149                         ata[4] += (float)(norm[1] * norm[2]);
2150                         ata[5] += (float)(norm[2] * norm[2]);
2151
2152                         double pn = p[0] * norm[0] + p[1] * norm[1] + p[2] * norm[2];
2153
2154                         atb[0] += (float)(norm[0] * pn);
2155                         atb[1] += (float)(norm[1] * pn);
2156                         atb[2] += (float)(norm[2] * pn);
2157
2158                         // Minimizer
2159                         mp[0] += p[0];
2160                         mp[1] += p[1];
2161                         mp[2] += p[2];
2162
2163                         ec++;
2164                 }
2165         }
2166
2167         if (ec == 0) {
2168                 return;
2169         }
2170         mp[0] /= ec;
2171         mp[1] /= ec;
2172         mp[2] /= ec;
2173
2174         // Solve least squares
2175         solve_least_squares(ata, atb, mp, rvalue);
2176 }
2177
2178 void Octree::computeMinimizer(LeafNode *leaf, int st[3], int len, float rvalue[3])
2179 {
2180         // First, gather all edge intersections
2181         float pts[12][3], norms[12][3];
2182         int parity[12];
2183         fillEdgeIntersections(leaf, st, len, pts, norms, parity);
2184
2185         // Next, construct QEF and minimizer
2186         float mp[3] = {0, 0, 0};
2187         minimize(rvalue, mp, pts, norms, parity);
2188
2189         /* Restraining the location of the minimizer */
2190         float nh1 = hermite_num * len;
2191         float nh2 = (1 + hermite_num) * len;
2192         if ((mode == DUALCON_MASS_POINT || mode == DUALCON_CENTROID) ||
2193             (rvalue[0] < st[0] - nh1 || rvalue[1] < st[1] - nh1 || rvalue[2] < st[2] - nh1 ||
2194              rvalue[0] > st[0] + nh2 || rvalue[1] > st[1] + nh2 || rvalue[2] > st[2] + nh2)) {
2195                 if (mode == DUALCON_CENTROID) {
2196                         // Use centroids
2197                         rvalue[0] = (float) st[0] + len / 2;
2198                         rvalue[1] = (float) st[1] + len / 2;
2199                         rvalue[2] = (float) st[2] + len / 2;
2200                 }
2201                 else {
2202                         // Use mass point instead
2203                         rvalue[0] = mp[0];
2204                         rvalue[1] = mp[1];
2205                         rvalue[2] = mp[2];
2206                 }
2207         }
2208 }
2209
2210 void Octree::generateMinimizer(Node *node, int st[3], int len, int height, int& offset)
2211 {
2212         int i, j;
2213
2214         if (height == 0) {
2215                 // Leaf cell, generate
2216
2217                 // First, find minimizer
2218                 float rvalue[3];
2219                 rvalue[0] = (float) st[0] + len / 2;
2220                 rvalue[1] = (float) st[1] + len / 2;
2221                 rvalue[2] = (float) st[2] + len / 2;
2222                 computeMinimizer(&node->leaf, st, len, rvalue);
2223
2224                 // Update
2225                 //float fnst[3];
2226                 for (j = 0; j < 3; j++) {
2227                         rvalue[j] = rvalue[j] * range / dimen + origin[j];
2228                         //fnst[j] = st[j] * range / dimen + origin[j];
2229                 }
2230
2231                 int mult = 0, smask = getSignMask(&node->leaf);
2232
2233                 if (use_manifold) {
2234                         mult = manifold_table[smask].comps;
2235                 }
2236                 else {
2237                         if (smask > 0 && smask < 255) {
2238                                 mult = 1;
2239                         }
2240                 }
2241
2242                 for (j = 0; j < mult; j++) {
2243                         add_vert(output_mesh, rvalue);
2244                 }
2245
2246                 // Store the index
2247                 setMinimizerIndex(&node->leaf, offset);
2248
2249                 offset += mult;
2250         }
2251         else {
2252                 // Internal cell, recur
2253                 int count = 0;
2254                 len >>= 1;
2255                 for (i = 0; i < 8; i++) {
2256                         if (hasChild(&node->internal, i)) {
2257                                 int nst[3];
2258                                 nst[0] = st[0] + vertmap[i][0] * len;
2259                                 nst[1] = st[1] + vertmap[i][1] * len;
2260                                 nst[2] = st[2] + vertmap[i][2] * len;
2261
2262                                 generateMinimizer(getChild(&node->internal, count),
2263                                                   nst, len, height - 1, offset);
2264                                 count++;
2265                         }
2266                 }
2267         }
2268 }
2269
2270 void Octree::processEdgeWrite(Node *node[4], int depth[4], int maxdep, int dir)
2271 {
2272         //int color = 0;
2273
2274         int i = 3;
2275         {
2276                 if (getEdgeParity((LeafNode *)(node[i]), processEdgeMask[dir][i])) {
2277                         int flip = 0;
2278                         int edgeind = processEdgeMask[dir][i];
2279                         if (getSign((LeafNode *)node[i], edgemap[edgeind][1]) > 0) {
2280                                 flip = 1;
2281                         }
2282
2283                         int num = 0;
2284                         {
2285                                 int ind[8];
2286                                 if (use_manifold) {
2287                                         int vind[2];
2288                                         int seq[4] = {0, 1, 3, 2};
2289                                         for (int k = 0; k < 4; k++) {
2290                                                 getMinimizerIndices((LeafNode *)(node[seq[k]]), processEdgeMask[dir][seq[k]], vind);
2291                                                 ind[num] = vind[0];
2292                                                 num++;
2293
2294                                                 if (vind[1] != -1) {
2295                                                         ind[num] = vind[1];
2296                                                         num++;
2297                                                         if (flip == 0) {
2298                                                                 ind[num - 1] = vind[0];
2299                                                                 ind[num - 2] = vind[1];
2300                                                         }
2301                                                 }
2302                                         }
2303
2304                                         /* we don't use the manifold option, but if it is
2305                                            ever enabled again note that it can output
2306                                            non-quads */
2307                                 }
2308                                 else {
2309                                         if (flip) {
2310                                                 ind[0] = getMinimizerIndex((LeafNode *)(node[2]));
2311                                                 ind[1] = getMinimizerIndex((LeafNode *)(node[3]));
2312                                                 ind[2] = getMinimizerIndex((LeafNode *)(node[1]));
2313                                                 ind[3] = getMinimizerIndex((LeafNode *)(node[0]));
2314                                         }
2315                                         else {
2316                                                 ind[0] = getMinimizerIndex((LeafNode *)(node[0]));
2317                                                 ind[1] = getMinimizerIndex((LeafNode *)(node[1]));
2318                                                 ind[2] = getMinimizerIndex((LeafNode *)(node[3]));
2319                                                 ind[3] = getMinimizerIndex((LeafNode *)(node[2]));
2320                                         }
2321
2322                                         add_quad(output_mesh, ind);
2323                                 }
2324                         }
2325                         return;
2326                 }
2327                 else {
2328                         return;
2329                 }
2330         }
2331 }
2332
2333
2334 void Octree::edgeProcContour(Node *node[4], int leaf[4], int depth[4], int maxdep, int dir)
2335 {
2336         if (!(node[0] && node[1] && node[2] && node[3])) {
2337                 return;
2338         }
2339         if (leaf[0] && leaf[1] && leaf[2] && leaf[3]) {
2340                 processEdgeWrite(node, depth, maxdep, dir);
2341         }
2342         else {
2343                 int i, j;
2344                 Node *chd[4][8];
2345                 for (j = 0; j < 4; j++) {
2346                         for (i = 0; i < 8; i++) {
2347                                 chd[j][i] = ((!leaf[j]) && hasChild(&node[j]->internal, i)) ?
2348                                             getChild(&node[j]->internal,
2349                                                      getChildCount(&node[j]->internal, i)) : NULL;
2350                         }
2351                 }
2352
2353                 // 2 edge calls
2354                 Node *ne[4];
2355                 int le[4];
2356                 int de[4];
2357                 for (i = 0; i < 2; i++) {
2358                         int c[4] = {edgeProcEdgeMask[dir][i][0],
2359                                         edgeProcEdgeMask[dir][i][1],
2360                                         edgeProcEdgeMask[dir][i][2],
2361                                         edgeProcEdgeMask[dir][i][3]};
2362
2363                         for (int j = 0; j < 4; j++) {
2364                                 if (leaf[j]) {
2365                                         le[j] = leaf[j];
2366                                         ne[j] = node[j];
2367                                         de[j] = depth[j];
2368                                 }
2369                                 else {
2370                                         le[j] = isLeaf(&node[j]->internal, c[j]);
2371                                         ne[j] = chd[j][c[j]];
2372                                         de[j] = depth[j] - 1;
2373                                 }
2374                         }
2375
2376                         edgeProcContour(ne, le, de, maxdep - 1, edgeProcEdgeMask[dir][i][4]);
2377                 }
2378
2379         }
2380 }
2381
2382 void Octree::faceProcContour(Node *node[2], int leaf[2], int depth[2], int maxdep, int dir)
2383 {
2384         if (!(node[0] && node[1])) {
2385                 return;
2386         }
2387
2388         if (!(leaf[0] && leaf[1])) {
2389                 int i, j;
2390                 // Fill children nodes
2391                 Node *chd[2][8];
2392                 for (j = 0; j < 2; j++) {
2393                         for (i = 0; i < 8; i++) {
2394                                 chd[j][i] = ((!leaf[j]) && hasChild(&node[j]->internal, i)) ?
2395                                             getChild(&node[j]->internal,
2396                                                      getChildCount(&node[j]->internal, i)) : NULL;
2397                         }
2398                 }
2399
2400                 // 4 face calls
2401                 Node *nf[2];
2402                 int df[2];
2403                 int lf[2];
2404                 for (i = 0; i < 4; i++) {
2405                         int c[2] = {faceProcFaceMask[dir][i][0], faceProcFaceMask[dir][i][1]};
2406                         for (int j = 0; j < 2; j++) {
2407                                 if (leaf[j]) {
2408                                         lf[j] = leaf[j];
2409                                         nf[j] = node[j];
2410                                         df[j] = depth[j];
2411                                 }
2412                                 else {
2413                                         lf[j] = isLeaf(&node[j]->internal, c[j]);
2414                                         nf[j] = chd[j][c[j]];
2415                                         df[j] = depth[j] - 1;
2416                                 }
2417                         }
2418                         faceProcContour(nf, lf, df, maxdep - 1, faceProcFaceMask[dir][i][2]);
2419                 }
2420
2421                 // 4 edge calls
2422                 int orders[2][4] = {{0, 0, 1, 1}, {0, 1, 0, 1}};
2423                 Node *ne[4];
2424                 int le[4];
2425                 int de[4];
2426
2427                 for (i = 0; i < 4; i++) {
2428                         int c[4] = {faceProcEdgeMask[dir][i][1], faceProcEdgeMask[dir][i][2],
2429                                         faceProcEdgeMask[dir][i][3], faceProcEdgeMask[dir][i][4]};
2430                         int *order = orders[faceProcEdgeMask[dir][i][0]];
2431
2432                         for (int j = 0; j < 4; j++) {
2433                                 if (leaf[order[j]]) {
2434                                         le[j] = leaf[order[j]];
2435                                         ne[j] = node[order[j]];
2436                                         de[j] = depth[order[j]];
2437                                 }
2438                                 else {
2439                                         le[j] = isLeaf(&node[order[j]]->internal, c[j]);
2440                                         ne[j] = chd[order[j]][c[j]];
2441                                         de[j] = depth[order[j]] - 1;
2442                                 }
2443                         }
2444
2445                         edgeProcContour(ne, le, de, maxdep - 1, faceProcEdgeMask[dir][i][5]);
2446                 }
2447         }
2448 }
2449
2450
2451 void Octree::cellProcContour(Node *node, int leaf, int depth)
2452 {
2453         if (node == NULL) {
2454                 return;
2455         }
2456
2457         if (!leaf) {
2458                 int i;
2459
2460                 // Fill children nodes
2461                 Node *chd[8];
2462                 for (i = 0; i < 8; i++) {
2463                         chd[i] = ((!leaf) && hasChild(&node->internal, i)) ?
2464                                  getChild(&node->internal,
2465                                           getChildCount(&node->internal, i)) : NULL;
2466                 }
2467
2468                 // 8 Cell calls
2469                 for (i = 0; i < 8; i++) {
2470                         cellProcContour(chd[i], isLeaf(&node->internal, i), depth - 1);
2471                 }
2472
2473                 // 12 face calls
2474                 Node *nf[2];
2475                 int lf[2];
2476                 int df[2] = {depth - 1, depth - 1};
2477                 for (i = 0; i < 12; i++) {
2478                         int c[2] = {cellProcFaceMask[i][0], cellProcFaceMask[i][1]};
2479
2480                         lf[0] = isLeaf(&node->internal, c[0]);
2481                         lf[1] = isLeaf(&node->internal, c[1]);
2482
2483                         nf[0] = chd[c[0]];
2484                         nf[1] = chd[c[1]];
2485
2486                         faceProcContour(nf, lf, df, depth - 1, cellProcFaceMask[i][2]);
2487                 }
2488
2489                 // 6 edge calls
2490                 Node *ne[4];
2491                 int le[4];
2492                 int de[4] = {depth - 1, depth - 1, depth - 1, depth - 1};
2493                 for (i = 0; i < 6; i++) {
2494                         int c[4] = {cellProcEdgeMask[i][0], cellProcEdgeMask[i][1], cellProcEdgeMask[i][2], cellProcEdgeMask[i][3]};
2495
2496                         for (int j = 0; j < 4; j++) {
2497                                 le[j] = isLeaf(&node->internal, c[j]);
2498                                 ne[j] = chd[c[j]];
2499                         }
2500
2501                         edgeProcContour(ne, le, de, depth - 1, cellProcEdgeMask[i][4]);
2502                 }
2503         }
2504
2505 }
2506
2507 void Octree::processEdgeParity(LeafNode *node[4], int depth[4], int maxdep, int dir)
2508 {
2509         int con = 0;
2510         for (int i = 0; i < 4; i++) {
2511                 // Minimal cell
2512                 // if(op == 0)
2513                 {
2514                         if (getEdgeParity(node[i], processEdgeMask[dir][i])) {
2515                                 con = 1;
2516                                 break;
2517                         }
2518                 }
2519         }
2520
2521         if (con == 1) {
2522                 for (int i = 0; i < 4; i++) {
2523                         setEdge(node[i], processEdgeMask[dir][i]);
2524                 }
2525         }
2526
2527 }
2528
2529 void Octree::edgeProcParity(Node *node[4], int leaf[4], int depth[4], int maxdep, int dir)
2530 {
2531         if (!(node[0] && node[1] && node[2] && node[3])) {
2532                 return;
2533         }
2534         if (leaf[0] && leaf[1] && leaf[2] && leaf[3]) {
2535                 processEdgeParity((LeafNode **)node, depth, maxdep, dir);
2536         }
2537         else {
2538                 int i, j;
2539                 Node *chd[4][8];
2540                 for (j = 0; j < 4; j++) {
2541                         for (i = 0; i < 8; i++) {
2542                                 chd[j][i] = ((!leaf[j]) && hasChild(&node[j]->internal, i)) ?
2543                                             getChild(&node[j]->internal, getChildCount(&node[j]->internal, i)) : NULL;
2544                         }
2545                 }
2546
2547                 // 2 edge calls
2548                 Node *ne[4];
2549                 int le[4];
2550                 int de[4];
2551                 for (i = 0; i < 2; i++) {
2552                         int c[4] = {edgeProcEdgeMask[dir][i][0],
2553                                         edgeProcEdgeMask[dir][i][1],
2554                                         edgeProcEdgeMask[dir][i][2],
2555                                         edgeProcEdgeMask[dir][i][3]};
2556
2557                         // int allleaf = 1;
2558                         for (int j = 0; j < 4; j++) {
2559
2560                                 if (leaf[j]) {
2561                                         le[j] = leaf[j];
2562                                         ne[j] = node[j];
2563                                         de[j] = depth[j];
2564                                 }
2565                                 else {
2566                                         le[j] = isLeaf(&node[j]->internal, c[j]);
2567                                         ne[j] = chd[j][c[j]];
2568                                         de[j] = depth[j] - 1;
2569
2570                                 }
2571
2572                         }
2573
2574                         edgeProcParity(ne, le, de, maxdep - 1, edgeProcEdgeMask[dir][i][4]);
2575                 }
2576
2577         }
2578 }
2579
2580 void Octree::faceProcParity(Node *node[2], int leaf[2], int depth[2], int maxdep, int dir)
2581 {
2582         if (!(node[0] && node[1])) {
2583                 return;
2584         }
2585
2586         if (!(leaf[0] && leaf[1])) {
2587                 int i, j;
2588                 // Fill children nodes
2589                 Node *chd[2][8];
2590                 for (j = 0; j < 2; j++) {
2591                         for (i = 0; i < 8; i++) {
2592                                 chd[j][i] = ((!leaf[j]) && hasChild(&node[j]->internal, i)) ?
2593                                             getChild(&node[j]->internal,
2594                                                      getChildCount(&node[j]->internal, i)) : NULL;
2595                         }
2596                 }
2597
2598                 // 4 face calls
2599                 Node *nf[2];
2600                 int df[2];
2601                 int lf[2];
2602                 for (i = 0; i < 4; i++) {
2603                         int c[2] = {faceProcFaceMask[dir][i][0], faceProcFaceMask[dir][i][1]};
2604                         for (int j = 0; j < 2; j++) {
2605                                 if (leaf[j]) {
2606                                         lf[j] = leaf[j];
2607                                         nf[j] = node[j];
2608                                         df[j] = depth[j];
2609                                 }
2610                                 else {
2611                                         lf[j] = isLeaf(&node[j]->internal, c[j]);
2612                                         nf[j] = chd[j][c[j]];
2613                                         df[j] = depth[j] - 1;
2614                                 }
2615                         }
2616                         faceProcParity(nf, lf, df, maxdep - 1, faceProcFaceMask[dir][i][2]);
2617                 }
2618
2619                 // 4 edge calls
2620                 int orders[2][4] = {{0, 0, 1, 1}, {0, 1, 0, 1}};
2621                 Node *ne[4];
2622                 int le[4];
2623                 int de[4];
2624
2625                 for (i = 0; i < 4; i++) {
2626                         int c[4] = {faceProcEdgeMask[dir][i][1], faceProcEdgeMask[dir][i][2],
2627                                         faceProcEdgeMask[dir][i][3], faceProcEdgeMask[dir][i][4]};
2628                         int *order = orders[faceProcEdgeMask[dir][i][0]];
2629
2630                         for (int j = 0; j < 4; j++) {
2631                                 if (leaf[order[j]]) {
2632                                         le[j] = leaf[order[j]];
2633                                         ne[j] = node[order[j]];
2634                                         de[j] = depth[order[j]];
2635                                 }
2636                                 else {
2637                                         le[j] = isLeaf((InternalNode *)(node[order[j]]), c[j]);
2638                                         ne[j] = chd[order[j]][c[j]];
2639                                         de[j] = depth[order[j]] - 1;
2640                                 }
2641                         }
2642
2643                         edgeProcParity(ne, le, de, maxdep - 1, faceProcEdgeMask[dir][i][5]);
2644                 }
2645         }
2646 }
2647
2648
2649 void Octree::cellProcParity(Node *node, int leaf, int depth)
2650 {
2651         if (node == NULL) {
2652                 return;
2653         }
2654
2655         if (!leaf) {
2656                 int i;
2657
2658                 // Fill children nodes
2659                 Node *chd[8];
2660                 for (i = 0; i < 8; i++) {
2661                         chd[i] = ((!leaf) && hasChild((InternalNode *)node, i)) ?
2662                                  getChild((InternalNode *)node,
2663                                           getChildCount((InternalNode *)node, i)) : NULL;
2664                 }
2665
2666                 // 8 Cell calls
2667                 for (i = 0; i < 8; i++) {
2668                         cellProcParity(chd[i], isLeaf((InternalNode *)node, i), depth - 1);
2669                 }
2670
2671                 // 12 face calls
2672                 Node *nf[2];
2673                 int lf[2];
2674                 int df[2] = {depth - 1, depth - 1};
2675                 for (i = 0; i < 12; i++) {
2676                         int c[2] = {cellProcFaceMask[i][0], cellProcFaceMask[i][1]};
2677
2678                         lf[0] = isLeaf((InternalNode *)node, c[0]);
2679                         lf[1] = isLeaf((InternalNode *)node, c[1]);
2680
2681                         nf[0] = chd[c[0]];
2682                         nf[1] = chd[c[1]];
2683
2684                         faceProcParity(nf, lf, df, depth - 1, cellProcFaceMask[i][2]);
2685                 }
2686
2687                 // 6 edge calls
2688                 Node *ne[4];
2689                 int le[4];
2690                 int de[4] = {depth - 1, depth - 1, depth - 1, depth - 1};
2691                 for (i = 0; i < 6; i++) {
2692                         int c[4] = {cellProcEdgeMask[i][0], cellProcEdgeMask[i][1], cellProcEdgeMask[i][2], cellProcEdgeMask[i][3]};
2693
2694                         for (int j = 0; j < 4; j++) {
2695                                 le[j] = isLeaf((InternalNode *)node, c[j]);
2696                                 ne[j] = chd[c[j]];
2697                         }
2698
2699                         edgeProcParity(ne, le, de, depth - 1, cellProcEdgeMask[i][4]);
2700                 }
2701         }
2702
2703 }
2704
2705 /* definitions for global arrays */
2706 const int edgemask[3] = {5, 3, 6};
2707
2708 const int faceMap[6][4] = {
2709         {4, 8, 5, 9},
2710         {6, 10, 7, 11},
2711         {0, 8, 1, 10},
2712         {2, 9, 3, 11},
2713         {0, 4, 2, 6},
2714         {1, 5, 3, 7}
2715 };
2716
2717 const int cellProcFaceMask[12][3] = {
2718         {0, 4, 0},
2719         {1, 5, 0},
2720         {2, 6, 0},
2721         {3, 7, 0},
2722         {0, 2, 1},
2723         {4, 6, 1},
2724         {1, 3, 1},
2725         {5, 7, 1},
2726         {0, 1, 2},
2727         {2, 3, 2},
2728         {4, 5, 2},
2729         {6, 7, 2}
2730 };
2731
2732 const int cellProcEdgeMask[6][5] = {
2733         {0, 1, 2, 3, 0},
2734         {4, 5, 6, 7, 0},
2735         {0, 4, 1, 5, 1},
2736         {2, 6, 3, 7, 1},
2737         {0, 2, 4, 6, 2},
2738         {1, 3, 5, 7, 2}
2739 };
2740
2741 const int faceProcFaceMask[3][4][3] = {
2742         {{4, 0, 0},
2743          {5, 1, 0},
2744          {6, 2, 0},
2745          {7, 3, 0}},
2746         {{2, 0, 1},
2747          {6, 4, 1},
2748          {3, 1, 1},
2749          {7, 5, 1}},
2750         {{1, 0, 2},
2751          {3, 2, 2},
2752          {5, 4, 2},
2753          {7, 6, 2}}
2754 };
2755
2756 const int faceProcEdgeMask[3][4][6] = {
2757         {{1, 4, 0, 5, 1, 1},
2758          {1, 6, 2, 7, 3, 1},
2759          {0, 4, 6, 0, 2, 2},
2760          {0, 5, 7, 1, 3, 2}},
2761         {{0, 2, 3, 0, 1, 0},
2762          {0, 6, 7, 4, 5, 0},
2763          {1, 2, 0, 6, 4, 2},
2764          {1, 3, 1, 7, 5, 2}},
2765         {{1, 1, 0, 3, 2, 0},
2766          {1, 5, 4, 7, 6, 0},
2767          {0, 1, 5, 0, 4, 1},
2768          {0, 3, 7, 2, 6, 1}}
2769 };
2770
2771 const int edgeProcEdgeMask[3][2][5] = {
2772         {{3, 2, 1, 0, 0},
2773          {7, 6, 5, 4, 0}},
2774         {{5, 1, 4, 0, 1},
2775          {7, 3, 6, 2, 1}},
2776         {{6, 4, 2, 0, 2},
2777          {7, 5, 3, 1, 2}},
2778 };
2779
2780 const int processEdgeMask[3][4] = {
2781         {3, 2, 1, 0},
2782         {7, 5, 6, 4},
2783         {11, 10, 9, 8}
2784 };
2785
2786 const int dirCell[3][4][3] = {
2787         {{0, -1, -1},
2788          {0, -1, 0},
2789          {0, 0, -1},
2790          {0, 0, 0}},
2791         {{-1, 0, -1},
2792          {-1, 0, 0},
2793          {0, 0, -1},
2794          {0, 0, 0}},
2795         {{-1, -1, 0},
2796          {-1, 0, 0},
2797          {0, -1, 0},
2798          {0, 0, 0}}
2799 };
2800
2801 const int dirEdge[3][4] = {
2802         {3, 2, 1, 0},
2803         {7, 6, 5, 4},
2804         {11, 10, 9, 8}
2805 };