//
|
// Created by Scheaven on 2020/5/31.
|
//
|
|
#include "geometry_util.h"
|
using namespace std;
|
|
|
SPoint::_Point() {}
|
SPoint::_Point(float x, float y)
|
{
|
this->x = x;
|
this->y = y;
|
this->z = 0;
|
}
|
|
SPoint::_Point(double a, double b, double c)
|
{
|
x = a; y = b; z = c;
|
}
|
|
float calDistance(SPoint A, SPoint B)
|
{
|
float x_d, y_d;
|
x_d = A.x - B.x;
|
y_d = A.y - B.y;
|
float len = sqrt(x_d * x_d + 4*y_d * y_d);
|
return len;
|
}
|
|
//bool Pt_in_Polygon(SPoint point, RegInfo* polygon)
|
//{
|
// int nCross = 0;
|
// for (int i = 0; i < polygon->count; ++i)
|
// {
|
// SPoint p1 = polygon[i];
|
// SPoint p2 = polygon[(i+1)%polygon->count];
|
//
|
// if ( p1.y == p2.y )
|
// continue;
|
// if ( p.y < min(p1.y, p2.y) )
|
// continue;
|
// if ( p.y >= max(p1.y, p2.y) )
|
// continue;
|
//
|
// float x = (float)(p.y - p1.y) * (float)(p2.x - p1.x) / (float)(p2.y - p1.y) + p1.x;
|
//
|
// if ( x > p.x )
|
// {
|
// nCross++;
|
// }
|
//
|
// }
|
//
|
// if ((nCross % 2) == 1)
|
// {
|
// return true;
|
// }
|
// else
|
// {
|
// return false;
|
// }
|
|
//}
|
|
|
float computeArea(const SPoint * pt,int n ) {
|
float area0 = 0.f;
|
for (int i = 0 ; i < n ; i++ )
|
{
|
int j = (i+1)%n;
|
area0 += pt[i].x * pt[j].y;
|
area0 -= pt[i].y * pt[j].x;
|
}
|
return 0.5f * fabs(area0);
|
}
|
|
struct PointAngle{
|
SPoint p;
|
float angle;
|
};
|
|
float Polygon::area() const {
|
return computeArea(pt,n);
|
}
|
|
static inline int cross(const SPoint* a,const SPoint* b) {
|
return a->x * b->y - a->y * b->x;
|
}
|
|
static inline SPoint* vsub(const SPoint* a,const SPoint* b, SPoint* res) {
|
res->x = a->x - b->x;
|
res->y = a->y - b->y;
|
return res;
|
}
|
|
|
static int line_sect(const SPoint* x0,const SPoint* x1,const SPoint* y0,const SPoint* y1, SPoint* res) {
|
SPoint dx, dy, d;
|
vsub(x1, x0, &dx);
|
vsub(y1, y0, &dy);
|
vsub(x0, y0, &d);
|
float dyx = (float)cross(&dy, &dx);
|
if (!dyx) return 0;
|
dyx = cross(&d, &dx) / dyx;
|
if (dyx <= 0 || dyx >= 1) return 0;
|
res->x = int(y0->x + dyx * dy.x);
|
res->y = int(y0->y + dyx * dy.y);
|
return 1;
|
}
|
|
static int left_of(const SPoint* a,const SPoint* b,const SPoint* c) {
|
SPoint tmp1, tmp2;
|
int x;
|
vsub(b, a, &tmp1);
|
vsub(c, b, &tmp2);
|
x = cross(&tmp1, &tmp2);
|
return x < 0 ? -1 : x > 0;
|
}
|
|
static void poly_edge_clip(const Polygon* sub,const SPoint* x0,const SPoint* x1, int left, Polygon* res) {
|
int i, side0, side1;
|
SPoint tmp;
|
const SPoint* v0 = sub->pt+ sub->n - 1;
|
const SPoint* v1;
|
res->clear();
|
|
side0 = left_of(x0, x1, v0);
|
if (side0 != -left) res->add(*v0);
|
|
for (i = 0; i < sub->n; i++) {
|
v1 = sub->pt + i;
|
side1 = left_of(x0, x1, v1);
|
if (side0 + side1 == 0 && side0)
|
/* last point and current straddle the edge */
|
if (line_sect(x0, x1, v0, v1, &tmp))
|
res->add(tmp);
|
if (i == sub->n - 1) break;
|
if (side1 != -left) res->add(*v1);
|
v0 = v1;
|
side0 = side1;
|
}
|
}
|
|
static int poly_winding(const Polygon* p) {
|
return left_of(p->pt, p->pt + 1, p->pt + 2);
|
}
|
|
|
void intersectPolygonSHPC(const Polygon * sub, const Polygon* clip, Polygon* res)
|
{
|
int i;
|
Polygon P1,P2;
|
Polygon *p1 = &P1;
|
Polygon *p2 = &P2;
|
Polygon *tmp;
|
|
int dir = poly_winding(clip);
|
poly_edge_clip(sub, clip->pt + clip->n - 1, clip->pt, dir, p2);
|
for (i = 0; i < clip->n - 1; i++) {
|
tmp = p2; p2 = p1; p1 = tmp;
|
if(p1->n == 0) {
|
p2->n = 0;
|
break;
|
}
|
poly_edge_clip(p1, clip->pt + i, clip->pt + i + 1, dir, p2);
|
}
|
res->clear();
|
for (i = 0 ; i < p2->n ; i++) res->add(p2->pt[i]);
|
}
|
|
void intersectPolygonSHPC(const Polygon & sub,const Polygon& clip,Polygon& res) {
|
intersectPolygonSHPC(&sub, &clip, &res);
|
}
|
|
Line::Line(SPoint a, SPoint b, bool _is_seg)
|
{
|
s = a; e = b; is_seg = _is_seg;
|
}
|
|
Line::Line() {}
|
|
|
bool PointCmp(const SPoint &a,const SPoint &b,const SPoint ¢er)
|
{
|
if (a.x >= 0 && b.x < 0)
|
return true;
|
if (a.x == 0 && b.x == 0)
|
return a.y > b.y;
|
int det = (a.x - center.x) * (b.y - center.y) - (b.x - center.x) * (a.y - center.y);
|
if (det < 0)
|
return true;
|
if (det > 0)
|
return false;
|
int d1 = (a.x - center.x) * (a.x - center.x) + (a.y - center.y) * (a.y - center.y);
|
int d2 = (b.x - center.x) * (b.x - center.y) + (b.y - center.y) * (b.y - center.y);
|
return d1 > d2;
|
}
|
|
SPoint ClockwiseSortPoints(std::vector<SPoint> &vPoints)
|
{
|
SPoint center;
|
double x = 0,y = 0;
|
for (int i = 0;i < vPoints.size();i++)
|
{
|
x += vPoints[i].x;
|
y += vPoints[i].y;
|
}
|
center.x = (int)x/vPoints.size();
|
center.y = (int)y/vPoints.size();
|
|
for(int i = 0;i < vPoints.size() - 1;i++)
|
{
|
for (int j = 0;j < vPoints.size() - i - 1;j++)
|
{
|
if (PointCmp(vPoints[j],vPoints[j+1],center))
|
{
|
SPoint tmp = vPoints[j];
|
vPoints[j] = vPoints[j + 1];
|
vPoints[j + 1] = tmp;
|
}
|
}
|
}
|
return center;
|
}
|
|
SPoint add(const SPoint& lhs, const SPoint& rhs)
|
{
|
SPoint res;
|
|
res.x = lhs.x + rhs.x;
|
res.y = lhs.y + rhs.y;
|
res.z = lhs.z + rhs.z;
|
|
return res;
|
}
|
SPoint sub(const SPoint& lhs, const SPoint& rhs)
|
{
|
SPoint res;
|
|
res.x = lhs.x - rhs.x;
|
res.y = lhs.y - rhs.y;
|
res.z = lhs.z - rhs.z;
|
|
return res;
|
}
|
SPoint mul(const SPoint& p, double ratio)
|
{
|
SPoint res;
|
|
res.x = p.x * ratio;
|
res.y = p.y * ratio;
|
res.z = p.z * ratio;
|
|
return res;
|
}
|
SPoint div(const SPoint& p, double ratio)
|
{
|
SPoint res;
|
|
res.x = p.x / ratio;
|
res.y = p.y / ratio;
|
res.z = p.z / ratio;
|
|
return res;
|
}
|
bool equal(const SPoint& lhs, const SPoint& rhs)
|
{
|
return(lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z);
|
}
|
|
double length(const SPoint& vec)
|
{
|
return (sqrt(pow(vec.x, 2) + pow(vec.y, 2) + pow(vec.z, 2)));
|
}
|
|
SPoint normalize(const SPoint& vec)
|
{
|
SPoint res;
|
|
res = div(vec, length(vec));
|
|
return res;
|
}
|
|
double dotMultiply(const SPoint& op, const SPoint& p1, const SPoint& p2)
|
{
|
return ((p1.x - op.x) * (p2.x - op.x) + (p1.y - op.y) * (p2.y - op.y) + (p1.z - op.z) * (p2.z - op.z));
|
}
|
double dotMultiply(const SPoint& vec1, const SPoint& vec2)
|
{
|
return(vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z);
|
}
|
|
SPoint multiply(const SPoint& op, const SPoint& p1, const SPoint& p2)
|
{
|
SPoint result;
|
|
result.x = (p1.y - op.y) * (p2.z - op.z) - (p2.y - op.y) * (p1.z - op.z);
|
result.y = (p1.z - op.z) * (p2.x - op.x) - (p2.z - op.z) * (p1.x - op.x);
|
result.z = (p1.x - op.x) * (p2.y - op.y) - (p2.x - op.x) * (p1.y - op.y);
|
|
return result;
|
}
|
|
SPoint multiply(const SPoint& vec1, const SPoint& vec2)
|
{
|
SPoint result;
|
|
result.x = vec1.y * vec2.z - vec2.y * vec1.z;
|
result.y = vec1.z * vec2.x - vec2.z * vec1.x;
|
result.z = vec1.x * vec2.y - vec2.x * vec1.y;
|
|
return result;
|
}
|
|
bool isponl(const SPoint& p, const Line& l)
|
{
|
SPoint line_vec = sub(l.e, l.s);
|
SPoint point_vec1 = sub(p, l.s);
|
SPoint point_vec2 = sub(p, l.e);
|
|
SPoint mul_vec = multiply(line_vec, point_vec1);
|
double dot = dotMultiply(point_vec1, point_vec2);
|
if (l.is_seg)
|
{
|
if (equal(p,l.s) || equal(p,l.e))
|
return true;
|
return (0.0 == length(mul_vec) && dot < 0.0);
|
|
}
|
else
|
{
|
return (0.0 == length(mul_vec));
|
}
|
}
|
|
double Cos(const SPoint& vec1, const SPoint& vec2)
|
{
|
SPoint unit_vec1 = normalize(vec1);
|
SPoint unit_vec2 = normalize(vec2);
|
|
return dotMultiply(unit_vec1, unit_vec2);
|
}
|
|
double Cos(const SPoint& op, const SPoint& p1, const SPoint& p2)
|
{
|
SPoint vec1 = sub(p1, op);
|
SPoint vec2 = sub(p2, op);
|
|
return Cos(vec1, vec2);
|
}
|
|
|
bool isSegIntersect(const Line& l1, const Line& l2, SPoint& inter_p)
|
{
|
SPoint line1 = sub(l1.e, l1.s);
|
SPoint line2 = sub(l2.e, l2.s);
|
SPoint norm1 = normalize(line1);
|
SPoint norm2 = normalize(line2);
|
if (l1.is_seg)
|
{
|
if (isponl(l1.s, l2))
|
{
|
inter_p = l1.s;
|
return true;
|
}
|
if (isponl(l1.e, l2))
|
{
|
inter_p = l1.e;
|
return true;
|
}
|
if (isponl(l2.s, l1))
|
{
|
inter_p = l2.s;
|
return true;
|
}
|
if (isponl(l2.e, l1))
|
{
|
inter_p = l2.e;
|
return true;
|
}
|
double dot1 = dotMultiply(multiply(sub(l2.s, l1.s), line1), multiply(sub(l2.e, l1.s), line1));
|
double dot2 = dotMultiply(multiply(sub(l1.s, l2.s), line2), multiply(sub(l1.e, l2.s), line2));
|
if (dot1 < 0.0 && dot2 < 0.0)
|
{
|
double t1 = length(multiply(sub(l1.s, l2.s), norm2)) / length(multiply(norm2, norm1));
|
double t2 = length(multiply(sub(l2.s, l1.s), norm1)) / length(multiply(norm1, norm2));
|
|
inter_p = add(l1.s, mul(norm1, t1));
|
return true;
|
}
|
else
|
{
|
return false;
|
}
|
|
}
|
else
|
{
|
if (Cos(line1, line2) == 1.0)
|
return false;
|
|
double t1 = length(multiply(sub(l1.s, l2.s), norm2)) / length(multiply(norm2, norm1));
|
double t2 = length(multiply(sub(l2.s, l1.s), norm1)) / length(multiply(norm1, norm2));
|
|
inter_p = add(l1.s, mul(norm1, t1));
|
return true;
|
}
|
}
|
|
// bool p_inPolygon(Point p, vector<Point> point_vec)
|
// {
|
// int nCross = 0;
|
// for (int i = 0; i < point_vec.size(); ++i)
|
// {
|
// Point p1(point_vec[i].x, point_vec[i].y);
|
// Point p2(point_vec[(i+1)%point_vec.size()].x, point_vec[(i+1)%point_vec.size()].y);
|
|
// if ( p1.y == p2.y )
|
// continue;
|
// if ( p.y < min(p1.y, p2.y) )
|
// continue;
|
// if ( p.y >= max(p1.y, p2.y) )
|
// continue;
|
|
// float x = (float)(p.y - p1.y) * (float)(p2.x - p1.x) / (float)(p2.y - p1.y) + p1.x;
|
|
// if ( x > p.x )
|
// {
|
// nCross++;
|
// }
|
|
// }
|
|
// if ((nCross % 2) == 1)
|
// {
|
// return true;
|
// }
|
// else
|
// {
|
// return false;
|
// }
|
// }
|
|
