fix voxel system to send new bistream format, add helpers to VoxelNode class

shared/src/VoxelNode.cpp

@@ -6,7 +6,9 @@
 //
 //
 
+#include "SharedUtil.h"
 #include "VoxelNode.h"
+#include "OctalCode.h"
 
 VoxelNode::VoxelNode() {
     
@@ -17,4 +19,62 @@ VoxelNode::VoxelNode() {
     for (int i = 0; i < 8; i++) {
         children[i] = NULL;
     }
+}
+
+VoxelNode::~VoxelNode() {
+    delete[] octalCode;
+    
+    // delete all of this node's children
+    for (int i = 0; i < 8; i++) {
+        delete children[i];
+    }
+}
+
+void VoxelNode::addChildAtIndex(int8_t childIndex) {
+    children[childIndex] = new VoxelNode();
+    
+    // give this child its octal code
+    children[childIndex]->octalCode = childOctalCode(octalCode, childIndex);
+}
+
+void VoxelNode::setColorFromAverageOfChildren(int * colorArray) {
+    if (colorArray == NULL) {
+        colorArray = new int[4];
+        memset(colorArray, 0, 4);
+        
+        for (int i = 0; i < 8; i++) {
+            if (children[i] != NULL && children[i]->color[3] == 1) {
+                for (int j = 0; j < 3; j++) {
+                    colorArray[j] += children[i]->color[j];
+                }
+                
+                colorArray[3]++;
+            }
+        }
+    }
+    
+    if (colorArray[3] > 4) {
+        // we need at least 4 colored children to have an average color value
+        // or if we have none we generate random values
+        
+        for (int c = 0; c < 3; c++) {
+            // set the average color value
+            color[c] = colorArray[c] / colorArray[3];
+        }
+        
+        // set the alpha to 1 to indicate that this isn't transparent
+        color[3] = 1;
+    } else {
+        // some children, but not enough
+        // set this node's alpha to 0
+        color[3] = 0;
+    }
+}
+
+void VoxelNode::setRandomColor(int minimumBrightness) {
+    for (int c = 0; c < 3; c++) {
+        color[c] = randomColorValue(minimumBrightness);
+    }
+    
+    color[3] = 1;
 }

shared/src/VoxelNode.h

@@ -14,6 +14,11 @@
 class VoxelNode {
 public:
     VoxelNode();
+    ~VoxelNode();
+    
+    void addChildAtIndex(int8_t childIndex);
+    void setColorFromAverageOfChildren(int * colorArray = NULL);
+    void setRandomColor(int minimumBrightness);
     
     unsigned char *octalCode;
     unsigned char color[4];

shared/src/VoxelTree.cpp

@@ -6,6 +6,7 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 //
 
+#include "SharedUtil.h"
 #include "OctalCode.h"
 #include "VoxelTree.h"
 
@@ -17,69 +18,180 @@ VoxelTree::VoxelTree() {
     *rootNode->octalCode = (char)0;
 }
 
-unsigned char * VoxelTree::loadBitstreamBuffer(char *& bitstreamBuffer,
+VoxelTree::~VoxelTree() {
+    // delete the children of the root node
+    // this recursively deletes the tree
+    for (int i = 0; i < 8; i++) {
+        delete rootNode->children[i];
+    }
+}
+
+VoxelNode * VoxelTree::nodeForOctalCode(VoxelNode *ancestorNode, unsigned char * needleCode) {
+    // find the appropriate branch index based on this ancestorNode
+    if (*needleCode == 0) {
+        return ancestorNode;
+    } else if (ancestorNode->childMask != 0) {
+        int8_t branchForNeedle = branchIndexWithDescendant(ancestorNode->octalCode, needleCode);
+        VoxelNode *childNode = ancestorNode->children[branchForNeedle];
+        
+        if (childNode != NULL) {
+            if (*childNode->octalCode == *needleCode) {
+                // the fact that the number of sections is equivalent does not always guarantee
+                // that this is the same node, however due to the recursive traversal
+                // we know that this is our node
+                return childNode;
+            } else {
+                // we need to go deeper
+                return nodeForOctalCode(childNode, needleCode);
+            }
+        }
+    }
+    
+    // we've been given a code we don't have a node for
+    // return this node as the last created parent
+    return ancestorNode;
+}
+
+VoxelNode * VoxelTree::createMissingNode(VoxelNode *lastParentNode, unsigned char *codeToReach) {
+    uint8_t indexOfNewChild = branchIndexWithDescendant(lastParentNode->octalCode, codeToReach);
+    lastParentNode->addChildAtIndex(indexOfNewChild);
+    
+    if (*lastParentNode->children[indexOfNewChild]->octalCode == *codeToReach) {
+        return lastParentNode;
+    } else {
+        return createMissingNode(lastParentNode->children[indexOfNewChild], codeToReach);
+    }
+}
+
+int VoxelTree::readNodeData(VoxelNode *destinationNode, unsigned char * nodeData, int bytesLeftToRead) {
+    
+    // instantiate variable for bytes already read
+    int bytesRead = 1;
+    int colorArray[4] = {};
+    
+    for (int i = 0; i < 8; i++) {
+        // check the colors mask to see if we have a child to color in
+        if (oneAtBit(*nodeData, i)) {
+            printf("Adding child with color at index %d\n", i);
+            
+            // create the child if it doesn't exist
+            if (destinationNode->children[i] == NULL) {
+                destinationNode->addChildAtIndex(i);
+            }
+            
+            // pull the color for this child
+            memcpy(destinationNode->children[i]->color, nodeData + bytesRead, 3);
+            destinationNode->children[i]->color[3] = 1;
+           
+            for (int j = 0; j < 3; j++) {
+                colorArray[j] += destinationNode->children[i]->color[j];
+            }
+            
+            bytesRead += 3;
+            colorArray[3]++;
+        }
+    }
+    
+    // average node's color based on color of children
+    destinationNode->setColorFromAverageOfChildren(colorArray);
+    
+    // give this destination node the child mask from the packet
+    printf("The child mask is\n");
+    outputBits(*(nodeData + bytesRead));
+    printf("\n");
+    destinationNode->childMask = *(nodeData + bytesRead);
+    
+    int childIndex = 0;
+    bytesRead++;
+    
+    while (bytesLeftToRead - bytesRead > 0 && childIndex < 8) {
+        // check the exists mask to see if we have a child to traverse into
+        
+        if (oneAtBit(destinationNode->childMask, childIndex)) {            
+            if (destinationNode->children[childIndex] == NULL) {
+                // add a child at that index, if it doesn't exist
+                destinationNode->addChildAtIndex(childIndex);
+            }
+            
+            // tell the child to read the subsequent data
+            bytesRead += readNodeData(destinationNode->children[childIndex], nodeData + bytesRead, bytesLeftToRead - bytesRead);
+        }
+        
+        childIndex++;
+    }
+    
+    return bytesRead;
+}
+
+void VoxelTree::readBitstreamToTree(unsigned char * bitstream, int bufferSizeBytes) {
+    VoxelNode *bitstreamRootNode = nodeForOctalCode(rootNode, (unsigned char *)bitstream);
+    
+    if (*bitstream != *bitstreamRootNode->octalCode) {
+        // if the octal code returned is not on the same level as
+        // the code being searched for, we have VoxelNodes to create
+        bitstreamRootNode = createMissingNode(bitstreamRootNode, (unsigned char *)bitstream);
+    }
+    
+    int octalCodeBytes = bytesRequiredForCodeLength(*bitstream);
+    readNodeData(bitstreamRootNode, bitstream + octalCodeBytes, bufferSizeBytes - octalCodeBytes);
+}
+
+unsigned char * VoxelTree::loadBitstreamBuffer(unsigned char *& bitstreamBuffer,
                                                unsigned char * stopOctalCode,
                                                VoxelNode *currentVoxelNode)
 {
-    static char *initialBitstreamPos = bitstreamBuffer;
+    static unsigned char *initialBitstreamPos = bitstreamBuffer;
     
-    char firstIndexToCheck = 0;
+    uint8_t firstIndexToCheck = 0;
     
     // we'll only be writing data if we're lower than
     // or at the same level as the stopOctalCode
     if (*currentVoxelNode->octalCode >= *stopOctalCode) {
-        if (currentVoxelNode->childMask != 0) {
-            if ((bitstreamBuffer - initialBitstreamPos) + MAX_TREE_SLICE_BYTES > MAX_VOXEL_PACKET_SIZE) {
-                // we can't send this packet, not enough room
-                // return our octal code as the stop
-                return currentVoxelNode->octalCode;
-            }
-            
-            if (strcmp((char *)stopOctalCode, (char *)currentVoxelNode->octalCode) == 0) {
-                // this is is the root node for this packet
-                // add the leading V
-                *(bitstreamBuffer++) = 'V';
-                
-                // add its octal code to the packet
-                int octalCodeBytes = bytesRequiredForCodeLength(*currentVoxelNode->octalCode);
-                
-                memcpy(bitstreamBuffer, currentVoxelNode->octalCode, octalCodeBytes);
-                bitstreamBuffer += octalCodeBytes;
-            }
-            
-            // default color mask is 0, increment pointer for colors
-            *bitstreamBuffer = 0;
-            
-            // keep a colorPointer so we can check how many colors were added
-            char *colorPointer = bitstreamBuffer + 1;
-            
-            for (int i = 0; i < 8; i++) {
-                
-                // check if the child exists and is not transparent
-                if (currentVoxelNode->children[i] != NULL
-                    && currentVoxelNode->children[i]->color[3] != 0) {
-                    
-                    // copy in the childs color to bitstreamBuffer
-                    memcpy(colorPointer, currentVoxelNode->children[i]->color, 3);
-                    colorPointer += 3;
-                    
-                    // set the colorMask by bitshifting the value of childExists
-                    *bitstreamBuffer += (1 << (7 - i));
-                }
-            }
-            
-            // push the bitstreamBuffer forwards for the number of added colors
-            bitstreamBuffer += (colorPointer - bitstreamBuffer);
-            
-            // copy the childMask to the current position of the bitstreamBuffer
-            // and push the buffer pointer forwards
-            
-            *(bitstreamBuffer++) = currentVoxelNode->childMask;
-        } else {
-            // if this node is a leaf, return a NULL stop code
-            // it has been visited
-            return NULL;
+        if ((bitstreamBuffer - initialBitstreamPos) + MAX_TREE_SLICE_BYTES > MAX_VOXEL_PACKET_SIZE) {
+            // we can't send this packet, not enough room
+            // return our octal code as the stop
+            return currentVoxelNode->octalCode;
         }
+        
+        if (strcmp((char *)stopOctalCode, (char *)currentVoxelNode->octalCode) == 0) {
+            // this is is the root node for this packet
+            // add the leading V
+            *(bitstreamBuffer++) = 'V';
+            
+            // add its octal code to the packet
+            int octalCodeBytes = bytesRequiredForCodeLength(*currentVoxelNode->octalCode);
+            
+            memcpy(bitstreamBuffer, currentVoxelNode->octalCode, octalCodeBytes);
+            bitstreamBuffer += octalCodeBytes;
+        }
+        
+        // default color mask is 0, increment pointer for colors
+        *bitstreamBuffer = 0;
+        
+        // keep a colorPointer so we can check how many colors were added
+        unsigned char *colorPointer = bitstreamBuffer + 1;
+        
+        for (int i = 0; i < 8; i++) {
+            
+            // check if the child exists and is not transparent
+            if (currentVoxelNode->children[i] != NULL
+                && currentVoxelNode->children[i]->color[3] != 0) {
+                
+                // copy in the childs color to bitstreamBuffer
+                memcpy(colorPointer, currentVoxelNode->children[i]->color, 3);
+                colorPointer += 3;
+                
+                // set the colorMask by bitshifting the value of childExists
+                *bitstreamBuffer += (1 << (7 - i));
+            }
+        }
+        
+        // push the bitstreamBuffer forwards for the number of added colors
+        bitstreamBuffer += (colorPointer - bitstreamBuffer);
+        
+        // copy the childMask to the current position of the bitstreamBuffer
+        // and push the buffer pointer forwards
+        *(bitstreamBuffer++) = currentVoxelNode->childMask;
     } else {
         firstIndexToCheck = *stopOctalCode > 0
             ? branchIndexWithDescendant(currentVoxelNode->octalCode, stopOctalCode)
@@ -98,7 +210,11 @@ unsigned char * VoxelTree::loadBitstreamBuffer(char *& bitstreamBuffer,
                 && childStopOctalCode == NULL) {
                 return currentVoxelNode->children[i]->octalCode;
             } else {
-                childStopOctalCode = loadBitstreamBuffer(bitstreamBuffer, stopOctalCode, currentVoxelNode->children[i]);
+                if (oneAtBit(currentVoxelNode->childMask, i)) {
+                    childStopOctalCode = loadBitstreamBuffer(bitstreamBuffer, stopOctalCode, currentVoxelNode->children[i]);
+                } else {
+                    childStopOctalCode = NULL;
+                }
             }
         }
         
@@ -107,5 +223,37 @@ unsigned char * VoxelTree::loadBitstreamBuffer(char *& bitstreamBuffer,
         }
     }
     
-    return childStopOctalCode;    
+    return childStopOctalCode;
+}
+
+void VoxelTree::printTreeForDebugging(VoxelNode *startNode) {
+    uint8_t colorMask = 0;
+    
+    // create the color mask
+    for (int i = 0; i < 8; i++) {
+        if (startNode->children[i] != NULL && startNode->children[i]->color[3] != 0) {
+            colorMask += (1 << (7 - i));
+        }
+    }
+    
+    outputBits(colorMask);
+    
+    // output the colors we have
+    for (int j = 0; j < 8; j++) {
+        if (startNode->children[j] != NULL && startNode->children[j]->color[3] != 0) {
+            for (int c = 0; c < 3; c++) {
+                outputBits(startNode->children[j]->color[c]);
+            }
+        }
+    }
+    
+    outputBits(startNode->childMask);
+    
+    // ask children to recursively output their trees
+    // if they aren't a leaf
+    for (int k = 0; k < 8; k++) {
+        if (startNode->children[k] != NULL && oneAtBit(startNode->childMask, k)) {
+            printTreeForDebugging(startNode->children[k]);
+        }
+    }
 }

shared/src/VoxelTree.h

@@ -15,14 +15,20 @@
 const int MAX_VOXEL_PACKET_SIZE = 1492;
 
 class VoxelTree {
+    VoxelNode * nodeForOctalCode(VoxelNode *ancestorNode, unsigned char * needleCode);
+    VoxelNode * createMissingNode(VoxelNode *lastParentNode, unsigned char *deepestCodeToCreate);
+    int readNodeData(VoxelNode *destinationNode, unsigned char * nodeData, int bufferSizeBytes);
 public:
     VoxelTree();
+    ~VoxelTree();
     
     VoxelNode *rootNode;
     
-    unsigned char * loadBitstreamBuffer(char *& bitstreamBuffer,
+    void readBitstreamToTree(unsigned char * bitstream, int bufferSizeBytes);
+    unsigned char * loadBitstreamBuffer(unsigned char *& bitstreamBuffer,
                                unsigned char * octalCode,
                                VoxelNode *currentVoxelNode);
+    void printTreeForDebugging(VoxelNode *startNode);
     
 };
 

voxel/src/main.cpp

@@ -23,7 +23,7 @@
 #include <sys/time.h>
 #include <arpa/inet.h>
 #include <ifaddrs.h>
-#endif _WIN32
+#endif
 
 const int VOXEL_LISTEN_PORT = 40106;
 
@@ -46,19 +46,11 @@ char DOMAIN_HOSTNAME[] = "highfidelity.below92.com";
 char DOMAIN_IP[100] = "";    //  IP Address will be re-set by lookup on startup
 const int DOMAINSERVER_PORT = 40102;
 
-const int MAX_VOXEL_TREE_DEPTH_LEVELS = 5;
+const int MAX_VOXEL_TREE_DEPTH_LEVELS = 2;
 
 AgentList agentList(VOXEL_LISTEN_PORT);
 in_addr_t localAddress;
 
-unsigned char randomColorValue() {
-    return MIN_BRIGHTNESS + (rand() % (255 - MIN_BRIGHTNESS));
-}
-
-bool randomBoolean() {
-    return rand() % 2;
-}
-
 void *reportAliveToDS(void *args) {
     
     timeval lastSend;
@@ -82,14 +74,14 @@ void *reportAliveToDS(void *args) {
     }
 }
 
-void randomlyFillVoxelTree(int levelsToGo, VoxelNode *currentRootNode) {
+int randomlyFillVoxelTree(int levelsToGo, VoxelNode *currentRootNode) {
     // randomly generate children for this node
     // the first level of the tree (where levelsToGo = MAX_VOXEL_TREE_DEPTH_LEVELS) has all 8
     if (levelsToGo > 0) {
         
-        int coloredChildren = 0;
+        int grandChildrenFromNode = 0;
         bool createdChildren = false;
-        unsigned char sumColor[3] = {};
+        int colorArray[4] = {};
         
         createdChildren = false;
 
@@ -102,51 +94,39 @@ void randomlyFillVoxelTree(int levelsToGo, VoxelNode *currentRootNode) {
                 currentRootNode->children[i]->octalCode = childOctalCode(currentRootNode->octalCode, i);
                 
                 // fill out the lower levels of the tree using that node as the root node
-                randomlyFillVoxelTree(levelsToGo - 1, currentRootNode->children[i]);
+                grandChildrenFromNode = randomlyFillVoxelTree(levelsToGo - 1, currentRootNode->children[i]);
                 
-                if (currentRootNode->children[i]) {
+                if (currentRootNode->children[i]->color[3] == 1) {
                     for (int c = 0; c < 3; c++) {
-                        sumColor[c] += currentRootNode->children[i]->color[c];
+                        colorArray[c] += currentRootNode->children[i]->color[c];
                     }
                     
-                    coloredChildren++;
+                    colorArray[3]++;
+                }
+                
+                if (grandChildrenFromNode > 0) {
+                    currentRootNode->childMask += (1 << (7 - i));
                 }
                 
-                currentRootNode->childMask += (1 << (7 - i));
                 createdChildren = true;
             }
         }
         
-        // figure out the color value for this node
-        
-        if (coloredChildren > 4 || !createdChildren) {
-            // we need at least 4 colored children to have an average color value
-            // or if we have none we generate random values
-            
-            for (int c = 0; c < 3; c++) {
-                if (coloredChildren > 4) {
-                    // set the average color value
-                    currentRootNode->color[c] = sumColor[c] / coloredChildren;
-                } else {
-                    // we have no children, we're a leaf
-                    // generate a random color value
-                    currentRootNode->color[c] = randomColorValue();
-                }
-            }
-            
-            // set the alpha to 1 to indicate that this isn't transparent
-            currentRootNode->color[3] = 1;
+        if (!createdChildren) {
+            // we didn't create any children for this node, making it a leaf
+            // give it a random color
+            currentRootNode->setRandomColor(MIN_BRIGHTNESS);
         } else {
-            // some children, but not enough
-            // set this node's alpha to 0
-            currentRootNode->color[3] = 0;
+            // set the color value for this node
+            currentRootNode->setColorFromAverageOfChildren(colorArray);
         }
+        
+        return createdChildren;
     } else {
         // this is a leaf node, just give it a color
-        currentRootNode->color[0] = randomColorValue();
-        currentRootNode->color[1] = randomColorValue();
-        currentRootNode->color[2] = randomColorValue();
-        currentRootNode->color[3] = 1;
+        currentRootNode->setRandomColor(MIN_BRIGHTNESS);
+        
+        return 0;
     }
 }
 
@@ -196,8 +176,8 @@ int main(int argc, const char * argv[])
     // octal codes to the tree nodes that it is creating
     randomlyFillVoxelTree(MAX_VOXEL_TREE_DEPTH_LEVELS, randomTree.rootNode);
     
-    char *voxelPacket = new char[MAX_VOXEL_PACKET_SIZE];
-    char *voxelPacketEnd;
+    unsigned char *voxelPacket = new unsigned char[MAX_VOXEL_PACKET_SIZE];
+    unsigned char *voxelPacketEnd;
     
     unsigned char *stopOctal;
     int packetCount;
@@ -218,6 +198,10 @@ int main(int argc, const char * argv[])
                     voxelPacketEnd = voxelPacket;
                     stopOctal = randomTree.loadBitstreamBuffer(voxelPacketEnd, stopOctal, randomTree.rootNode);
                     
+                    agentList.getAgentSocket().send((sockaddr *)&agentPublicAddress,
+                                                    voxelPacket,
+                                                    voxelPacketEnd - voxelPacket);
+                    
                     printf("Packet %d sent to agent at address %s is %ld bytes\n",
                            ++packetCount,
                            inet_ntoa(agentPublicAddress.sin_addr),