def initFromStateVectors(self,epoch,pV,vV):
self.epoch = epoch
# 1) Calculate auxilary vector h
hV = cross(pV,vV)
# 2) Normalize position,velocity, specific angular momentum, calculate radial velocity
p = linalg.norm(pV)
v = linalg.norm(vV)
h = linalg.norm(hV)
print "H:",h
radv = pV.dot(vV) / p
hVu = hV / h
pVu = pV / p
nV = cross(array([0,0,1]),hV)
n = linalg.norm(nV)
if n == 0:
nVu = array([0,0,0])
else:
nVu = nV/n
# 3) Calculate inclination
#self.i = arccos(hV[2]/h)
self.i = arcsin(linalg.norm(cross(array([0,0,1]),hVu)))
print "i1",self.i
print "RADVEL",radv
self.i = arccos(array([0,0,1]).dot(hV)/h)
#if radv < 0:
# self.i = PI2 - self.i
print "i2",self.i
# 4) Calculate node line
# 5) Calculate longitude of ascending node = right ascension of ascending node
'''
if self.i == 0:
self.lan=0
elif nV[1] >= 0:
self.lan = arccos(nV[0] / n)
else:
self.lan = PI2 - arccos(nV[0] / n)
'''
if self.i == 0:
self.lan = 0
else:
self.lan = arcsin(cross(array([1,0,0]),nVu).dot(array([0,0,1])))
print "lan1",self.lan
self.lan = arccos(array([1,0,0]).dot(nV)/n)
if nV[1] < 0:
self.lan = PI2-self.lan
print "lan2",self.lan
# 6) Eccentricity vector
#eV = (1.0 / self.ref.mu)*((v**2 - (self.ref.mu / p))*pV - radv*vV)
#eV2 = (1.0 / self.ref.mu) * ( hV - self.ref.mu * (pV/p))
#eV3 = hV/self.ref.mu - (pV/p)
# Source: cdeagle
eV = cross(vV,hV)/self.ref.mu - pVu
#print "eV1:",eV,linalg.norm(eV)
#print "eV2:",eV2,linalg.norm(eV2)
#print "eV3:",eV3,linalg.norm(eV3)
print "eV3:",eV,linalg.norm(eV)
self._e = linalg.norm(eV)
#eVu = eV / self.e
print "h",h
print "u",self.ref.mu
print "v",v
print "r",p
print "alte:",sqrt(1+(h**2/self.ref.mu**2)*(v**2-(2*self.ref.mu)/p)**2)
# 7) Argument of perigree
'''
if self.e == 0:
self.aop = 0
elif self.i == 0:
self.aop = arccos(eV[0] / self.e)
elif eV[2] >= 0:
print "AOP AOP AOP"
#self.aop = arccos(nV.dot(eV) / (n*self.e))
print cross(nV,eV).dot(hV)
self.aop = arcsin(cross(nVu,eVu).dot(hVu))
#self.aop = arccos(n*self.e)
else:
self.aop = PI2 - arccos(nV.dot(eV) / (n*self.e))
'''
#CDEagle method
# TODO CHECK how KSP handles this.
if self.e == 0:
self.aop = 0
elif self.i == 0 and self.e != 0:
#self.aop = arccos(eV[0] / self.e)
#self.aop = arctan2(eV[1],eV[0])
self.aop = arccos(array([1,0,0]).dot(eV) / self.e)
print eV
if eV[2] < 0:
#self.aop = -self.aop
self.aop = PI2-self.aop
#print "BOOM",eV
#if eV[2] < 0:
# print "BAM N***A"
# self.aop = PI2 - self.aop
elif self.i == 0 and self.e == 0:
#raise AttributeError("Perfectly circular orbits are not supported atm")
self.aop = 0
else:
#self.aop = arcsin(cross(nVu,eVu).dot(hVu))
self.aop = arccos(nV.dot(eV)/(n*self.e))
if eV[2] < 0:
self.aop = PI2-self.aop
# 8) Semi major axis
aE = v**2/2.0 - self.ref.mu / p
self._a = -self.ref.mu / (2*aE)
print "Old method for semi-major",self.a
self._a = h**2 / (self.ref.mu * (1-self.e**2))
print "New method for semi-major",self.a
#if self.e > 1:
# self._a = h**2 / (self.ref.mu * (self.e**2 - 1))
if self.e == 0:
if self.i == 0: #TODO update document to this
print "JEA JEA JEA JEA"*10
ta = arccos(array([1,0,0]).dot(pV) / p)
if pV[1] < 0: # Vallado pg. 111
ta = PI2 - ta
else: #TODO VERIFY THIS CASE
ta = arccos((nV.dot(pV))/(n*p))
if pV[2] < 0: # Vallado pg. 110
ta = PI2 - ta
E = ta
self.M0 = E
elif self.e < 1:
# 9) True anomaly, eccentric anomaly and mean anomaly
if radv >= 0:
ta = arccos((eV / self.e).dot(pV/p))
else:
ta = PI2 - arccos((eV / self.e).dot(pV/p))
E = arccos((self.e+cos(ta))/(1+ self.e*cos(ta)))
if radv < 0:
E = PI2 - E
self.M0 = E - self.e * sin(E)
elif self.e > 1:
# 9) Hyperbolic True anomaly, eccentric anomaly and mean anomaly
# http://scienceworld.wolfram.com/physics/HyperbolicOrbit.html
V = arccos((abs(self.a)*(self.e**2 - 1)) /(self.e * p) - 1/self.e)
ta = arccos((eV / self.e).dot(pV/p))
if radv < 0: #TODO: Should affect F too?
# Negative = heading towards periapsis
print "PI2"
V = PI2 - V
ta = PI2-ta
print "V",V
print "TA",ta
# http://www.bogan.ca/orbits/kepler/orbteqtn.html In you I trust
# Hyperbolic eccentric anomaly
cosV = cos(V)
F = arccosh((self.e+cosV)/(1+self.e*cosV))
if radv < 0:
F = -F
F2 = arcsinh((sqrt(self.e-1)*sin(V))/(1+self.e*cos(V)))
##F1 = F2
print "F1:",F
print "F2:",F2
self.M0 = self.e * sinh(F) - F
self.h = h
print "Semi-major:",self.a
print "Eccentricity:",self.e
print "Inclination:",degrees(self.i),"deg"
print "LAN:",degrees(self.lan),"deg"
print "AoP:",degrees(self.aop),"deg"
print "Mean anomaly:",self.M0
print "Specific angular momentum:",self.h
if self.e < 1:
print "Eccentric anomaly",E
print "True anomaly",ta
else:
print "Hyperbolic eccentric anomaly",F
print "Hyperbolic true anomaly",degrees(V)
print "Distance from object:",p
print "Velocity:",v
def lambert(r1vec,r2vec,tf,m,muC): # original documentation: # ············································· # # This routine implements a new algorithm that solves Lambert's problem. The # algorithm has two major characteristics that makes it favorable to other # existing ones. # # 1) It describes the generic orbit solution of the boundary condition # problem through the variable X=log(1+cos(alpha/2)). By doing so the # graph of the time of flight become defined in the entire real axis and # resembles a straight line. Convergence is granted within few iterations # for all the possible geometries (except, of course, when the transfer # angle is zero). When multiple revolutions are considered the variable is # X=tan(cos(alpha/2)*pi/2). # # 2) Once the orbit has been determined in the plane, this routine # evaluates the velocity vectors at the two points in a way that is not # singular for the transfer angle approaching to pi (Lagrange coefficient # based methods are numerically not well suited for this purpose). # # As a result Lambert's problem is solved (with multiple revolutions # being accounted for) with the same computational effort for all # possible geometries. The case of near 180 transfers is also solved # efficiently. # # We note here that even when the transfer angle is exactly equal to pi # the algorithm does solve the problem in the plane (it finds X), but it # is not able to evaluate the plane in which the orbit lies. A solution # to this would be to provide the direction of the plane containing the # transfer orbit from outside. This has not been implemented in this # routine since such a direction would depend on which application the # transfer is going to be used in. # # please report bugs to [email protected] # # adjusted documentation: # ······················· # # By default, the short-way solution is computed. The long way solution # may be requested by giving a negative value to the corresponding # time-of-flight [tf]. # # For problems with |m| > 0, there are generally two solutions. By # default, the right branch solution will be returned. The left branch # may be requested by giving a negative value to the corresponding # number of complete revolutions [m]. # Authors # .·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·.·`·. # Name : Dr. Dario Izzo # E-mail : [email protected] # Affiliation: ESA / Advanced Concepts Team (ACT) # Made readible and optimized for speed by Rody P.S. Oldenhuis # Code available in MGA.M on http://www.esa.int/gsp/ACT/inf/op/globopt.htm # last edited 12/Dec/2009 # ADJUSTED FOR EML-COMPILATION 24/Dec/2009 # initial values tol = 1e-12 bad = False days = 1 # work with non-dimensional units r1 = norm(r1vec) #sqrt(r1vec*r1vec.'); r1vec = r1vec/r1; r1vec = r1vec / r1 r2vec = r2vec / r1 V = sqrt(muC/r1) T = r1/V tf= tf*days/T # also transform to seconds # relevant geometry parameters (non dimensional) mr2vec = norm(r2vec) # make 100# sure it's in (-1 <= dth <= +1) dth = arccos( max(-1, min(1, (r1vec.dot(r2vec)/mr2vec)))) # decide whether to use the left or right branch (for multi-revolution # problems), and the long- or short way leftbranch = sign(m) longway = sign(tf) m = abs(m) tf = abs(tf) if (longway < 0): dth = 2*pi - dth # derived quantities c = sqrt(1.0 + mr2vec**2 - 2*mr2vec*cos(dth)) # non-dimensional chord s = (1.0 + mr2vec + c)/2.0 # non-dimensional semi-perimeter a_min = s/2.0 # minimum energy ellipse semi major axis Lambda = sqrt(mr2vec)*cos(dth/2.0)/s # lambda parameter (from BATTIN's book) crossprd = cross(r1vec,r2vec) mcr = norm(crossprd) # magnitues thereof nrmunit = crossprd/mcr # unit vector thereof # Initial values # ························································· # ELMEX requires this variable to be declared OUTSIDE the IF-statement logt = log(tf); # avoid re-computing the same value # single revolution (1 solution) if (m == 0): # initial values inn1 = -0.5233 # first initial guess inn2 = +0.5233 # second initial guess x1 = log(1 + inn1)# transformed first initial guess x2 = log(1 + inn2)# transformed first second guess # multiple revolutions (0, 1 or 2 solutions) # the returned soltuion depends on the sign of [m] else: # select initial values if (leftbranch < 0): inn1 = -0.5234 # first initial guess, left branch inn2 = -0.2234 # second initial guess, left branch else: inn1 = +0.7234 # first initial guess, right branch inn2 = +0.5234 # second initial guess, right branch x1 = tan(inn1*pi/2)# transformed first initial guess x2 = tan(inn2*pi/2)# transformed first second guess # since (inn1, inn2) < 0, initial estimate is always ellipse xx = array([inn1, inn2]) aa = a_min/(1 - xx**2) bbeta = longway * 2*arcsin(sqrt((s-c)/2./aa)) # make 100.4% sure it's in (-1 <= xx <= +1) if xx[0] > 1: xx[0] = 1 if xx[0] < -1: xx[0] = -1 if xx[1] > 1: xx[1] = 1 if xx[1] < -1: xx[1] = -1 aalfa = 2*arccos( xx ) # evaluate the time of flight via Lagrange expression y12 = aa*sqrt(aa)*((aalfa - sin(aalfa)) - (bbeta-sin(bbeta)) + 2*pi*m) # initial estimates for y if m == 0: y1 = log(y12[0]) - logt y2 = log(y12[1]) - logt else: y1 = y12[0] - tf y2 = y12[1] - tf # Solve for x # ························································· # Newton-Raphson iterations # NOTE - the number of iterations will go to infinity in case # m > 0 and there is no solution. Start the other routine in # that case err = 1e99 iterations = 0 xnew = 0 while (err > tol): # increment number of iterations iterations += 1 # new x xnew = (x1*y2 - y1*x2) / (y2-y1); # copy-pasted code (for performance) if m == 0: x = exp(xnew) - 1 else: x = arctan(xnew)*2/pi a = a_min/(1 - x**2); if (x < 1): # ellipse beta = longway * 2*arcsin(sqrt((s-c)/2/a)) # make 100.4% sure it's in (-1 <= xx <= +1) alfa = 2*arccos( max(-1, min(1, x)) ) else: # hyperbola alfa = 2*arccosh(x); beta = longway * 2*arcsinh(sqrt((s-c)/(-2*a))) # evaluate the time of flight via Lagrange expression if (a > 0): tof = a*sqrt(a)*((alfa - sin(alfa)) - (beta-sin(beta)) + 2*pi*m) else: tof = -a*sqrt(-a)*((sinh(alfa) - alfa) - (sinh(beta) - beta)) # new value of y if m ==0: ynew = log(tof) - logt else: ynew = tof - tf # save previous and current values for the next iterarion # (prevents getting stuck between two values) x1 = x2; x2 = xnew; y1 = y2; y2 = ynew; # update error err = abs(x1 - xnew); # escape clause if (iterations > 15): bad = True break # If the Newton-Raphson scheme failed, try to solve the problem # with the other Lambert targeter. if bad: # NOTE: use the original, UN-normalized quantities #[V1, V2, extremal_distances, exitflag] = ... # lambert_high_LancasterBlanchard(r1vec*r1, r2vec*r1, longway*tf*T, leftbranch*m, muC); print "FAILZ0r" return # convert converged value of x if m==0: x = exp(xnew) - 1 else: x = arctan(xnew)*2/pi #{ # The solution has been evaluated in terms of log(x+1) or tan(x*pi/2), we # now need the conic. As for transfer angles near to pi the Lagrange- # coefficients technique goes singular (dg approaches a zero/zero that is # numerically bad) we here use a different technique for those cases. When # the transfer angle is exactly equal to pi, then the ih unit vector is not # determined. The remaining equations, though, are still valid. #} # Solution for the semi-major axis a = a_min/(1-x**2); # Calculate psi if (x < 1): # ellipse beta = longway * 2*arcsin(sqrt((s-c)/2/a)) # make 100.4# sure it's in (-1 <= xx <= +1) alfa = 2*arccos( max(-1, min(1, x)) ) psi = (alfa-beta)/2 eta2 = 2*a*sin(psi)**2/s eta = sqrt(eta2); else: # hyperbola beta = longway * 2*arcsinh(sqrt((c-s)/2/a)) alfa = 2*arccosh(x) psi = (alfa-beta)/2 eta2 = -2*a*sinh(psi)**2/s eta = sqrt(eta2) # unit of the normalized normal vector ih = longway * nrmunit; # unit vector for normalized [r2vec] r2n = r2vec/mr2vec; # cross-products # don't use cross() (emlmex() would try to compile it, and this way it # also does not create any additional overhead) #crsprd1 = [ih(2)*r1vec(3)-ih(3)*r1vec(2),... # ih(3)*r1vec(1)-ih(1)*r1vec(3),... # ih(1)*r1vec(2)-ih(2)*r1vec(1)]; crsprd1 = cross(ih,r1vec) #crsprd2 = [ih(2)*r2n(3)-ih(3)*r2n(2),... # ih(3)*r2n(1)-ih(1)*r2n(3),... # ih(1)*r2n(2)-ih(2)*r2n(1)]; crsprd2 = cross(ih,r2n) # radial and tangential directions for departure velocity Vr1 = 1/eta/sqrt(a_min) * (2*Lambda*a_min - Lambda - x*eta) Vt1 = sqrt(mr2vec/a_min/eta2 * sin(dth/2)**2) # radial and tangential directions for arrival velocity Vt2 = Vt1/mr2vec Vr2 = (Vt1 - Vt2)/tan(dth/2) - Vr1 # terminal velocities V1 = (Vr1*r1vec + Vt1*crsprd1)*V V2 = (Vr2*r2n + Vt2*crsprd2)*V # exitflag #exitflag = 1 # (success) #print "V1:",V1 #print "V2:",V2 return V1,V2
