96 lines
2.2 KiB
C++
96 lines
2.2 KiB
C++
|
|
#include <math.h> /* acos(), M_PI */
|
|
|
|
#include "Polygon.h"
|
|
|
|
#define FP_EQUAL(x,y) (fabs((x)-(y)) < 1E-3)
|
|
|
|
/*
|
|
* from http://local.wasp.uwa.edu.au/~pbourke/geometry/insidepoly/
|
|
* Return the angle between two vectors on a plane
|
|
* The angle is from vector 1 to vector 2, positive anticlockwise
|
|
* The result is between -pi -> pi
|
|
*/
|
|
static double angle2D(double x1, double y1, double x2, double y2)
|
|
{
|
|
double dtheta, theta1, theta2;
|
|
|
|
theta1 = atan2(y1,x1);
|
|
theta2 = atan2(y2,x2);
|
|
dtheta = theta2 - theta1;
|
|
while (dtheta > M_PI)
|
|
dtheta -= 2.0 * M_PI;
|
|
while (dtheta < -M_PI)
|
|
dtheta += 2.0 * M_PI;
|
|
|
|
return dtheta;
|
|
}
|
|
|
|
static bool similarPoint(const Vector & v, const Vector & w)
|
|
{
|
|
return FP_EQUAL(v[0], w[0]) && FP_EQUAL(v[1], w[1]) && FP_EQUAL(v[2], w[2]);
|
|
}
|
|
|
|
Polygon & Polygon::add(const Vector & v)
|
|
{
|
|
int sz = size();
|
|
if (sz < 1 || !similarPoint(v, *(*this)[sz-1]))
|
|
{
|
|
push_back(new Vector(v));
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
Polygon & Polygon::add(refptr<Vector> v)
|
|
{
|
|
int sz = size();
|
|
if (sz < 1 || !similarPoint(*v, *(*this)[sz-1]))
|
|
{
|
|
push_back(v);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
bool Polygon::containsPoint2D(const Vector & v)
|
|
{
|
|
if (size() < 3)
|
|
{
|
|
return false;
|
|
}
|
|
if (similarPoint(*(*this)[0], *(*this)[size()-1]))
|
|
{
|
|
pop_back();
|
|
}
|
|
double angle_sum = 0.0;
|
|
for (int i = 0, sz = size(); i < sz; i++)
|
|
{
|
|
Vector v1 = *(*this)[i] - v;
|
|
Vector v2 = *(*this)[(i+1) % sz] - v;
|
|
double angle = angle2D(v1[0], v1[1], v2[0], v2[1]);
|
|
angle_sum += angle;
|
|
}
|
|
return FP_EQUAL(angle_sum, 2.0 * M_PI);
|
|
}
|
|
|
|
bool Polygon::containsPointConvex(const Vector & v)
|
|
{
|
|
if (size() < 3)
|
|
{
|
|
return false;
|
|
}
|
|
if (similarPoint(*(*this)[0], *(*this)[size()-1]))
|
|
{
|
|
pop_back();
|
|
}
|
|
double angle_sum = 0.0;
|
|
for (int i = 0, sz = size(); i < sz; i++)
|
|
{
|
|
Vector v1 = *(*this)[i] - v;
|
|
Vector v2 = *(*this)[(i+1) % sz] - v;
|
|
double cosine = (v1 % v2) / (v1.mag() * v2.mag());
|
|
double angle = acos(cosine);
|
|
angle_sum += angle;
|
|
}
|
|
return FP_EQUAL(angle_sum, 2.0 * M_PI);
|
|
}
|