*** empty log message ***

direct/src/deadrec/smoothMover.I

@@ -264,6 +264,33 @@ get_smooth_mat() {
   return _smooth_mat;
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::get_smooth_forward_velocity
+//       Access: Published
+//  Description: Returns the speed at which the avatar is moving, in
+//               feet per second, along its own forward axis (after
+//               applying the avatar's hpr).  This will be a positive
+//               number if the avatar is moving forward, and a
+//               negative number if it is moving backward.
+////////////////////////////////////////////////////////////////////
+INLINE float SmoothMover::
+get_smooth_forward_velocity() const {
+  return _smooth_forward_velocity;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::get_smooth_rotational_velocity
+//       Access: Published
+//  Description: Returns the speed at which the avatar is rotating in
+//               the horizontal plane (i.e. heading), in degrees per
+//               second.  This may be positive or negative, according
+//               to the direction of rotation.
+////////////////////////////////////////////////////////////////////
+INLINE float SmoothMover::
+get_smooth_rotational_velocity() const {
+  return _smooth_rotational_velocity;
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: SmoothMover::set_smooth_mode
 //       Access: Published, Static
@@ -361,3 +388,30 @@ INLINE double SmoothMover::
 get_max_position_age() {
   return _max_position_age;
 }
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::set_reset_velocity_age
+//       Access: Published, Static
+//  Description: Sets the amount of time that should elapse after the
+//               last position report before the velocity is reset to
+//               0.  This is similar to max_position_age, but it is
+//               only used to determine the resetting of the reported
+//               velocity.  It should always be greater than or equal
+//               to max_position_age.
+////////////////////////////////////////////////////////////////////
+INLINE void SmoothMover::
+set_reset_velocity_age(double age) {
+  _reset_velocity_age = age;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::get_reset_velocity_age
+//       Access: Published, Static
+//  Description: Returns the amount of time that should elapse after
+//               the last position report before the velocity is reset
+//               to 0.  See set_reset_velocity_age().
+////////////////////////////////////////////////////////////////////
+INLINE double SmoothMover::
+get_reset_velocity_age() {
+  return _reset_velocity_age;
+}

direct/src/deadrec/smoothMover.cxx

@@ -22,8 +22,9 @@
 
 SmoothMover::SmoothMode SmoothMover::_smooth_mode = SmoothMover::SM_off;
 SmoothMover::PredictionMode SmoothMover::_prediction_mode = SmoothMover::PM_off;
-double SmoothMover::_delay = 0.0;
-double SmoothMover::_max_position_age = 0.2;
+double SmoothMover::_delay = 0.2;
+double SmoothMover::_max_position_age = 0.25;
+double SmoothMover::_reset_velocity_age = 0.5;
 
 ////////////////////////////////////////////////////////////////////
 //     Function: SmoothMover::Constructor
@@ -41,7 +42,15 @@ SmoothMover() {
   _smooth_pos.set(0.0, 0.0, 0.0);
   _smooth_hpr.set(0.0, 0.0, 0.0);
   _smooth_mat = LMatrix4f::ident_mat();
+  _smooth_timestamp = 0.0;
+  _smooth_position_known = false;
   _computed_smooth_mat = true;
+
+  _smooth_forward_velocity = 0.0;
+  _smooth_rotational_velocity = 0.0;
+
+  _last_point_before = -1;
+  _last_point_after = -1;
 }
 
 ////////////////////////////////////////////////////////////////////
@@ -70,9 +79,23 @@ mark_position() {
   if (_smooth_mode == SM_off) {
     // With smoothing disabled, mark_position() simply stores its
     // current position in the smooth_position members.
-    _smooth_pos = _sample._pos;
-    _smooth_hpr = _sample._hpr;
-    _computed_smooth_mat = false;
+
+    // We also need to compute the velocity here.
+    if (_smooth_position_known) {
+      LVector3f pos_delta = _sample._pos - _smooth_pos;
+      LVecBase3f hpr_delta = _sample._hpr - _smooth_hpr;
+      double age = _sample._timestamp - _smooth_timestamp;
+      age = min(age, _max_position_age);
+
+      set_smooth_pos(_sample._pos, _sample._hpr, _sample._timestamp);
+      if (age != 0.0) {
+        compute_velocity(pos_delta, hpr_delta, age);
+      }
+
+    } else {
+      // No velocity is possible, just position and orientation.
+      set_smooth_pos(_sample._pos, _sample._hpr, _sample._timestamp);
+    }
 
   } else {
     // Otherwise, smoothing is in effect and we store a true position
@@ -82,6 +105,10 @@ mark_position() {
       // If we have too many position reports, throw away the oldest
       // one.
       _points.pop_front();
+
+      // That invalidates the index numbers.
+      _last_point_before = -1;
+      _last_point_after = -1;
     }
     
     _points.push_back(_sample);
@@ -89,6 +116,30 @@ mark_position() {
   }
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::clear_positions
+//       Access: Published
+//  Description: Erases all the old position reports.  This should be
+//               done, for instance, prior to teleporting the avatar
+//               to a new position; otherwise, the smoother might try
+//               to lerp the avatar there.  If reset_velocity is true,
+//               the velocity is also reset to 0.
+////////////////////////////////////////////////////////////////////
+void SmoothMover::
+clear_positions(bool reset_velocity) {
+  while (!_points.empty()) {
+    _points.pop_front();
+  }
+  _last_point_before = -1;
+  _last_point_after = -1;
+  _smooth_position_known = false;
+
+  if (reset_velocity) {
+    _smooth_forward_velocity = 0.0;
+    _smooth_rotational_velocity = 0.0;
+  }
+}
+
 ////////////////////////////////////////////////////////////////////
 //     Function: SmoothMover::compute_smooth_position
 //       Access: Published
@@ -100,15 +151,27 @@ mark_position() {
 ////////////////////////////////////////////////////////////////////
 void SmoothMover::
 compute_smooth_position(double timestamp) {
-  if (_smooth_mode == SM_off || _points.empty()) {
-    // With smoothing disabled, or with no position reports available,
-    // this function does nothing, except to ensure that any old bogus
-    // position reports are cleared.
-    while (!_points.empty()) {
-      _points.pop_front();
+  if (_points.empty()) {
+    // With no position reports available, this function does nothing,
+    // except to make sure that our velocity gets reset to zero after
+    // a period of time.
+
+    if (_smooth_position_known) {
+      double age = timestamp - _smooth_timestamp;
+      if (age > _reset_velocity_age) {
+        _smooth_forward_velocity = 0.0;
+        _smooth_rotational_velocity = 0.0;
+      }
     }
     return;
   }
+  if (_smooth_mode == SM_off) {
+    // With smoothing disabled, this function also does nothing,
+    // except to ensure that any old bogus position reports are
+    // cleared.
+    clear_positions(false);
+    return;
+  }
 
   // First, back up in time by the specified delay factor.
   timestamp -= _delay;
@@ -144,53 +207,27 @@ compute_smooth_position(double timestamp) {
     nassertv(point_after != -1);
     // If we only have an after point, we have to start there.
     const SamplePoint &point = _points[point_after];
-    _smooth_pos = point._pos;
-    _smooth_hpr = point._hpr;
-    _computed_smooth_mat = false;
+    set_smooth_pos(point._pos, point._hpr, timestamp);
+    _smooth_forward_velocity = 0.0;
+    _smooth_rotational_velocity = 0.0;
     return;
   }
 
-  if (point_after == -1) {
+  if (point_after == -1 || timestamp_before == timestamp_after) {
     // If we only have a before point, we have to stop there, unless
     // we have prediction in effect.
     const SamplePoint &point = _points[point_before];
-    _smooth_pos = point._pos;
-    _smooth_hpr = point._hpr;
-    _computed_smooth_mat = false;
+    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.
+      _smooth_forward_velocity = 0.0;
+      _smooth_rotational_velocity = 0.0;
+    }
 
   } else {
     // If we have two points, we can linearly interpolate between them.
-    SamplePoint &point_b = _points[point_before];
-    const SamplePoint &point_a = _points[point_after];
-
-    double age = (timestamp_after - timestamp_before);
-    if (age > _max_position_age) {
-      // If the first point is too old, assume there were a lot of
-      // implicit standing still messages that weren't sent.  Reset
-      // the first point to be timestamp_after - max_position_age.
-      timestamp_before = min(timestamp, timestamp_after - _max_position_age);
-      point_b._timestamp = timestamp_before;
-      age = (timestamp_after - timestamp_before);
-    }
-
-    double t = (timestamp - timestamp_before) / age;
-    _smooth_pos = point_b._pos + t * (point_a._pos - point_b._pos);
-
-    // To interpolate the hpr's, we must first make sure that both
-    // angles are on the same side of the discontinuity.
-    LVecBase3f a_hpr = point_a._hpr;
-    LVecBase3f b_hpr = point_b._hpr;
-
-    for (int j = 0; j < 3; j++) {
-      if ((a_hpr[j] - b_hpr[j]) > 180.0) {
-        a_hpr[j] -= 360.0;
-      } else if ((a_hpr[j] - b_hpr[j]) < -180.0) {
-        a_hpr[j] += 360.0;
-      }
-    }
-
-    _smooth_hpr = b_hpr + t * (a_hpr - b_hpr);
-    _computed_smooth_mat = false;
+    linear_interpolate(point_before, point_after, timestamp);
   }
 
   // Assume we'll never get another compute_smooth_position() request
@@ -198,6 +235,10 @@ compute_smooth_position(double timestamp) {
   // head of the queue before point_before.
   while (!_points.empty() && _points.front()._timestamp < timestamp_before) {
     _points.pop_front();
+
+    // This invalidates the index numbers.
+    _last_point_before = -1;
+    _last_point_after = -1;
   }
 }
 
@@ -227,6 +268,21 @@ write(ostream &out) const {
   }
 }
 
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::set_smooth_pos
+//       Access: Private
+//  Description: Sets the computed smooth position and orientation for
+//               the indicated timestamp.
+////////////////////////////////////////////////////////////////////
+void SmoothMover::
+set_smooth_pos(const LPoint3f &pos, const LVecBase3f &hpr,
+               double timestamp) {
+  _smooth_pos = pos;
+  _smooth_hpr = hpr;
+  _smooth_timestamp = timestamp;
+  _smooth_position_known = true;
+  _computed_smooth_mat = false;
+}
 
 ////////////////////////////////////////////////////////////////////
 //     Function: SmoothMover::compose_smooth_mat
@@ -239,3 +295,81 @@ compose_smooth_mat() {
   compose_matrix(_smooth_mat, _scale, _smooth_hpr, _smooth_pos);
   _computed_smooth_mat = true;
 }
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::linear_interpolate
+//       Access: Private
+//  Description: Interpolates the smooth position linearly between the
+//               two bracketing position reports.
+////////////////////////////////////////////////////////////////////
+void SmoothMover::
+linear_interpolate(int point_before, int point_after, double timestamp) {
+  SamplePoint &point_b = _points[point_before];
+  const SamplePoint &point_a = _points[point_after];
+
+  double age = (point_a._timestamp - point_b._timestamp);
+
+  if (point_before == _last_point_before && 
+      point_after == _last_point_after) {
+    // If these are the same two points we found last time (which is
+    // likely), we can save a bit of work.
+    double t = (timestamp - point_b._timestamp) / age;
+    set_smooth_pos(point_b._pos + t * (point_a._pos - point_b._pos),
+                   point_b._hpr + t * (point_a._hpr - point_b._hpr),
+                   timestamp);
+
+    // The velocity remains the same as last time.
+
+  } else {
+    _last_point_before = point_before;
+    _last_point_after = point_after;
+    
+    if (age > _max_position_age) {
+      // If the first point is too old, assume there were a lot of
+      // implicit standing still messages that weren't sent.  Reset
+      // the first point's timestamp to reflect this.
+      point_b._timestamp = min(timestamp, point_a._timestamp - _max_position_age);
+      age = (point_a._timestamp - point_b._timestamp);
+    }
+    
+    // To interpolate the hpr's, we must first make sure that both
+    // angles are on the same side of the discontinuity.
+    for (int j = 0; j < 3; j++) {
+      if ((point_b._hpr[j] - point_a._hpr[j]) > 180.0) {
+        point_b._hpr[j] -= 360.0;
+      } else if ((point_b._hpr[j] - point_a._hpr[j]) < -180.0) {
+        point_b._hpr[j] += 360.0;
+      }
+    }
+    
+    double t = (timestamp - point_b._timestamp) / age;
+    LVector3f pos_delta = point_a._pos - point_b._pos;
+    LVecBase3f hpr_delta = point_a._hpr - point_b._hpr;
+
+    set_smooth_pos(point_b._pos + t * pos_delta, 
+                   point_b._hpr + t * hpr_delta, 
+                   timestamp);
+    compute_velocity(pos_delta, hpr_delta, age);
+  }
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::compute_velocity
+//       Access: Private
+//  Description: Computes the forward and rotational velocities of the
+//               moving object.
+////////////////////////////////////////////////////////////////////
+void SmoothMover::
+compute_velocity(const LVector3f &pos_delta, const LVecBase3f &hpr_delta,
+                 double age) {
+  // Also compute the velocity.  To get just the forward component
+  // of velocity, we need to project the velocity vector onto the y
+  // axis, as rotated by the current hpr.
+  LMatrix3f rot_mat;
+  compose_matrix(rot_mat, LVecBase3f(1.0, 1.0, 1.0), _smooth_hpr);
+  LVector3f y_axis = LVector3f(0.0, 1.0, 0.0) * rot_mat;
+  float forward_distance = pos_delta.dot(y_axis);
+  
+  _smooth_forward_velocity = forward_distance / age;
+  _smooth_rotational_velocity = hpr_delta[0] / age;
+}

direct/src/deadrec/smoothMover.h

@@ -76,6 +76,7 @@ PUBLISHED:
   INLINE void set_timestamp(double timestamp);
 
   void mark_position();
+  void clear_positions(bool reset_velocity);
 
   INLINE void compute_smooth_position();
   void compute_smooth_position(double timestamp);
@@ -84,6 +85,10 @@ PUBLISHED:
   INLINE const LVecBase3f &get_smooth_hpr() const;
   INLINE const LMatrix4f &get_smooth_mat();
 
+  INLINE float get_smooth_forward_velocity() const;
+  INLINE float get_smooth_rotational_velocity() const;
+
+
   // These static methods control the global properties of all
   // SmoothMovers.
   enum SmoothMode {
@@ -107,11 +112,20 @@ PUBLISHED:
   INLINE static void set_max_position_age(double age); 
   INLINE static double get_max_position_age(); 
 
+  INLINE static void set_reset_velocity_age(double age); 
+  INLINE static double get_reset_velocity_age(); 
+
   void output(ostream &out) const;
   void write(ostream &out) const;
 
 private:
+  void set_smooth_pos(const LPoint3f &pos, const LVecBase3f &hpr,
+                      double timestamp);
   void compose_smooth_mat();
+  void linear_interpolate(int point_before, int point_after, double timestamp);
+  void compute_velocity(const LVector3f &pos_delta, 
+                        const LVecBase3f &hpr_delta,
+                        double age);
 
   enum Flags {
     F_got_timestamp  = 0x0001,
@@ -131,16 +145,23 @@ private:
   LPoint3f _smooth_pos;
   LVecBase3f _smooth_hpr;
   LMatrix4f _smooth_mat;
+  double _smooth_timestamp;
+  bool _smooth_position_known;
   bool _computed_smooth_mat;
-  
+
+  double _smooth_forward_velocity;
+  double _smooth_rotational_velocity;
 
   typedef CircBuffer<SamplePoint, max_position_reports> Points;
   Points _points;
+  int _last_point_before;
+  int _last_point_after;
 
   static SmoothMode _smooth_mode;
   static PredictionMode _prediction_mode;
   static double _delay;
   static double _max_position_age;
+  static double _reset_velocity_age;
 };
 
 #include "smoothMover.I"