Main Page | Modules | Class Hierarchy | Class List | File List | Class Members | File Members | Related Pages

VehPosePlayer.cc

Go to the documentation of this file.
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 // setup the player manager 00014 // the player manager takes care of all of the interactions between the 00015 // data file and the time source 00016 if (!_mgr.open("VehPose.rad", params, globals)) 00017 return false; 00018 00019 // get the actual canned data reader from the manager 00020 _player = _mgr.getPlayer(); 00021 00022 // clear the input area 00023 memset(&_input_area, 0, sizeof(_input_area)); 00024 00025 // check versions 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 // note, you don't have to simply reject other version, you can 00031 // try and adapt here 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 // Tell the player to expect pose. We don't store any element references 00039 // because we know these are all simple, flat structures which don't have 00040 // to be cleaned up 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 // Ready the player for action 00047 return _player->setup(); 00048 } 00049 00050 bool VehPosePlayer::advance() 00051 { 00052 // advance the file pointer, if necessary, 00053 // and cache the last read time for later 00054 // the player manager takes care of all the necessary interactions with 00055 // time, i.e., does reading advance time, or do we observe time to see 00056 // where to read. 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 // and set time from cached value 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 // get pose before target 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 // if we have an exact match, return 00097 if (time == prev_time) { 00098 pose = prev_pose; 00099 return true; 00100 } 00101 00102 // get pose after target 00103 utils::Time next_time; 00104 if (!_player->next() || !_player->process(next_time)) { 00105 return false; 00106 } 00107 00108 // if we have an exact match, return 00109 VehPose next_pose; 00110 set_pose(next_pose); 00111 if (time == next_time) { 00112 pose = next_pose; 00113 return true; 00114 } 00115 00116 // if we do not have two different elements, we cannot extrapolate 00117 if (prev_time == next_time) { 00118 time = utils::Time(); 00119 return false; 00120 } 00121 00122 // attempte to interpolate between the two poses 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 }

Generated on Tue Sep 7 20:37:49 2004 for ModUtils by doxygen 1.3.8