-
Notifications
You must be signed in to change notification settings - Fork 0
/
GeoFenceHelper.py
165 lines (151 loc) · 6.61 KB
/
GeoFenceHelper.py
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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
#
# geoFenceHelper.py
# shotmanager
#
# Helper class to do the math in fenceManager
#
# Created by Yi Lu
# Copyright (c) 2016 3D Robotics. All rights reserved.
from vector2 import *
import shotLogger
logger = shotLogger.logger
class GeoFenceHelper:
@staticmethod
def closestPointToSegment(point, segment):
"""
Find the closest point from a point to a segment
:param point: Vector2, source point
:param segment: list of Vector2 with length of 2, denoting two end points of a segment
:return: Tuple (Float, Vector2), first element is the position t of result point on the segment from 0-1
second element is a Vector2 denoting the resulting point
"""
if len(segment) < 2:
logger.log("[GeoFenceHelper]: Illegal segment %s" % segment)
return None
a = segment[0]
b = segment[1]
aToP = point - a
aToB = b - a
atb2 = aToB.x * aToB.x + aToB.y * aToB.y
atp_dot_atb = aToP.dot(aToB)
if atb2 != 0:
t = float(atp_dot_atb) / atb2
else:
# a and b are infinitely close
return a, (point - a).length()
intersect = Vector2(a.x + aToB.x * t, a.y + aToB.y * t)
distance = (point - intersect).length()
if 0 <= t <= 1:
return intersect, distance
elif t < 0:
return a, (point - a).length()
else:
return b, (point - b).length()
@staticmethod
def closestPointToPolygon(point, polygon):
"""
Find the closest point from a point to a polygon defined by a list of vertices
:param point: Vector2, source point
:param polygon: list of Vector2, defines the vertices of polygon, minimum length 3
:return: None if an illegal polygon has been passed
Vector2 denoting the resulting point if found
"""
# Polygon must has at least 3 vertices plus repeated origin
if len(polygon) < 4:
logger.log("[GeoFenceHelper]: Polygon need at least three vertices")
return None
intersect = None
distance = float("inf")
for i in range(0, len(polygon) - 1):
a = polygon[i]
b = polygon[i + 1]
segIntersect, segDistance = GeoFenceHelper.closestPointToSegment(point, [a, b])
if segDistance < distance:
intersect = segIntersect
distance = segDistance
return intersect
@staticmethod
def closestCollisionVectorToSegment(ray, segment):
"""
http://stackoverflow.com/questions/14307158/how-do-you-check-for-intersection-between-a-line-segment-and-a-line-ray-emanatin
Detect the collision point for a ray with a segment
:param ray: Tuple of Vector2, origin and direction denoting a ray
:param segment: Tuple of Vector2, denoting segment to be tested against collision
:return: None if ray is not intersecting with segment
Float t if intersecting, t is the position of intersection on the ray
in equation: r(t) = ray[0] + t * (ray[1] - ray[0])
"""
denom = ((ray[1].x - ray[0].x) * (segment[1].y - segment[0].y)) - (ray[1].y - ray[0].y) * (segment[1].x - segment[0].x)
if denom == 0:
return None
r = (((ray[0].y - segment[0].y) * (segment[1].x - segment[0].x)) - (ray[0].x - segment[0].x) * (segment[1].y - segment[0].y)) / denom
s = (((ray[0].y - segment[0].y) * (ray[1].x - ray[0].x)) - (ray[0].x - segment[0].x) * (ray[1].y - ray[0].y)) / denom
if r >= 0 and 0 <= s <= 1:
return r
return None
@staticmethod
def closestCollisionVectorToPolygon(ray, polygon):
"""
Detect the closet collision point for a ray with a polygon
:param ray: Tuple of Vector2, origin and direction denoting a ray
:param polygon: list of Vector2
:return: None if ray is not intersecting with polygon
(Int, Double, Vector2) if intersection is found
Int being edge index
Double being position along collision vector
Vector2 being collision point on Polygon
"""
# Polygon must has at least 3 vertices plus repeated origin
if len(polygon) < 4:
logger.log("[GeoFenceHelper]: Illegal polygon, vertex count must be 3 or more, got %s" % len(polygon))
return None
collidingPoint = (-1, float("inf"), None)
for i in range(len(polygon) - 1):
t = GeoFenceHelper.closestCollisionVectorToSegment(ray, (polygon[i], polygon[i + 1]))
if t is not None and 0 < t < collidingPoint[1]:
intersection = Vector2(ray[0].x + t * (ray[1].x - ray[0].x), ray[0].y + t * (ray[1].y - ray[0].y))
collidingPoint = (i, t, intersection)
if collidingPoint[0] == -1:
return None
return collidingPoint
@staticmethod
def isPointInPolygon(point, polygon):
"""
http://geomalgorithms.com/a03-_inclusion.html#wn_PnPoly()
:param point: Vector2 denoting the point
:param polygon: list of Vector2 denoting vertices of polygon
:return: Winding number of point and polygon:
0 if point is outside polygon
>0 if polygon is winding around point 1 or more times
<0 if polygon is not ccw?
"""
# Polygon must has at least 3 vertices plus repeated origin
if len(polygon) < 4:
logger.log("[GeoFenceHelper]: polygon must have 3 or more vertices, got %s" % len(polygon))
return None
wn = 0
for i in range(0, len(polygon) - 1):
v1 = polygon[i]
v2 = polygon[i + 1]
if v1.y <= point.y:
if v2.y > point.y:
if GeoFenceHelper.isLeft(v1, v2, point) > 0:
wn += 1
else:
if v2.y <= point.y:
if GeoFenceHelper.isLeft(v1, v2, point) < 0:
wn -= 1
return wn
@staticmethod
def isLeft(p0, p1, p2):
"""
http://geomalgorithms.com/a01-_area.html
Test if a point is Left|On|Right of an infinite 2D line.
:param p0: Vector2, first point of segment
:param p1: Vector2, second point of segment
:param p2: Vector2, point to be tested against segment
:return: >0 for P2 left of the line through P0 to P1
=0 for P2 on the line
<0 for P2 right of the line
"""
return (p1.x - p0.x) * (p2.y - p0.y) - (p2.x - p0.x) * (p1.y - p0.y)