Difference between revisions of "Astro Mechanics"
(Created page with " TGeoid <nowiki> //--------------------------------------------------------------------------- // Class Name: TGeoid // // Description: Encapsulates Geodetic Ellipsoid Co...") |
(No difference)
|
Latest revision as of 18:14, 30 July 2019
TGeoid
//---------------------------------------------------------------------------
// Class Name: TGeoid
//
// Description: Encapsulates Geodetic Ellipsoid Coordinate Transformation
// Calculations.
//
// Written By: R. James Lanzi
//
// Notes:
//
// Modification History:
// 4/24/00 R.J.L. Encapsulated Geodetic Subroutine Library in TGeoid Class
// 4/16/02 Modified Gravitational Model from GEM-model to J4-Model
// 8/14/02 Modified Stand-Off Buffer calculation to include Growth Factor
// Capability
// 10/18/02 Stripped Stand-Off Buffer calculation form TGeoid Base Class -
// encapsulated these routines in a new TGeoBoundary child class.
// 4/24/03 Added EFG2TxConstants function to permit more efficient use of
// computed lat, long, and altitude. Use instead of EFG2Teg when
// having geodetic coordinates is desired.
// 12/10/03 Added Time To Impact and Time To Apogee as a Return Arguments of
// EFG2Kepler
// 1/9/04 Modified EFG2Kepler to return negative Time To Apogee if descending
// 3/8/04 Consolidated argument checking code into safeAtan2 and safeAcos2
// functions. Converted all acos and atan2 functions into safe
// versions.
// 3/8/04 Modified algorithm to iterate for Eccentric anomaly at epoch
// (converges faster). Also modified comment to correctly reflect
// output of true anomaly at epoch in EFG2Kepler.
// 3/8/04 Added Lots and Lots of Comments to support upcoming code review.
// 3/17/04 RA2ll modifications - Modified basis vector formation algorithm.
// Strengthened convergence criterion.
// 3/17/04 Improved protection against floating point exceptions in RA2ll,
// EFG2llh, EFG2Kepler, EFG2Gravity
// 9/13/04 Implemented Recommendations of Code Review held on 7/27/04
// - Added Comments.
// - Added data access for J2, J3, & J4
// - Updated J2,J3,J4 default parameters-directly trace to WGS84 EGM
// - Added temporary variables when working in different unit systems
// - Modified EFGv2llhv to call EFG2TxConstants to minimize code
// segment duplication
// - Added Error Status Return value to all Kepler Returns.
// Included addition of enumerated data type: TGeoid::EKeplerStatus
// - Modified EFG2Gravity to eliminate trig and pow() calls
// 4/14/05 Implemented safeguards against divide-by-zero and sqrt argument
// faults in EFG2Kepler routine. (Vulnerability identified for perfectly
// circular orbits).
//
//---------------------------------------------------------------------------
#ifndef TGeoidH
#define TGeoidH
class TGeoid {
private:
double a, // Equatorial Radius
invf, // Inverse Flattening
wE, // Earth Rotation Rate
mu; // Geocentric Gravitational Constant
double J2,J3,J4; // Harmonic Gravitational Constants
double b; // Polar Radius
int kMaxIterate; // Used to cap number of times to iterate
double kDenomEps; // Denominator Epsilon for DivByZero Checks
double degToRad; // Degrees to Radians Conversion
double radToDeg; // Radians to Degress Conversion
public:
// Enumerated Error Status returned from all Kepler functions
enum EKeplerStatus {ksNormalExit, ksZeroRadius, ksZeroAngMom,
ksNonEllipticalOrbit};
public:
// Default Constructor uses WGS-84 Earth Model Constants
TGeoid(double = 6378137.000, // WGS84 Equatorial Radius (m)
double = 298.257223563, // WGS84 Inverse Flattening
double = 7.292115e-5, // WGS84 Rotation Rate of Earth (rad/sec)
double = 398600.5, // WGS84 Gravitational Constant (km^3/s^2)
double = 1.08263E-3, // J2 (derived from WGS84 C20)
double = -2.53215307E-6, // J3 (derived from WGS84 C30)
double = -1.61098761E-6); //J4 (derived from WGS84 C40)
//
// Model Constant Data Access Functions
//
double getEqRad(void) {return a;};
double getInvFlat(void) {return invf;};
double getPoleRad(void) {return b;};
double getOmega(void) {return wE;};
double getMu(void) {return mu;};
double getJ2(void) {return J2;};
double getJ3(void) {return J3;};
double getJ4(void) {return J4;};
void setEqRad(double _a);
void setInvFlat(double _invf);
void setPoleRad(double _b);
void setOmega(double _wE) {wE = _wE;};
void setMu(double _mu) {mu = _mu;};
void setJ2(double _J2) {J2 = _J2;};
void setJ3(double _J3) {J3 = _J3;};
void setJ4(double _J4) {J4 = _J4;};
void setMaxIterateCount(int _kMaxIterate) {kMaxIterate=_kMaxIterate;};
// Earth Radius Functions
double getLatCRad(double latC);
double getLatGRad(double latG);
//
// Geodetic Coordinate Transformation and Inverse Transformation Functions
//
// Latitude,Longitude <--> Range, Azimuth
void ll2RA(double lat0, double lon0, double lat1, double lon1,
double &Range, double &Azimuth );
void RA2ll(double lat0, double lon0, double Range, double Azimuth,
double &lat1, double &lon1 );
// Latitude,Longitude,Altitude <--> E,F,G Cartesian
void llh2EFG(double lat, double lon,double alt, double *EFG);
void EFG2llh(double *EFG, double &lat, double &lon, double &alt);
void llh2ECI(double time, double le0, double lat, double lon, double alt,
double *ECI);
// Lat., Long., Alt. <--> Cartesion + Geodetic Frame Direction Cosines
void llh2TxConstants(double latitude, double longitude, double altitude,
double *EFGSite,double *TegSite);
void EFG2TxConstants(double *EFGSite,double *TegSite, double &latitude,
double &longitude, double &altitude);
void EFG2Teg(double *EFGSite,double *TegSite);
// Latitude,Longitude,Altitude,Vel,FltEl,FltAz <--> E,F,G,Edot,Fdot,Gdot
void llhv2EFGv(double lat,double lon,double alt,
double vel,double fltel,double fltaz,
double *EFG,double *VEFG);
void EFGv2llhv(double *EFG, double *VEFG,
double &latitude, double &longitude, double &altitude,
double &Velocity, double &FltEl, double &FltAz);
void llhv2ECIv(double Time, double LE0, double latitude,
double longitude,double altitude,
double vel,double fltel,double fltaz,
double *ECI,double *VECI);
void ECIv2llhv(double time, double LE0, double *ECI, double *VECI,
double &latitude, double &longitude, double &altitude,
double &Velocity, double &FltEl, double &FltAz);
// Latitude,Longitude,Altitude <--> Slt Range, Look Az, Look El
void llh2RAE(double lat0, double lon0, double alt0,
double lat1, double lon1, double alt1,
double &Range, double &Azimuth, double &Elevation );
void RAE2llh(double lat0, double lon0, double alt0,
double Range, double Azimuth, double Elevation ,
double &lat1, double &lon1, double &alt1);
void RAE2EFG(double Range, double Azimuth, double Elevation,
double *EFGSite, double *TegSite, double *EFG);
// E,F,G,Edot,Fdot,Gdot <--> Keplerian Orbital Elements
EKeplerStatus EFG2Kepler(double GHA0, double t0,
double *EFG0, double *VEFG0, double ImpactTol,
double &a, double &p, double &ecc, double &inclination,
double &apogee, double &perigee, double &nu, double &nu0,
double &LonNode, double &ArgPeri,
double &LatImpact, double &LonImpact,
double &TimeToImpact, double &TimeToApogee,
int &OrbitFlag) ;
// Included for Backward Compatibility - Forwards call to current version
EKeplerStatus EFG2Kepler(double GHA0, double t0,
double *EFG0, double *VEFG0, double ImpactTol,
double &a, double &p, double &ecc, double &inclination,
double &apogee, double &perigee, double &nu, double &nu0,
double &LonNode, double &ArgPeri,
double &LatImpact, double &LonImpact,
int &OrbitFlag) ;
EKeplerStatus Kepler2EFG(double GHA0, double t, double a, double ecc, double nu0,
double Inclination, double LonNode, double ArgPeri,
double &nu, double *EFG, double *VEFG);
// Gravity/Coriolis Routines
void EFG2Gravity(double *EFG, double *gE);
void EFGv2RotAcc(double *x, double *v, double *arot);
// Private Utility Functions
protected:
double sign(double x) {return x<0.0 ? -1.0 : 1.0;};
void cross(double *firstVector, double *secondVector,double *outVector);
double dot(double *aVector, double *anotherVector);
void matv(double *aMatrix, double *aVector,
double *outVector,const int doTranspose);
double norm2(double *aVector);
public:
double safeAtan2(double y, double x);
double safeAcos(double x);
};
#endif
#include "TGeoid.h"
#include <math.h>
//
// ... Define PI constant if it is not already done
//
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
//-----------------------------------------------------------------------
// Function: TGeoid
//
// Purpose: Constructor Initializes Geoid Constants
//
//-----------------------------------------------------------------------
TGeoid::TGeoid(double _a, double _invf, double _wE, double _mu, double _J2,
double _J3, double _J4) {
a = _a;
invf = _invf;
b = a - a/invf;
wE = _wE;
mu = _mu;
J2 = _J2;
J3 = _J3;
J4 = _J4;
kMaxIterate = 30; // May or May not be suitable for Real-Time application
// Can be as low as 5 and produce satisfactory results
kDenomEps = 1.e-9;
// Global Conversion Factors
degToRad = M_PI/180.0;
radToDeg = 180.0/M_PI;
}
//-----------------------------------------------------------------------
// Function: setEqRad
//
// Purpose: Data Access Function for setting Equatorial Radius
//
//-----------------------------------------------------------------------
void TGeoid::setEqRad(double _a) {
a = _a;
b = a - a/invf;
}
//-----------------------------------------------------------------------
// Function: setInvFlat
//
// Purpose: Data Access Function for setting Inverse Flattening Constant
//
//-----------------------------------------------------------------------
void TGeoid::setInvFlat(double _invf) {
invf = _invf;
b = a - a/invf;
}
//-----------------------------------------------------------------------
// Function: setPoleRad
//
// Purpose: Data Access Function for setting Polar Radius
//
//-----------------------------------------------------------------------
void TGeoid::setPoleRad(double _b) {
b = _b;
invf = a/(a-b);
}
//-----------------------------------------------------------------------
// Function: getLatCRad
//
// Purpose: Fetch Geoid Radius at a Geocentric Latitude
//-----------------------------------------------------------------------
double TGeoid::getLatCRad(double LatC) {
double s,c;
s = sin(LatC*degToRad);
c = cos(LatC*degToRad);
return b/sqrt(s*s+(b/a)*(b/a)*c*c);
}
//-----------------------------------------------------------------------
// Function: getLatGRad
//
// Purpose: Fetch Geoid Radius at a Geodetic Latitude
//-----------------------------------------------------------------------
double TGeoid::getLatGRad(double LatG) {
double s,c;
s = sin(LatG*degToRad);
c = cos(LatG*degToRad);
return a*sqrt(c*c+(b/a)*(b/a)*(b/a)*(b/a)*s*s)/sqrt(c*c+(b/a)*(b/a)*s*s);
}
//-----------------------------------------------------------------------
// Function: ll2RA
//
// Purpose: Oblate Range/Azimuth Calculation from lat/lon coordinates
//
// Input Arguments:
// lat0,lon0 Reference Geodetic latitude and longitude (deg)
// lat1,lon1 Target Geodetic latitude and longitude (deg)
//
// Output Arguments (passed by reference):
// Range, Azimuth Range (m) and Azimuth (deg) from Reference to Target
// over oblate earth
//-----------------------------------------------------------------------
void TGeoid::ll2RA(double lat0, double lon0, double lat1, double lon1,
double &Range, double &Azimuth ) {
int i;
double Teg0[9],EFG0[3],EFG1[3],R0,R1;
double dR[3],dRg[3],phi;
// Start Point ECEF (3d) coordinates and direction cosines of geodetic frame
llh2TxConstants(lat0, lon0, 0.0, EFG0, Teg0);
// Finish point ECEF (3d) coordinates
llh2EFG(lat1, lon1, 0.0, EFG1);
// Earth Radii at Start and Finish Points
R0 = norm2(EFG0);
R1 = norm2(EFG1);
phi = 0.0;
for(i = 0; i<3; i++) {
// Look Vector from Start Point to Finish Point
dR[i] = EFG1[i] - EFG0[i];
// Dot product of Earth Centered Unit Vectors for Start and Finish Points
phi = phi + (EFG0[i]/R0)*(EFG1[i]/R1);
}
// Great Circle Angle between Start and Finish Points
phi = safeAcos(phi);
// Use Average Earth Radius for two points to compute a spherical earth range
Range = 0.5*(R0+R1)*phi;
// Compute components of look vector in Pt0 local North-East-Down frame
matv(Teg0,dR,dRg,1);
// Compute Azimuth from Pt0 local horizontal projection of look vector
Azimuth = safeAtan2(dRg[1],dRg[0])*radToDeg;
if(Azimuth < 0.0) Azimuth += 360.0;
}
//-----------------------------------------------------------------------
// Function: RA2ll
//
// Purpose: Oblate lat/lon Calculation from Range/Azimuth coordinates
//
// Input Arguments:
// lat0,lon0 Reference Geodetic latitude and longitude (deg)
// Range, Azimuth Range (m) and Azimuth (deg) from Reference to Target
// over oblate earth
//
// Output Arguments: (Passed by reference)
// lat1,lon1 Target Geodetic latitude and longitude (deg)
//
// NOTE: This function suffers from geometrically induced numerical ill
// conditioning for Range values that produce great-circle arc angles near
// 180-deg. The algorithm will function for such inputs; however, the
// iterations will last longer, and the results will not be perfectly invertible
// with the ll2RA function.
//
//-----------------------------------------------------------------------
void TGeoid::RA2ll(double lat0, double lon0, double Range, double Azimuth,
double &lat1, double &lon1 ) {
int Iterate;
double Teg0[9],EFG0[3],EFG1[3],R0,R1,alt1,Ravg,Elevation,deltaEl;
double phi;
double deltaR;
// Start Point ECEF coodinates and direction cosines of local geodetic frame
llh2TxConstants(lat0,lon0,0.0,EFG0,Teg0);
// Earth Radius at Start Point
R0 = norm2(EFG0);
// Initialize Guesses for Average Earth Radius and Elevation Bias
R1 = R0;
deltaEl = 0.0;
Iterate = 0;
do {
// Compute Average Earth Radius based Upon Finish Point Radius Guess
Ravg = 0.5*(R0+R1);
// Great circle angle between start and finish point
phi = Range/Ravg;
// Straight line distance from start to finish point
deltaR = R0*R0 + R1*R1 - 2.0*R0*R1*cos(phi);
if(deltaR >= 0.0) {
deltaR = sqrt(deltaR);
}
else {
// This condition can only happen from poor numerical conditioning
// around phi = 0
deltaR = 0.0;
}
// Best guess at straight line look elevation from start to finish
Elevation = (-phi/2.0 + deltaEl)*radToDeg;
// Shoot to finish point estimate
RAE2EFG(deltaR, Azimuth, Elevation, EFG0, Teg0, EFG1);
// Compute finish point estimate lat long and altitude
EFG2llh(EFG1,lat1,lon1,alt1);
// Adjust Finish Point Radius to be used in Great Circle Angle Calculation
R1 = getLatGRad(lat1);
// Adjust Elevation Bias
if(fabs(deltaR) > kDenomEps) {
deltaEl += -cos(phi/2.0)*alt1/deltaR;
}
Iterate++;
// Algorithm has converged when finish point lands on ellipsoid
} while (fabs(alt1)>1.e-5 && Iterate < kMaxIterate);
}
//-----------------------------------------------------------------------
// Function: llh2EFG
//
// Purpose: Convert from Latitude, Longitude, and Altitude
// to Earth Fixed Geocentric (EFG) X,Y, and Z coordinates
//
// Input:
// latitude Geodetic latitude (deg)
// longitude Longitude (deg)
// altitude Height above ellipsoid (m)
//
// Output: (Passed by pointer)
// EFG[3] E, F, and G coordinates in meters
//
//-----------------------------------------------------------------------
void TGeoid::llh2EFG(double latitude, double longitude, double altitude,
double *EFG)
{
double sph,cph,slm,clm,tmp,X;
// Do Trig Once.
sph = sin(latitude*degToRad);
cph = cos(latitude*degToRad);
slm = sin(longitude*degToRad);
clm = cos(longitude*degToRad);
// Intermediate Values
tmp = sqrt((b/a)*(b/a)*sph*sph+cph*cph);
X = a*cph/tmp + altitude*cph;
EFG[0] = X*clm;
EFG[1] = X*slm;
EFG[2] = b/a*b*sph/tmp + altitude*sph;
}
//-----------------------------------------------------------------------
// Function: EFG2llh
//
// Purpose: Convert from EFG (Earth Fixed Geocentric) Coordinates to
// Latitude, Longitude, Altitude
//
// Input Argument:
// EFG[3] E, F, and G coordinates in meters
//
// Output: (Passed by reference)
// latitude Geodetic latitude (deg)
// longitude Longitude (deg)
// altitude Height above ellipsoid (m)
//-----------------------------------------------------------------------
void TGeoid::EFG2llh(double *EFG, double &latitude, double &longitude,
double &altitude)
{
double lat_c,rI,u,uold,sph,cph,Xs,Zs,tmp,Dx,Dz,latRadians;
int i;
// Pos Vector Length
rI = norm2(EFG);
// Protect Against Floating Point Exceptions stemming from rI = 0
if(rI < kDenomEps) {
latitude = 0.0;
longitude = 0.0;
altitude = -getEqRad();
return;
}
// Geocentric Latitude
lat_c = safeAtan2(EFG[2],sqrt(EFG[0]*EFG[0]+EFG[1]*EFG[1]));
longitude = safeAtan2(EFG[1],EFG[0])*radToDeg;
sph = sin(lat_c);
cph = cos(lat_c);
// Iteration for Geodetic Latitude
// Source: GEM User's Manual, Pg. A-9 & A-10
u = 1.0;
for(i = 0; i<kMaxIterate; i++) {
uold = u;
u = b/a + (a-b/a*b)*u/rI/sqrt(cph*cph+u*u*sph*sph);
// Stop iterating when we have done the best we can
if(u == uold) break;
}
latRadians = safeAtan2(u*a/b*sph,cph);
sph = sin(latRadians);
cph = cos(latRadians);
latitude = latRadians*radToDeg;
// Surface 'X' and Z Coordinates
tmp = sqrt((b/a)*(b/a)*sph*sph+cph*cph);
Xs = a*cph/tmp;
Zs = b/a*b*sph/tmp;
// In-Plane Components of Vector from Sub-Point to Point of Interest
Dx = sqrt(EFG[0]*EFG[0]+EFG[1]*EFG[1]) - Xs;
Dz = EFG[2] - Zs;
// Altitude:
// Magnitude = Magniude of Subpoint to Vehicle Vector
// Sign based upon dot product of subpoint to Vehicle vector w/local Normal
altitude = sign(Dx*Xs+Dz*Zs)*sqrt(Dx*Dx + Dz*Dz);
}
//-----------------------------------------------------------------------
// Function: llhv2EFGv
//
// Purpose: Position and velocity coordinate transformations
// Geodetic to ECEF Cartesian
//
// Input:
// latitude Geodetic latitude (deg)
// longitude Longitude (deg)
// altitude Height above ellipsoid (m)
// vel Velocity (m/s)
// fltel Velocity vector elevation (deg)
// fltaz Velocity vector azimuth (deg)
//
// Output: (Passed by Pointer)
// EFG[3] E, F, and G coordinates in meters
// VEFG[3] Edot, Fdot, Gdot (m/s)
//
//-----------------------------------------------------------------------
void TGeoid::llhv2EFGv(double latitude,double longitude,double altitude,
double vel,double fltel,double fltaz,
double *EFG,double *VEFG)
{
double Teg[9],Vg[3],ce;
// Compute ECEF position coordinates and direction cosines of local geodetic
llh2TxConstants(latitude, longitude, altitude,EFG,Teg);
// Compute velocity components in local geodetic (NED) frame
ce = cos(fltel*degToRad);
Vg[0] = vel*ce*cos(fltaz*degToRad);
Vg[1] = vel*ce*sin(fltaz*degToRad);
Vg[2] = -vel*sin(fltel*degToRad);
// Transform to ECEF components
matv(Teg,Vg,VEFG,0);
}
//-----------------------------------------------------------------------
// Function: llh2ECI
//
// Purpose: Position coordinate transformations
// Geodetic to Earth Centered Inertial Cartesian
//
// Input:
// Time Elapsed Time since Epoch (sec)
// LE0 Greenwich Longitude at Epoch (deg)
// latitude Geodetic latitude (deg)
// longitude Longitude (deg)
// altitude Height above ellipsoid (m)
//
// Output:
// ECI[3] E, F, and G coordinates in meters
//
//-----------------------------------------------------------------------
void TGeoid::llh2ECI(double Time, double LE0, double latitude,
double longitude,double altitude,
double *ECI)
{
double LongI;
// Inertial Longitude = Start Position of Earth + Earth Rotation + Longitude
LongI = LE0 + wE*Time*radToDeg + longitude;
// Can use ECEF Library Routine to do Coordinate Transformation
llh2EFG(latitude, LongI, altitude, ECI);
}
//-----------------------------------------------------------------------
// Function: llhv2ECIv
//
// Purpose: Position and velocity coordinate transformations
// Geodetic to Earth Centered Inertial Cartesian
//
// Input:
// Time Elapsed Time since Epoch (sec)
// LE0 Greenwich Longitude at Epoch (deg)
// latitude Geodetic latitude (deg)
// longitude Longitude (deg)
// altitude Height above ellipsoid (m)
// vel Velocity (m/s)
// fltel Velocity vector elevation (deg)
// fltaz Velocity vector azimuth (deg)
//
// Output:
// ECI[3] E, F, and G coordinates in meters
// VECI[3] Edot, Fdot, Gdot (m/s)
//
//-----------------------------------------------------------------------
void TGeoid::llhv2ECIv(double Time, double LE0, double latitude,
double longitude,double altitude,
double vel,double fltel,double fltaz,
double *ECI,double *VECI)
{
double Teg[9],Vg[3],ce,LongI;
// Inertial Longitude = Start Position of Earth + Earth Rotation + Longitude
LongI = LE0 + wE*Time*radToDeg + longitude;
// Compute Inertial Coordinates and Direction Cosines of 'Inertial Geodetic'
llh2TxConstants(latitude, LongI, altitude, ECI, Teg);
// Compute Components of Earth Relative Velocity in Inertial Geodetic Frame
ce = cos(fltel*degToRad);
Vg[0] = vel*ce*cos(fltaz*degToRad);
Vg[1] = vel*ce*sin(fltaz*degToRad);
Vg[2] = -vel*sin(fltel*degToRad);
// Transform from Geodetic to Earth Centered Inertial Components
matv(Teg,Vg,VECI,0);
// Add Earths Rotation to form Inertial Velocity
VECI[0] = VECI[0] - wE*ECI[1];
VECI[1] = VECI[1] + wE*ECI[0];
}
//-----------------------------------------------------------------------
// Function: ECIv2llhv
//
// Purpose: Convert from Earth Centered Inertial cartesian components of
// position and velocity to local geodetic position and velocity
//
// Input:
// time Elapsed Time since Epoch (sec)
// LE0 Greenwich Longitude at Epoch (deg)
// ECI[3] XI,YI,ZI (m)
// VECI[3] X,Y,Z Inertial Velocity (m/s)
//
// Output: (Passed by reference)
// latitude Geodetic latitude (deg)
// longitude Longitude (deg)
// altitude Height above ellipsoid (m)
// Velocity Magnitude (m/s)
// FltAz Flight Azimuth (deg)
// FltEl Flight Elevation (deg)
//-----------------------------------------------------------------------
void TGeoid::ECIv2llhv(double time, double LE0, double *ECI, double *VECI,
double &latitude, double &longitude, double &altitude,
double &Velocity, double &FltEl, double &FltAz)
{
double LE,ce,se;
double EFG[3],VEFG[3],tmp[3];
// Locate 0-deg Longitude relative to ECI-X
LE = LE0 + wE*time*radToDeg;
ce = cos(LE*degToRad);
se = sin(LE*degToRad);
// Convert to Earth Relative Velocity
tmp[0] = VECI[0] + wE*ECI[1];
tmp[1] = VECI[1] - wE*ECI[0];
tmp[2] = VECI[2];
// Coordinate transformation
VEFG[0] = ce*tmp[0]+se*tmp[1];
VEFG[1] = -se*tmp[0]+ce*tmp[1];
VEFG[2] = tmp[2];
// Convert to EFG Coordinates
EFG[0] = ce*ECI[0]+se*ECI[1];
EFG[1] = -se*ECI[0]+ce*ECI[1];
EFG[2] = ECI[2];
// Can now use library routine for earth relative position and vel.
EFGv2llhv(EFG, VEFG, latitude, longitude, altitude, Velocity, FltEl, FltAz);
}
//-----------------------------------------------------------------------
// Function: EFGv2llhv
//
// Purpose: Convert from ECEF cartesian components of position and velocity
// to local geodetic position and velocity
//
// Input:
// EFG[3] E, F, and G (m)
// VEFG[3] E, F, and G Velocity (m/s)
//
// Output: (Passed by reference)
// latitude Geodetic latitude (deg)
// longitude Longitude (deg)
// altitude Height above ellipsoid (m)
// Velocity Magnitude (m/s)
// FltAz Flight Azimuth (deg)
// FltEl Flight Elevation (deg)
//-----------------------------------------------------------------------
void TGeoid::EFGv2llhv(double *EFG, double *VEFG,
double &latitude, double &longitude, double &altitude,
double &Velocity, double &FltEl, double &FltAz)
{
double TegSite[9];
double Vg[3],tmp;
// Magnitude of velocity vector
Velocity = norm2(VEFG);
EFG2TxConstants(EFG,TegSite,latitude,longitude,altitude);
// Transform velocity from ECEF components to local geodetic components
matv(TegSite,VEFG,Vg,1);
// Project velocity into local horizontal plane
tmp = sqrt(Vg[0]*Vg[0]+Vg[1]*Vg[1]);
// Can now use atan2 to compute flight elevation
FltEl = safeAtan2(-Vg[2],tmp)*radToDeg;
// Azimuth of velocity vector = azimuth of horizontal projection of velocity
FltAz = safeAtan2(Vg[1],Vg[0])*radToDeg;
if(FltAz < 0.0) FltAz += 360.0;
}
//-----------------------------------------------------------------------
// Function: llh2RAE
//
// Purpose: Slant Range/Look Azimuth/Look Elevation between two positions
// from lat/lon/alt coordinates
//
// Input:
// lat0,lon0 Reference Geodetic latitude, longitude (deg)
// alt0 and altitude (m)
// lat1,lon1 Target Geodetic latitude, longitude (deg)
// alt1 and altitude (m)
//
// Output: (Passed by reference)
// Range Slant Range from Reference to Target (m)
// Azimuth Look Azimuth from Reference to Target (d)
// Elevation Look Elevation from Reference to Target (d)
//-----------------------------------------------------------------------
void TGeoid::llh2RAE(double lat0, double lon0, double alt0,
double lat1, double lon1, double alt1,
double &Range, double &Azimuth, double &Elevation )
{
int i;
double Teg0[9],EFG0[3],EFG1[3];
double dRe[3],dRg[3];
double Rxy;
// Compute Starting Point ECEF-EFG coodinates and local geodetic frame
// direction cosines.
llh2TxConstants(lat0,lon0,alt0,EFG0,Teg0);
// Compute Finish Point ECEF-EFG coordinates
llh2EFG(lat1,lon1,alt1,EFG1);
// Compute Look Vector from Start(Pt0) to Finish (Pt1) (in ECEF coordinates)
for(i = 0;i<3;i++) {
dRe[i] = EFG1[i] - EFG0[i];
}
// Magnitude of Look Vector = Range
Range = norm2(dRe);
// Transform Look Vector into Local Geodetic components @ Pt0
matv(Teg0,dRe,dRg,1);
// Magnitude of look vector projection in local horizontal plane @ Pt0
Rxy = sqrt(dRg[0]*dRg[0]+dRg[1]*dRg[1]);
// Can now use atan2 to compute elevation of look vector
Elevation = safeAtan2(-dRg[2],Rxy)*radToDeg;
// Azimuth of look vector = azimuth of horizontal projection of look vector
Azimuth = safeAtan2(dRg[1],dRg[0])*radToDeg;
if(Azimuth < 0.0) Azimuth += 360.0;
}
//-----------------------------------------------------------------------
// Function: RAE2llh
//
// Purpose: Compute target geodetic coordinates (lat,lon,alt) from a
// reference point (lat, lon, alt) and a look vector (range,az,el)
//
// Input:
// lat0,lon0 Reference Geodetic latitude, longitude (deg)
// alt0 and altitude (m)
// Range Slant Range from Reference to Target (m)
// Azimuth Look Azimuth from Reference to Target (d)
// Elevation Look Elevation from Reference to Target (d)
//
// Output: (Passed by reference)
// lat1,lon1 Target Geodetic latitude, longitude (deg)
// alt1 and altitude (m)
//-----------------------------------------------------------------------
void TGeoid::RAE2llh(double lat0, double lon0, double alt0,
double Range, double Azimuth, double Elevation ,
double &lat1, double &lon1, double &alt1)
{
int i;
double Teg0[9],EFG0[3],EFG1[3];
double dRe[3],dRg[3];
// Get Start ECEF Coordinates and Direction Cosines of North-East-Down
llh2TxConstants(lat0,lon0,alt0,EFG0,Teg0);
// Compute Range Vector in North-East-Down Coordinate System
dRg[0] = Range*cos(Elevation*degToRad)*cos(Azimuth*degToRad);
dRg[1] = Range*cos(Elevation*degToRad)*sin(Azimuth*degToRad);
dRg[2] = -Range*sin(Elevation*degToRad);
// Transform to ECEF Coordinate System
matv(Teg0,dRg,dRe,0);
// Add to Start Position Vector to get Final Position in ECEF Coordinates
for(i=0;i<3;i++) {
EFG1[i] = EFG0[i] + dRe[i];
}
// Transform to Geodetic Lat, Long and Alt
EFG2llh(EFG1,lat1,lon1,alt1);
}
//-----------------------------------------------------------------------
// Function: llh2TxConstants
//
// Purpose: Compute Geodetic Transformation Constants from
// Tracking Site Latitude, Longitude and Altitude Coordinates
//
// Input:
// latitude Geodetic latitude (deg)
// longitude Longitude (deg)
// altitude Height above ellipsoid (m)
//
// Output:
// EFGSite[3] Site E, F, and G coordinates in metres
// TegSite[9] Site Direction Cosine matrix (row-wise) of
// North-East-Down Coordinate system wrt EFG
//
//-----------------------------------------------------------------------
void TGeoid::llh2TxConstants(double latitude, double longitude, double altitude,
double *EFGSite,double *TegSite)
{
double sph,cph,slm,clm;
// EFG to Lat/Lon/Alt Conversion
llh2EFG(latitude,longitude,altitude,EFGSite);
// Do Trig Once
sph = sin(latitude*degToRad);
cph = cos(latitude*degToRad);
slm = sin(longitude*degToRad);
clm = cos(longitude*degToRad);
// Geodetic (North-East-Down) to ECEF-EFG Transformation Matrix (Row Wise)
TegSite[0] = -sph*clm;
TegSite[1] = -slm;
TegSite[2] = -cph*clm;
TegSite[3] = -sph*slm;
TegSite[4] = clm;
TegSite[5] = -cph*slm;
TegSite[6] = cph;
TegSite[7] = 0.0;
TegSite[8] = -sph;
}
//-----------------------------------------------------------------------
// Function: EFG2Teg
//
// Purpose: Compute Geodetic Transformation Matrix from
// Site EFG Coordinates
//
// Input:
// EFGSite[3] Site E, F, and G coordinates in meters
//
// Output:
// TegSite[9] Site Direction Cosine matrix of
// North-East-Down Coordinate system wrt EFG
//
//-----------------------------------------------------------------------
void TGeoid::EFG2Teg(double *EFGSite,double *TegSite)
{
double latitude,longitude,altitude;
EFG2TxConstants(EFGSite,TegSite,latitude,longitude,altitude);
}
//-----------------------------------------------------------------------
// Function: EFG2TxConstants
//
// Purpose: Compute Geodetic Transformation Matrix and Geodetic Coordinates
// from Site EFG Coordinates
//
// Input:
// EFGSite[3] Site E, F, and G coordinates in meters
//
// Output:
// TegSite[9] Site Direction Cosine matrix of
// North-East-Down Coordinate system wrt EFG
// latitude, longitude, altitude Geodetic Coordinates
//
//-----------------------------------------------------------------------
void TGeoid::EFG2TxConstants(double *EFGSite,double *TegSite,
double &latitude, double &longitude, double &altitude)
{
double sph,cph,slm,clm;
// EFG to Lat/Lon/Alt Conversion
EFG2llh(EFGSite,latitude,longitude,altitude);
// Direction Cosines Stored Row-Wise
sph = sin(latitude*degToRad);
cph = cos(latitude*degToRad);
slm = sin(longitude*degToRad);
clm = cos(longitude*degToRad);
// Geodetic to EFG Transformation Matrix
TegSite[0] = -sph*clm;
TegSite[1] = -slm;
TegSite[2] = -cph*clm;
TegSite[3] = -sph*slm;
TegSite[4] = clm;
TegSite[5] = -cph*slm;
TegSite[6] = cph;
TegSite[7] = 0.0;
TegSite[8] = -sph;
}
//-----------------------------------------------------------------------
// Function: RAE2EFG
//
// Purpose: Compute target ECEF cartesion position coordinates given
// reference coordinates/transformation matrix and look vector.
//
// Input:
// Range Slant Range from measurement Site (m)
// Azimuth Look-Azimuth from Site to Target(deg)
// Elevation Look-Elevation from Site to Target(deg)
// EFGSite[3] Site EFG Coordinates (m)
// TegSite[9] Site Transformation Matrix
//
// Output:
// EFG[3] E, F, and G coordinates of target (m)
//
//-----------------------------------------------------------------------
void TGeoid::RAE2EFG(double Range, double Azimuth, double Elevation,
double *EFGSite, double *TegSite, double *EFG)
{
int i,j;
double ce,se,ca,sa,Xg[3];
// Do all trig once.
ce = cos(Elevation*degToRad);
se = sin(Elevation*degToRad);
ca = cos(Azimuth*degToRad);
sa = sin(Azimuth*degToRad);
// Geodetic - North-East-Down (NED) Coordinates
Xg[0] = Range*ce*ca;
Xg[1] = Range*ce*sa;
Xg[2] = -Range*se;
// Transform from Geodetic-NED to ECEF-EFG
for(i=0; i<3; i++) {
EFG[i] = EFGSite[i];
for(j=0; j<3; j++) {
EFG[i] = EFG[i] + TegSite[3*i+j]*Xg[j];
}
}
}
//-----------------------------------------------------------------------
// Function: EFG2Kepler
//
// Purpose: Compute Keplerian Orbital Parameters from ECEF State Vector
//
// NOTE: This function works for ELLIPTICAL orbits ONLY. Function returns
// error status if it detects parabolic or hyperbolic orbit.
//
// NOTE: This function will return present latitude/longitude for impact if
// it is in a non-intersecting earth orbit.
//
// Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics," Dover
// Publications, 1971.
//
// Input Arguments:
// GHA0 Greenwich Hour Angle at Epoch (h)
// t0 Elapsed time since epoch (s)
// EFG0[3] Cartesian Components of ECEF position (m)
// VEFG0[3] Cartesian Components of ECEF velocity (m/s)
// ImpactTol Altitude Tolerance for Impact Point Iteration (m)
//
// Output Arguments (Passed By Reference):
// a Semi-Major Axis (km)
// p Semi-Latus Rectum (km)
// Ecc Orbit Eccentricity
// inclination Orbit Inclination (d)
// apogee True Apogee Altitude over Geoid (km)
// perigee True Perigee Altitude over Geoid (km)
// nu0 True Anomaly at t0 (d)
// nuE True Anomaly at Epoch (d)
// LonNode Longitude of Ascending Node (d)
// ArgPeri Argument of Perigee (d)
// LatImpact Impact Latitude (d)
// LonImpact Impact Longitude (d)
// TimeToImpact Time from Current Time to Impact
// TimeToApogee Time from Current Time to Next Apogee
// OrbitFlag = 0 if orbit intersects earth, = 1 otherwise
//
//-----------------------------------------------------------------------
TGeoid::EKeplerStatus TGeoid::EFG2Kepler(double GHA0, double t0,
double *EFG0, double *VEFG0,
double ImpactTol, double &a, double &p, double &ecc,
double &inclination, double &apogee, double &perigee,
double &nu0, double &nuE, double &LonNode, double &ArgPeri,
double &LatImpact, double &LonImpact,
double &TimeToImpact, double &TimeToApogee,
int &OrbitFlag)
{
double h[3],P[3],Q[3],n[3];
double rdotV,VdotV,r;
double Energy, AngMom;
double cnu0, cE0, E0,Eepoch;
double cEi,Ei,cnui,snui,nui;
double ce,se;
double tmp;
double r0[3],V0[3];
double AltImpact;
double EFGImpact[3];
double lat,lon;
double EFG[3];
double RImpact;
double Period;
int i;
int IterCount;
// Default Return Parameters
TimeToImpact = 0.0;
EFGImpact[0] = EFG0[0];
EFGImpact[1] = EFG0[1];
EFGImpact[2] = EFG0[2];
OrbitFlag = 0;
EFG2llh(EFGImpact,LatImpact,LonImpact,AltImpact);
// Switch from m to km for better numeric conditioning
for(i=0;i<3;i++) {
r0[i] = EFG0[i]*.001;
V0[i] = VEFG0[i]*.001;
}
// Transform from Earth Relative to Inertial Velocity
V0[0] = V0[0] - wE*r0[1];
V0[1] = V0[1] + wE*r0[0];
// Preliminaries
rdotV = r0[0]*V0[0] + r0[1]*V0[1] + r0[2]*V0[2];
VdotV = V0[0]*V0[0] + V0[1]*V0[1] + V0[2]*V0[2];
r = norm2(r0);
// Protect against Floating Point Exceptions due to zero Radius
if(r < kDenomEps) {
return ksZeroRadius;
}
// Angular Momentum, perigee, and perp. vector
// h = r x v
h[0] = r0[1]*V0[2]-r0[2]*V0[1];
h[1] = r0[2]*V0[0]-r0[0]*V0[2];
h[2] = r0[0]*V0[1]-r0[1]*V0[0];
AngMom = norm2(h);
if(AngMom < kDenomEps) {
return ksZeroAngMom;
}
// Perigee Vector
tmp = VdotV-mu/r;
P[0] = tmp*r0[0]-rdotV*V0[0];
P[1] = tmp*r0[1]-rdotV*V0[1];
P[2] = tmp*r0[2]-rdotV*V0[2];
// Normalize Perigee Vector while protecting against Circular Orbit Case.
tmp = norm2(P);
if(fabs(tmp) > kDenomEps) {
for(i = 0; i<3; i++) {
P[i] = P[i]/tmp;
}
}
else {
// Note: For Circular Orbit VdotV = mu/r and rdotV = 0.
// Therefore Perigee vector above degenerates to zero vector
// so we arbitrarily pick present position to be perigee direction
P[0] = r0[0]/r;
P[1] = r0[1]/r;
P[2] = r0[2]/r;
}
// Q = hxP/|h| - Unit Vector Perpendicular to Perigee and Ang. Momentum
Q[0] = (h[1]*P[2]-h[2]*P[1])/AngMom;
Q[1] = (h[2]*P[0]-h[0]*P[2])/AngMom;
Q[2] = (h[0]*P[1]-h[1]*P[0])/AngMom;
// Node vector = kxh
tmp = sqrt(h[0]*h[0]+h[1]*h[1]);
n[2] = 0.0;
if(tmp < kDenomEps) {
n[0] = 1.0;
n[1] = 0.0;
} else {
n[0] = -h[1]/tmp;
n[1] = h[0]/tmp;
}
// Longitude of Ascending Node
LonNode = sign(n[1])*safeAcos(n[0])*radToDeg;
// Account for earth rotation
LonNode = LonNode + wE*t0*radToDeg + GHA0*15.0;
while(LonNode < 0.0) LonNode += 360.0;
while(LonNode > 360.0) LonNode -= 360.0;
// Argument of Perigee
tmp = n[0]*P[0] + n[1]*P[1];
ArgPeri = sign(P[2])*safeAcos(tmp)*radToDeg;
if(ArgPeri < 0.0) ArgPeri += 360.0;
// Orbital Energy (km^2/s^2)
Energy = VdotV/2.0-mu/r;
if(Energy > 0.0) {
return ksNonEllipticalOrbit; // Non-elliptical orbit is not handled
}
// Orbit Semi-Major Axis radius (km)
a = -mu/2.0/Energy;
// Orbit semi-latus rectum (km)
p = AngMom*AngMom/mu;
// Compute Orbit eccentricity and protect against poor numerical conditioning
// for very nearly circular orbits.
if((1.0-p/a) > 0.0 ) {
ecc = sqrt(1.0-p/a);
}
else {
ecc = 0.0;
}
// Orbit Inclination (rad)
if(fabs(AngMom) > kDenomEps) {
inclination = safeAcos(h[2]/AngMom);
}
else {
inclination = M_PI/2.0*sign(h[2]);
}
// Locate Perigee and Compute Height Above Ellipsoid (km)
for(i=0; i<3; i++) {
EFG[i] = a*(1.0-ecc)*P[i]*1000.0;
}
EFG2llh(EFG,lat,lon,perigee);
perigee *= 0.001;
// Locate Apogee and compute height above ellipsoid (km)
for(i=0; i<3; i++) {
EFG[i] = a*(1.0+ecc)*P[i]*1000.0;
}
EFG2llh(EFG,lat,lon,apogee);
apogee *= 0.001;
// Set orbit flag based upon sign of perigee height above ellipsoid
OrbitFlag = 0;
if(perigee > 0.0) OrbitFlag = 1;
Period = a*sqrt(a/mu);
// True Anomally at Thrust Termination
cnu0 = (P[0]*r0[0]+P[1]*r0[1]+P[2]*r0[2])/r;
nu0 = safeAcos(cnu0);
if(rdotV < 0.0) nu0 = 2.0*M_PI - nu0;
// Eccentric Anomally at Thrust Termination
cE0 = (ecc+cnu0)/(1.0+ecc*cnu0);
E0 = safeAcos(cE0);
if(rdotV < 0.0) E0 = -E0+2.0*M_PI;
// Iterate for Impact Coordinates
IterCount = 0;
AltImpact = ImpactTol + 10.0;
while(OrbitFlag == 0 && IterCount < kMaxIterate && AltImpact > ImpactTol) {
// Compute ECEF Coordinates of Current Guess
llh2EFG(LatImpact,LonImpact,0.0,EFGImpact);
// Earth radius is used to determine where in the orbit impact occurs
RImpact = 0.001*norm2(EFGImpact);
// True Anomally at Impact - based upon earth radius seed
if(ecc > kDenomEps) {
cnui = (p/RImpact-1.0)/ecc;
}
else {
cnui = 1.0;
}
nui = safeAcos(cnui);
// Impact occurs on downleg of trajectory
nui = -nui+2.0*M_PI;
snui = sin(nui);
// Eccentric Anomally at Impact - based upon earth radius seed
if(ecc > kDenomEps) {
cEi = (1.0-RImpact/a)/ecc;
}
else {
cEi = 1.0;
}
Ei = safeAcos(cEi);
// Adjust for downleg impact
Ei = -Ei+2.0*M_PI;
// Vacuum flight time to impact
TimeToImpact = Period*(Ei-E0+ecc*(sin(E0)-sin(Ei)));
// Inertial Impact Position (km)
h[0] = RImpact*(cnui*P[0]+snui*Q[0]);
h[1] = RImpact*(cnui*P[1]+snui*Q[1]);
h[2] = RImpact*(cnui*P[2]+snui*Q[2]);
// Transform to Earth Relative Impact Position in meters
ce = cos(wE*TimeToImpact);
se = sin(wE*TimeToImpact);
EFGImpact[0] = (h[0]*ce+h[1]*se)*1000.;
EFGImpact[1] = (-h[0]*se+h[1]*ce)*1000.;
EFGImpact[2] = h[2]*1000.;
// Compute Geodetic coordinates of impact. Will continue iteration if
// altitude is not sufficiently close to zero.
EFG2llh(EFGImpact,LatImpact,LonImpact,AltImpact);
AltImpact = fabs(AltImpact);
IterCount++;
}
//
// Solve for Eccentric Anomaly at Elapsed Time = 0.0
// Equation: f(Eepoch) = E-ecc*Sin(E) - (E0-ecc*sin(E0)-t0/Period) = 0
//
// Compute constant
tmp = E0-ecc*sin(E0) - t0/Period;
// Seed value for iteration
Eepoch = tmp + ecc*sin(nu0-t0/Period);
// Iterate 5 times using Newton's Method - df/dE analytically employed
for(i=0;i<5;i++) {
Eepoch = Eepoch - (Eepoch-ecc*sin(Eepoch)-tmp)/(1.0-ecc*cos(Eepoch));
}
// True Anomally at Epoch can now be computed
cnu0 = (cos(Eepoch)-ecc)/(1.0-ecc*cos(Eepoch));
nuE = safeAcos(cnu0)*radToDeg;
if(sin(Eepoch) < 0.0) nuE = 360.0 - nuE;
// Vacuum flight time to Apogee
TimeToApogee = Period*(M_PI-E0+ecc*sin(E0));
// Convert output angles from radians to degrees
inclination = inclination*radToDeg;
nu0 = nu0*radToDeg;
return ksNormalExit;
}
//-----------------------------------------------------------------------
// Function: EFG2Kepler
//
// Purpose: Compute Keplerian Orbital Parameters from ECEF State Vector
//
// Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics," Dover
// Publications, 1971.
//
// Usage: See Notes Above.
//
// NOTE: This Function is provided for backward compatibilty with old library
// calls. Newest EFG2Kepler function includes TimeToImpact and TimeToApogee
// as return arguments.
//-----------------------------------------------------------------------
TGeoid::EKeplerStatus TGeoid::EFG2Kepler(double GHA0, double t0,
double *EFG0, double *VEFG0,
double ImpactTol, double &a, double &p, double &ecc,
double &inclination, double &apogee, double &perigee,
double &nu0, double &nuE, double &LonNode, double &ArgPeri,
double &LatImpact, double &LonImpact,
int &OrbitFlag)
{
double TimeToImpact,TimeToApogee;
// Call Main Library Function, discarding time to impact & apogee
return EFG2Kepler(GHA0, t0, EFG0, VEFG0, ImpactTol, a, p, ecc, inclination,
apogee, perigee, nu0, nuE, LonNode, ArgPeri, LatImpact, LonImpact,
TimeToImpact, TimeToApogee, OrbitFlag);
}
//-----------------------------------------------------------------------
// Function: Kepler2EFG
//
// Purpose: Compute State Vector from Keplerian Orbital Elements
//
// NOTE: This function works for ELLIPTICAL orbits ONLY.
//
// Reference: Bate, Mueller, White, "Fundamentals of Astrodynamics," Dover
// Publications, 1971.
//
// Input Arguments:
// GHA0 Greenwich Hour Angle at Epoch (h)
// t Elapsed time since epoch (s)
// a Semi-Major Axis (km)
// ecc Orbit Eccentricity
// nu0 True Anomaly at Epoch (d)
// Inclination Orbit Inclination (d)
// nu True Anomaly at time = t (d)
// LonNode Longitude of Ascending Node (d)
// ArgPeri Argument of Perigee (d)
//
// Output Arguments:
// EFG0[3] EFG Coordinates (m)
// VEFG0[3] EFG Velocity (m/s)
//
//-----------------------------------------------------------------------
TGeoid::EKeplerStatus TGeoid::Kepler2EFG(double GHA0, double t, double a, double ecc, double nu0,
double Inclination, double LonNode, double ArgPeri,
double &nu, double *EFG, double *VEFG)
{
int i;
double p, E0, cE0, E, Period,cnu0,cnu,snu,tmp,sph0,cph0;
double r,rp[3],ri[3],vp[3],vi[3];
double Tip[9],Tei[9],co,cz,ci,so,sz,si;
if(ecc < 0.0 || ecc >= 1.0 || a <= 0.0) {
// If Elliptical Orbit Parameters are Out Of Bounds -
// Do not modify EFG or VEFG and return Error Status.
return ksNonEllipticalOrbit;
}
p = a*(1.0-ecc*ecc);
cnu0 = cos(nu0*degToRad);
// Compute eccentric anomally at epoch
cE0 = (ecc+cnu0)/(1.0+ecc*cnu0);
E0 = sign(sin(nu0*degToRad))*safeAcos(cE0);
Period = a*sqrt(a/mu);
// Eccentric anomally at elapsed time from epoch using Newton Iteration
tmp = E0-ecc*sin(E0) + t/Period;
E = tmp;
for(i=0;i<5;i++) {
E = E - (E-ecc*sin(E)-tmp)/(1.0-ecc*cos(E));
}
// True anomally at elapsed time from epoch
cnu = (cos(E)-ecc)/(1.0-ecc*cos(E));
nu = sign(sin(E))*safeAcos(cnu);
snu = sin(nu);
nu = nu*radToDeg;
if(nu < 0.0) nu+= 360.0;
r = p/(1.0+ecc*cnu);
// R and V in perifocal coordinate system
rp[0] = r*cnu;
rp[1] = r*snu;
rp[2] = 0.0;
vp[0] = -sqrt(mu/p)*snu;
vp[1] = sqrt(mu/p)*(ecc+cnu);
vp[2] = 0.0;
// Perifocal to inertial coordinate transformation
co = cos(ArgPeri*degToRad);
so = sin(ArgPeri*degToRad);
cz = cos(LonNode*degToRad);
sz = sin(LonNode*degToRad);
ci = cos(Inclination*degToRad);
si = sin(Inclination*degToRad);
Tip[0] = cz*co-sz*so*ci;
Tip[1] = -cz*so-sz*co*ci;
Tip[2] = sz*si;
Tip[3] = sz*co+cz*so*ci;
Tip[4] = -sz*so+cz*co*ci;
Tip[5] = -cz*si;
Tip[6] = so*si;
Tip[7] = co*si;
Tip[8] = ci;
// Inertial to ECEF transformation
cph0 = cos(GHA0*15.0*degToRad + wE*t);
sph0 = sin(GHA0*15.0*degToRad + wE*t);
Tei[0] = cph0;
Tei[1] = sph0;
Tei[2] = 0.0;
Tei[3] = -sph0;
Tei[4] = cph0;
Tei[5] = 0.0;
Tei[6] = 0.0;
Tei[7] = 0.0;
Tei[8] = 1.0;
// Transform from perifocal to inertial frame
matv(Tip,rp,ri,0);
matv(Tip,vp,vi,0);
//Transform from Inertial to ECEF Components
matv(Tei,ri,EFG,0);
matv(Tei,vi,VEFG,0);
// Convert from Inertial to Earth Relative Velocity
VEFG[0] = VEFG[0] + wE*EFG[1];
VEFG[1] = VEFG[1] - wE*EFG[0];
// Convert from km to meters
for(i=0;i<3;i++) {
EFG[i] *= 1000.0;
VEFG[i] *= 1000.0;
}
return ksNormalExit;
}
//-----------------------------------------------------------------------
// Function: EFG2Gravity
//
// Purpose: Compute Gravitational Acceleration at a Point
//
// Input Argument:
// x[3] ECEF Cartesion Location (m)
//
// Output Argument:
// gE[3] ECEF Cartesion Components of Gravitational Acceleration (m/s^2)
//-----------------------------------------------------------------------
void TGeoid::EFG2Gravity(double *x, double *gE) {
double rI,spc,cpc,cle,sle,aRI,gr,gph,muR2,rxy;
double aRI2,aRI3,aRI4;
double spc2,spc3,spc4;
// Length of Radius Vector
rI = norm2(x);
// Protect against Floating Point Exception due to bad coordinate data
if(rI < kDenomEps) {
gE[0] = gE[1] = gE[2] = 0.0;
return;
}
// Radius Vector projection on equatorial plane (m)
rxy = sqrt(x[0]*x[0]+x[1]*x[1]);
// Compute sin and cos of geocentric latitude
spc = x[2]/rI;
cpc = rxy/rI;
// Compute sin and cos of geodetic longitude
if(rxy >= kDenomEps) {
sle = x[1]/rxy;
cle = x[0]/rxy;
}
else {
sle = 0.0;
cle = 1.0;
}
// mu/R^2 (m/s^2)
muR2 = mu/(0.001*rI)/(0.001*rI)*1000.;
// Equatorial Radius to Position Radius Ratio
aRI = a/rI;
// Powers of a/rI
aRI2 = aRI*aRI;
aRI3 = aRI2*aRI;
aRI4 = aRI3*aRI;
// Powers of spc
spc2 = spc*spc;
spc3 = spc2*spc;
spc4 = spc3*spc;
// Radial Component of Gravitational Accel
gr = muR2*(1.0 - 1.5*J2*aRI2*(3.0*spc2-1.0)
- 2.0*J3*aRI3*(5.0*spc2-3.0)*spc
-0.625*J4*aRI4*(35.0*spc4-30.0*spc2+3.0) );
// Tangential Component of Gravitational Accel
gph = muR2*(3.0*J2*aRI2*spc +
0.5*J3*aRI3*(15.0*spc2-3.0) +
2.5*J4*aRI4*(7.0*spc2-3.0)*spc)*cpc;
// Transform into ECEF Components
gE[0] = (gph*spc - gr*cpc)*cle ;
gE[1] = (gph*spc - gr*cpc)*sle ;
gE[2] = -gph*cpc - gr*spc ;
}
//-----------------------------------------------------------------------
// Function: EFGv2RotAcc
//
// Purpose: Compute Coriolis and Centripetal Acceleration due to Earth Rotation
//
// Input Arguments:
// x[3] ECEF Cartesion Location (m)
// v[3] ECEF Cartesion Velocity Components (m/s)
//
// Output Argument:
// arot[3] ECEF Cartesion Components of Acceleration (m/s^2)
//-----------------------------------------------------------------------
void TGeoid::EFGv2RotAcc(double *x, double *v, double *arot) {
double w[3],wxr[3],wxwxr[3];
// ECEF Components of Earth Rotation Vector
w[0] = 0.0;
w[1] = 0.0;
w[2] = wE;
// Intermediate Vectors
cross(w,v,arot);
cross(w,x,wxr);
cross(w,wxr,wxwxr);
// Final Answer = 2* (wE x V) + wE x wE x r (assumes wE is constant)
for(int i=0;i<3;i++) {
arot[i] = 2.0*arot[i]+wxwxr[i];
}
}
//************************************************************************
// Utility Functions
//************************************************************************
//-----------------------------------------------------------------------
// Function: cross
//
// Purpose: Vector Cross Product : c = a x b
//
//-----------------------------------------------------------------------
void TGeoid::cross(double *a, double *b,double *c)
{
c[0] = a[1]*b[2] - a[2]*b[1];
c[1] = a[2]*b[0] - a[0]*b[2];
c[2] = a[0]*b[1] - a[1]*b[0];
}
//-----------------------------------------------------------------------
// Function: dot
//
// Purpose: Vector Dot Product : c = a . b
//
//-----------------------------------------------------------------------
double TGeoid::dot(double *a, double *b)
{
return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}
//-----------------------------------------------------------------------
// Function: matv
//
// Purpose: Matrix Vector Multiplication
//
// Input:
// a[9] Row-wise 3x3 matrix
// x[3] Input Vector
// itrn Transpose Flag =0 b = [a].x
// =1 b = trn[a].x
//
// Output:
// b Ouput Vector
//-----------------------------------------------------------------------
void TGeoid::matv(double *a, double *x,double *b,const int itrn)
{
for(int i=0; i<3; i++) {
b[i] = 0.0;
for(int j=0; j<3; j++) {
if(itrn == 0) {
b[i] = b[i] + a[3*i+j]*x[j];
}
else {
b[i] = b[i] + a[3*j+i]*x[j];
}
}
}
}
//-----------------------------------------------------------------------
// Function: norm2
//
// Purpose: Compute Euclidean length of 3-space vector
//
//-----------------------------------------------------------------------
double TGeoid::norm2(double *a)
{
return sqrt(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
}
//-----------------------------------------------------------------------
// Function: safeAtan2
//
// Purpose: Compute ArcTan with Argument Checking
//
//-----------------------------------------------------------------------
double TGeoid::safeAtan2(double y, double x)
{
if(x > kDenomEps || x < -kDenomEps) {
return atan2(y,x);
}
else if( y >= 0.0) {
return 0.5*M_PI;
}
return -0.5*M_PI;
}
//-----------------------------------------------------------------------
// Function: safeAcos
//
// Purpose: Compute ArcCos with Argument Checking
//
//-----------------------------------------------------------------------
double TGeoid::safeAcos(double x)
{
if(x <= 1.0 && x >= -1.0) {
return acos(x);
}
else if(x > 1.0) {
return 0.0;
}
return M_PI;
}
Internal Links
Parent Article: Main Page
