164 lines
5.7 KiB
Python
164 lines
5.7 KiB
Python
|
|
import math
|
|
from isceobj.Planet.Ellipsoid import Ellipsoid
|
|
from isceobj.Planet.AstronomicalHandbook import PlanetsData
|
|
from isceobj.Util.mathModule import MathModule as MM
|
|
|
|
class SCH(object):
|
|
"""A Class to convert between SCH and XYZ coordinates"""
|
|
|
|
def __init__(self,peg=None):
|
|
self.peg = peg
|
|
self.r_ov = [0 for i in range(3)]
|
|
self.M = [[0 for i in range(3)] for j in range(3)]
|
|
self.invM = [[0 for i in range(3)] for j in range(3)]
|
|
self.__initialize()
|
|
|
|
def __initialize(self):
|
|
self.initializeTranslationVector()
|
|
self.initializeRotationMatrix()
|
|
|
|
def initializeRotationMatrix(self):
|
|
lat = math.radians(self.peg.getLatitude())
|
|
lon = math.radians(self.peg.getLongitude())
|
|
heading = math.radians(self.peg.getHeading())
|
|
|
|
self.M[0][0] = math.cos(lat)*math.cos(lon)
|
|
self.M[0][1] = -math.sin(heading)*math.sin(lon) - math.sin(lat)*math.cos(lon)*math.cos(heading)
|
|
self.M[0][2] = math.sin(lon)*math.cos(heading) - math.sin(lat)*math.cos(lon)*math.sin(heading)
|
|
self.M[1][0] = math.cos(lat)*math.sin(lon)
|
|
self.M[1][1] = math.cos(lon)*math.sin(heading) - math.sin(lat)*math.sin(lon)*math.cos(heading)
|
|
self.M[1][2] = -math.cos(lon)*math.cos(heading) - math.sin(lat)*math.sin(lon)*math.sin(heading)
|
|
self.M[2][0] = math.sin(lat)
|
|
self.M[2][1] = math.cos(lat)*math.cos(heading)
|
|
self.M[2][2] = math.cos(lat)*math.sin(heading)
|
|
|
|
self.invM = MM.matrixTranspose(self.M)
|
|
|
|
def initializeTranslationVector(self):
|
|
lat = math.radians(self.peg.getLatitude())
|
|
lon = math.radians(self.peg.getLongitude())
|
|
radcur = self.peg.getRadiusOfCurvature() # Get the radius of curvature at the peg point
|
|
|
|
r_up = [0 for i in range(3)]
|
|
r_p = [0 for i in range(3)]
|
|
|
|
r_up[0
|
|
] = math.cos(lat)*math.cos(lon)
|
|
r_up[1] = math.cos(lat)*math.sin(lon)
|
|
r_up[2] = math.sin(lat)
|
|
|
|
# The Cartesian vector at the peg latitude and longitude at zero height
|
|
r_p = self._calculateXYZ()
|
|
|
|
for i in range(3):
|
|
self.r_ov[i] = r_p[i] - radcur*r_up[i]
|
|
|
|
def _calculateXYZ(self):
|
|
"""
|
|
Calculate the cartesian coordinate of the point assuming the WGS-84 ellipsoid (to be fixed)
|
|
"""
|
|
ellipsoid = Ellipsoid(a=PlanetsData.ellipsoid['Earth']['WGS-84'][0],
|
|
e2=PlanetsData.ellipsoid['Earth']['WGS-84'][1])
|
|
llh = [self.peg.getLatitude(),self.peg.getLongitude(),0.0]
|
|
xyz = ellipsoid.llh_to_xyz(llh)
|
|
return xyz
|
|
|
|
def xyz_to_sch(self,xyz):
|
|
radcur = self.peg.getRadiusOfCurvature() # Get the radius of curvature at the peg point
|
|
ellipsoid = Ellipsoid(a=radcur,e2=0.0)
|
|
|
|
|
|
|
|
schvt = [0 for i in range(3)]
|
|
rschv = [0 for i in range(3)]
|
|
|
|
for i in range(3):
|
|
schvt[i] = xyz[i] - self.r_ov[i]
|
|
|
|
schv = MM.matrixVectorProduct(self.invM,schvt)
|
|
llh = ellipsoid.xyz_to_llh(schv)
|
|
|
|
rschv[0] = radcur*math.radians(llh[1])
|
|
rschv[1] = radcur*math.radians(llh[0])
|
|
rschv[2] = llh[2]
|
|
|
|
return rschv
|
|
|
|
def sch_to_xyz(self,sch):
|
|
radcur = self.peg.getRadiusOfCurvature() # Get the radius of curvature at the peg point
|
|
ellipsoid = Ellipsoid(a=radcur,e2=0.0)
|
|
|
|
xyz = [0 for i in range(3)]
|
|
llh = [0 for i in range(3)]
|
|
|
|
llh[0] = math.degrees(sch[1]/radcur)
|
|
llh[1] = math.degrees(sch[0]/radcur)
|
|
llh[2] = sch[2]
|
|
|
|
schv = ellipsoid.llh_to_xyz(llh)
|
|
schvt = MM.matrixVectorProduct(self.M,schv)
|
|
|
|
for i in range(3):
|
|
xyz[i] = schvt[i] + self.r_ov[i]
|
|
|
|
return xyz
|
|
|
|
def vxyz_to_vsch(self,sch,vxyz):
|
|
"""
|
|
Convert from cartesian velocity to sch velocity
|
|
"""
|
|
schbasis = LocalSCH(peg=self.peg,sch=sch)
|
|
vsch = schbasis.xyz_to_localsch(vxyz)
|
|
|
|
return vsch
|
|
|
|
def vsch_to_vxyz(self,sch,vsch):
|
|
"""
|
|
Convert from sch velocity to cartesian velocity
|
|
"""
|
|
schbasis = LocalSCH(peg=self.peg,sch=sch)
|
|
vxyz = schbasis.localsch_to_xyz(vsch)
|
|
|
|
return vxyz
|
|
|
|
class LocalSCH(SCH):
|
|
# It almost might be better to define an SCH 'Location' object
|
|
# that can convert things between its local tangent plane and back
|
|
|
|
def __init__(self,peg=None,sch=None):
|
|
SCH.__init__(self,peg=peg)
|
|
self.sch = sch
|
|
self.sch2xyz = [[0 for i in range(3)] for j in range(3)]
|
|
self.xyz2sch = [[0 for i in range(3)] for j in range(3)]
|
|
|
|
self.__initialize()
|
|
|
|
def __initialize(self):
|
|
s = self.sch[0]/self.peg.getRadiusOfCurvature()
|
|
c = self.sch[1]/self.peg.getRadiusOfCurvature()
|
|
|
|
schxyzp = [[0 for i in range(3)] for j in range(3)]
|
|
schxyzp[0][0] = -math.sin(s)
|
|
schxyzp[0][1] = -math.sin(c)*math.cos(s)
|
|
schxyzp[0][1] = math.cos(s)*math.cos(c)
|
|
schxyzp[1][0] = math.cos(s)
|
|
schxyzp[1][1] = -math.sin(c)*math.sin(s)
|
|
schxyzp[1][2] = math.sin(s)*math.cos(c)
|
|
schxyzp[2][0] = 0.0
|
|
schxyzp[2][1] = math.cos(c)
|
|
schxyzp[2][2] = math.sin(c)
|
|
|
|
self.sch2xyz = MM.multiplyMatrices(self.M,schxyzp)
|
|
self.xyz2sch = MM.matrixTranspose(self.sch2xyz)
|
|
|
|
def xyz_to_localsch(self,xyz):
|
|
sch = MM.matrixVectorProduct(self.xyz2sch,xyz)
|
|
|
|
return sch
|
|
|
|
def localsch_to_xyz(self,sch):
|
|
xyz = MM.matrixVectorProduct(self.sch2xyz,sch)
|
|
|
|
return xyz
|