diff --git a/interface/src/Avatar.cpp b/interface/src/Avatar.cpp index c4d2ca8ec7..3d47cdad0a 100644 --- a/interface/src/Avatar.cpp +++ b/interface/src/Avatar.cpp @@ -6,6 +6,7 @@ // Copyright (c) 2013 High Fidelity, Inc. All rights reserved. #include +#include #include #include #include @@ -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++) { diff --git a/interface/src/Avatar.h b/interface/src/Avatar.h index 8fe4030446..34ed0004ff 100644 --- a/interface/src/Avatar.h +++ 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; diff --git a/interface/src/Environment.cpp b/interface/src/Environment.cpp index 6b5cca5bc4..990dd83698 100644 --- a/interface/src/Environment.cpp +++ b/interface/src/Environment.cpp @@ -6,6 +6,7 @@ // Copyright (c) 2013 High Fidelity, Inc. All rights reserved. #include +#include #include #include @@ -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; diff --git a/interface/src/Environment.h b/interface/src/Environment.h index 95112806e5..f711f0c38c 100644 --- a/interface/src/Environment.h +++ b/interface/src/Environment.h @@ -10,6 +10,7 @@ #define __interface__Environment__ #include +#include #include @@ -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 ServerData; QHash _data; + + QMutex _mutex; }; #endif /* defined(__interface__Environment__) */ diff --git a/libraries/avatars/src/Orientation.h b/libraries/avatars/src/Orientation.h index 82d6edc9e5..e546a95d60 100755 --- a/libraries/avatars/src/Orientation.h +++ b/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); };