Merge pull request #1412 from ZappoMan/bugfixes

Implement support for adjusting particles to clock skew

interface/src/Application.cpp

@@ -1251,12 +1251,10 @@ void Application::sendPingPackets() {
     const char nodesToPing[] = {NODE_TYPE_VOXEL_SERVER, NODE_TYPE_PARTICLE_SERVER, 
                                 NODE_TYPE_AUDIO_MIXER, NODE_TYPE_AVATAR_MIXER};
 
-    uint64_t currentTime = usecTimestampNow();
+    unsigned char pingPacket[MAX_PACKET_SIZE];
-    unsigned char pingPacket[numBytesForPacketHeader((unsigned char*) &PACKET_TYPE_PING) + sizeof(currentTime)];
+    int length = NodeList::getInstance()->fillPingPacket(pingPacket);
-    int numHeaderBytes = populateTypeAndVersion(pingPacket, PACKET_TYPE_PING);
 
-    memcpy(pingPacket + numHeaderBytes, &currentTime, sizeof(currentTime));
+    getInstance()->controlledBroadcastToNodes(pingPacket, length,
-    getInstance()->controlledBroadcastToNodes(pingPacket, sizeof(pingPacket),
                                               nodesToPing, sizeof(nodesToPing));
 }
 
@@ -4239,15 +4237,15 @@ void Application::trackIncomingVoxelPacket(unsigned char* messageData, ssize_t m
                         const HifiSockAddr& senderSockAddr, bool wasStatsPacket) {
                         
     // Attempt to identify the sender from it's address.
-    Node* voxelServer = NodeList::getInstance()->nodeWithAddress(senderSockAddr);
+    Node* serverNode = NodeList::getInstance()->nodeWithAddress(senderSockAddr);
-    if (voxelServer) {
+    if (serverNode) {
-        QUuid nodeUUID = voxelServer->getUUID();
+        QUuid nodeUUID = serverNode->getUUID();
         
         // now that we know the node ID, let's add these stats to the stats for that node...
         _voxelSceneStatsLock.lockForWrite();
         if (_octreeServerSceneStats.find(nodeUUID) != _octreeServerSceneStats.end()) {
             VoxelSceneStats& stats = _octreeServerSceneStats[nodeUUID];
-            stats.trackIncomingOctreePacket(messageData, messageLength, wasStatsPacket);
+            stats.trackIncomingOctreePacket(messageData, messageLength, wasStatsPacket, serverNode->getClockSkewUsec());
         }
         _voxelSceneStatsLock.unlock();
     }

interface/src/VoxelPacketProcessor.cpp

@@ -56,12 +56,12 @@ void VoxelPacketProcessor::processPacket(const HifiSockAddr& senderSockAddr, uns
     if (Menu::getInstance()->isOptionChecked(MenuOption::Voxels)) {
         app->trackIncomingVoxelPacket(packetData, messageLength, senderSockAddr, wasStatsPacket);
         
-        Node* voxelServer = NodeList::getInstance()->nodeWithAddress(senderSockAddr);
+        Node* serverNode = NodeList::getInstance()->nodeWithAddress(senderSockAddr);
-        if (voxelServer && *voxelServer->getActiveSocket() == senderSockAddr) {
+        if (serverNode && serverNode->getActiveSocket() && *serverNode->getActiveSocket() == senderSockAddr) {
         
             switch(packetData[0]) {
                 case PACKET_TYPE_PARTICLE_DATA: {
-                    app->_particles.processDatagram(QByteArray((char*) packetData, messageLength), senderSockAddr);
+                    app->_particles.processDatagram(QByteArray((char*) packetData, messageLength), senderSockAddr, serverNode);
                 } break;
                 
                 case PACKET_TYPE_ENVIRONMENT_DATA: {
@@ -69,7 +69,7 @@ void VoxelPacketProcessor::processPacket(const HifiSockAddr& senderSockAddr, uns
                 } break;
                 
                 default : {
-                    app->_voxels.setDataSourceUUID(voxelServer->getUUID());
+                    app->_voxels.setDataSourceUUID(serverNode->getUUID());
                     app->_voxels.parseData(packetData, messageLength);
                     app->_voxels.setDataSourceUUID(QUuid());
                 } break;

interface/src/ui/VoxelStatsDialog.cpp

@@ -34,16 +34,16 @@ VoxelStatsDialog::VoxelStatsDialog(QWidget* parent, NodeToVoxelSceneStats* model
         _labels[i] = NULL;
     }
 
-    this->setWindowTitle("Voxel Statistics");
+    this->setWindowTitle("Octree Server Statistics");
 
     // Create layouter
     _form = new QFormLayout();
     this->QDialog::setLayout(_form);
 
     // Setup stat items
-    _serverVoxels = AddStatItem("Voxels on Servers");
+    _serverVoxels = AddStatItem("Elements on Servers");
-    _localVoxels = AddStatItem("Local Voxels");
+    _localVoxels = AddStatItem("Local Elements");
-    _localVoxelsMemory = AddStatItem("Voxels Memory");
+    _localVoxelsMemory = AddStatItem("Elements Memory");
     _voxelsRendered = AddStatItem("Voxels Rendered");
     _sendingMode = AddStatItem("Sending Mode");
     
@@ -136,7 +136,7 @@ void VoxelStatsDialog::paintEvent(QPaintEvent* event) {
     label = _labels[_localVoxelsMemory];
     statsValue.str("");
     statsValue << 
-        "Nodes RAM: " << OctreeElement::getTotalMemoryUsage() / 1000000.f << "MB "
+        "Elements RAM: " << OctreeElement::getTotalMemoryUsage() / 1000000.f << "MB "
         "Geometry RAM: " << voxels->getVoxelMemoryUsageRAM() / 1000000.f << "MB " <<
         "VBO: " << voxels->getVoxelMemoryUsageVBO() / 1000000.f << "MB ";
     if (voxels->hasVoxelMemoryUsageGPU()) {
@@ -344,7 +344,11 @@ void VoxelStatsDialog::showOctreeServersOfType(int& serverCount, NODE_TYPE serve
                             QString incomingWastedBytesString = locale.toString((uint)stats.getIncomingWastedBytes());
                             QString incomingOutOfOrderString = locale.toString((uint)stats.getIncomingOutOfOrder());
                             QString incomingLikelyLostString = locale.toString((uint)stats.getIncomingLikelyLost());
-                            QString incomingFlightTimeString = locale.toString(stats.getIncomingFlightTimeAverage());
+
+                            int clockSkewInMS = node->getClockSkewUsec() / (int)USECS_PER_MSEC;
+                            QString incomingFlightTimeString = locale.toString((int)stats.getIncomingFlightTimeAverage());
+                            QString incomingPingTimeString = locale.toString(node->getPingMs());
+                            QString incomingClockSkewString = locale.toString(clockSkewInMS);
                 
                             serverDetails << "<br/>" << "Incoming Packets: " <<
                                 incomingPacketsString.toLocal8Bit().constData() << 
@@ -354,6 +358,12 @@ void VoxelStatsDialog::showOctreeServersOfType(int& serverCount, NODE_TYPE serve
                             serverDetails << "<br/>" <<
                                 " Average Flight Time: " << incomingFlightTimeString.toLocal8Bit().constData() << " msecs";
 
+                            serverDetails << "<br/>" << 
+                                " Average Ping Time: " << incomingPingTimeString.toLocal8Bit().constData() << " msecs";
+
+                            serverDetails << "<br/>" << 
+                                " Average Clock Skew: " << incomingClockSkewString.toLocal8Bit().constData() << " msecs";
+                    
                             serverDetails << "<br/>" << "Incoming" <<
                                 " Bytes: " <<  incomingBytesString.toLocal8Bit().constData() <<
                                 " Wasted Bytes: " << incomingWastedBytesString.toLocal8Bit().constData();

libraries/octree/src/Octree.cpp

@@ -1342,7 +1342,7 @@ bool Octree::readFromSVOFile(const char* fileName) {
             fileOk = true; // assume the file is ok
         }
         if (fileOk) {
-            ReadBitstreamToTreeParams args(WANT_COLOR, NO_EXISTS_BITS, NULL, 0, wantImportProgress);
+            ReadBitstreamToTreeParams args(WANT_COLOR, NO_EXISTS_BITS, NULL, 0, NULL, wantImportProgress);
             readBitstreamToTree(dataAt, dataLength, args);
         }
         delete[] entireFile;
@@ -1481,7 +1481,7 @@ void Octree::copyFromTreeIntoSubTree(Octree* sourceTree, OctreeElement* destinat
 
         // ask destination tree to read the bitstream
         bool wantImportProgress = true;
-        ReadBitstreamToTreeParams args(WANT_COLOR, NO_EXISTS_BITS, destinationNode, 0, wantImportProgress);
+        ReadBitstreamToTreeParams args(WANT_COLOR, NO_EXISTS_BITS, destinationNode, 0, NULL, wantImportProgress);
         readBitstreamToTree(packetData.getUncompressedData(), packetData.getUncompressedSize(), args);
     }
 }

libraries/octree/src/Octree.h

@@ -156,6 +156,7 @@ public:
     bool includeExistsBits;
     OctreeElement* destinationNode;
     QUuid sourceUUID;
+    Node* sourceNode;
     bool wantImportProgress;
     
     ReadBitstreamToTreeParams(
@@ -163,11 +164,13 @@ public:
         bool includeExistsBits = WANT_EXISTS_BITS,
         OctreeElement* destinationNode = NULL,
         QUuid sourceUUID = QUuid(),
+        Node* sourceNode = NULL,
         bool wantImportProgress = false) :
             includeColor(includeColor),
             includeExistsBits(includeExistsBits),
             destinationNode(destinationNode),
             sourceUUID(sourceUUID),
+            sourceNode(sourceNode),
             wantImportProgress(wantImportProgress)
     {}
 };

libraries/octree/src/OctreeEditPacketSender.cpp

@@ -81,7 +81,7 @@ bool OctreeEditPacketSender::serversExist() const {
 }
 
 // This method is called when the edit packet layer has determined that it has a fully formed packet destined for
-// a known nodeID. However, we also want to handle the case where the 
+// a known nodeID. 
 void OctreeEditPacketSender::queuePacketToNode(const QUuid& nodeUUID, unsigned char* buffer, ssize_t length) {
     NodeList* nodeList = NodeList::getInstance();
     for (NodeList::iterator node = nodeList->begin(); node != nodeList->end(); node++) {
@@ -243,6 +243,14 @@ void OctreeEditPacketSender::queueOctreeEditMessage(PACKET_TYPE type, unsigned c
                     initializePacket(packetBuffer, type);
                 }
 
+                // This is really the first time we know which server/node this particular edit message
+                // is going to, so we couldn't adjust for clock skew till now. But here's our chance.
+                // We call this virtual function that allows our specific type of EditPacketSender to
+                // fixup the buffer for any clock skew
+                if (node->getClockSkewUsec() != 0) {
+                    adjustEditPacketForClockSkew(codeColorBuffer, length, node->getClockSkewUsec());
+                }
+
                 memcpy(&packetBuffer._currentBuffer[packetBuffer._currentSize], codeColorBuffer, length);
                 packetBuffer._currentSize += length;
             }

libraries/octree/src/OctreeEditPacketSender.h

@@ -86,6 +86,7 @@ public:
 
     // you must override these...
     virtual unsigned char getMyNodeType() const = 0;
+    virtual void adjustEditPacketForClockSkew(unsigned char* codeColorBuffer, ssize_t length, int clockSkew) { };
     
 protected:
     bool _shouldSend;

libraries/octree/src/OctreeRenderer.cpp

@@ -26,7 +26,7 @@ void OctreeRenderer::init() {
 OctreeRenderer::~OctreeRenderer() {
 }
 
-void OctreeRenderer::processDatagram(const QByteArray& dataByteArray, const HifiSockAddr& senderSockAddr) {
+void OctreeRenderer::processDatagram(const QByteArray& dataByteArray, const HifiSockAddr& senderSockAddr, Node* sourceNode) {
     bool showTimingDetails = false; // Menu::getInstance()->isOptionChecked(MenuOption::PipelineWarnings);
     bool extraDebugging = false; // Menu::getInstance()->isOptionChecked(MenuOption::ExtraDebugging)
     PerformanceWarning warn(showTimingDetails, "OctreeRenderer::processDatagram()",showTimingDetails);
@@ -57,7 +57,8 @@ void OctreeRenderer::processDatagram(const QByteArray& dataByteArray, const Hifi
         bool packetIsCompressed = oneAtBit(flags, PACKET_IS_COMPRESSED_BIT);
         
         OCTREE_PACKET_SENT_TIME arrivedAt = usecTimestampNow();
-        int flightTime = arrivedAt - sentAt;
+        int clockSkew = sourceNode ? sourceNode->getClockSkewUsec() : 0;
+        int flightTime = arrivedAt - sentAt + clockSkew;
         
         OCTREE_PACKET_INTERNAL_SECTION_SIZE sectionLength = 0;
         int dataBytes = packetLength - OCTREE_PACKET_HEADER_SIZE;
@@ -88,7 +89,7 @@ void OctreeRenderer::processDatagram(const QByteArray& dataByteArray, const Hifi
             if (sectionLength) {
                 // ask the VoxelTree to read the bitstream into the tree
                 ReadBitstreamToTreeParams args(packetIsColored ? WANT_COLOR : NO_COLOR, WANT_EXISTS_BITS, NULL, 
-                                                getDataSourceUUID());
+                                                getDataSourceUUID(), sourceNode);
                 _tree->lockForWrite();
                 OctreePacketData packetData(packetIsCompressed);
                 packetData.loadFinalizedContent(dataAt, sectionLength);

libraries/octree/src/OctreeRenderer.h

@@ -43,7 +43,7 @@ public:
     virtual void renderElement(OctreeElement* element, RenderArgs* args) = 0;
     
     /// process incoming data
-    virtual void processDatagram(const QByteArray& dataByteArray, const HifiSockAddr& senderSockAddr);
+    virtual void processDatagram(const QByteArray& dataByteArray, const HifiSockAddr& senderSockAddr, Node* sourceNode);
 
     /// initialize and GPU/rendering related resources
     void init();

libraries/octree/src/OctreeSceneStats.cpp

@@ -798,7 +798,8 @@ const char* OctreeSceneStats::getItemValue(Item item) {
     return _itemValueBuffer;
 }
 
-void OctreeSceneStats::trackIncomingOctreePacket(unsigned char* messageData, ssize_t messageLength, bool wasStatsPacket) {
+void OctreeSceneStats::trackIncomingOctreePacket(unsigned char* messageData, ssize_t messageLength, 
+                                    bool wasStatsPacket, int nodeClockSkewUsec) {
     _incomingPacket++;
     _incomingBytes += messageLength;
     if (!wasStatsPacket) {
@@ -820,7 +821,7 @@ void OctreeSceneStats::trackIncomingOctreePacket(unsigned char* messageData, ssi
     //bool packetIsCompressed = oneAtBit(flags, PACKET_IS_COMPRESSED_BIT);
     
     OCTREE_PACKET_SENT_TIME arrivedAt = usecTimestampNow();
-    int flightTime = arrivedAt - sentAt;
+    int flightTime = arrivedAt - sentAt + nodeClockSkewUsec;
     const int USECS_PER_MSEC = 1000;
     float flightTimeMsecs = flightTime / USECS_PER_MSEC;
     _incomingFlightTimeAverage.updateAverage(flightTimeMsecs);

libraries/octree/src/OctreeSceneStats.h

@@ -153,7 +153,8 @@ public:
     unsigned long getLastFullElapsedTime() const { return _lastFullElapsed; }
 
     // Used in client implementations to track individual octree packets
-    void trackIncomingOctreePacket(unsigned char* messageData, ssize_t messageLength, bool wasStatsPacket);
+    void trackIncomingOctreePacket(unsigned char* messageData, ssize_t messageLength, 
+                                        bool wasStatsPacket, int nodeClockSkewUsec);
 
     unsigned int getIncomingPackets() const { return _incomingPacket; }
     unsigned long getIncomingBytes() const { return _incomingBytes; } 

libraries/particles/src/Particle.cpp

@@ -12,6 +12,7 @@
 
 #include <RegisteredMetaTypes.h>
 #include <SharedUtil.h> // usecTimestampNow()
+#include <Octree.h>
 
 #include "Particle.h"
 
@@ -136,6 +137,8 @@ int Particle::expectedEditMessageBytes() {
 int Particle::readParticleDataFromBuffer(const unsigned char* data, int bytesLeftToRead, ReadBitstreamToTreeParams& args) {
     int bytesRead = 0;
     if (bytesLeftToRead >= expectedBytes()) {
+        int clockSkew = args.sourceNode ? args.sourceNode->getClockSkewUsec() : 0;
+
         const unsigned char* dataAt = data;
 
         // id
@@ -154,11 +157,13 @@ int Particle::readParticleDataFromBuffer(const unsigned char* data, int bytesLef
         memcpy(&_lastUpdated, dataAt, sizeof(_lastUpdated));
         dataAt += sizeof(_lastUpdated);
         bytesRead += sizeof(_lastUpdated);
+        _lastUpdated -= clockSkew;
 
         // _lastEdited
         memcpy(&_lastEdited, dataAt, sizeof(_lastEdited));
         dataAt += sizeof(_lastEdited);
         bytesRead += sizeof(_lastEdited);
+        _lastEdited -= clockSkew;
 
         // radius
         memcpy(&_radius, dataAt, sizeof(_radius));
@@ -357,7 +362,7 @@ bool Particle::encodeParticleEditMessageDetails(PACKET_TYPE command, int count,
                 sizeOut += sizeof(details[i].creatorTokenID);
             }
 
-            // radius
+            // lastEdited
             memcpy(copyAt, &details[i].lastEdited, sizeof(details[i].lastEdited));
             copyAt += sizeof(details[i].lastEdited);
             sizeOut += sizeof(details[i].lastEdited);
@@ -420,6 +425,38 @@ bool Particle::encodeParticleEditMessageDetails(PACKET_TYPE command, int count,
     return success;
 }
 
+// adjust any internal timestamps to fix clock skew for this server
+void Particle::adjustEditPacketForClockSkew(unsigned char* codeColorBuffer, ssize_t length, int clockSkew) { 
+    unsigned char* dataAt = codeColorBuffer;
+    int octets = numberOfThreeBitSectionsInCode(dataAt);
+    int lengthOfOctcode = bytesRequiredForCodeLength(octets);
+    dataAt += lengthOfOctcode;
+
+    // id
+    uint32_t id;
+    memcpy(&id, dataAt, sizeof(id));
+    dataAt += sizeof(id);
+    // special case for handling "new" particles
+    if (id == NEW_PARTICLE) {
+        // If this is a NEW_PARTICLE, then we assume that there's an additional uint32_t creatorToken, that
+        // we want to send back to the creator as an map to the actual id
+        dataAt += sizeof(uint32_t);
+    }
+
+    // lastEdited
+    uint64_t lastEditedInLocalTime;
+    memcpy(&lastEditedInLocalTime, dataAt, sizeof(lastEditedInLocalTime));
+    uint64_t lastEditedInServerTime = lastEditedInLocalTime + clockSkew;
+    memcpy(dataAt, &lastEditedInServerTime, sizeof(lastEditedInServerTime));
+    const bool wantDebug = false;
+    if (wantDebug) {    
+        qDebug("Particle::adjustEditPacketForClockSkew()...\n");
+        qDebug("     lastEditedInLocalTime: %llu\n", lastEditedInLocalTime);
+        qDebug("                 clockSkew: %d\n", clockSkew);
+        qDebug("    lastEditedInServerTime: %llu\n", lastEditedInServerTime);
+    }
+}
+
 
 void Particle::update() {
 

libraries/particles/src/Particle.h

@@ -107,6 +107,8 @@ public:
     static bool encodeParticleEditMessageDetails(PACKET_TYPE command, int count, const ParticleDetail* details, 
                         unsigned char* bufferOut, int sizeIn, int& sizeOut);
                         
+    static void adjustEditPacketForClockSkew(unsigned char* codeColorBuffer, ssize_t length, int clockSkew);
+
     void update();
 
     void debugDump() const;

libraries/particles/src/ParticleEditPacketSender.cpp

@@ -37,6 +37,11 @@ void ParticleEditPacketSender::sendEditParticleMessage(PACKET_TYPE type, const P
     }
 }
 
+void ParticleEditPacketSender::adjustEditPacketForClockSkew(unsigned char* codeColorBuffer, ssize_t length, int clockSkew) { 
+    Particle::adjustEditPacketForClockSkew(codeColorBuffer, length, clockSkew);
+} 
+
+
 void ParticleEditPacketSender::queueParticleEditMessages(PACKET_TYPE type, int numberOfDetails, ParticleDetail* details) {
     if (!_shouldSend) {
         return; // bail early

libraries/particles/src/ParticleEditPacketSender.h

@@ -28,7 +28,8 @@ public:
     /// which case up to MaxPendingMessages will be buffered and processed when voxel servers are known.
     void queueParticleEditMessages(PACKET_TYPE type, int numberOfDetails, ParticleDetail* details);
 
-    // My server type is the voxel server
+    // My server type is the particle server
     virtual unsigned char getMyNodeType() const { return NODE_TYPE_PARTICLE_SERVER; }
+    virtual void adjustEditPacketForClockSkew(unsigned char* codeColorBuffer, ssize_t length, int clockSkew);
 };
 #endif // __shared__ParticleEditPacketSender__

libraries/particles/src/ParticleTree.cpp

@@ -47,7 +47,7 @@ bool ParticleTree::findAndUpdateOperation(OctreeElement* element, void* extraDat
     return true;
 }
 
-void ParticleTree::storeParticle(const Particle& particle) {
+void ParticleTree::storeParticle(const Particle& particle, Node* senderNode) {
     // First, look for the existing particle in the tree..
     FindAndUpdateParticleArgs args = { particle, false };
     recurseTreeWithOperation(findAndUpdateOperation, &args);
@@ -58,7 +58,7 @@ void ParticleTree::storeParticle(const Particle& particle) {
         float size = particle.getRadius();
         ParticleTreeElement* element = (ParticleTreeElement*)getOrCreateChildElementAt(position.x, position.y, position.z, size);
 
-        element->storeParticle(particle);
+        element->storeParticle(particle, senderNode);
     }    
     // what else do we need to do here to get reaveraging to work
     _isDirty = true;
@@ -165,7 +165,7 @@ int ParticleTree::processEditPacketData(PACKET_TYPE packetType, unsigned char* p
     switch (packetType) {
         case PACKET_TYPE_PARTICLE_ADD_OR_EDIT: {
             Particle newParticle = Particle::fromEditPacket(editData, maxLength, processedBytes);
-            storeParticle(newParticle);
+            storeParticle(newParticle, senderNode);
             if (newParticle.isNewlyCreated()) {
                 notifyNewlyCreatedParticle(newParticle, senderNode);
             }

libraries/particles/src/ParticleTree.h

@@ -44,7 +44,7 @@ public:
 
     virtual void update();    
 
-    void storeParticle(const Particle& particle);
+    void storeParticle(const Particle& particle, Node* senderNode = NULL);
     const Particle* findClosestParticle(glm::vec3 position, float targetRadius);
     const Particle* findParticleByID(uint32_t id);
 

libraries/particles/src/ParticleTreeElement.cpp

@@ -128,7 +128,7 @@ bool ParticleTreeElement::containsParticle(const Particle& particle) const {
 }
 
 bool ParticleTreeElement::updateParticle(const Particle& particle) {
-    const bool wantDebug = false;
+    const bool wantDebug = true;
     uint16_t numberOfParticles = _particles.size();
     for (uint16_t i = 0; i < numberOfParticles; i++) {
         if (_particles[i].getID() == particle.getID()) {
@@ -230,7 +230,7 @@ bool ParticleTreeElement::collapseChildren() {
 }
 
 
-void ParticleTreeElement::storeParticle(const Particle& particle) {
+void ParticleTreeElement::storeParticle(const Particle& particle, Node* senderNode) {
     _particles.push_back(particle);
     markWithChangedTime();
 }

libraries/particles/src/ParticleTreeElement.h

@@ -89,7 +89,7 @@ public:
     
 
 protected:
-    void storeParticle(const Particle& particle);
+    void storeParticle(const Particle& particle, Node* senderNode = NULL);
 
     ParticleTree* _myTree;
     std::vector<Particle> _particles;

libraries/shared/src/Node.cpp

@@ -32,7 +32,8 @@ Node::Node(const QUuid& uuid, char type, const HifiSockAddr& publicSocket, const
     _activeSocket(NULL),
     _bytesReceivedMovingAverage(NULL),
     _linkedData(NULL),
-    _isAlive(true)
+    _isAlive(true),
+    _clockSkewUsec(0)
 {
     pthread_mutex_init(&_mutex, 0);
 }

libraries/shared/src/Node.h

@@ -69,6 +69,9 @@ public:
     int getPingMs() const { return _pingMs; }
     void setPingMs(int pingMs) { _pingMs = pingMs; }
 
+    int getClockSkewUsec() const { return _clockSkewUsec; }
+    void setClockSkewUsec(int clockSkew) { _clockSkewUsec = clockSkew; }
+    
     void lock() { pthread_mutex_lock(&_mutex); }
     
     /// returns false if lock failed, true if you got the lock
@@ -93,6 +96,7 @@ private:
     NodeData* _linkedData;
     bool _isAlive;
     int _pingMs;
+    int _clockSkewUsec;
     pthread_mutex_t _mutex;
 };
 

libraries/shared/src/NodeList.cpp

@@ -112,9 +112,33 @@ void NodeList::timePingReply(const HifiSockAddr& nodeAddress, unsigned char *pac
         if (node->getPublicSocket() == nodeAddress ||
             node->getLocalSocket() == nodeAddress) {
 
-            int pingTime = usecTimestampNow() - *(uint64_t*)(packetData + numBytesForPacketHeader(packetData));
+            unsigned char* dataAt = packetData + numBytesForPacketHeader(packetData);
+            uint64_t ourOriginalTime = *(uint64_t*)(dataAt);
+            dataAt += sizeof(ourOriginalTime);
+            uint64_t othersReplyTime = *(uint64_t*)(dataAt);
+            uint64_t now = usecTimestampNow();
+            int pingTime = now - ourOriginalTime;
+            int oneWayFlightTime = pingTime/2; // half of the ping is our one way flight
+            
+            // The other node's expected time should be our original time plus the one way flight time
+            // anything other than that is clock skew
+            uint64_t othersExprectedReply = ourOriginalTime + oneWayFlightTime;
+            int clockSkew = othersReplyTime - othersExprectedReply;
             
             node->setPingMs(pingTime / 1000);
+            node->setClockSkewUsec(clockSkew);
+            
+            const bool wantDebug = false;
+            if (wantDebug) { 
+                qDebug() << "PING_REPLY from node " << *node << "\n" <<
+                    "                     now: " << now << "\n" <<
+                    "                 ourTime: " << ourOriginalTime << "\n" <<
+                    "                pingTime: " << pingTime << "\n" <<
+                    "        oneWayFlightTime: " << oneWayFlightTime << "\n" <<
+                    "         othersReplyTime: " << othersReplyTime << "\n" <<
+                    "    othersExprectedReply: " << othersExprectedReply << "\n" <<
+                    "               clockSkew: " << clockSkew << "\n";
+            }
             break;
         }
     }
@@ -131,9 +155,11 @@ void NodeList::processNodeData(const HifiSockAddr& senderSockAddr, unsigned char
             break;
         }
         case PACKET_TYPE_PING: {
-            // send it right back
+            // send back a reply
-            populateTypeAndVersion(packetData, PACKET_TYPE_PING_REPLY);
+            unsigned char replyPacket[MAX_PACKET_SIZE];
-            _nodeSocket.writeDatagram((char*) packetData, dataBytes, senderSockAddr.getAddress(), senderSockAddr.getPort());
+            int replyPacketLength = fillPingReplyPacket(packetData, replyPacket);
+            _nodeSocket.writeDatagram((char*)replyPacket, replyPacketLength, 
+                            senderSockAddr.getAddress(), senderSockAddr.getPort());
             break;
         }
         case PACKET_TYPE_PING_REPLY: {
@@ -616,21 +642,42 @@ void NodeList::sendAssignment(Assignment& assignment) {
                               assignmentServerSocket->getPort());
 }
 
+int NodeList::fillPingPacket(unsigned char* buffer) {
+    int numHeaderBytes = populateTypeAndVersion(buffer, PACKET_TYPE_PING);
+    uint64_t currentTime = usecTimestampNow();
+    memcpy(buffer + numHeaderBytes, &currentTime, sizeof(currentTime));
+    return numHeaderBytes + sizeof(currentTime);
+}
+
+int NodeList::fillPingReplyPacket(unsigned char* pingBuffer, unsigned char* replyBuffer) {
+    int numHeaderBytesOriginal = numBytesForPacketHeader(pingBuffer);
+    uint64_t timeFromOriginalPing = *(uint64_t*)(pingBuffer + numHeaderBytesOriginal);
+
+    int numHeaderBytesReply = populateTypeAndVersion(replyBuffer, PACKET_TYPE_PING_REPLY);
+    int length = numHeaderBytesReply;
+    uint64_t ourReplyTime = usecTimestampNow();
+
+    unsigned char* dataAt = replyBuffer + numHeaderBytesReply;
+    memcpy(dataAt, &timeFromOriginalPing, sizeof(timeFromOriginalPing));
+    dataAt += sizeof(timeFromOriginalPing);
+    length += sizeof(timeFromOriginalPing);
+
+    memcpy(dataAt, &ourReplyTime, sizeof(ourReplyTime));
+    dataAt += sizeof(ourReplyTime);
+    length += sizeof(ourReplyTime);
+    
+    return length;
+}
+
+
 void NodeList::pingPublicAndLocalSocketsForInactiveNode(Node* node) {
-    
+    unsigned char pingPacket[MAX_PACKET_SIZE];
-    uint64_t currentTime = 0;
+    int pingPacketLength = fillPingPacket(pingPacket);
-    
-    // setup a ping packet to send to this node
-    unsigned char pingPacket[numBytesForPacketHeader((uchar*) &PACKET_TYPE_PING) + sizeof(currentTime)];
-    int numHeaderBytes = populateTypeAndVersion(pingPacket, PACKET_TYPE_PING);
-    
-    currentTime = usecTimestampNow();
-    memcpy(pingPacket + numHeaderBytes, &currentTime, sizeof(currentTime));
     
     // send the ping packet to the local and public sockets for this node
-    _nodeSocket.writeDatagram((char*) pingPacket, sizeof(pingPacket),
+    _nodeSocket.writeDatagram((char*) pingPacket, pingPacketLength,
                               node->getLocalSocket().getAddress(), node->getLocalSocket().getPort());
-    _nodeSocket.writeDatagram((char*) pingPacket, sizeof(pingPacket),
+    _nodeSocket.writeDatagram((char*) pingPacket, pingPacketLength,
                               node->getPublicSocket().getAddress(), node->getPublicSocket().getPort());
 }
 

libraries/shared/src/NodeList.h

@@ -103,6 +103,8 @@ public:
     void setAssignmentServerSocket(const HifiSockAddr& serverSocket) { _assignmentServerSocket = serverSocket; }
     void sendAssignment(Assignment& assignment);
     
+    int fillPingPacket(unsigned char* buffer);
+    int fillPingReplyPacket(unsigned char* pingBuffer, unsigned char* replyBuffer);
     void pingPublicAndLocalSocketsForInactiveNode(Node* node);
     
     void sendKillNode(const char* nodeTypes, int numNodeTypes);

libraries/shared/src/PacketHeaders.cpp

@@ -56,6 +56,9 @@ PACKET_VERSION versionForPacketType(PACKET_TYPE type) {
         case PACKET_TYPE_PARTICLE_DATA:
             return 5;
         
+        case PACKET_TYPE_PING_REPLY:
+            return 1;
+        
         default:
             return 0;
     }

libraries/shared/src/SharedUtil.h

@@ -52,7 +52,9 @@ static const float METER            = 1.0f;
 static const float DECIMETER        = 0.1f;
 static const float CENTIMETER       = 0.01f;
 static const float MILLIIMETER      = 0.001f;
-static const uint64_t USECS_PER_SECOND = 1000 * 1000;
+static const uint64_t USECS_PER_MSEC = 1000;
+static const uint64_t MSECS_PER_SECOND = 1000;
+static const uint64_t USECS_PER_SECOND = USECS_PER_MSEC * MSECS_PER_SECOND;
 
 uint64_t usecTimestamp(const timeval *time);
 uint64_t usecTimestampNow();