C++ vpPoint::project方法代码示例

/*!
  Test the visibility of a line. As a result, a subsampled line of the given one with all its visible parts.

  \param a : First point of the line.
  \param b : Second point of the line.
  \param lines : List of lines corresponding of the visible parts of the given line.
  \param displayResults : True if the results have to be displayed. False otherwise.
*/
void
vpMbScanLine::queryLineVisibility(vpPoint a,
                         vpPoint b,
                         std::vector<std::pair<vpPoint, vpPoint> > &lines, const bool &displayResults)
{
  vpColVector _a, _b;
  createVectorFromPoint(a, _a, K);
  createVectorFromPoint(b, _b, K);

  double x0 = _a[0] / _a[2];
  double y0 = _a[1] / _a[2];
  double z0 = _a[2];
  double x1 = _b[0] / _b[2];
  double y1 = _b[1] / _b[2];
  double z1 = _b[2];

  vpMbScanLineEdge edge = makeMbScanLineEdge(a, b);
  lines.clear();

  if(displayResults){
#if defined(VISP_HAVE_X11) && defined(DEBUG_DISP)
    double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
    a.project();
    b.project();
    vpMeterPixelConversion::convertPoint(K,a.get_x(), a.get_y(),j1,i1);
    vpMeterPixelConversion::convertPoint(K,b.get_x(), b.get_y(),j2,i2);

    vpDisplay::displayLine(linedebugImg,i1,j1,i2,j2,vpColor::yellow,3);
#endif
  }

  if (!visibility_samples.count(edge))
      return;

  // Initialized as the biggest difference between the two points is on the X-axis
  double *v0(&x0), *w0(&z0);
  double *v1(&x1), *w1(&z1);
  unsigned int size(w);

  if (std::fabs(y0 - y1) > std::fabs(x0 - x1)) // Test if the biggest difference is on the Y-axis
  {
    v0 = &y0;
    v1 = &y1;
    size = h;
  }

  if (*v0 > *v1)
  {
      std::swap(v0, v1);
      std::swap(w0, w1);
      std::swap(a, b);
  }

  //if (*v0 >= size - 1 || *v1 < 0 || *v1 == *v0)
  if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits<double>::epsilon())
      return;

  const int _v0 = std::max(0, int(std::ceil(*v0)));
  const int _v1 = std::min<int>((int)(size - 1), (int)(std::ceil(*v1) - 1));

  const std::set<int> &visible_samples = visibility_samples[edge];
  int last = _v0;
  vpPoint line_start;
  vpPoint line_end;
  bool b_line_started = false;
  for(std::set<int>::const_iterator it = visible_samples.begin() ; it != visible_samples.end() ; ++it)
  {
      const int v = *it;
      const double alpha = getAlpha(v, (*v0) * (*w0), (*w0), (*v1) * (*w1), (*w1));
      const vpPoint p = mix(a, b, alpha);
      if (last + 1 != v)
      {
          if(b_line_started)
            lines.push_back(std::make_pair(line_start, line_end));
          b_line_started = false;
      }
      if (v == _v0)
      {
          line_start = a;
          line_end = p;
          b_line_started = true;
      }
      else if (v == _v1)
      {
          line_end = b;
          if (!b_line_started)
              line_start = p;
          b_line_started = true;
      }
      else
      {
          line_end = p;
//.........这里部分代码省略.........