Tomato: re-integrate ceres library with updates needed for tracking improvement
[blender.git] / intern / dualcon / intern / octree.h
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 #ifndef OCTREE_H
24 #define OCTREE_H
25
26 #include <cassert>
27 #include <cstring>
28 #include <stdio.h>
29 #include <math.h>
30 #include "GeoCommon.h"
31 #include "Projections.h"
32 #include "ModelReader.h"
33 #include "MemoryAllocator.h"
34 #include "cubes.h"
35 #include "Queue.h"
36 #include "manifold_table.h"
37 #include "dualcon.h"
38
39 /**
40  * Main class and structures for scan-convertion, sign-generation,
41  * and surface reconstruction.
42  *
43  * @author Tao Ju
44  */
45
46
47 /* Global defines */
48 // Uncomment to see debug information
49 // #define IN_DEBUG_MODE
50
51 // Uncomment to see more output messages during repair
52 // #define IN_VERBOSE_MODE
53
54 /* Set scan convert params */
55
56 #define EDGE_FLOATS 4
57
58 union Node;
59
60 struct InternalNode {
61         /* Treat as bitfield, bit N indicates whether child N exists or not */
62         unsigned char has_child;
63         /* Treat as bitfield, bit N indicates whether child N is a leaf or not */
64         unsigned char child_is_leaf;
65
66         /* Can have up to eight children */
67         Node *children[0];
68 };
69
70
71 /**
72  * Bits order
73  *
74  * Leaf node:
75  * Byte 0,1(0-11): edge parity
76  * Byte 1(4,5,6): mask of primary edges intersections stored
77  * Byte 1(7): in flood fill mode, whether the cell is in process
78  * Byte 2(0-8): signs
79  * Byte 3,4: in coloring mode, the mask for edges
80  * Byte 5: edge intersections(4 bytes per inter, or 12 bytes if USE_HERMIT)
81  */
82 struct LeafNode /* TODO: remove this attribute once everything is fixed */ {
83         unsigned short edge_parity : 12;
84         unsigned short primary_edge_intersections : 3;
85
86         /* XXX: maybe actually unused? */
87         unsigned short in_process : 1;
88
89         /* bitfield */
90         char signs;
91
92         int minimizer_index;
93
94         unsigned short flood_fill;
95
96         float edge_intersections[0];
97 };
98
99 /* Doesn't get directly allocated anywhere, just used for passing
100    pointers to nodes that could be internal or leaf. */
101 union Node {
102         InternalNode internal;
103         LeafNode leaf;
104 };
105
106 /* Global variables */
107 extern const int edgemask[3];
108 extern const int faceMap[6][4];
109 extern const int cellProcFaceMask[12][3];
110 extern const int cellProcEdgeMask[6][5];
111 extern const int faceProcFaceMask[3][4][3];
112 extern const int edgeProcEdgeMask[3][2][5];
113 extern const int faceProcEdgeMask[3][4][6];
114 extern const int processEdgeMask[3][4];
115 extern const int dirCell[3][4][3];
116 extern const int dirEdge[3][4];
117
118 /**
119  * Structures for detecting/patching open cycles on the dual surface
120  */
121
122 struct PathElement {
123         // Origin
124         int pos[3];
125
126         // link
127         PathElement *next;
128 };
129
130 struct PathList {
131         // Head
132         PathElement *head;
133         PathElement *tail;
134
135         // Length of the list
136         int length;
137
138         // Next list
139         PathList *next;
140 };
141
142
143 /**
144  * Class for building and processing an octree
145  */
146 class Octree
147 {
148 public:
149 /* Public members */
150
151 /// Memory allocators
152 VirtualMemoryAllocator *alloc[9];
153 VirtualMemoryAllocator *leafalloc[4];
154
155 /// Root node
156 Node *root;
157
158 /// Model reader
159 ModelReader *reader;
160
161 /// Marching cubes table
162 Cubes *cubes;
163
164 /// Length of grid
165 int dimen;
166 int mindimen, minshift;
167
168 /// Maximum depth
169 int maxDepth;
170
171 /// The lower corner of the bounding box and the size
172 float origin[3];
173 float range;
174
175 /// Counting information
176 int nodeCount;
177 int nodeSpace;
178 int nodeCounts[9];
179
180 int actualQuads, actualVerts;
181
182 PathList *ringList;
183
184 int maxTrianglePerCell;
185 int outType;     // 0 for OFF, 1 for PLY, 2 for VOL
186
187 // For flood filling
188 int use_flood_fill;
189 float thresh;
190
191 int use_manifold;
192
193 float hermite_num;
194
195 DualConMode mode;
196
197 public:
198 /**
199  * Construtor
200  */
201 Octree(ModelReader *mr,
202        DualConAllocOutput alloc_output_func,
203        DualConAddVert add_vert_func,
204        DualConAddQuad add_quad_func,
205        DualConFlags flags, DualConMode mode, int depth,
206        float threshold, float hermite_num);
207
208 /**
209  * Destructor
210  */
211 ~Octree();
212
213 /**
214  * Scan convert
215  */
216 void scanConvert();
217
218 void *getOutputMesh() {
219         return output_mesh;
220 }
221
222 private:
223 /* Helper functions */
224
225 /**
226  * Initialize memory allocators
227  */
228 void initMemory();
229
230 /**
231  * Release memory
232  */
233 void freeMemory();
234
235 /**
236  * Print memory usage
237  */
238 void printMemUsage();
239
240
241 /**
242  * Methods to set / restore minimum edges
243  */
244 void resetMinimalEdges();
245
246 void cellProcParity(Node *node, int leaf, int depth);
247 void faceProcParity(Node * node[2], int leaf[2], int depth[2], int maxdep, int dir);
248 void edgeProcParity(Node * node[4], int leaf[4], int depth[4], int maxdep, int dir);
249
250 void processEdgeParity(LeafNode * node[4], int depths[4], int maxdep, int dir);
251
252 /**
253  * Add triangles to the tree
254  */
255 void addTrian();
256 void addTrian(Triangle *trian, int triind);
257 InternalNode *addTrian(InternalNode *node, Projections *p, int height);
258
259 /**
260  * Method to update minimizer in a cell: update edge intersections instead
261  */
262 LeafNode *updateCell(LeafNode *node, Projections *p);
263
264 /* Routines to detect and patch holes */
265 int numRings;
266 int totRingLengths;
267 int maxRingLength;
268
269 /**
270  * Entry routine.
271  */
272 void trace();
273 /**
274  * Trace the given node, find patches and fill them in
275  */
276 Node *trace(Node *node, int *st, int len, int depth, PathList *& paths);
277 /**
278  * Look for path on the face and add to paths
279  */
280 void findPaths(Node * node[2], int leaf[2], int depth[2], int *st[2], int maxdep, int dir, PathList * &paths);
281 /**
282  * Combine two list1 and list2 into list1 using connecting paths list3,
283  * while closed paths are appended to rings
284  */
285 void combinePaths(PathList *& list1, PathList *list2, PathList *paths, PathList *& rings);
286 /**
287  * Helper function: combine current paths in list1 and list2 to a single path and append to list3
288  */
289 PathList *combineSinglePath(PathList *& head1, PathList *pre1, PathList *& list1, PathList *& head2, PathList *pre2, PathList *& list2);
290
291 /**
292  * Functions to patch rings in a node
293  */
294 Node *patch(Node * node, int st[3], int len, PathList * rings);
295 Node *patchSplit(Node * node, int st[3], int len, PathList * rings, int dir, PathList * &nrings1, PathList * &nrings2);
296 Node *patchSplitSingle(Node * node, int st[3], int len, PathElement * head, int dir, PathList * &nrings1, PathList * &nrings2);
297 Node *connectFace(Node * node, int st[3], int len, int dir, PathElement * f1, PathElement * f2);
298 Node *locateCell(InternalNode * node, int st[3], int len, int ori[3], int dir, int side, Node * &rleaf, int rst[3], int& rlen);
299 void compressRing(PathElement *& ring);
300 void getFacePoint(PathElement *leaf, int dir, int& x, int& y, float& p, float& q);
301 LeafNode *patchAdjacent(InternalNode * node, int len, int st1[3], LeafNode * leaf1, int st2[3], LeafNode * leaf2, int walkdir, int inc, int dir, int side, float alpha);
302 int findPair(PathElement *head, int pos, int dir, PathElement *& pre1, PathElement *& pre2);
303 int getSide(PathElement *e, int pos, int dir);
304 int isEqual(PathElement *e1, PathElement *e2);
305 void preparePrimalEdgesMask(InternalNode *node);
306 void testFacePoint(PathElement *e1, PathElement *e2);
307
308 /**
309  * Path-related functions
310  */
311 void deletePath(PathList *& head, PathList *pre, PathList *& curr);
312 void printPath(PathList *path);
313 void printPath(PathElement *path);
314 void printElement(PathElement *ele);
315 void printPaths(PathList *path);
316 void checkElement(PathElement *ele);
317 void checkPath(PathElement *path);
318
319
320 /**
321  * Routines to build signs to create a partitioned volume
322  *(after patching rings)
323  */
324 void buildSigns();
325 void buildSigns(unsigned char table[], Node * node, int isLeaf, int sg, int rvalue[8]);
326
327 /************************************************************************/
328 /* To remove disconnected components */
329 /************************************************************************/
330 void floodFill();
331 void clearProcessBits(Node *node, int height);
332 int floodFill(LeafNode * leaf, int st[3], int len, int height, int threshold);
333 int floodFill(Node * node, int st[3], int len, int height, int threshold);
334
335 /**
336  * Write out polygon file
337  */
338 void writeOut();
339
340 void countIntersection(Node *node, int height, int& nedge, int& ncell, int& nface);
341 void generateMinimizer(Node * node, int st[3], int len, int height, int& offset);
342 void computeMinimizer(LeafNode * leaf, int st[3], int len, float rvalue[3]);
343 /**
344  * Traversal functions to generate polygon model
345  * op: 0 for counting, 1 for writing OBJ, 2 for writing OFF, 3 for writing PLY
346  */
347 void cellProcContour(Node *node, int leaf, int depth);
348 void faceProcContour(Node * node[2], int leaf[2], int depth[2], int maxdep, int dir);
349 void edgeProcContour(Node * node[4], int leaf[4], int depth[4], int maxdep, int dir);
350 void processEdgeWrite(Node * node[4], int depths[4], int maxdep, int dir);
351
352 /* output callbacks/data */
353 DualConAllocOutput alloc_output;
354 DualConAddVert add_vert;
355 DualConAddQuad add_quad;
356 void *output_mesh;
357
358 private:
359 /************ Operators for all nodes ************/
360
361 /// Lookup table
362 int numChildrenTable[256];
363 int childrenCountTable[256][8];
364 int childrenIndexTable[256][8];
365 int numEdgeTable[8];
366 int edgeCountTable[8][3];
367
368 /// Build up lookup table
369 void buildTable()
370 {
371         for (int i = 0; i < 256; i++)
372         {
373                 numChildrenTable[i] = 0;
374                 int count = 0;
375                 for (int j = 0; j < 8; j++)
376                 {
377                         numChildrenTable[i] += ((i >> j) & 1);
378                         childrenCountTable[i][j] = count;
379                         childrenIndexTable[i][count] = j;
380                         count += ((i >> j) & 1);
381                 }
382         }
383
384         for (int i = 0; i < 8; i++)
385         {
386                 numEdgeTable[i] = 0;
387                 int count = 0;
388                 for (int j = 0; j < 3; j++)
389                 {
390                         numEdgeTable[i] += ((i >> j) & 1);
391                         edgeCountTable[i][j] = count;
392                         count += ((i >> j) & 1);
393                 }
394         }
395 }
396
397 int getSign(Node *node, int height, int index)
398 {
399         if (height == 0)
400         {
401                 return getSign(&node->leaf, index);
402         }
403         else {
404                 if (hasChild(&node->internal, index))
405                 {
406                         return getSign(getChild(&node->internal, getChildCount(&node->internal, index)),
407                                        height - 1,
408                                        index);
409                 }
410                 else {
411                         return getSign(getChild(&node->internal, 0),
412                                        height - 1,
413                                        7 - getChildIndex(&node->internal, 0));
414                 }
415         }
416 }
417
418 /************ Operators for leaf nodes ************/
419
420 void printInfo(int st[3])
421 {
422         printf("INFO AT: %d %d %d\n", st[0] >> minshift, st[1] >> minshift, st[2] >> minshift);
423         LeafNode *leaf = (LeafNode *)locateLeafCheck(st);
424         if (leaf)
425                 printInfo(leaf);
426         else
427                 printf("Leaf not exists!\n");
428 }
429
430 void printInfo(const LeafNode *leaf)
431 {
432         /*
433            printf("Edge mask: ");
434            for(int i = 0; i < 12; i ++)
435            {
436             printf("%d ", getEdgeParity(leaf, i));
437            }
438            printf("\n");
439            printf("Stored edge mask: ");
440            for(i = 0; i < 3; i ++)
441            {
442             printf("%d ", getStoredEdgesParity(leaf, i));
443            }
444            printf("\n");
445          */
446         printf("Sign mask: ");
447         for (int i = 0; i < 8; i++)
448         {
449                 printf("%d ", getSign(leaf, i));
450         }
451         printf("\n");
452
453 }
454
455 /// Retrieve signs
456 int getSign(const LeafNode *leaf, int index)
457 {
458         return ((leaf->signs >> index) & 1);
459 }
460
461 /// Set sign
462 void setSign(LeafNode *leaf, int index)
463 {
464         leaf->signs |= (1 << index);
465 }
466
467 void setSign(LeafNode *leaf, int index, int sign)
468 {
469         leaf->signs &= (~(1 << index));
470         leaf->signs |= ((sign & 1) << index);
471 }
472
473 int getSignMask(const LeafNode *leaf)
474 {
475         return leaf->signs;
476 }
477
478 void setInProcessAll(int st[3], int dir)
479 {
480         int nst[3], eind;
481         for (int i = 0; i < 4; i++)
482         {
483                 nst[0] = st[0] + dirCell[dir][i][0] * mindimen;
484                 nst[1] = st[1] + dirCell[dir][i][1] * mindimen;
485                 nst[2] = st[2] + dirCell[dir][i][2] * mindimen;
486                 eind = dirEdge[dir][i];
487
488                 LeafNode *cell = locateLeafCheck(nst);
489                 assert(cell);
490
491                 setInProcess(cell, eind);
492         }
493 }
494
495 void flipParityAll(int st[3], int dir)
496 {
497         int nst[3], eind;
498         for (int i = 0; i < 4; i++)
499         {
500                 nst[0] = st[0] + dirCell[dir][i][0] * mindimen;
501                 nst[1] = st[1] + dirCell[dir][i][1] * mindimen;
502                 nst[2] = st[2] + dirCell[dir][i][2] * mindimen;
503                 eind = dirEdge[dir][i];
504
505                 LeafNode *cell = locateLeaf(nst);
506                 flipEdge(cell, eind);
507         }
508 }
509
510 void setInProcess(LeafNode *leaf, int eind)
511 {
512         assert(eind >= 0 && eind <= 11);
513
514         leaf->flood_fill |= (1 << eind);
515 }
516
517 void setOutProcess(LeafNode *leaf, int eind)
518 {
519         assert(eind >= 0 && eind <= 11);
520
521         leaf->flood_fill &= ~(1 << eind);
522 }
523
524 int isInProcess(LeafNode *leaf, int eind)
525 {
526         assert(eind >= 0 && eind <= 11);
527
528         return (leaf->flood_fill >> eind) & 1;
529 }
530
531 /// Generate signs at the corners from the edge parity
532 void generateSigns(LeafNode *leaf, unsigned char table[], int start)
533 {
534         leaf->signs = table[leaf->edge_parity];
535
536         if ((start ^ leaf->signs) & 1)
537         {
538                 leaf->signs = ~(leaf->signs);
539         }
540 }
541
542 /// Get edge parity
543 int getEdgeParity(LeafNode *leaf, int index)
544 {
545         assert(index >= 0 && index <= 11);
546
547         return (leaf->edge_parity >> index) & 1;
548 }
549
550 /// Get edge parity on a face
551 int getFaceParity(LeafNode *leaf, int index)
552 {
553         int a = getEdgeParity(leaf, faceMap[index][0]) +
554                 getEdgeParity(leaf, faceMap[index][1]) +
555                 getEdgeParity(leaf, faceMap[index][2]) +
556                 getEdgeParity(leaf, faceMap[index][3]);
557         return (a & 1);
558 }
559 int getFaceEdgeNum(LeafNode *leaf, int index)
560 {
561         int a = getEdgeParity(leaf, faceMap[index][0]) +
562                 getEdgeParity(leaf, faceMap[index][1]) +
563                 getEdgeParity(leaf, faceMap[index][2]) +
564                 getEdgeParity(leaf, faceMap[index][3]);
565         return a;
566 }
567
568 /// Set edge parity
569 void flipEdge(LeafNode *leaf, int index)
570 {
571         assert(index >= 0 && index <= 11);
572
573         leaf->edge_parity ^= (1 << index);
574 }
575
576 /// Set 1
577 void setEdge(LeafNode *leaf, int index)
578 {
579         assert(index >= 0 && index <= 11);
580
581         leaf->edge_parity |= (1 << index);
582 }
583
584 /// Set 0
585 void resetEdge(LeafNode *leaf, int index)
586 {
587         assert(index >= 0 && index <= 11);
588
589         leaf->edge_parity &= ~(1 << index);
590 }
591
592 /// Flipping with a new intersection offset
593 void createPrimalEdgesMask(LeafNode *leaf)
594 {
595         leaf->primary_edge_intersections = getPrimalEdgesMask2(leaf);
596 }
597
598 void setStoredEdgesParity(LeafNode *leaf, int pindex)
599 {
600         assert(pindex <= 2 && pindex >= 0);
601
602         leaf->primary_edge_intersections |= (1 << pindex);
603 }
604 int getStoredEdgesParity(LeafNode *leaf, int pindex)
605 {
606         assert(pindex <= 2 && pindex >= 0);
607
608         return (leaf->primary_edge_intersections >> pindex) & 1;
609 }
610
611 LeafNode *flipEdge(LeafNode *leaf, int index, float alpha)
612 {
613         flipEdge(leaf, index);
614
615         if ((index & 3) == 0)
616         {
617                 int ind = index / 4;
618                 if (getEdgeParity(leaf, index) && !getStoredEdgesParity(leaf, ind))
619                 {
620                         // Create a new node
621                         int num = getNumEdges(leaf) + 1;
622                         setStoredEdgesParity(leaf, ind);
623                         int count = getEdgeCount(leaf, ind);
624                         LeafNode *nleaf = createLeaf(num);
625                         *nleaf = *leaf;
626
627                         setEdgeOffset(nleaf, alpha, count);
628
629                         if (num > 1)
630                         {
631                                 float *pts = leaf->edge_intersections;
632                                 float *npts = nleaf->edge_intersections;
633                                 for (int i = 0; i < count; i++)
634                                 {
635                                         for (int j = 0; j < EDGE_FLOATS; j++)
636                                         {
637                                                 npts[i * EDGE_FLOATS + j] = pts[i * EDGE_FLOATS + j];
638                                         }
639                                 }
640                                 for (int i = count + 1; i < num; i++)
641                                 {
642                                         for (int j = 0; j < EDGE_FLOATS; j++)
643                                         {
644                                                 npts[i * EDGE_FLOATS + j] = pts[(i - 1) * EDGE_FLOATS + j];
645                                         }
646                                 }
647                         }
648
649
650                         removeLeaf(num - 1, (LeafNode *)leaf);
651                         leaf = nleaf;
652                 }
653         }
654
655         return leaf;
656 }
657
658 /// Update parent link
659 void updateParent(InternalNode *node, int len, int st[3], LeafNode *leaf)
660 {
661         // First, locate the parent
662         int count;
663         InternalNode *parent = locateParent(node, len, st, count);
664
665         // Update
666         setChild(parent, count, (Node *)leaf);
667 }
668
669 void updateParent(InternalNode *node, int len, int st[3])
670 {
671         if (len == dimen)
672         {
673                 root = (Node *)node;
674                 return;
675         }
676
677         // First, locate the parent
678         int count;
679         InternalNode *parent = locateParent(len, st, count);
680
681         // UPdate
682         setChild(parent, count, (Node *)node);
683 }
684
685 /// Find edge intersection on a given edge
686 int getEdgeIntersectionByIndex(int st[3], int index, float pt[3], int check)
687 {
688         // First, locat the leaf
689         LeafNode *leaf;
690         if (check)
691         {
692                 leaf = locateLeafCheck(st);
693         }
694         else {
695                 leaf = locateLeaf(st);
696         }
697
698         if (leaf && getStoredEdgesParity(leaf, index))
699         {
700                 float off = getEdgeOffset(leaf, getEdgeCount(leaf, index));
701                 pt[0] = (float) st[0];
702                 pt[1] = (float) st[1];
703                 pt[2] = (float) st[2];
704                 pt[index] += off * mindimen;
705
706                 return 1;
707         }
708         else {
709                 return 0;
710         }
711 }
712
713 /// Retrieve number of edges intersected
714 int getPrimalEdgesMask(LeafNode *leaf)
715 {
716         return leaf->primary_edge_intersections;
717 }
718
719 int getPrimalEdgesMask2(LeafNode *leaf)
720 {
721         return (((leaf->edge_parity &   0x1) >> 0) |
722                 ((leaf->edge_parity &  0x10) >> 3) |
723                 ((leaf->edge_parity & 0x100) >> 6));
724 }
725
726 /// Get the count for a primary edge
727 int getEdgeCount(LeafNode *leaf, int index)
728 {
729         return edgeCountTable[getPrimalEdgesMask(leaf)][index];
730 }
731 int getNumEdges(LeafNode *leaf)
732 {
733         return numEdgeTable[getPrimalEdgesMask(leaf)];
734 }
735
736 int getNumEdges2(LeafNode *leaf)
737 {
738         return numEdgeTable[getPrimalEdgesMask2(leaf)];
739 }
740
741 /// Set edge intersection
742 void setEdgeOffset(LeafNode *leaf, float pt, int count)
743 {
744         float *pts = leaf->edge_intersections;
745         pts[EDGE_FLOATS * count] = pt;
746         pts[EDGE_FLOATS * count + 1] = 0;
747         pts[EDGE_FLOATS * count + 2] = 0;
748         pts[EDGE_FLOATS * count + 3] = 0;
749 }
750
751 /// Set multiple edge intersections
752 void setEdgeOffsets(LeafNode *leaf, float pt[3], int len)
753 {
754         float *pts = leaf->edge_intersections;
755         for (int i = 0; i < len; i++)
756         {
757                 pts[i] = pt[i];
758         }
759 }
760
761 /// Retrieve edge intersection
762 float getEdgeOffset(LeafNode *leaf, int count)
763 {
764         return leaf->edge_intersections[4 * count];
765 }
766
767 /// Update method
768 LeafNode *updateEdgeOffsets(LeafNode *leaf, int oldlen, int newlen, float offs[3])
769 {
770         // First, create a new leaf node
771         LeafNode *nleaf = createLeaf(newlen);
772         *nleaf = *leaf;
773
774         // Next, fill in the offsets
775         setEdgeOffsets(nleaf, offs, newlen);
776
777         // Finally, delete the old leaf
778         removeLeaf(oldlen, leaf);
779
780         return nleaf;
781 }
782
783 /// Set minimizer index
784 void setMinimizerIndex(LeafNode *leaf, int index)
785 {
786         leaf->minimizer_index = index;
787 }
788
789 /// Get minimizer index
790 int getMinimizerIndex(LeafNode *leaf)
791 {
792         return leaf->minimizer_index;
793 }
794
795 int getMinimizerIndex(LeafNode *leaf, int eind)
796 {
797         int add = manifold_table[getSignMask(leaf)].pairs[eind][0] - 1;
798         assert(add >= 0);
799         return leaf->minimizer_index + add;
800 }
801
802 void getMinimizerIndices(LeafNode *leaf, int eind, int inds[2])
803 {
804         const int *add = manifold_table[getSignMask(leaf)].pairs[eind];
805         inds[0] = leaf->minimizer_index + add[0] - 1;
806         if (add[0] == add[1])
807         {
808                 inds[1] = -1;
809         }
810         else {
811                 inds[1] = leaf->minimizer_index + add[1] - 1;
812         }
813 }
814
815
816 /// Set edge intersection
817 void setEdgeOffsetNormal(LeafNode *leaf, float pt, float a, float b, float c, int count)
818 {
819         float *pts = leaf->edge_intersections;
820         pts[4 * count] = pt;
821         pts[4 * count + 1] = a;
822         pts[4 * count + 2] = b;
823         pts[4 * count + 3] = c;
824 }
825
826 float getEdgeOffsetNormal(LeafNode *leaf, int count, float& a, float& b, float& c)
827 {
828         float *pts = leaf->edge_intersections;
829         a = pts[4 * count + 1];
830         b = pts[4 * count + 2];
831         c = pts[4 * count + 3];
832         return pts[4 * count];
833 }
834
835 /// Set multiple edge intersections
836 void setEdgeOffsetsNormals(LeafNode *leaf, float pt[], float a[], float b[], float c[], int len)
837 {
838         float *pts = leaf->edge_intersections;
839         for (int i = 0; i < len; i++)
840         {
841                 if (pt[i] > 1 || pt[i] < 0)
842                 {
843                         printf("\noffset: %f\n", pt[i]);
844                 }
845                 pts[i * 4] = pt[i];
846                 pts[i * 4 + 1] = a[i];
847                 pts[i * 4 + 2] = b[i];
848                 pts[i * 4 + 3] = c[i];
849         }
850 }
851
852 /// Retrieve complete edge intersection
853 void getEdgeIntersectionByIndex(LeafNode *leaf, int index, int st[3], int len, float pt[3], float nm[3])
854 {
855         int count = getEdgeCount(leaf, index);
856         float *pts = leaf->edge_intersections;
857
858         float off = pts[4 * count];
859
860         pt[0] = (float) st[0];
861         pt[1] = (float) st[1];
862         pt[2] = (float) st[2];
863         pt[index] += (off * len);
864
865         nm[0] = pts[4 * count + 1];
866         nm[1] = pts[4 * count + 2];
867         nm[2] = pts[4 * count + 3];
868 }
869
870 float getEdgeOffsetNormalByIndex(LeafNode *leaf, int index, float nm[3])
871 {
872         int count = getEdgeCount(leaf, index);
873         float *pts = leaf->edge_intersections;
874
875         float off = pts[4 * count];
876
877         nm[0] = pts[4 * count + 1];
878         nm[1] = pts[4 * count + 2];
879         nm[2] = pts[4 * count + 3];
880
881         return off;
882 }
883
884 void fillEdgeIntersections(LeafNode *leaf, int st[3], int len, float pts[12][3], float norms[12][3])
885 {
886         int i;
887         // int stt[3] = {0, 0, 0};
888
889         // The three primal edges are easy
890         int pmask[3] = {0, 4, 8};
891         for (i = 0; i < 3; i++)
892         {
893                 if (getEdgeParity(leaf, pmask[i]))
894                 {
895                         // getEdgeIntersectionByIndex(leaf, i, stt, 1, pts[pmask[i]], norms[pmask[i]]);
896                         getEdgeIntersectionByIndex(leaf, i, st, len, pts[pmask[i]], norms[pmask[i]]);
897                 }
898         }
899
900         // 3 face adjacent cubes
901         int fmask[3][2] = {{6, 10}, {2, 9}, {1, 5}};
902         int femask[3][2] = {{1, 2}, {0, 2}, {0, 1}};
903         for (i = 0; i < 3; i++)
904         {
905                 int e1 = getEdgeParity(leaf, fmask[i][0]);
906                 int e2 = getEdgeParity(leaf, fmask[i][1]);
907                 if (e1 || e2)
908                 {
909                         int nst[3] = {st[0], st[1], st[2]};
910                         nst[i] += len;
911                         // int nstt[3] = {0, 0, 0};
912                         // nstt[i] += 1;
913                         LeafNode *node = locateLeaf(nst);
914
915                         if (e1)
916                         {
917                                 // getEdgeIntersectionByIndex(node, femask[i][0], nstt, 1, pts[fmask[i][0]], norms[fmask[i][0]]);
918                                 getEdgeIntersectionByIndex(node, femask[i][0], nst, len, pts[fmask[i][0]], norms[fmask[i][0]]);
919                         }
920                         if (e2)
921                         {
922                                 // getEdgeIntersectionByIndex(node, femask[i][1], nstt, 1, pts[fmask[i][1]], norms[fmask[i][1]]);
923                                 getEdgeIntersectionByIndex(node, femask[i][1], nst, len, pts[fmask[i][1]], norms[fmask[i][1]]);
924                         }
925                 }
926         }
927
928         // 3 edge adjacent cubes
929         int emask[3] = {3, 7, 11};
930         int eemask[3] = {0, 1, 2};
931         for (i = 0; i < 3; i++)
932         {
933                 if (getEdgeParity(leaf, emask[i]))
934                 {
935                         int nst[3] = {st[0] + len, st[1] + len, st[2] + len};
936                         nst[i] -= len;
937                         // int nstt[3] = {1, 1, 1};
938                         // nstt[i] -= 1;
939                         LeafNode *node = locateLeaf(nst);
940
941                         // getEdgeIntersectionByIndex(node, eemask[i], nstt, 1, pts[emask[i]], norms[emask[i]]);
942                         getEdgeIntersectionByIndex(node, eemask[i], nst, len, pts[emask[i]], norms[emask[i]]);
943                 }
944         }
945 }
946
947
948 void fillEdgeIntersections(LeafNode *leaf, int st[3], int len, float pts[12][3], float norms[12][3], int parity[12])
949 {
950         int i;
951         for (i = 0; i < 12; i++)
952         {
953                 parity[i] = 0;
954         }
955         // int stt[3] = {0, 0, 0};
956
957         // The three primal edges are easy
958         int pmask[3] = {0, 4, 8};
959         for (i = 0; i < 3; i++)
960         {
961                 if (getStoredEdgesParity(leaf, i))
962                 {
963                         // getEdgeIntersectionByIndex(leaf, i, stt, 1, pts[pmask[i]], norms[pmask[i]]);
964                         getEdgeIntersectionByIndex(leaf, i, st, len, pts[pmask[i]], norms[pmask[i]]);
965                         parity[pmask[i]] = 1;
966                 }
967         }
968
969         // 3 face adjacent cubes
970         int fmask[3][2] = {{6, 10}, {2, 9}, {1, 5}};
971         int femask[3][2] = {{1, 2}, {0, 2}, {0, 1}};
972         for (i = 0; i < 3; i++)
973         {
974                 {
975                         int nst[3] = {st[0], st[1], st[2]};
976                         nst[i] += len;
977                         // int nstt[3] = {0, 0, 0};
978                         // nstt[i] += 1;
979                         LeafNode *node = locateLeafCheck(nst);
980                         if (node == NULL)
981                         {
982                                 continue;
983                         }
984
985                         int e1 = getStoredEdgesParity(node, femask[i][0]);
986                         int e2 = getStoredEdgesParity(node, femask[i][1]);
987
988                         if (e1)
989                         {
990                                 // getEdgeIntersectionByIndex(node, femask[i][0], nstt, 1, pts[fmask[i][0]], norms[fmask[i][0]]);
991                                 getEdgeIntersectionByIndex(node, femask[i][0], nst, len, pts[fmask[i][0]], norms[fmask[i][0]]);
992                                 parity[fmask[i][0]] = 1;
993                         }
994                         if (e2)
995                         {
996                                 // getEdgeIntersectionByIndex(node, femask[i][1], nstt, 1, pts[fmask[i][1]], norms[fmask[i][1]]);
997                                 getEdgeIntersectionByIndex(node, femask[i][1], nst, len, pts[fmask[i][1]], norms[fmask[i][1]]);
998                                 parity[fmask[i][1]] = 1;
999                         }
1000                 }
1001         }
1002
1003         // 3 edge adjacent cubes
1004         int emask[3] = {3, 7, 11};
1005         int eemask[3] = {0, 1, 2};
1006         for (i = 0; i < 3; i++)
1007         {
1008 //                      if(getEdgeParity(leaf, emask[i]))
1009                 {
1010                         int nst[3] = {st[0] + len, st[1] + len, st[2] + len};
1011                         nst[i] -= len;
1012                         // int nstt[3] = {1, 1, 1};
1013                         // nstt[i] -= 1;
1014                         LeafNode *node = locateLeafCheck(nst);
1015                         if (node == NULL)
1016                         {
1017                                 continue;
1018                         }
1019
1020                         if (getStoredEdgesParity(node, eemask[i]))
1021                         {
1022                                 // getEdgeIntersectionByIndex(node, eemask[i], nstt, 1, pts[emask[i]], norms[emask[i]]);
1023                                 getEdgeIntersectionByIndex(node, eemask[i], nst, len, pts[emask[i]], norms[emask[i]]);
1024                                 parity[emask[i]] = 1;
1025                         }
1026                 }
1027         }
1028 }
1029
1030 void fillEdgeOffsetsNormals(LeafNode *leaf, int st[3], int len, float pts[12], float norms[12][3], int parity[12])
1031 {
1032         int i;
1033         for (i = 0; i < 12; i++)
1034         {
1035                 parity[i] = 0;
1036         }
1037         // int stt[3] = {0, 0, 0};
1038
1039         // The three primal edges are easy
1040         int pmask[3] = {0, 4, 8};
1041         for (i = 0; i < 3; i++)
1042         {
1043                 if (getStoredEdgesParity(leaf, i))
1044                 {
1045                         pts[pmask[i]] = getEdgeOffsetNormalByIndex(leaf, i, norms[pmask[i]]);
1046                         parity[pmask[i]] = 1;
1047                 }
1048         }
1049
1050         // 3 face adjacent cubes
1051         int fmask[3][2] = {{6, 10}, {2, 9}, {1, 5}};
1052         int femask[3][2] = {{1, 2}, {0, 2}, {0, 1}};
1053         for (i = 0; i < 3; i++)
1054         {
1055                 {
1056                         int nst[3] = {st[0], st[1], st[2]};
1057                         nst[i] += len;
1058                         // int nstt[3] = {0, 0, 0};
1059                         // nstt[i] += 1;
1060                         LeafNode *node = locateLeafCheck(nst);
1061                         if (node == NULL)
1062                         {
1063                                 continue;
1064                         }
1065
1066                         int e1 = getStoredEdgesParity(node, femask[i][0]);
1067                         int e2 = getStoredEdgesParity(node, femask[i][1]);
1068
1069                         if (e1)
1070                         {
1071                                 pts[fmask[i][0]] = getEdgeOffsetNormalByIndex(node, femask[i][0], norms[fmask[i][0]]);
1072                                 parity[fmask[i][0]] = 1;
1073                         }
1074                         if (e2)
1075                         {
1076                                 pts[fmask[i][1]] = getEdgeOffsetNormalByIndex(node, femask[i][1], norms[fmask[i][1]]);
1077                                 parity[fmask[i][1]] = 1;
1078                         }
1079                 }
1080         }
1081
1082         // 3 edge adjacent cubes
1083         int emask[3] = {3, 7, 11};
1084         int eemask[3] = {0, 1, 2};
1085         for (i = 0; i < 3; i++)
1086         {
1087 //                      if(getEdgeParity(leaf, emask[i]))
1088                 {
1089                         int nst[3] = {st[0] + len, st[1] + len, st[2] + len};
1090                         nst[i] -= len;
1091                         // int nstt[3] = {1, 1, 1};
1092                         // nstt[i] -= 1;
1093                         LeafNode *node = locateLeafCheck(nst);
1094                         if (node == NULL)
1095                         {
1096                                 continue;
1097                         }
1098
1099                         if (getStoredEdgesParity(node, eemask[i]))
1100                         {
1101                                 pts[emask[i]] = getEdgeOffsetNormalByIndex(node, eemask[i], norms[emask[i]]);
1102                                 parity[emask[i]] = 1;
1103                         }
1104                 }
1105         }
1106 }
1107
1108
1109 /// Update method
1110 LeafNode *updateEdgeOffsetsNormals(LeafNode *leaf, int oldlen, int newlen, float offs[3], float a[3], float b[3], float c[3])
1111 {
1112         // First, create a new leaf node
1113         LeafNode *nleaf = createLeaf(newlen);
1114         *nleaf = *leaf;
1115
1116         // Next, fill in the offsets
1117         setEdgeOffsetsNormals(nleaf, offs, a, b, c, newlen);
1118
1119         // Finally, delete the old leaf
1120         removeLeaf(oldlen, leaf);
1121
1122         return nleaf;
1123 }
1124
1125 /// Locate a leaf
1126 /// WARNING: assuming this leaf already exists!
1127
1128 LeafNode *locateLeaf(int st[3])
1129 {
1130         Node *node = (Node *)root;
1131         for (int i = GRID_DIMENSION - 1; i > GRID_DIMENSION - maxDepth - 1; i--)
1132         {
1133                 int index = (((st[0] >> i) & 1) << 2) |
1134                             (((st[1] >> i) & 1) << 1) |
1135                             (((st[2] >> i) & 1));
1136                 node = getChild(&node->internal, getChildCount(&node->internal, index));
1137         }
1138
1139         return &node->leaf;
1140 }
1141
1142 LeafNode *locateLeaf(InternalNode *parent, int len, int st[3])
1143 {
1144         Node *node = (Node *)parent;
1145         int index;
1146         for (int i = len / 2; i >= mindimen; i >>= 1)
1147         {
1148                 index = (((st[0] & i) ? 4 : 0) |
1149                          ((st[1] & i) ? 2 : 0) |
1150                          ((st[2] & i) ? 1 : 0));
1151                 node = getChild(&node->internal,
1152                                 getChildCount(&node->internal, index));
1153         }
1154
1155         return &node->leaf;
1156 }
1157
1158 LeafNode *locateLeafCheck(int st[3])
1159 {
1160         Node *node = (Node *)root;
1161         for (int i = GRID_DIMENSION - 1; i > GRID_DIMENSION - maxDepth - 1; i--)
1162         {
1163                 int index = (((st[0] >> i) & 1) << 2) |
1164                             (((st[1] >> i) & 1) << 1) |
1165                             (((st[2] >> i) & 1));
1166                 if (!hasChild(&node->internal, index))
1167                 {
1168                         return NULL;
1169                 }
1170                 node = getChild(&node->internal, getChildCount(&node->internal, index));
1171         }
1172
1173         return &node->leaf;
1174 }
1175
1176 InternalNode *locateParent(int len, int st[3], int& count)
1177 {
1178         InternalNode *node = (InternalNode *)root;
1179         InternalNode *pre = NULL;
1180         int index = 0;
1181         for (int i = dimen / 2; i >= len; i >>= 1)
1182         {
1183                 index = (((st[0] & i) ? 4 : 0) |
1184                          ((st[1] & i) ? 2 : 0) |
1185                          ((st[2] & i) ? 1 : 0));
1186                 pre = node;
1187                 node = &getChild(node, getChildCount(node, index))->internal;
1188         }
1189
1190         count = getChildCount(pre, index);
1191         return pre;
1192 }
1193
1194 InternalNode *locateParent(InternalNode *parent, int len, int st[3], int& count)
1195 {
1196         InternalNode *node = parent;
1197         InternalNode *pre = NULL;
1198         int index = 0;
1199         for (int i = len / 2; i >= mindimen; i >>= 1)
1200         {
1201                 index = (((st[0] & i) ? 4 : 0) |
1202                          ((st[1] & i) ? 2 : 0) |
1203                          ((st[2] & i) ? 1 : 0));
1204                 pre = node;
1205                 node = (InternalNode *)getChild(node, getChildCount(node, index));
1206         }
1207
1208         count = getChildCount(pre, index);
1209         return pre;
1210 }
1211
1212 /************ Operators for internal nodes ************/
1213
1214 /// If child index exists
1215 int hasChild(InternalNode *node, int index)
1216 {
1217         return (node->has_child >> index) & 1;
1218 }
1219
1220 /// Test if child is leaf
1221 int isLeaf(InternalNode *node, int index)
1222 {
1223         return (node->child_is_leaf >> index) & 1;
1224 }
1225
1226 /// Get the pointer to child index
1227 Node *getChild(InternalNode *node, int count)
1228 {
1229         return node->children[count];
1230 };
1231
1232 /// Get total number of children
1233 int getNumChildren(InternalNode *node)
1234 {
1235         return numChildrenTable[node->has_child];
1236 }
1237
1238 /// Get the count of children
1239 int getChildCount(InternalNode *node, int index)
1240 {
1241         return childrenCountTable[node->has_child][index];
1242 }
1243 int getChildIndex(InternalNode *node, int count)
1244 {
1245         return childrenIndexTable[node->has_child][count];
1246 }
1247 int *getChildCounts(InternalNode *node)
1248 {
1249         return childrenCountTable[node->has_child];
1250 }
1251
1252 /// Get all children
1253 void fillChildren(InternalNode *node, Node *children[8], int leaf[8])
1254 {
1255         int count = 0;
1256         for (int i = 0; i < 8; i++)
1257         {
1258                 leaf[i] = isLeaf(node, i);
1259                 if (hasChild(node, i))
1260                 {
1261                         children[i] = getChild(node, count);
1262                         count++;
1263                 }
1264                 else {
1265                         children[i] = NULL;
1266                         leaf[i] = 0;
1267                 }
1268         }
1269 }
1270
1271 /// Sets the child pointer
1272 void setChild(InternalNode *node, int count, Node *chd)
1273 {
1274         node->children[count] = chd;
1275 }
1276 void setInternalChild(InternalNode *node, int index, int count, InternalNode *chd)
1277 {
1278         setChild(node, count, (Node *)chd);
1279         node->has_child |= (1 << index);
1280 }
1281 void setLeafChild(InternalNode *node, int index, int count, LeafNode *chd)
1282 {
1283         setChild(node, count, (Node *)chd);
1284         node->has_child |= (1 << index);
1285         node->child_is_leaf |= (1 << index);
1286 }
1287
1288 /// Add a kid to an existing internal node
1289 /// Fix me: can we do this without wasting memory ?
1290 /// Fixed: using variable memory
1291 InternalNode *addChild(InternalNode *node, int index, Node *child, int aLeaf)
1292 {
1293         // Create new internal node
1294         int num = getNumChildren(node);
1295         InternalNode *rnode = createInternal(num + 1);
1296
1297         // Establish children
1298         int i;
1299         int count1 = 0, count2 = 0;
1300         for (i = 0; i < 8; i++)
1301         {
1302                 if (i == index)
1303                 {
1304                         if (aLeaf)
1305                         {
1306                                 setLeafChild(rnode, i, count2, &child->leaf);
1307                         }
1308                         else {
1309                                 setInternalChild(rnode, i, count2, &child->internal);
1310                         }
1311                         count2++;
1312                 }
1313                 else if (hasChild(node, i))
1314                 {
1315                         if (isLeaf(node, i))
1316                         {
1317                                 setLeafChild(rnode, i, count2, &getChild(node, count1)->leaf);
1318                         }
1319                         else {
1320                                 setInternalChild(rnode, i, count2, &getChild(node, count1)->internal);
1321                         }
1322                         count1++;
1323                         count2++;
1324                 }
1325         }
1326
1327         removeInternal(num, node);
1328         return rnode;
1329 }
1330
1331 /// Allocate a node
1332 InternalNode *createInternal(int length)
1333 {
1334         InternalNode *inode = (InternalNode *)alloc[length]->allocate();
1335         inode->has_child = 0;
1336         inode->child_is_leaf = 0;
1337         return inode;
1338 }
1339
1340 LeafNode *createLeaf(int length)
1341 {
1342         assert(length <= 3);
1343
1344         LeafNode *lnode = (LeafNode *)leafalloc[length]->allocate();
1345         lnode->edge_parity = 0;
1346         lnode->primary_edge_intersections = 0;
1347         lnode->signs = 0;
1348
1349         return lnode;
1350 }
1351
1352 void removeInternal(int num, InternalNode *node)
1353 {
1354         alloc[num]->deallocate(node);
1355 }
1356
1357 void removeLeaf(int num, LeafNode *leaf)
1358 {
1359         assert(num >= 0 && num <= 3);
1360         leafalloc[num]->deallocate(leaf);
1361 }
1362
1363 /// Add a leaf (by creating a new par node with the leaf added)
1364 InternalNode *addLeafChild(InternalNode *par, int index, int count,
1365                            LeafNode *leaf)
1366 {
1367         int num = getNumChildren(par) + 1;
1368         InternalNode *npar = createInternal(num);
1369         *npar = *par;
1370
1371         if (num == 1)
1372         {
1373                 setLeafChild(npar, index, 0, leaf);
1374         }
1375         else {
1376                 int i;
1377                 for (i = 0; i < count; i++)
1378                 {
1379                         setChild(npar, i, getChild(par, i));
1380                 }
1381                 setLeafChild(npar, index, count, leaf);
1382                 for (i = count + 1; i < num; i++)
1383                 {
1384                         setChild(npar, i, getChild(par, i - 1));
1385                 }
1386         }
1387
1388         removeInternal(num - 1, par);
1389         return npar;
1390 }
1391
1392 InternalNode *addInternalChild(InternalNode *par, int index, int count,
1393                                InternalNode *node)
1394 {
1395         int num = getNumChildren(par) + 1;
1396         InternalNode *npar = createInternal(num);
1397         *npar = *par;
1398
1399         if (num == 1)
1400         {
1401                 setInternalChild(npar, index, 0, node);
1402         }
1403         else {
1404                 int i;
1405                 for (i = 0; i < count; i++)
1406                 {
1407                         setChild(npar, i, getChild(par, i));
1408                 }
1409                 setInternalChild(npar, index, count, node);
1410                 for (i = count + 1; i < num; i++)
1411                 {
1412                         setChild(npar, i, getChild(par, i - 1));
1413                 }
1414         }
1415
1416         removeInternal(num - 1, par);
1417         return npar;
1418 }
1419 };
1420
1421 #endif