programs/UrgScanner/UrgDataDraw.cpp

Go to the documentation of this file.

#include <QMouseEvent>
#include <cmath>
#include <deque>
#include "UrgDataDraw.h"
#include "RangeSensor.h"
#include "ticks.h"
#include <cstdio>

using namespace qrk;
using namespace std;


namespace
{
  typedef vector<Point3d<int> > Points;

  typedef struct
  {
    Points points;
    qrk::Point3d<int> rotate;
    int timestamp;
    vector<long> intensity;
  } Line;

  typedef deque<Line> Lines;
}


struct UrgDataDraw::pImpl
{
  enum {
    //AliveMsec = 3000,           // [msec]
    AliveMsec = 20,             // [msec]
  };

  QColor clear_color_;
  vector<long> data_;
  vector<long> intensity_data_;
  size_t data_max_;
  long length_min_;
  long length_max_;

  Lines saved_lines_data_;
  Lines normal_lines_data_;
  Line recent_line_data_;

  QPoint last_pos_;
  int x_rot_;
  int y_rot_;
  int z_rot_;

  bool h_type_;
  bool front_only_;
  bool pre_record_;

  int magnify_;
  bool no_plot_;
  bool intensity_mode_;

  vector<Point3d<double> > color_table_;


  pImpl(void)
    : clear_color_(Qt::black), data_max_(0),
      length_min_(0), length_max_(0),
      x_rot_(0), y_rot_(0), z_rot_(0),
      h_type_(false), front_only_(false), pre_record_(false),
      magnify_(50),
      no_plot_(false), intensity_mode_(false)
  {
    // 色変換テーブルの初期化
    for (int mm_height = 0; mm_height < 1000; ++mm_height) {
      QColor color;
      color.setHsv(static_cast<int>(360.0 * mm_height / 1000.0), 255, 255);

      int r, g, b;
      color.getRgb(&r, &g, &b);
      color_table_.push_back(Point3d<double>(r / 255.0, g / 255.0, b / 255.0));
    }
  }


  void initializeForm(UrgDataDraw* parent)
  {
    static_cast<void>(parent);
    parent->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
  }


  void initializeGL(UrgDataDraw* parent)
  {
    parent->qglClearColor(clear_color_);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    glEnable(GL_TEXTURE_2D);
  }


  void paintGL(UrgDataDraw* parent)
  {
    parent->qglClearColor(clear_color_);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glLoadIdentity();

    glRotated(x_rot_ / 16.0, 1.0, 0.0, 0.0);
    glRotated(y_rot_ / 16.0, 0.0, 1.0, 0.0);
    glRotated((z_rot_ / 16.0) + 90, 0.0, 0.0, 1.0);

    // 古いデータの削除
    while ((! normal_lines_data_.empty()) &&
           ((normal_lines_data_.front().timestamp + AliveMsec) < ticks())) {
      normal_lines_data_.pop_front();
    }

    glBegin(GL_POINTS);

    // 描画の拡大率
    double ratio = (1.0 / 2.0) + (5.0 * magnify_ / 100.0);

    // 通常データ
    if (! no_plot_) {
      for (Lines::iterator line_it = normal_lines_data_.begin();
           line_it != normal_lines_data_.end(); ++line_it) {
        // !!! false をマクロに置き換える
        drawLine(line_it, false, ratio);
      }
    }

    // 記録データ
    for (Lines::iterator line_it = saved_lines_data_.begin();
         line_it != saved_lines_data_.end(); ++line_it) {
      drawLine(line_it, true, ratio);
    }

    if (! no_plot_) {
      // 最新データの点に対して、レーザを描画する
      drawLaser(recent_line_data_, ratio);
    }
    glEnd();
  }

  void drawLaser(Line& line, double ratio)
  {
    glColor3d(0.6, 0.0, 0.0);

    int index = 0;
    for (Points::iterator it = line.points.begin();
         it != line.points.end(); ++it, ++index) {

      if ((it->x == 0) && (it->y == 0) && (it->z == 0)) {
        continue;
      }

      if ((index & 0x3) == 0x00) {
        glBegin(GL_LINE_STRIP);
        glVertex3d(0.0, 0.0, 0.0);
        glVertex3d(it->x * ratio, it->y * ratio, it->z * ratio);
        glEnd();
      }
    }
  }


