Use AvatarData::getClientGlobalPosition() for position in priority queue

It looks like it's the same as world position with the avatar mixer.
Also use one time stamp for current time in priority queue; use
std::chrono for some other timestamps in hope that it's faster.
Fix priority-code logic bug.

assignment-client/src/avatars/AvatarMixerSlave.cpp

@@ -13,6 +13,7 @@
 
 #include <algorithm>
 #include <random>
+#include <chrono>
 
 #include <glm/glm.hpp>
 #include <glm/gtx/norm.hpp>
@@ -33,6 +34,8 @@
 #include "AvatarMixer.h"
 #include "AvatarMixerClientData.h"
 
+namespace chrono = std::chrono;
+
 void AvatarMixerSlave::configure(ConstIter begin, ConstIter end) {
     _begin = begin;
     _end = end;
@@ -301,9 +304,7 @@ void AvatarMixerSlave::broadcastAvatarDataToAgent(const SharedNodePointer& node)
     };
     // Temporary info about the avatars we're sending:
     std::vector<AvatarSortData> avatarsToSort;
-    //std::unordered_map<const AvatarData*, Node*> avatarDataToNodes;
-    //std::unordered_map<const AvatarData*, SharedNodePointer> avatarDataToNodesShared;
-    //std::unordered_map<QUuid, uint64_t> avatarEncodeTimes;
+    avatarsToSort.reserve(_end - _begin);
     std::for_each(_begin, _end, [&](const SharedNodePointer& otherNode) {
         Node* otherNodeRaw = otherNode.data();
         // make sure this is an agent that we have avatar data for before considering it for inclusion
@@ -313,9 +314,6 @@ void AvatarMixerSlave::broadcastAvatarDataToAgent(const SharedNodePointer& node)
 
 
             AvatarData* otherAvatar = otherNodeData->getAvatarSharedPointer().get();
-            //avatarsToSort.push_back(otherAvatar);
-            //avatarDataToNodes[otherAvatar] = otherNode.data();
-            //avatarDataToNodesShared[otherAvatar] = otherNode;
             auto lastEncodeTime = nodeData->getLastOtherAvatarEncodeTime(otherAvatar->getSessionUUID());
             avatarsToSort.emplace_back(AvatarSortData(otherNode, otherAvatar, lastEncodeTime));
         }
@@ -326,7 +324,7 @@ void AvatarMixerSlave::broadcastAvatarDataToAgent(const SharedNodePointer& node)
         SortableAvatar() = delete;
         SortableAvatar(const AvatarData* avatar, const Node* avatarNode, uint64_t lastEncodeTime)
             : _avatar(avatar), _node(avatarNode), _lastEncodeTime(lastEncodeTime) {}
-        glm::vec3 getPosition() const override { return _avatar->getWorldPosition(); }
+        glm::vec3 getPosition() const override { return _avatar->getClientGlobalPosition(); }
         float getRadius() const override {
             glm::vec3 nodeBoxHalfScale = (_avatar->getWorldPosition() - _avatar->getGlobalBoundingBoxCorner() * _avatar->getSensorToWorldScale());
             return glm::max(nodeBoxHalfScale.x, glm::max(nodeBoxHalfScale.y, nodeBoxHalfScale.z));
@@ -452,7 +450,7 @@ void AvatarMixerSlave::broadcastAvatarDataToAgent(const SharedNodePointer& node)
         int minimRemainingAvatarBytes = minimumBytesPerAvatar * remainingAvatars;
         bool overBudget = (identityBytesSent + numAvatarDataBytes + minimRemainingAvatarBytes) > maxAvatarBytesPerFrame;
 
-        quint64 startAvatarDataPacking = usecTimestampNow();
+        auto startAvatarDataPacking = chrono::high_resolution_clock::now();
 
         ++numOtherAvatars;
 
@@ -504,12 +502,13 @@ void AvatarMixerSlave::broadcastAvatarDataToAgent(const SharedNodePointer& node)
         AvatarDataPacket::HasFlags hasFlagsOut; // the result of the toByteArray
         bool dropFaceTracking = false;
 
-        quint64 start = usecTimestampNow();
+        auto startSerialize = chrono::high_resolution_clock::now();
         QByteArray bytes = otherAvatar->toByteArray(detail, lastEncodeForOther, lastSentJointsForOther,
                                                     hasFlagsOut, dropFaceTracking, distanceAdjust, viewerPosition,
                                                     &lastSentJointsForOther);
-        quint64 end = usecTimestampNow();
-        _stats.toByteArrayElapsedTime += (end - start);
+        auto endSerialize = chrono::high_resolution_clock::now();
+        _stats.toByteArrayElapsedTime +=
+            (quint64) chrono::duration_cast<chrono::microseconds>(endSerialize - startSerialize).count();
 
         static auto maxAvatarDataBytes = avatarPacketList->getMaxSegmentSize() - NUM_BYTES_RFC4122_UUID;
         if (bytes.size() > maxAvatarDataBytes) {
@@ -558,8 +557,9 @@ void AvatarMixerSlave::broadcastAvatarDataToAgent(const SharedNodePointer& node)
 
         avatarPacketList->endSegment();
 
-        quint64 endAvatarDataPacking = usecTimestampNow();
-        _stats.avatarDataPackingElapsedTime += (endAvatarDataPacking - startAvatarDataPacking);
+        auto endAvatarDataPacking = chrono::high_resolution_clock::now();
+        _stats.avatarDataPackingElapsedTime +=
+            (quint64) chrono::duration_cast<chrono::microseconds>(endAvatarDataPacking - startAvatarDataPacking).count();
 
         // use helper to add any changed traits to our packet list
         traitBytesSent += addChangedTraitsToBulkPacket(nodeData, otherNodeData, *traitsPacketList);

libraries/shared/src/PrioritySortUtil.h

@@ -87,6 +87,7 @@ namespace PrioritySortUtil {
         PriorityQueue(const ConicalViewFrustums& views) : _views(views) { }
         PriorityQueue(const ConicalViewFrustums& views, float angularWeight, float centerWeight, float ageWeight)
                 : _views(views), _angularWeight(angularWeight), _centerWeight(centerWeight), _ageWeight(ageWeight)
+                , _usecCurrentTime(usecTimestampNow())
         { }
 
         void setViews(const ConicalViewFrustums& views) { _views = views; }
@@ -95,6 +96,7 @@ namespace PrioritySortUtil {
             _angularWeight = angularWeight;
             _centerWeight = centerWeight;
             _ageWeight = ageWeight;
+            _usecCurrentTime = usecTimestampNow();
         }
 
         size_t size() const { return _queue.size(); }
@@ -129,16 +131,17 @@ namespace PrioritySortUtil {
             glm::vec3 offset = position - view.getPosition();
             float distance = glm::length(offset) + 0.001f; // add 1mm to avoid divide by zero
             const float MIN_RADIUS = 0.1f; // WORKAROUND for zero size objects (we still want them to sort by distance)
-            float radius = glm::min(thing.getRadius(), MIN_RADIUS);
+            float radius = glm::max(thing.getRadius(), MIN_RADIUS);
+            // Other item's angle from view centre:
             float cosineAngle = (glm::dot(offset, view.getDirection()) / distance);
-            float age = (float)(usecTimestampNow() - thing.getTimestamp());
+            float age = (float)(_usecCurrentTime - thing.getTimestamp());
 
-            // we modulatate "age" drift rate by the cosineAngle term to make periphrial objects sort forward
+            // we modulate "age" drift rate by the cosineAngle term to make peripheral objects sort forward
             // at a reduced rate but we don't want the "age" term to go zero or negative so we clamp it
             const float MIN_COSINE_ANGLE_FACTOR = 0.1f;
             float cosineAngleFactor = glm::max(cosineAngle, MIN_COSINE_ANGLE_FACTOR);
 
-            float priority = _angularWeight * glm::max(radius, MIN_RADIUS) / distance
+            float priority = _angularWeight * radius / distance
                 + _centerWeight * cosineAngle
                 + _ageWeight * cosineAngleFactor * age;
 
@@ -157,6 +160,7 @@ namespace PrioritySortUtil {
         float _angularWeight { DEFAULT_ANGULAR_COEF };
         float _centerWeight { DEFAULT_CENTER_COEF };
         float _ageWeight { DEFAULT_AGE_COEF };
+        quint64 _usecCurrentTime { 0 };
     };
 } // namespace PrioritySortUtil