Line data Source code
1 : /*
2 : * polygon.cpp
3 : * Copyright (C) Andrew Tridgell 2011
4 : *
5 : * This file is free software: you can redistribute it and/or modify it
6 : * under the terms of the GNU General Public License as published by the
7 : * Free Software Foundation, either version 3 of the License, or
8 : * (at your option) any later version.
9 : *
10 : * This file is distributed in the hope that it will be useful, but
11 : * WITHOUT ANY WARRANTY; without even the implied warranty of
12 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 : * See the GNU General Public License for more details.
14 : *
15 : * You should have received a copy of the GNU General Public License along
16 : * with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : #include "AP_Math.h"
20 : #include "float.h"
21 :
22 : #pragma GCC optimize("O2")
23 :
24 : /*
25 : * The point in polygon algorithm is based on:
26 : * https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html
27 : */
28 :
29 :
30 : /*
31 : * Polygon_outside(): test for a point in a polygon
32 : * Input: P = a point,
33 : * V[] = vertex points of a polygon V[n+1] with V[n]=V[0]
34 : * Return: true if P is outside the polygon
35 : *
36 : * This does not take account of the curvature of the earth, but we
37 : * expect that to be very small over the distances involved in the
38 : * fence boundary
39 : */
40 : template <typename T>
41 3649514 : bool Polygon_outside(const Vector2<T> &P, const Vector2<T> *V, unsigned n)
42 : {
43 3649514 : const bool complete = Polygon_complete(V, n);
44 3649513 : if (complete) {
45 : // the last point is the same as the first point; treat as if
46 : // the last point wasn't passed in
47 3116 : n--;
48 : }
49 :
50 : unsigned i, j;
51 : // step through each edge pair-wise looking for crossings:
52 : bool outside = true;
53 22504129 : for (i=0; i<n; i++) {
54 18854616 : j = i+1;
55 18854616 : if (j >= n) {
56 : j = 0;
57 : }
58 18854616 : if ((V[i].y > P.y) == (V[j].y > P.y)) {
59 12655338 : continue;
60 : }
61 6199278 : const T dx1 = P.x - V[i].x;
62 6199278 : const T dx2 = V[j].x - V[i].x;
63 6199278 : const T dy1 = P.y - V[i].y;
64 6199278 : const T dy2 = V[j].y - V[i].y;
65 6199278 : const int8_t dx1s = (dx1 < 0) ? -1 : 1;
66 6199278 : const int8_t dx2s = (dx2 < 0) ? -1 : 1;
67 6199278 : const int8_t dy1s = (dy1 < 0) ? -1 : 1;
68 6199278 : const int8_t dy2s = (dy2 < 0) ? -1 : 1;
69 6199278 : const int8_t m1 = dx1s * dy2s;
70 6199278 : const int8_t m2 = dx2s * dy1s;
71 : // we avoid the 64 bit multiplies if we can based on sign checks.
72 6199278 : if (dy2 < 0) {
73 3099639 : if (m1 > m2) {
74 323929 : outside = !outside;
75 2775710 : } else if (m1 < m2) {
76 143481 : continue;
77 : } else {
78 : if (std::is_floating_point<T>::value) {
79 172633 : if ( dx1 * dy2 > dx2 * dy1 ) {
80 139574 : outside = !outside;
81 : }
82 : } else {
83 2459596 : if ( dx1 * (int64_t)dy2 > dx2 * (int64_t)dy1 ) {
84 1560408 : outside = !outside;
85 : }
86 : }
87 : }
88 : } else {
89 3099639 : if (m1 < m2) {
90 90221 : outside = !outside;
91 3009418 : } else if (m1 > m2) {
92 1850776 : continue;
93 : } else {
94 : if (std::is_floating_point<T>::value) {
95 47730 : if ( dx1 * dy2 < dx2 * dy1 ) {
96 39020 : outside = !outside;
97 : }
98 : } else {
99 1110912 : if ( dx1 * (int64_t)dy2 < dx2 * (int64_t)dy1 ) {
100 702261 : outside = !outside;
101 : }
102 : }
103 : }
104 : }
105 : }
106 3649513 : return outside;
107 : }
108 :
109 : /*
110 : * check if a polygon is complete.
111 : *
112 : * We consider a polygon to be complete if we have at least 4 points,
113 : * and the first point is the same as the last point. That is the
114 : * minimum requirement for the Polygon_outside function to work
115 : */
116 : template <typename T>
117 7111341 : bool Polygon_complete(const Vector2<T> *V, unsigned n)
118 : {
119 7111341 : return (n >= 4 && V[n-1] == V[0]);
120 : }
121 :
122 : // Necessary to avoid linker errors
123 : template bool Polygon_outside<int32_t>(const Vector2l &P, const Vector2l *V, unsigned n);
124 : template bool Polygon_complete<int32_t>(const Vector2l *V, unsigned n);
125 : template bool Polygon_outside<float>(const Vector2f &P, const Vector2f *V, unsigned n);
126 : template bool Polygon_complete<float>(const Vector2f *V, unsigned n);
127 :
128 : /*
129 : determine if the polygon of N verticies defined by points V is
130 : intersected by a line from point p1 to point p2
131 : intersection argument returns the intersection closest to p1
132 : */
133 7121 : bool Polygon_intersects(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2, Vector2f &intersection)
134 : {
135 7121 : const bool complete = Polygon_complete(V, N);
136 7121 : if (complete) {
137 : // if the last point is the same as the first point
138 : // treat as if the last point wasn't passed in
139 0 : N--;
140 : }
141 :
142 : float intersect_dist_sq = FLT_MAX;
143 65991 : for (uint8_t i=0; i<N; i++) {
144 58870 : uint8_t j = i+1;
145 58870 : if (j >= N) {
146 : j = 0;
147 : }
148 58870 : const Vector2f &v1 = V[i];
149 58870 : const Vector2f &v2 = V[j];
150 : // optimisations for common cases
151 58870 : if (v1.x > p1.x && v2.x > p1.x && v1.x > p2.x && v2.x > p2.x) {
152 50929 : continue;
153 : }
154 49267 : if (v1.y > p1.y && v2.y > p1.y && v1.y > p2.y && v2.y > p2.y) {
155 10048 : continue;
156 : }
157 39219 : if (v1.x < p1.x && v2.x < p1.x && v1.x < p2.x && v2.x < p2.x) {
158 16303 : continue;
159 : }
160 22916 : if (v1.y < p1.y && v2.y < p1.y && v1.y < p2.y && v2.y < p2.y) {
161 14975 : continue;
162 : }
163 7941 : Vector2f intersect_tmp;
164 7941 : if (Vector2f::segment_intersection(v1,v2,p1,p2,intersect_tmp)) {
165 3138 : float dist_sq = sq(intersect_tmp.x - p1.x) + sq(intersect_tmp.y - p1.y);
166 3138 : if (dist_sq < intersect_dist_sq) {
167 : intersect_dist_sq = dist_sq;
168 2974 : intersection = intersect_tmp;
169 : }
170 : }
171 : }
172 7121 : return (intersect_dist_sq < FLT_MAX);
173 : }
174 :
175 : /*
176 : return the closest distance that a line from p1 to p2 comes to an
177 : edge of closed polygon V, defined by N points
178 : negative numbers indicate the line cross into the polygon with the negative size being the distance from p2 to the intersection point closest to p1
179 : */
180 6828 : float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2f &p1, const Vector2f &p2)
181 : {
182 6828 : Vector2f intersection;
183 6828 : if (Polygon_intersects(V,N,p1,p2,intersection)) {
184 2658 : return -sqrtf(sq(intersection.x - p2.x) + sq(intersection.y - p2.y));
185 : }
186 : float closest_sq = FLT_MAX;
187 37514 : for (uint8_t i=0; i<N; i++) {
188 33344 : uint8_t j = i+1;
189 33344 : if (j >= N) {
190 : j = 0;
191 : }
192 33344 : const Vector2f &v1 = V[i];
193 33344 : const Vector2f &v2 = V[j];
194 :
195 33344 : float dist_sq = Vector2f::closest_distance_between_lines_squared(v1, v2, p1, p2);
196 33344 : if (dist_sq < closest_sq) {
197 : closest_sq = dist_sq;
198 : }
199 : }
200 4170 : return sqrtf(closest_sq);
201 : }
202 :
203 : /*
204 : return the closest distance that point p comes to an edge of closed
205 : polygon V, defined by N points
206 : */
207 3454706 : bool Polygon_closest_distance_point(const Vector2f *V, unsigned N,
208 : const Vector2f &p, Vector2f &closest_vec)
209 : {
210 : float closest_sq = FLT_MAX;
211 3454706 : if (Polygon_complete(V, N) && N > 0) {
212 10 : N--; // not a polygon
213 : }
214 3454706 : if (N < 3) {
215 : return false;
216 : }
217 :
218 : Vector2f best_v(0.0f, 0.0f);
219 :
220 21129201 : for (uint32_t i = 0; i < N; i++) {
221 17674495 : const Vector2f &a = V[i];
222 17674495 : const Vector2f &b = V[(i + 1) % N];
223 :
224 : // Built-in: closest point on segment ab to p (handles v==w).
225 17674495 : const Vector2f q = Vector2f::closest_point(p, a, b);
226 17674495 : const Vector2f v = q - p; // vector from p to boundary
227 17674495 : const float vsq = v.length_squared(); // squared distance
228 :
229 17674495 : if (vsq < closest_sq) {
230 : closest_sq = vsq;
231 7607780 : best_v = v;
232 : }
233 : }
234 :
235 3454706 : if (is_equal(closest_sq, FLT_MAX)) {
236 : return false;
237 : }
238 :
239 3454706 : closest_vec = best_v; // already correct direction & length
240 3454706 : return true;
241 : }
|
