diff --git a/animation-server/src/main.cpp b/animation-server/src/main.cpp
index c02e8e8383..d9d1f37722 100644
--- a/animation-server/src/main.cpp
+++ b/animation-server/src/main.cpp
@@ -244,7 +244,6 @@ static void renderMovingBug() {
 }
 
 
-
 float intensity = 0.5f;
 float intensityIncrement = 0.1f;
 const float MAX_INTENSITY = 1.0f;
@@ -644,14 +643,14 @@ void* animateVoxels(void* args) {
             sendDanceFloor();
         }
         
-        double end = usecTimestampNow();
-        double elapsedSeconds = (end - ::start) / 1000000.0;
+        long long end = usecTimestampNow();
+        long long elapsedSeconds = (end - ::start) / 1000000;
         if (::shouldShowPacketsPerSecond) {
             printf("packetsSent=%ld, bytesSent=%ld pps=%f bps=%f\n",packetsSent,bytesSent,
                 (float)(packetsSent/elapsedSeconds),(float)(bytesSent/elapsedSeconds));
         }
         // dynamically sleep until we need to fire off the next set of voxels
-        double usecToSleep =  ANIMATE_VOXELS_INTERVAL_USECS - (usecTimestampNow() - usecTimestamp(&lastSendTime));
+        long long usecToSleep =  ANIMATE_VOXELS_INTERVAL_USECS - (usecTimestampNow() - usecTimestamp(&lastSendTime));
         
         if (usecToSleep > 0) {
             usleep(usecToSleep);
diff --git a/audio-mixer/src/main.cpp b/audio-mixer/src/main.cpp
index b44bf587b8..04a8ae0885 100644
--- a/audio-mixer/src/main.cpp
+++ b/audio-mixer/src/main.cpp
@@ -391,7 +391,7 @@ int main(int argc, const char* argv[]) {
             numStatCollections++;
         }
         
-        double usecToSleep = usecTimestamp(&startTime) + (++nextFrame * BUFFER_SEND_INTERVAL_USECS) - usecTimestampNow();
+        long long usecToSleep = usecTimestamp(&startTime) + (++nextFrame * BUFFER_SEND_INTERVAL_USECS) - usecTimestampNow();
         
         if (usecToSleep > 0) {
             usleep(usecToSleep);
diff --git a/domain-server/src/main.cpp b/domain-server/src/main.cpp
index b1e2117db3..67c1123205 100644
--- a/domain-server/src/main.cpp
+++ b/domain-server/src/main.cpp
@@ -94,8 +94,6 @@ int main(int argc, const char * argv[])
     
     agentList->startSilentAgentRemovalThread();
     
-    uint16_t packetAgentID = 0;
-    
     while (true) {
         if (agentList->getAgentSocket()->receive((sockaddr *)&agentPublicAddress, packetData, &receivedBytes) &&
             (packetData[0] == PACKET_HEADER_DOMAIN_REPORT_FOR_DUTY || packetData[0] == PACKET_HEADER_DOMAIN_LIST_REQUEST)) {
@@ -135,19 +133,19 @@ int main(int argc, const char * argv[])
             if (numInterestTypes > 0) {
                 // if the agent has sent no types of interest, assume they want nothing but their own ID back
                 for (AgentList::iterator agent = agentList->begin(); agent != agentList->end(); agent++) {
-                    if (!agent->matches((sockaddr*) &agentPublicAddress, (sockaddr*) &agentLocalAddress, agentType)
-                        && memchr(agentTypesOfInterest, agent->getType(), numInterestTypes)) {
+                    if (!agent->matches((sockaddr*) &agentPublicAddress, (sockaddr*) &agentLocalAddress, agentType) &&
+                            memchr(agentTypesOfInterest, agent->getType(), numInterestTypes)) {
                         // this is not the agent themselves
                         // and this is an agent of a type in the passed agent types of interest
                         // or the agent did not pass us any specific types they are interested in
-                        
+                    
                         if (memchr(SOLO_AGENT_TYPES, agent->getType(), sizeof(SOLO_AGENT_TYPES)) == NULL) {
                             // this is an agent of which there can be multiple, just add them to the packet
                             // don't send avatar agents to other avatars, that will come from avatar mixer
                             if (agentType != AGENT_TYPE_AVATAR || agent->getType() != AGENT_TYPE_AVATAR) {
                                 currentBufferPos = addAgentToBroadcastPacket(currentBufferPos, &(*agent));
                             }
-                            
+                        
                         } else {
                             // solo agent, we need to only send newest
                             if (newestSoloAgents[agent->getType()] == NULL ||
@@ -156,19 +154,6 @@ int main(int argc, const char * argv[])
                                 newestSoloAgents[agent->getType()] = &(*agent);
                             }
                         }
-                    } else {
-                        double timeNow = usecTimestampNow();
-                        
-                        // this is the agent, just update last receive to now
-                        agent->setLastHeardMicrostamp(timeNow);
-                        
-                        // grab the ID for this agent so we can send it back with the packet
-                        packetAgentID = agent->getAgentID();
-                        
-                        if (packetData[0] == PACKET_HEADER_DOMAIN_REPORT_FOR_DUTY
-                            && memchr(SOLO_AGENT_TYPES, agentType, sizeof(SOLO_AGENT_TYPES))) {
-                            agent->setWakeMicrostamp(timeNow);
-                        }
                     }
                 }
                 
@@ -179,9 +164,18 @@ int main(int argc, const char * argv[])
                     currentBufferPos = addAgentToBroadcastPacket(currentBufferPos, soloAgent->second);
                 }
             }
+                        
+            // update last receive to now
+            long long timeNow = usecTimestampNow();
+            newAgent->setLastHeardMicrostamp(timeNow);
+            
+            if (packetData[0] == PACKET_HEADER_DOMAIN_REPORT_FOR_DUTY
+                && memchr(SOLO_AGENT_TYPES, agentType, sizeof(SOLO_AGENT_TYPES))) {
+                newAgent->setWakeMicrostamp(timeNow);
+            }
             
             // add the agent ID to the end of the pointer
-            currentBufferPos += packAgentId(currentBufferPos, packetAgentID);
+            currentBufferPos += packAgentId(currentBufferPos, newAgent->getAgentID());
             
             // send the constructed list back to this agent
             agentList->getAgentSocket()->send((sockaddr*) &agentPublicAddress,
diff --git a/eve/src/main.cpp b/eve/src/main.cpp
index 387c72633b..9310e2b316 100644
--- a/eve/src/main.cpp
+++ b/eve/src/main.cpp
@@ -128,7 +128,7 @@ int main(int argc, const char* argv[]) {
     broadcastPacket[0] = PACKET_HEADER_HEAD_DATA;
     
     timeval thisSend;
-    double numMicrosecondsSleep = 0;
+    long long numMicrosecondsSleep = 0;
     
     int handStateTimer = 0;
     
@@ -212,4 +212,4 @@ int main(int argc, const char* argv[]) {
     // stop the agent list's threads
     agentList->stopPingUnknownAgentsThread();
     agentList->stopSilentAgentRemovalThread();
-}
\ No newline at end of file
+}
diff --git a/injector/src/main.cpp b/injector/src/main.cpp
index f2e87990d1..a0c07179f7 100644
--- a/injector/src/main.cpp
+++ b/injector/src/main.cpp
@@ -187,7 +187,7 @@ int main(int argc, char* argv[]) {
             unsigned char broadcastPacket = PACKET_HEADER_INJECT_AUDIO;
             
             timeval thisSend;
-            double numMicrosecondsSleep = 0;
+            long long numMicrosecondsSleep = 0;
             
             timeval lastDomainServerCheckIn = {};
             
diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 51c6e65cb9..9b39c6508d 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -62,8 +62,6 @@
 
 using namespace std;
 
-const bool TESTING_AVATAR_TOUCH = false;
-
 //  Starfield information
 static char STAR_FILE[] = "https://s3-us-west-1.amazonaws.com/highfidelity/stars.txt";
 static char STAR_CACHE_FILE[] = "cachedStars.txt";
@@ -144,7 +142,6 @@ Application::Application(int& argc, char** argv, timeval &startup_time) :
         _viewFrustumOffsetDistance(25.0),
         _viewFrustumOffsetUp(0.0),
         _audioScope(256, 200, true),
-        _manualFirstPerson(false),
         _mouseX(0),
         _mouseY(0),
         _mousePressed(false),
@@ -170,7 +167,7 @@ Application::Application(int& argc, char** argv, timeval &startup_time) :
     _window->setWindowTitle("Interface");
     printLog("Interface Startup:\n");
     
-    unsigned int listenPort = AGENT_SOCKET_LISTEN_PORT;
+    unsigned int listenPort = 0; // bind to an ephemeral port by default
     const char** constArgv = const_cast<const char**>(argv);
     const char* portStr = getCmdOption(argc, constArgv, "--listenPort");
     if (portStr) {
@@ -308,7 +305,7 @@ void Application::paintGL() {
     
     if (_myCamera.getMode() == CAMERA_MODE_MIRROR) {
         _myCamera.setTightness     (100.0f); 
-        _myCamera.setTargetPosition(_myAvatar.getBallPosition(AVATAR_JOINT_HEAD_BASE));
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getWorldAlignedOrientation() * glm::quat(glm::vec3(0.0f, PIf, 0.0f)));
         
     } else if (OculusManager::isConnected()) {
@@ -320,11 +317,11 @@ void Application::paintGL() {
     
     } else if (_myCamera.getMode() == CAMERA_MODE_FIRST_PERSON) {
         _myCamera.setTightness(0.0f);  //  In first person, camera follows head exactly without delay
-        _myCamera.setTargetPosition(_myAvatar.getBallPosition(AVATAR_JOINT_HEAD_BASE));
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getHead().getCameraOrientation(_headCameraPitchYawScale));
         
     } else if (_myCamera.getMode() == CAMERA_MODE_THIRD_PERSON) {
-        _myCamera.setTargetPosition(_myAvatar.getHeadJointPosition());
+        _myCamera.setTargetPosition(_myAvatar.getUprightHeadPosition());
         _myCamera.setTargetRotation(_myAvatar.getHead().getCameraOrientation(_headCameraPitchYawScale));
     }
     
@@ -511,6 +508,9 @@ void Application::keyPressEvent(QKeyEvent* event) {
                 break;
                 
             case Qt::Key_E:
+                if (!_myAvatar.getDriveKeys(UP)) {
+                    _myAvatar.jump();
+                }
                 _myAvatar.setDriveKeys(UP, 1);
                 break;
                 
@@ -721,6 +721,9 @@ void Application::mousePressEvent(QMouseEvent* event) {
         if (event->button() == Qt::LeftButton) {
             _mouseX = event->x();
             _mouseY = event->y();
+            _mouseDragStartedX = _mouseX;
+            _mouseDragStartedY = _mouseY;
+            _mouseVoxelDragging = _mouseVoxel;
             _mousePressed = true;
             maybeEditVoxelUnderCursor();
             
@@ -827,6 +830,7 @@ void Application::terminate() {
 
 static void sendAvatarVoxelURLMessage(const QUrl& url) {
     uint16_t ownerID = AgentList::getInstance()->getOwnerID();
+    
     if (ownerID == UNKNOWN_AGENT_ID) {
         return; // we don't yet know who we are
     }
@@ -880,6 +884,10 @@ void Application::editPreferences() {
     headCameraPitchYawScale->setValue(_headCameraPitchYawScale);
     form->addRow("Head Camera Pitch/Yaw Scale:", headCameraPitchYawScale);
     
+    QDoubleSpinBox* leanScale = new QDoubleSpinBox();
+    leanScale->setValue(_myAvatar.getLeanScale());
+    form->addRow("Lean Scale:", leanScale);
+    
     QDialogButtonBox* buttons = new QDialogButtonBox(QDialogButtonBox::Ok | QDialogButtonBox::Cancel);
     dialog.connect(buttons, SIGNAL(accepted()), SLOT(accept()));
     dialog.connect(buttons, SIGNAL(rejected()), SLOT(reject()));
@@ -893,6 +901,7 @@ void Application::editPreferences() {
     sendAvatarVoxelURLMessage(url);
     
     _headCameraPitchYawScale = headCameraPitchYawScale->value();
+    _myAvatar.setLeanScale(leanScale->value());
 }
 
 void Application::pair() {
@@ -903,6 +912,8 @@ void Application::setHead(bool head) {
     if (head) {
         _myCamera.setMode(CAMERA_MODE_MIRROR);
         _myCamera.setModeShiftRate(100.0f);
+        _manualFirstPerson->setChecked(false);
+        
     } else {
         _myCamera.setMode(CAMERA_MODE_THIRD_PERSON);
         _myCamera.setModeShiftRate(1.0f);
@@ -920,7 +931,9 @@ void Application::setFullscreen(bool fullscreen) {
 }
 
 void Application::setRenderFirstPerson(bool firstPerson) {
-    _manualFirstPerson = firstPerson;    
+    if (firstPerson && _lookingInMirror->isChecked()) {
+        _lookingInMirror->trigger();
+    }
 }
 
 void Application::setFrustumOffset(bool frustumOffset) {
@@ -1003,6 +1016,12 @@ static void sendVoxelEditMessage(PACKET_HEADER header, VoxelDetail& detail) {
     }
 }
 
+const glm::vec3 Application::getMouseVoxelWorldCoordinates(const VoxelDetail _mouseVoxel) {
+    return glm::vec3((_mouseVoxel.x + _mouseVoxel.s / 2.f) * TREE_SCALE,
+                     (_mouseVoxel.y + _mouseVoxel.s / 2.f) * TREE_SCALE,
+                     (_mouseVoxel.z + _mouseVoxel.s / 2.f) * TREE_SCALE);
+}
+
 void Application::decreaseVoxelSize() {
     _mouseVoxelScale /= 2;
 }
@@ -1212,7 +1231,7 @@ void Application::initMenu() {
     (_gyroLook = optionsMenu->addAction("Gyro Look"))->setCheckable(true);
     _gyroLook->setChecked(false);
     (_mouseLook = optionsMenu->addAction("Mouse Look"))->setCheckable(true);
-    _mouseLook->setChecked(false);
+    _mouseLook->setChecked(true);
     (_showHeadMouse = optionsMenu->addAction("Head Mouse"))->setCheckable(true);
     _showHeadMouse->setChecked(false);
     (_transmitterDrives = optionsMenu->addAction("Transmitter Drive"))->setCheckable(true);
@@ -1236,7 +1255,6 @@ void Application::initMenu() {
     _renderAtmosphereOn->setShortcut(Qt::SHIFT | Qt::Key_A);
     (_renderGroundPlaneOn = renderMenu->addAction("Ground Plane"))->setCheckable(true);
     _renderGroundPlaneOn->setChecked(true);
-    _renderGroundPlaneOn->setShortcut(Qt::SHIFT | Qt::Key_G);
     (_renderAvatarsOn = renderMenu->addAction("Avatars"))->setCheckable(true);
     _renderAvatarsOn->setChecked(true);
     (_renderAvatarBalls = renderMenu->addAction("Avatar as Balls"))->setCheckable(true);
@@ -1245,8 +1263,8 @@ void Application::initMenu() {
     _renderFrameTimerOn->setChecked(false);
     (_renderLookatOn = renderMenu->addAction("Lookat Vectors"))->setCheckable(true);
     _renderLookatOn->setChecked(false);
-    
-    renderMenu->addAction("First Person", this, SLOT(setRenderFirstPerson(bool)), Qt::Key_P)->setCheckable(true);
+    (_manualFirstPerson = renderMenu->addAction(
+        "First Person", this, SLOT(setRenderFirstPerson(bool)), Qt::Key_P))->setCheckable(true);
     
     QMenu* toolsMenu = menuBar->addMenu("Tools");
     (_renderStatsOn = toolsMenu->addAction("Stats"))->setCheckable(true);
@@ -1432,6 +1450,26 @@ void Application::update(float deltaTime) {
     // tell my avatar the posiion and direction of the ray projected ino the world based on the mouse position        
     _myAvatar.setMouseRay(mouseRayOrigin, mouseRayDirection);
 
+    //  If we are dragging on a voxel, add thrust according to the amount the mouse is dragging
+    const float VOXEL_GRAB_THRUST = 5.0f;
+    if (_mousePressed && (_mouseVoxel.s != 0)) {
+        glm::vec2 mouseDrag(_mouseX - _mouseDragStartedX, _mouseY - _mouseDragStartedY);
+        glm::quat orientation = _myAvatar.getOrientation();
+        glm::vec3 front = orientation * IDENTITY_FRONT;
+        glm::vec3 up = orientation * IDENTITY_UP;
+        glm::vec3 towardVoxel = getMouseVoxelWorldCoordinates(_mouseVoxelDragging)
+                                - _myAvatar.getCameraPosition();
+        towardVoxel = front * glm::length(towardVoxel);
+        glm::vec3 lateralToVoxel = glm::cross(up, glm::normalize(towardVoxel)) * glm::length(towardVoxel);
+        _voxelThrust = glm::vec3(0, 0, 0);
+        _voxelThrust += towardVoxel * VOXEL_GRAB_THRUST * deltaTime * mouseDrag.y;
+        _voxelThrust += lateralToVoxel * VOXEL_GRAB_THRUST * deltaTime * mouseDrag.x;
+        
+        //  Add thrust from voxel grabbing to the avatar 
+        _myAvatar.addThrust(_voxelThrust);
+
+    }
+    
     _mouseVoxel.s = 0.0f;
     if (checkedVoxelModeAction() != 0 &&
         (fabs(_myAvatar.getVelocity().x) +
@@ -1560,30 +1598,27 @@ void Application::update(float deltaTime) {
         _myAvatar.simulate(deltaTime, NULL);
     }
     
-    if (TESTING_AVATAR_TOUCH) {
-        if (_myCamera.getMode() != CAMERA_MODE_THIRD_PERSON) {
-            _myCamera.setMode(CAMERA_MODE_THIRD_PERSON);
-            _myCamera.setModeShiftRate(1.0f);
-        }
-    } else {
         if (_myCamera.getMode() != CAMERA_MODE_MIRROR && !OculusManager::isConnected()) {        
-            if (_manualFirstPerson) {
+            if (_manualFirstPerson->isChecked()) {
                 if (_myCamera.getMode() != CAMERA_MODE_FIRST_PERSON ) {
                     _myCamera.setMode(CAMERA_MODE_FIRST_PERSON);
                     _myCamera.setModeShiftRate(1.0f);
                 }
-            } else {
-                if (_myAvatar.getIsNearInteractingOther()) {
-                    if (_myCamera.getMode() != CAMERA_MODE_FIRST_PERSON) {
-                        _myCamera.setMode(CAMERA_MODE_FIRST_PERSON);
-                        _myCamera.setModeShiftRate(1.0f);
-                    }
-                } else {
-                    if (_myCamera.getMode() != CAMERA_MODE_THIRD_PERSON) {
-                        _myCamera.setMode(CAMERA_MODE_THIRD_PERSON);
-                        _myCamera.setModeShiftRate(1.0f);
-                    }
+        } else {
+            const float THIRD_PERSON_SHIFT_VELOCITY = 2.0f;
+            const float TIME_BEFORE_SHIFT_INTO_FIRST_PERSON = 0.75f;
+            const float TIME_BEFORE_SHIFT_INTO_THIRD_PERSON = 0.1f;
+            
+            if ((_myAvatar.getElapsedTimeStopped() > TIME_BEFORE_SHIFT_INTO_FIRST_PERSON)
+                && (_myCamera.getMode() != CAMERA_MODE_FIRST_PERSON)) {
+                    _myCamera.setMode(CAMERA_MODE_FIRST_PERSON);
+                    _myCamera.setModeShiftRate(1.0f);
                 }
+            if ((_myAvatar.getSpeed() > THIRD_PERSON_SHIFT_VELOCITY)
+                && (_myAvatar.getElapsedTimeMoving() > TIME_BEFORE_SHIFT_INTO_THIRD_PERSON)
+                && (_myCamera.getMode() != CAMERA_MODE_THIRD_PERSON)) {
+                _myCamera.setMode(CAMERA_MODE_THIRD_PERSON);
+                _myCamera.setModeShiftRate(1000.0f);
             }
         }
     }
@@ -1951,6 +1986,10 @@ void Application::displaySide(Camera& whichCamera) {
     glEnable(GL_LIGHTING);
     glEnable(GL_DEPTH_TEST);
     
+    //  Enable to show line from me to the voxel I am touching
+    //renderLineToTouchedVoxel();
+    //renderThrustAtVoxel(_voxelThrust);
+    
     // draw a red sphere  
     float sphereRadius = 0.25f;
     glColor3f(1,0,0);
@@ -2069,9 +2108,8 @@ void Application::displayOverlay() {
     //  Show on-screen msec timer
     if (_renderFrameTimerOn->isChecked()) {
         char frameTimer[10];
-        double mSecsNow = floor(usecTimestampNow() / 1000.0 + 0.5);
-        mSecsNow = mSecsNow - floor(mSecsNow / 1000.0) * 1000.0;
-        sprintf(frameTimer, "%3.0f\n", mSecsNow);
+        long long mSecsNow = floor(usecTimestampNow() / 1000.0 + 0.5);
+        sprintf(frameTimer, "%d\n", (int)(mSecsNow % 1000));
         drawtext(_glWidget->width() - 100, _glWidget->height() - 20, 0.30, 0, 1.0, 0, frameTimer, 0, 0, 0);
         drawtext(_glWidget->width() - 102, _glWidget->height() - 22, 0.30, 0, 1.0, 0, frameTimer, 1, 1, 1);
     }
@@ -2167,6 +2205,32 @@ void Application::displayStats() {
     }
 }
 
+void Application::renderThrustAtVoxel(const glm::vec3& thrust) {
+    if (_mousePressed) {
+        glColor3f(1, 0, 0);
+        glLineWidth(2.0f);
+        glBegin(GL_LINES);
+        glm::vec3 voxelTouched = getMouseVoxelWorldCoordinates(_mouseVoxelDragging);
+        glVertex3f(voxelTouched.x, voxelTouched.y, voxelTouched.z);
+        glVertex3f(voxelTouched.x + thrust.x, voxelTouched.y + thrust.y, voxelTouched.z + thrust.z);
+        glEnd();
+    }
+    
+}
+void Application::renderLineToTouchedVoxel() {
+    //  Draw a teal line to the voxel I am currently dragging on
+    if (_mousePressed) {
+        glColor3f(0, 1, 1);
+        glLineWidth(2.0f);
+        glBegin(GL_LINES);
+        glm::vec3 voxelTouched = getMouseVoxelWorldCoordinates(_mouseVoxelDragging);
+        glVertex3f(voxelTouched.x, voxelTouched.y, voxelTouched.z);
+        glm::vec3 headPosition = _myAvatar.getHeadJointPosition();
+        glVertex3fv(&headPosition.x);
+        glEnd();
+    }
+}
+
 /////////////////////////////////////////////////////////////////////////////////////
 // renderViewFrustum()
 //
@@ -2443,6 +2507,8 @@ void Application::resetSensors() {
     QCursor::setPos(_headMouseX, _headMouseY);
     _myAvatar.reset();
     _myTransmitter.resetLevels();
+    _myAvatar.setVelocity(glm::vec3(0,0,0));
+    _myAvatar.setThrust(glm::vec3(0,0,0));
 }
 
 static void setShortcutsEnabled(QWidget* widget, bool enabled) {
diff --git a/interface/src/Application.h b/interface/src/Application.h
index 4f33f8fb1e..1d50b5065f 100644
--- a/interface/src/Application.h
+++ b/interface/src/Application.h
@@ -67,6 +67,8 @@ public:
 
     void wheelEvent(QWheelEvent* event);
     
+    const glm::vec3 getMouseVoxelWorldCoordinates(const VoxelDetail _mouseVoxel);
+    
     Avatar* getAvatar() { return &_myAvatar; }
     Camera* getCamera() { return &_myCamera; }
     ViewFrustum* getViewFrustum() { return &_viewFrustum; }
@@ -93,6 +95,9 @@ private slots:
     
     void setRenderFirstPerson(bool firstPerson);
     
+    void renderThrustAtVoxel(const glm::vec3& thrust);
+    void renderLineToTouchedVoxel();
+    
     void setFrustumOffset(bool frustumOffset);
     void cycleFrustumRenderMode();
     
@@ -141,7 +146,7 @@ private:
     void displayStats();
     
     void renderViewFrustum(ViewFrustum& viewFrustum);
-    
+        
     void setupPaintingVoxel();
     void shiftPaintingColor();
     void maybeEditVoxelUnderCursor();
@@ -186,6 +191,7 @@ private:
     QAction* _renderStatsOn;         // Whether to show onscreen text overlay with stats
     QAction* _renderFrameTimerOn;    // Whether to show onscreen text overlay with stats
     QAction* _renderLookatOn;        // Whether to show lookat vectors from avatar eyes if looking at something
+    QAction* _manualFirstPerson;     // Whether to force first-person mode
     QAction* _logOn;                 // Whether to show on-screen log
     QActionGroup* _voxelModeActions; // The group of voxel edit mode actions
     QAction* _addVoxelMode;          // Whether add voxel mode is enabled
@@ -249,14 +255,18 @@ private:
     Environment _environment;
     
     int _headMouseX, _headMouseY;
-    bool _manualFirstPerson;
     float _headCameraPitchYawScale;
     
     HandControl _handControl;
     
     int _mouseX;
     int _mouseY;
+    int _mouseDragStartedX;
+    int _mouseDragStartedY;
+    VoxelDetail _mouseVoxelDragging;
+    glm::vec3 _voxelThrust;
     bool _mousePressed; //  true if mouse has been pressed (clear when finished)
+
     
     VoxelDetail _mouseVoxel;      // details of the voxel under the mouse cursor
     float _mouseVoxelScale;       // the scale for adding/removing voxels
diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp
index 32b35aebac..a2ff33ca52 100644
--- a/interface/src/Avatar.cpp
+++ b/interface/src/Avatar.cpp
@@ -74,12 +74,13 @@ Avatar::Avatar(Agent* owningAgent) :
     _bodyRollDelta(0.0f),
     _movedHandOffset(0.0f, 0.0f, 0.0f),
     _mode(AVATAR_MODE_STANDING),
-    _cameraPosition(0.0f, 0.0f, 0.0f),
     _handHoldingPosition(0.0f, 0.0f, 0.0f),
     _velocity(0.0f, 0.0f, 0.0f),
     _thrust(0.0f, 0.0f, 0.0f),
+    _shouldJump(false),
     _speed(0.0f),
     _maxArmLength(0.0f),
+    _leanScale(0.5f),
     _pelvisStandingHeight(0.0f),
     _pelvisFloatingHeight(0.0f),
     _distanceToNearestAvatar(std::numeric_limits<float>::max()),
@@ -89,6 +90,9 @@ Avatar::Avatar(Agent* owningAgent) :
     _mouseRayDirection(0.0f, 0.0f, 0.0f),
     _interactingOther(NULL),
     _isMouseTurningRight(false),
+    _elapsedTimeMoving(0.0f),
+    _elapsedTimeStopped(0.0f),
+    _elapsedTimeSinceCollision(0.0f),
     _voxels(this)
 {
     // give the pointer to our head to inherited _headData variable from AvatarData
@@ -103,9 +107,11 @@ Avatar::Avatar(Agent* owningAgent) :
     initializeBodyBalls();
     
     _height               = _skeleton.getHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius + _bodyBall[ BODY_BALL_HEAD_BASE ].radius;
+    
     _maxArmLength         = _skeleton.getArmLength();
     _pelvisStandingHeight = _skeleton.getPelvisStandingHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius;
     _pelvisFloatingHeight = _skeleton.getPelvisFloatingHeight() + _bodyBall[ BODY_BALL_LEFT_HEEL ].radius;
+    _pelvisToHeadLength   = _skeleton.getPelvisToHeadLength();
     
     _avatarTouch.setReachableRadius(PERIPERSONAL_RADIUS);
     
@@ -284,19 +290,12 @@ void Avatar::updateHeadFromGyros(float deltaTime, SerialInterface* serialInterfa
     _head.setYaw(estimatedRotation.y * AMPLIFY_YAW);
     _head.setRoll(estimatedRotation.z * AMPLIFY_ROLL);
         
-    //  Update head lean distance based on accelerometer data
-    glm::vec3 headRotationRates(_head.getPitch(), _head.getYaw(), _head.getRoll());
-    
-    glm::vec3 leaning = (serialInterface->getLastAcceleration() - serialInterface->getGravity())
-    * LEAN_SENSITIVITY
-    * (1.f - fminf(glm::length(headRotationRates), HEAD_RATE_MAX) / HEAD_RATE_MAX);
-    leaning.y = 0.f;
-    if (glm::length(leaning) < LEAN_MAX) {
-        _head.setLeanForward(_head.getLeanForward() * (1.f - LEAN_AVERAGING * deltaTime) +
-                             (LEAN_AVERAGING * deltaTime) * leaning.z * LEAN_SENSITIVITY);
-        _head.setLeanSideways(_head.getLeanSideways() * (1.f - LEAN_AVERAGING * deltaTime) +
-                              (LEAN_AVERAGING * deltaTime) * leaning.x * LEAN_SENSITIVITY);
-    }
+    //  Update torso lean distance based on accelerometer data
+    glm::vec3 estimatedPosition = serialInterface->getEstimatedPosition() * _leanScale;
+    const float TORSO_LENGTH = 0.5f;
+    const float MAX_LEAN = 45.0f;
+    _head.setLeanSideways(glm::clamp(glm::degrees(atanf(-estimatedPosition.x / TORSO_LENGTH)), -MAX_LEAN, MAX_LEAN));
+    _head.setLeanForward(glm::clamp(glm::degrees(atanf(estimatedPosition.z / TORSO_LENGTH)), -MAX_LEAN, MAX_LEAN));
 }
 
 float Avatar::getAbsoluteHeadYaw() const {
@@ -315,45 +314,141 @@ glm::quat Avatar::getWorldAlignedOrientation () const {
     return computeRotationFromBodyToWorldUp() * getOrientation();
 }
 
+glm::vec3 Avatar::getUprightHeadPosition() const {
+    return _position + getWorldAlignedOrientation() * glm::vec3(0.0f, _pelvisToHeadLength, 0.0f);
+}
+
 void  Avatar::updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight) {
     //  Update head yaw and pitch based on mouse input
-    const float MOUSE_MOVE_RADIUS = 0.3f;
-    const float MOUSE_ROTATE_SPEED = 4.0f;
-    const float MOUSE_PITCH_SPEED = 2.0f;
+    const float MOUSE_ROTATE_SPEED = 0.01f;
+    const float MOUSE_PITCH_SPEED = 0.02f;
     const int TITLE_BAR_HEIGHT = 46;
-    float mouseLocationX = (float)mouseX / (float)screenWidth - 0.5f;
-    float mouseLocationY = (float)mouseY / (float)screenHeight - 0.5f;
     
     if ((mouseX > 1) && (mouseX < screenWidth) && (mouseY > TITLE_BAR_HEIGHT) && (mouseY < screenHeight)) {
         //
         //  Mouse must be inside screen (not at edge) and not on title bar for movement to happen
         //
-        if (mouseLocationX > MOUSE_MOVE_RADIUS) {
-            _head.addYaw(-(mouseLocationX - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_ROTATE_SPEED);
-        } else if (mouseLocationX < -MOUSE_MOVE_RADIUS) {
-            _head.addYaw(-(mouseLocationX + MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_ROTATE_SPEED);
-        }
         
-        if (mouseLocationY > MOUSE_MOVE_RADIUS) {
-            _head.addPitch(-(mouseLocationY - MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_PITCH_SPEED);
-        } else if (mouseLocationY < -MOUSE_MOVE_RADIUS) {
-            _head.addPitch(-(mouseLocationY + MOUSE_MOVE_RADIUS) / (0.5f - MOUSE_MOVE_RADIUS) * MOUSE_PITCH_SPEED);
+        int pixelMoveThreshold = screenWidth / 6;
+        glm::vec2 mouseVector(mouseX - (screenWidth / 2), mouseY - (screenHeight / 2));
+        if (glm::length(mouseVector) > pixelMoveThreshold) {
+            mouseVector -= glm::normalize(mouseVector) * (float) pixelMoveThreshold;
+            _head.addYaw(-mouseVector.x * MOUSE_ROTATE_SPEED);
+            _head.addPitch(-mouseVector.y * MOUSE_PITCH_SPEED);
         }
     }
-    return;
 }
 
+void Avatar::updateThrust(float deltaTime, Transmitter * transmitter) {
+    //
+    //  Gather thrust information from keyboard and sensors to apply to avatar motion 
+    //
+    glm::quat orientation = getOrientation();
+    glm::vec3 front = orientation * IDENTITY_FRONT;
+    glm::vec3 right = orientation * IDENTITY_RIGHT;
+    glm::vec3 up = orientation * IDENTITY_UP;
+
+    const float THRUST_MAG_UP = 800.0f;
+    const float THRUST_MAG_DOWN = 200.f;
+    const float THRUST_MAG_FWD = 300.f;
+    const float THRUST_MAG_BACK = 150.f;
+    const float THRUST_MAG_LATERAL = 200.f;
+    const float THRUST_JUMP = 120.f;
+    
+    //  Add Thrusts from keyboard
+    if (_driveKeys[FWD      ]) {_thrust       += THRUST_MAG_FWD * deltaTime * front;}
+    if (_driveKeys[BACK     ]) {_thrust       -= THRUST_MAG_BACK * deltaTime * front;}
+    if (_driveKeys[RIGHT    ]) {_thrust       += THRUST_MAG_LATERAL * deltaTime * right;}
+    if (_driveKeys[LEFT     ]) {_thrust       -= THRUST_MAG_LATERAL * deltaTime * right;}
+    if (_driveKeys[UP       ]) {_thrust       += THRUST_MAG_UP * deltaTime * up;}
+    if (_driveKeys[DOWN     ]) {_thrust       -= THRUST_MAG_DOWN * deltaTime * up;}
+    if (_driveKeys[ROT_RIGHT]) {_bodyYawDelta -= YAW_MAG    * deltaTime;}
+    if (_driveKeys[ROT_LEFT ]) {_bodyYawDelta += YAW_MAG    * deltaTime;}
+    
+    //  Add one time jumping force if requested
+    if (_shouldJump) {
+        _thrust += THRUST_JUMP * up;
+        _shouldJump = false;
+    }
+    
+    //  Add thrusts from Transmitter
+    if (transmitter) {
+        transmitter->checkForLostTransmitter();
+        glm::vec3 rotation = transmitter->getEstimatedRotation();
+        const float TRANSMITTER_MIN_RATE = 1.f;
+        const float TRANSMITTER_MIN_YAW_RATE = 4.f;
+        const float TRANSMITTER_LATERAL_FORCE_SCALE = 5.f;
+        const float TRANSMITTER_FWD_FORCE_SCALE = 25.f;
+        const float TRANSMITTER_UP_FORCE_SCALE = 100.f;
+        const float TRANSMITTER_YAW_SCALE = 10.0f;
+        const float TRANSMITTER_LIFT_SCALE = 3.f;
+        const float TOUCH_POSITION_RANGE_HALF = 32767.f;
+        if (fabs(rotation.z) > TRANSMITTER_MIN_RATE) {
+            _thrust += rotation.z * TRANSMITTER_LATERAL_FORCE_SCALE * deltaTime * right;
+        }
+        if (fabs(rotation.x) > TRANSMITTER_MIN_RATE) {
+            _thrust += -rotation.x * TRANSMITTER_FWD_FORCE_SCALE * deltaTime * front;
+        }
+        if (fabs(rotation.y) > TRANSMITTER_MIN_YAW_RATE) {
+            _bodyYawDelta += rotation.y * TRANSMITTER_YAW_SCALE * deltaTime;
+        }
+        if (transmitter->getTouchState()->state == 'D') {
+            _thrust += TRANSMITTER_UP_FORCE_SCALE *
+            (float)(transmitter->getTouchState()->y - TOUCH_POSITION_RANGE_HALF) / TOUCH_POSITION_RANGE_HALF *
+            TRANSMITTER_LIFT_SCALE *
+            deltaTime *
+            up;
+        }
+    }
+}
+    
 void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     
-    //figure out if the mouse cursor is over any body spheres...
-    checkForMouseRayTouching();
+    glm::quat orientation = getOrientation();
+    glm::vec3 front = orientation * IDENTITY_FRONT;
+    glm::vec3 right = orientation * IDENTITY_RIGHT;
+
+    // Update movement timers
+    if (!_owningAgent) {
+        _elapsedTimeSinceCollision += deltaTime;
+        const float VELOCITY_MOVEMENT_TIMER_THRESHOLD = 0.2f;
+        if (glm::length(_velocity) < VELOCITY_MOVEMENT_TIMER_THRESHOLD) {
+            _elapsedTimeMoving = 0.f;
+            _elapsedTimeStopped += deltaTime;
+        } else {
+            _elapsedTimeStopped = 0.f;
+            _elapsedTimeMoving += deltaTime;
+        }
+    }
+    
+    //  Collect thrust forces from keyboard and devices 
+    if (!_owningAgent) {
+        updateThrust(deltaTime, transmitter);
+    }
     
     // copy velocity so we can use it later for acceleration
     glm::vec3 oldVelocity = getVelocity();
     
+    if (!_owningAgent) {
+        // update position by velocity
+        _position += _velocity * deltaTime;
+        
+        // calculate speed
+        _speed = glm::length(_velocity);
+    }
+    
+    //figure out if the mouse cursor is over any body spheres...
+    if (!_owningAgent) {
+        checkForMouseRayTouching();
+    }
+
     // update balls
     if (_balls) { _balls->simulate(deltaTime); }
     
+    // update torso rotation based on head lean
+    _skeleton.joint[AVATAR_JOINT_TORSO].rotation = glm::quat(glm::radians(glm::vec3(
+        _head.getLeanForward(), 0.0f, _head.getLeanSideways())));
+    
 	// update avatar skeleton
     _skeleton.update(deltaTime, getOrientation(), _position);
     
@@ -388,12 +483,13 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     
     //update the movement of the hand and process handshaking with other avatars...
     updateHandMovementAndTouching(deltaTime);
-    
     _avatarTouch.simulate(deltaTime);
     
     // apply gravity and collision with the ground/floor
     if (!_owningAgent && USING_AVATAR_GRAVITY) {
         _velocity += _gravity * (GRAVITY_EARTH * deltaTime);
+    }
+    if (!_owningAgent) {
         updateCollisionWithEnvironment();
     }
     
@@ -410,57 +506,11 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
         updateCollisionWithVoxels();
     }
     
-    glm::quat orientation = getOrientation();
-    glm::vec3 front = orientation * IDENTITY_FRONT;
-    glm::vec3 right = orientation * IDENTITY_RIGHT;
-    glm::vec3 up = orientation * IDENTITY_UP;
-    
-    // driving the avatar around should only apply if this is my avatar (as opposed to an avatar being driven remotely)
-    const float THRUST_MAG = 600.0f;
-    
     if (!_owningAgent) {
-        
-        _thrust = glm::vec3(0.0f, 0.0f, 0.0f);
-        
-        //  Add Thrusts from keyboard
-        if (_driveKeys[FWD      ]) {_thrust       += THRUST_MAG * deltaTime * front;}
-        if (_driveKeys[BACK     ]) {_thrust       -= THRUST_MAG * deltaTime * front;}
-        if (_driveKeys[RIGHT    ]) {_thrust       += THRUST_MAG * deltaTime * right;}
-        if (_driveKeys[LEFT     ]) {_thrust       -= THRUST_MAG * deltaTime * right;}
-        if (_driveKeys[UP       ]) {_thrust       += THRUST_MAG * deltaTime * up;}
-        if (_driveKeys[DOWN     ]) {_thrust       -= THRUST_MAG * deltaTime * up;}
-        if (_driveKeys[ROT_RIGHT]) {_bodyYawDelta -= YAW_MAG    * deltaTime;}
-        if (_driveKeys[ROT_LEFT ]) {_bodyYawDelta += YAW_MAG    * deltaTime;}
-        
-        //  Add thrusts from Transmitter
-        if (transmitter) {
-            transmitter->checkForLostTransmitter();
-            glm::vec3 rotation = transmitter->getEstimatedRotation();
-            const float TRANSMITTER_MIN_RATE = 1.f;
-            const float TRANSMITTER_MIN_YAW_RATE = 4.f;
-            const float TRANSMITTER_LATERAL_FORCE_SCALE = 25.f;
-            const float TRANSMITTER_FWD_FORCE_SCALE = 100.f;
-            const float TRANSMITTER_YAW_SCALE = 10.0f;
-            const float TRANSMITTER_LIFT_SCALE = 3.f;
-            const float TOUCH_POSITION_RANGE_HALF = 32767.f;
-            if (fabs(rotation.z) > TRANSMITTER_MIN_RATE) {
-                _thrust += rotation.z * TRANSMITTER_LATERAL_FORCE_SCALE * deltaTime * right;
-            }
-            if (fabs(rotation.x) > TRANSMITTER_MIN_RATE) {
-                _thrust += -rotation.x * TRANSMITTER_FWD_FORCE_SCALE * deltaTime * front;
-            }
-            if (fabs(rotation.y) > TRANSMITTER_MIN_YAW_RATE) {
-                _bodyYawDelta += rotation.y * TRANSMITTER_YAW_SCALE * deltaTime;
-            }
-            if (transmitter->getTouchState()->state == 'D') {
-                _thrust += THRUST_MAG *
-                (float)(transmitter->getTouchState()->y - TOUCH_POSITION_RANGE_HALF) / TOUCH_POSITION_RANGE_HALF *
-                TRANSMITTER_LIFT_SCALE *
-                deltaTime *
-                up;
-            }
-        }
-        
+    
+        // add thrust to velocity
+        _velocity += _thrust * deltaTime;
+
         // update body yaw by body yaw delta
         orientation = orientation * glm::quat(glm::radians(
                                                            glm::vec3(_bodyPitchDelta, _bodyYawDelta, _bodyRollDelta) * deltaTime));
@@ -472,12 +522,22 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
         _bodyYawDelta   *= bodySpinMomentum;
         _bodyRollDelta  *= bodySpinMomentum;
         
-        // add thrust to velocity
-        _velocity += _thrust * deltaTime;
-        
-        // calculate speed
-        _speed = glm::length(_velocity);
-        
+        // Decay velocity.  If velocity is really low, increase decay to simulate static friction
+        const float VELOCITY_DECAY_UNDER_THRUST = 0.2;
+        const float VELOCITY_FAST_DECAY = 0.6;
+        const float VELOCITY_SLOW_DECAY = 3.0;
+        const float VELOCITY_FAST_THRESHOLD = 2.0f;
+        float decayConstant, decay;
+        if (glm::length(_thrust) > 0.f) {
+            decayConstant = VELOCITY_DECAY_UNDER_THRUST;
+        } else if (glm::length(_velocity) > VELOCITY_FAST_THRESHOLD) {
+            decayConstant = VELOCITY_FAST_DECAY;
+        } else {
+            decayConstant = VELOCITY_SLOW_DECAY;
+        }             
+        decay = glm::clamp(1.0f - decayConstant * deltaTime, 0.0f, 1.0f);
+        _velocity *= decay;
+                
         //pitch and roll the body as a function of forward speed and turning delta
         const float BODY_PITCH_WHILE_WALKING      = -20.0;
         const float BODY_ROLL_WHILE_TURNING       = 0.2;
@@ -497,29 +557,6 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
         //the following will be used to make the avatar upright no matter what gravity is
         setOrientation(computeRotationFromBodyToWorldUp(tiltDecay) * orientation);
         
-        // update position by velocity
-        _position += _velocity * deltaTime;
-        
-        // decay velocity
-        const float VELOCITY_DECAY = 0.9;
-        float decay = 1.0 - VELOCITY_DECAY * deltaTime;
-        if ( decay < 0.0 ) {
-            _velocity = glm::vec3( 0.0f, 0.0f, 0.0f );
-        } else {
-            _velocity *= decay;
-        }
-        
-        // If another avatar is near, dampen velocity as a function of closeness
-        if (_distanceToNearestAvatar < PERIPERSONAL_RADIUS) {
-            float closeness = 1.0f - (_distanceToNearestAvatar / PERIPERSONAL_RADIUS);
-            float drag = 1.0f - closeness * AVATAR_BRAKING_STRENGTH * deltaTime;
-            if ( drag > 0.0f ) {
-                _velocity *= drag;
-            } else {
-                _velocity = glm::vec3( 0.0f, 0.0f, 0.0f );
-            }
-        }
-        
         //  Compute instantaneous acceleration
         float forwardAcceleration = glm::length(glm::dot(getBodyFrontDirection(), getVelocity() - oldVelocity)) / deltaTime;
         const float ACCELERATION_PITCH_DECAY = 0.4f;
@@ -581,7 +618,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     // set head lookat position
     if (!_owningAgent) {
         if (_interactingOther) {
-            _head.setLookAtPosition(_interactingOther->caclulateAverageEyePosition());
+            _head.setLookAtPosition(_interactingOther->calculateAverageEyePosition());
         } else {
             _head.setLookAtPosition(glm::vec3(0.0f, 0.0f, 0.0f)); // 0,0,0 represents NOT looking at anything
         }
@@ -599,6 +636,10 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     } else {
         _mode = AVATAR_MODE_INTERACTING;
     }
+    
+    // Zero thrust out now that we've added it to velocity in this frame
+    _thrust = glm::vec3(0, 0, 0);
+
 }
 
 void Avatar::checkForMouseRayTouching() {
@@ -779,41 +820,52 @@ void Avatar::updateCollisionWithSphere(glm::vec3 position, float radius, float d
 }
 
 void Avatar::updateCollisionWithEnvironment() {
+    
     glm::vec3 up = getBodyUpDirection();
     float radius = _height * 0.125f;
+    const float ENVIRONMENT_SURFACE_ELASTICITY = 1.0f;
+    const float ENVIRONMENT_SURFACE_DAMPING = 0.01;
     glm::vec3 penetration;
     if (Application::getInstance()->getEnvironment()->findCapsulePenetration(
                                                                              _position - up * (_pelvisFloatingHeight - radius),
                                                                              _position + up * (_height - _pelvisFloatingHeight - radius), radius, penetration)) {
-        applyCollisionWithScene(penetration);
+        applyHardCollision(penetration, ENVIRONMENT_SURFACE_ELASTICITY, ENVIRONMENT_SURFACE_DAMPING);
     }
 }
 
+
 void Avatar::updateCollisionWithVoxels() {
     float radius = _height * 0.125f;
+    const float VOXEL_ELASTICITY = 1.4f;
+    const float VOXEL_DAMPING = 0.0;
     glm::vec3 penetration;
     if (Application::getInstance()->getVoxels()->findCapsulePenetration(
                                                                         _position - glm::vec3(0.0f, _pelvisFloatingHeight - radius, 0.0f),
                                                                         _position + glm::vec3(0.0f, _height - _pelvisFloatingHeight - radius, 0.0f), radius, penetration)) {
-        applyCollisionWithScene(penetration);
+        applyHardCollision(penetration, VOXEL_ELASTICITY, VOXEL_DAMPING);
     }
 }
 
-void Avatar::applyCollisionWithScene(const glm::vec3& penetration) {
+void Avatar::applyHardCollision(const glm::vec3& penetration, float elasticity, float damping) {
+    //
+    //  Update the avatar in response to a hard collision.  Position will be reset exactly
+    //  to outside the colliding surface.  Velocity will be modified according to elasticity.
+    //
+    //  if elasticity = 1.0, collision is inelastic.
+    //  if elasticity > 1.0, collision is elastic. 
+    //  
     _position -= penetration;
-    static float STATIC_FRICTION_VELOCITY = 0.15f;
-    static float STATIC_FRICTION_DAMPING = 0.0f;
-    static float KINETIC_FRICTION_DAMPING = 0.95f;
-    
+    static float HALTING_VELOCITY = 0.2f;
     // cancel out the velocity component in the direction of penetration
     float penetrationLength = glm::length(penetration);
     if (penetrationLength > EPSILON) {
+        _elapsedTimeSinceCollision = 0.0f;
         glm::vec3 direction = penetration / penetrationLength;
-        _velocity -= glm::dot(_velocity, direction) * direction;
-        _velocity *= KINETIC_FRICTION_DAMPING;
-        //  If velocity is quite low, apply static friction that takes away energy
-        if (glm::length(_velocity) < STATIC_FRICTION_VELOCITY) {
-            _velocity *= STATIC_FRICTION_DAMPING;
+        _velocity -= glm::dot(_velocity, direction) * direction * elasticity;
+        _velocity *= glm::clamp(1.f - damping, 0.0f, 1.0f);
+        if ((glm::length(_velocity) < HALTING_VELOCITY) && (glm::length(_thrust) == 0.f)) {
+            // If moving really slowly after a collision, and not applying forces, stop altogether
+            _velocity *= 0.f;
         }
     }
 }
@@ -836,7 +888,6 @@ void Avatar::updateAvatarCollisions(float deltaTime) {
                 // apply forces from collision
                 applyCollisionWithOtherAvatar(otherAvatar, deltaTime);
             }
-            
             // test other avatar hand position for proximity
             glm::vec3 v(_skeleton.joint[ AVATAR_JOINT_RIGHT_SHOULDER ].position);
             v -= otherAvatar->getPosition();
@@ -911,9 +962,7 @@ void Avatar::setGravity(glm::vec3 gravity) {
 }
 
 void Avatar::render(bool lookingInMirror, bool renderAvatarBalls) {
-    
-    _cameraPosition = Application::getInstance()->getCamera()->getPosition();
-    
+
     if (!_owningAgent && usingBigSphereCollisionTest) {
         // show TEST big sphere
         glColor4f(0.5f, 0.6f, 0.8f, 0.7);
@@ -932,7 +981,7 @@ void Avatar::render(bool lookingInMirror, bool renderAvatarBalls) {
     
     // if this is my avatar, then render my interactions with the other avatar
     if (!_owningAgent) {
-        _avatarTouch.render(getCameraPosition());
+        _avatarTouch.render(Application::getInstance()->getCamera()->getPosition());
     }
     
     //  Render the balls
@@ -1130,32 +1179,27 @@ glm::quat Avatar::computeRotationFromBodyToWorldUp(float proportion) const {
     return glm::angleAxis(angle * proportion, axis);
 }
 
+float Avatar::getBallRenderAlpha(int ball, bool lookingInMirror) const {
+    const float RENDER_OPAQUE_OUTSIDE = 1.25f; // render opaque if greater than this distance
+    const float DO_NOT_RENDER_INSIDE = 0.75f; // do not render if less than this distance
+    float distanceToCamera = glm::length(Application::getInstance()->getCamera()->getPosition() - _bodyBall[ball].position);
+    return (lookingInMirror || _owningAgent) ? 1.0f : glm::clamp(
+        (distanceToCamera - DO_NOT_RENDER_INSIDE) / (RENDER_OPAQUE_OUTSIDE - DO_NOT_RENDER_INSIDE), 0.f, 1.f);
+}
+
 void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) {
     
-    const float RENDER_OPAQUE_BEYOND = 1.0f;        //  Meters beyond which body is shown opaque
-    const float RENDER_TRANSLUCENT_BEYOND = 0.5f;
-    
     //  Render the body as balls and cones
     if (renderAvatarBalls || !_voxels.getVoxelURL().isValid()) {
         for (int b = 0; b < NUM_AVATAR_BODY_BALLS; b++) {
-            float distanceToCamera = glm::length(_cameraPosition - _bodyBall[b].position);
-            
-            float alpha = lookingInMirror ? 1.0f : glm::clamp((distanceToCamera - RENDER_TRANSLUCENT_BEYOND) /
-                                                              (RENDER_OPAQUE_BEYOND - RENDER_TRANSLUCENT_BEYOND), 0.f, 1.f);
-            
-            if (lookingInMirror || _owningAgent) {
-                alpha = 1.0f;
-            }
+            float alpha = getBallRenderAlpha(b, lookingInMirror);
             
             //  Always render other people, and render myself when beyond threshold distance
             if (b == BODY_BALL_HEAD_BASE) { // the head is rendered as a special
-                if (lookingInMirror || _owningAgent || distanceToCamera > RENDER_OPAQUE_BEYOND * 0.5) {
-                    _head.render(lookingInMirror, _cameraPosition, alpha);
+                if (alpha > 0.0f) {
+                    _head.render(lookingInMirror, alpha);
                 }
-            } else if (_owningAgent || distanceToCamera > RENDER_TRANSLUCENT_BEYOND
-                       || b == BODY_BALL_RIGHT_ELBOW
-                       || b == BODY_BALL_RIGHT_WRIST
-                       || b == BODY_BALL_RIGHT_FINGERTIPS ) {
+            } else if (alpha > 0.0f) {
                 //  Render the body ball sphere
                 if (_owningAgent || b == BODY_BALL_RIGHT_ELBOW
                     || b == BODY_BALL_RIGHT_WRIST
@@ -1207,7 +1251,10 @@ void Avatar::renderBody(bool lookingInMirror, bool renderAvatarBalls) {
         }
     } else {
         //  Render the body's voxels
-        _voxels.render(false);
+        float alpha = getBallRenderAlpha(BODY_BALL_HEAD_BASE, lookingInMirror);
+        if (alpha > 0.0f) {
+            _voxels.render(false);
+        }
     }
 }
 
@@ -1225,6 +1272,8 @@ void Avatar::loadData(QSettings* settings) {
     
     _voxels.setVoxelURL(settings->value("voxelURL").toUrl());
     
+    _leanScale = loadSetting(settings, "leanScale", 0.5f);
+    
     settings->endGroup();
 }
 
@@ -1246,6 +1295,8 @@ void Avatar::saveData(QSettings* set) {
     
     set->setValue("voxelURL", _voxels.getVoxelURL());
     
+    set->setValue("leanScale", _leanScale);
+    
     set->endGroup();
 }
 
diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h
index 0fc5b31286..db4f4dd7a6 100644
--- a/interface/src/Avatar.h
+++ b/interface/src/Avatar.h
@@ -85,6 +85,7 @@ public:
     void init();
     void reset();
     void simulate(float deltaTime, Transmitter* transmitter);
+    void updateThrust(float deltaTime, Transmitter * transmitter);
     void updateHeadFromGyros(float frametime, SerialInterface * serialInterface);
     void updateFromMouse(int mouseX, int mouseY, int screenWidth, int screenHeight);
     void addBodyYaw(float y) {_bodyYaw += y;};
@@ -96,6 +97,8 @@ public:
     void setMovedHandOffset        (glm::vec3 movedHandOffset        ) { _movedHandOffset = movedHandOffset;}
     void setThrust                 (glm::vec3 newThrust              ) { _thrust          = newThrust; };
     void setDisplayingLookatVectors(bool      displayingLookatVectors) { _head.setRenderLookatVectors(displayingLookatVectors);}
+    void setVelocity               (const glm::vec3 velocity         ) { _velocity = velocity; };
+    void setLeanScale              (float     scale                  ) { _leanScale = scale;}
     void setGravity                (glm::vec3 gravity);
     void setMouseRay               (const glm::vec3 &origin, const glm::vec3 &direction);
     void setOrientation            (const glm::quat& orientation);
@@ -115,17 +118,24 @@ public:
     float            getSpeed                  ()                const { return _speed;}
     float            getHeight                 ()                const { return _height;}
     AvatarMode       getMode                   ()                const { return _mode;}
+    float            getLeanScale              ()                const { return _leanScale;}
+    float            getElapsedTimeStopped     ()                const { return _elapsedTimeStopped;}
+    float            getElapsedTimeMoving      ()                const { return _elapsedTimeMoving;}
+    float            getElapsedTimeSinceCollision()              const { return _elapsedTimeSinceCollision;}
     float            getAbsoluteHeadYaw        () const;
     float            getAbsoluteHeadPitch      () const;
     Head&            getHead                   () {return _head; }
     glm::quat        getOrientation            () const;
     glm::quat        getWorldAlignedOrientation() const;
     
+    glm::vec3 getUprightHeadPosition() const;
+    
     AvatarVoxelSystem* getVoxels() { return &_voxels; }
     
     //  Set what driving keys are being pressed to control thrust levels
     void setDriveKeys(int key, bool val) { _driveKeys[key] = val; };
     bool getDriveKeys(int key) { return _driveKeys[key]; };
+    void jump() { _shouldJump = true; };
 
     //  Set/Get update the thrust that will move the avatar around
     void addThrust(glm::vec3 newThrust) { _thrust += newThrust; };
@@ -175,32 +185,37 @@ private:
     glm::vec3   _movedHandOffset;
     AvatarBall	_bodyBall[ NUM_AVATAR_BODY_BALLS ];
     AvatarMode  _mode;
-    glm::vec3   _cameraPosition;
     glm::vec3   _handHoldingPosition;
     glm::vec3   _velocity;
     glm::vec3   _thrust;
+    bool        _shouldJump;
     float       _speed;
     float       _maxArmLength;
-    glm::quat   _righting;
+    float       _leanScale;
     int         _driveKeys[MAX_DRIVE_KEYS];
     float       _pelvisStandingHeight;
     float       _pelvisFloatingHeight;
+    float       _pelvisToHeadLength;
     float       _height;
     Balls*      _balls;
     AvatarTouch _avatarTouch;
-    float       _distanceToNearestAvatar; //  How close is the nearest avatar?
+    float       _distanceToNearestAvatar;       //  How close is the nearest avatar?
     glm::vec3   _gravity;
     glm::vec3   _worldUpDirection;
     glm::vec3   _mouseRayOrigin;
     glm::vec3   _mouseRayDirection;
     Avatar*     _interactingOther;
     bool        _isMouseTurningRight;
+    float       _elapsedTimeMoving;             //  Timers to drive camera transitions when moving
+    float       _elapsedTimeStopped;
+    float       _elapsedTimeSinceCollision;
     
     AvatarVoxelSystem _voxels;
     
     // private methods...
-    glm::vec3 caclulateAverageEyePosition() { return _head.caclulateAverageEyePosition(); } // get the position smack-dab between the eyes (for lookat)
+    glm::vec3 calculateAverageEyePosition() { return _head.calculateAverageEyePosition(); } // get the position smack-dab between the eyes (for lookat)
     glm::quat computeRotationFromBodyToWorldUp(float proportion = 1.0f) const;
+    float getBallRenderAlpha(int ball, bool lookingInMirror) const;
     void renderBody(bool lookingInMirror, bool renderAvatarBalls);
     void initializeBodyBalls();
     void resetBodyBalls();
@@ -213,7 +228,7 @@ private:
     void updateCollisionWithSphere( glm::vec3 position, float radius, float deltaTime );
     void updateCollisionWithEnvironment();
     void updateCollisionWithVoxels();
-    void applyCollisionWithScene(const glm::vec3& penetration);
+    void applyHardCollision(const glm::vec3& penetration, float elasticity, float damping);
     void applyCollisionWithOtherAvatar( Avatar * other, float deltaTime );
     void checkForMouseRayTouching();
     void renderJointConnectingCone(glm::vec3 position1, glm::vec3 position2, float radius1, float radius2);
diff --git a/interface/src/AvatarVoxelSystem.cpp b/interface/src/AvatarVoxelSystem.cpp
index 8a7708587f..dc3b937da9 100644
--- a/interface/src/AvatarVoxelSystem.cpp
+++ b/interface/src/AvatarVoxelSystem.cpp
@@ -17,7 +17,7 @@
 #include "renderer/ProgramObject.h"
 
 const float AVATAR_TREE_SCALE = 1.0f;
-const int MAX_VOXELS_PER_AVATAR = 2000;
+const int MAX_VOXELS_PER_AVATAR = 10000;
 const int BONE_ELEMENTS_PER_VOXEL = BONE_ELEMENTS_PER_VERTEX * VERTICES_PER_VOXEL;
 
 AvatarVoxelSystem::AvatarVoxelSystem(Avatar* avatar) :
diff --git a/interface/src/Head.cpp b/interface/src/Head.cpp
index d58c29f84d..c7fac99d8b 100644
--- a/interface/src/Head.cpp
+++ b/interface/src/Head.cpp
@@ -5,6 +5,7 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 
 #include <glm/gtx/quaternion.hpp>
+#include "Application.h"
 #include "Avatar.h"
 #include "Head.h"
 #include "Util.h"
@@ -161,7 +162,7 @@ void Head::determineIfLookingAtSomething() {
     if ( fabs(_lookAtPosition.x + _lookAtPosition.y + _lookAtPosition.z) == 0.0 ) { // a lookatPosition of 0,0,0 signifies NOT looking
         _lookingAtSomething = false;
     } else {
-        glm::vec3 targetLookatAxis = glm::normalize(_lookAtPosition - caclulateAverageEyePosition());
+        glm::vec3 targetLookatAxis = glm::normalize(_lookAtPosition - calculateAverageEyePosition());
         float dot = glm::dot(targetLookatAxis, getFrontDirection());
         if (dot < MINIMUM_EYE_ROTATION_DOT) { // too far off from center for the eyes to rotate 
             _lookingAtSomething = false;
@@ -202,7 +203,7 @@ void Head::calculateGeometry() {
 }
 
 
-void Head::render(bool lookingInMirror, glm::vec3 cameraPosition, float alpha) {
+void Head::render(bool lookingInMirror, float alpha) {
 
     _renderAlpha = alpha;
     _lookingInMirror = lookingInMirror;
@@ -212,7 +213,7 @@ void Head::render(bool lookingInMirror, glm::vec3 cameraPosition, float alpha) {
     glEnable(GL_DEPTH_TEST);
     glEnable(GL_RESCALE_NORMAL);
     
-    renderMohawk(cameraPosition);
+    renderMohawk();
     renderHeadSphere();
     renderEyeBalls();    
     renderEars();
@@ -256,7 +257,7 @@ void Head::createMohawk() {
     }
 }
 
-void Head::renderMohawk(glm::vec3 cameraPosition) {
+void Head::renderMohawk() {
     
     if (!_mohawkTriangleFan) {
         createMohawk();
@@ -267,7 +268,7 @@ void Head::renderMohawk(glm::vec3 cameraPosition) {
 
             glm::vec3 baseAxis   = _hairTuft[t].midPosition - _hairTuft[t].basePosition;
             glm::vec3 midAxis    = _hairTuft[t].endPosition - _hairTuft[t].midPosition;
-            glm::vec3 viewVector = _hairTuft[t].basePosition - cameraPosition;
+            glm::vec3 viewVector = _hairTuft[t].basePosition - Application::getInstance()->getCamera()->getPosition();
             
             glm::vec3 basePerpendicular = glm::normalize(glm::cross(baseAxis, viewVector));
             glm::vec3 midPerpendicular  = glm::normalize(glm::cross(midAxis,  viewVector));
diff --git a/interface/src/Head.h b/interface/src/Head.h
index 4150cbbc50..d331b98efc 100644
--- a/interface/src/Head.h
+++ b/interface/src/Head.h
@@ -33,8 +33,8 @@ public:
     
     void reset();
     void simulate(float deltaTime, bool isMine);
-    void render(bool lookingInMirror, glm::vec3 cameraPosition, float alpha);
-    void renderMohawk(glm::vec3 cameraPosition);
+    void render(bool lookingInMirror, float alpha);
+    void renderMohawk();
 
     void setScale          (float     scale             ) { _scale              = scale;              }
     void setPosition       (glm::vec3 position          ) { _position           = position;           }
@@ -55,7 +55,7 @@ public:
     
     const bool getReturnToCenter() const { return _returnHeadToCenter; } // Do you want head to try to return to center (depends on interface detected)
     float getAverageLoudness() {return _averageLoudness;};
-    glm::vec3 caclulateAverageEyePosition() { return _leftEyePosition + (_rightEyePosition - _leftEyePosition ) * ONE_HALF; }
+    glm::vec3 calculateAverageEyePosition() { return _leftEyePosition + (_rightEyePosition - _leftEyePosition ) * ONE_HALF; }
     
     float yawRate;
     float noise;
diff --git a/interface/src/SerialInterface.cpp b/interface/src/SerialInterface.cpp
index 28ae7eceb4..76487bb668 100644
--- a/interface/src/SerialInterface.cpp
+++ b/interface/src/SerialInterface.cpp
@@ -6,6 +6,7 @@
 //
 
 #include "SerialInterface.h"
+#include "SharedUtil.h"
 #include "Util.h"
 #include <glm/gtx/vector_angle.hpp>
 #include <math.h>
@@ -155,11 +156,11 @@ void SerialInterface::renderLevels(int width, int height) {
         // Acceleration rates
         glColor4f(1, 1, 1, 1);
         glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER, LEVEL_CORNER_Y + 42);
-        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)((_lastAcceleration.x - _gravity.x) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 42);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.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)((_lastAcceleration.y - _gravity.y) *ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 57);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.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)((_lastAcceleration.z - _gravity.z) * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
+        glVertex2f(LEVEL_CORNER_X + LEVEL_CENTER + (int)(_estimatedAcceleration.z * ACCEL_VIEW_SCALING), LEVEL_CORNER_Y + 72);
         
         // Estimated Position
         glColor4f(0, 1, 1, 1);
@@ -217,6 +218,7 @@ void SerialInterface::readData(float deltaTime) {
         
         _lastAcceleration = glm::vec3(-accelXRate, -accelYRate, -accelZRate) * LSB_TO_METERS_PER_SECOND2;
                 
+        
         int rollRate, yawRate, pitchRate;
         
         convertHexToInt(sensorBuffer + 22, rollRate);
@@ -225,35 +227,87 @@ void SerialInterface::readData(float deltaTime) {
         
         //  Convert the integer rates to floats
         const float LSB_TO_DEGREES_PER_SECOND = 1.f / 16.4f;     //  From MPU-9150 register map, 2000 deg/sec.
-        _lastRotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
-        _lastRotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
-        _lastRotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
+        glm::vec3 rotationRates;
+        rotationRates[0] = ((float) -pitchRate) * LSB_TO_DEGREES_PER_SECOND;
+        rotationRates[1] = ((float) -yawRate) * LSB_TO_DEGREES_PER_SECOND;
+        rotationRates[2] = ((float) -rollRate) * LSB_TO_DEGREES_PER_SECOND;
 
+        // update and subtract the long term average
+        _averageRotationRates = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates +
+                1.f/(float)LONG_TERM_RATE_SAMPLES * rotationRates;
+        rotationRates -= _averageRotationRates;
+
+        // compute the angular acceleration
+        glm::vec3 angularAcceleration = (deltaTime < EPSILON) ? glm::vec3() : (rotationRates - _lastRotationRates) / deltaTime;
+        _lastRotationRates = rotationRates;
+        
         //  Update raw rotation estimates
         glm::quat estimatedRotation = glm::quat(glm::radians(_estimatedRotation)) *
-            glm::quat(glm::radians(deltaTime * (_lastRotationRates - _averageRotationRates)));
+            glm::quat(glm::radians(deltaTime * _lastRotationRates));
+        
+        //  Update acceleration estimate: first, subtract gravity as rotated into current frame
+        _estimatedAcceleration = (totalSamples < GRAVITY_SAMPLES) ? glm::vec3() :
+            _lastAcceleration - glm::inverse(estimatedRotation) * _gravity;
+        
+        // update and subtract the long term average
+        _averageAcceleration = (1.f - 1.f/(float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration +
+                1.f/(float)LONG_TERM_RATE_SAMPLES * _estimatedAcceleration;
+        _estimatedAcceleration -= _averageAcceleration;
+        
+        //  Consider updating our angular velocity/acceleration to linear acceleration mapping
+        if (glm::length(_estimatedAcceleration) > EPSILON &&
+                (glm::length(_lastRotationRates) > EPSILON || glm::length(angularAcceleration) > EPSILON)) {
+            // compute predicted linear acceleration, find error between actual and predicted
+            glm::vec3 predictedAcceleration = _angularVelocityToLinearAccel * _lastRotationRates +
+                _angularAccelToLinearAccel * angularAcceleration;
+            glm::vec3 error = _estimatedAcceleration - predictedAcceleration;
+            
+            // the "error" is actually what we want: the linear acceleration minus rotational influences
+            _estimatedAcceleration = error;
+            
+            // adjust according to error in each dimension, in proportion to input magnitudes
+            for (int i = 0; i < 3; i++) {
+                if (fabsf(error[i]) < EPSILON) {
+                    continue;
+                }
+                const float LEARNING_RATE = 0.001f;
+                float rateSum = fabsf(_lastRotationRates.x) + fabsf(_lastRotationRates.y) + fabsf(_lastRotationRates.z);
+                if (rateSum > EPSILON) {
+                    for (int j = 0; j < 3; j++) {
+                        float proportion = LEARNING_RATE * fabsf(_lastRotationRates[j]) / rateSum;
+                        if (proportion > EPSILON) {
+                            _angularVelocityToLinearAccel[j][i] += error[i] * proportion / _lastRotationRates[j];
+                        }
+                    }
+                }
+                float accelSum = fabsf(angularAcceleration.x) + fabsf(angularAcceleration.y) + fabsf(angularAcceleration.z);
+                if (accelSum > EPSILON) {
+                    for (int j = 0; j < 3; j++) {
+                        float proportion = LEARNING_RATE * fabsf(angularAcceleration[j]) / accelSum;
+                        if (proportion > EPSILON) {
+                            _angularAccelToLinearAccel[j][i] += error[i] * proportion / angularAcceleration[j];
+                        }
+                    }                
+                }
+            }
+        }
+        
+        // rotate estimated acceleration into global rotation frame
+        _estimatedAcceleration = estimatedRotation * _estimatedAcceleration;
         
         //  Update estimated position and velocity
-        float const DECAY_VELOCITY = 0.95f;
-        float const DECAY_POSITION = 0.95f;
-        _estimatedVelocity += deltaTime * (_lastAcceleration - _averageAcceleration);
+        float const DECAY_VELOCITY = 0.975f;
+        float const DECAY_POSITION = 0.975f;
+        _estimatedVelocity += deltaTime * _estimatedAcceleration;
         _estimatedPosition += deltaTime * _estimatedVelocity;
         _estimatedVelocity *= DECAY_VELOCITY;
         _estimatedPosition *= DECAY_POSITION;
                 
         //  Accumulate a set of initial baseline readings for setting gravity
         if (totalSamples == 0) {
-            _averageRotationRates = _lastRotationRates;
-            _averageAcceleration = _lastAcceleration;
             _gravity = _lastAcceleration;
         } 
         else {
-            //  Cumulate long term average to (hopefully) take DC bias out of rotation rates
-            _averageRotationRates = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageRotationRates
-                                    + 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastRotationRates;
-            _averageAcceleration = (1.f - 1.f / (float)LONG_TERM_RATE_SAMPLES) * _averageAcceleration
-                                    + 1.f / (float)LONG_TERM_RATE_SAMPLES * _lastAcceleration;
-            
             if (totalSamples < GRAVITY_SAMPLES) {
                 _gravity = (1.f - 1.f/(float)GRAVITY_SAMPLES) * _gravity +
                 1.f/(float)GRAVITY_SAMPLES * _lastAcceleration;
@@ -299,6 +353,7 @@ void SerialInterface::resetAverages() {
     _estimatedRotation = glm::vec3(0, 0, 0);
     _estimatedPosition = glm::vec3(0, 0, 0);
     _estimatedVelocity = glm::vec3(0, 0, 0);
+    _estimatedAcceleration = glm::vec3(0, 0, 0);
 }
 
 void SerialInterface::resetSerial() {
diff --git a/interface/src/SerialInterface.h b/interface/src/SerialInterface.h
index 3fc9ea920a..8c918e65ff 100644
--- a/interface/src/SerialInterface.h
+++ b/interface/src/SerialInterface.h
@@ -32,18 +32,27 @@ public:
                         _estimatedPosition(0, 0, 0),
                         _estimatedVelocity(0, 0, 0),
                         _lastAcceleration(0, 0, 0),
-                        _lastRotationRates(0, 0, 0)
+                        _lastRotationRates(0, 0, 0),
+                        _angularVelocityToLinearAccel( // experimentally derived initial values
+                            0.003f, -0.001f, -0.006f, 
+                            -0.005f, -0.001f, -0.006f,
+                            0.010f, 0.004f, 0.007f),
+                        _angularAccelToLinearAccel( // experimentally derived initial values
+                            0.0f, 0.0f, 0.002f,
+                            0.0f, 0.0f, 0.001f,
+                            -0.002f, -0.002f, 0.0f)
                     {}
     
     void pair();
     void readData(float deltaTime);
-    const float getLastPitchRate() const { return _lastRotationRates[0] - _averageRotationRates[0]; }
-    const float getLastYawRate() const { return _lastRotationRates[1] - _averageRotationRates[1]; }
-    const float getLastRollRate() const { return _lastRotationRates[2] - _averageRotationRates[2]; }
+    const float getLastPitchRate() const { return _lastRotationRates[0]; }
+    const float getLastYawRate() const { return _lastRotationRates[1]; }
+    const float getLastRollRate() const { return _lastRotationRates[2]; }
     const glm::vec3& getLastRotationRates() const { return _lastRotationRates; };
     const glm::vec3& getEstimatedRotation() const { return _estimatedRotation; };
     const glm::vec3& getEstimatedPosition() const { return _estimatedPosition; };
     const glm::vec3& getEstimatedVelocity() const { return _estimatedVelocity; };
+    const glm::vec3& getEstimatedAcceleration() const { return _estimatedAcceleration; };
     const glm::vec3& getLastAcceleration() const { return _lastAcceleration; };
     const glm::vec3& getGravity() const { return _gravity; };
     
@@ -64,8 +73,12 @@ private:
     glm::vec3 _estimatedRotation;
     glm::vec3 _estimatedPosition;
     glm::vec3 _estimatedVelocity;
+    glm::vec3 _estimatedAcceleration;
     glm::vec3 _lastAcceleration;
     glm::vec3 _lastRotationRates;
+    
+    glm::mat3 _angularVelocityToLinearAccel;
+    glm::mat3 _angularAccelToLinearAccel;
 };
 
 #endif
diff --git a/interface/src/Skeleton.cpp b/interface/src/Skeleton.cpp
index 2fa43c9010..1167194534 100644
--- a/interface/src/Skeleton.cpp
+++ b/interface/src/Skeleton.cpp
@@ -132,15 +132,15 @@ void Skeleton::update(float deltaTime, const glm::quat& orientation, glm::vec3 p
 
     for (int b = 0; b < NUM_AVATAR_JOINTS; b++) {
         if (joint[b].parent == AVATAR_JOINT_NULL) {
-            joint[b].rotation = orientation;
+            joint[b].absoluteRotation = orientation * joint[b].rotation;
             joint[b].position = position;
         }
         else {
-            joint[b].rotation = joint[ joint[b].parent ].rotation;
+            joint[b].absoluteRotation = joint[ joint[b].parent ].absoluteRotation * joint[b].rotation;
             joint[b].position = joint[ joint[b].parent ].position;
         }
 
-        glm::vec3 rotatedJointVector = joint[b].rotation * joint[b].defaultPosePosition;
+        glm::vec3 rotatedJointVector = joint[b].absoluteRotation * joint[b].defaultPosePosition;
         joint[b].position += rotatedJointVector;
     }    
 }
@@ -174,6 +174,13 @@ float Skeleton::getPelvisFloatingHeight() {
            FLOATING_HEIGHT;
 }
 
+float Skeleton::getPelvisToHeadLength() {
+    return 
+        joint[ AVATAR_JOINT_TORSO     ].length +
+        joint[ AVATAR_JOINT_CHEST     ].length +
+        joint[ AVATAR_JOINT_NECK_BASE ].length +
+        joint[ AVATAR_JOINT_HEAD_BASE ].length; 
+}
 
 
 
diff --git a/interface/src/Skeleton.h b/interface/src/Skeleton.h
index e98c2e7b12..bb953fe947 100644
--- a/interface/src/Skeleton.h
+++ b/interface/src/Skeleton.h
@@ -56,6 +56,7 @@ public:
     float getHeight();
     float getPelvisStandingHeight();
     float getPelvisFloatingHeight();
+    float getPelvisToHeadLength();
     //glm::vec3 getJointVectorFromParent(AvatarJointID jointID) {return joint[jointID].position - joint[joint[jointID].parent].position; }
     
     struct AvatarJoint
@@ -68,6 +69,7 @@ public:
         glm::quat     absoluteBindPoseRotation;  // the absolute rotation when the avatar is in the "T-pose"
         float         bindRadius;                // the radius of the bone capsule that envelops the vertices to bind
         glm::quat     rotation;                  // the parent-relative rotation (orientation) of the joint as a quaternion
+        glm::quat     absoluteRotation;          // the absolute rotation of the joint as a quaternion
         float         length;                    // the length of vector connecting the joint and its parent
     };
 
diff --git a/interface/src/Transmitter.cpp b/interface/src/Transmitter.cpp
index a02e864087..8cbb4a5c29 100644
--- a/interface/src/Transmitter.cpp
+++ b/interface/src/Transmitter.cpp
@@ -70,9 +70,7 @@ void Transmitter::processIncomingData(unsigned char* packetData, int numBytes) {
 
         //  Update estimated absolute position from rotation rates
         _estimatedRotation += _lastRotationRate * DELTA_TIME;
-        
-        printf("The accel %f, %f, %f\n", _lastAcceleration.x, _lastAcceleration.y, _lastAcceleration.z);
-    
+            
         // Sensor Fusion!  Slowly adjust estimated rotation to be relative to gravity (average acceleration)
         const float GRAVITY_FOLLOW_RATE = 1.f;
         float rollAngle = angleBetween(glm::vec3(_lastAcceleration.x, _lastAcceleration.y, 0.f), glm::vec3(0,-1,0)) *
diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp
index 74fe5abd1c..cc566dd6fb 100644
--- a/interface/src/Util.cpp
+++ b/interface/src/Util.cpp
@@ -498,9 +498,9 @@ void runTimingTests() {
 }
 
 float loadSetting(QSettings* settings, const char* name, float defaultValue) {
-    float value = settings->value(name, 0.0f).toFloat();
+    float value = settings->value(name, defaultValue).toFloat();
     if (isnan(value)) {
         value = defaultValue;
     }
     return value;
-}
\ No newline at end of file
+}
diff --git a/interface/src/VoxelSystem.cpp b/interface/src/VoxelSystem.cpp
index 9cde8906be..716fe708a3 100644
--- a/interface/src/VoxelSystem.cpp
+++ b/interface/src/VoxelSystem.cpp
@@ -165,15 +165,15 @@ int VoxelSystem::parseData(unsigned char* sourceBuffer, int numBytes) {
 
 void VoxelSystem::setupNewVoxelsForDrawing() {
     PerformanceWarning warn(_renderWarningsOn, "setupNewVoxelsForDrawing()"); // would like to include _voxelsInArrays, _voxelsUpdated
-    double start = usecTimestampNow();
-    double sinceLastTime = (start - _setupNewVoxelsForDrawingLastFinished) / 1000.0;
+    long long start = usecTimestampNow();
+    long long sinceLastTime = (start - _setupNewVoxelsForDrawingLastFinished) / 1000;
 
     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
     }
 
-    double sinceLastViewCulling = (start - _lastViewCulling) / 1000.0;
+    long long sinceLastViewCulling = (start - _lastViewCulling) / 1000;
     // If the view frustum is no longer changing, but has changed, since last time, then remove nodes that are out of view
     if ((sinceLastViewCulling >= std::max(_lastViewCullingElapsed, VIEW_CULLING_RATE_IN_MILLISECONDS)) 
             && !isViewChanging() && hasViewChanged()) {
@@ -189,8 +189,8 @@ void VoxelSystem::setupNewVoxelsForDrawing() {
         // 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;
+        long long endViewCulling = usecTimestampNow();
+        _lastViewCullingElapsed = (endViewCulling - start) / 1000;
     }    
     
     bool didWriteFullVBO = _writeRenderFullVBO;
@@ -226,8 +226,8 @@ void VoxelSystem::setupNewVoxelsForDrawing() {
 
     pthread_mutex_unlock(&_bufferWriteLock);
 
-    double end = usecTimestampNow();
-    double elapsedmsec = (end - start) / 1000.0;
+    long long end = usecTimestampNow();
+    long long elapsedmsec = (end - start) / 1000;
     _setupNewVoxelsForDrawingLastFinished = end;
     _setupNewVoxelsForDrawingLastElapsed = elapsedmsec;
 }
diff --git a/libraries/audio/src/AudioInjector.cpp b/libraries/audio/src/AudioInjector.cpp
index f9f1bcc094..24e0a369d5 100644
--- a/libraries/audio/src/AudioInjector.cpp
+++ b/libraries/audio/src/AudioInjector.cpp
@@ -115,7 +115,7 @@ void AudioInjector::injectAudio(UDPSocket* injectorSocket, sockaddr* destination
             
             injectorSocket->send(destinationSocket, dataPacket, sizeof(dataPacket));
             
-            double usecToSleep = usecTimestamp(&startTime) + (++nextFrame * INJECT_INTERVAL_USECS) - usecTimestampNow();
+            long long usecToSleep = usecTimestamp(&startTime) + (++nextFrame * INJECT_INTERVAL_USECS) - usecTimestampNow();
             if (usecToSleep > 0) {
                 usleep(usecToSleep);
             }
diff --git a/libraries/shared/src/Agent.cpp b/libraries/shared/src/Agent.cpp
index 4556f670d6..88d0f901f2 100644
--- a/libraries/shared/src/Agent.cpp
+++ b/libraries/shared/src/Agent.cpp
@@ -6,6 +6,8 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 //
 
+#include "stdio.h"
+
 #include <pthread.h>
 #include "Agent.h"
 #include "AgentTypes.h"
diff --git a/libraries/shared/src/Agent.h b/libraries/shared/src/Agent.h
index 5201fd9048..18d42fdf3d 100644
--- a/libraries/shared/src/Agent.h
+++ b/libraries/shared/src/Agent.h
@@ -37,11 +37,11 @@ public:
     uint16_t getAgentID() const { return _agentID; }
     void setAgentID(uint16_t agentID) { _agentID = agentID;}
     
-    double getWakeMicrostamp() const { return _wakeMicrostamp; }
-    void setWakeMicrostamp(double wakeMicrostamp) { _wakeMicrostamp = wakeMicrostamp; }
+    long long getWakeMicrostamp() const { return _wakeMicrostamp; }
+    void setWakeMicrostamp(long long wakeMicrostamp) { _wakeMicrostamp = wakeMicrostamp; }
     
-    double getLastHeardMicrostamp() const { return _lastHeardMicrostamp; }
-    void setLastHeardMicrostamp(double lastHeardMicrostamp) { _lastHeardMicrostamp = lastHeardMicrostamp; }
+    long long getLastHeardMicrostamp() const { return _lastHeardMicrostamp; }
+    void setLastHeardMicrostamp(long long lastHeardMicrostamp) { _lastHeardMicrostamp = lastHeardMicrostamp; }
     
     sockaddr* getPublicSocket() const { return _publicSocket; }
     void setPublicSocket(sockaddr* publicSocket) { _publicSocket = publicSocket; }
@@ -71,8 +71,8 @@ private:
     
     char _type;
     uint16_t _agentID;
-    double _wakeMicrostamp;
-    double _lastHeardMicrostamp;
+    long long _wakeMicrostamp;
+    long long _lastHeardMicrostamp;
     sockaddr* _publicSocket;
     sockaddr* _localSocket;
     sockaddr* _activeSocket;
diff --git a/libraries/shared/src/AgentList.cpp b/libraries/shared/src/AgentList.cpp
index 9c309bb23f..7a4803586e 100644
--- a/libraries/shared/src/AgentList.cpp
+++ b/libraries/shared/src/AgentList.cpp
@@ -62,7 +62,6 @@ AgentList::AgentList(char newOwnerType, unsigned int newSocketListenPort) :
     _agentSocket(newSocketListenPort),
     _ownerType(newOwnerType),
     _agentTypesOfInterest(NULL),
-    _socketListenPort(newSocketListenPort),
     _ownerID(UNKNOWN_AGENT_ID),
     _lastAgentID(0) {
     pthread_mutex_init(&mutex, 0);
@@ -206,6 +205,7 @@ void AgentList::sendDomainServerCheckIn() {
     
     // construct the DS check in packet if we need to
     static unsigned char* checkInPacket = NULL;
+    static int checkInPacketSize;
 
     if (!checkInPacket) {
         int numBytesAgentsOfInterest = _agentTypesOfInterest ? strlen((char*) _agentTypesOfInterest) : 0;
@@ -224,7 +224,7 @@ void AgentList::sendDomainServerCheckIn() {
         
         packetPosition += packSocket(checkInPacket + sizeof(PACKET_HEADER) + sizeof(AGENT_TYPE),
                                      getLocalAddress(),
-                                     htons(_socketListenPort));
+                                     htons(_agentSocket.getListeningPort()));
         
         // add the number of bytes for agent types of interest
         *(packetPosition++) = numBytesAgentsOfInterest;
@@ -237,10 +237,10 @@ void AgentList::sendDomainServerCheckIn() {
             packetPosition += numBytesAgentsOfInterest;
         }
         
-        *packetPosition = '\0';
+        checkInPacketSize = packetPosition - checkInPacket;
     }
     
-    _agentSocket.send(DOMAIN_IP, DOMAINSERVER_PORT, checkInPacket, strlen((char*) checkInPacket));
+    _agentSocket.send(DOMAIN_IP, DOMAINSERVER_PORT, checkInPacket, checkInPacketSize);
 }
 
 int AgentList::processDomainServerList(unsigned char *packetData, size_t dataBytes) {
@@ -393,7 +393,7 @@ void *pingUnknownAgents(void *args) {
             }
         }
         
-        double usecToSleep = PING_INTERVAL_USECS - (usecTimestampNow() - usecTimestamp(&lastSend));
+        long long usecToSleep = PING_INTERVAL_USECS - (usecTimestampNow() - usecTimestamp(&lastSend));
         
         if (usecToSleep > 0) {
             usleep(usecToSleep);
@@ -414,7 +414,7 @@ void AgentList::stopPingUnknownAgentsThread() {
 
 void *removeSilentAgents(void *args) {
     AgentList* agentList = (AgentList*) args;
-    double checkTimeUSecs, sleepTime;
+    long long checkTimeUSecs, sleepTime;
     
     while (!silentAgentThreadStopFlag) {
         checkTimeUSecs = usecTimestampNow();
@@ -423,7 +423,7 @@ void *removeSilentAgents(void *args) {
             
             if ((checkTimeUSecs - agent->getLastHeardMicrostamp()) > AGENT_SILENCE_THRESHOLD_USECS
             	&& agent->getType() != AGENT_TYPE_VOXEL_SERVER) {
-                
+            
                 printLog("Killed ");
                 Agent::printLog(*agent);
                 
diff --git a/libraries/shared/src/AgentList.h b/libraries/shared/src/AgentList.h
index 3007dbc8e3..1b51913928 100644
--- a/libraries/shared/src/AgentList.h
+++ b/libraries/shared/src/AgentList.h
@@ -58,7 +58,7 @@ public:
     
     UDPSocket* getAgentSocket() { return &_agentSocket; }
     
-    unsigned int getSocketListenPort() const { return _socketListenPort; };
+    unsigned int getSocketListenPort() const { return _agentSocket.getListeningPort(); };
     
     void(*linkedDataCreateCallback)(Agent *);
     
diff --git a/libraries/shared/src/PerfStat.cpp b/libraries/shared/src/PerfStat.cpp
index 3c30c62993..2de5aa2816 100644
--- a/libraries/shared/src/PerfStat.cpp
+++ b/libraries/shared/src/PerfStat.cpp
@@ -104,7 +104,7 @@ int PerfStat::DumpStats(char** array) {
 
 // Destructor handles recording all of our stats
 PerformanceWarning::~PerformanceWarning() {
-    double end = usecTimestampNow();
+    long long end = usecTimestampNow();
     double elapsedmsec = (end - _start) / 1000.0;
     if ((_alwaysDisplay || _renderWarningsOn) && elapsedmsec > 1) {
         if (elapsedmsec > 1000) {
diff --git a/libraries/shared/src/PerfStat.h b/libraries/shared/src/PerfStat.h
index 8898899960..f57d49aa46 100644
--- a/libraries/shared/src/PerfStat.h
+++ b/libraries/shared/src/PerfStat.h
@@ -84,7 +84,7 @@ typedef std::map<std::string,PerfStatHistory,std::less<std::string> >::iterator
 
 class PerformanceWarning {
 private:
-	double _start;
+	long long _start;
 	const char* _message;
 	bool _renderWarningsOn;
 	bool _alwaysDisplay;
diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp
index 66a3ba0012..9c4748f501 100644
--- a/libraries/shared/src/SharedUtil.cpp
+++ b/libraries/shared/src/SharedUtil.cpp
@@ -22,14 +22,14 @@
 #include <CoreFoundation/CoreFoundation.h>
 #endif
 
-double usecTimestamp(timeval *time) {
-    return (time->tv_sec * 1000000.0 + time->tv_usec);
+long long usecTimestamp(timeval *time) {
+    return (time->tv_sec * 1000000 + time->tv_usec);
 }
 
-double usecTimestampNow() {
+long long usecTimestampNow() {
     timeval now;
     gettimeofday(&now, NULL);
-    return (now.tv_sec * 1000000.0 + now.tv_usec);
+    return (now.tv_sec * 1000000 + now.tv_usec);
 }
 
 float randFloat () {
@@ -102,12 +102,12 @@ void setAtBit(unsigned char& byte, int bitIndex) {
 }
 
 int  getSemiNibbleAt(unsigned char& byte, int bitIndex) {
-    return (byte >> (7 - bitIndex) & 3); // semi-nibbles store 00, 01, 10, or 11
+    return (byte >> (6 - bitIndex) & 3); // semi-nibbles store 00, 01, 10, or 11
 }
 
 void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) {
     //assert(value <= 3 && value >= 0);
-    byte += ((value & 3) << (7 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11
+    byte += ((value & 3) << (6 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11
 }
 
 bool isInEnvironment(const char* environment) {
diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h
index ba531d23cc..57dc75c7b2 100644
--- a/libraries/shared/src/SharedUtil.h
+++ b/libraries/shared/src/SharedUtil.h
@@ -36,8 +36,8 @@ static const float DECIMETER        = 0.1f;
 static const float CENTIMETER       = 0.01f;
 static const float MILLIIMETER      = 0.001f;
 
-double usecTimestamp(timeval *time);
-double usecTimestampNow();
+long long usecTimestamp(timeval *time);
+long long usecTimestampNow();
 
 float randFloat();
 int randIntInRange (int min, int max);
diff --git a/libraries/shared/src/SimpleMovingAverage.h b/libraries/shared/src/SimpleMovingAverage.h
index e24b639133..b1d0709342 100644
--- a/libraries/shared/src/SimpleMovingAverage.h
+++ b/libraries/shared/src/SimpleMovingAverage.h
@@ -25,7 +25,7 @@ public:
     float getAverageSampleValuePerSecond();
 private:
     int _numSamples;
-    double _lastEventTimestamp;
+    long long _lastEventTimestamp;
     float _average;
     float _eventDeltaAverage;
     
diff --git a/libraries/shared/src/UDPSocket.cpp b/libraries/shared/src/UDPSocket.cpp
index b7c2275635..69cf0cfebe 100644
--- a/libraries/shared/src/UDPSocket.cpp
+++ b/libraries/shared/src/UDPSocket.cpp
@@ -117,7 +117,7 @@ unsigned short loadBufferWithSocketInfo(char* addressBuffer, sockaddr* socket) {
     }
 }
 
-UDPSocket::UDPSocket(int listeningPort) : blocking(true) {
+UDPSocket::UDPSocket(int listeningPort) : listeningPort(listeningPort), blocking(true) {
     init();
     // create the socket
     handle = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
@@ -140,6 +140,13 @@ UDPSocket::UDPSocket(int listeningPort) : blocking(true) {
         return;
     }
     
+    // if we requested an ephemeral port, get the actual port
+    if (listeningPort == 0) {
+        socklen_t addressLength = sizeof(sockaddr_in);
+        getsockname(handle, (sockaddr*) &bind_address, &addressLength);
+        listeningPort = ntohs(bind_address.sin_port);
+    }
+    
     // set timeout on socket recieve to 0.5 seconds
     struct timeval tv;
     tv.tv_sec = 0;
diff --git a/libraries/shared/src/UDPSocket.h b/libraries/shared/src/UDPSocket.h
index b56a1cc0cf..8539ff93c2 100644
--- a/libraries/shared/src/UDPSocket.h
+++ b/libraries/shared/src/UDPSocket.h
@@ -23,14 +23,16 @@ public:
     UDPSocket(int listening_port);
     ~UDPSocket();
     bool init();
+    int getListeningPort() const { return listeningPort; }
     void setBlocking(bool blocking);
-    bool isBlocking() { return blocking; }
+    bool isBlocking() const { return blocking; }
     int send(sockaddr* destAddress, const void* data, size_t byteLength) const;
     int send(char* destAddress, int destPort, const void* data, size_t byteLength) const;
     bool receive(void* receivedData, ssize_t* receivedBytes) const;
     bool receive(sockaddr* recvAddress, void* receivedData, ssize_t* receivedBytes) const;
 private:
     int handle;
+    int listeningPort;
     bool blocking;
 };
 
diff --git a/libraries/voxels/src/VoxelNode.h b/libraries/voxels/src/VoxelNode.h
index 952ad15bb7..4aa64f019e 100644
--- a/libraries/voxels/src/VoxelNode.h
+++ b/libraries/voxels/src/VoxelNode.h
@@ -29,7 +29,7 @@ private:
 #endif
     glBufferIndex _glBufferIndex;
     bool _isDirty;
-    double _lastChanged;
+    long long _lastChanged;
     bool _shouldRender;
     bool _isStagedForDeletion;
     AABox _box;
@@ -80,7 +80,7 @@ public:
     void printDebugDetails(const char* label) const;
     bool isDirty() const { return _isDirty; };
     void clearDirtyBit() { _isDirty = false; };
-    bool hasChangedSince(double time) const { return (_lastChanged > time);  };
+    bool hasChangedSince(long long time) const { return (_lastChanged > time);  };
     void markWithChangedTime() { _lastChanged = usecTimestampNow();  };
     void handleSubtreeChanged(VoxelTree* myTree);
     
@@ -103,8 +103,8 @@ public:
     void setColor(const nodeColor& color);
     const nodeColor& getTrueColor() const { return _trueColor; };
     const nodeColor& getColor() const { return _currentColor; };
-    void setDensity(const float density) { _density = density; };
-    const float getDensity() const { return _density; };
+    void setDensity(float density) { _density = density; };
+    float getDensity() const { return _density; };
 #else
     void setFalseColor(colorPart red, colorPart green, colorPart blue) { /* no op */ };
     void setFalseColored(bool isFalseColored) { /* no op */ };
diff --git a/voxel-server/src/main.cpp b/voxel-server/src/main.cpp
index 1f6a386eda..aae631bb16 100644
--- a/voxel-server/src/main.cpp
+++ b/voxel-server/src/main.cpp
@@ -32,7 +32,7 @@
 
 const char* LOCAL_VOXELS_PERSIST_FILE = "resources/voxels.svo";
 const char* VOXELS_PERSIST_FILE = "/etc/highfidelity/voxel-server/resources/voxels.svo";
-const double VOXEL_PERSIST_INTERVAL = 1000.0 * 30; // every 30 seconds
+const long long VOXEL_PERSIST_INTERVAL = 1000 * 30; // every 30 seconds
 
 const int VOXEL_LISTEN_PORT = 40106;
 
@@ -118,7 +118,7 @@ void resInVoxelDistributor(AgentList* agentList,
     bool searchReset = false;
     int  searchLoops = 0;
     int  searchLevelWas = agentData->getMaxSearchLevel();
-    double start = usecTimestampNow();
+    long long start = usecTimestampNow();
     while (!searchReset && agentData->nodeBag.isEmpty()) {
         searchLoops++;
 
@@ -137,19 +137,19 @@ void resInVoxelDistributor(AgentList* agentList,
             }
         }
     }
-    double end = usecTimestampNow();
-    double elapsedmsec = (end - start)/1000.0;
+    long long end = usecTimestampNow();
+    int elapsedmsec = (end - start)/1000;
     if (elapsedmsec > 100) {
         if (elapsedmsec > 1000) {
-            double elapsedsec = (end - start)/1000000.0;
-            printf("WARNING! searchForColoredNodes() took %lf seconds to identify %d nodes at level %d in %d loops\n",
+            int elapsedsec = (end - start)/1000000;
+            printf("WARNING! searchForColoredNodes() took %d seconds to identify %d nodes at level %d in %d loops\n",
                 elapsedsec, agentData->nodeBag.count(), searchLevelWas, searchLoops);
         } else {
-            printf("WARNING! searchForColoredNodes() took %lf milliseconds to identify %d nodes at level %d in %d loops\n",
+            printf("WARNING! searchForColoredNodes() took %d milliseconds to identify %d nodes at level %d in %d loops\n",
                 elapsedmsec, agentData->nodeBag.count(), searchLevelWas, searchLoops);
         }
     } else if (::debugVoxelSending) {
-        printf("searchForColoredNodes() took %lf milliseconds to identify %d nodes at level %d in %d loops\n",
+        printf("searchForColoredNodes() took %d milliseconds to identify %d nodes at level %d in %d loops\n",
                 elapsedmsec, agentData->nodeBag.count(), searchLevelWas, searchLoops);
     }
 
@@ -161,7 +161,7 @@ void resInVoxelDistributor(AgentList* agentList,
         int packetsSentThisInterval = 0;
         int truePacketsSent = 0;
         int trueBytesSent = 0;
-        double start = usecTimestampNow();
+        long long start = usecTimestampNow();
 
         bool shouldSendEnvironments = shouldDo(ENVIRONMENT_SEND_INTERVAL_USECS, VOXEL_SEND_INTERVAL_USECS);
         while (packetsSentThisInterval < PACKETS_PER_CLIENT_PER_INTERVAL - (shouldSendEnvironments ? 1 : 0)) {
@@ -206,19 +206,19 @@ void resInVoxelDistributor(AgentList* agentList,
             trueBytesSent += envPacketLength;
             truePacketsSent++;
         }
-        double end = usecTimestampNow();
-        double elapsedmsec = (end - start)/1000.0;
+        long long end = usecTimestampNow();
+        int elapsedmsec = (end - start)/1000;
         if (elapsedmsec > 100) {
             if (elapsedmsec > 1000) {
-                double elapsedsec = (end - start)/1000000.0;
-                printf("WARNING! packetLoop() took %lf seconds to generate %d bytes in %d packets at level %d, %d nodes still to send\n",
+                int elapsedsec = (end - start)/1000000;
+                printf("WARNING! packetLoop() took %d seconds to generate %d bytes in %d packets at level %d, %d nodes still to send\n",
                         elapsedsec, trueBytesSent, truePacketsSent, searchLevelWas, agentData->nodeBag.count());
             } else {
-                printf("WARNING! packetLoop() took %lf milliseconds to generate %d bytes in %d packets at level %d, %d nodes still to send\n",
+                printf("WARNING! packetLoop() took %d milliseconds to generate %d bytes in %d packets at level %d, %d nodes still to send\n",
                         elapsedmsec, trueBytesSent, truePacketsSent, searchLevelWas, agentData->nodeBag.count());
             }
         } else if (::debugVoxelSending) {
-            printf("packetLoop() took %lf milliseconds to generate %d bytes in %d packets at level %d, %d nodes still to send\n",
+            printf("packetLoop() took %d milliseconds to generate %d bytes in %d packets at level %d, %d nodes still to send\n",
                     elapsedmsec, trueBytesSent, truePacketsSent, searchLevelWas, agentData->nodeBag.count());
         }
 
@@ -245,7 +245,7 @@ void deepestLevelVoxelDistributor(AgentList* agentList,
     pthread_mutex_lock(&::treeLock);
 
     int maxLevelReached = 0;
-    double start = usecTimestampNow();
+    long long start = usecTimestampNow();
 
     // FOR NOW... agent tells us if it wants to receive only view frustum deltas
     bool wantDelta = agentData->getWantDelta();
@@ -281,19 +281,19 @@ void deepestLevelVoxelDistributor(AgentList* agentList,
         }
 
     }
-    double end = usecTimestampNow();
-    double elapsedmsec = (end - start)/1000.0;
+    long long end = usecTimestampNow();
+    int elapsedmsec = (end - start)/1000;
     if (elapsedmsec > 100) {
         if (elapsedmsec > 1000) {
-            double elapsedsec = (end - start)/1000000.0;
-            printf("WARNING! searchForColoredNodes() took %lf seconds to identify %d nodes at level %d\n",
+            int elapsedsec = (end - start)/1000000;
+            printf("WARNING! searchForColoredNodes() took %d seconds to identify %d nodes at level %d\n",
                 elapsedsec, agentData->nodeBag.count(), maxLevelReached);
         } else {
-            printf("WARNING! searchForColoredNodes() took %lf milliseconds to identify %d nodes at level %d\n",
+            printf("WARNING! searchForColoredNodes() took %d milliseconds to identify %d nodes at level %d\n",
                 elapsedmsec, agentData->nodeBag.count(), maxLevelReached);
         }
     } else if (::debugVoxelSending) {
-        printf("searchForColoredNodes() took %lf milliseconds to identify %d nodes at level %d\n",
+        printf("searchForColoredNodes() took %d milliseconds to identify %d nodes at level %d\n",
                 elapsedmsec, agentData->nodeBag.count(), maxLevelReached);
     }
 
@@ -304,7 +304,7 @@ void deepestLevelVoxelDistributor(AgentList* agentList,
         int packetsSentThisInterval = 0;
         int truePacketsSent = 0;
         int trueBytesSent = 0;
-        double start = usecTimestampNow();
+        long long start = usecTimestampNow();
 
         bool shouldSendEnvironments = shouldDo(ENVIRONMENT_SEND_INTERVAL_USECS, VOXEL_SEND_INTERVAL_USECS);
         while (packetsSentThisInterval < PACKETS_PER_CLIENT_PER_INTERVAL - (shouldSendEnvironments ? 1 : 0)) {
@@ -351,19 +351,19 @@ void deepestLevelVoxelDistributor(AgentList* agentList,
             truePacketsSent++;
         }
         
-        double end = usecTimestampNow();
-        double elapsedmsec = (end - start)/1000.0;
+        long long end = usecTimestampNow();
+        int elapsedmsec = (end - start)/1000;
         if (elapsedmsec > 100) {
             if (elapsedmsec > 1000) {
-                double elapsedsec = (end - start)/1000000.0;
-                printf("WARNING! packetLoop() took %lf seconds to generate %d bytes in %d packets %d nodes still to send\n",
+                int elapsedsec = (end - start)/1000000;
+                printf("WARNING! packetLoop() took %d seconds to generate %d bytes in %d packets %d nodes still to send\n",
                         elapsedsec, trueBytesSent, truePacketsSent, agentData->nodeBag.count());
             } else {
-                printf("WARNING! packetLoop() took %lf milliseconds to generate %d bytes in %d packets, %d nodes still to send\n",
+                printf("WARNING! packetLoop() took %d milliseconds to generate %d bytes in %d packets, %d nodes still to send\n",
                         elapsedmsec, trueBytesSent, truePacketsSent, agentData->nodeBag.count());
             }
         } else if (::debugVoxelSending) {
-            printf("packetLoop() took %lf milliseconds to generate %d bytes in %d packets, %d nodes still to send\n",
+            printf("packetLoop() took %d milliseconds to generate %d bytes in %d packets, %d nodes still to send\n",
                     elapsedmsec, trueBytesSent, truePacketsSent, agentData->nodeBag.count());
         }
         
@@ -380,10 +380,10 @@ void deepestLevelVoxelDistributor(AgentList* agentList,
     pthread_mutex_unlock(&::treeLock);
 }
 
-double lastPersistVoxels = 0;
+long long lastPersistVoxels = 0;
 void persistVoxelsWhenDirty() {
-    double now = usecTimestampNow();
-    double sinceLastTime = (now - ::lastPersistVoxels) / 1000.0;
+    long long now = usecTimestampNow();
+    long long sinceLastTime = (now - ::lastPersistVoxels) / 1000;
 
     // check the dirty bit and persist here...
     if (::wantVoxelPersist && ::serverTree.isDirty() && sinceLastTime > VOXEL_PERSIST_INTERVAL) {
@@ -428,7 +428,7 @@ void *distributeVoxelsToListeners(void *args) {
         }
         
         // dynamically sleep until we need to fire off the next set of voxels
-        double usecToSleep =  VOXEL_SEND_INTERVAL_USECS - (usecTimestampNow() - usecTimestamp(&lastSendTime));
+        long long usecToSleep =  VOXEL_SEND_INTERVAL_USECS - (usecTimestampNow() - usecTimestamp(&lastSendTime));
         
         if (usecToSleep > 0) {
             usleep(usecToSleep);