From b459cbc8ea05d733bbc6a3b2c730619be6ff9898 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Wed, 8 May 2013 01:16:07 -0700
Subject: [PATCH] Fixed a couple bugs in pointToVoxel() - fixed a byte packing
 bug related to having exactly 7 bits of octal data   where we weren't
 correctly packing the last bit and moving the byte count   forward. This
 resulted in bogus colors being written - fixed a bug for points that exactly
 match an octet boundary test creating   voxels that are one a unit smaller
 than they should be

---
 libraries/shared/src/SharedUtil.cpp | 33 ++++++++++++++---------------
 1 file changed, 16 insertions(+), 17 deletions(-)

diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp
index 7c8f7f5e9b..ab445341fc 100644
--- a/libraries/shared/src/SharedUtil.cpp
+++ b/libraries/shared/src/SharedUtil.cpp
@@ -231,27 +231,26 @@ bool createVoxelEditMessage(unsigned char command, short int sequence,
 unsigned char* pointToVoxel(float x, float y, float z, float s, unsigned char r, unsigned char g, unsigned char b ) {
 
     float xTest, yTest, zTest, sTest; 
-    xTest = yTest = zTest = sTest = 0.5; 
+    xTest = yTest = zTest = sTest = 0.5f;
 
     // First determine the voxelSize that will properly encode a 
     // voxel of size S.
-    int voxelSizeInBits = 0;
+    unsigned int voxelSizeInOctets = 1;
     while (sTest > s) {
         sTest /= 2.0;
-        voxelSizeInBits+=3;
+        voxelSizeInOctets++;
     }
 
-	unsigned int voxelSizeInBytes = (voxelSizeInBits/8)+1;
-	unsigned int voxelSizeInOctets = (voxelSizeInBits/3);
-	unsigned int voxelBufferSize = voxelSizeInBytes+1+3; // 1 for size, 3 for color
+    unsigned int voxelSizeInBytes = bytesRequiredForCodeLength(voxelSizeInOctets); // (voxelSizeInBits/8)+1;
+    unsigned int voxelBufferSize = voxelSizeInBytes+3; // 3 for color
 
     // allocate our resulting buffer
     unsigned char* voxelOut = new unsigned char[voxelBufferSize];
-    
+
     // first byte of buffer is always our size in octets
     voxelOut[0]=voxelSizeInOctets;
 
-    sTest = 0.5;  // reset sTest so we can do this again.
+    sTest = 0.5f; // reset sTest so we can do this again.
 
     unsigned char byte = 0; // we will be adding coding bits here
     int bitInByteNDX = 0; // keep track of where we are in byte as we go
@@ -260,7 +259,7 @@ unsigned char* pointToVoxel(float x, float y, float z, float s, unsigned char r,
 
     // Now we actually fill out the voxel code
     while (octetsDone < voxelSizeInOctets) {
-        if (x > xTest) { 
+        if (x >= xTest) {
             //<write 1 bit>
             byte = (byte << 1) | true;
             xTest += sTest/2.0; 
@@ -272,14 +271,14 @@ unsigned char* pointToVoxel(float x, float y, float z, float s, unsigned char r,
         bitInByteNDX++;
         // If we've reached the last bit of the byte, then we want to copy this byte
         // into our buffer. And get ready to start on a new byte
-        if (bitInByteNDX > 7) {
+        if (bitInByteNDX == 8) {
             voxelOut[byteNDX]=byte;
             byteNDX++;
             bitInByteNDX=0;
             byte=0;
         }
 
-        if (y > yTest) { 
+        if (y >= yTest) {
             //<write 1 bit>
             byte = (byte << 1) | true;
             yTest += sTest/2.0; 
@@ -291,14 +290,14 @@ unsigned char* pointToVoxel(float x, float y, float z, float s, unsigned char r,
         bitInByteNDX++;
         // If we've reached the last bit of the byte, then we want to copy this byte
         // into our buffer. And get ready to start on a new byte
-        if (bitInByteNDX > 7) {
+        if (bitInByteNDX == 8) {
             voxelOut[byteNDX]=byte;
             byteNDX++;
             bitInByteNDX=0;
             byte=0;
         }
 
-        if (z > zTest) { 
+        if (z >= zTest) {
             //<write 1 bit>
             byte = (byte << 1) | true;
             zTest += sTest/2.0; 
@@ -310,7 +309,7 @@ unsigned char* pointToVoxel(float x, float y, float z, float s, unsigned char r,
         bitInByteNDX++;
         // If we've reached the last bit of the byte, then we want to copy this byte
         // into our buffer. And get ready to start on a new byte
-        if (bitInByteNDX > 7) {
+        if (bitInByteNDX == 8) {
             voxelOut[byteNDX]=byte;
             byteNDX++;
             bitInByteNDX=0;
@@ -323,13 +322,13 @@ unsigned char* pointToVoxel(float x, float y, float z, float s, unsigned char r,
 
     // If we've got here, and we didn't fill the last byte, we need to zero pad this
     // byte before we copy it into our buffer.
-    if (bitInByteNDX > 0 && bitInByteNDX < 7) {
+    if (bitInByteNDX > 0 && bitInByteNDX < 8) {
         // Pad the last byte
-        while (bitInByteNDX <= 7) {
+        while (bitInByteNDX < 8) {
             byte = (byte << 1) | false;
             bitInByteNDX++;
         }
-        
+    
         // Copy it into our output buffer
         voxelOut[byteNDX]=byte;
         byteNDX++;