00001
00005
#include <utils/Linear.h>
00006
00007
#include "VehPosePlayer.h"
00008
#include "VehPoseSource.h"
00009
00010 bool VehPosePlayer::open(utils::ConfigFile& params,
00011 utils::SymbolTable* globals)
00012 {
00013
00014
00015
00016
if (!_mgr.open(
"VehPose.rad", params, globals))
00017
return false;
00018
00019
00020 _player = _mgr.getPlayer();
00021
00022
00023 memset(&_input_area, 0,
sizeof(_input_area));
00024
00025
00026
int major_version = _mgr.getPlayer()->getHeader().
00027 getInt(
"int DataFormat.version_major", 1);
00028
int minor_version = _mgr.getPlayer()->getHeader().
00029 getInt(
"int DataFormat.version_minor", 1);
00030
00031
00032
if (major_version != 1 && minor_version != 0) {
00033 printf(
"VehPosePlayer::init: Cannot read version %d.%d\n",
00034 major_version, minor_version);
00035
return false;
00036 }
00037
00038
00039
00040
00041 _player->expect(
"x",
"double", &_input_area.x);
00042 _player->expect(
"y",
"double", &_input_area.y);
00043 _player->expect(
"z",
"float", &_input_area.z);;
00044 _player->expect(
"ori",
"[float : 4]", &_input_area.ori);
00045
00046
00047
return _player->setup();
00048 }
00049
00050 bool VehPosePlayer::advance()
00051 {
00052
00053
00054
00055
00056
00057
return _mgr.next(_play_time);
00058 }
00059
00060
void VehPosePlayer::set_pose(
VehPose& pose)
00061 {
00062 pose.
pos = utils::Vec3d(_input_area.
x, _input_area.
y, _input_area.
z);
00063 pose.
ori = utils::Rotation(_input_area.
ori[0], _input_area.
ori[1],
00064 _input_area.
ori[2], _input_area.
ori[3]);
00065 }
00066
00067 bool VehPosePlayer::getCurPose(utils::Time& time,
VehPose& pose)
00068 {
00069 set_pose(pose);
00070
00071
00072 time = _play_time;
00073
00074
return true;
00075 }
00076
00077 bool VehPosePlayer::nextPose(utils::Time& time,
VehPose& pose,
bool blocking)
00078 {
00079
if (blocking || (!blocking && _mgr.poll())) {
00080
if (!
advance())
00081
return false;
00082 }
00083
return getCurPose(time, pose);
00084 }
00085
00086 bool VehPosePlayer::getPose(utils::Time time,
VehPose& pose)
00087 {
00088
00089 utils::Time prev_time = time;
00090
if (!_player->get(prev_time)) {
00091
return false;
00092 }
00093
VehPose prev_pose;
00094 set_pose(prev_pose);
00095
00096
00097
if (time == prev_time) {
00098 pose = prev_pose;
00099
return true;
00100 }
00101
00102
00103 utils::Time next_time;
00104
if (!_player->next() || !_player->process(next_time)) {
00105
return false;
00106 }
00107
00108
00109
VehPose next_pose;
00110 set_pose(next_pose);
00111
if (time == next_time) {
00112 pose = next_pose;
00113
return true;
00114 }
00115
00116
00117
if (prev_time == next_time) {
00118 time = utils::Time();
00119
return false;
00120 }
00121
00122
00123
double period = (next_time-prev_time).getValue();
00124
double elapsed = (time-prev_time).getValue();
00125
double t = elapsed/period;
00126
00127
VehPoseSource::interpolate(prev_pose, next_pose, t, pose);
00128
return true;
00129 }