  void drawLine(Lines::iterator line, bool record, double ratio)
  {
    if (! record) {
      // 時間に応じた色分け
      double gradation =
        1.0 * (AliveMsec - (ticks() - line->timestamp)) / AliveMsec;
      glColor3d(1.0 * gradation, 1.0 * gradation, 1.0 * gradation);

    } else {
      // 通常時の色
      glColor3d(0.0, 1.0, 0.0);
    }

    int index = 0;
    for (Points::iterator it = line->points.begin();
         it != line->points.end(); ++it, ++index) {

      if (record) {
        if (intensity_mode_) {
          // 強度情報に応じた色分け
          //int ratio = static_cast<int>((line->intensity[index] % 4000) / 4.0);
          int ratio = (line->intensity[index] + 1000) % 1000;
          Point3d<double>& color = color_table_[ratio];
          glColor3d(color.x, color.y, color.z);

        } else {

          // 記録中は、奥行きによって色を変える
          int mm = ((it->x % 1000) + 1000) % 1000;
          Point3d<double>& color = color_table_[mm];
          glColor3d(color.x, color.y, color.z);
        }
      }
      glVertex3d(it->x * ratio, it->y * ratio, it->z * ratio);
    }
  }


  void convertScanData(Line& line, RangeSensor& sensor)
  {
    data_max_ = sensor.maxScanLines();
    length_min_ = sensor.minDistance();
    length_max_ = sensor.maxDistance();

    line.timestamp = ticks();
    int n = sensor.capture(data_, NULL);
    if (n <= 0) {
      return;
    }

    // 強度データの取得
    int intensity_n = sensor.captureWithIntensity(data_, intensity_data_,
                                                  NULL);
    if (intensity_n == 0) {
      // 強度データが返されないならば、モードを無効とみなす
      intensity_mode_ = false;
    }

    // 測距データを２次元展開してから、Wii の向きに応じてさらに回転させる
    for (int i = 0; i < n; ++i) {
      long length = data_[i];
      if ((length <= length_min_) || (length >= length_max_)) {
        continue;
      }

      int index = i;
      double radian = sensor.index2rad(index);
      if (front_only_ && (fabs(radian) > M_PI / 2.0)) {
        // 前面以外のデータは無視する
        continue;
      }

      Point3d<int> p;
      if (! h_type_) {
        p.x = static_cast<int>(length * cos(radian));
        p.y = static_cast<int>(length * sin(radian));
        p.z = 0;
      } else {
        p.x = 0;
        p.y = -static_cast<int>(length * cos(radian));
        p.z = -static_cast<int>(length * sin(radian));
      }

      if (h_type_) {
        adjustTypeH(p, static_cast<int>(180.0 * radian / M_PI));
      }

      // wiimote の角度に基づいた回転演算を行う
      rotateX(p, line.rotate.x);
      rotateY(p, line.rotate.y);
      rotateY(p, line.rotate.z);

      line.points.push_back(p);
      if (intensity_n > index) {
        line.intensity.push_back(intensity_data_[index]);
      }
    }
  }


  void adjustTypeH(Point3d<int>& p, int degree)
  {
    if ((degree > -20) && (degree < 20)) {
      // 前方は、Z 軸に対して、90 度ほど回転
      //rotateZ(p, +90);
      // !!! なぜか、-90 度にすると、適切に動作する？
      rotateZ(p, -90);

      // さらに、Y 軸の負方向に移動して、X 軸の負方向に移動させる
      // !!!

      //p = Point3d<int>(0, 0, 0);

    } else if (degree < -45) {
      // 右側は、Y 軸に対して、+60 度ほど回転
      rotateY(p, +(60 + 15 + 0));

      // さらに、Z 軸の正方向に移動して、X 軸の負方向に移動させる(誤差無視)
      // !!!

    } else if (degree > +45) {
      // 左側は、Y 軸に対して、-60 度ほど回転
      rotateY(p, -(60 + 15 + 0));

      // さらに、Z 軸の負方向に移動して、X 軸の負方向に移動させる(誤差無視)
      // !!!
    } else {
      // それ以外は、今回は利用しない
      p = Point3d<int>(0, 0, 0);
    }
  }


  void rotateX(Point3d<int>& point, int rotate_degree)
  {
    double radian = rotate_degree * M_PI / 180.0;
    double z2 = (point.z * cos(-radian)) - (point.y * sin(-radian));
    double y2 = (point.z * sin(-radian)) + (point.y * cos(-radian));

    point.z = static_cast<int>(z2);
    point.y = static_cast<int>(y2);
  }


