VehPoseSource.cc

Go to the documentation of this file.

#include <TimeSource/TimeSource.h>

#include "VehPoseSource.h"

// default implementation:  You can usually come up with a better one
bool VehPoseSource::getCurPose(utils::Time& time, VehPose& pose, bool)
{
  time = TimeSource::now();
  return getPose(time, pose);
}

void VehPoseSource::interpolate(const VehPose& prev_pose, 
                                const VehPose& next_pose, double t,
                                VehPose& sensor_pose)
{
  // use rotation's simple linear interpolation to get the proper rotation
  sensor_pose.ori = utils::Rotation::slerp(prev_pose.ori, next_pose.ori, t);

  // and just use linear interpolation on the individual axes to the position
  utils::Vec3d delta = next_pose.pos - prev_pose.pos;
  sensor_pose.pos = prev_pose.pos + delta*t;
}

---