[ create a new paste ] login | about

Link: http://codepad.org/PWNm2NAE    [ raw code | output | fork ]

de666 - C++, pasted on Mar 18:
#include <iostream>
#include <vector>
#include <cassert>

using std::vector;

struct point
{
  double x, y;
};

inline double sqr(double a) { return a*a; }
inline point operator-(const point &a, const point &b)
{
  point c = {a.x - b.x, a.y - b.y};
  return c;
}
inline double sqrlen(const point &a) { return sqr(a.x) + sqr(a.y); }

double sqr_distance(const vector<point> &points, const point &q)
{
  const size_t count = points.size();
  assert(count>0);
  point
      b = points[0],
      dbq = b - q;
  double dist = sqrlen(dbq);
  for (size_t i = 1; i<count; ++i)
  {
    const point
      a = b,
      daq = dbq;
    b = points[i];
    dbq = b - q;
    const point dab = a - b;
    const double
      inv_sqrlen = 1./sqrlen(dab),
      t = (dab.x*daq.x + dab.y*daq.y)*inv_sqrlen;
    if (t<0.)
      continue;
    double current_dist;
    if (t<=1.)
      current_dist = sqr(dab.x*dbq.y - dab.y*dbq.x)*inv_sqrlen;
    else//t>1.
      current_dist = sqrlen(dbq);
    if (current_dist<dist)
        dist = current_dist;
  }
  return dist;
}

inline double randf(double rand_max = 1.) { return double(rand())/RAND_MAX; }

inline std::ostream &operator<<(std::ostream &s, const point &p)
{ return s << '{' << p.x << ';' << p.y << '}'; }

int main(int argc, const char *argv[])
{
  if (false)
  {//test
    const point pts[] = {{0., 0.}, {1., 1.}, {2., 0.}};
    vector<point> points(3);
    points[0] = pts[0];
    points[1] = pts[1];
    points[2] = pts[2];
    const point
      a = {-1., 1.},
      b = {0., 1.},
      c = {.5, 0.},
      d = {.5, 1.},
      e = {1., 0.};
    std::cout << a << ": " << sqr_distance(points, a) << '\n'
      << b << ": " << sqr_distance(points, b) << '\n'
      << c << ": " << sqr_distance(points, c) << '\n'
      << d << ": " << sqr_distance(points, d) << '\n'
      << e << ": " << sqr_distance(points, e) << '\n'
      <<std::endl;
  }
  const size_t n = argc>1 ? atoi(argv[1]) : 1000;
  vector<point> points(n);
  for (size_t i = 0; i<n; ++i)
  {
    const point p = {randf(), randf()};
    points[i] = p;
  }
  const point q = {0., 0.};
  const double sqr_dist = sqr_distance(points, q);
  std::cout << "distance from {" << q.x << ';' << q.y << "} to a " << n << " point polyline is "
    << sqrt(sqr_dist) << std::endl;
  return 0;
}


Output:
1
distance from {0;0} to a 1000 point polyline is 0.00581902


Create a new paste based on this one


Comments: