ticksPosition.cpp

/*
  指定時刻の位置を補間して返す
  Satofumi KAMIMURA
  $Id$
*/

#include "ticksPosition.h"
#include <algorithm>

using namespace VXV;


TicksPosition::TicksPosition(void) {
}


TicksPosition::~TicksPosition(void) {
}


double TicksPosition::calcAngle(const VXV::Direction& a,
                                const VXV::Direction& b, double k) const {
  double a_rad = a.to_rad();
  double b_rad = b.to_rad();
  double diff = b_rad - a_rad;
  if (diff > M_PI) {
    diff -= 2.0 * M_PI;
  } else if (diff < -M_PI) {
    diff += 2.0 * M_PI;
  }
  return a_rad + diff * k;
}


VXV::Position3D TicksPosition::getPosition(unsigned long ticks) const {
  if (pos_list.empty()) {
    return Position3D();
  }

  // 範囲外の時刻については、一番近いときの位置を返す
  std::list<ticksPos_t>::const_iterator pre = pos_list.begin();
  if (pre->ticks > ticks) {
    return pos_list.front().pos;
  }
  for (std::list<ticksPos_t>::const_iterator it = pos_list.begin();
       it != pos_list.end(); ++it) {
    if (it->ticks == ticks) {
      // 指定時刻印と一致した位置を返す
      return it->pos;
    }
    if (it->ticks > ticks) {
      // 補間した位置を返す
      double k = (ticks - pre->ticks) / (0.0 + it->ticks - pre->ticks);
      int x = static_cast<int>(rint(pre->pos.x + (it->pos.x - pre->pos.x) *k));
      int y = static_cast<int>(rint(pre->pos.y + (it->pos.y - pre->pos.y) *k));
      int z = static_cast<int>(rint(pre->pos.z + (it->pos.z - pre->pos.z) *k));
      return Position3D(x, y, z,
                        Direction::rad(calcAngle(pre->pos.xt, it->pos.xt, k)),
                        Direction::rad(calcAngle(pre->pos.yt, it->pos.yt, k)),
                        Direction::rad(calcAngle(pre->pos.zt, it->pos.zt, k)));
    }
    pre = it;
  }
  return pos_list.back().pos;
}


void TicksPosition::add(const VXV::Position3D& position, unsigned long ticks) {

  std::list<ticksPos_t> add_list(1);
  add_list.front().pos = position;
  add_list.front().ticks = ticks;

  pos_list.merge(add_list, ticks_less());
}


void TicksPosition::clear(void) {
  pos_list.clear();
}


void TicksPosition::del_olderThan(unsigned long ticks) {

  // ticks 以降を削除
  ticksPos_t cmp;
  cmp.ticks = ticks;
  std::list<ticksPos_t>::iterator
    it = upper_bound(pos_list.begin(), pos_list.end(), cmp, ticks_less());
  if (it != pos_list.end()) {
    pos_list.erase(pos_list.begin(), it);
  }
}