Panda3D
|
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 }