Fixed threading issue with Environment, more work on avatar righting.

interface/src/Avatar.cpp

@@ -340,7 +340,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     float gravityLength = glm::length(_gravity);
     if (gravityLength > 0.0f) {
         glm::vec3 targetUp = _gravity / -gravityLength;
-        const glm::vec3& currentUp = _orientation.getUp();
+        const glm::vec3& currentUp = _righting * glm::vec3(0.0f, 1.0f, 0.0f);
         float angle = glm::degrees(acosf(glm::dot(currentUp, targetUp)));
         if (angle > 0.0f) {
             glm::vec3 axis;
@@ -349,11 +349,7 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
             } else {
                 axis = glm::normalize(glm::cross(currentUp, targetUp));
             }
-            _orientation.rotate(glm::angleAxis(min(deltaTime * ANGULAR_RIGHTING_SPEED, angle), axis));
-            glm::vec3 eulerAngles = glm::eulerAngles(_orientation.getQuat());
-            _bodyYaw = eulerAngles.y;
-            _bodyPitch = eulerAngles.x;
-            _bodyRoll = eulerAngles.z;
+            _righting = glm::angleAxis(min(deltaTime * ANGULAR_RIGHTING_SPEED, angle), axis) * _righting;
         }
     }
     
@@ -616,7 +612,7 @@ void Avatar::updateCollisionWithSphere(glm::vec3 position, float radius, float d
 
 void Avatar::updateCollisionWithEnvironment() {
     if (_position.y < _pelvisStandingHeight) {
-        applyCollisionWithScene(glm::vec3(0.0f, _pelvisStandingHeight - _position.y, 0.0f));
+        //applyCollisionWithScene(glm::vec3(0.0f, _pelvisStandingHeight - _position.y, 0.0f));
     }
 
     float radius = _height * 0.125f;
@@ -1005,6 +1001,7 @@ void Avatar::updateSkeleton() {
     _orientation.yaw  (_bodyYaw  );
     _orientation.pitch(_bodyPitch);
     _orientation.roll (_bodyRoll );
+    _orientation.rotate(_righting);
     
     // calculate positions of all bones by traversing the skeleton tree:
     for (int b = 0; b < NUM_AVATAR_JOINTS; b++) {

interface/src/Avatar.h

@@ -169,6 +169,7 @@ private:
     float       _speed;
     float		_maxArmLength;
     Orientation	_orientation;
+    glm::quat   _righting;
     int         _driveKeys[MAX_DRIVE_KEYS];
     float       _pelvisStandingHeight;
     float       _height;

interface/src/Environment.cpp

@@ -6,6 +6,7 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 
 #include <QByteArray>
+#include <QMutexLocker>
 #include <QtDebug>
 
 #include <GeometryUtil.h>
@@ -40,6 +41,9 @@ void Environment::init() {
 }
 
 void Environment::renderAtmospheres(Camera& camera) {    
+    // get the lock for the duration of the call
+    QMutexLocker locker(&_mutex);
+    
     foreach (const ServerData& serverData, _data) {
         foreach (const EnvironmentData& environmentData, serverData) {
             renderAtmosphere(camera, environmentData);
@@ -47,7 +51,10 @@ void Environment::renderAtmospheres(Camera& camera) {
     }
 }
 
-glm::vec3 Environment::getGravity (const glm::vec3& position) const {
+glm::vec3 Environment::getGravity (const glm::vec3& position) {
+    // get the lock for the duration of the call
+    QMutexLocker locker(&_mutex);
+    
     glm::vec3 gravity;
     foreach (const ServerData& serverData, _data) {
         foreach (const EnvironmentData& environmentData, serverData) {
@@ -60,24 +67,30 @@ glm::vec3 Environment::getGravity (const glm::vec3& position) const {
     return gravity;
 }
 
-const EnvironmentData& Environment::getClosestData(const glm::vec3& position) const {
-    const EnvironmentData* closest;
+const EnvironmentData Environment::getClosestData(const glm::vec3& position) {
+    // get the lock for the duration of the call
+    QMutexLocker locker(&_mutex);
+    
+    EnvironmentData closest;
     float closestDistance = FLT_MAX;
     foreach (const ServerData& serverData, _data) {
         foreach (const EnvironmentData& environmentData, serverData) {
             float distance = glm::distance(position, environmentData.getAtmosphereCenter()) -
                 environmentData.getAtmosphereOuterRadius();
             if (distance < closestDistance) {
-                closest = &environmentData;
+                closest = environmentData;
                 closestDistance = distance;
             }
         }
     }
-    return *closest;
+    return closest;
 }
 
 bool Environment::findCapsulePenetration(const glm::vec3& start, const glm::vec3& end,
-                                         float radius, glm::vec3& penetration) const {
+                                         float radius, glm::vec3& penetration) {
+    // get the lock for the duration of the call
+    QMutexLocker locker(&_mutex);
+    
     bool found = false;
     penetration = glm::vec3(0.0f, 0.0f, 0.0f);
     foreach (const ServerData& serverData, _data) {
@@ -98,6 +111,9 @@ int Environment::parseData(sockaddr *senderAddress, unsigned char* sourceBuffer,
     EnvironmentData newData;
     int bytesRead = newData.parseData(sourceBuffer, numBytes);
     
+    // get the lock for the duration of the call
+    QMutexLocker locker(&_mutex);
+    
     // update the mapping by address/ID
     _data[*senderAddress][newData.getID()] = newData;
     

interface/src/Environment.h

@@ -10,6 +10,7 @@
 #define __interface__Environment__
 
 #include <QHash>
+#include <QMutex>
 
 #include <UDPSocket.h>
 
@@ -25,10 +26,10 @@ public:
     void init();
     void renderAtmospheres(Camera& camera);
     
-    glm::vec3 getGravity (const glm::vec3& position) const;
-    const EnvironmentData& getClosestData(const glm::vec3& position) const;
+    glm::vec3 getGravity (const glm::vec3& position);
+    const EnvironmentData getClosestData(const glm::vec3& position);
     
-    bool findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration) const;
+    bool findCapsulePenetration(const glm::vec3& start, const glm::vec3& end, float radius, glm::vec3& penetration);
     
     int parseData(sockaddr *senderAddress, unsigned char* sourceBuffer, int numBytes);
     
@@ -67,6 +68,8 @@ private:
     typedef QHash<int, EnvironmentData> ServerData;
     
     QHash<sockaddr, ServerData> _data;
+    
+    QMutex _mutex;
 };
 
 #endif /* defined(__interface__Environment__) */