def turn_3pt(self, A, B, C, d):
line_AB = LineSegment(A, B)
line_BC = LineSegment(B, C)
q = line_AB.Ly.angle_to(line_BC.Ly, B)
Q = (line_AB.Ly + line_BC.Ly).normalized()
w = math.copysign(1.0, q)
R1 = -(A * Q)**2 / (A.y**2 * (-1 + B.y**2) +
2 * A.x * A.z * B.x * B.z + 2 * A.y * B.y *
(A.x * B.x + A.z * B.z) + A.z**2 *
(-1 + B.z**2) - A.x**2 * (B.y**2 + B.z**2))
t = math.acos(math.sqrt(math.cos(d)**2 - R1) / math.sqrt(1 - R1))
ABCz = w * (line_AB.Lz + line_BC.Lz).normalized()
V = B.deviate(ABCz, t)
E = line_AB.projection(V)
F = line_BC.projection(V)
VEa = V.angle_to(E)
VFa = V.angle_to(F)
assert (math.isclose(VEa, VFa, rel_tol=1e-4))
BEp = B.angle_to(E) / line_AB.length
BFp = B.angle_to(F) / line_BC.length
return Blip.from_vector(E), Blip.from_vector(F), VEa, BEp, BFp
def joint(self, A, B, C, wa, wb, wc, radius_max):
line_AB = LineCorridor(A, B, wa / earth_radius, wb / earth_radius)
line_BC = LineCorridor(B, C, wb / earth_radius, wc / earth_radius)
E, F, V, VEa, AEp, BFp = line_AB.make_joint(line_BC)
return Blip.from_vector(E), Blip.from_vector(F), VEa, AEp
def turn_4pt(self, A, B, C, D):
line_AB = LineSegment(A, B)
line_BC = LineSegment(B, C)
line_CD = LineSegment(C, D)
ABCa = line_AB.surface_angle(line_BC)
BCDa = line_BC.surface_angle(line_CD)
w1 = math.copysign(1.0, ABCa)
w2 = math.copysign(1.0, BCDa)
assert (0 < w1 * w2)
ABCz = -(line_AB.Lz + line_BC.Lz).normalized()
BCDz = -(line_BC.Lz + line_CD.Lz).normalized()
ABCy = B @ ABCz
BCDy = C @ BCDz
# the center of the circle inscribed is V
V = -(ABCy @ BCDy).normalized() * math.copysign(1.0, ABCa)
E = line_AB.projection(V)
F = line_BC.projection(V)
G = line_CD.projection(V)
VEa = V.angle_to(E)
VFa = V.angle_to(F)
VGa = V.angle_to(G)
assert (math.isclose(VEa, VFa, rel_tol=1e-4)
and math.isclose(VFa, VGa, rel_tol=1e-4)
and math.isclose(VGa, VEa, rel_tol=1e-4))
AEp = A.angle_to(E) / line_AB.length
BFp = B.angle_to(F) / line_BC.length
CGp = C.angle_to(G) / line_CD.length
return Blip.from_vector(E), Blip.from_vector(F), Blip.from_vector(
G), w1 * VFa, AEp, BFp, CGp
def turn_3pt(self, A, B, C, radius) : line_AB = LineTo(A, B) line_BC = LineTo(B, C) ABCa = line_AB.surfacea(line_BC) ABCz = - (line_AB.Lz + line_BC.Lz).normalized() V = B.deviate(ABCz, radius) E = line_AB.projection(V) F = line_BC.projection(V) VEa = V.angle_to(E) VFa = V.angle_to(F) BEp = B.angle(E) / line_AB.angle_ab BFp = B.angle(F) / line_EF assert( math.isclose(VEa, VFa) ) return Blip.from_vector(E), Blip.from_vector(F), VEa
def pass_2(self, r_prev):
r_next = list()
a_lst = [line[4] for line in r_prev]
for i, line in enumerate(r_prev):
verb, lat, lon, radius, alt, spd, width = line
if verb == 'GOTO' and radius == 0.0 and r_prev[
i + 1][0] == 'GOTO' and r_prev[i + 1][3] == 0.0:
A = Blip(r_prev[i - 1][1], r_prev[i - 1][2]).as_vector
B = Blip(lat, lon).as_vector
C = Blip(r_prev[i + 1][1], r_prev[i + 1][2]).as_vector
E, F, VEa, AEp = self.joint(A, B, C, r_prev[i - 1][6],
r_prev[i][6], r_prev[i + 1][6],
400.0)
r_next.append([
"GOTO", E.lat, E.lon, 0,
interpolate(AEp, a_lst[i], a_lst[i + 1]), spd, width
])
r_next.append([
"JOINT", F.lat, F.lon, VEa * earth_radius,
interpolate(AEp, a_lst[i], a_lst[i + 1]), spd, width
])
else:
r_next.append(line)
return r_next
def pass_1(self, r_prev):
r_next = list()
a_lst = [line[4] for line in r_prev]
for i, line in enumerate(r_prev):
verb, lat, lon, radius, alt, spd, width = line
if verb == '__TURN_3PT__':
A = Blip(r_prev[i - 1][1], r_prev[i - 1][2]).as_vector
B = Blip(lat, lon).as_vector
C = Blip(r_prev[i + 1][1], r_prev[i + 1][2]).as_vector
r = radius / earth_radius
E, F, VEa, AEp, BFp = self.turn_3pt(A, B, C, r)
r_next.append([
"GOTO", E.lat, E.lon, 0,
interpolate(AEp, a_lst[i], a_lst[i + 1]), spd, width
])
r_next.append([
"GOTO", F.lat, F.lon, VEa * earth_radius,
interpolate(BFp, a_lst[i], a_lst[i + 1]), spd, width
])
elif verb == '__TURN_4PT_1__':
A = Blip(r_prev[i - 1][1], r_prev[i - 1][2]).as_vector
B = Blip(lat, lon).as_vector
C = Blip(r_prev[i + 1][1], r_prev[i + 1][2]).as_vector
D = Blip(r_prev[i + 2][1], r_prev[i + 2][2]).as_vector
E, F, G, VFa, AEp, BFp, CGp = self.turn_4pt(A, B, C, D)
r_next.append([
"GOTO", E.lat, E.lon, 0.0,
interpolate(AEp, a_lst[i - 1], a_lst[i + 1]), spd, width
])
r_next.append([
"GOTO", F.lat, F.lon, VFa * earth_radius,
interpolate(BFp, a_lst[i], a_lst[i + 1]), spd, width
])
r_next.append([
"GOTO", G.lat, G.lon, VFa * earth_radius,
interpolate(CGp, a_lst[i + 1], a_lst[i + 2]), spd, width
])
elif verb == '__TURN_4PT_2__':
pass # this point was treated previously with __TURN_4PT_1__
else:
r_next.append(line)
return r_next
V = w * (Jy @ Iy).normalized()
with GlobePlotMpl() as plt:
plt.add_point(A, 'A', 'r')
plt.add_point(B, 'B', 'g')
plt.add_point(C, 'C', 'b')
plt.add_point(Ix, 'Ix', 'orange')
plt.add_point(Iy, 'Iy', 'yellow')
plt.add_point(Iz, 'Iz', 'orange')
plt.add_point(Jx, 'Jx', 'dodgerblue')
plt.add_point(Jy, 'Jy', 'cyan')
plt.add_point(Jz, 'Jz', 'dodgerblue')
plt.add_point(V, 'v', 'magenta')
plt.add_circle(V, B)
return V
if __name__ == '__main__':
from goto.globe.blip import Blip
A = Blip(0.0, 0.0).as_vector
B = Blip(30.0, 0.0).as_vector
C = Blip(35.0, -15.0).as_vector
circle_find_center(A, B, C)
def read_wskpt(verb, lat, lon, radius, alt, spd, width):
return WskPt(verb, Blip(lat, lon).as_vector, radius, alt, spd, width)
#!/usr/bin/env python3
import collections
from cc_pathlib import Path
from goto.globe.blip import Blip
WskPt = collections.namedtuple('WskPt', ['verb', 'pos', 'radius', 'alt', 'spd', 'width'])
w_lst = list()
for p in Path("source.json").load() :
w = WskPt(("GOTO" if len(w_lst) else "START"), Blip(p['lat'], p['lon']), p['leg_radius'], p['alt'], p['speed'], p['corridor_radius'])
w_lst.append([w.verb, w.pos.lat, w.pos.lon, -w.radius, w.alt, w.spd, w.width])
Path("2_effective.json").save(w_lst, filter_opt={'verbose':True})
s = math.copysign(1.0, s)
Px, Py, Pz = self.progress_frame(t)
w = self.a_width * (1 - t) + self.b_width * t
Pm = Pz.deviate(Py, d * math.pi * s)
return Px.deviate(Pm, w)
if __name__ == '__main__':
from cc_pathlib import Path
from goto.globe.blip import Blip
from goto.globe.plot import GlobePlotMpl, GlobePlotGps
A = Blip(-30.0, 0.0).as_vector
B = Blip(30.0, 0.0).as_vector
C = Blip(0.0, 15.0).as_vector
u = ArcCorridor(A, B, C, 0.04, 0.06)
# with GlobePlotMpl() as plt :
# plt.add_point(A, 'A', 'r')
# plt.add_point(B, 'B', 'g')
# plt.add_point(C, 'C', 'b')
# plt.add_point(u.Vx, 'V', 'k')
# plt.add_segment(u)
# plt.add_border(u, "orange")
with GlobePlotGps(Path("test.plot.json")) as plt:
plt.add_point(A, 'A')