  void rotateY(Point3d<int>& point, int rotate_degree)
  {
    double radian = -rotate_degree * M_PI / 180.0;
    double z2 = (point.z * cos(-radian)) - (point.x * sin(-radian));
    double x2 = (point.z * sin(-radian)) + (point.x * cos(-radian));

    point.z = static_cast<int>(z2);
    point.x = static_cast<int>(x2);
  }


  void rotateZ(Point3d<int>& point, int rotate_degree)
  {
    double radian = rotate_degree * M_PI / 180.0;
    double x2 = (point.x * cos(-radian)) - (point.y * sin(-radian));
    double y2 = (point.x * sin(-radian)) + (point.y * cos(-radian));

    point.x = static_cast<int>(x2);
    point.y = static_cast<int>(y2);
  }


  void addSaveLine(Line& line)
  {
    saved_lines_data_.push_back(line);
  }


  void addTemporaryLine(Line& line)
  {
    normal_lines_data_.push_back(line);
  }


  void setXRotation(UrgDataDraw* parent, int angle)
  {
    normalizeAngle(&angle);
    if (angle != x_rot_) {
      x_rot_ = angle;
      parent->updateGL();
    }
  }


  void setYRotation(UrgDataDraw* parent, int angle)
  {
    normalizeAngle(&angle);
    if (angle != y_rot_) {
      y_rot_ = angle;
      parent->updateGL();
    }
  }


  void setZRotation(UrgDataDraw* parent, int angle)
  {
    normalizeAngle(&angle);
    if (angle != z_rot_) {
      z_rot_ = angle;
      parent->updateGL();
    }
  }


  void normalizeAngle(int *angle)
  {
    while (*angle < 0) {
      *angle += 360 * 16;
    }

    while (*angle > 360 * 16) {
      *angle -= 360 * 16;
    }
  }
};


UrgDataDraw::UrgDataDraw(QWidget* parent)
  : QGLWidget(parent), pimpl(new pImpl)
{
  pimpl->initializeForm(this);
}


UrgDataDraw::~UrgDataDraw(void)
{
}


void UrgDataDraw::initializeGL(void)
{
  pimpl->initializeGL(this);
}


void UrgDataDraw::resizeGL(int width, int height)
{
  glViewport(0, 0, width, height);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  double aspect = 1.0 * width / height;
  glOrtho(-5000 * aspect, 5000 * aspect, -5000, 5000, -100000, 100000);

  glMatrixMode(GL_MODELVIEW);
}


void UrgDataDraw::paintGL(void)
{
  pimpl->paintGL(this);
}


QSize UrgDataDraw::minimumSizeHint(void) const
{
  return QSize(150, 150);
}


void UrgDataDraw::redraw(RangeSensor& sensor,
                         Point3d<int>& wii_rotate, bool record, bool no_plot)
{
  //fprintf(stderr, "redraw.\n");

  // 取得中のデータを表示するか、の設定を更新する
  pimpl->no_plot_ = no_plot;

  // 距離データの取得と３次元変換
  Line line;
  line.rotate = wii_rotate;
  pimpl->convertScanData(line, sensor);

  if (record) {
    if (pimpl->pre_record_ == false) {
      // 新しい記録が開始されたら、前回の記録データを削除
      pimpl->saved_lines_data_.clear();
    }
    // 描画データとして登録
    pimpl->addSaveLine(line);

  } else {
    // 一時データとして登録
    pimpl->addTemporaryLine(line);
  }

  // 最新データをレーザ表示用に登録
  if (! line.points.empty()) {
    swap(pimpl->recent_line_data_, line);
  }

  pimpl->pre_record_ = record;

  updateGL();
}


void UrgDataDraw::clearCaptureData(void)
{
  pimpl->normal_lines_data_.clear();
  pimpl->recent_line_data_.points.clear();
  update();
}


void UrgDataDraw::mousePressEvent(QMouseEvent *event)
{
  pimpl->last_pos_ = event->pos();
}


void UrgDataDraw::mouseMoveEvent(QMouseEvent *event)
{
  int dx = event->x() - pimpl->last_pos_.x();
  int dy = event->y() - pimpl->last_pos_.y();

  if (event->buttons() & Qt::LeftButton) {
    pimpl->setXRotation(this, pimpl->x_rot_ + 8 * dy);
    pimpl->setZRotation(this, pimpl->z_rot_ + 8 * dx);

  } else if (event->buttons() & Qt::RightButton) {
    pimpl->setXRotation(this, pimpl->x_rot_ + 8 * dy);
    pimpl->setYRotation(this, pimpl->y_rot_ + 8 * dx);
  }
  pimpl->last_pos_ = event->pos();
}


