home *** CD-ROM | disk | FTP | other *** search
/ rtsi.com / 2014.01.www.rtsi.com.tar / www.rtsi.com / OS9 / OSK / EFFO / forum16.lzh / SOFTWARE / C / ORBIT / orbitr.c < prev    next >
Text File  |  1991-01-05  |  14KB  |  541 lines
  1. /* N3EMO Orbit Simulator routines  v3.7 */
  2.  
  3. /* Copyright (c) 1986,1987,1988,1989,1990 Robert W. Berger N3EMO
  4.    May be freely distributed, provided this notice remains intact. */
  5.  
  6. #include <stdio.h>
  7. #include <math.h>
  8.  
  9. #define SSPELLIPSE 0        /* If non zero, use ellipsoidal earth model
  10.                    when calculating longitude, latitude, and
  11.                    height */
  12.  
  13.  
  14. #ifndef PI
  15. #define PI 3.14159265
  16. #endif
  17.  
  18. /* Patch Tyko: */
  19. #define atan2(y,x) (atan((y)/(x))+PI)
  20.  
  21. #ifdef PI2
  22. #undef PI2
  23. #endif
  24.  
  25. #ifdef E
  26. #undef E
  27. #endif
  28.  
  29. typedef double mat3x3[3][3];
  30.  
  31. #define PI2 (PI*2)
  32. #define MinutesPerDay (24*60.0)
  33. #define SecondsPerDay (60*MinutesPerDay)
  34. #define HalfSecond (0.5/SecondsPerDay)
  35. #define EarthRadius 6378.16             /* Kilometers, at equator */
  36.  
  37. #define EarthFlat (1/298.25)            /* Earth Flattening Coeff. */
  38. #define SiderealSolar 1.0027379093
  39. #define SidRate (PI2*SiderealSolar/SecondsPerDay)    /* radians/second */
  40. #define GM 398600            /* Kilometers^3/seconds^2 */
  41. #define DegreesPerRadian (180/PI)
  42. #define RadiansPerDegree (PI/180)
  43. #define ABS(x) ((x) < 0 ? (-(x)) : (x))
  44. #define SQR(x) ((x)*(x))
  45.  
  46. #define Epsilon (RadiansPerDegree/3600)     /* 1 arc second */
  47. #define SunRadius 695000        
  48. #define SunSemiMajorAxis  149598845.0          /* Kilometers            */
  49.  
  50.  
  51. double SidDay,SidReference;    /* Date and sidereal time    */
  52.  
  53. /* Keplerian elements for the sun */
  54. double SunEpochTime,SunInclination,SunRAAN,SunEccentricity,
  55.        SunArgPerigee,SunMeanAnomaly,SunMeanMotion;
  56.  
  57. /* values for shadow geometry */
  58. double SinPenumbra,CosPenumbra;
  59.  
  60.  
  61. /* Solve Kepler's equation                                      */
  62. /* Inputs:                                                      */
  63. /*      MeanAnomaly     Time Since last perigee, in radians.    */
  64. /*                      PI2 = one complete orbit.               */
  65. /*      Eccentricity    Eccentricity of orbit's ellipse.        */
  66. /* Output:                                                      */
  67. /*      TrueAnomaly     Angle between perigee, geocenter, and   */
  68. /*                      current position.                       */
  69.  
  70. long calls = 0;
  71. long iters = 0;
  72.  
  73. dumpstats()
  74. {
  75. printf("Average iterations = %lf\n",((double) iters)/calls);
  76. }
  77.  
  78. double Kepler(MeanAnomaly,Eccentricity)
  79. register double MeanAnomaly,Eccentricity;
  80.  
  81. {
  82. register double E;              /* Eccentric Anomaly                    */
  83. register double Error;
  84. register double TrueAnomaly;
  85.  
  86. calls++;
  87.  
  88.     E = MeanAnomaly ;/*+ Eccentricity*sin(MeanAnomaly);   /* Initial guess */
  89.     do
  90.         {
  91.         Error = (E - Eccentricity*sin(E) - MeanAnomaly)
  92.                 / (1 - Eccentricity*cos(E));
  93.         E -= Error;
  94. iters++;
  95.         }
  96.    while (ABS(Error) >= Epsilon);
  97.  
  98.     if (ABS(E-PI) < Epsilon)
  99.         TrueAnomaly = PI;
  100.       else
  101.         TrueAnomaly = 2*atan(sqrt((1+Eccentricity)/(1-Eccentricity))
  102.                                 *tan(E/2));
  103.     if (TrueAnomaly < 0)
  104.         TrueAnomaly += PI2;
  105.  
  106.     return TrueAnomaly;
  107. }
  108.  
  109. GetSubSatPoint(SatX,SatY,SatZ,Time,Latitude,Longitude,Height)
  110. double SatX,SatY,SatZ,Time;
  111. double *Latitude,*Longitude,*Height;
  112. {
  113.     double r;
  114.     long i;
  115.  
  116.     r = sqrt(SQR(SatX) + SQR(SatY) + SQR(SatZ));
  117.  
  118.     *Longitude = PI2*((Time-SidDay)*SiderealSolar + SidReference)
  119.             - atan2(SatY,SatX);
  120.  
  121.     /* i = floor(Longitude/2*pi)        */
  122.     i = *Longitude/PI2;
  123.     if(i < 0)
  124.         i--;
  125.  
  126.     *Longitude -= i*PI2;
  127.  
  128.     *Latitude = atan(SatZ/sqrt(SQR(SatX) + SQR(SatY)));
  129.  
  130. #if SSPELLIPSE
  131. #else
  132.     *Height = r - EarthRadius;
  133. #endif
  134. }
  135.  
  136.  
  137. GetPrecession(SemiMajorAxis,Eccentricity,Inclination,
  138.         RAANPrecession,PerigeePrecession)
  139. double SemiMajorAxis,Eccentricity,Inclination;
  140. double *RAANPrecession,*PerigeePrecession;
  141. {
  142.   *RAANPrecession = 9.95*pow(EarthRadius/SemiMajorAxis,3.5) * cos(Inclination)
  143.                  / SQR(1-SQR(Eccentricity)) * RadiansPerDegree;
  144.  
  145.   *PerigeePrecession = 4.97*pow(EarthRadius/SemiMajorAxis,3.5)
  146.          * (5*SQR(cos(Inclination))-1)
  147.                  / SQR(1-SQR(Eccentricity)) * RadiansPerDegree;
  148. }
  149.  
  150. /* Compute the satellite postion and velocity in the RA based coordinate
  151.    system */
  152.  
  153. GetSatPosition(EpochTime,EpochRAAN,EpochArgPerigee,SemiMajorAxis,
  154.     Inclination,Eccentricity,RAANPrecession,PerigeePrecession,
  155.     Time,TrueAnomaly,X,Y,Z,Radius,VX,VY,VZ)
  156.  
  157. double EpochTime,EpochRAAN,EpochArgPerigee;
  158. double SemiMajorAxis,Inclination,Eccentricity;
  159. double RAANPrecession,PerigeePrecession,Time, TrueAnomaly;
  160. double *X,*Y,*Z,*Radius,*VX,*VY,*VZ;
  161.  
  162. {
  163.     double RAAN,ArgPerigee;
  164.  
  165.  
  166.     double Xw,Yw,VXw,VYw;    /* In orbital plane */
  167.     double Tmp;
  168.     double Px,Qx,Py,Qy,Pz,Qz;    /* Escobal transformation 31 */
  169.     double CosArgPerigee,SinArgPerigee;
  170.     double CosRAAN,SinRAAN,CoSinclination,SinInclination;
  171.  
  172.         *Radius = SemiMajorAxis*(1-SQR(Eccentricity))
  173.                         / (1+Eccentricity*cos(TrueAnomaly));
  174.  
  175.  
  176.     Xw = *Radius * cos(TrueAnomaly);
  177.     Yw = *Radius * sin(TrueAnomaly);
  178.     
  179.     Tmp = sqrt(GM/(SemiMajorAxis*(1-SQR(Eccentricity))));
  180.  
  181.     VXw = -Tmp*sin(TrueAnomaly);
  182.     VYw = Tmp*(cos(TrueAnomaly) + Eccentricity);
  183.  
  184.     ArgPerigee = EpochArgPerigee + (Time-EpochTime)*PerigeePrecession;
  185.     RAAN = EpochRAAN - (Time-EpochTime)*RAANPrecession;
  186.  
  187.     CosRAAN = cos(RAAN); SinRAAN = sin(RAAN);
  188.     CosArgPerigee = cos(ArgPerigee); SinArgPerigee = sin(ArgPerigee);
  189.     CoSinclination = cos(Inclination); SinInclination = sin(Inclination);
  190.     
  191.     Px = CosArgPerigee*CosRAAN - SinArgPerigee*SinRAAN*CoSinclination;
  192.     Py = CosArgPerigee*SinRAAN + SinArgPerigee*CosRAAN*CoSinclination;
  193.     Pz = SinArgPerigee*SinInclination;
  194.     Qx = -SinArgPerigee*CosRAAN - CosArgPerigee*SinRAAN*CoSinclination;
  195.     Qy = -SinArgPerigee*SinRAAN + CosArgPerigee*CosRAAN*CoSinclination;
  196.     Qz = CosArgPerigee*SinInclination;
  197.  
  198.     *X = Px*Xw + Qx*Yw;        /* Escobal, transformation #31 */
  199.     *Y = Py*Xw + Qy*Yw;
  200.     *Z = Pz*Xw + Qz*Yw;
  201.  
  202.     *VX = Px*VXw + Qx*VYw;
  203.     *VY = Py*VXw + Qy*VYw;
  204.     *VZ = Pz*VXw + Qz*VYw;
  205. }
  206.  
  207. /* Compute the site postion and velocity in the RA based coordinate
  208.    system. SiteMatrix is set to a matrix which is used by GetTopoCentric
  209.    to convert geocentric coordinates to topocentric (observer-centered)
  210.     coordinates. */
  211.  
  212. GetSitPosition(SiteLat,SiteLong,SiteElevation,CurrentTime,
  213.              SiteX,SiteY,SiteZ,SiteVX,SiteVY,SiteMatrix)
  214.  
  215. double SiteLat,SiteLong,SiteElevation,CurrentTime;
  216. double *SiteX,*SiteY,*SiteZ,*SiteVX,*SiteVY;
  217. mat3x3 SiteMatrix;
  218.  
  219. {
  220.     static double G1,G2; /* Used to correct for flattening of the Earth */
  221.     static double CosLat,SinLat;
  222.     static double OldSiteLat = -100000;  /* Used to avoid unneccesary recomputation */
  223.     static double OldSiteElevation = -100000;
  224.     double Lat;
  225.     double SiteRA;    /* Right Ascension of site            */
  226.     double CosRA,SinRA;
  227.  
  228.     if ((SiteLat != OldSiteLat) || (SiteElevation != OldSiteElevation))
  229.     {
  230.     OldSiteLat = SiteLat;
  231.     OldSiteElevation = SiteElevation;
  232.     Lat = atan(1/(1-SQR(EarthFlat))*tan(SiteLat));
  233.  
  234.     CosLat = cos(Lat);
  235.     SinLat = sin(Lat);
  236.  
  237.     G1 = EarthRadius/(sqrt(1-(2*EarthFlat-SQR(EarthFlat))*SQR(SinLat)));
  238.     G2 = G1*SQR(1-EarthFlat);
  239.     G1 += SiteElevation;
  240.     G2 += SiteElevation;
  241.     }
  242.  
  243.  
  244.     SiteRA = PI2*((CurrentTime-SidDay)*SiderealSolar + SidReference)
  245.              - SiteLong;
  246.     CosRA = cos(SiteRA);
  247.     SinRA = sin(SiteRA);
  248.     
  249.  
  250.     *SiteX = G1*CosLat*CosRA;
  251.     *SiteY = G1*CosLat*SinRA;
  252.     *SiteZ = G2*SinLat;
  253.     *SiteVX = -SidRate * *SiteY;
  254.     *SiteVY = SidRate * *SiteX;
  255.  
  256.     SiteMatrix[0][0] = SinLat*CosRA;
  257.     SiteMatrix[0][1] = SinLat*SinRA;
  258.     SiteMatrix[0][2] = -CosLat;
  259.     SiteMatrix[1][0] = -SinRA;
  260.     SiteMatrix[1][1] = CosRA;
  261.     SiteMatrix[1][2] = 0.0;
  262.     SiteMatrix[2][0] = CosRA*CosLat;
  263.     SiteMatrix[2][1] = SinRA*CosLat;
  264.     SiteMatrix[2][2] = SinLat;
  265. }
  266.  
  267. GetRange(SiteX,SiteY,SiteZ,SiteVX,SiteVY,
  268.     SatX,SatY,SatZ,SatVX,SatVY,SatVZ,Range,RangeRate)
  269.  
  270. double SiteX,SiteY,SiteZ,SiteVX,SiteVY;
  271. double SatX,SatY,SatZ,SatVX,SatVY,SatVZ;
  272. double *Range,*RangeRate;
  273. {
  274.     double DX,DY,DZ;
  275.  
  276.     DX = SatX - SiteX; DY = SatY - SiteY; DZ = SatZ - SiteZ;
  277.  
  278.     *Range = sqrt(SQR(DX)+SQR(DY)+SQR(DZ));    
  279.  
  280.     *RangeRate = ((SatVX-SiteVX)*DX + (SatVY-SiteVY)*DY + SatVZ*DZ)
  281.             / *Range;
  282. }
  283.  
  284. /* Convert from geocentric RA based coordinates to topocentric
  285.    (observer centered) coordinates */
  286.  
  287. GetTopocentric(SatX,SatY,SatZ,SiteX,SiteY,SiteZ,SiteMatrix,X,Y,Z)
  288. double SatX,SatY,SatZ,SiteX,SiteY,SiteZ;
  289. double *X,*Y,*Z;
  290. mat3x3 SiteMatrix;
  291. {
  292.     SatX -= SiteX;
  293.     SatY -= SiteY;
  294.     SatZ -= SiteZ;
  295.  
  296.     *X = SiteMatrix[0][0]*SatX + SiteMatrix[0][1]*SatY
  297.     + SiteMatrix[0][2]*SatZ; 
  298.     *Y = SiteMatrix[1][0]*SatX + SiteMatrix[1][1]*SatY
  299.     + SiteMatrix[1][2]*SatZ; 
  300.     *Z = SiteMatrix[2][0]*SatX + SiteMatrix[2][1]*SatY
  301.     + SiteMatrix[2][2]*SatZ; 
  302. }
  303.  
  304. GetBearings(SatX,SatY,SatZ,SiteX,SiteY,SiteZ,SiteMatrix,Azimuth,Elevation)
  305. double SatX,SatY,SatZ,SiteX,SiteY,SiteZ;
  306. mat3x3 SiteMatrix;
  307. double *Azimuth,*Elevation;
  308. {
  309.     double x,y,z;
  310.  
  311.     GetTopocentric(SatX,SatY,SatZ,SiteX,SiteY,SiteZ,SiteMatrix,&x,&y,&z);
  312.  
  313.     *Elevation = atan(z/sqrt(SQR(x) + SQR(y)));
  314.  
  315.     *Azimuth = PI - atan2(y,x);
  316.  
  317.     if (*Azimuth < 0)
  318.     *Azimuth += PI;
  319. }
  320.  
  321. Eclipsed(SatX,SatY,SatZ,SatRadius,CurrentTime)
  322. double SatX,SatY,SatZ,SatRadius,CurrentTime;
  323. {
  324.     double MeanAnomaly,TrueAnomaly;
  325.     double SunX,SunY,SunZ,SunRad;
  326.     double vx,vy,vz;
  327.     double CosTheta;
  328.  
  329.     MeanAnomaly = SunMeanAnomaly+ (CurrentTime-SunEpochTime)*SunMeanMotion*PI2;
  330.     TrueAnomaly = Kepler(MeanAnomaly,SunEccentricity);
  331.  
  332.     GetSatPosition(SunEpochTime,SunRAAN,SunArgPerigee,SunSemiMajorAxis,
  333.         SunInclination,SunEccentricity,0.0,0.0,CurrentTime,
  334.         TrueAnomaly,&SunX,&SunY,&SunZ,&SunRad,&vx,&vy,&vz);
  335.  
  336.     CosTheta = (SunX*SatX + SunY*SatY + SunZ*SatZ)/(SunRad*SatRadius)
  337.          *CosPenumbra + (SatRadius/EarthRadius)*SinPenumbra;
  338.  
  339.     if (CosTheta < 0)
  340.         if (CosTheta < -sqrt(SQR(SatRadius)-SQR(EarthRadius))/SatRadius
  341.                 *CosPenumbra + (SatRadius/EarthRadius)*SinPenumbra)
  342.       
  343.         return 1;
  344.     return 0;
  345. }
  346.  
  347. /* Initialize the Sun's keplerian elements for a given epoch.
  348.    Formulas are from "Explanatory Supplement to the Astronomical Ephemeris".
  349.    Also init the sidereal reference                */
  350.  
  351. InitOrbitRoutines(EpochDay)
  352. double EpochDay;
  353. {
  354.     double T,T2,T3,Omega;
  355.     int n;
  356.     double SunTrueAnomaly,SunDistance;
  357.  
  358.     T = (floor(EpochDay)-0.5)/36525;
  359.     T2 = T*T;
  360.     T3 = T2*T;
  361.  
  362.     SidDay = floor(EpochDay);
  363.  
  364.     SidReference = (6.6460656 + 2400.051262*T + 0.00002581*T2)/24;
  365.     SidReference -= floor(SidReference);
  366.  
  367.     /* Omega is used to correct for the nutation and the abberation */
  368.     Omega = (259.18 - 1934.142*T) * RadiansPerDegree;
  369.     n = Omega / PI2;
  370.     Omega -= n*PI2;
  371.  
  372.     SunEpochTime = EpochDay;
  373.     SunRAAN = 0;
  374.  
  375.     SunInclination = (23.452294 - 0.0130125*T - 0.00000164*T2
  376.             + 0.000000503*T3 +0.00256*cos(Omega)) * RadiansPerDegree;
  377.     SunEccentricity = (0.01675104 - 0.00004180*T - 0.000000126*T2);
  378.     SunArgPerigee = (281.220833 + 1.719175*T + 0.0004527*T2
  379.             + 0.0000033*T3) * RadiansPerDegree;
  380.     SunMeanAnomaly = (358.475845 + 35999.04975*T - 0.00015*T2
  381.             - 0.00000333333*T3) * RadiansPerDegree;
  382.     n = SunMeanAnomaly / PI2;
  383.     SunMeanAnomaly -= n*PI2;
  384.  
  385.     SunMeanMotion = 1/(365.24219879 - 0.00000614*T);
  386.  
  387.     SunTrueAnomaly = Kepler(SunMeanAnomaly,SunEccentricity);
  388.     SunDistance = SunSemiMajorAxis*(1-SQR(SunEccentricity))
  389.             / (1+SunEccentricity*cos(SunTrueAnomaly));
  390.  
  391.     SinPenumbra = (SunRadius-EarthRadius)/SunDistance;
  392.     CosPenumbra = sqrt(1-SQR(SinPenumbra));
  393. }
  394.  
  395.  
  396. SPrintTime(Str,Time)
  397. char *Str;
  398. double Time;
  399. {
  400.     int day,hours,minutes,seconds;
  401.  
  402.     day = Time;
  403.     Time -= day;
  404.     if (Time < 0)
  405.         Time += 1.0;   /* Correct for truncation problems with negatives */
  406.  
  407.     hours = Time*24;
  408.     Time -=  hours/24.0;
  409.  
  410.     minutes = Time*MinutesPerDay;
  411.     Time -= minutes/MinutesPerDay;
  412.  
  413.     seconds = Time*SecondsPerDay;
  414.     seconds -= seconds/SecondsPerDay;
  415.  
  416.     sprintf(Str,"%02d%02d:%02d",hours,minutes,seconds);
  417. }
  418.  
  419. PrintTime(OutFile,Time)
  420. FILE *OutFile;
  421. double Time;
  422. {
  423.     char str[100];
  424.  
  425.     SPrintTime(str,Time);
  426.     fprintf(OutFile,"%s",str);
  427. }
  428.  
  429.  
  430. /* Get the Day Number for a given date. January 1 of the reference year
  431.    is day 0. Note that the Day Number may be negative, if the sidereal
  432.    reference is in the future.                                          */
  433.  
  434. /* Date calculation routines
  435.   Robert Berger @ Carnegie Mellon
  436.  
  437.   January 1, 1900 is day 0
  438.   valid from 1900 through 2099 */
  439.  
  440. /* #include <stdio.h> */
  441.  
  442. char *MonthNames[] = { "Jan","Feb","Mar","Apr","May","Jun","Jul",
  443.                         "Aug","Sep","Oct","Nov","Dec" };
  444.  
  445. int MonthDays[] = {0,31,59,90,120,151,181,212,243,273,304,334};
  446.         
  447.  
  448. char *DayNames[] = { "Sunday","Monday","Tuesday","Wednesday","Thursday",
  449.                         "Friday","Saturday"};
  450.  
  451.  
  452. long GetDayNum(Year,Month,Day)
  453. {
  454.     long Result;
  455.     
  456.     /* Heuristic to allow 4 or 2 digit year specifications */
  457.     if (Year < 50)
  458.     Year += 2000;
  459.       else if (Year < 100)
  460.     Year += 1900;
  461.     
  462.     Result = ((((long) Year-1901)*1461)>>2) + MonthDays[Month-1] + Day + 365;
  463.     if (Year%4 == 0 && Month > 2)
  464.         Result++;
  465.  
  466.     return Result;
  467. }
  468.  
  469. GetDate(DayNum,Year,Month,Day)
  470. long DayNum;
  471. int *Year,*Month,*Day;    
  472. {
  473.     int M,L;
  474.     long Y;
  475.     
  476.     Y = 4*DayNum;
  477.     Y /= 1461;
  478.  
  479.     DayNum =  DayNum -365 - (((Y-1)*1461)>>2);
  480.     
  481.     L = 0;
  482.     if (Y%4 == 0 && DayNum > MonthDays[2])
  483.         L = 1;
  484.     
  485.     M = 1;
  486.          
  487.     while (DayNum > MonthDays[M]+L)
  488.     M++;
  489.     
  490.     DayNum -= (MonthDays[M-1]);
  491.     if (M > 2)
  492.         DayNum -= L;
  493.    
  494.     *Year = Y+1900;
  495.     *Month = M;
  496.     *Day = DayNum;
  497. }    
  498.  
  499. /* Sunday = 0 */
  500. GetDayOfWeek(DayNum)
  501. long DayNum;
  502. {
  503.     return DayNum % 7;
  504. }    
  505.  
  506. SPrintDate(Str,DayNum)
  507. char *Str;
  508. long DayNum;
  509. {
  510.     int Month,Day,Year;
  511.     
  512.     GetDate(DayNum,&Year,&Month,&Day);
  513.     sprintf(Str,"%d %s %d",Day,
  514.                 MonthNames[Month-1],Year);
  515. } 
  516.  
  517. SPrintDayOfWeek(Str,DayNum)
  518. char *Str;
  519. long DayNum;
  520. {
  521.     strcpy(Str,DayNames[DayNum%7]);
  522. }
  523.  
  524. PrintDate(OutFile,DayNum)
  525. FILE *OutFile;
  526. long DayNum;
  527. {
  528.     char str[100];
  529.  
  530.     SPrintDate(str,DayNum);
  531.     fprintf(OutFile,"%s",str);
  532. }
  533.  
  534. PrintDayOfWeek(OutFile,DayNum)
  535. FILE *OutFile;
  536. long DayNum;
  537. {
  538.     fprintf(OutFile,"%s",DayNames[DayNum%7]);
  539. }    
  540.   
  541.