KJ135
/
ISCE_INSAR
mirror of http://172.16.0.12:5000/WBJY/ISCE_INSAR.git
4
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ISCE_INSAR/components/isceobj/Planet/Ellipsoid.py

795 lines
30 KiB
Python
Raw Normal View History

Adding all files
2019-01-16 19:40:08 +00:00
#!/usr/bin/env python3
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Copyright 2010 California Institute of Technology. ALL RIGHTS RESERVED.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
# United States Government Sponsorship acknowledged. This software is subject to
# U.S. export control laws and regulations and has been classified as 'EAR99 NLR'
# (No [Export] License Required except when exporting to an embargoed country,
# end user, or in support of a prohibited end use). By downloading this software,
# the user agrees to comply with all applicable U.S. export laws and regulations.
# The user has the responsibility to obtain export licenses, or other export
# authority as may be required before exporting this software to any 'EAR99'
# embargoed foreign country or citizen of those countries.
#
# Author: Eric Gurrola
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
from __future__ import print_function
import math
import numbers
from iscesys.Component.Component import Component
from isceobj.Util.geo import ellipsoid
##
## NB: The Heritage class is the oiginal ellipsoid's interface.
## The geo/ellipsoid.Ellipsoid is mixed with to make
## a class that is both new and backwards compatible.
## A class to represent an ellipsoid.
## The parameters maintained internally are the following:
##
## a = semi major axis - the greatest distance from center to ellipse
## e2 = eccentricity-squared - the square of the ratio of the focal distance
## (from center) and a
##
## Other parameters can be used in describing an ellipsoid.
## The other parameters that this class will compute are the following:
##
## b = semi minor axis = a * sqrt(1 - e**2) - the smallest distance from
## center to ellipse
## e = eccentricity = sqrt(e2)
## c = focal distance from center = a * e
## f = flattening = 1 - sqrt(1 - e**2) = (b-a)/a
##
## Any of these four auxiliary ellipse parameters can be used to set the
## eccentricity-squared. If e or f are equated to a value then e2 is set
## accordingly. If b or c are equated to a value, then e2 will be set using
## the current value of a. So, the correct value of a must be set prior
## to setting b or c.
##
## When you create an object of class Ellipsoid you have the opportunity to
## set the semi major axis and eccentricity-squared. If you don't give these
## arguments in the constructor, then your object is initialized as a unit
## sphere.
class Heritage(object):
## I made this a class variable because it is constant
'''
dictionaryOfVariables = {'SEMIMAJOR_AXIS': ['a',float,'mandatory'],
'ECCENTRICITY_SQUARED': ['e2', float,'mandatory'],
'MODEL':['model', str,'optional']
}
'''
def __init__(self):
self.descriptionOfVariables = {}
self.pegLat = 0.0
self.pegLon = 0.0
self.pegHdg = 0.0
return None
def __str__(self):
retstr = "Semimajor axis: %s\n"
retlst = (self.a,)
retstr += "Eccentricity squared: %s\n"
retlst += (self.e2,)
return retstr % retlst
def get_model(self):
return self.model
def set_model(self, model):
self.model = model
def get_a(self):
return self.a
def set_a(self,a):
if a >= 0.0:
self.a = a
else:
message = (
"attempt to set ellipsoid semi major axis to negative value %f"+
" is invalid"
)
raise ValueError(message % a)
return None
def get_e2(self):
return self.e2
def set_e2(self,e2):
self.e2 = e2
def get_e(self):
return self.e2**0.5
def set_e(self,e):
self.sete2(e**2)
def get_f(self):
return 1.0 - (1.0-self.e2)**0.5
def set_f(self,f):
self.e2 = 1.0 - (1.0-f)**2
def get_b(self):
return self.a * (1.0-self.e2)**0.5
def set_b(self,b):
self.e2 = 1.0 - (b/self.a)**2
def get_c(self):
return self.c
def set_c(self,c):
self.c = c
# model = property(get_model,set_model,doc="ellipse model,for instance WGS-84")
# a = property(get_a, set_a, doc="ellipse semi major axis")
# e2 = property(get_e2, set_e2, doc="ellipse eccentricity-squared")
# e = property(get_e, set_e, doc="ellipse eccentricity")
# f = property(get_f, set_f, doc="ellipse flattening = 1 - sqrt(1-e2)")
# b = property(get_b, set_b, doc="ellipse semi minor axis --- should only be set after setting a")
# c = property(get_c, set_c, doc="ellipse distance from center to focus --- should only be set after setting a")
##
# Convert position relative to the ellipsoid from (x,y,z) to (lat,lon,height).
#
# (x,y,z) is the cartesian coordinate of the point with origin at the center of the ellipsoid
# +x axis is defined by latitude = 0 degrees, longitude = 0 degrees
# +y axis is defined by latitude = 0 degrees, longitude = 90 degrees
# +z axis is defined by latitude = 90 degrees --- polar axis
#
# (lat,lon,height) are defined in terms of a meridional plane containing the polar axis and the point,
# in which there an ellipse cross-section of the ellipsoid, and a normal line in that plane passing
# through the point and is normal to the ellipsoid.
# lat is the geodetic latitude defined as the angle in this meridional plane between the normal line and the equatorial plane.
# lon is the longitude defined as the angle in the equatorial plane from the x axis to the meridional plane
# height is the signed distance from the ellipsoid surface to the point along the normal line
def xyz_to_llh_old(self,xyz):
"""xyz_to_llh(xyz): returns llh=(lat (deg), lon (deg), h (meters)) for the instance ellipsoid \ngiven the coordinates of a point at xyz=(z,y,z) (meters)\n"""
r_llh = [None]*3
d_llh = [None]*3
r_q2 = 1.0/(1.0 - self.e2)
r_q = math.sqrt(r_q2)
r_q3 = r_q2 - 1.0
r_b = self.a*math.sqrt(1.0 - self.e2)
r_llh[1] = math.atan2(xyz[1],xyz[0])
r_p = math.sqrt(xyz[0]**2 + xyz[1]**2)
r_tant = (xyz[2]/r_p)*r_q
r_theta = math.atan(r_tant)
# r_tant = math.atan2(r_q*xyz[2],r_p)
r_tant = (xyz[2] + r_q3*r_b*math.sin(r_theta)**3)/(r_p - self.e2*self.a*math.cos(r_theta)**3)
r_llh[0] = math.atan(r_tant)
# r_llh[0] = math.atan2((xyz[2] + r_q3*r_b*math.sin(r_theta)**3),(r_p - self.e2*self.a*math.cos(r_theta)**3))
r_re = self.a/math.sqrt(1.0 - self.e2*math.sin(r_llh[0])**2)
r_llh[2] = r_p/math.cos(r_llh[0]) - r_re
d_llh[0] = math.degrees(r_llh[0])
d_llh[1] = math.degrees(r_llh[1])
d_llh[2] = r_llh[2]
return d_llh
def xyz_to_llh(self,xyz):
"""xyz_to_llh(xyz): returns llh=(lat (deg), lon (deg), h (meters)) for the instance ellipsoid \n
given the coordinates of a point at xyz=(z,y,z) (meters). \n
Based on closed form solution of H. Vermeille, Journal of Geodesy (2002) 76:451-454. \n
Handles simple list or tuples (xyz represents a single point) or a list of lists or tuples (xyz represents several points)"""
a2 = self.a**2
e4 = self.e2**2
# just to cast back to single list once done
onePosition = False
if isinstance(xyz[0],numbers.Real):
xyz = [xyz]
onePosition = True
r_llh = [0]*3
d_llh = [[0]*3 for i in range(len(xyz))]
for i in range(len(xyz)):
xy2 = xyz[i][0]**2+xyz[i][1]**2
p = (xy2)/a2
q = (1.-self.e2)*xyz[i][2]**2/a2
r = (p+q-e4)/6.
s = e4*p*q/(4.*r**3)
t = (1.+s+math.sqrt(s*(2.+s)))**(1./3.)
u = r*(1.+t+1./t)
v = math.sqrt(u**2+e4*q)
w = self.e2*(u+v-q)/(2.*v)
k = math.sqrt(u+v+w**2)-w
D = k*math.sqrt(xy2)/(k+self.e2)
r_llh[0] = math.atan2(xyz[i][2],D)
r_llh[1] = math.atan2(xyz[i][1],xyz[i][0])
r_llh[2] = (k+self.e2-1.)*math.sqrt(D**2+xyz[i][2]**2)/k
d_llh[i][0] = math.degrees(r_llh[0])
d_llh[i][1] = math.degrees(r_llh[1])
d_llh[i][2] = r_llh[2]
if onePosition:
return d_llh[0]
else:
return d_llh
##
# Convert position relative to the ellipsoid from (lat,lon,height) to (x,y,z).
#
# (x,y,z) is the cartesian coordinate of the point with origin at the center of the ellipsoid
# +x axis is defined by latitude = 0 degrees, longitude = 0 degrees
# +y axis is defined by latitude = 0 degrees, longitude = 90 degrees
# +z axis is defined by latitude = 90 degrees --- polar axis
#
# (lat,lon,height) are defined in terms of a meridional plane containing the polar axis and the point,
# in which there an ellipse cross-section of the ellipsoid, and a normal line in that plane passing
# through the point and is normal to the ellipsoid.
# lat is the geodetic latitude defined as the angle in this meridional plane between the normal line and the equatorial plane.
# lon is the longitude defined as the angle in the equatorial plane from the x axis to the meridional plane
# height is the signed distance from the ellipsoid surface to the point along the normal line
def llh_to_xyz(self,llh):
"""llh_to_xyz(llh): returns (z,y,z) (meters) coordinates of a point given the point at \nllh=(lat (deg), lon (deg), h (meters)) for the instance ellipsoid\n
Handles simple list or tuples (xyz represents a single point) or a list of lists or tuples (xyz represents several points)
"""
# just to cast back to single list once done
onePosition = False
if isinstance(llh[0],numbers.Real):
llh = [llh]
onePosition = True
r_v = [[0]*3 for i in range(len(llh))]
for i in range(len(llh)):
r_lat = math.radians(llh[i][0])
r_lon = math.radians(llh[i][1])
hgt = llh[i][2]
r_re = self.a/math.sqrt(1.0 - self.e2*math.sin(r_lat)**2)
r_v[i][0] = (r_re + hgt)*math.cos(r_lat)*math.cos(r_lon)
r_v[i][1] = (r_re + hgt)*math.cos(r_lat)*math.sin(r_lon)
r_v[i][2] = (r_re*(1.0-self.e2) + hgt)*math.sin(r_lat)
if onePosition:
return r_v[0]
else:
return r_v
##
# Compute the distance along a geodesic on the ellipsoid between the projection of two points onto the surface of the ellipsoid
#These results are based on the memo
#
#"Summary of Mocomp Reference Line Determination Study" , IOM 3346-93-163
#
#and the paper
#
#"A Rigourous Non-iterative Procedure for Rapid Inverse Solution of Very
#Long Geodesics" by E. M. Sadano, Bulletine Geodesique 1958
def geo_dis(self,llh1,llh2):
"""geo_dis(llh1,llh2): returns geodesic distance (meters) for the instance ellipsoid \ngiven a starting position on the ellipsoid (at height zero) below the point \nllh1=(lat (deg), lon (deg), h (meters)) and ending position on the ellipsoid \nbelow the point llh2=(lat (deg), lon (deg), h (meters)). \n"""
dis = 0.0
if(self.e2 == 0):
dis,hdg = self._sphericalDistance(llh1,llh2) #2013-06-03 Kosal: added _
else:
dis,hdg = self._ellipsoidalDistance(llh1,llh2) #2013-06-03 Kosal: added _
return dis
##
# Compute the heading (the angle from north) one would travel in going from one point on an ellipsoid to another
def geo_hdg(self,llh1,llh2):
"""geo_hdg(llh1,llh2): returns the heading angle (degrees) for a geodesic on the instance ellipsoid \ngiven a starting position on the ellipsoid (at height zero) below the point llh1=(lat (deg), lon (deg), h (meters)) \nand ending position on the ellipsoid below llh2=(lat (deg), lon (deg), h (meters)). \n"""
hdg = 0.0
if(self.e2 == 0):
dis,hdg = self._sphericalDistance(llh1,llh2) #2013-06-03 Kosal: added _
else:
dis,hdg = self._ellipsoidalDistance(llh1,llh2) #2013-06-03 Kosal: added _
return hdg
def _sphericalDistance(self,llh1,llh2):
r_sinlati = math.sin(math.radians(llh1[0]))
r_coslati = math.cos(math.radians(llh1[0]))
r_sinlatf = math.sin(math.radians(llh2[0]))
r_coslatf = math.cos(math.radians(llh2[0]))
r_tanlatf = math.tan(math.radians(llh2[0]))
r_t1 = math.radians(llh2[1]) - math.radians(llh1[1])
if (math.fabs(r_t1) > math.pi):
r_t1 = (2.0*math.pi - math.fabs(r_t1))*math.copysign(1.0,-r_t1)
r_sinlon = math.sin(r_t1)
r_coslon = math.cos(r_t1)
r_t2 = r_coslati*r_coslatf*r_coslon + r_sinlati*r_sinlatf
r_t3 = r_coslati*r_tanlatf - r_sinlati*r_coslon
r_geodis = self.a*math.acos(r_t2)
r_geohdg = math.atan2(r_sinlon,r_t3)
return r_geodis,r_geohdg
def _ellipsoidalDistance(self,llh1,llh2):
r_geodis = 0.0
r_geohdg = None
r_e = self.get_e()
r_f = self.get_f()
r_ep = r_e*r_f/(self.e2-r_f)
r_n = r_f/self.e2
r_sqrtome2 = math.sqrt(1.0 - self.e2)
r_b0 = self.a*r_sqrtome2
r_k1 = (16.0*self.e2*r_n**2 + r_ep**2)/r_ep**2
r_k2 = (16.0*self.e2*r_n**2)/(16.0*self.e2*r_n**2 + r_ep**2)
r_k3 = (16.0*self.e2*r_n**2)/r_ep**2
r_k4 = (16.0*r_n - r_ep**2)/(16.0*self.e2*r_n**2 + r_ep**2)
r_k5 = 16.0/(self.e2*(16.0*self.e2*r_n**2 + r_ep**2))
r_tanlati = math.tan(math.radians(llh1[0]))
r_tanlatf = math.tan(math.radians(llh2[0]))
r_l = math.fabs(math.radians(llh2[1])-math.radians(llh1[1]))
r_lsign = math.radians(llh2[1]) - math.radians(llh1[1])
r_sinlon = math.sin(r_l)
r_coslon = math.cos(r_l)
r_tanbetai = r_sqrtome2*r_tanlati
r_tanbetaf = r_sqrtome2*r_tanlatf
r_cosbetai = 1.0/math.sqrt(1.0 + r_tanbetai**2)
r_cosbetaf = 1.0/math.sqrt(1.0 + r_tanbetaf**2)
r_sinbetai = r_tanbetai*r_cosbetai
r_sinbetaf = r_tanbetaf*r_cosbetaf
r_ac = r_sinbetai*r_sinbetaf
r_bc = r_cosbetai*r_cosbetaf
r_cosphi = r_ac + r_bc*r_coslon
r_sinphi = math.copysign(1.0,r_sinlon)*math.sqrt(1.0 - min(r_cosphi**2,1.0))
r_phi = math.fabs(math.atan2(r_sinphi,r_cosphi))
if(self.a*math.fabs(r_phi) > 1e-6):
r_ca = (r_bc*r_sinlon)/r_sinphi
r_cb = r_ca**2
r_cc = (r_cosphi*(1.0 - r_cb))/r_k1
r_cd = (-2.0*r_ac)/r_k1
r_ce = -r_ac*r_k2
r_cf = r_k3*r_cc
r_cg = r_phi**2/r_sinphi
r_x = ((r_phi*(r_k4 + r_cb) + r_sinphi*(r_cc + r_cd) + r_cg*(r_cf + r_ce))*r_ca)/r_k5
r_lambda = r_l + r_x
r_sinlam = math.sin(r_lambda)
r_coslam = math.cos(r_lambda)
r_cosph0 = r_ac + r_bc*r_coslam
r_sinph0 = math.copysign(1.0,r_sinlam)*math.sqrt(1.0 - r_cosph0**2)
r_phi0 = math.fabs(math.atan2(r_sinph0,r_cosph0))
r_sin2phi = 2.0*r_sinph0*r_cosph0
r_cosbeta0 = (r_bc*r_sinlam)/r_sinph0
r_q = 1.0 - r_cosbeta0**2
r_cos2sig = (2.0*r_ac - r_q*r_cosph0)/r_q
r_cos4sig = 2.0*(r_cos2sig**2 - 0.5)
r_ch = r_b0*(1.0 + (r_q*r_ep**2)/4.0 - (3.0*(r_q**2)*r_ep**4)/64.0)
r_ci = r_b0*((r_q*r_ep**2)/4.0 - ((r_q**2)*r_ep**4)/16.0)
r_cj = (r_q**2*r_b0*r_ep**4)/128.0
r_t2 = (r_tanbetaf*r_cosbetai - r_coslam*r_sinbetai)
r_sinlon = r_sinlam*math.copysign(1.0,r_lsign)
r_cotalpha12 = (r_tanbetaf*r_cosbetai - r_coslam*r_sinbetai)/r_sinlam
r_cotalpha21 = (r_sinbetaf*r_coslam - r_cosbetaf*r_tanbetai)/r_sinlam
r_geodis = r_ch*r_phi0 + r_ci*r_sinph0*r_cos2sig - r_cj*r_sin2phi*r_cos4sig
r_geohdg = math.atan2(r_sinlon,r_t2)
else:
r_geodis = 0.0
r_geohdg = None
return r_geodis, r_geohdg
##
# Compute the radius of curvature in the east direction on an ellipsoid
def eastRadiusOfCurvature(self,llh):
"""eastRadiusOfCurvature(llh): returns Radius of Curvature (meters) \nin the East direction for the instance ellipsoid \ngiven a position llh=(lat (deg), lon (deg), h (meters))"""
r_lat = math.radians(llh[0])
reast = self.a/math.sqrt(1.0 - self.e2*math.sin(r_lat)**2)
return reast
##
# Compute the radius of curvature in the north direction on an ellipsoid
def northRadiusOfCurvature(self,llh):
"""northRadiusOfCurvature(llh): returns Radius of Curvature (meters) \nin the North direction for the instance ellipsoid \ngiven a position llh=(lat (deg), lon (deg), h (meters))"""
r_lat = math.radians(llh[0])
rnorth = (self.a*(1.0 - self.e2))/(1.0 - self.e2*math.sin(r_lat)**2)**(1.5)
return rnorth
##
# Compute the radius of curvature on an ellipsoid
def radiusOfCurvature(self,llh,hdg=0):
"""
radiusOfCurvature(llh,[hdg]): returns Radius of Curvature (meters)
in the direction specified by hdg for the instance ellipsoid
given a position llh=(lat (deg), lon (deg), h (meters)).
If no heading is given the default is 0, or North.
"""
r_lat = math.radians(llh[0])
r_hdg = math.radians(hdg)
reast = self.eastRadiusOfCurvature(llh)
rnorth = self.northRadiusOfCurvature(llh)
#radius of curvature for point on ellipsoid
rdir = (reast*rnorth)/(
reast*math.cos(r_hdg)**2 + rnorth*math.sin(r_hdg)**2)
#add height of the llh point
return rdir + llh[2]
##
# Compute the local, equivalent spherical radius
def localRadius(self,llh):
"""
localRadius(llh): returns the equivalent spherical radius (meters)
for the instance ellipsoid given a position llh=(lat (deg), lon (deg),
h (meters))
"""
latg = math.atan(math.tan(math.radians(llh[0]))*self.a**2/self.get_b()**2)
arg = math.cos(latg)**2/self.a**2 + math.sin(latg)**2/self.get_b()**2
re = 1.0/math.sqrt(arg)
return re
def setSCH(self, pegLat, pegLon, pegHdg, pegHgt=0.0):
"""
Set up an SCH coordinate system at the given peg point.
Set a peg point on the ellipse at pegLat, pegLon, pegHdg (in degrees).
Set the radius of curvature and the transformation matrix and offset
vector needed to transform between (s,c,h) and ecef (x,y,z).
"""
self.pegLat = pegLat
self.pegLon = pegLon
self.pegHdg = pegHdg
self.pegHgt = pegHgt
self.pegLLH = [pegLat, pegLon, pegHgt]
#determine the radius of curvature at the peg point, i.e, the
#the radius of the SCH sphere
self.pegRadCur = self.radiusOfCurvature(self.pegLLH, pegHdg)
#determine the rotation matrix (from radar_to_xyz.F)
import numpy
r_lat = numpy.radians(pegLat)
r_lon = numpy.radians(pegLon)
r_hdg = numpy.radians(pegHdg)
r_clt = numpy.cos(r_lat)
r_slt = numpy.sin(r_lat)
r_clo = numpy.cos(r_lon)
r_slo = numpy.sin(r_lon)
r_chg = numpy.cos(r_hdg)
r_shg = numpy.sin(r_hdg)
ptm11 = r_clt*r_clo
ptm12 = -r_shg*r_slo - r_slt*r_clo*r_chg
ptm13 = r_slo*r_chg - r_slt*r_clo*r_shg
ptm21 = r_clt*r_slo
ptm22 = r_clo*r_shg - r_slt*r_slo*r_chg
ptm23 = -r_clo*r_chg - r_slt*r_slo*r_shg
ptm31 = r_slt
ptm32 = r_clt*r_chg
ptm33 = r_clt*r_shg
self.pegRotMatNP = numpy.matrix(
[[ptm11, ptm12, ptm13],
[ptm21, ptm22, ptm23],
[ptm31, ptm32, ptm33]]
)
self.pegRotMatInvNP = self.pegRotMatNP.transpose()
self.pegRotMat = self.pegRotMatNP.tolist()
self.pegRotMatInv = self.pegRotMatInvNP.tolist()
#find the translation vector as a column matrix
self.pegXYZ = self.llh_to_xyz(self.pegLLH)
self.pegXYZNP = numpy.matrix(self.pegXYZ).transpose()
#Outward normal to ellispoid at the peg point
self.pegNormal = [r_clt*r_clo, r_clt*r_slo, r_slt]
self.pegNormalNP = numpy.matrix(self.pegNormal).transpose()
#Offset Vector - to center of SCH sphere
self.pegOVNP = self.pegXYZNP - self.pegRadCur*self.pegNormalNP
self.pegOV = self.pegOVNP.transpose().tolist()[0]
return
def schbasis(self, posSCH):
"""
xyzschMat = elp.schbasis(posSCH)
Given an instance elp of an Ellipsoid with a peg point defined by a
previous call to setSCH and an SCH position (as a list) return the
transformation matrices from the XYZ frame to the SCH frame and the
inverse from the SCH frame to the XYZ frame.
The returned object is a namedtuple with numpy matrices in elements
named 'sch_to_xyz' and 'xyz_to_sch'
sch_to_xyzMat = (elp.schbasis(posSCH)).sch_to_xyz
xyz_to_schMat = (elp.schbasis(posSCH)).xyz_to_sch
"""
import numpy
r_coss = numpy.cos(posSCH[0]/self.pegRadCur)
r_sins = numpy.sin(posSCH[0]/self.pegRadCur)
r_cosc = numpy.cos(posSCH[1]/self.pegRadCur)
r_sinc = numpy.sin(posSCH[1]/self.pegRadCur)
r_matschxyzp = numpy.matrix([
[-r_sins, -r_sinc*r_coss, r_coss*r_cosc],
[ r_coss, -r_sinc*r_sins, r_sins*r_cosc],
[ 0.0, r_cosc, r_sinc]])
#compute sch to xyz matrix
r_sch_to_xyzMat = self.pegRotMatNP*r_matschxyzp
#get the inverse
r_xyz_to_schMat = r_sch_to_xyzMat.transpose()
from collections import namedtuple
schxyzMat = namedtuple("schxyzMat", "sch_to_xyz xyz_to_sch")
return schxyzMat(r_sch_to_xyzMat, r_xyz_to_schMat)
def sch_to_xyz(self, posSCH):
"""
Given an sch coordinate system (defined by setSCH) and an input SCH
point (a list), return the corresponding earth-centered-earth-fixed
xyz position.
"""
#compute the linear portion of the transformation
#create the SCH sphere object
sph = Ellipsoid()
sph.a = self.pegRadCur
sph.e2 = 0.
import numpy
#on SCH sphere, longitude = S/pegRadCur, latitude = C/pegRadCur,
#height = H
r_llh = [numpy.degrees(posSCH[1]/sph.a),
numpy.degrees(posSCH[0]/sph.a),
posSCH[2]]
#convert sphere llh to sphere xyz coordinates
r_schvt = numpy.matrix(sph.llh_to_xyz(r_llh)).transpose()
#Rotate the sch position into the ecef orientation defined by the peg
r_xyzv = self.pegRotMatNP*r_schvt
#add the origin of the SCH sphere and return as list
return ((r_xyzv + self.pegOVNP).transpose()).tolist()[0]
def xyz_to_sch(self, posXYZ):
"""
Given an sch coordinate system (defined by setSCH) and an input XYZ
point (an earth-centered-earth-fixed point as a list), return the
corresponding SCH position.
"""
#create a spherical object of the radius of the SCH sphere
sph = Ellipsoid()
sph.a = self.pegRadCur
sph.e2 = 0.
#use numpy matrices for matrix manipulations
import numpy
r_xyz = numpy.matrix(posXYZ).transpose()
#compute the xyz position relative to the SCH sphere origin
r_xyzt = r_xyz - self.pegOVNP
#Rotate the XYZ position from the ecef basis to the SCH sphere basis
#defined by the peg, and pass the SCH sphere XYZ position to
#llh_to_xyz to get the llh on the sch sphere
r_xyzp = ((self.pegRotMatInvNP*r_xyzt).transpose()).tolist()[0]
r_llh = sph.xyz_to_llh(r_xyzp)
#S = SCH-sphere-radius*longitude, C = SCH-sphere-radius*latitude,
#H = height above SCH sphere
return [self.pegRadCur*numpy.radians(r_llh[1]),
self.pegRadCur*numpy.radians(r_llh[0]),
r_llh[2]]
def schdot_to_xyzdot(self, posSCH, velSCH):
"""
velXYZ = elp.schdot_to_xyzdot(posSCH, velSCH)
where elp is an instance of Ellipsoid and posSCH, velSCH are the
position and velocity in the SCH coordinate system defined by a
previous call to elp.setSCH and posXYZ, velXYZ are the position
and velocity in the ecef cartesian coordinate system.
posSCH, velSCH, posXYZ, and velXYZ are all lists.
"""
import numpy
sch_to_xyzMat = (self.schbasis(posSCH)).sch_to_xyz
velSCHNP = numpy.matrix(velSCH).transpose()
velXYZNP = sch_to_xyzMat*velSCHNP
velXYZ = velXYZNP.transpose().tolist()[0]
posXYZ = self.sch_to_xyz(posSCH)
return posXYZ, velXYZ
def xyzdot_to_schdot(self, posXYZ, velXYZ):
"""
posSCH, velSCH = elp.xyzdot_to_schdot(posXYZ, velXYZ)
where elp is an instance of Ellipsoid and posXYZ, velXYZ are the
position and velocity in the ecef cartesian coordinate system and
posSCH, velSCH are the position and velocity in the SCH coordinate
system defined by a previous call to elp.setSCH.
posSCH, velSCH, posXYZ, and velXYZ are all lists.
"""
import numpy
posSCH = self.xyz_to_sch(posXYZ)
xyz_to_schMat = (self.schbasis(posSCH)).xyz_to_sch
velSCHNP = xyz_to_schMat*numpy.matrix(velXYZ).transpose()
velSCH = velSCHNP.transpose().tolist()[0]
return posSCH, velSCH
def enubasis(self, posLLH):
"""
xyzenuMat = elp.enubasis(posLLH)
Given an instance elp of an Ellipsoid LLH position (as a list) return the
transformation matrices from the XYZ frame to the ENU frame and the
inverse from the ENU frame to the XYZ frame.
The returned object is a namedtuple with numpy matrices in elements
named 'enu_to_xyz' and 'xyz_to_enu'
enu_to_xyzMat = (elp.enubasis(posLLH)).enu_to_xyz
xyz_to_enuMat = (elp.enubasis(posLLH)).xyz_to_enu
"""
import numpy
r_lat = numpy.radians(posLLH[0])
r_lon = numpy.radians(posLLH[1])
r_clt = numpy.cos(r_lat)
r_slt = numpy.sin(r_lat)
r_clo = numpy.cos(r_lon)
r_slo = numpy.sin(r_lon)
r_enu_to_xyzMat = numpy.matrix([
[-r_slo, -r_slt*r_clo, r_clt*r_clo],
[ r_clo, -r_slt*r_slo, r_clt*r_slo],
[ 0.0 , r_clt , r_slt]])
r_xyz_to_enuMat = r_enu_to_xyzMat.transpose()
from collections import namedtuple
enuxyzMat = namedtuple("enuxyzMat", "enu_to_xyz xyz_to_enu")
return enuxyzMat(r_enu_to_xyzMat, r_xyz_to_enuMat)
pass
SEMIMAJOR_AXIS = Component.Parameter(
'a',
public_name='SEMIMAJOR_AXIS',
default=1.0,
type=float,
mandatory=False,
intent='input',
doc='Ellipsoid semimajor axis'
)
ECCENTRICITY_SQUARED = Component.Parameter(
'e2',
public_name='ECCENTRICITY_SQUARED',
default=0.0,
type=float,
mandatory=False,
intent='input',
doc='Ellipsoid eccentricity squared'
)
MODEL = Component.Parameter(
'model',
public_name='MODEL',
default='Unit Sphere',
type=str,
mandatory=False,
intent='input',
doc='Ellipsoid model'
)
## This Ellipsoid is an amalgalm of the Heritage ellipsoid and the new one, as
## of 9/8/12: ellipsoid.Ellipsoid-- decorated properties superceed explicit
## properties, while the getters and setter are retained for backwards
## compatability-- they now call the decorated properties-- which still act
## as mutator methods, and allow an ellipsoid to be defined with "a" and any
## one of "e2", "e", "f", "finv" "cosOE", "b", Since each one of these setters
## modifies e2, the value checking is left to that method-- an error is raised
## if e2 is not on (0,1]. Other ellipsoid parameter properties are inherited
## from ellipsoid._OblateEllipsoid, including all the second and third
## flattenigs, eccentricity (though these are rarely used, as the need to do
## algebraic expansions in them is no longer extant). The base-clase also has
## functions for the various radii-of-curvature and conversions between common,
## reduced, conformal, authalic, rectifying, geocentric, and isometric
## latitudes as well as spheric and iterative "exact" solutions to great
## circle distance and bearing problems. Another base-class:
## EllipsoidTransformations has the methods for computing ECEF<-->LLH
## (approximate or iterative exact) and for computing the affine
## transformations to various tangent plane (LTP) coordinate systems...
class Ellipsoid(Component,ellipsoid.Ellipsoid, Heritage):
parameter_list = (
SEMIMAJOR_AXIS,
ECCENTRICITY_SQUARED,
MODEL
)
family = 'ellipsoid'
def __init__(self,family='', name='', a=1.0, e2=0.0, model="Unit Sphere"):
Component.__init__(self, family if family else self.__class__.family, name=name)
ellipsoid.Ellipsoid.__init__(self, a, e2, model=model)
Heritage.__init__(self)
return None
#Make sure if init as Configurable that the base class gets initialized
def _configure(self):
ellipsoid.Ellipsoid.__init__(self, self.a, self.e2, self.model)
# \f$ c = a\epsilon \f$
@property
def c(self):
return self.a*(self.e2)**0.5
@c.setter
def c(self,val):
self.e2 = (val/self.a)**2
pass
@property
def model(self):
return self._model
@model.setter
def model(self, model):
self._model = model
pass
pass