def frame(self, other=g3d.Vector(0.0, 0.0, 1.0, True)):
""" return a frame where z is oriented toward other (by default north):
* no solution is returned if self and other are colinears
* if other is not specified, v_north is used as other
* in this case, the frame is not defined at poles
"""
if other is None:
""" return an arbitrarily oriented frame optimized to reduce numerical errors,
the frame can be defined everywhere """
c_tpl = self.normal.as_tuple
c_max = max(range(len(c_tpl)), key=lambda i: c_tpl[i])
c_lst = list(c_tpl)
c_lst[c_max] = 0
num = sum(c_lst)
den = c_tpl[c_max]
y_lst = [1.0, 1.0, 1.0]
y_lst[c_max] = -num / den
x = self.normal
y = g3d.Vector(*y_lst).normalized()
z = x @ y
else:
x = self.normal
y = (other @ x).normalized()
z = x @ y
return y, z
def as_vector(self):
theta = (math.pi / 2) - math.radians(self.lat)
phi = math.radians(self.lon)
sin_theta, cos_theta = math.sin(theta), math.cos(theta)
sin_phi, cos_phi = math.sin(phi), math.cos(phi)
v = g3d.Vector(sin_theta * cos_phi, sin_theta * sin_phi, cos_theta,
True)
# the vector must be on the unit sphere
assert (math.isclose(v.norm, 1.0))
return v
def step(self, vx=None, vz=None, turn_value=0.0, turn_mode="psid"):
if vx is None:
vx = self.vx
if vz is None:
vz = self.vz
if turn_mode == "psid":
psid = turn_value
rol = math.atan(psid * vx / earth_gravity)
elif turn_mode == "rol":
rol = turn_value
psid = earth_gravity * math.tan(rol) / self.vx
else:
raise ValueError
self.psi += psid * self.dt
px = goto.globe.blip.Blip(self.lat, self.lon).to_vector()
north = g3d.Vector(0.0, 0.0, 1.0, True)
nx = px
ny = (north @ nx).normalized()
nz = (nx @ ny)
hx = nx
hz = math.cos(self.psi) * nz + math.sin(self.psi) * ny
hy = (hz @ hx)
# l'angle d'avancement autour de la terre est donné par :
alpha = (self.vx * self.dt) / (earth_radius + self.alt +
0.5 * self.vz * self.dt)
jx = math.cos(alpha) * hx + math.sin(alpha) * hz
b = goto.globe.blip.Blip.from_vector(jx)
self.lat = b.lat
self.lon = b.lon
self.alt += self.vz * self.dt
self.t_ms += self.dt_ms
self.t = self.t_ms / 1000.0
self.record()
def new_symbolic(a, b):
u = LineSegment(g3d.Vector(0.0, 0.0, 1.0, True),
g3d.Vector(1.0, 0.0, 0.0, True))
u.A = g3d.Vector(*sympy.symbols(f'{a}_x {a}_y {a}_z'))
u.B = g3d.Vector(*sympy.symbols(f'{a}_x {a}_y {a}_z'))
u.Lx = g3d.Vector(
*sympy.symbols(f'{a}{b}^Lx_x {a}{b}^Lx_y {a}{b}^Lx_z'))
u.Ly = g3d.Vector(
*sympy.symbols(f'{a}{b}^Ly_x {a}{b}^Ly_y {a}{b}^Ly_z'))
u.Lz = g3d.Vector(
*sympy.symbols(f'{a}{b}^Lz_x {a}{b}^Lz_y {a}{b}^Lz_z'))
u.length = sympy.symbols(f'{a}{b}^d')
return u
class LineSegment():
""" plus de blip ici !!! que des vecteurs unitaires !!! """
north = g3d.Vector(0.0, 0.0, 1.0, True)
def __init__(self, A: g3d.Vector, B: g3d.Vector):
self.A = A.normalized()
self.B = B.normalized()
# frame, local to A, oriented along AB
self.Lx = self.A
self.Lz = (self.A @ self.B).normalized(
) # z is perpendicular to the the trajectory disk
self.Ly = (self.Lz @ self.Lx
) # y is perpendicular to z and x, along the line
self.length = self.A.angle_to(self.B) # angle/distance between a and b
@staticmethod
def new_symbolic(a, b):
u = LineSegment(g3d.Vector(0.0, 0.0, 1.0, True),
g3d.Vector(1.0, 0.0, 0.0, True))
u.A = g3d.Vector(*sympy.symbols(f'{a}_x {a}_y {a}_z'))
u.B = g3d.Vector(*sympy.symbols(f'{a}_x {a}_y {a}_z'))
u.Lx = g3d.Vector(
*sympy.symbols(f'{a}{b}^Lx_x {a}{b}^Lx_y {a}{b}^Lx_z'))
u.Ly = g3d.Vector(
*sympy.symbols(f'{a}{b}^Ly_x {a}{b}^Ly_y {a}{b}^Ly_z'))
u.Lz = g3d.Vector(
*sympy.symbols(f'{a}{b}^Lz_x {a}{b}^Lz_y {a}{b}^Lz_z'))
u.length = sympy.symbols(f'{a}{b}^d')
return u
def intersection(self, other):
return (self.Lz @ other.Lz).normalized()
def side_point(self, t, d):
Px, Py, Pz = self.progress_frame(t)
return Px.deviate(Pz, -d)
def progress_point(self, t: float):
return self.Lx.deviate(self.Ly, t * self.length)
def progress_frame(self, t: float):
Px = self.progress_point(t)
Pz = self.Lz
Py = Pz @ Px
return Px, Py, Pz
def projected_frame(self, Mx: g3d.Vector):
""" return Px, Py Pz where Px is the projection of Mx on the Line
"""
Pz = self.Lz
Py = (Pz @ Mx).normalized()
Px = (Py @ Pz) # the projection
return Px, Py, Pz
def projection(self, Mx: g3d.Vector):
""" return Px, Py Pz where Px is the projection of Mx on the Line
"""
Pz = self.Lz
Py = (Pz @ Mx).normalized()
Px = (Py @ Pz) # the projection
return Px
def surface_angle(self, other):
""" from one line to another, what is the angle """
assert (self.B == other.A)
return -self.Ly.angle_to(other.Ly, self.B)
def status(self, Mx: g3d.Vector):
""" blip_m is the real position of the aircraft, maybe not exactly on the the line
this function returns:
* p, the blip of the aircraft as projected orthogonally onto the line
* t, the relative position of p on the line ab (between 0.0 and 1.0)
* h, the true heading to follow at p in order to stay on the line
* q, the distance between m and p
"""
Lx, Ly, Lz = self.Lx, self.Ly, self.Lz
print(f"Lx = {Lx}")
print(f"Ly = {Ly}")
print(f"Lz = {Lz}")
# frame, local to P, oriented along AB
Pz = Lz
Py = (Pz @ Mx).normalized()
Px = (Py @ Pz) # the projection, not normalized
print(f"Px = {Px}")
print(f"Py = {Py}")
print(f"Pz = {Pz}")
t = Lx.angle_to(Px, Lz) / self.length # the progress
# frame, local to P, oriented to the north
Nx = Px
Ny = (g3d.v_north @ Nx).normalized()
Nz = (Nx @ Ny)
h = math.degrees(Py.angle_to(Nz, Px))
d = Mx.angle_to(Px)
return Px, t, h, d
sys.exit(0)
val = {
'A_x': A.x, 'A_y': A.y, 'A_z': A.z,
'B_x': B.x, 'B_y': B.y, 'B_z': B.z,
'I_x': I.x, 'I_y': I.y, 'I_z': I.z,
'Q_x': Q.x, 'Q_y': Q.y, 'Q_z': Q.z,
}
# symbolic part
t = sympy.symbols('t')
A = g3d.Vector( * sympy.symbols('A_x A_y A_z'), True )
B = g3d.Vector( * sympy.symbols('B_x B_y B_z'), True )
C = g3d.Vector( * sympy.symbols('C_x C_y C_z'), True )
I = g3d.Vector( * sympy.symbols('I_x I_y I_z'), True )
Q = g3d.Vector( * sympy.symbols('Q_x Q_y Q_z'), True )
V = B * sympy.cos(t) + Q * sympy.sin(t)
Pab_Z = g3d.Vector( * sympy.symbols('PabZx PabZy PabZz'), True )
Pab_Y = (Pab_Z @ V).normalized()
Pab_X = (Pab_Y @ Pab_Z)
left_angle = (Pab_X * V)
center_angle = (I * V)