Repository navigation
Expand file tree
/
Copy pathuseful_things.py
More file actions
116 lines (91 loc) · 2.64 KB
/
Copy pathuseful_things.py
File metadata and controls
116 lines (91 loc) · 2.64 KB
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
"""
Useful things
"""
import numpy as np
####################
####################
####################
def Dot(u, v):
return (u[0] * v[0]) + (u[1] * v[1]);
####################
####################
####################
def Add(pt1, pt2):
return (pt1[0] + pt2[0], pt1[1] + pt2[1]);
####################
####################
####################
def Substract(pt1, pt2):
return (pt1[0] - pt2[0], pt1[1] - pt2[1]);
####################
####################
####################
def Norm(pt):
l = np.sqrt(pt[0] * pt[0] + pt[1] * pt[1]);
return (pt[0] / l, pt[1] / l);
####################
####################
####################
def Dist(pt1, pt2):
pt = (pt2[0] - pt1[0], pt2[1] - pt1[1]);
return np.sqrt(pt[0] * pt[0] + pt[1] * pt[1]);
####################
####################
####################
def DistSquared(pt1, pt2):
pt = (pt2[0] - pt1[0], pt2[1] - pt1[1]);
return pt[0] * pt[0] + pt[1] * pt[1];
####################
####################
####################
def InterpolateLine(pt1, pt2, num_pt):
sample_pts = []
dir_pt = Norm( (pt2[0] - pt1[0], pt2[1] - pt1[1]) );
line_len = Dist(pt1, pt2);
d_delta = line_len / num_pt;
for i in frange(0, line_len , d_delta):
sample_pts.append( (pt1[0] + dir_pt[0] * i, pt1[1] + dir_pt[1] * i) );
return sample_pts;
####################
####################
####################
def frange(start, stop, step):
x = start;
while x < stop:
yield x;
x += step;
####################
####################
####################
def CreatePoly(num_side, center_pt, radius):
poly = [];
add_value = (np.pi * 2.0 / num_side);
for i in frange(0, np.pi * 2.0 , add_value):
x_pt = center_pt[0] + radius * np.sin(i);
y_pt = center_pt[1] + radius * np.cos(i);
poly.append((x_pt, y_pt));
return poly;
####################
####################
####################
def DistanceToAFiniteLine(v, w, p):
machine_eps = np.finfo(float).eps;
l2 = DistSquared(v, w);
if (l2 > -machine_eps and l2 < machine_eps):
return Dist(p, v);
t = Dot( Substract(p, v), Substract(w, v) ) / l2;
if(t < 0):
return Dist(p, v);
elif (t > 1.0):
return Dist(p, w);
wv = Substract(w, v);
wv = (wv[0] * t, wv[1] * t);
projection = Add(v, wv);
return Dist(projection, p);
####################
####################
####################
#def IsClose(a, b, rel_tol=1e-09, abs_tol=0.0):
# return abs(a-b) <= max(rel_tol * max(abs(a), abs(b)), abs_tol);
def IsClose(a, b):
return abs(a-b) <= 1e-5;