def cross(a, b):
"""
線分の交点を返す
"""
line_a = sg.Line(sg.Point(a[0][0], a[0][1]), sg.Point(a[1][0], a[1][1]))
line_b = sg.Line(sg.Point(b[0][0], b[0][1]), sg.Point(b[1][0], b[1][1]))
result = line_a.intersection(line_b)
return result
def intersection(self, s: "Segment2D") -> Vector2D:
l1 = spg.Line(
spg.Point(self.start.x, self.start.y), spg.Point(self.end.x, self.end.y)
)
l2 = spg.Line(spg.Point(s.start.x, s.start.y), spg.Point(s.end.x, s.end.y))
intersections = l1.intersection(l2)
if len(intersections) != 1:
raise Exception("TODO")
if not isinstance(intersections[0], spg.Point2D):
raise Exception("TODO")
return Vector2D(float(intersections[0].x), float(intersections[0].y))
def align_rectangle_point(x1, y1, x2, y2, x3, y3) -> tuple:
'''Re-aligns the third point among three points to form a rectangle'''
line1 = SG.Line(SG.Point(x1,y1), SG.Point(x2,y2))
line2 = line1.perpendicular_line(SG.Point(x2,y2))
line3 = line2.perpendicular_line(SG.Point(x3,y3))
fixed_point = line2.intersection(line3)[0]
return int(fixed_point.x), int(fixed_point.y)
def align_rectangles_working(x1, y1, x2, y2, x3, y3):
line1 = SG.Line(SG.Point(x1, y1), SG.Point(x2, y2))
line2 = line1.perpendicular_line(SG.Point(x2, y2))
line3 = line2.perpendicular_line(SG.Point(x3, y3))
# returns correct x3, y3 unlike the method above
# please generate x4, y4
# Return type is that of sympy Point
return line2.intersection(line3)
def policzPktyZaokraglenia(geometria, temp):
"""
Funkcja ma na celu obliczenie punktów na krzywej wirnika w górnej jego części (zob. krzywą :math:`ED` na rysunku przekroju wirnika). Procedura obliczania punktów na krzywej jest identyczna jak w przypadku funkcji :func:`dodajWspolrzedne`.
:param geometria: obiekt klasy Geometry zawierający dane geometryczne wirnika
:type geometria: Geometry
:param temp: tymczasowy kontener na dane, użyty w celu przechowywania informacji.
:type temp: dictionary
:return kolo_BC: obiekt *list* zawierający punkty leżące na zaokrągleniu w górnej części wirnika.
"""
lopatka_XYZ = geometria.lopatka_XYZ
r2 = geometria.r2
R = geometria.R
C = temp['C']
R_pos = temp['R_pos']
pc = temp['pc']
pkty_XYZ = temp['pkty_XYZ']
def krzywiznaParametrycznie(t):
x = (-R_pos[0] + R * np.cos(t))
y = R_pos[1] + R * np.sin(t)
return M([x, y])
R_C = sg.Line(pc, sg.Point(C[0], C[1]))
R_B = sg.Line(pc, sg.Point(r2, lopatka_XYZ[-2][2]))
ang = 2 * np.pi - float(sympy.N(R_C.angle_between(R_B)))
vec_I = np.linspace(ang, 2. * np.pi, num=5)
kolo_BC = []
for i in vec_I:
temp = [0.0]
temp.extend(krzywiznaParametrycznie(i))
kolo_BC.append(temp)
kolo_BC.append(pkty_XYZ[-2])
kolo_BC = np.array(kolo_BC)[1:]
return kolo_BC
def split(self, edge, rotate_up):
"""
:param edge:
:param rotate_up: 線より上の部分が回転するかどうか
:return:
"""
line = sg.Line(edge[0], edge[1])
points = ConvexHull(self.vertices).vertices
polygon_points = []
for i in points:
polygon_points.append(self.vertices[i])
polygon = sg.Polygon(*polygon_points)
new_vertices = sg.intersection(polygon, line)
up = []
down = []
on_line = 0
for v in self.vertices:
check = is_up(v, edge)
if check > 0:
up.append(v)
else:
if check == 0:
on_line += 1
down.append(v)
if on_line == len(down):
return None, None
nv_cnt = 0
for nv in new_vertices:
if type(nv) == Segment:
continue
up.append(nv)
down.append(nv)
nv_cnt += 1
if nv_cnt != 2:
return None, None
if rotate_up:
up = [get_symmetric_point(p, edge) for p in up]
else:
down = [get_symmetric_point(p, edge) for p in down]
up_polygon = PolygonNode(edge, rotate_up, up, self.node_id)
down_polygon = PolygonNode(edge, not rotate_up, down, self.node_id)
# GC
self.vertices = []
return up_polygon, down_polygon
def expanded(self, distance: float) -> "Polygon2D":
expanded_lines = []
for s in self._segments():
# Get the direction vector for the segment
direction = Vector2D(s.end.x - s.start.x, s.end.y - s.start.y).normalized()
# Get the perpendicular normal vector
normal = Vector2D(-direction.y, direction.x)
# Scale the normal to the desired distance
diff = normal.scaled(distance)
# Use the scaled normal vector to create the expanded line
expanded_line = Line2D(
Vector2D(s.mid().x + diff.x, s.mid().y + diff.y), direction
)
expanded_lines.append(expanded_line)
# Too lazy to write intersection logic so convert to SymPy's line class
# and use that to calculate the intersection points
sympy_lines = [
spg.Line(
spg.Point(l.point.x, l.point.y),
spg.Point(l.point.x + l.direction.x, l.point.y + l.direction.y),
)
for l in expanded_lines
]
expanded_vertices = []
for curr in range(0, len(sympy_lines)):
prev = curr - 1 if curr > 0 else len(sympy_lines) - 1
intersections = sympy_lines[prev].intersection(sympy_lines[curr])
if len(intersections) != 1:
raise Exception("TODO")
if not isinstance(intersections[0], spg.Point2D):
raise Exception("TODO")
expanded_vertices.append(
Vector2D(float(intersections[0].x), float(intersections[0].y))
)
return Polygon2D(expanded_vertices)
def add_cell(self, network):
cell = Circle((self.centre_x, self.centre_y), self.radius)
nodes = network.vertices
ridges = network.ridge_vertices
nodes_to_delete = []
ridges_to_delete = []
for i in range(len(nodes)):
if cell.contains_point(nodes[i]) == True:
nodes_to_delete = np.append(nodes_to_delete, i)
for i in range(len(ridges)):
if ridges[i][0] in nodes_to_delete and ridges[i][
1] in nodes_to_delete:
ridges_to_delete = np.append(ridges_to_delete, i)
ridges_to_delete = np.array(sorted(ridges_to_delete, reverse=True))
for ridge in ridges_to_delete:
network.ridge_vertices = np.delete(network.ridge_vertices,
int(ridge),
axis=0)
center = sg.Point(self.centre_x, self.centre_y)
circ = sg.Circle(center, self.radius)
k = 0
for node in nodes_to_delete:
node = int(node)
for ridge in network.ridge_vertices:
if ridge[0] == node:
if len(nodes[node]) == 3:
In_point = sg.Point(nodes[node][0], nodes[node][1],
nodes[node][2])
Out_point = sg.Point(nodes[ridge[1]][0],
nodes[ridge[1]][1],
nodes[ridge[1]][1])
elif len(nodes[node]) == 2:
In_point = sg.Point(nodes[node][0], nodes[node][1])
Out_point = sg.Point(nodes[ridge[1]][0],
nodes[ridge[1]][1])
line = sg.Line(In_point, Out_point)
intersec = sg.intersection(circ, line)
if length_square(nodes[ridge[1]] -
intersec[0]) >= length_square(
nodes[ridge[1]] - intersec[1]):
nodes = np.append(
nodes,
[[float(intersec[1].x),
float(intersec[1].y)]],
axis=0)
else:
nodes = np.append(
nodes,
[[float(intersec[0].x),
float(intersec[0].y)]],
axis=0)
ridge[0] = len(nodes) - 1
k += 1
if ridge[1] == node:
if len(nodes[node]) == 3:
In_point = sg.Point(nodes[node][0], nodes[node][1],
nodes[node][2])
Out_point = sg.Point(nodes[ridge[0]][0],
nodes[ridge[0]][1],
nodes[ridge[0]][1])
elif len(nodes[node]) == 2:
In_point = sg.Point(nodes[node][0], nodes[node][1])
Out_point = sg.Point(nodes[ridge[0]][0],
nodes[ridge[0]][1])
line = sg.Line(In_point, Out_point)
intersec = sg.intersection(circ, line)
if length_square(nodes[ridge[0]] -
intersec[0]) >= length_square(
nodes[ridge[0]] - intersec[1]):
nodes = np.append(
nodes,
[[float(intersec[1].x),
float(intersec[1].y)]],
axis=0)
else:
nodes = np.append(
nodes,
[[float(intersec[0].x),
float(intersec[0].y)]],
axis=0)
ridge[1] = len(nodes) - 1
k += 1
nodes_to_delete = np.array(sorted(nodes_to_delete, reverse=True))
for point in nodes_to_delete:
nodes = np.delete(nodes, int(point), 0)
# Renumber points after deleting some
for ridge in network.ridge_vertices:
for i in range(2):
r = 0
for node in nodes_to_delete:
if node < ridge[i]:
r += 1
ridge[i] = ridge[i] - r
network.vertices = nodes
network.vertices_ini = np.array(network.vertices.tolist())
network = network.create_ridge_node_list()
network = network.sort_nodes()
network.interior_nodes = network.interior_nodes[:-k]
self.boundary_cell = list(range(len(nodes) - k, len(nodes)))
return network
def standardize(self):
# Fix p1 to the origin and translate triangle
points = self.vertices
fixed = points[0]
points = [p - fixed for p in points]
t_fix = g.Triangle(*points)
# Rotate triangle into Q1 and fix side to x-axis
origin = g.Point(0, 0)
origin_sides = []
for s in t_fix.sides:
if s.p1 == origin or s.p2 == origin:
origin_sides.append(s)
xvector = g.Line(p1=origin, p2=g.Point(1, 0))
angle_segments = []
for os in origin_sides:
if os.p1 == origin:
other = os.p2
else:
other = os.p1
sg_orient = g.Segment(p1=origin, p2=other)
angle = xvector.angle_between(sg_orient)
angle_segments.append((float(angle), sg_orient))
print(angle_segments)
rotate_angle, rotate_segment = max(angle_segments,
key=lambda t: float(t[0]))
print(rotate_angle, rotate_segment)
# Check which quadrant the evaluation segment is in. If Q3 or Q4, do nothing to angle; if Q1 or Q2, negate angle
if float(rotate_segment.p2.y) > 0:
rotate_angle = -rotate_angle
rotate_points = t_fix.vertices
t_rot = g.Triangle(*[i.rotate(rotate_angle) for i in rotate_points])
# Reflect the triangle about its base midpoint if the angle from the origin is less than or equal to 45
origin_angle = float(t_rot.angles[origin])
print(t_fix)
base_points = []
alterior_vertex = None
for v in t_rot.vertices:
if float(v.y) == float(0):
base_points.append(v)
else:
alterior_vertex = v
base = g.Segment(*base_points)
if origin_angle <= (np.pi / 4):
midpoint = base.midpoint
x_dist = alterior_vertex.x - midpoint.x
new_av = g.Point(x=(midpoint.x - x_dist), y=alterior_vertex.y)
vertices = base_points + [new_av]
t = g.Triangle(*vertices)
else:
t = t_rot
return t
def init(self):
# select random point for each obstacle
for obs in self.obstacles:
obs.bk = obs.samplePosition()
self.obsBkPairLines = []
for i in range(len(self.obstacles)):
for j in range(i + 1, len(self.obstacles)):
bk1 = self.obstacles[i].bk
bk2 = self.obstacles[j].bk
pline = symgeo.Line(symgeo.Point(bk1[0], bk1[1]),
symgeo.Point(bk2[0], bk2[1]))
self.obsBkPairLines.append(pline)
# select central point c
self.centralPoint = (self.width / 2, self.height / 2)
foundCP = False
while foundCP == False:
if (self.isInObstacle(self.centralPoint)
== False) and (self.isInObsBkLinePair(self.centralPoint)
== False):
foundCP = True
else:
cpX = np.random.normal() * (
self.sampleWidthScale) + self.width / 2
cpY = np.random.random() * (
self.sampleHeightScale) + self.height / 2
self.centralPoint = (int(cpX), int(cpY))
#print "Resampling " + str(self.centralPoint)
# init four boundary line
self.boundary_lines = []
self.x_axis = symgeo.Line(symgeo.Point(0, 0),
symgeo.Point(self.width - 1, 0))
self.y_axis = symgeo.Line(symgeo.Point(0, 0),
symgeo.Point(0, self.height - 1))
self.x_high = symgeo.Line(
symgeo.Point(0, self.height - 1),
symgeo.Point(self.width - 1, self.height - 1))
self.y_high = symgeo.Line(
symgeo.Point(self.width - 1, 0),
symgeo.Point(self.width - 1, self.height - 1))
self.boundary_lines.append(self.x_axis)
self.boundary_lines.append(self.y_high)
self.boundary_lines.append(self.x_high)
self.boundary_lines.append(self.y_axis)
# init lines from center point to four corners
self.center_corner_lines_info = []
self.center_corner_lines_info.append([
(0, 0),
numpy.arctan2(float(-self.centralPoint[1]),
float(-self.centralPoint[0]))
])
self.center_corner_lines_info.append([
(0, self.height),
numpy.arctan2(float(self.height - self.centralPoint[1]),
float(-self.centralPoint[0]))
])
self.center_corner_lines_info.append([
(self.width, self.height),
numpy.arctan2(float(self.height - self.centralPoint[1]),
float(self.width - self.centralPoint[0]))
])
self.center_corner_lines_info.append([
(self.width, 0),
numpy.arctan2(float(-self.centralPoint[1]),
float(self.width - self.centralPoint[0]))
])
for ccl_info in self.center_corner_lines_info:
if ccl_info[1] < 0:
ccl_info[1] += 2 * numpy.pi
self.center_corner_lines_info.sort(key=lambda x: x[1], reverse=False)
#print "CENTER CORNER LINES "
#print self.center_corner_lines_info
self.ray_info_list = []
# init alpah and beta segments
for obs in self.obstacles:
obs.alpha_ray = symgeo.Ray(
symgeo.Point(obs.bk[0], obs.bk[1]),
symgeo.Point(self.centralPoint[0], self.centralPoint[1]))
obs.beta_ray = symgeo.Ray(
symgeo.Point(obs.bk[0], obs.bk[1]),
symgeo.Point(2 * obs.bk[0] - self.centralPoint[0],
2 * obs.bk[1] - self.centralPoint[1]))
a_pt = self.findIntersectionWithBoundary(obs.alpha_ray)
#print str(obs.alpha_ray) + " --> " + str(a_pt)
b_pt = self.findIntersectionWithBoundary(obs.beta_ray)
#print str(obs.beta_ray) + " --> " + str(b_pt)
obs.alpha_seg = None
obs.beta_seg = None
if a_pt != None:
alpha_seg = shpgeo.LineString([obs.bk, (a_pt.x, a_pt.y)])
obs.alpha_seg = LineSegmentMgr(alpha_seg, 'A', obs)
if b_pt != None:
beta_seg = shpgeo.LineString([obs.bk, (b_pt.x, b_pt.y)])
obs.beta_seg = LineSegmentMgr(beta_seg, 'B', obs)
alpha_ray_rad = numpy.arctan(float(obs.alpha_ray.slope))
if obs.alpha_ray.p1.x > obs.alpha_ray.p2.x:
alpha_ray_rad += numpy.pi
if alpha_ray_rad < 0:
alpha_ray_rad += 2 * numpy.pi
alpha_ray_info = (obs.idx, 'A', alpha_ray_rad)
beta_ray_rad = numpy.arctan(float(obs.beta_ray.slope))
if obs.beta_ray.p1.x > obs.beta_ray.p2.x:
beta_ray_rad += numpy.pi
if beta_ray_rad < 0:
beta_ray_rad += 2 * numpy.pi
beta_ray_info = (obs.idx, 'B', beta_ray_rad)
#print "ALPHA RAY SLOPE " + str(alpha_ray_info)
#print "BETA RAY SLOPE " + str(beta_ray_info)
self.ray_info_list.append(alpha_ray_info)
self.ray_info_list.append(beta_ray_info)
obs.alpha_seg_info = ((a_pt.x, a_pt.y), alpha_ray_rad)
obs.beta_seg_info = ((b_pt.x, b_pt.y), beta_ray_rad)
self.ray_info_list.sort(key=lambda x: x[2], reverse=False)
def dodajWspolrzedne(wek):
r"""
Funkcja zawiera opis krzywizny łopatki we współrzędnych parametrycznych. W pierwszym etapie zostają wyznaczone kąty :math:`{\kappa}_{1}` i :math:`{\kappa}_{2}`. Następnie tworzona jest tablica zawierająca osiem równoodległych wartości z pomiędzy tych kątów. Tak otrzymane dane zostają użyte przy określaniu punktów leżących na krzywej :math:`AC`.
.. figure:: ./image/kat.png
:align: center
:alt: Krzywizna łopatki
:figclass: align-center
:scale: 18%
Szkic krzywizny łopatki
:param wek: tablica zawierająca położenie punktów :math:`A`, :math:`B` i :math:`C`.
:type wek: lista zawierająca współrzędne punktów na krzywiźnie łopatki
"""
# Deklaracja zmiennych
srOk = M([wek[2][0], wek[2][1]])
L1 = sg.Point(wek[0][0], wek[0][1])
L6 = sympy.N(sg.Point(wek[1][0], wek[1][1]))
# Srodek okregu, ktory zawiera krzywizne wirnika
L_cent = sg.Point(srOk[0], srOk[1])
# Zwroc promien wirnika
promien = sympy.N(L1.distance(L_cent))
# Prosta pozioma przechodzaca przez srodek ukladu wspolrzednych
hl = sg.Line(sg.Point(0., 0.), sg.Point(1., 0.))
# Prosta przechodzaca przez srodek krzywizny i punkt poczatkowy lopatki
l_6 = sg.Line(L6, L_cent)
# Prosta przechodzaca przez srodek krzywizny i punkt koncowy lopatki
l_1 = sg.Line(L1, L_cent)
# Oblicz kat pomiedzy prosta l_6 a prosta pozioma
ang_6_hl = float(sympy.N(l_6.angle_between(hl)) + np.pi)
# Oblicz kat pomiedzy prosta l_1 a prosta pozioma
ang_1_hl = float(sympy.N(l_1.angle_between(hl)) + np.pi)
# Zdefiniuj w ilu punktach krzywizna wirnika powinna zostac obliczona
podzial = 8
# Stworz wektor zawierajacy katy, ktore zostana uzyte do obliczenia punktow
vec_I = np.linspace(ang_6_hl, ang_1_hl, num=podzial)[1:-1]
# Funkcja opisujace krzywizne lopatki w sposob parametryczny
def krzywiznaParametrycznie(t):
x = srOk[0] + promien * np.cos(t)
y = srOk[1] + promien * np.sin(t)
return [x, y]
# Stworz wektor zawierajacy wspolrzedne punktow opisujacych lopatke
r_temp = [
[L6.x, L6.y],
]
for i in vec_I:
# Oblicz polozenie punktu na podstawie parametrycznej funkcji opisujacej
# krzywizne lopatki i kata okreslajacego wspolrzedne.
krzyw = krzywiznaParametrycznie(i)
r_temp.append(krzyw)
r_temp.append([L1.x, L1.y])
r_temp.reverse()
r_temp.append(srOk)
return r_temp
def wspolrzednePktPrzekroju(geometria, temp):
r"""
Funkcja służąca do obliczenia niezbędnych wymiarów wirnika. Na poniższym rysunku został przedstawiony przekrój wirnika i punkty określające jego wymiary.
.. figure:: ./image/przekroj.png
:align: center
:alt: Szkic przekroju wirnika
:figclass: align-center
:scale: 20%
Szkic przekroju wirnika
Punkty te odpowiadają następującym wymiarowm pobranym z GUI:
* Promień otworu - odl. od osi pionowej do punktu A
* Promień zewnętrzny - odl. od osi pionowej do punktu B
* Promień u wylotu - odl. od osi pion
* Wysokość łopatki - odcinek :math:`|BC|`
* Kąt alfa - kąt :math:`\alpha`
* Promień zaokrąglenia - odcinek :math:`|RD|=|RE|`
* Wysokość pod naddatek - odcinek :math:`|EF|`
* Wysokość wirnika - odległość od punktu F do osi poziomej.
W celu stworzenia geometrii w programie GMSH należy obliczyć położenie punktu :math:`D`. Punkt ten jest określane poprzez sprawdzenie punktów wspólnych okręglu zakreślonego w punkcie :math:`R` o promieniu :math:`|RD|` z prostą przechodzącą przez punkt :math:`C` odchyloną od poziomu o kąt :math:`\alpha`. Zadanie to zostało wykonane przy użyciu modułu SymPy.
:param geometria: obiekt klasy Geometry zawierający dane geometryczne wirnika
:type geometria: Geometry
:param temp: tymczasowy kontener na dane, użyty w celu przechowywania informacji.
:type temp: dictionary
:return temp: uaktualniony kontener na dane.
"""
# Deklaracja zmiennych
alfa = geometria.alfa
h1 = geometria.h1
h2 = geometria.h2
h3 = geometria.h3
r3 = geometria.r3
r4 = geometria.r4
R = geometria.R
# Deklaracja punktow na przekroju
A = M([r3, h1])
C = M([r4, h2])
D = M([r4, h3])
R_pos = M([(r4 + R), C[1]])
temp['C'] = C
temp['R_pos'] = R_pos
# Tworzenie funkcji liniowej okreslajacej pochylenie wirnika
a = np.tan(np.deg2rad(-alfa))
b = h1 - a * r3
funLin = lambda x: a * x + b
temp['funLin'] = funLin
# Wykorzystanie biblioteki Sympy do okreslenia punktu przeciecia sie
# zaokraglonej czesci wirnika z pochylona plaszczyzna
p1 = sg.Point(A[0], A[1])
p2 = sg.Point(r3 - 2.0, funLin(r3 - 2.0))
l = sg.Line(p1, p2)
pc = sg.Point(R_pos[0], R_pos[1])
c = sg.Circle(pc, R)
temp['pc'] = pc
# Punkty przeciecia sie okregu z prosta
punkty = sg.intersection(c, l)
# Okresl czy istnieja punkty przeciecia i wybierz poprawne
if len(punkty) == 0:
text = "Powierzchnia wylotu nie moze zostać stworzona. Zmien wartosc \
wymiaru wysokosci lopatki, kat alfa, badz promien zaokraglenia"
raise ValueError(text)
if len(punkty) == 2:
w = min(punkty, key=lambda p: p.y)
B = M([sympy.N(w).x, sympy.N(w).y])
else:
w = punkty
B = M([sympy.N(punkty).x, sympy.N(punkty).y])
# Zbierz wszystkie punkty w jednej macierzy 'pkty_YZ'
pkty_YZ = M([A, B, C, D, R_pos])
# Przystosuj zmienne do obliczen w GMSH'u
for i in pkty_YZ:
i[0], i[1] = float(i[0]), float(i[1])
i[0] = -i[0]
# Dodaj trzeci wymiar do obliczonych punktow
pkty_XYZ = np.insert(pkty_YZ, 0, 0.0, axis=1)
temp['pkty_XYZ'] = pkty_XYZ
return temp
def footprint(sensor):
'''
Caculates the foot print of the off nadir camera by projecting rays from
the sensor corners through the "lens" (focal length) out onto the ground.
It's a lot of fun linear algebra that the SYMPY library handles.
'''
# Setup DF to house camera footprint polygons
footprint = pd.DataFrame(
np.zeros((1, 5)), columns=['fov_h', 'fov_v', 'path', 'pp_x', 'pp_y'])
# convert sensor dimensions to meters, divide x/y for corner coord calc
print("SENSOR", sensor)
f = sensor['focal'] * 0.001
sx = sensor['sensor_x'] / 2 * 0.001
sy = sensor['sensor_y'] / 2 * 0.001
# calculate the critical pitch (in degrees) where the horizon will be
# visible with the horizon viable, the ray projections go backward
# and produce erroneous IFOV polygons (90 - 0.5*vert_fov)
# exit with error message if critical pitch is exceeded
crit_pitch = 90 - np.rad2deg(np.arctan(sy / f))
if sensor['gimy'] >= crit_pitch:
print('!!! The provided parameters indicate that the vertical field')
print('\t of view extends above the horizon. Please start over and')
print('\t try a shallower camera angle. The maximum angle for this')
print('\t camera is %0.2f' % (crit_pitch))
sys.exit()
# calculate horz and vert field of view angles
footprint.fov_h = 2 * np.rad2deg(np.arctan(sx / f))
footprint.fov_v = 2 * np.rad2deg(np.arctan(sy / f))
# sensor corners (UR,LR,LL,UL), north-oriented and zero pitch
corners = np.array([[0 + sx, 0 - f, sensor['alt'] + sy],
[0 + sx, 0 - f, sensor['alt'] - sy],
[0 - sx, 0 - f, sensor['alt'] - sy],
[0 - sx, 0 - f, sensor['alt'] + sy]])
# offset corner points by cam x,y,z for rotation
cam_pt = np.atleast_2d(np.array([0, 0, sensor['alt']]))
corner_p = corners - cam_pt
# convert off nadir angle to radians
pitch = np.deg2rad(90.0 - sensor['gimy'])
# setup pitch rotation matrix (r_x)
r_x = np.matrix([[1.0, 0.0, 0.0], [0.0,
np.cos(pitch), -1 * np.sin(pitch)],
[0.0, np.sin(pitch), np.cos(pitch)]])
# rotate corner_p by r_x, add back cam x,y,z offsets
p_out = np.matmul(corner_p, r_x) + cam_pt
# GEOMETRY
# Set Sympy 3D point for the camera and a 3D plane for intersection
cam_sp = spg.Point3D(0, 0, sensor['alt'])
plane = spg.Plane(spg.Point3D(0, 0, 0), normal_vector=(0, 0, 1))
# blank array for footprint intersection coords
inter_points = np.zeros((corners.shape[0], 2))
# for each sensor corner point
idx_b = 0
for pt in np.asarray(p_out):
# create a Sympy 3D point and create a Sympy 3D ray from
# corner point through camera point
pt_sp = spg.Point3D(pt[0], pt[1], pt[2])
ray = spg.Ray3D(pt_sp, cam_sp)
# calculate the intersection of the ray with the plane
inter_pt = plane.intersection(ray)
# Extract out the X,Y coords fot eh intersection point
# ground intersect points will be in this order (LL,UL,UR,LR)
inter_points[idx_b, 0] = inter_pt[0].x.evalf()
inter_points[idx_b, 1] = inter_pt[0].y.evalf()
idx_b += 1
# append inter_points to footprints as a matplotlib path object
footprint.path[0] = mplPath.Path(inter_points)
# calculate the principle point by intersecting the corners of the ifov path
ll_pt = spg.Point(inter_points[0, 0], inter_points[0, 1])
ul_pt = spg.Point(inter_points[1, 0], inter_points[1, 1])
ur_pt = spg.Point(inter_points[2, 0], inter_points[2, 1])
lr_pt = spg.Point(inter_points[3, 0], inter_points[3, 1])
line_ll_ur = spg.Line(ll_pt, ur_pt)
line_lr_ul = spg.Line(lr_pt, ul_pt)
pp_inter = line_ll_ur.intersection(line_lr_ul)
footprint.pp_x = pp_inter[0].x.evalf()
footprint.pp_y = pp_inter[0].y.evalf()
print("LL", ll_pt, "UL", ul_pt, "UR", ur_pt, "LR", lr_pt)
return footprint
def plot_map(self):
if self.launch_location == 'izu':
#for IZU URA-SABAKU!!
# Set limit range in maps
self.set_coordinate_izu()
# for tamura version
# Set map image
img_map = Image.open("./map/Izu_map_mag.png")
img_list = np.asarray(img_map)
img_height = img_map.size[0]
img_width = img_map.size[1]
img_origin = np.array(
[722, 749]) # TODO : compute by lat/long of launcher point
#pixel2meter = (139.431463 - 139.41283)/1800.0 * lon2met
pixel2meter = 0.946981208125
# Define image range
img_left = -1.0 * img_origin[0] * pixel2meter
img_right = (img_width - img_origin[0]) * pixel2meter
img_top = img_origin[1] * pixel2meter
img_bottom = -1.0 * (img_height - img_origin[1]) * pixel2meter
fig = plt.figure(figsize=(12, 10))
# plot setting
ax = fig.add_subplot(111)
color_line = '#ffff33' # Yellow
color_circle = 'r' # Red
# Set circle object
cir_rail = patches.Circle(xy=self.xy_rail,
radius=self.lim_radius,
ec=color_circle,
fill=False)
cir_switch = patches.Circle(xy=self.xy_switch,
radius=self.lim_radius,
ec=color_circle,
fill=False)
cir_tent = patches.Circle(xy=self.xy_tent,
radius=self.lim_radius,
ec=color_circle,
fill=False)
ax.add_patch(cir_rail)
ax.add_patch(cir_switch)
ax.add_patch(cir_tent)
# plot map
plt.imshow(img_list,
extent=(img_left, img_right, img_bottom, img_top))
# Write landing permission range
plt.plot(self.xy_rail[0],
self.xy_rail[1],
'r.',
color=color_circle,
markersize=12)
plt.plot(self.xy_switch[0],
self.xy_switch[1],
'.',
color=color_circle)
plt.plot(self.xy_tent[0], self.xy_tent[1], '.', color=color_circle)
plt.plot(self.xy_range[:, 0],
self.xy_range[:, 1],
'--',
color=color_line)
"""
# plot landing point for 2018/3/23
plt.plot(self.xy_land[0], self.xy_land[1], 'r*', markersize = 12, label='actual langing point')
"""
elif self.launch_location == 'noshiro_sea':
#for NOSHIRO SEA!!
# Set limit range in maps
self.set_coordinate_noshiro()
# Set map image
img_map = Image.open("./map/noshiro_new_rotate.png")
img_list = np.asarray(img_map)
img_height = img_map.size[1]
# print(img_map.size)
img_width = img_map.size[0]
img_origin = np.array(
[894, 647]) # TODO : compute by lat/long of launcher point
#pixel2meter
pixel2meter = 8.96708
# Define image range
img_left = -1.0 * img_origin[0] * pixel2meter
img_right = (img_width - img_origin[0]) * pixel2meter
img_top = img_origin[1] * pixel2meter
img_bottom = -1.0 * (img_height - img_origin[1]) * pixel2meter
#calculate intersections of "inside_circle" and "over_line"
center1 = sg.Point(self.xy_center[0], self.xy_center[1])
radius1 = self.hachiya_radius
circle1 = sg.Circle(center1, radius1)
line = sg.Line(sg.Point(self.xy_point[0, 0], self.xy_point[0, 1]),
sg.Point(self.xy_point[1, 0], self.xy_point[1, 1]))
result1 = sg.intersection(circle1, line)
intersection1_1 = np.array(
[float(result1[0].x), float(result1[0].y)])
intersection1_2 = np.array(
[float(result1[1].x), float(result1[1].y)])
#caluculate equation of hachiya_line(="over_line")
self.a = (self.xy_point[1, 1] - self.xy_point[0, 1]) / (
self.xy_point[1, 0] - self.xy_point[0, 0])
self.b = (self.xy_point[0, 1] * self.xy_point[1, 0] -
self.xy_point[1, 1] * self.xy_point[0, 0]) / (
self.xy_point[1, 0] - self.xy_point[0, 0])
self.x = np.arange(intersection1_1[0], intersection1_2[0], 1)
self.y = self.a * self.x + self.b
self.hachiya_line = np.array([self.a, self.b])
# plot setting
plt.figure(figsize=(10, 10))
ax = plt.axes()
color_line = '#ffff33' # Yellow
color_circle = 'r' # Red
# Set circle object
cir_rail = patches.Circle(xy=self.xy_rail,
radius=self.lim_radius,
ec=color_line,
fill=False)
#cir_switch = patches.Circle(xy=self.xy_switch, radius=self.lim_radius, ec=color_circle, fill=False)
#cir_tent = patches.Circle(xy=self.xy_tent, radius=self.lim_radius, ec=color_circle, fill=False)
cir_center = patches.Circle(xy=self.xy_center,
radius=self.hachiya_radius,
ec=color_circle,
fill=False)
ax.add_patch(cir_rail)
#ax.add_patch(cir_switch)
#ax.add_patch(cir_tent)
ax.add_patch(cir_center)
# plot map
plt.imshow(img_list,
extent=(img_left, img_right, img_bottom, img_top))
# Write landing permission range
plt.plot(self.x, self.y, "r")
plt.plot(self.xy_rail[0], self.xy_rail[1], '.', color=color_circle)
#plt.plot(self.xy_switch[0], self.xy_switch[1], '.', color=color_circle)
#plt.plot(self.xy_tent[0], self.xy_tent[1], '.', color=color_circle)
#plt.plot(self.xy_range[:,0], self.xy_range[:,1], '--', color=color_line)
plt.plot(self.xy_center[0],
self.xy_center[1],
'.',
color=color_circle)
else:
raise NotImplementedError(
'Available location is: izu or noshiro_sea')
return None