tolerate clock skew in motion smoothing

direct/src/deadrec/smoothMover.I

@@ -237,18 +237,17 @@ set_r(float r) {
 }
 
 ////////////////////////////////////////////////////////////////////
-//     Function: SmoothMover::set_timestamp
+//     Function: SmoothMover::set_phony_timestamp
 //       Access: Published
-//  Description: Specifies the time that the current position report
-//               applies.  This should be called, along with set_pos()
-//               and set_hpr(), before a call to mark_position().
-//
-//               With no parameter, set_timestamp uses
-//               ClockObject::get_frame_time() as the default time.
+//  Description: Lies and specifies that the current position report
+//               was received now.  This is usually used for very old
+//               position reports for which we're not sure of the
+//               actual receipt time.
 ////////////////////////////////////////////////////////////////////
 INLINE void SmoothMover::
-set_timestamp() {
-  set_timestamp(ClockObject::get_global_clock()->get_frame_time());
+set_phony_timestamp() {
+  double now = ClockObject::get_global_clock()->get_frame_time();
+  _sample._timestamp = now;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -261,7 +260,7 @@ set_timestamp() {
 INLINE void SmoothMover::
 set_timestamp(double timestamp) {
   _sample._timestamp = timestamp;
-  _sample._flags |= F_got_timestamp;
+  record_timestamp_delay(timestamp);
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -419,6 +418,38 @@ get_delay() {
   return _delay;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::set_accept_clock_skew
+//       Access: Published, Static
+//  Description: Sets the 'accept clock skew' flag.  When this flag is
+//               true, clock skew from the other clients will be
+//               tolerated by delaying each smooth mover's position an
+//               additional amount, on top of that specified by
+//               set_delay(), based on the measured average latency
+//               for timestamp messages received by the client.
+//
+//               In this way, if the other client has significant
+//               clock skew with respect to our clock, it will be
+//               evident as a large positive or negative average
+//               latency for timestamps.  By subtracting out this
+//               average latency, we compensate for poor clock sync.
+////////////////////////////////////////////////////////////////////
+INLINE void SmoothMover::
+set_accept_clock_skew(bool flag) {
+  _accept_clock_skew = flag;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::get_accept_clock_skew
+//       Access: Published, Static
+//  Description: Returns the current state of the 'accept clock skew'
+//               flag.  See set_accept_clock_skew().
+////////////////////////////////////////////////////////////////////
+INLINE bool SmoothMover::
+get_accept_clock_skew() {
+  return _accept_clock_skew;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: SmoothMover::set_max_position_age
 //       Access: Published, Static
@@ -469,3 +500,19 @@ INLINE double SmoothMover::
 get_reset_velocity_age() {
   return _reset_velocity_age;
 }
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::get_avg_timestamp_delay
+//       Access: Private
+//  Description: Returns the average delay observed in the last n
+//               timestamps received from this client, in seconds.
+//               This number represents the combination of the network
+//               lag from this client, as well as the client's clock
+//               skew relative to our clock.  It could be negative if
+//               the client's clock is running faster than our clock.
+////////////////////////////////////////////////////////////////////
+INLINE double SmoothMover::
+get_avg_timestamp_delay() const {
+  nassertr(!_timestamp_delays.empty(), 0.0);
+  return (double)_net_timestamp_delay / (double)_timestamp_delays.size() / 1000.0;
+}

direct/src/deadrec/smoothMover.cxx

@@ -23,6 +23,7 @@
 SmoothMover::SmoothMode SmoothMover::_smooth_mode = SmoothMover::SM_off;
 SmoothMover::PredictionMode SmoothMover::_prediction_mode = SmoothMover::PM_off;
 double SmoothMover::_delay = 0.2;
+bool SmoothMover::_accept_clock_skew = true;
 double SmoothMover::_max_position_age = 0.25;
 double SmoothMover::_reset_velocity_age = 0.3;
 
@@ -37,7 +38,6 @@ SmoothMover() {
   _sample._pos.set(0.0, 0.0, 0.0);
   _sample._hpr.set(0.0, 0.0, 0.0);
   _sample._timestamp = 0.0;
-  _sample._flags = 0;
 
   _smooth_pos.set(0.0, 0.0, 0.0);
   _smooth_hpr.set(0.0, 0.0, 0.0);
@@ -52,6 +52,13 @@ SmoothMover() {
 
   _last_point_before = -1;
   _last_point_after = -1;
+
+  _net_timestamp_delay = 0;
+  // Record one delay of 0 on the top of the delays array, just to
+  // guarantee that the array is never completely empty.
+  _timestamp_delays.push_back(0);
+
+  _last_heard_from = 0.0;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -135,7 +142,6 @@ mark_position() {
     }
     
     _points.push_back(_sample);
-    _sample._flags = 0;
   }
 }
 
@@ -207,8 +213,12 @@ compute_smooth_position(double timestamp) {
 
   // First, back up in time by the specified delay factor.
   timestamp -= _delay;
+  if (_accept_clock_skew) {
+    timestamp -= get_avg_timestamp_delay();
+  }
 
   // Now look for the two bracketing position reports.
+  int point_way_before = -1;
   int point_before = -1;
   double timestamp_before = 0.0;
   int point_after = -1;
@@ -226,6 +236,7 @@ compute_smooth_position(double timestamp) {
       // This point is before the desired time.  Find the newest of
       // these.
       if (point_before == -1 || point._timestamp > timestamp_before) {
+        point_way_before = point_before;
         point_before = i;
         timestamp_before = point._timestamp;
       }
@@ -261,23 +272,20 @@ compute_smooth_position(double timestamp) {
     // the avatar is going by a tiny bit, if we don't have current
     // enough data.  This works only if we have at least two points of
     // old data.
-    if (point_before > 0) {
+    if (point_way_before != -1) {
       // To implement simple prediction, we simply back up in time to
       // the previous two timestamps, and base our linear
       // interpolation off of those two, extending into the future.
-      int point_before_before = point_before - 1;
-      SamplePoint &point = _points[point_before_before];
-      if (point._timestamp < timestamp_before) {
-        point_after = point_before;
-        timestamp_after = timestamp_before;
-        point_before = point_before_before;
-        timestamp_before = point._timestamp;
-
-        if (timestamp > timestamp_after + _max_position_age) {
-          // Don't allow the prediction to get too far into the
-          // future.
-          timestamp = timestamp_after + _max_position_age;
-        }
+      SamplePoint &point = _points[point_way_before];
+      point_after = point_before;
+      timestamp_after = timestamp_before;
+      point_before = point_way_before;
+      timestamp_before = point._timestamp;
+      
+      if (timestamp > timestamp_after + _max_position_age) {
+        // Don't allow the prediction to get too far into the
+        // future.
+        timestamp = timestamp_after + _max_position_age;
       }
     }
   }
@@ -285,11 +293,22 @@ compute_smooth_position(double timestamp) {
   if (point_after == -1) {
     // If we only have a before point even after we've checked for the
     // possibility of using prediction, then we have to stop there.
-    const SamplePoint &point = _points[point_before];
-    set_smooth_pos(point._pos, point._hpr, timestamp);
-    if (timestamp - point._timestamp > _reset_velocity_age) {
-      // Furthermore, if the before point is old enough, zero out the
-      // velocity.
+    if (point_way_before != -1) {
+      // Use the previous two points, if we've got 'em, so we can
+      // still reflect the avatar's velocity.
+      linear_interpolate(point_way_before, point_before, timestamp_before);
+
+    } else {
+      // If we really only have one point, use it.
+      const SamplePoint &point = _points[point_before];
+      set_smooth_pos(point._pos, point._hpr, timestamp);
+    }
+
+    if (timestamp - _last_heard_from > _reset_velocity_age) {
+      // Furthermore, if we haven't heard from this client in a while,
+      // reset the velocity.  This decision is based entirely on our
+      // local timestamps, not on the other client's reported
+      // timestamps.
       _smooth_forward_velocity = 0.0;
       _smooth_rotational_velocity = 0.0;
     }
@@ -306,8 +325,9 @@ compute_smooth_position(double timestamp) {
 
   // Assume we'll never get another compute_smooth_position() request
   // for an older time than this, and remove all the timestamps at the
-  // head of the queue before point_before.
-  while (!_points.empty() && _points.front()._timestamp < timestamp_before) {
+  // head of the queue before point_way_before.
+  double timestamp_way_before = _points[point_way_before]._timestamp;
+  while (!_points.empty() && _points.front()._timestamp < timestamp_way_before) {
     _points.pop_front();
 
     // This invalidates the index numbers.
@@ -475,3 +495,29 @@ compute_velocity(const LVector3f &pos_delta, const LVecBase3f &hpr_delta,
   _smooth_forward_velocity = forward_distance / age;
   _smooth_rotational_velocity = hpr_delta[0] / age;
 }
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::record_timestamp_delay
+//       Access: Private
+//  Description: Records the delay measured in receiving this
+//               particular timestamp.  The average delay of the last
+//               n timestamps will be used to smooth the motion
+//               properly.
+////////////////////////////////////////////////////////////////////
+void SmoothMover::
+record_timestamp_delay(double timestamp) {
+  double now = ClockObject::get_global_clock()->get_frame_time();
+
+  // Convert the delay to an integer number of milliseconds.  Integers
+  // are better than doubles because they don't accumulate errors over
+  // time.
+  int delay = (int)((now - timestamp) * 1000.0);
+  if (_timestamp_delays.full()) {
+    _net_timestamp_delay -= _timestamp_delays.front();
+    _timestamp_delays.pop_front();
+  }
+  _net_timestamp_delay += delay;
+  _timestamp_delays.push_back(delay);
+
+  _last_heard_from = now;
+}

direct/src/deadrec/smoothMover.h

@@ -25,6 +25,7 @@
 #include "circBuffer.h"
 
 static const int max_position_reports = 10;
+static const int max_timestamp_delays = 10;
 
 ////////////////////////////////////////////////////////////////////
 //       Class : SmoothMover
@@ -77,7 +78,7 @@ PUBLISHED:
 
   bool set_mat(const LMatrix4f &mat);
 
-  INLINE void set_timestamp();
+  INLINE void set_phony_timestamp();
   INLINE void set_timestamp(double timestamp);
 
   void mark_position();
@@ -120,6 +121,9 @@ PUBLISHED:
   INLINE static void set_delay(double delay); 
   INLINE static double get_delay(); 
 
+  INLINE static void set_accept_clock_skew(bool flag); 
+  INLINE static bool get_accept_clock_skew(); 
+
   INLINE static void set_max_position_age(double age); 
   INLINE static double get_max_position_age(); 
 
@@ -138,9 +142,8 @@ private:
                         const LVecBase3f &hpr_delta,
                         double age);
 
-  enum Flags {
-    F_got_timestamp  = 0x0001,
-  };
+  void record_timestamp_delay(double timestamp);
+  INLINE double get_avg_timestamp_delay() const;
 
   LVecBase3f _scale;
 
@@ -149,7 +152,6 @@ private:
     LPoint3f _pos;
     LVecBase3f _hpr;
     double _timestamp;
-    int _flags;
   };
   SamplePoint _sample;
 
@@ -169,9 +171,20 @@ private:
   int _last_point_before;
   int _last_point_after;
 
+  // This array is used to record the average delay in receiving
+  // timestamps from a particular client, in milliseconds.  This value
+  // will measure both the latency and clock skew from that client,
+  // allowing us to present smooth motion in spite of extreme latency
+  // or poor clock synchronization.
+  typedef CircBuffer<int, max_timestamp_delays> TimestampDelays;
+  TimestampDelays _timestamp_delays;
+  int _net_timestamp_delay;
+  double _last_heard_from;
+
   static SmoothMode _smooth_mode;
   static PredictionMode _prediction_mode;
   static double _delay;
+  static bool _accept_clock_skew;
   static double _max_position_age;
   static double _reset_velocity_age;
 };

direct/src/distributed/DistributedSmoothNode.py

@@ -11,7 +11,7 @@ globalClock = ClockObject.getGlobalClock()
 # If a packet appears to have originated from more than MaxFuture
 # seconds in the future, assume we're out of sync with the other
 # avatar and suggest a resync for both.
-MaxFuture = base.config.GetFloat("smooth-max-future", 0.1)
+MaxFuture = base.config.GetFloat("smooth-max-future", 0.2)
 
 # These flags indicate whether global smoothing and/or prediction is
 # allowed or disallowed.
@@ -20,9 +20,12 @@ EnablePrediction = base.config.GetBool("smooth-enable-prediction", 1)
 
 # These values represent the amount of time, in seconds, to delay the
 # apparent position of other avatars, when non-predictive and
-# predictive smoothing is in effect, respectively.
-Lag = base.config.GetDouble("smooth-lag", 0.2)
-PredictionLag = base.config.GetDouble("smooth-prediction-lag", 0.0)
+# predictive smoothing is in effect, respectively.  This is in
+# addition to the automatic delay of the observed average latency from
+# each avatar, which is intended to compensate for relative clock
+# skew.
+Lag = base.config.GetDouble("smooth-lag", 0.1)
+PredictionLag = base.config.GetDouble("smooth-prediction-lag", -0.1)
 
 
 
@@ -165,7 +168,7 @@ class DistributedSmoothNode(DistributedNode.DistributedNode):
         """
         self.smoother.clearPositions(0)
         self.smoother.setMat(self.getMat())
-        self.smoother.setTimestamp()
+        self.smoother.setPhonyTimestamp()
         self.smoother.markPosition()
         
 
@@ -279,8 +282,7 @@ class DistributedSmoothNode(DistributedNode.DistributedNode):
         # represent a time up to about 5 minutes in the past), but we
         # don't really need to know the timestamp anyway.  We'll just
         # arbitrarily place it at right now.
-        now = globalClock.getFrameTime()
-        self.smoother.setTimestamp(now)
+        self.smoother.setPhonyTimestamp()
         self.smoother.clearPositions(1)
         self.smoother.markPosition()