mirror of
https://github.com/panda3d/panda3d.git
synced 2025-10-04 10:54:24 -04:00
376 lines
13 KiB
C++
376 lines
13 KiB
C++
// Filename: smoothMover.cxx
|
|
// Created by: drose (19Oct01)
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
//
|
|
// PANDA 3D SOFTWARE
|
|
// Copyright (c) 2001, Disney Enterprises, Inc. All rights reserved
|
|
//
|
|
// All use of this software is subject to the terms of the Panda 3d
|
|
// Software license. You should have received a copy of this license
|
|
// along with this source code; you will also find a current copy of
|
|
// the license at http://www.panda3d.org/license.txt .
|
|
//
|
|
// To contact the maintainers of this program write to
|
|
// panda3d@yahoogroups.com .
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
|
|
#include "smoothMover.h"
|
|
#include "notify.h"
|
|
#include "compose_matrix.h"
|
|
|
|
SmoothMover::SmoothMode SmoothMover::_smooth_mode = SmoothMover::SM_off;
|
|
SmoothMover::PredictionMode SmoothMover::_prediction_mode = SmoothMover::PM_off;
|
|
double SmoothMover::_delay = 0.2;
|
|
double SmoothMover::_max_position_age = 0.25;
|
|
double SmoothMover::_reset_velocity_age = 0.3;
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: SmoothMover::Constructor
|
|
// Access: Published
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
SmoothMover::
|
|
SmoothMover() {
|
|
_scale.set(1.0, 1.0, 1.0);
|
|
_sample._pos.set(0.0, 0.0, 0.0);
|
|
_sample._hpr.set(0.0, 0.0, 0.0);
|
|
_sample._timestamp = 0.0;
|
|
_sample._flags = 0;
|
|
|
|
_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;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: SmoothMover::Destructor
|
|
// Access: Published
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
SmoothMover::
|
|
~SmoothMover() {
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: SmoothMover::mark_position
|
|
// Access: Published
|
|
// Description: Stores the position, orientation, and timestamp (if
|
|
// relevant) indicated by previous calls to set_pos(),
|
|
// set_hpr(), and set_timestamp() in a new position
|
|
// report.
|
|
//
|
|
// When compute_smooth_position() is called, it uses
|
|
// these stored position reports to base its computation
|
|
// of the known position.
|
|
////////////////////////////////////////////////////////////////////
|
|
void SmoothMover::
|
|
mark_position() {
|
|
if (_smooth_mode == SM_off) {
|
|
// With smoothing disabled, mark_position() simply stores its
|
|
// current position in the smooth_position members.
|
|
|
|
// 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
|
|
// report.
|
|
|
|
if (_points.full()) {
|
|
// 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);
|
|
_sample._flags = 0;
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// 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
|
|
// Description: Computes the smoothed position (and orientation) of
|
|
// the mover at the indicated point in time, based on
|
|
// the previous position reports. After this call has
|
|
// been made, get_smooth_pos() etc. may be called to
|
|
// retrieve the smoothed position.
|
|
////////////////////////////////////////////////////////////////////
|
|
void SmoothMover::
|
|
compute_smooth_position(double timestamp) {
|
|
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;
|
|
|
|
// Now look for the two bracketing position reports.
|
|
int point_before = -1;
|
|
double timestamp_before = 0.0;
|
|
int point_after = -1;
|
|
double timestamp_after = 0.0;
|
|
|
|
int num_points = _points.size();
|
|
for (int i = 0; i < num_points; i++) {
|
|
const SamplePoint &point = _points[i];
|
|
if (point._timestamp <= timestamp) {
|
|
// This point is before the desired time. Find the newest of
|
|
// these.
|
|
if (point_before == -1 || point._timestamp > timestamp_before) {
|
|
point_before = i;
|
|
timestamp_before = point._timestamp;
|
|
}
|
|
}
|
|
if (point._timestamp >= timestamp) {
|
|
// This point is after the desired time. Find the oldest of
|
|
// these.
|
|
if (point_after == -1 || point._timestamp < timestamp_after) {
|
|
point_after = i;
|
|
timestamp_after = point._timestamp;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (point_before == -1) {
|
|
nassertv(point_after != -1);
|
|
// If we only have an after point, we have to start there.
|
|
const SamplePoint &point = _points[point_after];
|
|
set_smooth_pos(point._pos, point._hpr, timestamp);
|
|
_smooth_forward_velocity = 0.0;
|
|
_smooth_rotational_velocity = 0.0;
|
|
return;
|
|
}
|
|
|
|
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];
|
|
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.
|
|
linear_interpolate(point_before, point_after, timestamp);
|
|
}
|
|
|
|
// Assume we'll never get another compute_smooth_position() request
|
|
// for an older time than this, and remove all the timestamps at the
|
|
// 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;
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: SmoothMover::output
|
|
// Access: Published
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
void SmoothMover::
|
|
output(ostream &out) const {
|
|
out << "SmoothMover, " << _points.size() << " sample points.";
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: SmoothMover::write
|
|
// Access: Published
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
void SmoothMover::
|
|
write(ostream &out) const {
|
|
out << "SmoothMover, " << _points.size() << " sample points:\n";
|
|
int num_points = _points.size();
|
|
for (int i = 0; i < num_points; i++) {
|
|
const SamplePoint &point = _points[i];
|
|
out << " " << i << ". time = " << point._timestamp << " pos = "
|
|
<< point._pos << " hpr = " << point._hpr << "\n";
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// 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
|
|
// Access: Private
|
|
// Description: Computes the smooth_mat based on smooth_pos and
|
|
// smooth_hpr.
|
|
////////////////////////////////////////////////////////////////////
|
|
void SmoothMover::
|
|
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;
|
|
}
|