Panda3D
 All Classes Functions Variables Enumerations
smoothMover.cxx
1 // Filename: smoothMover.cxx
2 // Created by: drose (19Oct01)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "smoothMover.h"
16 #include "pnotify.h"
17 #include "config_deadrec.h"
18 
19 ////////////////////////////////////////////////////////////////////
20 // Function: SmoothMover::Constructor
21 // Access: Published
22 // Description:
23 ////////////////////////////////////////////////////////////////////
24 SmoothMover::
25 SmoothMover() {
26  _sample._pos.set(0.0, 0.0, 0.0);
27  _sample._hpr.set(0.0, 0.0, 0.0);
28  _sample._timestamp = 0.0;
29 
30  _smooth_pos.set(0.0, 0.0, 0.0);
31  _smooth_hpr.set(0.0, 0.0, 0.0);
32  _forward_axis.set(0.0, 1.0, 0.0);
33  _smooth_timestamp = 0.0;
34  _smooth_position_known = false;
35  _smooth_position_changed = true;
36  _computed_forward_axis = true;
37 
38  _smooth_forward_velocity = 0.0;
39  _smooth_lateral_velocity = 0.0;
40  _smooth_rotational_velocity = 0.0;
41 
42  _has_most_recent_timestamp = false;
43 
44  _last_point_before = -1;
45  _last_point_after = -1;
46 
47  _net_timestamp_delay = 0;
48  // Record one delay of 0 on the top of the delays array, just to
49  // guarantee that the array is never completely empty.
50  _timestamp_delays.push_back(0);
51 
52  _last_heard_from = 0.0;
53 
54  _smooth_mode = SM_off;
55  _prediction_mode = PM_off;
56  _delay = 0.2;
57  _accept_clock_skew = accept_clock_skew;
58  _directional_velocity = true;
59  _default_to_standing_still = true;
60  _max_position_age = 0.25;
61  _expected_broadcast_period = 0.2;
62  _reset_velocity_age = 0.3;
63 }
64 
65 ////////////////////////////////////////////////////////////////////
66 // Function: SmoothMover::Destructor
67 // Access: Published
68 // Description:
69 ////////////////////////////////////////////////////////////////////
70 SmoothMover::
71 ~SmoothMover() {
72 }
73 
74 ////////////////////////////////////////////////////////////////////
75 // Function: SmoothMover::mark_position
76 // Access: Published
77 // Description: Stores the position, orientation, and timestamp (if
78 // relevant) indicated by previous calls to set_pos(),
79 // set_hpr(), and set_timestamp() in a new position
80 // report.
81 //
82 // When compute_smooth_position() is called, it uses
83 // these stored position reports to base its computation
84 // of the known position.
85 ////////////////////////////////////////////////////////////////////
86 void SmoothMover::
88  /*
89  if (deadrec_cat.is_debug()) {
90  deadrec_cat.debug() << "mark_position\n";
91  }
92  */
93  if (_smooth_mode == SM_off) {
94  // With smoothing disabled, mark_position() simply stores its
95  // current position in the smooth_position members.
96 
97  // In this mode, we also ignore the supplied timestamp, and just
98  // use the current frame time--there's no need to risk trusting
99  // the timestamp from another client.
100  double timestamp = ClockObject::get_global_clock()->get_frame_time();
101 
102  // We also need to compute the velocity here.
103  if (_smooth_position_known) {
104  LVector3 pos_delta = _sample._pos - _smooth_pos;
105  LVecBase3 hpr_delta = _sample._hpr - _smooth_hpr;
106  double age = timestamp - _smooth_timestamp;
107  age = min(age, _max_position_age);
108 
109  set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
110  if (age != 0.0) {
111  compute_velocity(pos_delta, hpr_delta, age);
112  }
113 
114  } else {
115  // No velocity is possible, just position and orientation.
116  set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
117  }
118 
119  } else {
120  // Otherwise, smoothing is in effect and we store a true position
121  // report.
122 
123  if (!_points.empty() && _points.back()._timestamp > _sample._timestamp) {
124  if (deadrec_cat.is_debug()) {
125  deadrec_cat.debug()
126  << "*** timestamp out of order " << _points.back()._timestamp << " "
127  << _sample._timestamp << "\n";
128  }
129 
130  // If we get a timestamp out of order, one of us must have just
131  // reset our clock. Flush the sequence and start again.
132  _points.clear();
133 
134  // That invalidates the index numbers.
135  _last_point_before = -1;
136  _last_point_after = -1;
137 
138  _points.push_back(_sample);
139 
140  } else if (!_points.empty() && _points.back()._timestamp == _sample._timestamp) {
141  if (deadrec_cat.is_debug()) {
142  deadrec_cat.debug()
143  << "*** same timestamp\n";
144  }
145  // If the new timestamp is the same as the last timestamp, the
146  // value simply replaces the previous value.
147  _points.back() = _sample;
148 
149  } else if ((int)_points.size() >= max_position_reports) {
150  if (deadrec_cat.is_debug()) {
151  deadrec_cat.debug()
152  << "*** dropped oldest position report\n";
153  }
154  // If we have too many position reports, throw away the oldest
155  // one.
156  _points.pop_front();
157 
158  --_last_point_before;
159  --_last_point_after;
160 
161  _points.push_back(_sample);
162 
163  } else {
164  // In the ordinary case, just add another sample.
165  _points.push_back(_sample);
166  }
167  }
168  //cout << "mark_position: " << _points.back()._pos << endl;
169 }
170 
171 ////////////////////////////////////////////////////////////////////
172 // Function: SmoothMover::clear_positions
173 // Access: Published
174 // Description: Erases all the old position reports. This should be
175 // done, for instance, prior to teleporting the avatar
176 // to a new position; otherwise, the smoother might try
177 // to lerp the avatar there. If reset_velocity is true,
178 // the velocity is also reset to 0.
179 ////////////////////////////////////////////////////////////////////
180 void SmoothMover::
181 clear_positions(bool reset_velocity) {
182  if (deadrec_cat.is_debug()) {
183  deadrec_cat.debug()
184  << "clear_positions " << reset_velocity << "\n";
185  }
186 
187  _points.clear();
188  _last_point_before = -1;
189  _last_point_after = -1;
190  _smooth_position_known = false;
191  _has_most_recent_timestamp = false;
192 
193  if (reset_velocity) {
194  _smooth_forward_velocity = 0.0;
195  _smooth_lateral_velocity = 0.0;
196  _smooth_rotational_velocity = 0.0;
197  }
198 }
199 
200 ////////////////////////////////////////////////////////////////////
201 // Function: SmoothMover::compute_smooth_position
202 // Access: Published
203 // Description: Computes the smoothed position (and orientation) of
204 // the mover at the indicated point in time, based on
205 // the previous position reports. After this call has
206 // been made, get_smooth_pos() etc. may be called to
207 // retrieve the smoothed position.
208 //
209 // The return value is true if the value has changed (or
210 // might have changed) since the last call to
211 // compute_smooth_position(), or false if it remains the
212 // same.
213 ////////////////////////////////////////////////////////////////////
214 bool SmoothMover::
215 compute_smooth_position(double timestamp) {
216  if (deadrec_cat.is_spam()) {
217  deadrec_cat.spam()
218  << _points.size() << " points\n";
219  }
220 
221  if (_points.empty()) {
222  // With no position reports available, this function does nothing,
223  // except to make sure that our velocity gets reset to zero after
224  // a period of time.
225 
226  if (_smooth_position_known) {
227  double age = timestamp - _smooth_timestamp;
228  if (age > _reset_velocity_age) {
229  if (deadrec_cat.is_debug()) {
230  deadrec_cat.debug()
231  << "points empty; reset velocity, age = " << age << "\n";
232  }
233  _smooth_forward_velocity = 0.0;
234  _smooth_lateral_velocity = 0.0;
235  _smooth_rotational_velocity = 0.0;
236  }
237  }
238  bool result = _smooth_position_changed;
239  _smooth_position_changed = false;
240 
241  if (deadrec_cat.is_spam()) {
242  deadrec_cat.spam()
243  << " no points: " << result << "\n";
244  }
245  return result;
246  }
247  if (_smooth_mode == SM_off) {
248  // With smoothing disabled, this function also does nothing,
249  // except to ensure that any old bogus position reports are
250  // cleared.
251  clear_positions(false);
252  bool result = _smooth_position_changed;
253  _smooth_position_changed = false;
254 
255  if (deadrec_cat.is_spam()) {
256  deadrec_cat.spam()
257  << " disabled: " << result << "\n";
258  }
259  return result;
260  }
261 
262  // First, back up in time by the specified delay factor.
263  double orig_timestamp = timestamp;
264  timestamp -= _delay;
265  if (_accept_clock_skew) {
266  timestamp -= get_avg_timestamp_delay();
267  }
268 
269  if (deadrec_cat.is_spam()) {
270  deadrec_cat.spam()
271  << "time = " << timestamp << ", " << _points.size()
272  << " points, last = " << _last_point_before << ", "
273  << _last_point_after << "\n";
274  deadrec_cat.spam(false)
275  << " ";
276  for (int pi = 0; pi < (int)_points.size(); pi++) {
277  deadrec_cat.spam(false) << _points[pi]._timestamp << " ";
278  }
279  deadrec_cat.spam(false) << "\n";
280  }
281 
282  // Now look for the two bracketing position reports.
283  int point_way_before = -1;
284  int point_before = -1;
285  double timestamp_before = 0.0;
286  int point_after = -1;
287  double timestamp_after = 0.0;
288 
289  int num_points = _points.size();
290  int i;
291 
292  // Find the newest of the points before the indicated time. Assume
293  // that this will be no older than _last_point_before.
294  i = max(0, _last_point_before);
295  while (i < num_points && _points[i]._timestamp < timestamp) {
296  point_before = i;
297  timestamp_before = _points[i]._timestamp;
298  ++i;
299  }
300  point_way_before = max(point_before - 1, -1);
301 
302  // Now the next point is presumably the oldest point after the
303  // indicated time.
304  if (i < num_points) {
305  point_after = i;
306  timestamp_after = _points[i]._timestamp;
307  }
308 
309  if (deadrec_cat.is_spam()) {
310  deadrec_cat.spam()
311  << " found points (" << point_way_before << ") " << point_before
312  << ", " << point_after << "\n";
313  }
314 
315  if (point_before < 0) {
316  nassertr(point_after >= 0, false);
317  // If we only have an after point, we have to start there.
318  bool result = !(_last_point_before == point_before &&
319  _last_point_after == point_after);
320  const SamplePoint &point = _points[point_after];
321  set_smooth_pos(point._pos, point._hpr, timestamp);
322  _smooth_forward_velocity = 0.0;
323  _smooth_lateral_velocity = 0.0;
324  _smooth_rotational_velocity = 0.0;
325  _last_point_before = point_before;
326  _last_point_after = point_after;
327  if (deadrec_cat.is_spam()) {
328  deadrec_cat.spam()
329  << " only an after point: " << _last_point_before << ", "
330  << _last_point_after << "\n";
331  }
332  return result;
333  }
334 
335  bool result = true;
336 
337  if (point_after < 0 && _prediction_mode != PM_off) {
338  // With prediction in effect, we're allowed to anticipate where
339  // the avatar is going by a tiny bit, if we don't have current
340  // enough data. This works only if we have at least two points of
341  // old data.
342  if (point_way_before >= 0) {
343  // To implement simple prediction, we simply back up in time to
344  // the previous two timestamps, and base our linear
345  // interpolation off of those two, extending into the future.
346  SamplePoint &point = _points[point_way_before];
347  point_after = point_before;
348  timestamp_after = timestamp_before;
349  point_before = point_way_before;
350  timestamp_before = point._timestamp;
351 
352  if (timestamp > timestamp_after + _max_position_age) {
353  // Don't allow the prediction to get too far into the
354  // future.
355  timestamp = timestamp_after + _max_position_age;
356  }
357  }
358  }
359 
360  if (point_after < 0) {
361  // If we only have a before point even after we've checked for the
362  // possibility of using prediction, then we have to stop there.
363  if (point_way_before >= 0) {
364  // Use the previous two points, if we've got 'em, so we can
365  // still reflect the avatar's velocity.
366  if (deadrec_cat.is_spam()) {
367  deadrec_cat.spam()
368  << " previous two\n";
369  }
370  linear_interpolate(point_way_before, point_before, timestamp_before);
371 
372  } else {
373  if (deadrec_cat.is_spam()) {
374  deadrec_cat.spam()
375  << " one point\n";
376  }
377  // If we really only have one point, use it.
378  const SamplePoint &point = _points[point_before];
379  set_smooth_pos(point._pos, point._hpr, timestamp);
380  }
381 
382  double age = timestamp - timestamp_before;
383  if (age > _reset_velocity_age) {
384  if (deadrec_cat.is_spam()) {
385  deadrec_cat.spam()
386  << " reset_velocity, age = " << age << "\n";
387  }
388  _smooth_forward_velocity = 0.0;
389  _smooth_lateral_velocity = 0.0;
390  _smooth_rotational_velocity = 0.0;
391  }
392 
393  result = !(_last_point_before == point_before &&
394  _last_point_after == point_after);
395  } else {
396  // If we have two points, we can linearly interpolate between them.
397  if (deadrec_cat.is_spam()) {
398  deadrec_cat.spam()
399  << " normal interpolate\n";
400  }
401  SamplePoint &point_b = _points[point_before];
402  const SamplePoint &point_a = _points[point_after];
403 
404  if (point_b._pos == point_a._pos && point_b._hpr == point_a._hpr) {
405  // The points are equivalent, so just return that.
406  if (deadrec_cat.is_spam()) {
407  deadrec_cat.spam()
408  << "Points are equivalent\n";
409  }
410  set_smooth_pos(point_b._pos, point_b._hpr, timestamp);
411 
412  // This implies that velocity is 0.
413  _smooth_forward_velocity = 0.0;
414  _smooth_lateral_velocity = 0.0;
415  _smooth_rotational_velocity = 0.0;
416 
417  } else {
418  // The points are different, so we have to do some work.
419  double age = (point_a._timestamp - point_b._timestamp);
420 
421  if (_default_to_standing_still && (age > _max_position_age)) {
422  // If the first point is too old, assume there were a lot of
423  // implicit standing still messages that weren't sent. Insert a new
424  // sample point to reflect this.
425  if (deadrec_cat.is_spam()) {
426  deadrec_cat.spam()
427  << " first point too old: age = " << age << "\n";
428  }
429  SamplePoint new_point = point_b;
430  new_point._timestamp = point_a._timestamp - _expected_broadcast_period;
431  if (deadrec_cat.is_spam()) {
432  deadrec_cat.spam()
433  << " constructed new timestamp at " << new_point._timestamp
434  << "\n";
435  }
436  if (new_point._timestamp > point_b._timestamp) {
437  _points.insert(_points.begin() + point_after, new_point);
438 
439  // Now we've monkeyed with the sequence. Start over.
440  if (deadrec_cat.is_spam()) {
441  deadrec_cat.spam()
442  << " recursing after time adjustment.\n";
443  }
444  return compute_smooth_position(orig_timestamp);
445  }
446  }
447 
448  linear_interpolate(point_before, point_after, timestamp);
449  }
450  }
451 
452  if (deadrec_cat.is_spam()) {
453  deadrec_cat.spam()
454  << " changing " << _last_point_before << ", " << _last_point_after
455  << " to " << point_before << ", " << point_after << "\n";
456  }
457  _last_point_before = point_before;
458  _last_point_after = point_after;
459 
460 
461  // Assume we'll never get another compute_smooth_position() request
462  // for an older time than this, and remove all the timestamps at the
463  // head of the queue up to but not including point_way_before.
464  while (point_way_before > 0) {
465  nassertr(!_points.empty(), result);
466  _points.pop_front();
467 
468  --point_way_before;
469  --_last_point_before;
470  --_last_point_after;
471  if (deadrec_cat.is_spam()) {
472  deadrec_cat.spam()
473  << " popping old point: " << _last_point_before << ", "
474  << _last_point_after << "\n";
475  }
476  }
477 
478  // If we are not using prediction mode, we can also remove
479  // point_way_before.
480  if (_prediction_mode == PM_off) {
481  if (point_way_before == 0) {
482  nassertr(!_points.empty(), result);
483  _points.pop_front();
484 
485  --point_way_before;
486  --_last_point_before;
487  --_last_point_after;
488  if (deadrec_cat.is_spam()) {
489  deadrec_cat.spam()
490  << " popping way_before point: " << _last_point_before << ", "
491  << _last_point_after << "\n";
492  }
493  }
494 
495  // And if there's only one point left, remove even that one
496  // after a while.
497  /* jbutler: commented this out, seems to cause the smoothing pop that occurs
498  when this object is stopped for a while then starts moving again
499  if (_points.size() == 1) {
500  double age = timestamp - _points.back()._timestamp;
501  if (deadrec_cat.is_spam()) {
502  deadrec_cat.spam()
503  << "considering clearing all points, age = " << age << "\n";
504  }
505  if (age > _reset_velocity_age) {
506  if (deadrec_cat.is_spam()) {
507  deadrec_cat.spam()
508  << "clearing all points.\n";
509  }
510  _points.clear();
511  }
512  }
513  */
514  }
515 
516  if (deadrec_cat.is_spam()) {
517  deadrec_cat.spam()
518  << " result = " << result << "\n";
519  }
520 
521  return result;
522 }
523 
524 ////////////////////////////////////////////////////////////////////
525 // Function: SmoothMover::get_latest_position
526 // Access: Published
527 // Description: Updates the smooth_pos (and smooth_hpr, etc.) members
528 // to reflect the absolute latest position known for
529 // this avatar. This may result in a pop to the most
530 // recent position.
531 //
532 // Returns true if the latest position is known, false
533 // otherwise.
534 ////////////////////////////////////////////////////////////////////
535 bool SmoothMover::
537  if (deadrec_cat.is_debug()) {
538  deadrec_cat.debug() << "get_latest_position\n";
539  }
540  if (_points.empty()) {
541  // Nothing to do if there are no points.
542  return _smooth_position_known;
543  }
544 
545  const SamplePoint &point = _points.back();
546  set_smooth_pos(point._pos, point._hpr, point._timestamp);
547  _smooth_forward_velocity = 0.0;
548  _smooth_lateral_velocity = 0.0;
549  _smooth_rotational_velocity = 0.0;
550  return true;
551 }
552 
553 ////////////////////////////////////////////////////////////////////
554 // Function: SmoothMover::output
555 // Access: Published
556 // Description:
557 ////////////////////////////////////////////////////////////////////
558 void SmoothMover::
559 output(ostream &out) const {
560  out << "SmoothMover, " << _points.size() << " sample points.";
561 }
562 
563 ////////////////////////////////////////////////////////////////////
564 // Function: SmoothMover::write
565 // Access: Published
566 // Description:
567 ////////////////////////////////////////////////////////////////////
568 void SmoothMover::
569 write(ostream &out) const {
570  out << "SmoothMover, " << _points.size() << " sample points:\n";
571  int num_points = _points.size();
572  for (int i = 0; i < num_points; i++) {
573  const SamplePoint &point = _points[i];
574  out << " " << i << ". time = " << point._timestamp << " pos = "
575  << point._pos << " hpr = " << point._hpr << "\n";
576  }
577 }
578 
579 ////////////////////////////////////////////////////////////////////
580 // Function: SmoothMover::set_smooth_pos
581 // Access: Private
582 // Description: Sets the computed smooth position and orientation for
583 // the indicated timestamp.
584 ////////////////////////////////////////////////////////////////////
585 void SmoothMover::
586 set_smooth_pos(const LPoint3 &pos, const LVecBase3 &hpr,
587  double timestamp) {
588  if (deadrec_cat.is_spam()) {
589  deadrec_cat.spam()
590  << "set_smooth_pos(" << pos << ", " << hpr << ", "
591  << timestamp << ")\n";
592  }
593 
594  if (_smooth_pos != pos) {
595  _smooth_pos = pos;
596  _smooth_position_changed = true;
597  }
598  if (_smooth_hpr != hpr) {
599  _smooth_hpr = hpr;
600  _smooth_position_changed = true;
601  _computed_forward_axis = false;
602  }
603 
604  _smooth_timestamp = timestamp;
605  _smooth_position_known = true;
606 }
607 
608 ////////////////////////////////////////////////////////////////////
609 // Function: SmoothMover::linear_interpolate
610 // Access: Private
611 // Description: Interpolates the smooth position linearly between the
612 // two bracketing position reports.
613 ////////////////////////////////////////////////////////////////////
614 void SmoothMover::
615 linear_interpolate(int point_before, int point_after, double timestamp) {
616  SamplePoint &point_b = _points[point_before];
617  const SamplePoint &point_a = _points[point_after];
618 
619  double age = (point_a._timestamp - point_b._timestamp);
620 
621  /*
622  Points::const_iterator pi;
623  cout << "linear_interpolate: ";
624  for (pi = _points.begin(); pi != _points.end(); ++pi) {
625  cout << "(" << (*pi)._pos << "), ";
626  }
627  cout << endl;
628  */
629 
630  if (point_before == _last_point_before &&
631  point_after == _last_point_after) {
632  if (deadrec_cat.is_spam()) {
633  deadrec_cat.spam()
634  << " same two points\n";
635  }
636 
637  // If these are the same two points we found last time (which is
638  // likely), we can save a bit of work.
639  double t = (timestamp - point_b._timestamp) / age;
640 
641  if (deadrec_cat.is_spam()) {
642  deadrec_cat.spam()
643  << " interp " << t << ": " << point_b._pos << " to " << point_a._pos
644  << "\n";
645  }
646  set_smooth_pos(point_b._pos + t * (point_a._pos - point_b._pos),
647  point_b._hpr + t * (point_a._hpr - point_b._hpr),
648  timestamp);
649 
650  // The velocity remains the same as last time.
651 
652  } else {
653  // To interpolate the hpr's, we must first make sure that both
654  // angles are on the same side of the discontinuity.
655  for (int j = 0; j < 3; j++) {
656  if ((point_b._hpr[j] - point_a._hpr[j]) > 180.0) {
657  point_b._hpr[j] -= 360.0;
658  } else if ((point_b._hpr[j] - point_a._hpr[j]) < -180.0) {
659  point_b._hpr[j] += 360.0;
660  }
661  }
662 
663  double t = (timestamp - point_b._timestamp) / age;
664  LVector3 pos_delta = point_a._pos - point_b._pos;
665  LVecBase3 hpr_delta = point_a._hpr - point_b._hpr;
666 
667  if (deadrec_cat.is_spam()) {
668  deadrec_cat.spam()
669  << " interp " << t << ": " << point_b._pos << " to " << point_a._pos
670  << "\n";
671  }
672  set_smooth_pos(point_b._pos + t * pos_delta,
673  point_b._hpr + t * hpr_delta,
674  timestamp);
675  compute_velocity(pos_delta, hpr_delta, age);
676  }
677 }
678 
679 ////////////////////////////////////////////////////////////////////
680 // Function: SmoothMover::compute_velocity
681 // Access: Private
682 // Description: Computes the forward and rotational velocities of the
683 // moving object.
684 ////////////////////////////////////////////////////////////////////
685 void SmoothMover::
686 compute_velocity(const LVector3 &pos_delta, const LVecBase3 &hpr_delta,
687  double age) {
688  _smooth_rotational_velocity = hpr_delta[0] / age;
689 
690  if (_directional_velocity) {
691  // To get just the forward component of velocity, we need to project
692  // the velocity vector onto the y axis, as rotated by the current
693  // hpr.
694  if (!_computed_forward_axis) {
695  LMatrix3 rot_mat;
696  compose_matrix(rot_mat, LVecBase3(1.0, 1.0, 1.0), _smooth_hpr);
697  _forward_axis = LVector3(0.0, 1.0, 0.0) * rot_mat;
698 
699  if (deadrec_cat.is_spam()) {
700  deadrec_cat.spam()
701  << " compute forward_axis = " << _forward_axis << "\n";
702  }
703  }
704 
705  LVector3 lateral_axis = _forward_axis.cross(LVector3(0.0,0.0,1.0));
706 
707  PN_stdfloat forward_distance = pos_delta.dot(_forward_axis);
708  PN_stdfloat lateral_distance = pos_delta.dot(lateral_axis);
709 
710  _smooth_forward_velocity = forward_distance / age;
711  _smooth_lateral_velocity = lateral_distance / age;
712 
713  } else {
714  _smooth_forward_velocity = pos_delta.length();
715  _smooth_lateral_velocity = 0.0f;
716  }
717 
718  if (deadrec_cat.is_spam()) {
719  deadrec_cat.spam()
720  << " compute_velocity = " << _smooth_forward_velocity << "\n";
721  }
722 }
723 
724 ////////////////////////////////////////////////////////////////////
725 // Function: SmoothMover::record_timestamp_delay
726 // Access: Private
727 // Description: Records the delay measured in receiving this
728 // particular timestamp. The average delay of the last
729 // n timestamps will be used to smooth the motion
730 // properly.
731 ////////////////////////////////////////////////////////////////////
732 void SmoothMover::
733 record_timestamp_delay(double timestamp) {
735 
736  // Convert the delay to an integer number of milliseconds. Integers
737  // are better than doubles because they don't accumulate errors over
738  // time.
739  int delay = (int)((now - timestamp) * 1000.0);
740  if (_timestamp_delays.full()) {
741  _net_timestamp_delay -= _timestamp_delays.front();
742  _timestamp_delays.pop_front();
743  }
744  _net_timestamp_delay += delay;
745  _timestamp_delays.push_back(delay);
746 
747  _last_heard_from = now;
748 }
749 
750 ////////////////////////////////////////////////////////////////////
751 // Function: SmoothMover::handle_wrt_reparent
752 // Access: Private
753 // Description: Node is being wrtReparented, update recorded
754 // sample positions to reflect new parent
755 ////////////////////////////////////////////////////////////////////
756 void SmoothMover::
757 handle_wrt_reparent(NodePath &old_parent, NodePath &new_parent) {
758  Points::iterator pi;
759  NodePath np = old_parent.attach_new_node("smoothMoverWrtReparent");
760 
761  //cout << "handle_wrt_reparent: ";
762  for (pi = _points.begin(); pi != _points.end(); pi++) {
763  np.set_pos_hpr((*pi)._pos, (*pi)._hpr);
764  (*pi)._pos = np.get_pos(new_parent);
765  (*pi)._hpr = np.get_hpr(new_parent);
766  //cout << "(" << (*pi)._pos << "), ";
767  }
768  //cout << endl;
769 
770  np.set_pos_hpr(_sample._pos, _sample._hpr);
771  _sample._pos = np.get_pos(new_parent);
772  _sample._hpr = np.get_hpr(new_parent);
773 
774  np.set_pos_hpr(_smooth_pos, _smooth_hpr);
775  _smooth_pos = np.get_pos(new_parent);
776  _smooth_hpr = np.get_hpr(new_parent);
777 
778  _computed_forward_axis = false;
779 
780  np.detach_node();
781 }
static ClockObject * get_global_clock()
Returns a pointer to the global ClockObject.
Definition: clockObject.I:271
This is the base class for all three-component vectors and points.
Definition: lvecBase3.h:105
void push_back(const Thing &t)
Adds an item to the end of the buffer.
Definition: circBuffer.I:207
void set_pos_hpr(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z, PN_stdfloat h, PN_stdfloat p, PN_stdfloat r)
Sets the translation and rotation component of the transform, leaving scale untouched.
Definition: nodePath.I:905
void clear_positions(bool reset_velocity)
Erases all the old position reports.
bool get_latest_position()
Updates the smooth_pos (and smooth_hpr, etc.) members to reflect the absolute latest position known f...
void mark_position()
Stores the position, orientation, and timestamp (if relevant) indicated by previous calls to set_pos(...
Definition: smoothMover.cxx:87
bool compute_smooth_position()
Computes the smoothed position (and orientation) of the mover at the indicated point in time...
Definition: smoothMover.I:348
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
Definition: lvector3.h:100
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
Definition: lpoint3.h:99
LVecBase3 get_hpr() const
Retrieves the rotation component of the transform.
Definition: nodePath.cxx:1253
void handle_wrt_reparent(NodePath &old_parent, NodePath &new_parent)
Node is being wrtReparented, update recorded sample positions to reflect new parent.
bool full() const
Returns true if the buffer is full; if this is true, push_back() will fail.
Definition: circBuffer.I:81
float length() const
Returns the length of the vector, by the Pythagorean theorem.
Definition: lvecBase3.h:765
LPoint3 get_pos() const
Retrieves the translation component of the transform.
Definition: nodePath.cxx:1178
double get_frame_time(Thread *current_thread=Thread::get_current_thread()) const
Returns the time in seconds as of the last time tick() was called (typically, this will be as of the ...
Definition: clockObject.I:48
void pop_front()
Removes the first item from the buffer.
Definition: circBuffer.I:152
const Thing & front() const
Returns a reference to the first item in the queue.
Definition: circBuffer.I:96
This is a 3-by-3 transform matrix.
Definition: lmatrix.h:110
void detach_node(Thread *current_thread=Thread::get_current_thread())
Disconnects the referenced node from its parent, but does not immediately delete it.
Definition: nodePath.cxx:804
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
Definition: nodePath.h:165
NodePath attach_new_node(PandaNode *node, int sort=0, Thread *current_thread=Thread::get_current_thread()) const
Attaches a new node, with or without existing parents, to the scene graph below the referenced node o...
Definition: nodePath.cxx:723