def invE(self,offsetArcLength=None,arcLengthArg=None,rtnIter=False):
"""Find eccentric anomaly corresponding to the input arc length"""
### Parse arguments
assert not (offsetArcLength is not None and arcLengthArg is not None)
if arcLengthArg is None: arcLengthTarget = self.arcChf + offsetArcLength
else : arcLengthTarget = float(arcLengthArg)
### Initial estimate of Eccentric Anomaly
### - highest positive or negative multiple of halfpi that gives
### an arc length less than or equal to targetLength
nQuad = int(arcLengthTarget / self.quadArc)
arcLengthGuess = nQuad * self.quadArc
if arcLengthGuess > arcLengthTarget:
nQuad -= 1
arcLengthGuess -= self.quadArc
eaGuess0 = eaGuess = nQuad * halfpi
eaGuess0PlusTwoPi = eaGuess0 + twopi
deltaArc = arcLengthTarget - arcLengthGuess
tol = self.aChf * 1e-9
itter = 0
while abs(deltaArc) > tol:
itter += 1
### Vector from primary focus to Ecc. Anom. guess
cosea = math.cos(eaGuess)
sinea = math.sin(eaGuess)
x = self.aChf * cosea
y = self.bChf * sinea
### Delta-[x,y] of tangent at (x,y) for distance of deltaArc
delx = -self.aChf * sinea
dely = self.bChf * cosea
fac = deltaArc / sp.vnorm(sp.vpack(delx,dely,0.))
delx *= fac
dely *= fac
### Delta-vector tangent at (x,y) for distance of deltaArc
### - [X,Y,Z] = [x+delx, y+dely, 0]
### - scale X by 1/a and Y by 1/b to get Eccentric Anomaly
eaGuess = sp.recrad(sp.vpack((x+delx)/self.aChf
,(y+dely)/self.bChf
,0.
)
)[iRA]
while eaGuess0 > eaGuess: eaGuess += twopi
while (eaGuess0PlusTwoPi) <= eaGuess: eaGuess -= twopi
arcLengthGuess = self.E(eaGuess)
deltaArc = arcLengthTarget - arcLengthGuess
return rtnIter and (eaGuess,itter,) or eaGuess
def TrueToEccAnom(self,ta):
"""Convert True Anomaly to Eccentric Anomaly
https://en.wikipedia.org/wiki/Eccentric_anomaly#From_the_true_anomaly
"""
return sp.recrad(sp.vpack(self.eccChf + math.cos(ta)
,self.bOverA * math.sin(ta)
,0.
)
)[iRA]
def XYtoTrueAnom(self,x,y): return sp.recrad(sp.vpack(x-self.cChf,y,0.))[iRA]
def inverseDeputy(self,stateDepEqcm=False):
"""Invert depyty-based calculations. Chief-only calculations were
performed in __init__() method above
"""
####################################################################
### Process argmument ...
if stateDepEqcm:
### Use deputy state argument (6-vector), if supplied
rDepEqcm = stateDepEqcm[:3]
velDepEqcm = stateDepEqcm[3:]
else:
### Use result of .Deputy() method if no state is supplied
rDepEqcm = self.rDepEqcm
velDepEqcm = self.velDepEqcm
####################################################################
### Equations (11 and 12) are already done in Equations 1 through 5
### in the constructor __init__() above
### - .mtxEciToRsw1
### - .{r,vel}ChfRsw1
### - .lambdaPerigee
### - .trueAnom1
####################################################################
### Equations (13)
### in the constructor __init__() above
### - .mtxEciToRsw1
self.zinvArcDep = self.arcChf + rDepEqcm[iAeq]
self.zinvEccAnom2 = self.invE(arcLengthArg=self.zinvArcDep)
self.zinvTrueAnom2 = self.XYtoTrueAnom(self.aChf * math.cos(self.zinvEccAnom2)
,self.bChf * math.sin(self.zinvEccAnom2)
)
####################################################################
### Equation (14)
self.zinvDeltaLambdaDep = (twopi + self.zinvTrueAnom2 - self.trueAnom1) % twopi
####################################################################
### Equations (6) repeat from Deputy method, as 2 may be different
### Equivalent chief position at point 2, using PQW2
rChf2 = self.pChf / (1. + (self.eccChf * math.cos(self.zinvTrueAnom2)))
pHat = self.vEccHatChf
qHat = sp.ucrss([0.,0.,1.],pHat)
self.zinvRChfPqw2 = sp.vscl(rChf2,sp.vadd(sp.vscl(math.cos(self.zinvTrueAnom2),pHat),sp.vscl(math.sin(self.zinvTrueAnom2),qHat)))
self.zinvVelChfPqw2 = sp.vscl(math.sqrt(self.mu/self.pChf)
,sp.vadd(sp.vscl( -math.sin(self.zinvTrueAnom2),pHat)
,sp.vscl(self.eccChf+math.cos(self.zinvTrueAnom2),qHat)
)
)
####################################################################
### Equations (7) repeat from Deputy method, as 2 may be different
### Convert from PQW2 to RSW2
self.zinvMtxPqw2toRsw2 = RVtoRSW(self.zinvRChfPqw2,self.zinvVelChfPqw2)
self.zinvRChfRsw2 = sp.mxv(self.zinvMtxPqw2toRsw2,self.zinvRChfPqw2)
self.zinvVelChfRsw2 = sp.mxv(self.zinvMtxPqw2toRsw2,self.zinvVelChfPqw2)
####################################################################
### Equations (15)
self.zinvDeltaPhiDep = rDepEqcm[iZeq] / rChf2
rDepHatRsw1 = sp.radrec(1., self.zinvDeltaLambdaDep, self.zinvDeltaPhiDep)
####################################################################
### - Equations (8) repeat from Deputy method, as deputy may differ
### - Transformation matrix from RSW to SEZ frame
### - Matrix is ROT2[90-deltaPhiDep] ROT3[deltaLambdaDep]
self.zinvMtxRswToSez = sp.eul2m(halfpi-self.zinvDeltaPhiDep,self.zinvDeltaLambdaDep,0.,2,3,1)
####################################################################
### Equations (16)
### - Transform deputy unit vector from RSW1 to SEZ
### - Scale deputy unit vectors in RSW1 and in SEZ
### - factor is X (R) component of deputy EQCM X (R), plus R
### component of chief RSW2
### - Transform deputy vector from RSW1 to ECI (transpose matrix)
rDepHatSez = sp.mxv(self.zinvMtxRswToSez,rDepHatRsw1)
sezScale = (rDepEqcm[iReq] + self.zinvRChfRsw2[iRrsw]) / rDepHatSez[iZsez]
self.zinvRDepSez = sp.vscl(sezScale, rDepHatSez)
self.zinvRDepRsw1 = sp.vscl(sezScale, rDepHatRsw1)
self.zinvRDepEci = sp.mtxv(self.mtxEciToRsw1, self.zinvRDepRsw1)
####################################################################
### Equations (17)
### - final velocity displacements
### - Not working yet: what is rChf?
self.zinvVelDepSez = sp.vpack(-velDepEqcm[iZeq] * sezScale / rChf2
#,(velDepEqcm[iAeq] + self.velChfRsw1[iSrsw]) * sezScale * self.zinvDeltaPhiDep / rChf2
,(velDepEqcm[iAeq] + self.velChfRsw1[iSrsw]) * sezScale * math.cos(self.zinvDeltaPhiDep) / rChf2
,velDepEqcm[iReq] + self.zinvVelChfRsw2[iRrsw]
)
self.zinvVelDepRsw1 = sp.mtxv(self.zinvMtxRswToSez, self.zinvVelDepSez)
self.zinvVelDepEci = sp.mtxv(self.mtxEciToRsw1, self.zinvVelDepRsw1)
return self.zinvRDepEci,self.zinvVelDepEci
def Deputy(self,stateDep):
"""Make depyty-based calculations. Chief-only calculations were
performed in __init__() method above
"""
####################################################################
### Extract Deputy ECI Position and velocity from 6-element deputy
### ECI state
self.rDepEci,self.velDepEci = stateDep[:3], stateDep[3:]
####################################################################
### Equations (1) and (2)
### Convert deputy position and velocity to RSW using
### [Rhat|Shat|What]1 matrix
(self.rDepRsw1
,self.velDepRsw1
) = [ sp.mxv(self.mtxEciToRsw1,v)
for v in
[self.rDepEci,self.velDepEci]
]
####################################################################
### Equations (3)
### Get delta-lambda to deputy as RA from (Radius,RA,DEC)
### returned by recrad.
RrdDepRsw1 = sp.recrad(self.rDepRsw1)
self.deltaLambdaDep = sp.recrad(self.rDepRsw1)[iRA]
####################################################################
### Equations (5)
### Deputy True anomaly
self.trueAnom2 = (twopi + self.deltaLambdaDep - self.lambdaPerigee) % twopi
####################################################################
### Equations (6)
### Equivalent chief position at point 2, using PQW2
rChf2 = self.pChf / (1. + (self.eccChf * math.cos(self.trueAnom2)))
pHat = self.vEccHatChf
qHat = sp.ucrss([0.,0.,1.],pHat)
self.rChfPqw2 = sp.vscl(rChf2,sp.vadd(sp.vscl(math.cos(self.trueAnom2),pHat),sp.vscl(math.sin(self.trueAnom2),qHat)))
self.velChfPqw2 = sp.vscl(math.sqrt(self.mu/self.pChf)
,sp.vadd(sp.vscl( -math.sin(self.trueAnom2),pHat)
,sp.vscl(self.eccChf+math.cos(self.trueAnom2),qHat)
)
)
####################################################################
### Equations (7)
### Convert from PQW2 to RSW2
self.mtxPqw2toRsw2 = RVtoRSW(self.rChfPqw2,self.velChfPqw2)
self.rChfRsw2 = sp.mxv(self.mtxPqw2toRsw2,self.rChfPqw2)
self.velChfRsw2 = sp.mxv(self.mtxPqw2toRsw2,self.velChfPqw2)
####################################################################
### Equations (8)
### Transform Deputy vectors to SEZ frame
### - deltaPhiDep is DEC from [Radius,RA,DEC] of rDepRsw1
### - Matrix is ROT2[90-deltaPhiDep] ROT3[deltaLambdaDep]
self.deltaPhiDep = RrdDepRsw1[iDEC]
self.mtxRswToSez = sp.eul2m(halfpi-self.deltaPhiDep,self.deltaLambdaDep,0.,2,3,1)
self.rDepSez = sp.mxv(self.mtxRswToSez,self.rDepRsw1)
self.velDepSez = sp.mxv(self.mtxRswToSez,self.velDepRsw1)
####################################################################
### Equations (9)
### Transform Deputy vectors from SEZ to EQCM frame
### 9.1) Convert True Anomalies 2 (Deputy) to Eccentric Anomaly 2
### - https://en.wikipedia.org/wiki/Eccentric_anomaly#From_the_true_anomaly
### - Possible exceptions: divBy0; domain error.
### - Chief done in __init__() method above
self.eccAnom2 = self.TrueToEccAnom(self.trueAnom2)
### Ensure .eccAnom2 is within PI/2 of .eccAnom1
while self.eccAnom2 > (self.eccAnom1+math.pi): self.eccAnom2 -= (2 * math.pi)
while self.eccAnom2 <= (self.eccAnom1-math.pi): self.eccAnom2 += (2 * math.pi)
### 9.2) Get arc length from positive semi-major axis to deputy
self.arcDep = self.E(self.eccAnom2)
### 9.3) Relate the deputy relative to chief at point 2
self.rDepEqcm = sp.vpack( self.rDepSez[iZsez] - self.rChfRsw2[iRrsw]
, self.arcDep - self.arcChf
, self.deltaPhiDep * rChf2
)
self.velDepEqcm = sp.vpack( self.velDepSez[iZsez] - self.velChfRsw2[iRrsw]
#, (self.velDepSez[iE] * rChf2 / (RrdDepRsw1[iRadius] * self.deltaPhiDep )) - self.velChfRsw1[iE]
, (self.velDepSez[iE] * rChf2 / (RrdDepRsw1[iRadius] * math.cos(self.deltaPhiDep))) - self.velChfRsw1[iE]
, -self.velDepSez[iSsez] * rChf2 / RrdDepRsw1[iRadius]
)
return self.rDepEqcm, self.velDepEqcm
