def odot(gm, sma, ecc, ean): rad = r(sma, ecc, ean) if ecc > 1.0: sf = math.sqrt(-gm * sma) / rad efac = math.sqrt(ecc ** 2 - 1.0) return sf * matrix.Vector3((-math.sinh(ean), efac * math.cosh(ean), 0)) sf = math.sqrt(gm * sma) / rad efac = math.sqrt(1.0 - ecc ** 2) return sf * matrix.Vector3((-math.sin(ean), efac * math.cos(ean), 0))
def __init__(self, dl, cw): super(NavBall, self).__init__(dl, cw) self.add_prop('pit', 'n.pitch2') self.add_prop('hdg', 'n.heading2') self.add_prop('rll', 'n.roll2') self.cardinals = { 'N': matrix.Vector3((1, 0, 0)), 'S': matrix.Vector3((-1, 0, 0)), 'E': matrix.Vector3((0, 1, 0)), 'W': matrix.Vector3((0, -1, 0)), 'O': matrix.Vector3((0, 0, -1)), 'X': matrix.Vector3((0, 0, 1)), } self.size = min(self.height * 2, self.width) - 1 self.half_size = self.size / 2 self.center = (self.width / 2, self.height / 2)
def point(self, pit, hdg): # pointing vector in local co-ordinates pvec = matrix.Vector3((math.sin(pit), math.sin(hdg) * math.cos(pit), math.cos(hdg) * math.cos(pit))) self.pvec = matrix.RotationMatrix(2, self.lon) * matrix.RotationMatrix( 1, -self.lat) * pvec
def ovec(sma, ecc, ean): tan = tan_from_ean(ean, ecc) rad = r(sma, ecc, ean) return rad * matrix.Vector3((math.cos(tan), math.sin(tan), 0))
def compute_3d_elements(self, rvec, vvec): # Compute 3D elements from 3D state vector ### uses eqns from https://downloads.rene-schwarz.com/download/M002-Cartesian_State_Vectors_to_Keplerian_Orbit_Elements.pdf # specific orbital energy, epsilon = v.v / 2 - mu / |r| soe = vvec.dot(vvec) / 2.0 - self.gm / rvec.mag # a = -mu / 2epsilon if soe == 0: # It's parabolic, we may as well give up now # XXX this isn't exactly great, but the odds of hitting a perfectly # parabolic orbit in practice are negligible, so let's punt return {} sma = -self.gm / (2.0 * soe) # specific angular momentum # h = r x v sam = rvec.cross(vvec) # eccentricity vector # e = v x h / mu - rhat evec = (1.0 / self.gm) * vvec.cross(sam) - rvec.hat # eccentricity = |e| ecc = evec.mag apa = (1 + ecc) * sma pea = (1 - ecc) * sma data = {'sma': sma, 'ecc': ecc, 'apa': apa - self.rad, 'pea': pea - self.rad, 'sam': sam} # vector to ascending node n = matrix.Vector3((-sam.y, sam.x, 0)) if sma > 0: # mean motion n = sqrt(mu / a^3) mmo = math.sqrt(self.gm / sma ** 3) data['mmo'] = mmo # period T = 2pi / n per = 2.0 * math.pi / mmo data['per'] = per else: # hyperbolic mean motion n = sqrt(-mu / a^3) mmo = math.sqrt(-self.gm / sma ** 3) data['mmo'] = mmo # anomalies (since periapsis) if ecc == 0: tan = 0 ean = 0 else: # true anomaly nu tan = angle_between(evec.hat, rvec.hat) if vvec.dot(rvec) < 0: tan = 2.0 * math.pi - tan # eccentric anomaly E = 2 arctan(tan(nu/2)/sqrt((1+e)/(1-e))) ean = ean_from_tan(tan, ecc) data['tan'] = tan data['ean'] = ean # mean anomaly M = E - e sin E data['man'] = man_from_ean(ean, ecc) # inclination inc = angle_between(sam.hat, matrix.Vector3.ez()) data['inc'] = inc # longitude of ascending node if n.mag == 0: lan = -tan else: lan = angle_between(n.hat, matrix.Vector3.ex()) if n.y < 0: lan = 2.0 * math.pi - lan data['lan'] = lan # argument of periapsis if ecc == 0: ape = 2.0 * math.pi - lan elif n.mag == 0: ape = 0 else: ape = angle_between(n.hat, evec.hat) if evec.z < 0: ape = 2.0 * math.pi - ape data['ape'] = ape # time to apsides if mmo > 0: if ecc < 1.0: man = math.fmod(data['man'] + 2.0 * math.pi, 2.0 * math.pi) # time to periapsis mtp = 2.0 * math.pi - man ttp = mtp / mmo data['pet'] = ttp # time to apoapsis mta = math.pi - man if mta < 0: mta += 2.0 * math.pi tta = mta / mmo data['apt'] = tta else: # time to periapsis mtp = -data['man'] ttp = mtp / mmo data['pet'] = ttp return data
lanx = matrix.RotationMatrix(2, lan) incx = matrix.RotationMatrix(0, inc) apex = matrix.RotationMatrix(2, ape) return lanx * incx * apex ### def angle_between(w, z): # assumes w and z are unit vectors dot = sum(wi*zi for wi,zi in zip(w.data, z.data)) dot = min(max(dot, -1.0), 1.0) return math.acos(dot) if __name__ == "__main__": # round-trip test in_r = matrix.Vector3((50, 0, 0)) in_v = matrix.Vector3((1.0, 2.0, 0.1)) in_gm = 50 in_rad = 0 in_dt = 1e-5 pbody = ParentBody(in_rad, in_gm) elts = pbody.compute_3d_elements(in_r, in_v) ecc = elts['ecc'] for k,v in elts.iteritems(): if ecc < 1.0 and ('an' in k or k in ('inc', 'ape', 'mmo')): v = math.degrees(v) print k, v print ean = ean_from_tan(elts['tan'], elts['ecc']) out_r, out_v = pbody.compute_3d_vector(elts['sma'], elts['ecc'], ean, elts['ape'], elts['inc'], elts['lan']) print out_r