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