void UrgDataDraw::setTypeH(bool checked)
{
  pimpl->h_type_ = checked;
}


void UrgDataDraw::setFrontOnly(bool front_only)
{
  pimpl->front_only_ = front_only;
}


void UrgDataDraw::magnifyChanged(int value)
{
  pimpl->magnify_ = value;
  update();
}


void UrgDataDraw::resetView(void)
{
  pimpl->x_rot_ = 0;
  pimpl->y_rot_ = 0;
  pimpl->z_rot_ = 0;
}


void UrgDataDraw::loadVrml(const string& fileName)
{
  // !!! 余力あらば、Qt API で書き直す

  FILE* fd = fopen(fileName.c_str(), "r");
  if (fd == NULL) {
    return;
  }

  enum { BufferMax = 256 };
  char buffer[BufferMax];

  bool in_data = false;

  Line line_data;
  while (fgets(buffer, BufferMax, fd) != NULL) {

    if (strstr(buffer, "end") != NULL) {
      // "end" を見付けたら、おしまい
      break;
    }

    if (in_data) {
      double x, y, z;
      if (sscanf(buffer, "%lf %lf %lf", &x, &y, &z) == 3) {
        Point3d<int> point(static_cast<int>(x * 1000.0),
                          static_cast<int>(y * 1000.0),
                          static_cast<int>(z * 1000.0));
        line_data.points.push_back(point);
      }
    }

    if (strstr(buffer, "begin") != NULL) {
      // "begin" を見付けたら、データ取得モードとする
      in_data = true;
    }
  }

  pimpl->saved_lines_data_.clear();
  pimpl->saved_lines_data_.push_back(line_data);
}


void UrgDataDraw::saveVrml(const string& fileName)
{
  // !!! いずれ、Qt API で書き直す

  const char header[] =
    "#VRML V2.0 utf8\n"
    "Shape\n"
    "{\n"
    "  geometry PointSet\n"
    "  {\n";

  const char coord_header[] =
    "    coord Coordinate\n"
    "    {\n";

  const char point_header[] =
    "      point\n"
    "      [\n";

  const char point_footer[] =
    "      ]\n";

  const char coord_footer[] =
    "    }\n";

  const char color_header[] =
    "    color Color\n"
    "    {\n"
    "      color\n"
    "      [\n";

  const char color_footer[] =
    "      ]\n"
    "    }\n";

  const char footer[] =
    "  }\n"
    "}\n";

  FILE* fd = fopen(fileName.c_str(), "w");
  if (fd == NULL) {
    return;
  }

  // ヘッダ部の出力
  fprintf(fd, "%s", header);

  // 点列データの出力
  fprintf(fd, "%s", coord_header);
  fprintf(fd, "%s", point_header);
  fprintf(fd, "        # begin points.\n");
  for (Lines::iterator line_it = pimpl->saved_lines_data_.begin();
       line_it != pimpl->saved_lines_data_.end(); ++line_it) {
    for (Points::iterator it = line_it->points.begin();
         it != line_it->points.end(); ++it) {

      fprintf(fd, "        %.3f %.3f %.3f\n",
              it->x / 1000.0, it->y / 1000.0, it->z / 1000.0);
    }
  }
  fprintf(fd, "        # end points.\n");
  fprintf(fd, "%s", point_footer);
  fprintf(fd, "%s", coord_footer);

  // 色データの出力
  fprintf(fd, "%s", color_header);
  for (Lines::iterator line_it = pimpl->saved_lines_data_.begin();
       line_it != pimpl->saved_lines_data_.end(); ++line_it) {
    for (Points::iterator it = line_it->points.begin();
         it != line_it->points.end(); ++it) {

      int mm = ((it->z % 1000) + 1000) % 1000;
      Point3d<double>& color = pimpl->color_table_[mm];
      fprintf(fd, "        %f %f %f,\n", color.x, color.y, color.z);
    }
  }
  fprintf(fd, "%s", color_footer);

  // フッタ部の出力
  fprintf(fd, "%s", footer);
  fclose(fd);
}


void UrgDataDraw::setIntensityMode(bool on)
{
  pimpl->intensity_mode_ = on;
}