update para

Scheaven

2021-01-29 56def8d41711125189536c87796af2d6702a21f9

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
// 
// 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; 
} 
 
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; 
} 
 
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); 
}