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