计算几何源码

本人收集的一些计算几何源码，包括判断点是否在多边形内，计算多边形面积、方向，直线、线段相交的源码。

bool collinear(double x1, double y1, double x2, double y2, double x3, double y3, double epsilon)
{
  return (fabs((x2 - x1) * (y3 - y1)  - (x3 - x1) * (y2 - y1)) <= epsilon);
}

 

bool point_in_polygon(double px, double py, const polygon<double, 2> &polygon)
{
 bool result = false;
 if (polygon.size() < 3)
  return false;
 std::size_t j = polygon.size() - 1;
 for(std::size_t i = 0; i < polygon.size(); ++i)
 {
  if (((polygon[i].y <= py) && (py < polygon[j].y)) || ((polygon[j].y <= py) && (py < polygon[i].y)))
  {
   if (px - polygon[i].x < ((polygon[j].x - polygon[i].x) * (py - polygon[i].y) / (polygon[j].y - polygon[i].y)))
   {
    result = !result;
   }
  }
  j = i;
 }
 return result;
}

 

int polygon_orientation(LPPOINT pp, int n)
{
 double t = 0;
 for(int i = 1; i <= n-1; i++)
  t += pp[i-1].x*pp[i].y - pp[i].x*pp[i-1].y;
 t += pp[n-1].x*pp[0].y - pp[0].x*pp[n-1].y;

 return t < 0 ? CounterClockwise : Clockwise;
}

 

int polygon_orientation(const polygon<T, 2> &polygon)
{
 if(polygon.size() < 3)
 {
  return 0;
 }

 double area = double(0.0);
 std::size_t prev_index = polygon.size() - 1;

 for(std::size_t index = 0; index < polygon.size(); ++index)
 {
  area += (polygon[prev_index].x * polygon[index].y - polygon[index].x * polygon[prev_index].y);
  prev_index = index;
 }

 return (((area >= double(0.0))) ? CounterClockwise : Clockwise);
}

 

double area(const polygon<double,2>& polygon)
{
 if(polygon.size() < 3)
  return double(0.0);
 double result = double(0.0);
 std::size_t j = polygon.size() - 1;
 for(std::size_t i = 0; i < polygon.size(); ++i)
 {
  result += ((polygon[j].x * polygon[i].y) - (polygon[j].y * polygon[i].x));
  j = i;
 }
 return fabs(result * double(0.5));
}

 

void intersect_line_to_line(double x3, double y3, double x4, double y4, double x1, double y1, double x2, double y2, double &ix, double &iy)
{
 double dx1 = x2 - x1;
 double dx2 = x4 - x3;
 double dx3 = x1 - x3;

 double dy1 = y2 - y1;
 double dy2 = y1 - y3;
 double dy3 = y4 - y3;

 double ratio = dx1 * dy3 - dy1 * dx2;

 if (ratio != 0.0)
 {
  ratio = (dy2 * dx2 - dx3 * dy3) / ratio;
  ix    = x1 + ratio * dx1;
  iy    = y1 + ratio * dy1;
 }
 else
 {
  if ((dx1 * -dy2) == (-dx3 * dy1))
  {
   ix = x3;
   iy = y3;
  }
  else
  {
   ix = x4;
   iy = y4;
  }
 }
}

 

bool intersect_segment_to_segment(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, double &ix, double &iy)
{
 double ax = x2 - x1;
 double bx = x3 - x4;

 double lowerx;
 double upperx;
 double uppery;
 double lowery;

 if (ax < double(0.0))
 {
  lowerx = x2;
  upperx = x1;
 }
 else
 {
  upperx = x2;
  lowerx = x1;
 }

 if (bx > double(0.0))
 {
  if ((upperx < x4) || (x3 < lowerx))
   return false;
 }
 else if ((upperx < x3) || (x4 < lowerx))
  return false;

 double ay = y2 - y1;
 double by = y3 - y4;

 if (ay < double(0.0))
 {
  lowery = y2;
  uppery = y1;
 }
 else
 {
  uppery = y2;
  lowery = y1;
 }

 if (by > double(0.0))
 {
  if ((uppery < y4) || (y3 < lowery))
   return false;
 }
 else if ((uppery < y3) || (y4 < lowery))
  return false;

 double cx = x1 - x3;
 double cy = y1 - y3;
 double d  = (by * cx) - (bx * cy);
 double f  = (ay * bx) - (ax * by);

 if (f > double(0.0))
 {
  if ((d < double(0.0)) || (d > f))
   return false;
 }
 else if ((d > double(0.0)) || (d < f))
  return false;

 double e = (ax * cy) - (ay * cx);

 if (f > double(0.0))
 {
  if ((e < double(0.0)) || (e > f))
   return false;
 }
 else if ((e > double(0.0)) || (e < f))
  return false;

 double ratio = (ax * -by) - (ay * -bx);

 if (ratio != double(0.0))
 {
  ratio = ((cy * -bx) - (cx * -by)) / ratio;
  ix    = x1 + (ratio * ax);
  iy    = y1 + (ratio * ay);
 }
 else
 {
  if ((ax * -cy) == (-cx * ay))
  {
   ix = x3;
   iy = y3;
  }
  else
  {
   ix = x4;
   iy = y4;
  }
 }

 return true;
}