00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "smoothMover.h"
00016 #include "pnotify.h"
00017 #include "config_deadrec.h"
00018
00019
00020
00021
00022
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
00049
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
00067
00068
00069
00070 SmoothMover::
00071 ~SmoothMover() {
00072 }
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086 void SmoothMover::
00087 mark_position() {
00088
00089
00090
00091
00092
00093 if (_smooth_mode == SM_off) {
00094
00095
00096
00097
00098
00099
00100 double timestamp = ClockObject::get_global_clock()->get_frame_time();
00101
00102
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
00116 set_smooth_pos(_sample._pos, _sample._hpr, timestamp);
00117 }
00118
00119 } else {
00120
00121
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
00131
00132 _points.clear();
00133
00134
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
00146
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
00155
00156 _points.pop_front();
00157
00158 --_last_point_before;
00159 --_last_point_after;
00160
00161 _points.push_back(_sample);
00162
00163 } else {
00164
00165 _points.push_back(_sample);
00166 }
00167 }
00168
00169 }
00170
00171
00172
00173
00174
00175
00176
00177
00178
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
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
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
00223
00224
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
00249
00250
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
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
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
00293
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
00303
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
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
00339
00340
00341
00342 if (point_way_before >= 0) {
00343
00344
00345
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
00354
00355 timestamp = timestamp_after + _max_position_age;
00356 }
00357 }
00358 }
00359
00360 if (point_after < 0) {
00361
00362
00363 if (point_way_before >= 0) {
00364
00365
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
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
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
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
00413 _smooth_forward_velocity = 0.0;
00414 _smooth_lateral_velocity = 0.0;
00415 _smooth_rotational_velocity = 0.0;
00416
00417 } else {
00418
00419 double age = (point_a._timestamp - point_b._timestamp);
00420
00421 if (_default_to_standing_still && (age > _max_position_age)) {
00422
00423
00424
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
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
00462
00463
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
00479
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
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
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
00526
00527
00528
00529
00530
00531
00532
00533
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
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
00555
00556
00557
00558 void SmoothMover::
00559 output(ostream &out) const {
00560 out << "SmoothMover, " << _points.size() << " sample points.";
00561 }
00562
00563
00564
00565
00566
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
00581
00582
00583
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
00610
00611
00612
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
00623
00624
00625
00626
00627
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
00638
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
00651
00652 } else {
00653
00654
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
00681
00682
00683
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
00692
00693
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
00726
00727
00728
00729
00730
00731
00732 void SmoothMover::
00733 record_timestamp_delay(double timestamp) {
00734 double now = ClockObject::get_global_clock()->get_frame_time();
00735
00736
00737
00738
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
00752
00753
00754
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
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
00767 }
00768
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 }