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)