From 1bb18f17bb7ccc1d3e26a09c8e2eba0250e7fc23 Mon Sep 17 00:00:00 2001
From: David Rose <david@ddrose.com>
Date: Fri, 19 Oct 2001 23:17:08 +0000
Subject: [PATCH] *** empty log message ***

---
 direct/src/deadrec/smoothMover.I   | 24 +++++++++++++++++++
 direct/src/deadrec/smoothMover.cxx | 37 ++++++++++++++++++++++++++----
 direct/src/deadrec/smoothMover.h   |  4 ++++
 3 files changed, 61 insertions(+), 4 deletions(-)

diff --git a/direct/src/deadrec/smoothMover.I b/direct/src/deadrec/smoothMover.I
index 4c180f997e..242813655d 100644
--- a/direct/src/deadrec/smoothMover.I
+++ b/direct/src/deadrec/smoothMover.I
@@ -337,3 +337,27 @@ INLINE double SmoothMover::
 get_delay() {
   return _delay;
 }
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::set_max_position_age
+//       Access: Published, Static
+//  Description: Sets the maximum amount of time a position is allowed
+//               to remain unchanged before assuming it represents the
+//               avatar actually standing still.
+////////////////////////////////////////////////////////////////////
+INLINE void SmoothMover::
+set_max_position_age(double age) {
+  _max_position_age = age;
+}
+
+////////////////////////////////////////////////////////////////////
+//     Function: SmoothMover::get_max_position_age
+//       Access: Published, Static
+//  Description: Returns the maximum amount of time a position is
+//               allowed to remain unchanged before assuming it
+//               represents the avatar actually standing still.
+////////////////////////////////////////////////////////////////////
+INLINE double SmoothMover::
+get_max_position_age() {
+  return _max_position_age;
+}
diff --git a/direct/src/deadrec/smoothMover.cxx b/direct/src/deadrec/smoothMover.cxx
index 1a217d7cce..d3369a37ce 100644
--- a/direct/src/deadrec/smoothMover.cxx
+++ b/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.0;
+double SmoothMover::_max_position_age = 0.2;
 
 ////////////////////////////////////////////////////////////////////
 //     Function: SmoothMover::Constructor
@@ -101,7 +102,11 @@ 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.
+    // this function does nothing, except to ensure that any old bogus
+    // position reports are cleared.
+    while (!_points.empty()) {
+      _points.pop_front();
+    }
     return;
   }
 
@@ -155,12 +160,36 @@ compute_smooth_position(double timestamp) {
 
   } else {
     // If we have two points, we can linearly interpolate between them.
-    const SamplePoint &point_b = _points[point_before];
+    SamplePoint &point_b = _points[point_before];
     const SamplePoint &point_a = _points[point_after];
 
-    double t = (timestamp - timestamp_before) / (timestamp_after - timestamp_before);
+    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);
-    _smooth_hpr = point_b._hpr + t * (point_a._hpr - point_b._hpr);
+
+    // 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;
   }
 
diff --git a/direct/src/deadrec/smoothMover.h b/direct/src/deadrec/smoothMover.h
index 55f3586d44..953224361c 100644
--- a/direct/src/deadrec/smoothMover.h
+++ b/direct/src/deadrec/smoothMover.h
@@ -104,6 +104,9 @@ PUBLISHED:
   INLINE static void set_delay(double delay); 
   INLINE static double get_delay(); 
 
+  INLINE static void set_max_position_age(double age); 
+  INLINE static double get_max_position_age(); 
+
   void output(ostream &out) const;
   void write(ostream &out) const;
 
@@ -137,6 +140,7 @@ private:
   static SmoothMode _smooth_mode;
   static PredictionMode _prediction_mode;
   static double _delay;
+  static double _max_position_age;
 };
 
 #include "smoothMover.I"