00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #ifndef SMOOTHMOVER_H
00016 #define SMOOTHMOVER_H
00017
00018 #include "directbase.h"
00019 #include "luse.h"
00020 #include "clockObject.h"
00021 #include "circBuffer.h"
00022 #include "nodePath.h"
00023 #include "pdeque.h"
00024
00025 static const int max_position_reports = 10;
00026 static const int max_timestamp_delays = 10;
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 class EXPCL_DIRECT SmoothMover {
00048 PUBLISHED:
00049 SmoothMover();
00050 ~SmoothMover();
00051
00052
00053
00054
00055
00056
00057 INLINE bool set_pos(const LVecBase3 &pos);
00058 INLINE bool set_pos(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z);
00059 INLINE bool set_x(PN_stdfloat x);
00060 INLINE bool set_y(PN_stdfloat y);
00061 INLINE bool set_z(PN_stdfloat z);
00062
00063 INLINE bool set_hpr(const LVecBase3 &hpr);
00064 INLINE bool set_hpr(PN_stdfloat h, PN_stdfloat p, PN_stdfloat r);
00065 INLINE bool set_h(PN_stdfloat h);
00066 INLINE bool set_p(PN_stdfloat p);
00067 INLINE bool set_r(PN_stdfloat r);
00068
00069 INLINE bool set_pos_hpr(const LVecBase3 &pos, const LVecBase3 &hpr);
00070 INLINE bool set_pos_hpr(PN_stdfloat x, PN_stdfloat y, PN_stdfloat z, PN_stdfloat h, PN_stdfloat p, PN_stdfloat r);
00071
00072 INLINE const LPoint3 &get_sample_pos() const;
00073 INLINE const LVecBase3 &get_sample_hpr() const;
00074
00075 INLINE void set_phony_timestamp(double timestamp = 0.0, bool period_adjust = false);
00076
00077 INLINE void set_timestamp(double timestamp);
00078
00079 INLINE bool has_most_recent_timestamp() const;
00080 INLINE double get_most_recent_timestamp() const;
00081
00082 void mark_position();
00083 void clear_positions(bool reset_velocity);
00084
00085 INLINE bool compute_smooth_position();
00086 bool compute_smooth_position(double timestamp);
00087 bool get_latest_position();
00088
00089 INLINE const LPoint3 &get_smooth_pos() const;
00090 INLINE const LVecBase3 &get_smooth_hpr() const;
00091
00092 INLINE void apply_smooth_pos(NodePath &node) const;
00093 INLINE void apply_smooth_pos_hpr(NodePath &pos_node, NodePath &hpr_node) const;
00094 INLINE void apply_smooth_hpr(NodePath &node) const;
00095
00096 INLINE void compute_and_apply_smooth_pos(NodePath &node);
00097 INLINE void compute_and_apply_smooth_pos_hpr(NodePath &pos_node, NodePath &hpr_node);
00098 INLINE void compute_and_apply_smooth_hpr(NodePath &hpr_node);
00099
00100 INLINE PN_stdfloat get_smooth_forward_velocity() const;
00101 INLINE PN_stdfloat get_smooth_lateral_velocity() const;
00102 INLINE PN_stdfloat get_smooth_rotational_velocity() const;
00103 INLINE const LVecBase3 &get_forward_axis() const;
00104
00105 void handle_wrt_reparent(NodePath &old_parent, NodePath &new_parent);
00106
00107 enum SmoothMode {
00108 SM_off,
00109 SM_on,
00110
00111
00112 };
00113 enum PredictionMode {
00114 PM_off,
00115 PM_on,
00116
00117
00118
00119 };
00120
00121 INLINE void set_smooth_mode(SmoothMode mode);
00122 INLINE SmoothMode get_smooth_mode();
00123
00124 INLINE void set_prediction_mode(PredictionMode mode);
00125 INLINE PredictionMode get_prediction_mode();
00126
00127 INLINE void set_delay(double delay);
00128 INLINE double get_delay();
00129
00130 INLINE void set_accept_clock_skew(bool flag);
00131 INLINE bool get_accept_clock_skew();
00132
00133 INLINE void set_max_position_age(double age);
00134 INLINE double get_max_position_age();
00135
00136 INLINE void set_expected_broadcast_period(double period);
00137 INLINE double get_expected_broadcast_period();
00138
00139 INLINE void set_reset_velocity_age(double age);
00140 INLINE double get_reset_velocity_age();
00141
00142 INLINE void set_directional_velocity(bool flag);
00143 INLINE bool get_directional_velocity();
00144
00145 INLINE void set_default_to_standing_still(bool flag);
00146 INLINE bool get_default_to_standing_still();
00147
00148 void output(ostream &out) const;
00149 void write(ostream &out) const;
00150
00151 private:
00152 void set_smooth_pos(const LPoint3 &pos, const LVecBase3 &hpr,
00153 double timestamp);
00154 void linear_interpolate(int point_before, int point_after, double timestamp);
00155 void compute_velocity(const LVector3 &pos_delta,
00156 const LVecBase3 &hpr_delta,
00157 double age);
00158
00159 void record_timestamp_delay(double timestamp);
00160 INLINE double get_avg_timestamp_delay() const;
00161
00162 public:
00163
00164
00165 class SamplePoint {
00166 public:
00167 LPoint3 _pos;
00168 LVecBase3 _hpr;
00169 double _timestamp;
00170 };
00171
00172 private:
00173 SamplePoint _sample;
00174
00175 LPoint3 _smooth_pos;
00176 LVecBase3 _smooth_hpr;
00177 LVector3 _forward_axis;
00178 double _smooth_timestamp;
00179 bool _smooth_position_known;
00180 bool _smooth_position_changed;
00181 bool _computed_forward_axis;
00182
00183 double _smooth_forward_velocity;
00184 double _smooth_lateral_velocity;
00185 double _smooth_rotational_velocity;
00186
00187 bool _has_most_recent_timestamp;
00188 double _most_recent_timestamp;
00189
00190
00191 typedef pdeque<SamplePoint> Points;
00192 Points _points;
00193 int _last_point_before;
00194 int _last_point_after;
00195
00196
00197
00198
00199
00200
00201 typedef CircBuffer<int, max_timestamp_delays> TimestampDelays;
00202 TimestampDelays _timestamp_delays;
00203 int _net_timestamp_delay;
00204 double _last_heard_from;
00205
00206 SmoothMode _smooth_mode;
00207 PredictionMode _prediction_mode;
00208 double _delay;
00209 bool _accept_clock_skew;
00210 double _max_position_age;
00211 double _expected_broadcast_period;
00212 double _reset_velocity_age;
00213 bool _directional_velocity;
00214 bool _default_to_standing_still;
00215 };
00216
00217 #include "smoothMover.I"
00218
00219 #endif