Panda3D
 All Classes Functions Variables Enumerations
smoothMover.cxx
00001 // Filename: smoothMover.cxx
00002 // Created by:  drose (19Oct01)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "smoothMover.h"
00016 #include "pnotify.h"
00017 #include "config_deadrec.h"
00018 
00019 ////////////////////////////////////////////////////////////////////
00020 //     Function: SmoothMover::Constructor
00021 //       Access: Published
00022 //  Description: 
00023 ////////////////////////////////////////////////////////////////////
00024 SmoothMover::
00025 SmoothMover() {
00026   _sample._pos.set(0.0, 0.0, 0.0);
00027   _sample._hpr.set(0.0, 0.0, 0.0);
00028   _sample._timestamp = 0.0;
00029 
00030   _smooth_pos.set(0.0, 0.0, 0.0);
00031   _smooth_hpr.set(0.0, 0.0, 0.0);
00032   _forward_axis.set(0.0, 1.0, 0.0);
00033   _smooth_timestamp = 0.0;
00034   _smooth_position_known = false;
00035   _smooth_position_changed = true;
00036   _computed_forward_axis = true;
00037 
00038   _smooth_forward_velocity = 0.0;
00039   _smooth_lateral_velocity = 0.0;
00040   _smooth_rotational_velocity = 0.0;
00041 
00042   _has_most_recent_timestamp = false;
00043 
00044   _last_point_before = -1;
00045   _last_point_after = -1;
00046 
00047   _net_timestamp_delay = 0;
00048   // Record one delay of 0 on the top of the delays array, just to
00049   // guarantee that the array is never completely empty.
00050   _timestamp_delays.push_back(0);
00051 
00052   _last_heard_from = 0.0;
00053 
00054   _smooth_mode = SM_off;
00055   _prediction_mode = PM_off;
00056   _delay = 0.2;
00057   _accept_clock_skew = accept_clock_skew;
00058   _directional_velocity = true;
00059   _default_to_standing_still = true;
00060   _max_position_age = 0.25;
00061   _expected_broadcast_period = 0.2;
00062   _reset_velocity_age = 0.3;
00063 }
00064 
00065 ////////////////////////////////////////////////////////////////////
00066 //     Function: SmoothMover::Destructor
00067 //       Access: Published
00068 //  Description: 
00069 ////////////////////////////////////////////////////////////////////
00070 SmoothMover::
00071 ~SmoothMover() {
00072 }
00073 
00074 ////////////////////////////////////////////////////////////////////
00075 //     Function: SmoothMover::mark_position
00076 //       Access: Published
00077 //  Description: Stores the position, orientation, and timestamp (if
00078 //               relevant) indicated by previous calls to set_pos(),
00079 //               set_hpr(), and set_timestamp() in a new position
00080 //               report.
00081 //
00082 //               When compute_smooth_position() is called, it uses
00083 //               these stored position reports to base its computation
00084 //               of the known position.
00085 ////////////////////////////////////////////////////////////////////
00086 void SmoothMover::
00087 mark_position() {
00088   /*
00089   if (deadrec_cat.is_debug()) {
00090     deadrec_cat.debug() << "mark_position\n";
00091   }
00092   */
00093   if (_smooth_mode == SM_off) {
00094     // With smoothing disabled, mark_position() simply stores its
00095     // current position in the smooth_position members.
00096 
00097     // In this mode, we also ignore the supplied timestamp, and just
00098     // use the current frame time--there's no need to risk trusting
00099     // the timestamp from another client.
00100     double timestamp = ClockObject::get_global_clock()->get_frame_time();
00101 
00102     // We also need to compute the velocity here.
00103     if (_smooth_position_known) {
00104       LVector3 pos_delta = _sample._pos - _smooth_pos;
00105       LVecBase3 hpr_delta = _sample._hpr - _smooth_hpr;
00106       double age = timestamp - _smooth_timestamp;
00107       age = min(age, _max_position_age);
00108 
00109       set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
00110       if (age != 0.0) {
00111         compute_velocity(pos_delta, hpr_delta, age);
00112       }
00113 
00114     } else {
00115       // No velocity is possible, just position and orientation.
00116       set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
00117     }
00118 
00119   } else {
00120     // Otherwise, smoothing is in effect and we store a true position
00121     // report.
00122 
00123     if (!_points.empty() && _points.back()._timestamp > _sample._timestamp) {
00124       if (deadrec_cat.is_debug()) {
00125         deadrec_cat.debug()
00126           << "*** timestamp out of order " << _points.back()._timestamp << " "
00127           << _sample._timestamp << "\n";
00128       }
00129 
00130       // If we get a timestamp out of order, one of us must have just
00131       // reset our clock.  Flush the sequence and start again.
00132       _points.clear();
00133 
00134       // That invalidates the index numbers.
00135       _last_point_before = -1;
00136       _last_point_after = -1;
00137 
00138       _points.push_back(_sample);
00139 
00140     } else if (!_points.empty() && _points.back()._timestamp == _sample._timestamp) {
00141       if (deadrec_cat.is_debug()) {
00142         deadrec_cat.debug()
00143           << "*** same timestamp\n";
00144       }
00145       // If the new timestamp is the same as the last timestamp, the
00146       // value simply replaces the previous value.
00147       _points.back() = _sample;
00148 
00149     } else if ((int)_points.size() >= max_position_reports) {
00150       if (deadrec_cat.is_debug()) {
00151         deadrec_cat.debug()
00152           << "*** dropped oldest position report\n";
00153       }
00154       // If we have too many position reports, throw away the oldest
00155       // one.
00156       _points.pop_front();
00157 
00158       --_last_point_before;
00159       --_last_point_after;
00160 
00161       _points.push_back(_sample);
00162 
00163     } else {
00164       // In the ordinary case, just add another sample.
00165       _points.push_back(_sample);
00166     }
00167   }
00168   //cout << "mark_position: " << _points.back()._pos << endl;
00169 }
00170 
00171 ////////////////////////////////////////////////////////////////////
00172 //     Function: SmoothMover::clear_positions
00173 //       Access: Published
00174 //  Description: Erases all the old position reports.  This should be
00175 //               done, for instance, prior to teleporting the avatar
00176 //               to a new position; otherwise, the smoother might try
00177 //               to lerp the avatar there.  If reset_velocity is true,
00178 //               the velocity is also reset to 0.
00179 ////////////////////////////////////////////////////////////////////
00180 void SmoothMover::
00181 clear_positions(bool reset_velocity) {
00182   if (deadrec_cat.is_debug()) {
00183     deadrec_cat.debug()
00184       << "clear_positions " << reset_velocity << "\n";
00185   }
00186 
00187   _points.clear();
00188   _last_point_before = -1;
00189   _last_point_after = -1;
00190   _smooth_position_known = false;
00191   _has_most_recent_timestamp = false;
00192 
00193   if (reset_velocity) {
00194     _smooth_forward_velocity = 0.0;
00195     _smooth_lateral_velocity = 0.0;
00196     _smooth_rotational_velocity = 0.0;
00197   }
00198 }
00199 
00200 ////////////////////////////////////////////////////////////////////
00201 //     Function: SmoothMover::compute_smooth_position
00202 //       Access: Published
00203 //  Description: Computes the smoothed position (and orientation) of
00204 //               the mover at the indicated point in time, based on
00205 //               the previous position reports.  After this call has
00206 //               been made, get_smooth_pos() etc. may be called to
00207 //               retrieve the smoothed position.
00208 //
00209 //               The return value is true if the value has changed (or
00210 //               might have changed) since the last call to
00211 //               compute_smooth_position(), or false if it remains the
00212 //               same.
00213 ////////////////////////////////////////////////////////////////////
00214 bool SmoothMover::
00215 compute_smooth_position(double timestamp) {
00216   if (deadrec_cat.is_spam()) {
00217     deadrec_cat.spam()
00218       << _points.size() << " points\n";
00219   }
00220 
00221   if (_points.empty()) {
00222     // With no position reports available, this function does nothing,
00223     // except to make sure that our velocity gets reset to zero after
00224     // a period of time.
00225 
00226     if (_smooth_position_known) {
00227       double age = timestamp - _smooth_timestamp;
00228       if (age > _reset_velocity_age) {
00229         if (deadrec_cat.is_debug()) {
00230           deadrec_cat.debug()
00231             << "points empty; reset velocity, age = " << age << "\n";
00232         }
00233         _smooth_forward_velocity = 0.0;
00234         _smooth_lateral_velocity = 0.0;
00235         _smooth_rotational_velocity = 0.0;
00236       }
00237     }
00238     bool result = _smooth_position_changed;
00239     _smooth_position_changed = false;
00240 
00241     if (deadrec_cat.is_spam()) {
00242       deadrec_cat.spam()
00243         << "  no points: " << result << "\n";
00244     }
00245     return result;
00246   }
00247   if (_smooth_mode == SM_off) {
00248     // With smoothing disabled, this function also does nothing,
00249     // except to ensure that any old bogus position reports are
00250     // cleared.
00251     clear_positions(false);
00252     bool result = _smooth_position_changed;
00253     _smooth_position_changed = false;
00254 
00255     if (deadrec_cat.is_spam()) {
00256       deadrec_cat.spam()
00257         << "  disabled: " << result << "\n";
00258     }
00259     return result;
00260   }
00261 
00262   // First, back up in time by the specified delay factor.
00263   double orig_timestamp = timestamp;
00264   timestamp -= _delay;
00265   if (_accept_clock_skew) {
00266     timestamp -= get_avg_timestamp_delay();
00267   }
00268 
00269   if (deadrec_cat.is_spam()) {
00270     deadrec_cat.spam()
00271       << "time = " << timestamp << ", " << _points.size()
00272       << " points, last = " << _last_point_before << ", " 
00273       << _last_point_after << "\n";
00274     deadrec_cat.spam(false)
00275       << "  ";
00276     for (int pi = 0; pi < (int)_points.size(); pi++) {
00277       deadrec_cat.spam(false) << _points[pi]._timestamp << " ";
00278     }
00279     deadrec_cat.spam(false) << "\n";
00280   }
00281 
00282   // Now look for the two bracketing position reports.
00283   int point_way_before = -1;
00284   int point_before = -1;
00285   double timestamp_before = 0.0;
00286   int point_after = -1;
00287   double timestamp_after = 0.0;
00288 
00289   int num_points = _points.size();
00290   int i;
00291 
00292   // Find the newest of the points before the indicated time.  Assume
00293   // that this will be no older than _last_point_before.
00294   i = max(0, _last_point_before);
00295   while (i < num_points && _points[i]._timestamp < timestamp) {
00296     point_before = i;
00297     timestamp_before = _points[i]._timestamp;
00298     ++i;
00299   }
00300   point_way_before = max(point_before - 1, -1);
00301 
00302   // Now the next point is presumably the oldest point after the
00303   // indicated time.
00304   if (i < num_points) {
00305     point_after = i;
00306     timestamp_after = _points[i]._timestamp;
00307   }
00308 
00309   if (deadrec_cat.is_spam()) {
00310     deadrec_cat.spam()
00311       << "  found points (" << point_way_before << ") " << point_before
00312       << ", " << point_after << "\n";
00313   }
00314 
00315   if (point_before < 0) {
00316     nassertr(point_after >= 0, false);
00317     // If we only have an after point, we have to start there.
00318     bool result = !(_last_point_before == point_before && 
00319                     _last_point_after == point_after);
00320     const SamplePoint &point = _points[point_after];
00321     set_smooth_pos(point._pos, point._hpr, timestamp);
00322     _smooth_forward_velocity = 0.0;
00323     _smooth_lateral_velocity = 0.0;
00324     _smooth_rotational_velocity = 0.0;
00325     _last_point_before = point_before;
00326     _last_point_after = point_after;
00327     if (deadrec_cat.is_spam()) {
00328       deadrec_cat.spam()
00329         << "  only an after point: " << _last_point_before << ", " 
00330         << _last_point_after << "\n";
00331     }
00332     return result;
00333   }
00334 
00335   bool result = true;
00336 
00337   if (point_after < 0 && _prediction_mode != PM_off) {
00338     // With prediction in effect, we're allowed to anticipate where
00339     // the avatar is going by a tiny bit, if we don't have current
00340     // enough data.  This works only if we have at least two points of
00341     // old data.
00342     if (point_way_before >= 0) {
00343       // To implement simple prediction, we simply back up in time to
00344       // the previous two timestamps, and base our linear
00345       // interpolation off of those two, extending into the future.
00346       SamplePoint &point = _points[point_way_before];
00347       point_after = point_before;
00348       timestamp_after = timestamp_before;
00349       point_before = point_way_before;
00350       timestamp_before = point._timestamp;
00351       
00352       if (timestamp > timestamp_after + _max_position_age) {
00353         // Don't allow the prediction to get too far into the
00354         // future.
00355         timestamp = timestamp_after + _max_position_age;
00356       }
00357     }
00358   }
00359 
00360   if (point_after < 0) {
00361     // If we only have a before point even after we've checked for the
00362     // possibility of using prediction, then we have to stop there.
00363     if (point_way_before >= 0) {
00364       // Use the previous two points, if we've got 'em, so we can
00365       // still reflect the avatar's velocity.
00366       if (deadrec_cat.is_spam()) {
00367         deadrec_cat.spam()
00368           << "  previous two\n";
00369       }
00370       linear_interpolate(point_way_before, point_before, timestamp_before);
00371 
00372     } else {
00373       if (deadrec_cat.is_spam()) {
00374         deadrec_cat.spam()
00375           << "  one point\n";
00376       }
00377       // If we really only have one point, use it.
00378       const SamplePoint &point = _points[point_before];
00379       set_smooth_pos(point._pos, point._hpr, timestamp);
00380     }
00381 
00382     double age = timestamp - timestamp_before;
00383     if (age > _reset_velocity_age) {
00384       if (deadrec_cat.is_spam()) {
00385         deadrec_cat.spam()
00386           << "  reset_velocity, age = " << age << "\n";
00387       }
00388       _smooth_forward_velocity = 0.0;
00389       _smooth_lateral_velocity = 0.0;
00390       _smooth_rotational_velocity = 0.0;
00391     }
00392 
00393     result = !(_last_point_before == point_before && 
00394                _last_point_after == point_after);
00395   } else {
00396     // If we have two points, we can linearly interpolate between them.
00397     if (deadrec_cat.is_spam()) {
00398       deadrec_cat.spam()
00399         << "  normal interpolate\n";
00400     }
00401     SamplePoint &point_b = _points[point_before];
00402     const SamplePoint &point_a = _points[point_after];
00403     
00404     if (point_b._pos == point_a._pos && point_b._hpr == point_a._hpr) {
00405       // The points are equivalent, so just return that.
00406       if (deadrec_cat.is_spam()) {
00407         deadrec_cat.spam()
00408           << "Points are equivalent\n";
00409       }
00410       set_smooth_pos(point_b._pos, point_b._hpr, timestamp);
00411 
00412       // This implies that velocity is 0.
00413       _smooth_forward_velocity = 0.0;
00414       _smooth_lateral_velocity = 0.0;
00415       _smooth_rotational_velocity = 0.0;
00416 
00417     } else {
00418       // The points are different, so we have to do some work.
00419       double age = (point_a._timestamp - point_b._timestamp);
00420     
00421       if (_default_to_standing_still && (age > _max_position_age)) {
00422         // If the first point is too old, assume there were a lot of
00423         // implicit standing still messages that weren't sent.  Insert a new
00424         // sample point to reflect this.
00425         if (deadrec_cat.is_spam()) {
00426           deadrec_cat.spam()
00427             << "  first point too old: age = " << age << "\n";
00428         }
00429         SamplePoint new_point = point_b;
00430         new_point._timestamp = point_a._timestamp - _expected_broadcast_period;
00431         if (deadrec_cat.is_spam()) {
00432           deadrec_cat.spam()
00433             << "  constructed new timestamp at " << new_point._timestamp
00434             << "\n";
00435         }
00436         if (new_point._timestamp > point_b._timestamp) {
00437           _points.insert(_points.begin() + point_after, new_point);
00438           
00439           // Now we've monkeyed with the sequence.  Start over.
00440           if (deadrec_cat.is_spam()) {
00441             deadrec_cat.spam()
00442               << "  recursing after time adjustment.\n";
00443           }
00444           return compute_smooth_position(orig_timestamp);
00445         }
00446       }
00447 
00448       linear_interpolate(point_before, point_after, timestamp);
00449     }
00450   }
00451 
00452   if (deadrec_cat.is_spam()) {
00453     deadrec_cat.spam()
00454       << "  changing " << _last_point_before << ", " << _last_point_after
00455       << " to " << point_before << ", " << point_after << "\n";
00456   }
00457   _last_point_before = point_before;
00458   _last_point_after = point_after;
00459 
00460 
00461   // Assume we'll never get another compute_smooth_position() request
00462   // for an older time than this, and remove all the timestamps at the
00463   // head of the queue up to but not including point_way_before.
00464   while (point_way_before > 0) {
00465     nassertr(!_points.empty(), result);
00466     _points.pop_front();
00467 
00468     --point_way_before;
00469     --_last_point_before;
00470     --_last_point_after;
00471     if (deadrec_cat.is_spam()) {
00472       deadrec_cat.spam()
00473         << "  popping old point: " << _last_point_before << ", " 
00474         << _last_point_after << "\n";
00475     }
00476   }
00477 
00478   // If we are not using prediction mode, we can also remove
00479   // point_way_before.
00480   if (_prediction_mode == PM_off) {
00481     if (point_way_before == 0) {
00482       nassertr(!_points.empty(), result);
00483       _points.pop_front();
00484       
00485       --point_way_before;
00486       --_last_point_before;
00487       --_last_point_after;
00488       if (deadrec_cat.is_spam()) {
00489         deadrec_cat.spam()
00490           << "  popping way_before point: " << _last_point_before << ", " 
00491           << _last_point_after << "\n";
00492       }
00493     }
00494 
00495     // And if there's only one point left, remove even that one
00496     // after a while.
00497     /* jbutler: commented this out, seems to cause the smoothing pop that occurs
00498                 when this object is stopped for a while then starts moving again
00499     if (_points.size() == 1) {
00500       double age = timestamp - _points.back()._timestamp;
00501       if (deadrec_cat.is_spam()) {
00502         deadrec_cat.spam()
00503           << "considering clearing all points, age = " << age << "\n";
00504       }
00505       if (age > _reset_velocity_age) {
00506         if (deadrec_cat.is_spam()) {
00507           deadrec_cat.spam()
00508             << "clearing all points.\n";
00509         }
00510         _points.clear();
00511       }
00512     }
00513     */
00514   }
00515 
00516   if (deadrec_cat.is_spam()) {
00517     deadrec_cat.spam()
00518       << "  result = " << result << "\n";
00519   }
00520 
00521   return result;
00522 }
00523 
00524 ////////////////////////////////////////////////////////////////////
00525 //     Function: SmoothMover::get_latest_position
00526 //       Access: Published
00527 //  Description: Updates the smooth_pos (and smooth_hpr, etc.) members
00528 //               to reflect the absolute latest position known for
00529 //               this avatar.  This may result in a pop to the most
00530 //               recent position.
00531 //
00532 //               Returns true if the latest position is known, false
00533 //               otherwise.
00534 ////////////////////////////////////////////////////////////////////
00535 bool SmoothMover::
00536 get_latest_position() {
00537   if (deadrec_cat.is_debug()) {
00538     deadrec_cat.debug() << "get_latest_position\n";
00539   }
00540   if (_points.empty()) {
00541     // Nothing to do if there are no points.
00542     return _smooth_position_known;
00543   }
00544 
00545   const SamplePoint &point = _points.back();
00546   set_smooth_pos(point._pos, point._hpr, point._timestamp);
00547   _smooth_forward_velocity = 0.0;
00548   _smooth_lateral_velocity = 0.0;
00549   _smooth_rotational_velocity = 0.0;
00550   return true;
00551 }
00552 
00553 ////////////////////////////////////////////////////////////////////
00554 //     Function: SmoothMover::output
00555 //       Access: Published
00556 //  Description: 
00557 ////////////////////////////////////////////////////////////////////
00558 void SmoothMover::
00559 output(ostream &out) const {
00560   out << "SmoothMover, " << _points.size() << " sample points.";
00561 }
00562 
00563 ////////////////////////////////////////////////////////////////////
00564 //     Function: SmoothMover::write
00565 //       Access: Published
00566 //  Description: 
00567 ////////////////////////////////////////////////////////////////////
00568 void SmoothMover::
00569 write(ostream &out) const {
00570   out << "SmoothMover, " << _points.size() << " sample points:\n";
00571   int num_points = _points.size();
00572   for (int i = 0; i < num_points; i++) {
00573     const SamplePoint &point = _points[i];
00574     out << "  " << i << ". time = " << point._timestamp << " pos = " 
00575         << point._pos << " hpr = " << point._hpr << "\n";
00576   }
00577 }
00578 
00579 ////////////////////////////////////////////////////////////////////
00580 //     Function: SmoothMover::set_smooth_pos
00581 //       Access: Private
00582 //  Description: Sets the computed smooth position and orientation for
00583 //               the indicated timestamp.
00584 ////////////////////////////////////////////////////////////////////
00585 void SmoothMover::
00586 set_smooth_pos(const LPoint3 &pos, const LVecBase3 &hpr,
00587                double timestamp) {
00588   if (deadrec_cat.is_spam()) {
00589     deadrec_cat.spam()
00590       << "set_smooth_pos(" << pos << ", " << hpr << ", "
00591       << timestamp << ")\n";
00592   }
00593 
00594   if (_smooth_pos != pos) {
00595     _smooth_pos = pos;
00596     _smooth_position_changed = true;
00597   }
00598   if (_smooth_hpr != hpr) {
00599     _smooth_hpr = hpr;
00600     _smooth_position_changed = true;
00601     _computed_forward_axis = false;
00602   }
00603 
00604   _smooth_timestamp = timestamp;
00605   _smooth_position_known = true;
00606 }
00607 
00608 ////////////////////////////////////////////////////////////////////
00609 //     Function: SmoothMover::linear_interpolate
00610 //       Access: Private
00611 //  Description: Interpolates the smooth position linearly between the
00612 //               two bracketing position reports.
00613 ////////////////////////////////////////////////////////////////////
00614 void SmoothMover::
00615 linear_interpolate(int point_before, int point_after, double timestamp) {
00616   SamplePoint &point_b = _points[point_before];
00617   const SamplePoint &point_a = _points[point_after];
00618 
00619   double age = (point_a._timestamp - point_b._timestamp);
00620 
00621   /*
00622   Points::const_iterator pi;
00623   cout << "linear_interpolate: ";
00624   for (pi = _points.begin(); pi != _points.end(); ++pi) {
00625     cout << "(" << (*pi)._pos << "), ";
00626   }
00627   cout << endl;
00628   */
00629 
00630   if (point_before == _last_point_before && 
00631       point_after == _last_point_after) {
00632     if (deadrec_cat.is_spam()) {
00633       deadrec_cat.spam()
00634         << "  same two points\n";
00635     }
00636 
00637     // If these are the same two points we found last time (which is
00638     // likely), we can save a bit of work.
00639     double t = (timestamp - point_b._timestamp) / age;
00640 
00641     if (deadrec_cat.is_spam()) {
00642       deadrec_cat.spam()
00643         << "   interp " << t << ": " << point_b._pos << " to " << point_a._pos
00644         << "\n";
00645     }
00646     set_smooth_pos(point_b._pos + t * (point_a._pos - point_b._pos),
00647                    point_b._hpr + t * (point_a._hpr - point_b._hpr),
00648                    timestamp);
00649 
00650     // The velocity remains the same as last time.
00651 
00652   } else {
00653     // To interpolate the hpr's, we must first make sure that both
00654     // angles are on the same side of the discontinuity.
00655     for (int j = 0; j < 3; j++) {
00656       if ((point_b._hpr[j] - point_a._hpr[j]) > 180.0) {
00657         point_b._hpr[j] -= 360.0;
00658       } else if ((point_b._hpr[j] - point_a._hpr[j]) < -180.0) {
00659         point_b._hpr[j] += 360.0;
00660       }
00661     }
00662     
00663     double t = (timestamp - point_b._timestamp) / age;
00664     LVector3 pos_delta = point_a._pos - point_b._pos;
00665     LVecBase3 hpr_delta = point_a._hpr - point_b._hpr;
00666 
00667     if (deadrec_cat.is_spam()) {
00668       deadrec_cat.spam()
00669         << "   interp " << t << ": " << point_b._pos << " to " << point_a._pos
00670         << "\n";
00671     }
00672     set_smooth_pos(point_b._pos + t * pos_delta, 
00673                    point_b._hpr + t * hpr_delta, 
00674                    timestamp);
00675     compute_velocity(pos_delta, hpr_delta, age);
00676   }
00677 }
00678 
00679 ////////////////////////////////////////////////////////////////////
00680 //     Function: SmoothMover::compute_velocity
00681 //       Access: Private
00682 //  Description: Computes the forward and rotational velocities of the
00683 //               moving object.
00684 ////////////////////////////////////////////////////////////////////
00685 void SmoothMover::
00686 compute_velocity(const LVector3 &pos_delta, const LVecBase3 &hpr_delta,
00687                  double age) {
00688   _smooth_rotational_velocity = hpr_delta[0] / age;
00689 
00690   if (_directional_velocity) {
00691     // To get just the forward component of velocity, we need to project
00692     // the velocity vector onto the y axis, as rotated by the current
00693     // hpr.
00694     if (!_computed_forward_axis) {
00695       LMatrix3 rot_mat;
00696       compose_matrix(rot_mat, LVecBase3(1.0, 1.0, 1.0), _smooth_hpr);
00697       _forward_axis = LVector3(0.0, 1.0, 0.0) * rot_mat;
00698       
00699       if (deadrec_cat.is_spam()) {
00700         deadrec_cat.spam()
00701           << "  compute forward_axis = " << _forward_axis << "\n";
00702       }
00703     }
00704     
00705     LVector3 lateral_axis = _forward_axis.cross(LVector3(0.0,0.0,1.0));
00706     
00707     PN_stdfloat forward_distance = pos_delta.dot(_forward_axis);
00708     PN_stdfloat lateral_distance = pos_delta.dot(lateral_axis);
00709     
00710     _smooth_forward_velocity = forward_distance / age;
00711     _smooth_lateral_velocity = lateral_distance / age;
00712 
00713   } else {
00714     _smooth_forward_velocity = pos_delta.length();
00715     _smooth_lateral_velocity = 0.0f;
00716   }   
00717 
00718   if (deadrec_cat.is_spam()) {
00719     deadrec_cat.spam()
00720       << "  compute_velocity = " << _smooth_forward_velocity << "\n";
00721   }
00722 }
00723 
00724 ////////////////////////////////////////////////////////////////////
00725 //     Function: SmoothMover::record_timestamp_delay
00726 //       Access: Private
00727 //  Description: Records the delay measured in receiving this
00728 //               particular timestamp.  The average delay of the last
00729 //               n timestamps will be used to smooth the motion
00730 //               properly.
00731 ////////////////////////////////////////////////////////////////////
00732 void SmoothMover::
00733 record_timestamp_delay(double timestamp) {
00734   double now = ClockObject::get_global_clock()->get_frame_time();
00735 
00736   // Convert the delay to an integer number of milliseconds.  Integers
00737   // are better than doubles because they don't accumulate errors over
00738   // time.
00739   int delay = (int)((now - timestamp) * 1000.0);
00740   if (_timestamp_delays.full()) {
00741     _net_timestamp_delay -= _timestamp_delays.front();
00742     _timestamp_delays.pop_front();
00743   }
00744   _net_timestamp_delay += delay;
00745   _timestamp_delays.push_back(delay);
00746 
00747   _last_heard_from = now;
00748 }
00749 
00750 ////////////////////////////////////////////////////////////////////
00751 //     Function: SmoothMover::handle_wrt_reparent
00752 //       Access: Private
00753 //  Description: Node is being wrtReparented, update recorded
00754 //               sample positions to reflect new parent
00755 ////////////////////////////////////////////////////////////////////
00756 void SmoothMover::
00757 handle_wrt_reparent(NodePath &old_parent, NodePath &new_parent) {
00758   Points::iterator pi;
00759   NodePath np = old_parent.attach_new_node("smoothMoverWrtReparent");
00760 
00761   //cout << "handle_wrt_reparent: ";
00762   for (pi = _points.begin(); pi != _points.end(); pi++) {
00763     np.set_pos_hpr((*pi)._pos, (*pi)._hpr);
00764     (*pi)._pos = np.get_pos(new_parent);
00765     (*pi)._hpr = np.get_hpr(new_parent);
00766     //cout << "(" << (*pi)._pos << "), ";
00767   }
00768   //cout << endl;
00769   
00770   np.set_pos_hpr(_sample._pos, _sample._hpr);
00771   _sample._pos = np.get_pos(new_parent);
00772   _sample._hpr = np.get_hpr(new_parent);
00773 
00774   np.set_pos_hpr(_smooth_pos, _smooth_hpr);
00775   _smooth_pos = np.get_pos(new_parent);
00776   _smooth_hpr = np.get_hpr(new_parent);
00777 
00778   _computed_forward_axis = false;
00779 
00780   np.detach_node();
00781 }
 All Classes Functions Variables Enumerations