Merge branch 'master' of https://github.com/worklist/hifi

interface/src/Avatar.cpp

@@ -266,21 +266,15 @@ void Avatar::reset() {
 
 
 //  Update avatar head rotation with sensor data
-void Avatar::updateHeadFromGyros(float frametime, SerialInterface* serialInterface, glm::vec3* gravity) {
+void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterface, glm::vec3* gravity) {
     float measuredPitchRate = 0.0f;
     float measuredRollRate = 0.0f;
     float measuredYawRate = 0.0f;
     
-    if (serialInterface->active && USING_INVENSENSE_MPU9150) {
+    measuredPitchRate = serialInterface->getLastPitchRate();
-        measuredPitchRate = serialInterface->getLastPitchRate();
+    measuredYawRate = serialInterface->getLastYawRate();
-        measuredYawRate = serialInterface->getLastYawRate();
+    measuredRollRate = serialInterface->getLastRollRate();
-        measuredRollRate = serialInterface->getLastRollRate();
+   
-    } else {
-        measuredPitchRate = serialInterface->getRelativeValue(HEAD_PITCH_RATE);
-        measuredYawRate = serialInterface->getRelativeValue(HEAD_YAW_RATE);
-        measuredRollRate = serialInterface->getRelativeValue(HEAD_ROLL_RATE);
-    }
-    
     //  Update avatar head position based on measured gyro rates
     const float MAX_PITCH = 45;
     const float MIN_PITCH = -45;
@@ -289,13 +283,34 @@ void Avatar::updateHeadFromGyros(float frametime, SerialInterface* serialInterfa
     const float MAX_ROLL = 50;
     const float MIN_ROLL = -50;
     
-    addHeadPitch(measuredPitchRate * frametime);
+    addHeadPitch(measuredPitchRate * deltaTime);
-    addHeadYaw(measuredYawRate * frametime);
+    addHeadYaw(measuredYawRate * deltaTime);
-    addHeadRoll(measuredRollRate * frametime);
+    addHeadRoll(measuredRollRate * deltaTime);
     
     setHeadPitch(glm::clamp(getHeadPitch(), MIN_PITCH, MAX_PITCH));
     setHeadYaw(glm::clamp(getHeadYaw(), MIN_YAW, MAX_YAW));
     setHeadRoll(glm::clamp(getHeadRoll(), MIN_ROLL, MAX_ROLL));
+    
+    //  Update head lean distance based on accelerometer data
+    const float LEAN_SENSITIVITY = 0.15;
+    const float LEAN_MAX = 0.45;
+    const float LEAN_AVERAGING = 10.0;
+    glm::vec3 headRotationRates(getHeadPitch(), getHeadYaw(), getHeadRoll());
+    float headRateMax = 50.f;
+    
+    
+    glm::vec3 leaning = (serialInterface->getLastAcceleration() -  serialInterface->getGravity())
+                        * LEAN_SENSITIVITY
+                        * (1.f - fminf(glm::length(headRotationRates), headRateMax) / headRateMax);
+    leaning.y = 0.f;
+    if (glm::length(leaning) < LEAN_MAX) {
+        _head.leanForward = _head.leanForward * (1.f - LEAN_AVERAGING * deltaTime) +
+                                (LEAN_AVERAGING * deltaTime) * leaning.z * LEAN_SENSITIVITY;
+        _head.leanSideways = _head.leanSideways * (1.f - LEAN_AVERAGING * deltaTime) +
+                                (LEAN_AVERAGING * deltaTime) * leaning.x * LEAN_SENSITIVITY;
+    }
+    setHeadLeanSideways(_head.leanSideways);
+    setHeadLeanForward(_head.leanForward); 
 }
 
 float Avatar::getAbsoluteHeadYaw() const {
@@ -329,6 +344,8 @@ void Avatar::simulate(float deltaTime) {
     // update balls
     if (_balls) { _balls->simulate(deltaTime); }
     
+    // if other avatar, update head position from network data
+    
 	// update avatar skeleton
 	updateSkeleton();
 	
@@ -517,14 +534,12 @@ void Avatar::updateHandMovementAndTouching(float deltaTime) {
 
 void Avatar::updateHead(float deltaTime) {
 
-    // hold on to this - used for testing....
+    // Get head position data from network for other people
-    /*
+    if (!_isMine) {
-    static float test = 0.0f;
+        _head.leanSideways = getHeadLeanSideways();
-    test += deltaTime;
+        _head.leanForward = getHeadLeanForward(); 
-    _head.leanForward  = 0.02 * sin( test * 0.2f );
+    }
-    _head.leanSideways = 0.02 * sin( test * 0.3f );
+    
-    */
-
     //apply the head lean values to the springy position...
     if (fabs(_head.leanSideways + _head.leanForward) > 0.0f) {
         glm::vec3 headLean = 
@@ -551,7 +566,7 @@ void Avatar::updateHead(float deltaTime) {
     }
 
     //  Decay head back to center if turned on
-    if (_returnHeadToCenter) {
+    if (_isMine && _returnHeadToCenter) {
         //  Decay back toward center
         _headPitch *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime);
         _headYaw   *= (1.0f - DECAY * _head.returnSpringScale * 2 * deltaTime);
@@ -559,15 +574,14 @@ void Avatar::updateHead(float deltaTime) {
     }
     
     //  For invensense gyro, decay only slightly when roughly centered
-    if (USING_INVENSENSE_MPU9150) {
+    if (_isMine) {
-        const float RETURN_RANGE = 5.0;
+        const float RETURN_RANGE = 15.0;
-        const float RETURN_STRENGTH = 1.0;
+        const float RETURN_STRENGTH = 2.0;
         if (fabs(_headPitch) < RETURN_RANGE) { _headPitch *= (1.0f - RETURN_STRENGTH * deltaTime); }
         if (fabs(_headYaw) < RETURN_RANGE) { _headYaw *= (1.0f - RETURN_STRENGTH * deltaTime); }
         if (fabs(_headRoll) < RETURN_RANGE) { _headRoll *= (1.0f - RETURN_STRENGTH * deltaTime); }
-
     }
-    
+
     if (_head.noise) {
         //  Move toward new target
         _headPitch += (_head.pitchTarget - _headPitch) * 10 * deltaTime; // (1.f - DECAY*deltaTime)*Pitch + ;

interface/src/SerialInterface.cpp

@@ -29,7 +29,7 @@ int serialBufferPos = 0;
 const int ZERO_OFFSET = 2048;
 const short NO_READ_MAXIMUM_MSECS = 3000;
 const short SAMPLES_TO_DISCARD = 100;               //  Throw out the first few samples
-const int GRAVITY_SAMPLES = 200;                    //  Use the first samples to compute gravity vector
+const int GRAVITY_SAMPLES = 60;                     //  Use the first samples to compute gravity vector
 
 const bool USING_INVENSENSE_MPU9150 = 1;
 
@@ -131,42 +131,11 @@ void SerialInterface::initializePort(char* portname, int baud) {
  #endif
 }
 
-//  Reset Trailing averages to the current measurement
-void SerialInterface::resetTrailingAverages() {
-    for (int i = 1; i < NUM_CHANNELS; i++)  trailingAverage[i] = lastMeasured[i];
-}
-
 //  Render the serial interface channel values onscreen as vertical lines
 void SerialInterface::renderLevels(int width, int height) {
-    int i;
-    int disp_x = 10;
-    const int GAP = 16;
     char val[40];
-    if (!USING_INVENSENSE_MPU9150) {
+    if (USING_INVENSENSE_MPU9150) {
-        for(i = 0; i < NUM_CHANNELS; i++) {
+        //  For invensense gyros, render as horizontal bars
-            //  Actual value
-            glLineWidth(2.0);
-            glColor4f(1, 1, 1, 1);
-            glBegin(GL_LINES);
-            glVertex2f(disp_x, height * 0.95);
-            glVertex2f(disp_x, height * (0.25 + 0.75f * getValue(i) / 4096));
-            glColor4f(1, 0, 0, 1);
-            glVertex2f(disp_x - 3, height * (0.25 + 0.75f * getValue(i) / 4096));
-            glVertex2f(disp_x, height * (0.25 + 0.75f * getValue(i) / 4096));
-            glEnd();
-            //  Trailing Average value
-            glBegin(GL_LINES);
-            glColor4f(1, 1, 1, 1);
-            glVertex2f(disp_x, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
-            glVertex2f(disp_x + 4, height * (0.25 + 0.75f * getTrailingValue(i) / 4096));
-            glEnd();
-            
-            sprintf(val, "%d", getValue(i));
-            drawtext(disp_x - GAP / 2, (height * 0.95) + 2, 0.08, 90, 1.0, 0, val, 0, 1, 0);
-            
-            disp_x += GAP;
-        }
-    } else {
         const int LEVEL_CORNER_X = 10;
         const int LEVEL_CORNER_Y = 200;
         
@@ -177,18 +146,37 @@ void SerialInterface::renderLevels(int width, int height) {
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 15, 0.10, 0, 1.0, 1, val, 0, 1, 0);
         sprintf(val, "Roll  %4.1f", _lastRollRate);
         drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 30, 0.10, 0, 1.0, 1, val, 0, 1, 0);
+        sprintf(val, "X     %4.3f", _lastAccelX);
+        drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 45, 0.10, 0, 1.0, 1, val, 0, 1, 0);
+        sprintf(val, "Y     %4.3f", _lastAccelY);
+        drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 60, 0.10, 0, 1.0, 1, val, 0, 1, 0);
+        sprintf(val, "Z     %4.3f", _lastAccelZ);
+        drawtext(LEVEL_CORNER_X, LEVEL_CORNER_Y + 75, 0.10, 0, 1.0, 1, val, 0, 1, 0);
         
         //  Draw the levels as horizontal lines        
         const int LEVEL_CENTER = 150;
+        const float ACCEL_VIEW_SCALING = 50.f;
         glLineWidth(2.0);
         glColor4f(1, 1, 1, 1);
         glBegin(GL_LINES);
+        // Gyro rates
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y - 3);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastYawRate, LEVEL_CORNER_Y - 3);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 12);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastPitchRate, LEVEL_CORNER_Y + 12);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 27);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + _lastRollRate, LEVEL_CORNER_Y + 27);
+        // Acceleration
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelX - _gravity.x)* ACCEL_VIEW_SCALING),
+                   LEVEL_CORNER_Y + 42);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 57);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelY - _gravity.y) * ACCEL_VIEW_SCALING),
+                   LEVEL_CORNER_Y + 57);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 72);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAccelZ - _gravity.z) * ACCEL_VIEW_SCALING),
+                   LEVEL_CORNER_Y + 72);
+
         glEnd();
         //  Draw green vertical centerline
         glColor4f(0, 1, 0, 0.5);
