Merge pull request #389 from ey6es/master

Fixed threading issue with Environment, some progress on orienting based on gravity (still a work in progress).

interface/src/Avatar.cpp

@@ -6,6 +6,7 @@
 //  Copyright (c) 2013 High Fidelity, Inc. All rights reserved.
 
 #include <glm/glm.hpp>
+#include <glm/gtx/quaternion.hpp>
 #include <vector>
 #include <lodepng.h>
 #include <SharedUtil.h>
@@ -50,6 +51,7 @@ const float HEAD_MIN_YAW                  = -85;
 const float PERIPERSONAL_RADIUS           = 1.0f;
 const float AVATAR_BRAKING_STRENGTH       = 40.0f;
 const float JOINT_TOUCH_RANGE             = 0.0005f;
+const float ANGULAR_RIGHTING_SPEED        = 45.0f;
 
 float skinColor    [] = {1.0, 0.84, 0.66};
 float darkSkinColor[] = {0.9, 0.78, 0.63};
@@ -335,7 +337,21 @@ void Avatar::simulate(float deltaTime, Transmitter* transmitter) {
     _bodyRoll  *= tiltDecay;
     
     //the following will be used to make the avatar upright no matter what gravity is
-    //float f = angleBetween(_orientation.getUp(), _gravity);
+    float gravityLength = glm::length(_gravity);
+    if (gravityLength > 0.0f) {
+        glm::vec3 targetUp = _gravity / -gravityLength;
+        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;
+            if (angle > 180.0f - EPSILON) { // 180 degree rotation; must use another axis
+                axis = _orientation.getRight();
+            } else {
+                axis = glm::normalize(glm::cross(currentUp, targetUp));
+            }
+            _righting = glm::angleAxis(min(deltaTime * ANGULAR_RIGHTING_SPEED, angle), axis) * _righting;
+        }
+    }
     
     // update position by velocity
     _position += _velocity * deltaTime;
@@ -985,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__) */

libraries/avatars/src/Orientation.h

@@ -19,34 +19,36 @@ const glm::vec3 IDENTITY_FRONT = glm::vec3( 0.0f, 0.0f, 1.0f);
 class Orientation
 {
 public:
-	Orientation();
-	
-	void set(Orientation);
-	void setToPitchYawRoll(float pitch_change, float yaw_change, float roll_change);
-	void setToIdentity();
+    Orientation();
+    
+    void set(Orientation);
+    void setToPitchYawRoll(float pitch_change, float yaw_change, float roll_change);
+    void setToIdentity();
 
-	void pitch(float pitch_change);
-	void yaw  (float yaw_change);
-	void roll (float roll_change);
+    void pitch(float pitch_change);
+    void yaw  (float yaw_change);
+    void roll (float roll_change);
     
     void rotate(float pitch, float yaw, float roll);
     void rotate(glm::vec3 EulerAngles);
     void rotate(glm::quat quaternion);
 
-	const glm::vec3 & getRight() const {return  right;}
-	const glm::vec3 & getUp   () const {return  up;   }
-	const glm::vec3 & getFront() const {return  front;}
+    const glm::quat& getQuat() const {return quat;}
 
-	const glm::vec3 & getIdentityRight() const {return IDENTITY_RIGHT;}
-	const glm::vec3 & getIdentityUp   () const {return IDENTITY_UP;}
-	const glm::vec3 & getIdentityFront() const {return IDENTITY_FRONT;}
+    const glm::vec3& getRight() const {return  right;}
+    const glm::vec3& getUp   () const {return  up;   }
+    const glm::vec3& getFront() const {return  front;}
+
+    const glm::vec3& getIdentityRight() const {return IDENTITY_RIGHT;}
+    const glm::vec3& getIdentityUp   () const {return IDENTITY_UP;}
+    const glm::vec3& getIdentityFront() const {return IDENTITY_FRONT;}
 
 private:
 
     glm::quat quat;
     glm::vec3 right;
-	glm::vec3 up;
-	glm::vec3 front;
+    glm::vec3 up;
+    glm::vec3 front;
     
     void rotateAndGenerateDirections(glm::quat rotation);
 };