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