15 #include "smoothMover.h"
17 #include "config_deadrec.h"
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;
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;
38 _smooth_forward_velocity = 0.0;
39 _smooth_lateral_velocity = 0.0;
40 _smooth_rotational_velocity = 0.0;
42 _has_most_recent_timestamp =
false;
44 _last_point_before = -1;
45 _last_point_after = -1;
47 _net_timestamp_delay = 0;
52 _last_heard_from = 0.0;
54 _smooth_mode = SM_off;
55 _prediction_mode = PM_off;
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;
93 if (_smooth_mode == SM_off) {
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);
109 set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
111 compute_velocity(pos_delta, hpr_delta, age);
116 set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
123 if (!_points.empty() && _points.back()._timestamp > _sample._timestamp) {
124 if (deadrec_cat.is_debug()) {
126 <<
"*** timestamp out of order " << _points.back()._timestamp <<
" "
127 << _sample._timestamp <<
"\n";
135 _last_point_before = -1;
136 _last_point_after = -1;
138 _points.push_back(_sample);
140 }
else if (!_points.empty() && _points.back()._timestamp == _sample._timestamp) {
141 if (deadrec_cat.is_debug()) {
143 <<
"*** same timestamp\n";
147 _points.back() = _sample;
149 }
else if ((
int)_points.size() >= max_position_reports) {
150 if (deadrec_cat.is_debug()) {
152 <<
"*** dropped oldest position report\n";
158 --_last_point_before;
161 _points.push_back(_sample);
165 _points.push_back(_sample);
182 if (deadrec_cat.is_debug()) {
184 <<
"clear_positions " << reset_velocity <<
"\n";
188 _last_point_before = -1;
189 _last_point_after = -1;
190 _smooth_position_known =
false;
191 _has_most_recent_timestamp =
false;
193 if (reset_velocity) {
194 _smooth_forward_velocity = 0.0;
195 _smooth_lateral_velocity = 0.0;
196 _smooth_rotational_velocity = 0.0;
216 if (deadrec_cat.is_spam()) {
218 << _points.size() <<
" points\n";
221 if (_points.empty()) {
226 if (_smooth_position_known) {
227 double age = timestamp - _smooth_timestamp;
228 if (age > _reset_velocity_age) {
229 if (deadrec_cat.is_debug()) {
231 <<
"points empty; reset velocity, age = " << age <<
"\n";
233 _smooth_forward_velocity = 0.0;
234 _smooth_lateral_velocity = 0.0;
235 _smooth_rotational_velocity = 0.0;
238 bool result = _smooth_position_changed;
239 _smooth_position_changed =
false;
241 if (deadrec_cat.is_spam()) {
243 <<
" no points: " << result <<
"\n";
247 if (_smooth_mode == SM_off) {
252 bool result = _smooth_position_changed;
253 _smooth_position_changed =
false;
255 if (deadrec_cat.is_spam()) {
257 <<
" disabled: " << result <<
"\n";
263 double orig_timestamp = timestamp;
265 if (_accept_clock_skew) {
266 timestamp -= get_avg_timestamp_delay();
269 if (deadrec_cat.is_spam()) {
271 <<
"time = " << timestamp <<
", " << _points.size()
272 <<
" points, last = " << _last_point_before <<
", "
273 << _last_point_after <<
"\n";
274 deadrec_cat.spam(
false)
276 for (
int pi = 0; pi < (int)_points.size(); pi++) {
277 deadrec_cat.spam(
false) << _points[pi]._timestamp <<
" ";
279 deadrec_cat.spam(
false) <<
"\n";
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;
289 int num_points = _points.size();
294 i = max(0, _last_point_before);
295 while (i < num_points && _points[i]._timestamp < timestamp) {
297 timestamp_before = _points[i]._timestamp;
300 point_way_before = max(point_before - 1, -1);
304 if (i < num_points) {
306 timestamp_after = _points[i]._timestamp;
309 if (deadrec_cat.is_spam()) {
311 <<
" found points (" << point_way_before <<
") " << point_before
312 <<
", " << point_after <<
"\n";
315 if (point_before < 0) {
316 nassertr(point_after >= 0,
false);
318 bool result = !(_last_point_before == point_before &&
319 _last_point_after == 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()) {
329 <<
" only an after point: " << _last_point_before <<
", "
330 << _last_point_after <<
"\n";
337 if (point_after < 0 && _prediction_mode != PM_off) {
342 if (point_way_before >= 0) {
347 point_after = point_before;
348 timestamp_after = timestamp_before;
349 point_before = point_way_before;
350 timestamp_before = point._timestamp;
352 if (timestamp > timestamp_after + _max_position_age) {
355 timestamp = timestamp_after + _max_position_age;
360 if (point_after < 0) {
363 if (point_way_before >= 0) {
366 if (deadrec_cat.is_spam()) {
368 <<
" previous two\n";
370 linear_interpolate(point_way_before, point_before, timestamp_before);
373 if (deadrec_cat.is_spam()) {
379 set_smooth_pos(point._pos, point._hpr, timestamp);
382 double age = timestamp - timestamp_before;
383 if (age > _reset_velocity_age) {
384 if (deadrec_cat.is_spam()) {
386 <<
" reset_velocity, age = " << age <<
"\n";
388 _smooth_forward_velocity = 0.0;
389 _smooth_lateral_velocity = 0.0;
390 _smooth_rotational_velocity = 0.0;
393 result = !(_last_point_before == point_before &&
394 _last_point_after == point_after);
397 if (deadrec_cat.is_spam()) {
399 <<
" normal interpolate\n";
404 if (point_b._pos == point_a._pos && point_b._hpr == point_a._hpr) {
406 if (deadrec_cat.is_spam()) {
408 <<
"Points are equivalent\n";
410 set_smooth_pos(point_b._pos, point_b._hpr, timestamp);
413 _smooth_forward_velocity = 0.0;
414 _smooth_lateral_velocity = 0.0;
415 _smooth_rotational_velocity = 0.0;
419 double age = (point_a._timestamp - point_b._timestamp);
421 if (_default_to_standing_still && (age > _max_position_age)) {
425 if (deadrec_cat.is_spam()) {
427 <<
" first point too old: age = " << age <<
"\n";
430 new_point._timestamp = point_a._timestamp - _expected_broadcast_period;
431 if (deadrec_cat.is_spam()) {
433 <<
" constructed new timestamp at " << new_point._timestamp
436 if (new_point._timestamp > point_b._timestamp) {
437 _points.insert(_points.begin() + point_after, new_point);
440 if (deadrec_cat.is_spam()) {
442 <<
" recursing after time adjustment.\n";
448 linear_interpolate(point_before, point_after, timestamp);
452 if (deadrec_cat.is_spam()) {
454 <<
" changing " << _last_point_before <<
", " << _last_point_after
455 <<
" to " << point_before <<
", " << point_after <<
"\n";
457 _last_point_before = point_before;
458 _last_point_after = point_after;
464 while (point_way_before > 0) {
465 nassertr(!_points.empty(), result);
469 --_last_point_before;
471 if (deadrec_cat.is_spam()) {
473 <<
" popping old point: " << _last_point_before <<
", "
474 << _last_point_after <<
"\n";
480 if (_prediction_mode == PM_off) {
481 if (point_way_before == 0) {
482 nassertr(!_points.empty(), result);
486 --_last_point_before;
488 if (deadrec_cat.is_spam()) {
490 <<
" popping way_before point: " << _last_point_before <<
", "
491 << _last_point_after <<
"\n";
516 if (deadrec_cat.is_spam()) {
518 <<
" result = " << result <<
"\n";
537 if (deadrec_cat.is_debug()) {
538 deadrec_cat.debug() <<
"get_latest_position\n";
540 if (_points.empty()) {
542 return _smooth_position_known;
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;
559 output(ostream &out)
const {
560 out <<
"SmoothMover, " << _points.size() <<
" sample points.";
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";
588 if (deadrec_cat.is_spam()) {
590 <<
"set_smooth_pos(" << pos <<
", " << hpr <<
", "
591 << timestamp <<
")\n";
594 if (_smooth_pos != pos) {
596 _smooth_position_changed =
true;
598 if (_smooth_hpr != hpr) {
600 _smooth_position_changed =
true;
601 _computed_forward_axis =
false;
604 _smooth_timestamp = timestamp;
605 _smooth_position_known =
true;
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];
619 double age = (point_a._timestamp - point_b._timestamp);
630 if (point_before == _last_point_before &&
631 point_after == _last_point_after) {
632 if (deadrec_cat.is_spam()) {
634 <<
" same two points\n";
639 double t = (timestamp - point_b._timestamp) / age;
641 if (deadrec_cat.is_spam()) {
643 <<
" interp " << t <<
": " << point_b._pos <<
" to " << point_a._pos
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),
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;
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;
667 if (deadrec_cat.is_spam()) {
669 <<
" interp " << t <<
": " << point_b._pos <<
" to " << point_a._pos
672 set_smooth_pos(point_b._pos + t * pos_delta,
673 point_b._hpr + t * hpr_delta,
675 compute_velocity(pos_delta, hpr_delta, age);
688 _smooth_rotational_velocity = hpr_delta[0] / age;
690 if (_directional_velocity) {
694 if (!_computed_forward_axis) {
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;
699 if (deadrec_cat.is_spam()) {
701 <<
" compute forward_axis = " << _forward_axis <<
"\n";
707 PN_stdfloat forward_distance = pos_delta.dot(_forward_axis);
708 PN_stdfloat lateral_distance = pos_delta.dot(lateral_axis);
710 _smooth_forward_velocity = forward_distance / age;
711 _smooth_lateral_velocity = lateral_distance / age;
714 _smooth_forward_velocity = pos_delta.
length();
715 _smooth_lateral_velocity = 0.0f;
718 if (deadrec_cat.is_spam()) {
720 <<
" compute_velocity = " << _smooth_forward_velocity <<
"\n";
733 record_timestamp_delay(
double timestamp) {
739 int delay = (int)((now - timestamp) * 1000.0);
740 if (_timestamp_delays.
full()) {
741 _net_timestamp_delay -= _timestamp_delays.
front();
744 _net_timestamp_delay += delay;
747 _last_heard_from = now;
762 for (pi = _points.begin(); pi != _points.end(); pi++) {
764 (*pi)._pos = np.
get_pos(new_parent);
765 (*pi)._hpr = np.
get_hpr(new_parent);
771 _sample._pos = np.
get_pos(new_parent);
772 _sample._hpr = np.
get_hpr(new_parent);
775 _smooth_pos = np.
get_pos(new_parent);
776 _smooth_hpr = np.
get_hpr(new_parent);
778 _computed_forward_axis =
false;
static ClockObject * get_global_clock()
Returns a pointer to the global ClockObject.
This is the base class for all three-component vectors and points.
void push_back(const Thing &t)
Adds an item to the end of the buffer.
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.
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(...
bool compute_smooth_position()
Computes the smoothed position (and orientation) of the mover at the indicated point in time...
This is a three-component vector distance (as opposed to a three-component point, which represents a ...
This is a three-component point in space (as opposed to a three-component vector, which represents a ...
LVecBase3 get_hpr() const
Retrieves the rotation component of the transform.
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.
float length() const
Returns the length of the vector, by the Pythagorean theorem.
LPoint3 get_pos() const
Retrieves the translation component of the transform.
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 ...
void pop_front()
Removes the first item from the buffer.
const Thing & front() const
Returns a reference to the first item in the queue.
This is a 3-by-3 transform matrix.
void detach_node(Thread *current_thread=Thread::get_current_thread())
Disconnects the referenced node from its parent, but does not immediately delete it.
NodePath is the fundamental system for disambiguating instances, and also provides a higher-level int...
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...