pathUtils.cpp

/*
  経路追従の補助コマンド
  Satofumi KAMIMURA
  $Id$
*/

#include "pathUtils.h"
#include "timeUtils.h"


typedef struct {
  double a;                     
  double b;                     
  double c;                     
} line_t;


void VXV::waitStable(RunInterface& obj,
                     unsigned long poll_msec, unsigned long timeout_msec) {
  unsigned long total_wait = 0;

  while (!obj.isStable()) {
    VXV::Delay(poll_msec);
    total_wait += poll_msec;
    if ((timeout_msec > 0) && (poll_msec >= timeout_msec)) {
      break;
    }
  }
}


static void createLine(line_t *line, const VXV::Position& position) {
  line->a = cos(position.zt.to_rad());
  line->b = -sin(position.zt.to_rad());
  line->c = position.y * cos(position.zt.to_rad())
    - position.x * sin(position.zt.to_rad());
}


VXV::Position VXV::getNextLinePoint(const VXV::Position& base,
                                    const VXV::Position& next, int radius) {

  VXV::Direction center = next.zt - base.zt;

  line_t first, second;
  createLine(&first, base);
  createLine(&second, next);

  double t = first.b * second.a - second.b * first.a;
  if (fabs(t) < 0.00001) {
    return base;                // 平行
  }
  double x = (second.a * first.c - first.a * second.c) / t;
  double y = (second.c * first.b - first.c * second.b) / t;
  double l = fabs(radius * sin(M_PI - center.to_rad() / 2.0));

  VXV::Position change(static_cast<int>(x + l * -cos(base.zt.to_rad())),
                       static_cast<int>(y + l * -sin(base.zt.to_rad())),
                       base.zt);

  return change;
}


bool VXV::followLinesUpdate(RunCtrl& obj,
                            std::deque<VXV::Position>& lines, int radius,
                            const CoordinateCtrl* crd) {
  switch (lines.size()) {
  case 0:
    return false;
    break;

  case 1:
    if (obj.getLengthToLine(lines[0], crd) > 0) {
      lines.pop_front();
    }
    break;

  default:
    VXV::Position next = VXV::getNextLinePoint(lines[0], lines[1], radius);
    if (obj.getLengthToLine(next, crd) > 0) {
      lines.pop_front();
      obj.followLine(lines.front(), crd);
    }
    break;
  }
  return true;
}