@@ -197,18 +185,6 @@ void SerialInterface::renderLevels(int width, int height) {
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 30);
         glEnd();
     }
-    
-    //  Display Serial latency block
-    if (LED) {
-        glColor3f(1,0,0);
-        glBegin(GL_QUADS); {
-            glVertex2f(width - 100, height - 100);
-            glVertex2f(width, height - 100);
-            glVertex2f(width, height);
-            glVertex2f(width - 100, height);
-        }
-        glEnd();
-    }
 }
 
 void convertHexToInt(unsigned char* sourceBuffer, int& destinationInt) {
@@ -237,15 +213,17 @@ void SerialInterface::readData() {
         
         int accelXRate, accelYRate, accelZRate;
         
-        convertHexToInt(sensorBuffer + 6, accelXRate); 
+        convertHexToInt(sensorBuffer + 6, accelZRate);
         convertHexToInt(sensorBuffer + 10, accelYRate);
-        convertHexToInt(sensorBuffer + 14, accelZRate);
+        convertHexToInt(sensorBuffer + 14, accelXRate);
         
-        const float LSB_TO_METERS_PER_SECOND = 1.f / 16384.f;
+        const float LSB_TO_METERS_PER_SECOND2 = 1.f / 16384.f * 9.80665f;
+                                                                //  From MPU-9150 register map, with setting on
+                                                                //  highest resolution = +/- 2G
         
-        _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND;
+        _lastAccelX = ((float) accelXRate) * LSB_TO_METERS_PER_SECOND2;
-        _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND;
+        _lastAccelY = ((float) accelYRate) * LSB_TO_METERS_PER_SECOND2;
-        _lastAccelZ = ((float) accelZRate) * LSB_TO_METERS_PER_SECOND;
+        _lastAccelZ = ((float) -accelZRate) * LSB_TO_METERS_PER_SECOND2;
         
         int rollRate, yawRate, pitchRate;
         
@@ -262,61 +240,20 @@ void SerialInterface::readData() {
         _lastYawRate = ((float) yawRate) * LSB_TO_DEGREES_PER_SECOND;
         _lastPitchRate = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND + PITCH_BIAS;
         
-        totalSamples++;
+        //  Accumulate an initial reading for gravity
-    } else {
+        //  Use a set of initial samples to compute gravity
-        //  This array sets the rate of trailing averaging for each channel:
+        if (totalSamples < GRAVITY_SAMPLES) {
-        //  If the sensor rate is 100Hz, 0.001 will make the long term average a 10-second average
+            _gravity.x += _lastAccelX;
-        const float AVG_RATE[] =  {0.002, 0.002, 0.002, 0.002, 0.002, 0.002};
+            _gravity.y += _lastAccelY;
-        char bufchar[1];
+            _gravity.z += _lastAccelZ;
-        
-        while (read(_serialDescriptor, &bufchar, 1) > 0) {
-            serialBuffer[serialBufferPos] = bufchar[0];
-            serialBufferPos++;
-            //  Have we reached end of a line of input?
-            if ((bufchar[0] == '\n') || (serialBufferPos >= MAX_BUFFER)) {
-                std::string serialLine(serialBuffer, serialBufferPos-1);
-                //printLog("%s\n", serialLine.c_str());
-                int spot;
-                //int channel = 0;
-                std::string val;
-                for (int i = 0; i < NUM_CHANNELS + 2; i++) {
-                    spot = serialLine.find_first_of(" ", 0);
-                    if (spot != std::string::npos) {
-                        val = serialLine.substr(0,spot);
-                        //printLog("%s\n", val.c_str());
-                        if (i < NUM_CHANNELS) lastMeasured[i] = atoi(val.c_str());
-                        else samplesAveraged = atoi(val.c_str());
-                    } else LED = atoi(serialLine.c_str());
-                    serialLine = serialLine.substr(spot+1, serialLine.length() - spot - 1);
-                }
-                
-                //  Update Trailing Averages
-                for (int i = 0; i < NUM_CHANNELS; i++) {
-                    if (totalSamples > SAMPLES_TO_DISCARD) {
-                        trailingAverage[i] = (1.f - AVG_RATE[i])*trailingAverage[i] +
-                        AVG_RATE[i]*(float)lastMeasured[i];
-                    } else {
-                        trailingAverage[i] = (float)lastMeasured[i];
-                    }
-                    
-                }
-                
-                //  Use a set of initial samples to compute gravity
-                if (totalSamples < GRAVITY_SAMPLES) {
-                    gravity.x += lastMeasured[ACCEL_X];
-                    gravity.y += lastMeasured[ACCEL_Y];
-                    gravity.z += lastMeasured[ACCEL_Z];
-                }
-                if (totalSamples == GRAVITY_SAMPLES) {
-                    gravity = glm::normalize(gravity);
-                    printLog("gravity: %f,%f,%f\n", gravity.x, gravity.y, gravity.z);
-                }
-                
-                totalSamples++;
-                serialBufferPos = 0;
-            }
         }
-    }
+        if (totalSamples == GRAVITY_SAMPLES) {
+            _gravity /= (float) totalSamples;
+            printLog("Gravity: %f\n", glm::length(_gravity));
+        }
+
+        totalSamples++;
+    } 
     
     if (initialSamples == totalSamples) {        
         timeval now;
@@ -336,23 +273,10 @@ void SerialInterface::resetSerial() {
 #ifdef __APPLE__
     active = false;
     totalSamples = 0;
+    _gravity = glm::vec3(0, 0, 0);
     
     gettimeofday(&lastGoodRead, NULL);
     
-    if (!USING_INVENSENSE_MPU9150) {
-        gravity = glm::vec3(0, -1, 0);
-        
-        //  Clear the measured and average channel data
-        for (int i = 0; i < NUM_CHANNELS; i++) {
-            lastMeasured[i] = 0;
-            trailingAverage[i] = 0.0;
-        }
-        //  Clear serial input buffer
-        for (int i = 1; i < MAX_BUFFER; i++) {
-            serialBuffer[i] = ' ';
-        }
-    }
-    
 #endif
 }
 

interface/src/SerialInterface.h

@@ -32,7 +32,7 @@
 #define HEAD_YAW_RATE 0
 #define HEAD_ROLL_RATE 2
 
-extern const bool USING_INVENSENSE_MPU9150;
+//const bool USING_INVENSENSE_MPU9150;
 
 class SerialInterface {
 public:
@@ -50,30 +50,20 @@ public:
     float getLastYawRate() const { return _lastYawRate; }
     float getLastPitchRate() const { return _lastPitchRate; }
     float getLastRollRate() const { return _lastRollRate; }
+    glm::vec3 getLastAcceleration() { return glm::vec3(_lastAccelX, _lastAccelY, _lastAccelZ); };
+    glm::vec3 getGravity() {return _gravity;};
     
-    int getLED() {return LED;};
-    int getNumSamples() {return samplesAveraged;};
-    int getValue(int num) {return lastMeasured[num];};
-    int getRelativeValue(int num) {return static_cast<int>(lastMeasured[num] - trailingAverage[num]);};
-    float getTrailingValue(int num) {return trailingAverage[num];};
-    
-    void resetTrailingAverages();
     void renderLevels(int width, int height);
     bool active;
-    glm::vec3 getGravity() {return gravity;};
     
 private:
     void initializePort(char* portname, int baud);
     void resetSerial();
 
     int _serialDescriptor;
-    int lastMeasured[NUM_CHANNELS];
-    float trailingAverage[NUM_CHANNELS];
-    int samplesAveraged;
-    int LED;
     int totalSamples;
     timeval lastGoodRead;
-    glm::vec3 gravity;
+    glm::vec3 _gravity;
     float _lastAccelX;
     float _lastAccelY;
     float _lastAccelZ;

interface/src/VoxelSystem.cpp

@@ -43,7 +43,7 @@ GLubyte identityIndices[] = { 0,2,1,    0,3,2,    // Z- .
 
 VoxelSystem::VoxelSystem() {
     _voxelsInReadArrays = _voxelsInWriteArrays = _voxelsUpdated = 0;
-    _alwaysRenderFullVBO = true;
+    _renderFullVBO = true;
     _tree = new VoxelTree();
     pthread_mutex_init(&_bufferWriteLock, NULL);
 }
@@ -138,8 +138,9 @@ void VoxelSystem::setupNewVoxelsForDrawing() {
     PerformanceWarning warn(_renderWarningsOn, "setupNewVoxelsForDrawing()"); // would like to include _voxelsInArrays, _voxelsUpdated
     double start = usecTimestampNow();
     double sinceLastTime = (start - _setupNewVoxelsForDrawingLastFinished) / 1000.0;
-    
+
-    if (sinceLastTime <= std::max(_setupNewVoxelsForDrawingLastElapsed, SIXTY_FPS_IN_MILLISECONDS)) {
+    bool iAmDebugging = false;  // if you're debugging set this to true, so you won't get skipped for slow debugging
+    if (!iAmDebugging && sinceLastTime <= std::max(_setupNewVoxelsForDrawingLastElapsed, SIXTY_FPS_IN_MILLISECONDS)) {
         return; // bail early, it hasn't been long enough since the last time we ran
     }
 
@@ -147,19 +148,37 @@ void VoxelSystem::setupNewVoxelsForDrawing() {
     // If the view frustum has changed, since last time, then remove nodes that are out of view
     if ((sinceLastViewCulling >= std::max(_lastViewCullingElapsed, VIEW_CULLING_RATE_IN_MILLISECONDS)) && hasViewChanged()) {
         _lastViewCulling = start;
+
+        // When we call removeOutOfView() voxels, we don't actually remove the voxels from the VBOs, but we do remove
+        // them from tree, this makes our tree caclulations faster, but doesn't require us to fully rebuild the VBOs (which
+        // can be expensive).
         removeOutOfView();
+        
+        // Once we call cleanupRemovedVoxels() we do need to rebuild our VBOs (if anything was actually removed). So,
+        // we should consider putting this someplace else... as this might be able to occur less frequently, and save us on
+        // VBO reubuilding. Possibly we should do this only if our actual VBO usage crosses some lower boundary.
+        cleanupRemovedVoxels();
+
         double endViewCulling = usecTimestampNow();
         _lastViewCullingElapsed = (endViewCulling - start) / 1000.0;
     }    
     
     if (_tree->isDirty()) {
-        PerformanceWarning warn(_renderWarningsOn, "calling... newTreeToArrays()");
+        static char buffer[64] = { 0 };
+        if (_renderWarningsOn) { 
+            sprintf(buffer, "newTreeToArrays() _renderFullVBO=%s", (_renderFullVBO ? "yes" : "no")); 
+        };
+        PerformanceWarning warn(_renderWarningsOn, buffer);
         _callsToTreesToArrays++;
-        if (_alwaysRenderFullVBO) {
+        if (_renderFullVBO) {
             _voxelsInWriteArrays = 0; // reset our VBO
         }
         _voxelsUpdated = newTreeToArrays(_tree->rootNode);
         _tree->clearDirtyBit(); // after we pull the trees into the array, we can consider the tree clean
+        
+        // since we called treeToArrays, we can assume that our VBO is in sync, and so partial updates to the VBOs are
+        // ok again, until/unless we call removeOutOfView() 
+        _renderFullVBO = false; 
     } else {
         _voxelsUpdated = 0;
     }
@@ -176,6 +195,16 @@ void VoxelSystem::setupNewVoxelsForDrawing() {
     _setupNewVoxelsForDrawingLastElapsed = elapsedmsec;
 }
 
+void VoxelSystem::cleanupRemovedVoxels() {
+    PerformanceWarning warn(_renderWarningsOn, "cleanupRemovedVoxels()");
+    if (!_removedVoxels.isEmpty()) {
+        while (!_removedVoxels.isEmpty()) {
+            delete _removedVoxels.extract();
+        }
+        _renderFullVBO = true; // if we remove voxels, we must update our full VBOs
+    }
+}
+
 void VoxelSystem::copyWrittenDataToReadArrays() {
     PerformanceWarning warn(_renderWarningsOn, "copyWrittenDataToReadArrays()"); // would like to include _voxelsInArrays, _voxelsUpdated
     if (_voxelsDirty && _voxelsUpdated) {
@@ -210,16 +239,16 @@ int VoxelSystem::newTreeToArrays(VoxelNode* node) {
             voxelsUpdated += newTreeToArrays(node->getChildAtIndex(i));
         }
     }
-    if (_alwaysRenderFullVBO) {
+    if (_renderFullVBO) {
-        voxelsUpdated += newway__updateNodeInArray(node);
+        voxelsUpdated += updateNodeInArraysAsFullVBO(node);
     } else {
-        voxelsUpdated += oldway__updateNodeInArray(node);
+        voxelsUpdated += updateNodeInArraysAsPartialVBO(node);
     }
     node->clearDirtyBit(); // always clear the dirty bit, even if it doesn't need to be rendered
     return voxelsUpdated;
 }
 
-int VoxelSystem::newway__updateNodeInArray(VoxelNode* node) {
+int VoxelSystem::updateNodeInArraysAsFullVBO(VoxelNode* node) {
     // If we've run out of room, then just bail...
     if (_voxelsInWriteArrays >= MAX_VOXELS_PER_SYSTEM) {
         return 0;
@@ -238,18 +267,24 @@ int VoxelSystem::newway__updateNodeInArray(VoxelNode* node) {
             *(writeVerticesAt+j) = startVertex[j % 3] + (identityVertices[j] * voxelScale);
             *(writeColorsAt  +j) = node->getColor()[j % 3];
         }
-         _voxelsInWriteArrays++; // our know vertices in the arrays
+        node->setBufferIndex(nodeIndex);
+        _voxelDirtyArray[nodeIndex] = true; // just in case we switch to Partial mode
+        _voxelsInWriteArrays++; // our know vertices in the arrays
         return 1; // rendered
     }    
     return 0; // not-rendered
 }
 
-int VoxelSystem::oldway__updateNodeInArray(VoxelNode* node) {
+int VoxelSystem::updateNodeInArraysAsPartialVBO(VoxelNode* node) {
-    // Now, if we've changed any attributes (our renderness, our color, etc) then update the Arrays... for us
+    // If we've run out of room, then just bail...
-    if (node->isDirty() && (node->getShouldRender() || node->isKnownBufferIndex())) {
+    if (_voxelsInWriteArrays >= MAX_VOXELS_PER_SYSTEM) {
+        return 0;
+    }
+    
+    // Now, if we've changed any attributes (our renderness, our color, etc) then update the Arrays...
+    if (node->isDirty()) {
         glm::vec3 startVertex;
         float voxelScale = 0;
-        
         // If we're should render, use our legit location and scale, 
         if (node->getShouldRender()) {
             startVertex = node->getCorner();
@@ -267,8 +302,9 @@ int VoxelSystem::oldway__updateNodeInArray(VoxelNode* node) {
             nodeIndex = node->getBufferIndex();
         } else {
             nodeIndex = _voxelsInWriteArrays;
+            node->setBufferIndex(nodeIndex);
+            _voxelsInWriteArrays++;
         }
-        
         _voxelDirtyArray[nodeIndex] = true;
 
         // populate the array with points for the 8 vertices
@@ -279,10 +315,6 @@ int VoxelSystem::oldway__updateNodeInArray(VoxelNode* node) {
             *(writeVerticesAt+j) = startVertex[j % 3] + (identityVertices[j] * voxelScale);
             *(writeColorsAt  +j) = node->getColor()[j % 3];
         }
-        if (!node->isKnownBufferIndex()) {
-            node->setBufferIndex(nodeIndex);
-            _voxelsInWriteArrays++; // our know vertices in the arrays
-        }
         return 1; // updated!
     }
     return 0; // not-updated
@@ -373,53 +405,90 @@ void VoxelSystem::init() {
     delete[] normalsArray;
 }
 
-void VoxelSystem::updateVBOs() {
+void VoxelSystem::updateFullVBOs() {
-    PerformanceWarning warn(_renderWarningsOn, "updateVBOs()"); // would like to include _callsToTreesToArrays
+    glBufferIndex segmentStart = 0;
-    if (_voxelsDirty) {
+    glBufferIndex segmentEnd = _voxelsInWriteArrays;
-        if (_alwaysRenderFullVBO) {
+
-            glBufferIndex segmentStart = 0;
+    int segmentLength = (segmentEnd - segmentStart) + 1;
-            glBufferIndex segmentEnd = _voxelsInWriteArrays;
+    GLintptr   segmentStartAt   = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
-        
+    GLsizeiptr segmentSizeBytes = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
-            int segmentLength = (segmentEnd - segmentStart) + 1;
+    GLfloat* readVerticesFrom   = _readVerticesArray + (segmentStart * VERTEX_POINTS_PER_VOXEL);
-            GLintptr   segmentStartAt   = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
+    glBindBuffer(GL_ARRAY_BUFFER, _vboVerticesID);
-            GLsizeiptr segmentSizeBytes = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
+    glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readVerticesFrom);
-            GLfloat* readVerticesFrom   = _readVerticesArray + (segmentStart * VERTEX_POINTS_PER_VOXEL);
+    segmentStartAt          = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
-            glBindBuffer(GL_ARRAY_BUFFER, _vboVerticesID);
+    segmentSizeBytes        = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
-            glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readVerticesFrom);
+    GLubyte* readColorsFrom = _readColorsArray   + (segmentStart * VERTEX_POINTS_PER_VOXEL);
-            segmentStartAt          = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
+    glBindBuffer(GL_ARRAY_BUFFER, _vboColorsID);
-            segmentSizeBytes        = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
+    glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readColorsFrom);
-            GLubyte* readColorsFrom = _readColorsArray   + (segmentStart * VERTEX_POINTS_PER_VOXEL);
+    
-            glBindBuffer(GL_ARRAY_BUFFER, _vboColorsID);
+    // consider the _voxelDirtyArray[] clean!
-            glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readColorsFrom);
+    memset(_voxelDirtyArray, false, _voxelsInWriteArrays * sizeof(bool));
-        } else {
+}
-            glBufferIndex segmentStart = 0;
+
-            glBufferIndex segmentEnd = 0;
+void VoxelSystem::updatePartialVBOs() {
-            bool inSegment = false;
+    glBufferIndex segmentStart = 0;
-            for (glBufferIndex i = 0; i < _voxelsInWriteArrays; i++) {
+    glBufferIndex segmentEnd = 0;
-                if (!inSegment) {
+    bool inSegment = false;
-                    if (_voxelDirtyArray[i]) {
+    for (glBufferIndex i = 0; i < _voxelsInWriteArrays; i++) {
-                        segmentStart = i;
+        bool thisVoxelDirty = _voxelDirtyArray[i];
-                        inSegment = true;
+        if (!inSegment) {
-                        _voxelDirtyArray[i] = false; // consider us clean!
+            if (thisVoxelDirty) {
-                    }
+                segmentStart = i;
-                } else {
+                inSegment = true;
-                    if (!_voxelDirtyArray[i] || (i == (_voxelsInWriteArrays - 1)) ) {
+                _voxelDirtyArray[i] = false; // consider us clean!
-                        segmentEnd = i;
-                        inSegment = false;
-                        int segmentLength = (segmentEnd - segmentStart) + 1;
-                        GLintptr   segmentStartAt   = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
-                        GLsizeiptr segmentSizeBytes = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
-                        GLfloat* readVerticesFrom   = _readVerticesArray + (segmentStart * VERTEX_POINTS_PER_VOXEL);
-                        glBindBuffer(GL_ARRAY_BUFFER, _vboVerticesID);
-                        glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readVerticesFrom);
-                        segmentStartAt          = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
-                        segmentSizeBytes        = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
-                        GLubyte* readColorsFrom = _readColorsArray   + (segmentStart * VERTEX_POINTS_PER_VOXEL);
-                        glBindBuffer(GL_ARRAY_BUFFER, _vboColorsID);
-                        glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readColorsFrom);
-                    }
-                }
             }
+        } else {
+            if (!thisVoxelDirty) {
+                // If we got here because because this voxel is NOT dirty, so the last dirty voxel was the one before
+                // this one and so that's where the "segment" ends
+                segmentEnd = i - 1; 
+                inSegment = false;
+                int segmentLength = (segmentEnd - segmentStart) + 1;
+                GLintptr   segmentStartAt   = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
+                GLsizeiptr segmentSizeBytes = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
+                GLfloat* readVerticesFrom   = _readVerticesArray + (segmentStart * VERTEX_POINTS_PER_VOXEL);
+                glBindBuffer(GL_ARRAY_BUFFER, _vboVerticesID);
+                glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readVerticesFrom);
+                segmentStartAt          = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
+                segmentSizeBytes        = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
+                GLubyte* readColorsFrom = _readColorsArray   + (segmentStart * VERTEX_POINTS_PER_VOXEL);
+                glBindBuffer(GL_ARRAY_BUFFER, _vboColorsID);
+                glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readColorsFrom);
+            }
+            _voxelDirtyArray[i] = false; // consider us clean!
+        }
+    }
+    
+    // if we got to the end of the array, and we're in an active dirty segment...
+    if (inSegment) {
+        segmentEnd = _voxelsInWriteArrays - 1; 
+        inSegment = false;
+        int segmentLength = (segmentEnd - segmentStart) + 1;
+        GLintptr   segmentStartAt   = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
+        GLsizeiptr segmentSizeBytes = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLfloat);
+        GLfloat* readVerticesFrom   = _readVerticesArray + (segmentStart * VERTEX_POINTS_PER_VOXEL);
+        glBindBuffer(GL_ARRAY_BUFFER, _vboVerticesID);
+        glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readVerticesFrom);
+        segmentStartAt          = segmentStart * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
+        segmentSizeBytes        = segmentLength * VERTEX_POINTS_PER_VOXEL * sizeof(GLubyte);
+        GLubyte* readColorsFrom = _readColorsArray   + (segmentStart * VERTEX_POINTS_PER_VOXEL);
+        glBindBuffer(GL_ARRAY_BUFFER, _vboColorsID);
+        glBufferSubData(GL_ARRAY_BUFFER, segmentStartAt, segmentSizeBytes, readColorsFrom);
+    }
+}
+
+void VoxelSystem::updateVBOs() {
+    static char buffer[40] = { 0 };
+    if (_renderWarningsOn) { 
+        sprintf(buffer, "updateVBOs() _renderFullVBO=%s", (_renderFullVBO ? "yes" : "no")); 
+    };
+    PerformanceWarning warn(_renderWarningsOn, buffer); // would like to include _callsToTreesToArrays
+    if (_voxelsDirty) {
+        // updatePartialVBOs() is not yet working. For now, ALWAYS call updateFullVBOs()
+        if (_renderFullVBO) {
+            updateFullVBOs();
+        } else {
+            updatePartialVBOs(); // too many small segments?
         }
         _voxelsDirty = false;
     }
@@ -672,10 +741,14 @@ void VoxelSystem::removeOutOfView() {
     removeOutOfViewArgs args(this);
     _tree->recurseTreeWithOperation(removeOutOfViewOperation,(void*)&args);
 
-    if (_renderWarningsOn) {
+    if (args.nodesRemoved) {
-        printLog("removeOutOfView() scanned=%ld removed=%ld inside=%ld intersect=%ld outside=%ld bag.count()=%d \n", 
+        _tree->setDirtyBit();
+    }
+    bool showRemoveDebugDetails = false;
+    if (showRemoveDebugDetails) {
+        printLog("removeOutOfView() scanned=%ld removed=%ld inside=%ld intersect=%ld outside=%ld _removedVoxels.count()=%d \n", 
                 args.nodesScanned, args.nodesRemoved, args.nodesInside, 
-                args.nodesIntersect, args.nodesOutside, args.dontRecurseBag.count()
+                args.nodesIntersect, args.nodesOutside, _removedVoxels.count()
             );
     }
 }
@@ -695,3 +768,133 @@ bool VoxelSystem::findRayIntersection(const glm::vec3& origin, const glm::vec3&
     detail.blue = node->getColor()[2];
     return true;
 }
+
+class falseColorizeRandomEveryOtherArgs {
+public:
+    falseColorizeRandomEveryOtherArgs() : totalNodes(0), colorableNodes(0), coloredNodes(0), colorThis(true) {};
+    unsigned long totalNodes;
+    unsigned long colorableNodes;
+    unsigned long coloredNodes;
+    bool colorThis;
+};
+
+bool VoxelSystem::falseColorizeRandomEveryOtherOperation(VoxelNode* node, void* extraData) {
+    falseColorizeRandomEveryOtherArgs* args = (falseColorizeRandomEveryOtherArgs*)extraData;
+    args->totalNodes++;
+    if (node->isColored()) {
+        args->colorableNodes++;
+        if (args->colorThis) {
+            args->coloredNodes++;
+            node->setFalseColor(255, randomColorValue(150), randomColorValue(150));
+        }
+        args->colorThis = !args->colorThis;
+    }
+    return true; // keep going!
+}
+
+void VoxelSystem::falseColorizeRandomEveryOther() {
+    falseColorizeRandomEveryOtherArgs args;
+    _tree->recurseTreeWithOperation(falseColorizeRandomEveryOtherOperation,&args);
+    printLog("randomized false color for every other node: total %ld, colorable %ld, colored %ld\n", 
+        args.totalNodes, args.colorableNodes, args.coloredNodes);
+    setupNewVoxelsForDrawing();
+}
+
+class collectStatsForTreesAndVBOsArgs {
+public:
+    collectStatsForTreesAndVBOsArgs() : 
+        totalNodes(0), 
+        dirtyNodes(0), 
+        shouldRenderNodes(0),
+        coloredNodes(0),
+        nodesInVBO(0),
+        nodesInVBOOverExpectedMax(0),
+        duplicateVBOIndex(0)
+        {
+            memset(hasIndexFound, false, MAX_VOXELS_PER_SYSTEM * sizeof(bool));
+        };
+
+    unsigned long totalNodes;
+    unsigned long dirtyNodes;
+    unsigned long shouldRenderNodes;
+    unsigned long coloredNodes;
+    unsigned long nodesInVBO;
+    unsigned long nodesInVBOOverExpectedMax;
+    unsigned long duplicateVBOIndex;
+    unsigned long expectedMax;
+    
+    bool colorThis;
+    bool hasIndexFound[MAX_VOXELS_PER_SYSTEM];
+};
+
+bool VoxelSystem::collectStatsForTreesAndVBOsOperation(VoxelNode* node, void* extraData) {
+    collectStatsForTreesAndVBOsArgs* args = (collectStatsForTreesAndVBOsArgs*)extraData;
+    args->totalNodes++;
+
+    if (node->isColored()) {
+        args->coloredNodes++;
+    }
+
+    if (node->getShouldRender()) {
+        args->shouldRenderNodes++;
+    }
+
+    if (node->isDirty()) {
+        args->dirtyNodes++;
+    }
+
+    if (node->isKnownBufferIndex()) {
+        args->nodesInVBO++;
+        unsigned long nodeIndex = node->getBufferIndex();
+        if (args->hasIndexFound[nodeIndex]) {
+            args->duplicateVBOIndex++;
+            printLog("duplicateVBO found... index=%ld, isDirty=%s, shouldRender=%s \n", nodeIndex, 
+                    node->isDirty() ? "yes" : "no" , node->getShouldRender()  ? "yes" : "no" );
+        } else {
+            args->hasIndexFound[nodeIndex] = true;
+        }
+        if (nodeIndex > args->expectedMax) {
+            args->nodesInVBOOverExpectedMax++;
+        }
+    }
+
+    return true; // keep going!
+}
+
+void VoxelSystem::collectStatsForTreesAndVBOs() {
+
+    glBufferIndex minDirty = GLBUFFER_INDEX_UNKNOWN;
+    glBufferIndex maxDirty = 0;
+
+    for (glBufferIndex i = 0; i < _voxelsInWriteArrays; i++) {
+        if (_voxelDirtyArray[i]) {
+            minDirty = std::min(minDirty,i);
+            maxDirty = std::max(maxDirty,i);
+        }
+    }
+
+    collectStatsForTreesAndVBOsArgs args;
+    args.expectedMax = _voxelsInWriteArrays;
+    _tree->recurseTreeWithOperation(collectStatsForTreesAndVBOsOperation,&args);
+
+    printLog("_voxelsDirty=%s _voxelsInWriteArrays=%ld minDirty=%ld maxDirty=%ld \n", (_voxelsDirty ? "yes" : "no"), 
+        _voxelsInWriteArrays, minDirty, maxDirty);
+
+    printLog("stats: total %ld, dirty %ld, colored %ld, shouldRender %ld, inVBO %ld, nodesInVBOOverExpectedMax %ld, duplicateVBOIndex %ld\n", 
+        args.totalNodes, args.dirtyNodes, args.coloredNodes, args.shouldRenderNodes, 
+        args.nodesInVBO, args.nodesInVBOOverExpectedMax, args.duplicateVBOIndex);
+
+    glBufferIndex minInVBO = GLBUFFER_INDEX_UNKNOWN;
+    glBufferIndex maxInVBO = 0;
+
+    for (glBufferIndex i = 0; i < MAX_VOXELS_PER_SYSTEM; i++) {
+        if (args.hasIndexFound[i]) {
+            minInVBO = std::min(minInVBO,i);
+            maxInVBO = std::max(maxInVBO,i);
+        }
+    }
+
+    printLog("minInVBO=%ld maxInVBO=%ld _voxelsInWriteArrays=%ld _voxelsInReadArrays=%ld\n", 
+            minInVBO, maxInVBO, _voxelsInWriteArrays, _voxelsInReadArrays);
+
+}

interface/src/VoxelSystem.h

@@ -57,6 +57,7 @@ public:
     void trueColorize();
     void falseColorizeInView(ViewFrustum* viewFrustum);
     void falseColorizeDistanceFromView(ViewFrustum* viewFrustum);
+    void falseColorizeRandomEveryOther();
 
     void killLocalVoxels();
     void setRenderPipelineWarnings(bool on) { _renderWarningsOn = on; };
@@ -67,6 +68,8 @@ public:
     
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,
                              VoxelDetail& detail, float& distance, BoxFace& face);
+
+    void collectStatsForTreesAndVBOs();
     
 private:
     int  _callsToTreesToArrays;
@@ -82,9 +85,11 @@ private:
     static bool falseColorizeDistanceFromViewOperation(VoxelNode* node, void* extraData);
     static bool getDistanceFromViewRangeOperation(VoxelNode* node, void* extraData);
     static bool removeOutOfViewOperation(VoxelNode* node, void* extraData);
+    static bool falseColorizeRandomEveryOtherOperation(VoxelNode* node, void* extraData);
+    static bool collectStatsForTreesAndVBOsOperation(VoxelNode* node, void* extraData);
 
-    int newway__updateNodeInArray(VoxelNode* node);
+    int updateNodeInArraysAsFullVBO(VoxelNode* node);
-    int oldway__updateNodeInArray(VoxelNode* node);
+    int updateNodeInArraysAsPartialVBO(VoxelNode* node);
 
     // these are kinda hacks, used by getDistanceFromViewRangeOperation() probably shouldn't be here
     static float _maxDistance;
@@ -103,7 +108,7 @@ private:
     unsigned long _voxelsInReadArrays;
     unsigned long _unusedArraySpace;
     
-    bool _alwaysRenderFullVBO;
+    bool _renderFullVBO;
     
     double _setupNewVoxelsForDrawingLastElapsed;
     double _setupNewVoxelsForDrawingLastFinished;
@@ -120,11 +125,18 @@ private:
     ViewFrustum _lastKnowViewFrustum;
 
     int newTreeToArrays(VoxelNode *currentNode);
+    void cleanupRemovedVoxels();
+
     void setupNewVoxelsForDrawing();
     void copyWrittenDataToReadArrays();
-    void updateVBOs();
+
-    
     bool _voxelsDirty;
+
+public:
+    void updateVBOs();
+    void updateFullVBOs(); // all voxels in the VBO
+    void updatePartialVBOs(); // multiple segments, only dirty voxels
+    
 };
 
 #endif

interface/src/main.cpp

@@ -364,10 +364,6 @@ void reset_sensors() {
     headMouseY = HEIGHT/2;
     
     myAvatar.reset();
-    
-    if (serialPort.active) {
-        serialPort.resetTrailingAverages();
-    }
 }
 
 //
@@ -379,15 +375,9 @@ void updateAvatar(float deltaTime) {
     myAvatar.updateHeadFromGyros(deltaTime, &serialPort, &gravity);
 
     //  Grab latest readings from the gyros
-    float measuredYawRate, measuredPitchRate;
+    float measuredPitchRate = serialPort.getLastPitchRate();
-    if (USING_INVENSENSE_MPU9150) {
+    float measuredYawRate = serialPort.getLastYawRate();
-        measuredPitchRate = serialPort.getLastPitchRate();
+    
-        measuredYawRate = serialPort.getLastYawRate();
-    } else {
-        measuredPitchRate = serialPort.getRelativeValue(HEAD_PITCH_RATE);
-        measuredYawRate = serialPort.getRelativeValue(HEAD_YAW_RATE);
-    }
-        
     //  Update gyro-based mouse (X,Y on screen)
     const float MIN_MOUSE_RATE = 30.0;
     const float MOUSE_SENSITIVITY = 0.1f;
@@ -1330,6 +1320,20 @@ int doRandomizeVoxelColors(int state) {
     return state;
 }
 
+int doFalseRandomizeEveryOtherVoxelColors(int state) {
+    if (state == MENU_ROW_PICKED) {
+        ::voxels.falseColorizeRandomEveryOther();
+    }
+    return state;
+}
+
+int doTreeStats(int state) {
+    if (state == MENU_ROW_PICKED) {
+        ::voxels.collectStatsForTreesAndVBOs();
+    }
+    return state;
+}
+
 int doFalseRandomizeVoxelColors(int state) {
     if (state == MENU_ROW_PICKED) {
         ::voxels.falseColorizeRandom();
@@ -1426,9 +1430,11 @@ void initMenu() {
     menuColumnDebug->addRow("Kill Local Voxels", doKillLocalVoxels);
     menuColumnDebug->addRow("Randomize Voxel TRUE Colors", doRandomizeVoxelColors);
     menuColumnDebug->addRow("FALSE Color Voxels Randomly", doFalseRandomizeVoxelColors);
+    menuColumnDebug->addRow("FALSE Color Voxel Every Other Randomly", doFalseRandomizeEveryOtherVoxelColors);
     menuColumnDebug->addRow("FALSE Color Voxels by Distance", doFalseColorizeByDistance);
     menuColumnDebug->addRow("FALSE Color Voxel Out of View", doFalseColorizeInView);
     menuColumnDebug->addRow("Show TRUE Colors", doTrueVoxelColors);
+    menuColumnDebug->addRow("Calculate Tree Stats", doTreeStats);
 }
 
 void testPointToVoxel() {
@@ -1628,7 +1634,12 @@ void key(unsigned char k, int x, int y) {
     }
     
     //  Process keypresses 
-     if (k == 'q' || k == 'Q')  ::terminate();
+
+    if (k == 'S') {
+        ::voxels.collectStatsForTreesAndVBOs();
+    }
+    
+    if (k == 'q' || k == 'Q')  ::terminate();
     if (k == '/')  ::renderStatsOn = !::renderStatsOn;        // toggle stats
     if (k == '*')  ::renderStarsOn = !::renderStarsOn;        // toggle stars
     if (k == 'V' || k == 'v')  ::renderVoxels = !::renderVoxels;        // toggle voxels
@@ -1775,7 +1786,7 @@ void idle(void) {
         }
         
         //  Read serial port interface devices
-        if (serialPort.active && USING_INVENSENSE_MPU9150) {
+        if (serialPort.active) {
             serialPort.readData();
         }
         
@@ -1809,10 +1820,6 @@ void idle(void) {
         lastTimeIdle = check;
     }
     
-    //  Read serial data 
-    if (serialPort.active && !USING_INVENSENSE_MPU9150) {
-        serialPort.readData();
-    }
 }
 
 void reshape(int width, int height) {

libraries/avatars/src/AvatarData.cpp

@@ -42,6 +42,8 @@ AvatarData::AvatarData() :
     _headYaw(0),
     _headPitch(0),
     _headRoll(0),
+    _headLeanSideways(0),
+    _headLeanForward(0),
     _handState(0),
     _cameraPosition(0,0,0),
     _cameraDirection(0,0,0),
@@ -84,7 +86,13 @@ int AvatarData::getBroadcastData(unsigned char* destinationBuffer) {
     destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headPitch);
     destinationBuffer += packFloatAngleToTwoByte(destinationBuffer, _headRoll);
     
-    // Hand Position 
+    // Head lean X,Z (head lateral and fwd/back motion relative to torso)
+    memcpy(destinationBuffer, &_headLeanSideways, sizeof(float));
+    destinationBuffer += sizeof(float);
+    memcpy(destinationBuffer, &_headLeanForward, sizeof(float));
+    destinationBuffer += sizeof(float);
+
+    // Hand Position
     memcpy(destinationBuffer, &_handPosition, sizeof(float) * 3);
     destinationBuffer += sizeof(float) * 3;
     
@@ -150,6 +158,12 @@ int AvatarData::parseData(unsigned char* sourceBuffer, int numBytes) {
     sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_headPitch);
     sourceBuffer += unpackFloatAngleFromTwoByte((uint16_t *)sourceBuffer, &_headRoll);
 
+    //  Head position relative to pelvis
+    memcpy(&_headLeanSideways, sourceBuffer, sizeof(float));
+    sourceBuffer += sizeof(float);
+    memcpy(&_headLeanForward, sourceBuffer, sizeof(float));
+    sourceBuffer += sizeof(float);
+
     // Hand Position
     memcpy(&_handPosition, sourceBuffer, sizeof(float) * 3);
     sourceBuffer += sizeof(float) * 3;

libraries/avatars/src/AvatarData.h

@@ -55,6 +55,12 @@ public:
     void  addHeadYaw(float y){_headYaw -= y; }
     void  addHeadRoll(float r){_headRoll += r; }
 
+    //  Head vector deflection from pelvix in X,Z
+    void setHeadLeanSideways(float s) {_headLeanSideways = s; };
+    float getHeadLeanSideways() const { return _headLeanSideways; };
+    void setHeadLeanForward(float f) {_headLeanForward = f; };
+    float getHeadLeanForward() const { return _headLeanForward; };
+    
     //  Hand State
     void setHandState(char s) { _handState = s; };
     char getHandState() const {return _handState; };
@@ -104,6 +110,9 @@ protected:
     float _headYaw;
     float _headPitch;
     float _headRoll;
+    
+    float _headLeanSideways;
+    float _headLeanForward;
 
     //  Audio loudness (used to drive facial animation)
     float _audioLoudness;

libraries/voxels/src/VoxelNode.cpp

@@ -94,6 +94,7 @@ VoxelNode* VoxelNode::removeChildAtIndex(int childIndex) {
     VoxelNode* returnedChild = _children[childIndex];
     if (_children[childIndex]) {
         _children[childIndex] = NULL;
+        _isDirty = true;
     }
     return returnedChild;
 }
@@ -150,9 +151,7 @@ void VoxelNode::setFalseColor(colorPart red, colorPart green, colorPart blue) {
         _currentColor[1] = green;
         _currentColor[2] = blue;
         _currentColor[3] = 1; // XXXBHG - False colors are always considered set
-        //if (_shouldRender) {
+        _isDirty = true;
-            _isDirty = true;
-        //}
     }
 }
 
@@ -163,9 +162,7 @@ void VoxelNode::setFalseColored(bool isFalseColored) {
             memcpy(&_currentColor,&_trueColor,sizeof(nodeColor));
         }
         _falseColored = isFalseColored; 
-        //if (_shouldRender) {
+        _isDirty = true;
-            _isDirty = true;
-        //}
     }
 };
 

libraries/voxels/src/VoxelTree.h

@@ -61,6 +61,7 @@ public:
 
     bool isDirty() const { return _isDirty; };
     void clearDirtyBit() { _isDirty = false; };
+    void setDirtyBit() { _isDirty = true; };
     unsigned long int getNodesChangedFromBitstream() const { return _nodesChangedFromBitstream; };
 
     bool findRayIntersection(const glm::vec3& origin, const glm::vec3& direction,