732 lines
19 KiB
Fortran
732 lines
19 KiB
Fortran
program ellipsoid_test
|
|
|
|
type ellipsoid
|
|
sequence
|
|
real (8) r_a
|
|
real (8) r_e2
|
|
end type ellipsoid
|
|
type (ellipsoid) elp
|
|
|
|
type pegtype
|
|
sequence
|
|
real (8) r_lat
|
|
real (8) r_lon
|
|
real (8) r_hdg
|
|
end type pegtype
|
|
type (pegtype) peg
|
|
|
|
c OUTPUT VARIABLES:
|
|
|
|
type pegtrans
|
|
sequence
|
|
real (8) r_mat(3,3)
|
|
real (8) r_matinv(3,3)
|
|
real (8) r_ov(3)
|
|
real (8) r_radcur
|
|
end type pegtrans
|
|
type (pegtrans) ptm
|
|
|
|
real(8) r_xyz(3), r_llh(3), r_sch(3), r_xyzdot(3), r_schdot(3)
|
|
real(8), parameter :: r2d = 180.0d0/acos(-1.0d0)
|
|
real(8), parameter :: d2r = acos(-1.0d0)/180.0d0
|
|
integer, parameter :: i_llh_to_xyz = 1
|
|
integer, parameter :: i_xyz_to_llh = 2
|
|
integer, parameter :: i_sch_to_xyz = 0
|
|
integer, parameter :: i_xyz_to_sch = 1
|
|
integer i, j
|
|
|
|
elp%r_a = 6378137.0d0
|
|
elp%r_e2 = 0.0066943799901d0
|
|
|
|
print*, "latlon xyz_to_llh"
|
|
r_xyz(1) = 7000000.0d0
|
|
r_xyz(2) = -7500000.0d0
|
|
r_xyz(3) = 8000000.0d0
|
|
print*, "r_xyz = ", r_xyz
|
|
call latlon(elp,r_xyz,r_llh,i_xyz_to_llh)
|
|
print*, "r_llh = ", r_llh(1)*r2d, r_llh(2)*r2d, r_llh(3)
|
|
print*
|
|
|
|
print*, "latlon llh_to_xyz"
|
|
r_llh(1) = -33.0d0
|
|
r_llh(2) = 118.0d0
|
|
r_llh(3) = 2000.0d0
|
|
print*, "r_llh = ", r_llh
|
|
r_llh(1) = r_llh(1)*d2r
|
|
r_llh(2) = r_llh(2)*d2r
|
|
call latlon(elp,r_xyz,r_llh,i_llh_to_xyz)
|
|
print*, "r_xyz = ", r_xyz
|
|
print*
|
|
|
|
peg%r_lat = 66.0d0
|
|
peg%r_lon = -105.0d0
|
|
peg%r_hdg = 36.0d0
|
|
print*, "peg point = ", peg%r_lat, peg%r_lon, peg%r_hdg
|
|
peg%r_lat = peg%r_lat*d2r
|
|
peg%r_lon = peg%r_lon*d2r
|
|
peg%r_hdg = peg%r_hdg*d2r
|
|
call radar_to_xyz(elp,peg,ptm)
|
|
print*, "ptm%r_radcur = ", ptm%r_radcur
|
|
print*, "ptm%r_ov = ", ptm%r_ov
|
|
print*, "ptm%r_mat = "
|
|
do i = 1, 3
|
|
print*, " row1: ", (ptm%r_mat(i,j), j=1,3)
|
|
enddo
|
|
print*, "ptm%r_matinv = "
|
|
do i = 1, 3
|
|
print*, " row1: ", (ptm%r_matinv(i,j), j=1,3)
|
|
enddo
|
|
print*
|
|
|
|
print*, "convert_sch_to_xyz, sch_to_xyz"
|
|
r_sch(1) = 1468.0d0
|
|
r_sch(2) = -234.0d0
|
|
r_sch(3) = 7000.0d0
|
|
print*, "r_sch = ", r_sch
|
|
call convert_sch_to_xyz(ptm,r_sch,r_xyz,i_sch_to_xyz)
|
|
print*, "r_xyz = ", r_xyz
|
|
call latlon(elp,r_xyz,r_llh,i_xyz_to_llh)
|
|
print*, "r_llh = ", r_llh(1)*r2d, r_llh(2)*r2d, r_llh(3)
|
|
print*
|
|
|
|
print*, "convert_sch_to_xyz, xyz_to_sch"
|
|
r_xyz(1) = -672100.0d0
|
|
r_xyz(2) = -2514000.0d0
|
|
r_xyz(3) = 5811000.0d0
|
|
print*, "r_xyz = ", r_xyz
|
|
call convert_sch_to_xyz(ptm,r_sch,r_xyz,i_xyz_to_sch)
|
|
print*, "r_sch = ", r_sch
|
|
call latlon(elp,r_xyz,r_llh,i_xyz_to_llh)
|
|
print*, "r_llh = ", r_llh(1)*r2d, r_llh(2)*r2d, r_llh(3)
|
|
print*
|
|
|
|
|
|
print*, "convert_schdot_to_xyzdot, sch_to_xyz"
|
|
r_sch(1) = 1468.0d0
|
|
r_sch(2) = -234.0d0
|
|
r_sch(3) = 7000.0d0
|
|
print*, "r_sch = ", r_sch
|
|
r_schdot(1) = 800.0d0
|
|
r_schdot(2) = -400.0d0
|
|
r_schdot(3) = 100.0d0
|
|
print*, "r_schdot = ", r_schdot
|
|
call convert_schdot_to_xyzdot(ptm,r_sch,r_xyz,r_schdot,r_xyzdot,i_sch_to_xyz)
|
|
call convert_sch_to_xyz(ptm,r_sch,r_xyz,i_sch_to_xyz)
|
|
print*, "r_xyz = ", r_xyz
|
|
print*, "r_xyzdot = ", r_xyzdot
|
|
|
|
|
|
print*, "convert_schdot_to_xyzdot, xyz_to_sch"
|
|
r_xyz(1) = -672100.0d0
|
|
r_xyz(2) = -2514000.0d0
|
|
r_xyz(3) = 5811000.0d0
|
|
print*, "r_xyz = ", r_xyz
|
|
r_xyzdot(1) = 800.0d0
|
|
r_xyzdot(2) = -400.0d0
|
|
r_xyzdot(3) = 100.0d0
|
|
print*, "r_xyzdot = ", r_xyzdot
|
|
call convert_sch_to_xyz(ptm,r_sch,r_xyz,i_xyz_to_sch)
|
|
call convert_schdot_to_xyzdot(ptm,r_sch,r_xyz,r_schdot,r_xyzdot,i_xyz_to_sch)
|
|
print*, "r_sch = ", r_sch
|
|
print*, "r_schdot = ", r_schdot
|
|
|
|
end
|
|
|
|
|
|
c****************************************************************
|
|
|
|
subroutine radar_to_xyz(elp,peg,ptm)
|
|
|
|
c****************************************************************
|
|
c**
|
|
c** FILE NAME: radar_to_xyz.for
|
|
c**
|
|
c** DATE WRITTEN:1/15/93
|
|
c**
|
|
c** PROGRAMMER:Scott Hensley
|
|
c**
|
|
c** FUNCTIONAL DESCRIPTION:This routine computes the transformation
|
|
c** matrix and translation vector needed to get between radar (s,c,h)
|
|
c** coordinates and (x,y,z) WGS-84 coordinates.
|
|
c**
|
|
c** ROUTINES CALLED:euler,
|
|
c**
|
|
c** NOTES: none
|
|
c**
|
|
c** UPDATE LOG:
|
|
c**
|
|
c*****************************************************************
|
|
|
|
implicit none
|
|
|
|
c INPUT VARIABLES:
|
|
|
|
type ellipsoid
|
|
sequence
|
|
real (8) r_a
|
|
real (8) r_e2
|
|
end type ellipsoid
|
|
type (ellipsoid) elp
|
|
|
|
type pegtype
|
|
sequence
|
|
real (8) r_lat
|
|
real (8) r_lon
|
|
real (8) r_hdg
|
|
end type pegtype
|
|
type (pegtype) peg
|
|
|
|
c OUTPUT VARIABLES:
|
|
|
|
type pegtrans
|
|
sequence
|
|
real (8) r_mat(3,3)
|
|
real (8) r_matinv(3,3)
|
|
real (8) r_ov(3)
|
|
real (8) r_radcur
|
|
end type pegtrans
|
|
type (pegtrans) ptm
|
|
|
|
c LOCAL VARIABLES:
|
|
integer i,j,i_type
|
|
real*8 r_llh(3),r_p(3),r_slt,r_clt,r_clo,r_slo,r_up(3)
|
|
real*8 r_chg,r_shg,rdir
|
|
|
|
c DATA STATEMENTS:none
|
|
|
|
C FUNCTION STATEMENTS:
|
|
external rdir
|
|
|
|
c PROCESSING STEPS:
|
|
|
|
c first determine the rotation matrix
|
|
|
|
r_clt = cos(peg%r_lat)
|
|
r_slt = sin(peg%r_lat)
|
|
r_clo = cos(peg%r_lon)
|
|
r_slo = sin(peg%r_lon)
|
|
r_chg = cos(peg%r_hdg)
|
|
r_shg = sin(peg%r_hdg)
|
|
|
|
ptm%r_mat(1,1) = r_clt*r_clo
|
|
ptm%r_mat(1,2) = -r_shg*r_slo - r_slt*r_clo*r_chg
|
|
ptm%r_mat(1,3) = r_slo*r_chg - r_slt*r_clo*r_shg
|
|
ptm%r_mat(2,1) = r_clt*r_slo
|
|
ptm%r_mat(2,2) = r_clo*r_shg - r_slt*r_slo*r_chg
|
|
ptm%r_mat(2,3) = -r_clo*r_chg - r_slt*r_slo*r_shg
|
|
ptm%r_mat(3,1) = r_slt
|
|
ptm%r_mat(3,2) = r_clt*r_chg
|
|
ptm%r_mat(3,3) = r_clt*r_shg
|
|
|
|
do i=1,3
|
|
do j=1,3
|
|
ptm%r_matinv(i,j) = ptm%r_mat(j,i)
|
|
enddo
|
|
enddo
|
|
|
|
c find the translation vector
|
|
|
|
ptm%r_radcur = rdir(elp%r_a,elp%r_e2,peg%r_hdg,peg%r_lat)
|
|
|
|
i_type = 1
|
|
r_llh(1) = peg%r_lat
|
|
r_llh(2) = peg%r_lon
|
|
r_llh(3) = 0.0d0
|
|
call latlon(elp,r_p,r_llh,i_type)
|
|
|
|
r_clt = cos(peg%r_lat)
|
|
r_slt = sin(peg%r_lat)
|
|
r_clo = cos(peg%r_lon)
|
|
r_slo = sin(peg%r_lon)
|
|
r_up(1) = r_clt*r_clo
|
|
r_up(2) = r_clt*r_slo
|
|
r_up(3) = r_slt
|
|
|
|
do i=1,3
|
|
ptm%r_ov(i) = r_p(i) - ptm%r_radcur*r_up(i)
|
|
enddo
|
|
|
|
end
|
|
|
|
c****************************************************************
|
|
c
|
|
c Various curvature functions
|
|
c
|
|
c
|
|
c****************************************************************
|
|
c**
|
|
c** FILE NAME: curvature.f
|
|
c**
|
|
c** DATE WRITTEN: 12/02/93
|
|
c**
|
|
c** PROGRAMMER:Scott Hensley
|
|
c**
|
|
c** FUNCTIONAL DESCRIPTION: This routine computes the curvature for
|
|
c** of various types required for ellipsoidal or spherical earth
|
|
c** calculations.
|
|
c**
|
|
c** ROUTINES CALLED:none
|
|
c**
|
|
c** NOTES: none
|
|
c**
|
|
c** UPDATE LOG:
|
|
c**
|
|
c*****************************************************************
|
|
|
|
real*8 function reast(r_a,r_e2,r_lat)
|
|
|
|
implicit none
|
|
real*8 r_a,r_e2,r_lat
|
|
|
|
reast = r_a/sqrt(1.d0 - r_e2*sin(r_lat)**2)
|
|
|
|
end
|
|
|
|
real*8 function rnorth(r_a,r_e2,r_lat)
|
|
|
|
implicit none
|
|
real*8 r_a,r_e2,r_lat
|
|
|
|
rnorth = (r_a*(1.d0 - r_e2))/(1.d0 - r_e2*sin(r_lat)**2)**(1.5d0)
|
|
|
|
end
|
|
|
|
real*8 function rdir(r_a,r_e2,r_hdg,r_lat)
|
|
|
|
implicit none
|
|
real*8 r_a,r_e2,r_lat,r_hdg,r_re,r_rn,reast,rnorth
|
|
|
|
r_re = reast(r_a,r_e2,r_lat)
|
|
r_rn = rnorth(r_a,r_e2,r_lat)
|
|
|
|
rdir = (r_re*r_rn)/(r_re*cos(r_hdg)**2 + r_rn*sin(r_hdg)**2)
|
|
|
|
end
|
|
|
|
|
|
|
|
|
|
c****************************************************************
|
|
|
|
subroutine convert_sch_to_xyz(ptm,r_schv,r_xyzv,i_type)
|
|
|
|
c****************************************************************
|
|
c**
|
|
c** FILE NAME: convert_sch_to_xyz.for
|
|
c**
|
|
c** DATE WRITTEN:1/15/93
|
|
c**
|
|
c** PROGRAMMER:Scott Hensley
|
|
c**
|
|
c** FUNCTIONAL DESCRIPTION: This routine applies the affine matrix
|
|
c** provided to convert the sch coordinates xyz WGS-84 coordintes or
|
|
c** the inverse transformation.
|
|
c**
|
|
c** ROUTINES CALLED:latlon,matvec
|
|
c**
|
|
c** NOTES: none
|
|
c**
|
|
c** UPDATE LOG:
|
|
c**
|
|
c*****************************************************************
|
|
|
|
implicit none
|
|
|
|
c INPUT VARIABLES:
|
|
|
|
c structure /pegtrans/ !transformation parameters
|
|
c real*8 r_mat(3,3)
|
|
c real*8 r_matinv(3,3)
|
|
c real*8 r_ov(3)
|
|
c real*8 r_radcur
|
|
c end structure
|
|
c record /pegtrans/ ptm
|
|
|
|
type pegtrans
|
|
sequence
|
|
real (8) r_mat(3,3)
|
|
real (8) r_matinv(3,3)
|
|
real (8) r_ov(3)
|
|
real (8) r_radcur
|
|
end type pegtrans
|
|
type (pegtrans) ptm
|
|
|
|
real*8 r_schv(3) !sch coordinates of a point
|
|
real*8 r_xyzv(3) !WGS-84 coordinates of a point
|
|
integer i_type !i_type = 0 sch => xyz ;
|
|
!i_type = 1 xyz => sch
|
|
|
|
c OUTPUT VARIABLES: see input
|
|
|
|
c LOCAL VARIABLES:
|
|
integer i_t
|
|
real*8 r_schvt(3),r_llh(3)
|
|
c structure /ellipsoid/
|
|
c real*8 r_a
|
|
c real*8 r_e2
|
|
c end structure
|
|
c record /ellipsoid/ sph
|
|
|
|
type ellipsoid
|
|
sequence
|
|
real (8) r_a
|
|
real (8) r_e2
|
|
end type ellipsoid
|
|
type (ellipsoid) sph
|
|
|
|
c DATA STATEMENTS:
|
|
|
|
C FUNCTION STATEMENTS:none
|
|
|
|
c PROCESSING STEPS:
|
|
|
|
c compute the linear portion of the transformation
|
|
|
|
sph%r_a = ptm%r_radcur
|
|
sph%r_e2 = 0.0d0
|
|
|
|
if(i_type .eq. 0)then
|
|
|
|
r_llh(1) = r_schv(2)/ptm%r_radcur
|
|
r_llh(2) = r_schv(1)/ptm%r_radcur
|
|
r_llh(3) = r_schv(3)
|
|
|
|
i_t = 1
|
|
call latlon(sph,r_schvt,r_llh,i_t)
|
|
call matvec(ptm%r_mat,r_schvt,r_xyzv)
|
|
call lincomb(1.d0,r_xyzv,1.d0,ptm%r_ov,r_xyzv)
|
|
|
|
elseif(i_type .eq. 1)then
|
|
|
|
call lincomb(1.d0,r_xyzv,-1.d0,ptm%r_ov,r_schvt)
|
|
call matvec(ptm%r_matinv,r_schvt,r_schv)
|
|
i_t = 2
|
|
call latlon(sph,r_schv,r_llh,i_t)
|
|
|
|
r_schv(1) = ptm%r_radcur*r_llh(2)
|
|
r_schv(2) = ptm%r_radcur*r_llh(1)
|
|
r_schv(3) = r_llh(3)
|
|
|
|
endif
|
|
|
|
end
|
|
|
|
|
|
c****************************************************************
|
|
|
|
subroutine convert_schdot_to_xyzdot(ptm,r_sch,r_xyz,r_schdot,
|
|
+ r_xyzdot,i_type)
|
|
|
|
c****************************************************************
|
|
c**
|
|
c** FILE NAME: convert_schdot_to_xyzdot.f
|
|
c**
|
|
c** DATE WRITTEN:1/15/93
|
|
c**
|
|
c** PROGRAMMER:Scott Hensley
|
|
c**
|
|
c** FUNCTIONAL DESCRIPTION: This routine applies the affine matrix
|
|
c** provided to convert the sch velocity to xyz WGS-84 velocity or
|
|
c** the inverse transformation.
|
|
c**
|
|
c** ROUTINES CALLED: latlon,matvec
|
|
c**
|
|
c** NOTES: none
|
|
c**
|
|
c** UPDATE LOG:
|
|
c**
|
|
c*****************************************************************
|
|
|
|
implicit none
|
|
|
|
c INPUT VARIABLES:
|
|
|
|
c structure /pegtrans/ !transformation parameters
|
|
c real*8 r_mat(3,3)
|
|
c real*8 r_matinv(3,3)
|
|
c real*8 r_ov(3)
|
|
c real*8 r_radcur
|
|
c end structure
|
|
c record /pegtrans/ ptm
|
|
|
|
type pegtrans !transformation parameters
|
|
sequence
|
|
real*8 r_mat(3,3)
|
|
real*8 r_matinv(3,3)
|
|
real*8 r_ov(3)
|
|
real*8 r_radcur
|
|
end type pegtrans
|
|
type (pegtrans) ptm
|
|
|
|
real*8 r_sch(3) !sch coordinates of a point
|
|
real*8 r_xyz(3) !xyz coordinates of a point
|
|
real*8 r_schdot(3) !sch velocity
|
|
real*8 r_xyzdot(3) !WGS-84 velocity
|
|
integer i_type !i_type = 0 sch => xyz
|
|
!i_type = 1 xyz => sch
|
|
|
|
c OUTPUT VARIABLES: see input
|
|
|
|
c LOCAL VARIABLES:
|
|
|
|
real*8 r_cs,r_ss,r_cc,r_sc,r_hu,r_huf,r_temp(3),r_vpxyz(3)
|
|
real*8 r_tv(3),r_xp(3),r_xtemp,r_xn,r_xpr,r_xndot
|
|
|
|
c DATA STATEMENTS:
|
|
|
|
C FUNCTION STATEMENTS:none
|
|
|
|
c PROCESSING STEPS:
|
|
|
|
if(i_type .eq. 0)then !convert from sch velocity to xyz velocity
|
|
|
|
c To convert the velocity data, transfer the s and c velocities
|
|
c to the surface and then compute the xyz prime velocity
|
|
|
|
r_cs = cos(r_sch(1)/ptm%r_radcur)
|
|
r_ss = sin(r_sch(1)/ptm%r_radcur)
|
|
r_cc = cos(r_sch(2)/ptm%r_radcur)
|
|
r_sc = sin(r_sch(2)/ptm%r_radcur)
|
|
|
|
r_hu = ptm%r_radcur + r_sch(3)
|
|
r_hu = ptm%r_radcur/r_hu
|
|
r_huf = 1.d0/r_hu
|
|
r_temp(1) = r_schdot(1)*r_hu*r_cc
|
|
r_temp(2) = r_schdot(2)*r_hu
|
|
|
|
c compute the primed velocity
|
|
|
|
r_vpxyz(1) = -r_huf*r_cc*r_ss*r_temp(1) - r_huf*r_sc*r_cs*
|
|
+ r_temp(2) + r_cc*r_cs*r_schdot(3)
|
|
r_vpxyz(2) = r_huf*r_cc*r_cs*r_temp(1) - r_huf*r_sc*r_ss*
|
|
+ r_temp(2) + r_cc*r_ss*r_schdot(3)
|
|
r_vpxyz(3) = r_huf*r_cc*r_temp(2) + r_sc*r_schdot(3)
|
|
|
|
c convert to xyz velocity (WGS-84)
|
|
|
|
call matvec(ptm%r_mat,r_vpxyz,r_xyzdot)
|
|
|
|
elseif(i_type .eq. 1)then !convert from xyz velocity to sch velocity
|
|
|
|
c convert xyz position and velocity to primed position and velocity
|
|
|
|
call matvec(ptm%r_matinv,r_xyzdot,r_vpxyz)
|
|
call lincomb(1.d0,r_xyz,-1.d0,ptm%r_ov,r_tv)
|
|
call matvec(ptm%r_matinv,r_tv,r_xp)
|
|
|
|
c convert to an sch velocity
|
|
|
|
r_xtemp = ptm%r_radcur + r_sch(3)
|
|
r_xp(1) = r_xtemp*cos(r_sch(2)/ptm%r_radcur)*
|
|
+ cos(r_sch(1)/ptm%r_radcur)
|
|
r_xp(2) = r_xtemp*cos(r_sch(2)/ptm%r_radcur)*
|
|
+ sin(r_sch(1)/ptm%r_radcur)
|
|
r_xp(3) = r_xtemp*sin(r_sch(2)/ptm%r_radcur)
|
|
|
|
r_xn = sqrt(r_xp(1)**2+r_xp(2)**2+r_xp(3)**2)
|
|
r_xpr = r_xp(1)**2 + r_xp(2)**2
|
|
r_xndot = (r_xp(1)*r_vpxyz(1) + r_xp(2)*r_vpxyz(2) +
|
|
+ r_xp(3)*r_vpxyz(3))/r_xn
|
|
|
|
r_schdot(1) = (ptm%r_radcur/r_xpr)*(r_xp(1)*
|
|
+ r_vpxyz(2)-r_xp(2)*r_vpxyz(1))
|
|
r_schdot(2) = (ptm%r_radcur/(r_xn*sqrt(r_xpr)))*
|
|
+ (r_xn*r_vpxyz(3) - r_xp(3)*r_xndot)
|
|
r_schdot(3) = r_xndot
|
|
|
|
c rescale to aircraft height
|
|
|
|
r_schdot(1) = (sqrt(r_xpr)/ptm%r_radcur)*r_schdot(1)
|
|
r_schdot(2) = (r_xn/ptm%r_radcur)*r_schdot(2)
|
|
|
|
endif
|
|
|
|
end
|
|
|
|
|
|
|
|
c****************************************************************
|
|
subroutine latlon(elp,r_v,r_llh,i_type)
|
|
|
|
c****************************************************************
|
|
c**
|
|
c** FILE NAME: latlon.f
|
|
c**
|
|
c** DATE WRITTEN:7/22/93
|
|
c**
|
|
c** PROGRAMMER:Scott Hensley
|
|
c**
|
|
c** FUNCTIONAL DESCRIPTION:This program converts a vector to
|
|
c** lat,lon and height above the reference ellipsoid or given a
|
|
c** lat,lon and height produces a geocentric vector.
|
|
c**
|
|
c** ROUTINES CALLED:none
|
|
c**
|
|
c** NOTES: none
|
|
c**
|
|
c** UPDATE LOG:
|
|
c**
|
|
c****************************************************************
|
|
|
|
implicit none
|
|
|
|
c INPUT VARIABLES:
|
|
integer i_type !1=lat,lon to vector,2= vector to lat,lon
|
|
c structure /ellipsoid/
|
|
c real*8 r_a
|
|
c real*8 r_e2
|
|
c end structure
|
|
c record /ellipsoid/ elp
|
|
|
|
type ellipsoid
|
|
sequence
|
|
real (8) r_a
|
|
real (8) r_e2
|
|
end type ellipsoid
|
|
type (ellipsoid) elp
|
|
|
|
real*8 r_v(3) !geocentric vector (meters)
|
|
real*8 r_llh(3) !latitude (deg -90 to 90),longitude (deg -180 to 180),height
|
|
|
|
c OUTPUT VARIABLES: see input
|
|
|
|
c LOCAL VARIABLES:
|
|
real*8 pi,r_dtor,r_re,r_q2,r_q3,r_b,r_q
|
|
real*8 r_p,r_tant,r_theta,r_a,r_e2
|
|
|
|
c DATA STATEMENTS:
|
|
data pi /3.141592653589793238d0/
|
|
data r_dtor /1.74532925199d-2/
|
|
|
|
C FUNCTION STATEMENTS:
|
|
|
|
c PROCESSING STEPS:
|
|
|
|
r_a = elp%r_a
|
|
r_e2 = elp%r_e2
|
|
|
|
if(i_type .eq. 1)then !convert lat,lon to vector
|
|
|
|
r_re = r_a/sqrt(1.d0 - r_e2*sin(r_llh(1))**2)
|
|
|
|
r_v(1) = (r_re + r_llh(3))*cos(r_llh(1))*cos(r_llh(2))
|
|
r_v(2) = (r_re + r_llh(3))*cos(r_llh(1))*sin(r_llh(2))
|
|
r_v(3) = (r_re*(1.d0-r_e2) + r_llh(3))*sin(r_llh(1))
|
|
|
|
elseif(i_type .eq. 2)then !convert vector to lat,lon
|
|
|
|
r_q2 = 1.d0/(1.d0 - r_e2)
|
|
r_q = sqrt(r_q2)
|
|
r_q3 = r_q2 - 1.d0
|
|
r_b = r_a*sqrt(1.d0 - r_e2)
|
|
|
|
r_llh(2) = atan2(r_v(2),r_v(1))
|
|
|
|
r_p = sqrt(r_v(1)**2 + r_v(2)**2)
|
|
r_tant = (r_v(3)/r_p)*r_q
|
|
r_theta = atan(r_tant)
|
|
r_tant = (r_v(3) + r_q3*r_b*sin(r_theta)**3)/
|
|
+ (r_p - r_e2*r_a*cos(r_theta)**3)
|
|
r_llh(1) = atan(r_tant)
|
|
r_re = r_a/sqrt(1.d0 - r_e2*sin(r_llh(1))**2)
|
|
r_llh(3) = r_p/cos(r_llh(1)) - r_re
|
|
|
|
endif
|
|
|
|
end
|
|
|
|
|
|
c****************************************************************
|
|
|
|
subroutine lincomb(r_k1,r_u,r_k2,r_v,r_w)
|
|
|
|
c****************************************************************
|
|
c**
|
|
c** FILE NAME: lincomb.f
|
|
c**
|
|
c** DATE WRITTEN: 8/3/90
|
|
c**
|
|
c** PROGRAMMER:Scott Hensley
|
|
c**
|
|
c** FUNCTIONAL DESCRIPTION: The subroutine forms the linear combination
|
|
c** of two vectors.
|
|
c**
|
|
c** ROUTINES CALLED:none
|
|
c**
|
|
c** NOTES: none
|
|
c**
|
|
c** UPDATE LOG:
|
|
c**
|
|
c*****************************************************************
|
|
|
|
implicit none
|
|
|
|
c INPUT VARIABLES:
|
|
real*8 r_u(3) !3x1 vector
|
|
real*8 r_v(3) !3x1 vector
|
|
real*8 r_k1 !scalar
|
|
real*8 r_k2 !scalar
|
|
|
|
c OUTPUT VARIABLES:
|
|
real*8 r_w(3) !3x1 vector
|
|
|
|
c LOCAL VARIABLES:none
|
|
|
|
c PROCESSING STEPS:
|
|
|
|
c compute linear combination
|
|
|
|
r_w(1) = r_k1*r_u(1) + r_k2*r_v(1)
|
|
r_w(2) = r_k1*r_u(2) + r_k2*r_v(2)
|
|
r_w(3) = r_k1*r_u(3) + r_k2*r_v(3)
|
|
|
|
end
|
|
|
|
|
|
c****************************************************************
|
|
|
|
subroutine matvec(r_t,r_v,r_w)
|
|
|
|
c****************************************************************
|
|
c**
|
|
c** FILE NAME: matvec.f
|
|
c**
|
|
c** DATE WRITTEN: 7/20/90
|
|
c**
|
|
c** PROGRAMMER:Scott Hensley
|
|
c**
|
|
c** FUNCTIONAL DESCRIPTION: The subroutine takes a 3x3 matrix
|
|
c** and a 3x1 vector a multiplies them to return another 3x1
|
|
c** vector.
|
|
c**
|
|
c** ROUTINES CALLED:none
|
|
c**
|
|
c** NOTES: none
|
|
c**
|
|
c** UPDATE LOG:
|
|
c**
|
|
c*****************************************************************
|
|
|
|
implicit none
|
|
|
|
c INPUT VARIABLES:
|
|
real*8 r_t(3,3) !3x3 matrix
|
|
real*8 r_v(3) !3x1 vector
|
|
|
|
c OUTPUT VARIABLES:
|
|
real*8 r_w(3) !3x1 vector
|
|
|
|
c LOCAL VARIABLES:none
|
|
|
|
c PROCESSING STEPS:
|
|
|
|
c compute matrix product
|
|
|
|
r_w(1) = r_t(1,1)*r_v(1) + r_t(1,2)*r_v(2) + r_t(1,3)*r_v(3)
|
|
r_w(2) = r_t(2,1)*r_v(1) + r_t(2,2)*r_v(2) + r_t(2,3)*r_v(3)
|
|
r_w(3) = r_t(3,1)*r_v(1) + r_t(3,2)*r_v(2) + r_t(3,3)*r_v(3)
|
|
|
|
end
|
|
|
|
|