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++) {
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) {
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 ...
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 ...
bool full() const
Returns true if the buffer is full; if this is true, push_back() will fail.
void handle_wrt_reparent(NodePath &old_parent, NodePath &new_parent)
Node is being wrtReparented, update recorded sample positions to reflect new parent.
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...
void pop_front()
Removes the first item from the buffer.
LVecBase3 get_hpr() const
Retrieves the rotation component of the transform.
LPoint3 get_pos() const
Retrieves the translation component of the transform.
float length() const
Returns the length of the vector, by the Pythagorean theorem.
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...