urgIntersection.cpp

/*
  交点を求める計算
  いずれ、置き換える

  Satofumi KAMIMURA
  $Id$
*/

#include "urgIntersection.h"

using namespace VXV;

static const double ERROR_VALUE = 10.0;

typedef CoordinateCtrl::polygon_t crd_polygon_t;

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


// ２点を通る直線(ax + by + c = 0)を作成する
static void createLine(lineParam_t *line,
                       const Grid& p0,  const Grid& p1) {
  line->a = p1.y - p0.y;
  line->b = p0.x - p1.x;
  line->c = -line->a * p0.x - line->b * p0.y;
}


// 点が直線上にあるかの判定を行う
static bool onLine(const Grid& p0, const Grid& p1, double x, double y) {
  double large[2] = { p0.x, p0.y };
  double small[2] = { p1.x, p1.y };
  double value[2] = { x, y };

  for (int i = 0; i <= 1; ++i) {
    if (large[i] < small[i]) {
      double tmp = large[i];
      large[i] = small[i];
      small[i] = tmp;
    }
    if ((value[i] < small[i]) || (value[i] > large[i])) {
      return false;
    }
  }
  return true;
}


// ２直線の交点を求め、ある点からの距離を返す
static int calcLength(const Grid& p0, const Grid& p1,
                      const Grid& p2, const Grid& p3,
                      const Grid& from) {
  lineParam_t line1, line2;
  createLine(&line1, p0, p1);
  createLine(&line2, p2, p3);

  double t = line1.b * line2.a - line2.b * line1.a;
  if (fabs(t) < 0.00001) {
    return INT_MAX;             // ２直線は平行
  }
  double x = (line1.c * line2.b - line2.c * line1.b) / t;
  double y = (line1.a * line2.c - line2.a * line1.c) / t;

  if (!onLine(p0, p1, x, y) || !onLine(p2, p3, x, y)) {
    return INT_MAX;             // データが線分上にない
  }
  return (int)sqrt((x - from.x)*(x - from.x) + (y - from.y)*(y - from.y));
}


int getLengthToEnvironment(const CoordinateCtrl::line_t& line,
                           const std::vector<crd_polygon_t>& polygons) {
  double angle = line.position.zt.to_rad();
  Grid p0(line.position.x, line.position.y);
  Grid p1(line.position.x + (int)(line.length * cos(angle)),
               line.position.y + (int)(line.length * sin(angle)));

  int length_min = line.length;
  for (std::vector<crd_polygon_t>::const_iterator lines = polygons.begin();
       lines != polygons.end(); ++lines) {
    Grid p2 = (*lines).back();
    for (unsigned int i = 0; i < lines->size(); ++i) {
      //地図上の直線を作成
      Grid p3((*lines)[i].x, (*lines)[i].y);

      // ２直線の交点を求める
      int length = 0;
      if ((length = calcLength(&p0, &p1, &p2, &p3, &p0)) >= 0) {
        // 現在までの最短距離と比較して保持
        if (length < length_min) {
          length_min = length;
        }
      }
      p2 = p3;
    }
  }

  // 誤差を乗せる
  if (length_min + ERROR_VALUE < line.length) {
    length_min
      += (int)(2.0 * ERROR_VALUE * rand()/(RAND_MAX+1.0) - ERROR_VALUE);
  }
  return length_min;
}