diff --git a/direct/src/deadrec/smoothMover.I b/direct/src/deadrec/smoothMover.I index 242813655d..82276123c3 100644 --- a/direct/src/deadrec/smoothMover.I +++ b/direct/src/deadrec/smoothMover.I @@ -264,6 +264,33 @@ get_smooth_mat() { return _smooth_mat; } +//////////////////////////////////////////////////////////////////// +// Function: SmoothMover::get_smooth_forward_velocity +// Access: Published +// Description: Returns the speed at which the avatar is moving, in +// feet per second, along its own forward axis (after +// applying the avatar's hpr). This will be a positive +// number if the avatar is moving forward, and a +// negative number if it is moving backward. +//////////////////////////////////////////////////////////////////// +INLINE float SmoothMover:: +get_smooth_forward_velocity() const { + return _smooth_forward_velocity; +} + +//////////////////////////////////////////////////////////////////// +// Function: SmoothMover::get_smooth_rotational_velocity +// Access: Published +// Description: Returns the speed at which the avatar is rotating in +// the horizontal plane (i.e. heading), in degrees per +// second. This may be positive or negative, according +// to the direction of rotation. +//////////////////////////////////////////////////////////////////// +INLINE float SmoothMover:: +get_smooth_rotational_velocity() const { + return _smooth_rotational_velocity; +} + //////////////////////////////////////////////////////////////////// // Function: SmoothMover::set_smooth_mode // Access: Published, Static @@ -361,3 +388,30 @@ INLINE double SmoothMover:: get_max_position_age() { return _max_position_age; } + +//////////////////////////////////////////////////////////////////// +// Function: SmoothMover::set_reset_velocity_age +// Access: Published, Static +// Description: Sets the amount of time that should elapse after the +// last position report before the velocity is reset to +// 0. This is similar to max_position_age, but it is +// only used to determine the resetting of the reported +// velocity. It should always be greater than or equal +// to max_position_age. +//////////////////////////////////////////////////////////////////// +INLINE void SmoothMover:: +set_reset_velocity_age(double age) { + _reset_velocity_age = age; +} + +//////////////////////////////////////////////////////////////////// +// Function: SmoothMover::get_reset_velocity_age +// Access: Published, Static +// Description: Returns the amount of time that should elapse after +// the last position report before the velocity is reset +// to 0. See set_reset_velocity_age(). +//////////////////////////////////////////////////////////////////// +INLINE double SmoothMover:: +get_reset_velocity_age() { + return _reset_velocity_age; +} diff --git a/direct/src/deadrec/smoothMover.cxx b/direct/src/deadrec/smoothMover.cxx index d3369a37ce..c239e0405d 100644 --- a/direct/src/deadrec/smoothMover.cxx +++ b/direct/src/deadrec/smoothMover.cxx @@ -22,8 +22,9 @@ SmoothMover::SmoothMode SmoothMover::_smooth_mode = SmoothMover::SM_off; SmoothMover::PredictionMode SmoothMover::_prediction_mode = SmoothMover::PM_off; -double SmoothMover::_delay = 0.0; -double SmoothMover::_max_position_age = 0.2; +double SmoothMover::_delay = 0.2; +double SmoothMover::_max_position_age = 0.25; +double SmoothMover::_reset_velocity_age = 0.5; //////////////////////////////////////////////////////////////////// // Function: SmoothMover::Constructor @@ -41,7 +42,15 @@ SmoothMover() { _smooth_pos.set(0.0, 0.0, 0.0); _smooth_hpr.set(0.0, 0.0, 0.0); _smooth_mat = LMatrix4f::ident_mat(); + _smooth_timestamp = 0.0; + _smooth_position_known = false; _computed_smooth_mat = true; + + _smooth_forward_velocity = 0.0; + _smooth_rotational_velocity = 0.0; + + _last_point_before = -1; + _last_point_after = -1; } //////////////////////////////////////////////////////////////////// @@ -70,9 +79,23 @@ mark_position() { if (_smooth_mode == SM_off) { // With smoothing disabled, mark_position() simply stores its // current position in the smooth_position members. - _smooth_pos = _sample._pos; - _smooth_hpr = _sample._hpr; - _computed_smooth_mat = false; + + // We also need to compute the velocity here. + if (_smooth_position_known) { + LVector3f pos_delta = _sample._pos - _smooth_pos; + LVecBase3f hpr_delta = _sample._hpr - _smooth_hpr; + double age = _sample._timestamp - _smooth_timestamp; + age = min(age, _max_position_age); + + set_smooth_pos(_sample._pos, _sample._hpr, _sample._timestamp); + if (age != 0.0) { + compute_velocity(pos_delta, hpr_delta, age); + } + + } else { + // No velocity is possible, just position and orientation. + set_smooth_pos(_sample._pos, _sample._hpr, _sample._timestamp); + } } else { // Otherwise, smoothing is in effect and we store a true position @@ -82,6 +105,10 @@ mark_position() { // If we have too many position reports, throw away the oldest // one. _points.pop_front(); + + // That invalidates the index numbers. + _last_point_before = -1; + _last_point_after = -1; } _points.push_back(_sample); @@ -89,6 +116,30 @@ mark_position() { } } +//////////////////////////////////////////////////////////////////// +// Function: SmoothMover::clear_positions +// Access: Published +// Description: Erases all the old position reports. This should be +// done, for instance, prior to teleporting the avatar +// to a new position; otherwise, the smoother might try +// to lerp the avatar there. If reset_velocity is true, +// the velocity is also reset to 0. +//////////////////////////////////////////////////////////////////// +void SmoothMover:: +clear_positions(bool reset_velocity) { + while (!_points.empty()) { + _points.pop_front(); + } + _last_point_before = -1; + _last_point_after = -1; + _smooth_position_known = false; + + if (reset_velocity) { + _smooth_forward_velocity = 0.0; + _smooth_rotational_velocity = 0.0; + } +} + //////////////////////////////////////////////////////////////////// // Function: SmoothMover::compute_smooth_position // Access: Published @@ -100,15 +151,27 @@ mark_position() { //////////////////////////////////////////////////////////////////// void SmoothMover:: compute_smooth_position(double timestamp) { - if (_smooth_mode == SM_off || _points.empty()) { - // With smoothing disabled, or with no position reports available, - // this function does nothing, except to ensure that any old bogus - // position reports are cleared. - while (!_points.empty()) { - _points.pop_front(); + if (_points.empty()) { + // With no position reports available, this function does nothing, + // except to make sure that our velocity gets reset to zero after + // a period of time. + + if (_smooth_position_known) { + double age = timestamp - _smooth_timestamp; + if (age > _reset_velocity_age) { + _smooth_forward_velocity = 0.0; + _smooth_rotational_velocity = 0.0; + } } return; } + if (_smooth_mode == SM_off) { + // With smoothing disabled, this function also does nothing, + // except to ensure that any old bogus position reports are + // cleared. + clear_positions(false); + return; + } // First, back up in time by the specified delay factor. timestamp -= _delay; @@ -144,53 +207,27 @@ compute_smooth_position(double timestamp) { nassertv(point_after != -1); // If we only have an after point, we have to start there. const SamplePoint &point = _points[point_after]; - _smooth_pos = point._pos; - _smooth_hpr = point._hpr; - _computed_smooth_mat = false; + set_smooth_pos(point._pos, point._hpr, timestamp); + _smooth_forward_velocity = 0.0; + _smooth_rotational_velocity = 0.0; return; } - if (point_after == -1) { + if (point_after == -1 || timestamp_before == timestamp_after) { // If we only have a before point, we have to stop there, unless // we have prediction in effect. const SamplePoint &point = _points[point_before]; - _smooth_pos = point._pos; - _smooth_hpr = point._hpr; - _computed_smooth_mat = false; + set_smooth_pos(point._pos, point._hpr, timestamp); + if (timestamp - point._timestamp > _reset_velocity_age) { + // Furthermore, if the before point is old enough, zero out the + // velocity. + _smooth_forward_velocity = 0.0; + _smooth_rotational_velocity = 0.0; + } } else { // If we have two points, we can linearly interpolate between them. - SamplePoint &point_b = _points[point_before]; - const SamplePoint &point_a = _points[point_after]; - - double age = (timestamp_after - timestamp_before); - if (age > _max_position_age) { - // If the first point is too old, assume there were a lot of - // implicit standing still messages that weren't sent. Reset - // the first point to be timestamp_after - max_position_age. - timestamp_before = min(timestamp, timestamp_after - _max_position_age); - point_b._timestamp = timestamp_before; - age = (timestamp_after - timestamp_before); - } - - double t = (timestamp - timestamp_before) / age; - _smooth_pos = point_b._pos + t * (point_a._pos - point_b._pos); - - // To interpolate the hpr's, we must first make sure that both - // angles are on the same side of the discontinuity. - LVecBase3f a_hpr = point_a._hpr; - LVecBase3f b_hpr = point_b._hpr; - - for (int j = 0; j < 3; j++) { - if ((a_hpr[j] - b_hpr[j]) > 180.0) { - a_hpr[j] -= 360.0; - } else if ((a_hpr[j] - b_hpr[j]) < -180.0) { - a_hpr[j] += 360.0; - } - } - - _smooth_hpr = b_hpr + t * (a_hpr - b_hpr); - _computed_smooth_mat = false; + linear_interpolate(point_before, point_after, timestamp); } // Assume we'll never get another compute_smooth_position() request @@ -198,6 +235,10 @@ compute_smooth_position(double timestamp) { // head of the queue before point_before. while (!_points.empty() && _points.front()._timestamp < timestamp_before) { _points.pop_front(); + + // This invalidates the index numbers. + _last_point_before = -1; + _last_point_after = -1; } } @@ -227,6 +268,21 @@ write(ostream &out) const { } } +//////////////////////////////////////////////////////////////////// +// Function: SmoothMover::set_smooth_pos +// Access: Private +// Description: Sets the computed smooth position and orientation for +// the indicated timestamp. +//////////////////////////////////////////////////////////////////// +void SmoothMover:: +set_smooth_pos(const LPoint3f &pos, const LVecBase3f &hpr, + double timestamp) { + _smooth_pos = pos; + _smooth_hpr = hpr; + _smooth_timestamp = timestamp; + _smooth_position_known = true; + _computed_smooth_mat = false; +} //////////////////////////////////////////////////////////////////// // Function: SmoothMover::compose_smooth_mat @@ -239,3 +295,81 @@ compose_smooth_mat() { compose_matrix(_smooth_mat, _scale, _smooth_hpr, _smooth_pos); _computed_smooth_mat = true; } + +//////////////////////////////////////////////////////////////////// +// Function: SmoothMover::linear_interpolate +// Access: Private +// Description: Interpolates the smooth position linearly between the +// two bracketing position reports. +//////////////////////////////////////////////////////////////////// +void SmoothMover:: +linear_interpolate(int point_before, int point_after, double timestamp) { + SamplePoint &point_b = _points[point_before]; + const SamplePoint &point_a = _points[point_after]; + + double age = (point_a._timestamp - point_b._timestamp); + + if (point_before == _last_point_before && + point_after == _last_point_after) { + // If these are the same two points we found last time (which is + // likely), we can save a bit of work. + double t = (timestamp - point_b._timestamp) / age; + set_smooth_pos(point_b._pos + t * (point_a._pos - point_b._pos), + point_b._hpr + t * (point_a._hpr - point_b._hpr), + timestamp); + + // The velocity remains the same as last time. + + } else { + _last_point_before = point_before; + _last_point_after = point_after; + + if (age > _max_position_age) { + // If the first point is too old, assume there were a lot of + // implicit standing still messages that weren't sent. Reset + // the first point's timestamp to reflect this. + point_b._timestamp = min(timestamp, point_a._timestamp - _max_position_age); + age = (point_a._timestamp - point_b._timestamp); + } + + // To interpolate the hpr's, we must first make sure that both + // angles are on the same side of the discontinuity. + for (int j = 0; j < 3; j++) { + if ((point_b._hpr[j] - point_a._hpr[j]) > 180.0) { + point_b._hpr[j] -= 360.0; + } else if ((point_b._hpr[j] - point_a._hpr[j]) < -180.0) { + point_b._hpr[j] += 360.0; + } + } + + double t = (timestamp - point_b._timestamp) / age; + LVector3f pos_delta = point_a._pos - point_b._pos; + LVecBase3f hpr_delta = point_a._hpr - point_b._hpr; + + set_smooth_pos(point_b._pos + t * pos_delta, + point_b._hpr + t * hpr_delta, + timestamp); + compute_velocity(pos_delta, hpr_delta, age); + } +} + +//////////////////////////////////////////////////////////////////// +// Function: SmoothMover::compute_velocity +// Access: Private +// Description: Computes the forward and rotational velocities of the +// moving object. +//////////////////////////////////////////////////////////////////// +void SmoothMover:: +compute_velocity(const LVector3f &pos_delta, const LVecBase3f &hpr_delta, + double age) { + // Also compute the velocity. To get just the forward component + // of velocity, we need to project the velocity vector onto the y + // axis, as rotated by the current hpr. + LMatrix3f rot_mat; + compose_matrix(rot_mat, LVecBase3f(1.0, 1.0, 1.0), _smooth_hpr); + LVector3f y_axis = LVector3f(0.0, 1.0, 0.0) * rot_mat; + float forward_distance = pos_delta.dot(y_axis); + + _smooth_forward_velocity = forward_distance / age; + _smooth_rotational_velocity = hpr_delta[0] / age; +} diff --git a/direct/src/deadrec/smoothMover.h b/direct/src/deadrec/smoothMover.h index 953224361c..f9a2accaa6 100644 --- a/direct/src/deadrec/smoothMover.h +++ b/direct/src/deadrec/smoothMover.h @@ -76,6 +76,7 @@ PUBLISHED: INLINE void set_timestamp(double timestamp); void mark_position(); + void clear_positions(bool reset_velocity); INLINE void compute_smooth_position(); void compute_smooth_position(double timestamp); @@ -84,6 +85,10 @@ PUBLISHED: INLINE const LVecBase3f &get_smooth_hpr() const; INLINE const LMatrix4f &get_smooth_mat(); + INLINE float get_smooth_forward_velocity() const; + INLINE float get_smooth_rotational_velocity() const; + + // These static methods control the global properties of all // SmoothMovers. enum SmoothMode { @@ -107,11 +112,20 @@ PUBLISHED: INLINE static void set_max_position_age(double age); INLINE static double get_max_position_age(); + INLINE static void set_reset_velocity_age(double age); + INLINE static double get_reset_velocity_age(); + void output(ostream &out) const; void write(ostream &out) const; private: + void set_smooth_pos(const LPoint3f &pos, const LVecBase3f &hpr, + double timestamp); void compose_smooth_mat(); + void linear_interpolate(int point_before, int point_after, double timestamp); + void compute_velocity(const LVector3f &pos_delta, + const LVecBase3f &hpr_delta, + double age); enum Flags { F_got_timestamp = 0x0001, @@ -131,16 +145,23 @@ private: LPoint3f _smooth_pos; LVecBase3f _smooth_hpr; LMatrix4f _smooth_mat; + double _smooth_timestamp; + bool _smooth_position_known; bool _computed_smooth_mat; - + + double _smooth_forward_velocity; + double _smooth_rotational_velocity; typedef CircBuffer Points; Points _points; + int _last_point_before; + int _last_point_after; static SmoothMode _smooth_mode; static PredictionMode _prediction_mode; static double _delay; static double _max_position_age; + static double _reset_velocity_age; }; #include "smoothMover.I"