Andy K
Satellite Observers Workbench. NOT yet complete, just published for forum posters to \"cherry pick\" pieces of code as requiered as an example.
Embed:
(wiki syntax)
Show/hide line numbers
sgp4sdp4.c
00001 /* 00002 * Unit SGP4SDP4 00003 * Author: Dr TS Kelso 00004 * Original Version: 1991 Oct 30 00005 * Current Revision: 1992 Sep 03 00006 * Version: 1.50 00007 * Copyright: 1991-1992, All Rights Reserved 00008 * 00009 * Ported to C by: Neoklis Kyriazis April 10 2001 00010 */ 00011 00012 #include "sgp4sdp4.h" 00013 00014 /* SGP4 */ 00015 /* This function is used to calculate the position and velocity */ 00016 /* of near-earth (period < 225 minutes) satellites. tsince is */ 00017 /* time since epoch in minutes, tle is a pointer to a tle_t */ 00018 /* structure with Keplerian orbital elements and pos and vel */ 00019 /* are vector_t structures returning ECI satellite position and */ 00020 /* velocity. Use Convert_Sat_State() to convert to km and km/s.*/ 00021 void 00022 SGP4(double tsince, tle_t *tle, vector_t *pos, vector_t *vel, double* phase) 00023 { 00024 static double 00025 aodp,aycof,c1,c4,c5,cosio,d2,d3,d4,delmo,omgcof, 00026 eta,omgdot,sinio,xnodp,sinmo,t2cof,t3cof,t4cof,t5cof, 00027 x1mth2,x3thm1,x7thm1,xmcof,xmdot,xnodcf,xnodot,xlcof; 00028 00029 double 00030 cosuk,sinuk,rfdotk,vx,vy,vz,ux,uy,uz,xmy,xmx, 00031 cosnok,sinnok,cosik,sinik,rdotk,xinck,xnodek,uk, 00032 rk,cos2u,sin2u,u,sinu,cosu,betal,rfdot,rdot,r,pl, 00033 elsq,esine,ecose,epw,cosepw,x1m5th,xhdot1,tfour, 00034 sinepw,capu,ayn,xlt,aynl,xll,axn,xn,beta,xl,e,a, 00035 tcube,delm,delomg,templ,tempe,tempa,xnode,tsq,xmp, 00036 omega,xnoddf,omgadf,xmdf,a1,a3ovk2,ao,betao,betao2, 00037 c1sq,c2,c3,coef,coef1,del1,delo,eeta,eosq,etasq, 00038 perige,pinvsq,psisq,qoms24,s4,temp,temp1,temp2, 00039 temp3,temp4,temp5,temp6,theta2,theta4,tsi; 00040 00041 int i; 00042 00043 /* Initialization */ 00044 if (isFlagClear(SGP4_INITIALIZED_FLAG)) 00045 { 00046 SetFlag(SGP4_INITIALIZED_FLAG); 00047 00048 /* Recover original mean motion (xnodp) and */ 00049 /* semimajor axis (aodp) from input elements. */ 00050 a1 = pow(xke/tle->xno,tothrd); 00051 cosio = cos(tle->xincl); 00052 theta2 = cosio*cosio; 00053 x3thm1 = 3*theta2-1.0; 00054 eosq = tle->eo*tle->eo; 00055 betao2 = 1-eosq; 00056 betao = sqrt(betao2); 00057 del1 = 1.5*ck2*x3thm1/(a1*a1*betao*betao2); 00058 ao = a1*(1-del1*(0.5*tothrd+del1*(1+134/81*del1))); 00059 delo = 1.5*ck2*x3thm1/(ao*ao*betao*betao2); 00060 xnodp = tle->xno/(1+delo); 00061 aodp = ao/(1-delo); 00062 00063 /* For perigee less than 220 kilometers, the "simple" flag is set */ 00064 /* and the equations are truncated to linear variation in sqrt a */ 00065 /* and quadratic variation in mean anomaly. Also, the c3 term, */ 00066 /* the delta omega term, and the delta m term are dropped. */ 00067 if((aodp*(1-tle->eo)/ae) < (220/xkmper+ae)) 00068 SetFlag(SIMPLE_FLAG); 00069 else 00070 ClearFlag(SIMPLE_FLAG); 00071 00072 /* For perigee below 156 km, the */ 00073 /* values of s and qoms2t are altered. */ 00074 s4 = __s__; 00075 qoms24 = qoms2t; 00076 perige = (aodp*(1-tle->eo)-ae)*xkmper; 00077 if(perige < 156) 00078 { 00079 if(perige <= 98) 00080 s4 = 20; 00081 else 00082 s4 = perige-78; 00083 qoms24 = pow((120-s4)*ae/xkmper,4); 00084 s4 = s4/xkmper+ae; 00085 }; /* End of if(perige <= 98) */ 00086 00087 pinvsq = 1/(aodp*aodp*betao2*betao2); 00088 tsi = 1/(aodp-s4); 00089 eta = aodp*tle->eo*tsi; 00090 etasq = eta*eta; 00091 eeta = tle->eo*eta; 00092 psisq = fabs(1-etasq); 00093 coef = qoms24*pow(tsi,4); 00094 coef1 = coef/pow(psisq,3.5); 00095 c2 = coef1*xnodp*(aodp*(1+1.5*etasq+eeta*(4+etasq))+ 00096 0.75*ck2*tsi/psisq*x3thm1*(8+3*etasq*(8+etasq))); 00097 c1 = tle->bstar*c2; 00098 sinio = sin(tle->xincl); 00099 a3ovk2 = -xj3/ck2*pow(ae,3); 00100 c3 = coef*tsi*a3ovk2*xnodp*ae*sinio/tle->eo; 00101 x1mth2 = 1-theta2; 00102 c4 = 2*xnodp*coef1*aodp*betao2*(eta*(2+0.5*etasq)+ 00103 tle->eo*(0.5+2*etasq)-2*ck2*tsi/(aodp*psisq)* 00104 (-3*x3thm1*(1-2*eeta+etasq*(1.5-0.5*eeta))+0.75* 00105 x1mth2*(2*etasq-eeta*(1+etasq))*cos(2*tle->omegao))); 00106 c5 = 2*coef1*aodp*betao2*(1+2.75*(etasq+eeta)+eeta*etasq); 00107 theta4 = theta2*theta2; 00108 temp1 = 3*ck2*pinvsq*xnodp; 00109 temp2 = temp1*ck2*pinvsq; 00110 temp3 = 1.25*ck4*pinvsq*pinvsq*xnodp; 00111 xmdot = xnodp+0.5*temp1*betao*x3thm1+ 00112 0.0625*temp2*betao*(13-78*theta2+137*theta4); 00113 x1m5th = 1-5*theta2; 00114 omgdot = -0.5*temp1*x1m5th+0.0625*temp2*(7-114*theta2+ 00115 395*theta4)+temp3*(3-36*theta2+49*theta4); 00116 xhdot1 = -temp1*cosio; 00117 xnodot = xhdot1+(0.5*temp2*(4-19*theta2)+ 00118 2*temp3*(3-7*theta2))*cosio; 00119 omgcof = tle->bstar*c3*cos(tle->omegao); 00120 xmcof = -tothrd*coef*tle->bstar*ae/eeta; 00121 xnodcf = 3.5*betao2*xhdot1*c1; 00122 t2cof = 1.5*c1; 00123 xlcof = 0.125*a3ovk2*sinio*(3+5*cosio)/(1+cosio); 00124 aycof = 0.25*a3ovk2*sinio; 00125 delmo = pow(1+eta*cos(tle->xmo),3); 00126 sinmo = sin(tle->xmo); 00127 x7thm1 = 7*theta2-1; 00128 if (isFlagClear(SIMPLE_FLAG)) 00129 { 00130 c1sq = c1*c1; 00131 d2 = 4*aodp*tsi*c1sq; 00132 temp = d2*tsi*c1/3; 00133 d3 = (17*aodp+s4)*temp; 00134 d4 = 0.5*temp*aodp*tsi*(221*aodp+31*s4)*c1; 00135 t3cof = d2+2*c1sq; 00136 t4cof = 0.25*(3*d3+c1*(12*d2+10*c1sq)); 00137 t5cof = 0.2*(3*d4+12*c1*d3+6*d2*d2+15*c1sq*(2*d2+c1sq)); 00138 }; /* End of if (isFlagClear(SIMPLE_FLAG)) */ 00139 }; /* End of SGP4() initialization */ 00140 00141 /* Update for secular gravity and atmospheric drag. */ 00142 xmdf = tle->xmo+xmdot*tsince; 00143 omgadf = tle->omegao+omgdot*tsince; 00144 xnoddf = tle->xnodeo+xnodot*tsince; 00145 omega = omgadf; 00146 xmp = xmdf; 00147 tsq = tsince*tsince; 00148 xnode = xnoddf+xnodcf*tsq; 00149 tempa = 1-c1*tsince; 00150 tempe = tle->bstar*c4*tsince; 00151 templ = t2cof*tsq; 00152 if (isFlagClear(SIMPLE_FLAG)) 00153 { 00154 delomg = omgcof*tsince; 00155 delm = xmcof*(pow(1+eta*cos(xmdf),3)-delmo); 00156 temp = delomg+delm; 00157 xmp = xmdf+temp; 00158 omega = omgadf-temp; 00159 tcube = tsq*tsince; 00160 tfour = tsince*tcube; 00161 tempa = tempa-d2*tsq-d3*tcube-d4*tfour; 00162 tempe = tempe+tle->bstar*c5*(sin(xmp)-sinmo); 00163 templ = templ+t3cof*tcube+tfour*(t4cof+tsince*t5cof); 00164 }; /* End of if (isFlagClear(SIMPLE_FLAG)) */ 00165 00166 a = aodp*pow(tempa,2); 00167 e = tle->eo-tempe; 00168 xl = xmp+omega+xnode+xnodp*templ; 00169 beta = sqrt(1-e*e); 00170 xn = xke/pow(a,1.5); 00171 00172 /* Long period periodics */ 00173 axn = e*cos(omega); 00174 temp = 1/(a*beta*beta); 00175 xll = temp*xlcof*axn; 00176 aynl = temp*aycof; 00177 xlt = xl+xll; 00178 ayn = e*sin(omega)+aynl; 00179 00180 /* Solve Kepler's' Equation */ 00181 capu = FMod2p(xlt-xnode); 00182 temp2 = capu; 00183 00184 i = 0; 00185 do 00186 { 00187 sinepw = sin(temp2); 00188 cosepw = cos(temp2); 00189 temp3 = axn*sinepw; 00190 temp4 = ayn*cosepw; 00191 temp5 = axn*cosepw; 00192 temp6 = ayn*sinepw; 00193 epw = (capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2; 00194 if(fabs(epw-temp2) <= e6a) break; 00195 temp2 = epw; 00196 } 00197 while( i++ < 10 ); 00198 00199 /* Short period preliminary quantities */ 00200 ecose = temp5+temp6; 00201 esine = temp3-temp4; 00202 elsq = axn*axn+ayn*ayn; 00203 temp = 1-elsq; 00204 pl = a*temp; 00205 r = a*(1-ecose); 00206 temp1 = 1/r; 00207 rdot = xke*sqrt(a)*esine*temp1; 00208 rfdot = xke*sqrt(pl)*temp1; 00209 temp2 = a*temp1; 00210 betal = sqrt(temp); 00211 temp3 = 1/(1+betal); 00212 cosu = temp2*(cosepw-axn+ayn*esine*temp3); 00213 sinu = temp2*(sinepw-ayn-axn*esine*temp3); 00214 u = AcTan(sinu, cosu); 00215 sin2u = 2*sinu*cosu; 00216 cos2u = 2*cosu*cosu-1; 00217 temp = 1/pl; 00218 temp1 = ck2*temp; 00219 temp2 = temp1*temp; 00220 00221 /* Update for short periodics */ 00222 rk = r*(1-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u; 00223 uk = u-0.25*temp2*x7thm1*sin2u; 00224 xnodek = xnode+1.5*temp2*cosio*sin2u; 00225 xinck = tle->xincl+1.5*temp2*cosio*sinio*cos2u; 00226 rdotk = rdot-xn*temp1*x1mth2*sin2u; 00227 rfdotk = rfdot+xn*temp1*(x1mth2*cos2u+1.5*x3thm1); 00228 00229 /* Orientation vectors */ 00230 sinuk = sin(uk); 00231 cosuk = cos(uk); 00232 sinik = sin(xinck); 00233 cosik = cos(xinck); 00234 sinnok = sin(xnodek); 00235 cosnok = cos(xnodek); 00236 xmx = -sinnok*cosik; 00237 xmy = cosnok*cosik; 00238 ux = xmx*sinuk+cosnok*cosuk; 00239 uy = xmy*sinuk+sinnok*cosuk; 00240 uz = sinik*sinuk; 00241 vx = xmx*cosuk-cosnok*sinuk; 00242 vy = xmy*cosuk-sinnok*sinuk; 00243 vz = sinik*cosuk; 00244 00245 /* Position and velocity */ 00246 pos->x = rk*ux; 00247 pos->y = rk*uy; 00248 pos->z = rk*uz; 00249 vel->x = rdotk*ux+rfdotk*vx; 00250 vel->y = rdotk*uy+rfdotk*vy; 00251 vel->z = rdotk*uz+rfdotk*vz; 00252 00253 *phase = xlt-xnode-omgadf+twopi; 00254 if(*phase < 0) *phase += twopi; 00255 *phase = FMod2p(*phase); 00256 00257 tle->omegao1=omega; 00258 tle->xincl1=xinck; 00259 tle->xnodeo1=xnodek; 00260 00261 } /*SGP4*/ 00262 00263 /*------------------------------------------------------------------*/ 00264 00265 /* SDP4 */ 00266 /* This function is used to calculate the position and velocity */ 00267 /* of deep-space (period > 225 minutes) satellites. tsince is */ 00268 /* time since epoch in minutes, tle is a pointer to a tle_t */ 00269 /* structure with Keplerian orbital elements and pos and vel */ 00270 /* are vector_t structures returning ECI satellite position and */ 00271 /* velocity. Use Convert_Sat_State() to convert to km and km/s. */ 00272 void 00273 SDP4(double tsince, tle_t *tle, vector_t *pos, vector_t *vel, double* phase) 00274 { 00275 int i; 00276 00277 static double 00278 x3thm1,c1,x1mth2,c4,xnodcf,t2cof,xlcof,aycof,x7thm1; 00279 00280 double 00281 a,axn,ayn,aynl,beta,betal,capu,cos2u,cosepw,cosik, 00282 cosnok,cosu,cosuk,ecose,elsq,epw,esine,pl,theta4, 00283 rdot,rdotk,rfdot,rfdotk,rk,sin2u,sinepw,sinik, 00284 sinnok,sinu,sinuk,tempe,templ,tsq,u,uk,ux,uy,uz, 00285 vx,vy,vz,xinck,xl,xlt,xmam,xmdf,xmx,xmy,xnoddf, 00286 xnodek,xll,a1,a3ovk2,ao,c2,coef,coef1,x1m5th, 00287 xhdot1,del1,r,delo,eeta,eta,etasq,perige, 00288 psisq,tsi,qoms24,s4,pinvsq,temp,tempa,temp1, 00289 temp2,temp3,temp4,temp5,temp6; 00290 00291 static deep_arg_t deep_arg; 00292 00293 /* Initialization */ 00294 if (isFlagClear(SDP4_INITIALIZED_FLAG)) 00295 { 00296 SetFlag(SDP4_INITIALIZED_FLAG); 00297 00298 /* Recover original mean motion (xnodp) and */ 00299 /* semimajor axis (aodp) from input elements. */ 00300 a1 = pow(xke/tle->xno,tothrd); 00301 deep_arg.cosio = cos(tle->xincl); 00302 deep_arg.theta2 = deep_arg.cosio*deep_arg.cosio; 00303 x3thm1 = 3*deep_arg.theta2-1; 00304 deep_arg.eosq = tle->eo*tle->eo; 00305 deep_arg.betao2 = 1-deep_arg.eosq; 00306 deep_arg.betao = sqrt(deep_arg.betao2); 00307 del1 = 1.5*ck2*x3thm1/(a1*a1*deep_arg.betao*deep_arg.betao2); 00308 ao = a1*(1-del1*(0.5*tothrd+del1*(1+134/81*del1))); 00309 delo = 1.5*ck2*x3thm1/(ao*ao*deep_arg.betao*deep_arg.betao2); 00310 deep_arg.xnodp = tle->xno/(1+delo); 00311 deep_arg.aodp = ao/(1-delo); 00312 00313 /* For perigee below 156 km, the values */ 00314 /* of s and qoms2t are altered. */ 00315 s4 = __s__; 00316 qoms24 = qoms2t; 00317 perige = (deep_arg.aodp*(1-tle->eo)-ae)*xkmper; 00318 if(perige < 156) 00319 { 00320 if(perige <= 98) 00321 s4 = 20; 00322 else 00323 s4 = perige-78; 00324 qoms24 = pow((120-s4)*ae/xkmper,4); 00325 s4 = s4/xkmper+ae; 00326 } 00327 pinvsq = 1/(deep_arg.aodp*deep_arg.aodp* 00328 deep_arg.betao2*deep_arg.betao2); 00329 deep_arg.sing = sin(tle->omegao); 00330 deep_arg.cosg = cos(tle->omegao); 00331 tsi = 1/(deep_arg.aodp-s4); 00332 eta = deep_arg.aodp*tle->eo*tsi; 00333 etasq = eta*eta; 00334 eeta = tle->eo*eta; 00335 psisq = fabs(1-etasq); 00336 coef = qoms24*pow(tsi,4); 00337 coef1 = coef/pow(psisq,3.5); 00338 c2 = coef1*deep_arg.xnodp*(deep_arg.aodp*(1+1.5*etasq+eeta* 00339 (4+etasq))+0.75*ck2*tsi/psisq*x3thm1*(8+3*etasq*(8+etasq))); 00340 c1 = tle->bstar*c2; 00341 deep_arg.sinio = sin(tle->xincl); 00342 a3ovk2 = -xj3/ck2*pow(ae,3); 00343 x1mth2 = 1-deep_arg.theta2; 00344 c4 = 2*deep_arg.xnodp*coef1*deep_arg.aodp*deep_arg.betao2* 00345 (eta*(2+0.5*etasq)+tle->eo*(0.5+2*etasq)-2*ck2*tsi/ 00346 (deep_arg.aodp*psisq)*(-3*x3thm1*(1-2*eeta+etasq* 00347 (1.5-0.5*eeta))+0.75*x1mth2*(2*etasq-eeta*(1+etasq))* 00348 cos(2*tle->omegao))); 00349 theta4 = deep_arg.theta2*deep_arg.theta2; 00350 temp1 = 3*ck2*pinvsq*deep_arg.xnodp; 00351 temp2 = temp1*ck2*pinvsq; 00352 temp3 = 1.25*ck4*pinvsq*pinvsq*deep_arg.xnodp; 00353 deep_arg.xmdot = deep_arg.xnodp+0.5*temp1*deep_arg.betao* 00354 x3thm1+0.0625*temp2*deep_arg.betao* 00355 (13-78*deep_arg.theta2+137*theta4); 00356 x1m5th = 1-5*deep_arg.theta2; 00357 deep_arg.omgdot = -0.5*temp1*x1m5th+0.0625*temp2* 00358 (7-114*deep_arg.theta2+395*theta4)+ 00359 temp3*(3-36*deep_arg.theta2+49*theta4); 00360 xhdot1 = -temp1*deep_arg.cosio; 00361 deep_arg.xnodot = xhdot1+(0.5*temp2*(4-19*deep_arg.theta2)+ 00362 2*temp3*(3-7*deep_arg.theta2))*deep_arg.cosio; 00363 xnodcf = 3.5*deep_arg.betao2*xhdot1*c1; 00364 t2cof = 1.5*c1; 00365 xlcof = 0.125*a3ovk2*deep_arg.sinio*(3+5*deep_arg.cosio)/ 00366 (1+deep_arg.cosio); 00367 aycof = 0.25*a3ovk2*deep_arg.sinio; 00368 x7thm1 = 7*deep_arg.theta2-1; 00369 00370 /* initialize Deep() */ 00371 Deep(dpinit, tle, &deep_arg); 00372 }; /*End of SDP4() initialization */ 00373 00374 /* Update for secular gravity and atmospheric drag */ 00375 xmdf = tle->xmo+deep_arg.xmdot*tsince; 00376 deep_arg.omgadf = tle->omegao+deep_arg.omgdot*tsince; 00377 xnoddf = tle->xnodeo+deep_arg.xnodot*tsince; 00378 tsq = tsince*tsince; 00379 deep_arg.xnode = xnoddf+xnodcf*tsq; 00380 tempa = 1-c1*tsince; 00381 tempe = tle->bstar*c4*tsince; 00382 templ = t2cof*tsq; 00383 deep_arg.xn = deep_arg.xnodp; 00384 00385 /* Update for deep-space secular effects */ 00386 deep_arg.xll = xmdf; 00387 deep_arg.t = tsince; 00388 00389 Deep(dpsec, tle, &deep_arg); 00390 00391 xmdf = deep_arg.xll; 00392 a = pow(xke/deep_arg.xn,tothrd)*tempa*tempa; 00393 deep_arg.em = deep_arg.em-tempe; 00394 xmam = xmdf+deep_arg.xnodp*templ; 00395 00396 /* Update for deep-space periodic effects */ 00397 deep_arg.xll = xmam; 00398 00399 Deep(dpper, tle, &deep_arg); 00400 00401 xmam = deep_arg.xll; 00402 xl = xmam+deep_arg.omgadf+deep_arg.xnode; 00403 beta = sqrt(1-deep_arg.em*deep_arg.em); 00404 deep_arg.xn = xke/pow(a,1.5); 00405 00406 /* Long period periodics */ 00407 axn = deep_arg.em*cos(deep_arg.omgadf); 00408 temp = 1/(a*beta*beta); 00409 xll = temp*xlcof*axn; 00410 aynl = temp*aycof; 00411 xlt = xl+xll; 00412 ayn = deep_arg.em*sin(deep_arg.omgadf)+aynl; 00413 00414 /* Solve Kepler's Equation */ 00415 capu = FMod2p(xlt-deep_arg.xnode); 00416 temp2 = capu; 00417 00418 i = 0; 00419 do 00420 { 00421 sinepw = sin(temp2); 00422 cosepw = cos(temp2); 00423 temp3 = axn*sinepw; 00424 temp4 = ayn*cosepw; 00425 temp5 = axn*cosepw; 00426 temp6 = ayn*sinepw; 00427 epw = (capu-temp4+temp3-temp2)/(1-temp5-temp6)+temp2; 00428 if(fabs(epw-temp2) <= e6a) break; 00429 temp2 = epw; 00430 } 00431 while( i++ < 10 ); 00432 00433 /* Short period preliminary quantities */ 00434 ecose = temp5+temp6; 00435 esine = temp3-temp4; 00436 elsq = axn*axn+ayn*ayn; 00437 temp = 1-elsq; 00438 pl = a*temp; 00439 r = a*(1-ecose); 00440 temp1 = 1/r; 00441 rdot = xke*sqrt(a)*esine*temp1; 00442 rfdot = xke*sqrt(pl)*temp1; 00443 temp2 = a*temp1; 00444 betal = sqrt(temp); 00445 temp3 = 1/(1+betal); 00446 cosu = temp2*(cosepw-axn+ayn*esine*temp3); 00447 sinu = temp2*(sinepw-ayn-axn*esine*temp3); 00448 u = AcTan(sinu,cosu); 00449 sin2u = 2*sinu*cosu; 00450 cos2u = 2*cosu*cosu-1; 00451 temp = 1/pl; 00452 temp1 = ck2*temp; 00453 temp2 = temp1*temp; 00454 00455 /* Update for short periodics */ 00456 rk = r*(1-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u; 00457 uk = u-0.25*temp2*x7thm1*sin2u; 00458 xnodek = deep_arg.xnode+1.5*temp2*deep_arg.cosio*sin2u; 00459 xinck = deep_arg.xinc+1.5*temp2*deep_arg.cosio*deep_arg.sinio*cos2u; 00460 rdotk = rdot-deep_arg.xn*temp1*x1mth2*sin2u; 00461 rfdotk = rfdot+deep_arg.xn*temp1*(x1mth2*cos2u+1.5*x3thm1); 00462 00463 /* Orientation vectors */ 00464 sinuk = sin(uk); 00465 cosuk = cos(uk); 00466 sinik = sin(xinck); 00467 cosik = cos(xinck); 00468 sinnok = sin(xnodek); 00469 cosnok = cos(xnodek); 00470 xmx = -sinnok*cosik; 00471 xmy = cosnok*cosik; 00472 ux = xmx*sinuk+cosnok*cosuk; 00473 uy = xmy*sinuk+sinnok*cosuk; 00474 uz = sinik*sinuk; 00475 vx = xmx*cosuk-cosnok*sinuk; 00476 vy = xmy*cosuk-sinnok*sinuk; 00477 vz = sinik*cosuk; 00478 00479 /* Position and velocity */ 00480 pos->x = rk*ux; 00481 pos->y = rk*uy; 00482 pos->z = rk*uz; 00483 vel->x = rdotk*ux+rfdotk*vx; 00484 vel->y = rdotk*uy+rfdotk*vy; 00485 vel->z = rdotk*uz+rfdotk*vz; 00486 00487 /* Phase in rads */ 00488 *phase = xlt-deep_arg.xnode-deep_arg.omgadf+twopi; 00489 if(*phase < 0) *phase += twopi; 00490 *phase = FMod2p(*phase); 00491 00492 tle->omegao1=deep_arg.omgadf; 00493 tle->xincl1=deep_arg.xinc; 00494 tle->xnodeo1=deep_arg.xnode; 00495 } /* SDP4 */ 00496 00497 /*------------------------------------------------------------------*/ 00498 00499 /* DEEP */ 00500 /* This function is used by SDP4 to add lunar and solar */ 00501 /* perturbation effects to deep-space orbit objects. */ 00502 void 00503 Deep(int ientry, tle_t *tle, deep_arg_t *deep_arg) 00504 { 00505 static double 00506 thgr,xnq,xqncl,omegaq,zmol,zmos,savtsn,ee2,e3,xi2, 00507 xl2,xl3,xl4,xgh2,xgh3,xgh4,xh2,xh3,sse,ssi,ssg,xi3, 00508 se2,si2,sl2,sgh2,sh2,se3,si3,sl3,sgh3,sh3,sl4,sgh4, 00509 ssl,ssh,d3210,d3222,d4410,d4422,d5220,d5232,d5421, 00510 d5433,del1,del2,del3,fasx2,fasx4,fasx6,xlamo,xfact, 00511 xni,atime,stepp,stepn,step2,preep,pl,sghs,xli, 00512 d2201,d2211,sghl,sh1,pinc,pe,shs,zsingl,zcosgl, 00513 zsinhl,zcoshl,zsinil,zcosil; 00514 00515 double 00516 a1,a2,a3,a4,a5,a6,a7,a8,a9,a10,ainv2,alfdp,aqnv, 00517 sgh,sini2,sinis,sinok,sh,si,sil,day,betdp,dalf, 00518 bfact,c,cc,cosis,cosok,cosq,ctem,f322,zx,zy, 00519 dbet,dls,eoc,eq,f2,f220,f221,f3,f311,f321,xnoh, 00520 f330,f441,f442,f522,f523,f542,f543,g200,g201, 00521 g211,pgh,ph,s1,s2,s3,s4,s5,s6,s7,se,sel,ses,xls, 00522 g300,g310,g322,g410,g422,g520,g521,g532,g533,gam, 00523 sinq,sinzf,sis,sl,sll,sls,stem,temp,temp1,x1,x2, 00524 x2li,x2omi,x3,x4,x5,x6,x7,x8,xl,xldot,xmao,xnddt, 00525 xndot,xno2,xnodce,xnoi,xomi,xpidot,z1,z11,z12,z13, 00526 z2,z21,z22,z23,z3,z31,z32,z33,ze,zf,zm,/* zmo, (see below) */zn, 00527 zsing,zsinh,zsini,zcosg,zcosh,zcosi,delt=0,ft=0; 00528 00529 /* Compiler complains defined but not used. I never like to 00530 edit other peoples libraries but I also dislike compiler 00531 warnings. AjK, 7th Sep 2010. */ 00532 double zmo __attribute__((unused)); 00533 00534 switch(ientry) 00535 { 00536 case dpinit : /* Entrance for deep space initialization */ 00537 thgr = ThetaG(tle->epoch, deep_arg); 00538 eq = tle->eo; 00539 xnq = deep_arg->xnodp; 00540 aqnv = 1/deep_arg->aodp; 00541 xqncl = tle->xincl; 00542 xmao = tle->xmo; 00543 xpidot = deep_arg->omgdot+deep_arg->xnodot; 00544 sinq = sin(tle->xnodeo); 00545 cosq = cos(tle->xnodeo); 00546 omegaq = tle->omegao; 00547 00548 /* Initialize lunar solar terms */ 00549 day = deep_arg->ds50+18261.5; /*Days since 1900 Jan 0.5*/ 00550 if (day != preep) 00551 { 00552 preep = day; 00553 xnodce = 4.5236020-9.2422029E-4*day; 00554 stem = sin(xnodce); 00555 ctem = cos(xnodce); 00556 zcosil = 0.91375164-0.03568096*ctem; 00557 zsinil = sqrt(1-zcosil*zcosil); 00558 zsinhl = 0.089683511*stem/zsinil; 00559 zcoshl = sqrt(1-zsinhl*zsinhl); 00560 c = 4.7199672+0.22997150*day; 00561 gam = 5.8351514+0.0019443680*day; 00562 zmol = FMod2p(c-gam); 00563 zx = 0.39785416*stem/zsinil; 00564 zy = zcoshl*ctem+0.91744867*zsinhl*stem; 00565 zx = AcTan(zx,zy); 00566 zx = gam+zx-xnodce; 00567 zcosgl = cos(zx); 00568 zsingl = sin(zx); 00569 zmos = 6.2565837+0.017201977*day; 00570 zmos = FMod2p(zmos); 00571 } /* End if(day != preep) */ 00572 00573 /* Do solar terms */ 00574 savtsn = 1E20; 00575 zcosg = zcosgs; 00576 zsing = zsings; 00577 zcosi = zcosis; 00578 zsini = zsinis; 00579 zcosh = cosq; 00580 zsinh = sinq; 00581 cc = c1ss; 00582 zn = zns; 00583 ze = zes; 00584 zmo = zmos; 00585 xnoi = 1/xnq; 00586 00587 /* Loop breaks when Solar terms are done a second */ 00588 /* time, after Lunar terms are initialized */ 00589 for(;;) 00590 { 00591 /* Solar terms done again after Lunar terms are done */ 00592 a1 = zcosg*zcosh+zsing*zcosi*zsinh; 00593 a3 = -zsing*zcosh+zcosg*zcosi*zsinh; 00594 a7 = -zcosg*zsinh+zsing*zcosi*zcosh; 00595 a8 = zsing*zsini; 00596 a9 = zsing*zsinh+zcosg*zcosi*zcosh; 00597 a10 = zcosg*zsini; 00598 a2 = deep_arg->cosio*a7+ deep_arg->sinio*a8; 00599 a4 = deep_arg->cosio*a9+ deep_arg->sinio*a10; 00600 a5 = -deep_arg->sinio*a7+ deep_arg->cosio*a8; 00601 a6 = -deep_arg->sinio*a9+ deep_arg->cosio*a10; 00602 x1 = a1*deep_arg->cosg+a2*deep_arg->sing; 00603 x2 = a3*deep_arg->cosg+a4*deep_arg->sing; 00604 x3 = -a1*deep_arg->sing+a2*deep_arg->cosg; 00605 x4 = -a3*deep_arg->sing+a4*deep_arg->cosg; 00606 x5 = a5*deep_arg->sing; 00607 x6 = a6*deep_arg->sing; 00608 x7 = a5*deep_arg->cosg; 00609 x8 = a6*deep_arg->cosg; 00610 z31 = 12*x1*x1-3*x3*x3; 00611 z32 = 24*x1*x2-6*x3*x4; 00612 z33 = 12*x2*x2-3*x4*x4; 00613 z1 = 3*(a1*a1+a2*a2)+z31*deep_arg->eosq; 00614 z2 = 6*(a1*a3+a2*a4)+z32*deep_arg->eosq; 00615 z3 = 3*(a3*a3+a4*a4)+z33*deep_arg->eosq; 00616 z11 = -6*a1*a5+deep_arg->eosq*(-24*x1*x7-6*x3*x5); 00617 z12 = -6*(a1*a6+a3*a5)+ deep_arg->eosq* 00618 (-24*(x2*x7+x1*x8)-6*(x3*x6+x4*x5)); 00619 z13 = -6*a3*a6+deep_arg->eosq*(-24*x2*x8-6*x4*x6); 00620 z21 = 6*a2*a5+deep_arg->eosq*(24*x1*x5-6*x3*x7); 00621 z22 = 6*(a4*a5+a2*a6)+ deep_arg->eosq* 00622 (24*(x2*x5+x1*x6)-6*(x4*x7+x3*x8)); 00623 z23 = 6*a4*a6+deep_arg->eosq*(24*x2*x6-6*x4*x8); 00624 z1 = z1+z1+deep_arg->betao2*z31; 00625 z2 = z2+z2+deep_arg->betao2*z32; 00626 z3 = z3+z3+deep_arg->betao2*z33; 00627 s3 = cc*xnoi; 00628 s2 = -0.5*s3/deep_arg->betao; 00629 s4 = s3*deep_arg->betao; 00630 s1 = -15*eq*s4; 00631 s5 = x1*x3+x2*x4; 00632 s6 = x2*x3+x1*x4; 00633 s7 = x2*x4-x1*x3; 00634 se = s1*zn*s5; 00635 si = s2*zn*(z11+z13); 00636 sl = -zn*s3*(z1+z3-14-6*deep_arg->eosq); 00637 sgh = s4*zn*(z31+z33-6); 00638 sh = -zn*s2*(z21+z23); 00639 if (xqncl < 5.2359877E-2) sh = 0; 00640 ee2 = 2*s1*s6; 00641 e3 = 2*s1*s7; 00642 xi2 = 2*s2*z12; 00643 xi3 = 2*s2*(z13-z11); 00644 xl2 = -2*s3*z2; 00645 xl3 = -2*s3*(z3-z1); 00646 xl4 = -2*s3*(-21-9*deep_arg->eosq)*ze; 00647 xgh2 = 2*s4*z32; 00648 xgh3 = 2*s4*(z33-z31); 00649 xgh4 = -18*s4*ze; 00650 xh2 = -2*s2*z22; 00651 xh3 = -2*s2*(z23-z21); 00652 00653 if(isFlagSet(LUNAR_TERMS_DONE_FLAG)) break; 00654 00655 /* Do lunar terms */ 00656 sse = se; 00657 ssi = si; 00658 ssl = sl; 00659 ssh = sh/deep_arg->sinio; 00660 ssg = sgh-deep_arg->cosio*ssh; 00661 se2 = ee2; 00662 si2 = xi2; 00663 sl2 = xl2; 00664 sgh2 = xgh2; 00665 sh2 = xh2; 00666 se3 = e3; 00667 si3 = xi3; 00668 sl3 = xl3; 00669 sgh3 = xgh3; 00670 sh3 = xh3; 00671 sl4 = xl4; 00672 sgh4 = xgh4; 00673 zcosg = zcosgl; 00674 zsing = zsingl; 00675 zcosi = zcosil; 00676 zsini = zsinil; 00677 zcosh = zcoshl*cosq+zsinhl*sinq; 00678 zsinh = sinq*zcoshl-cosq*zsinhl; 00679 zn = znl; 00680 cc = c1l; 00681 ze = zel; 00682 zmo = zmol; 00683 SetFlag(LUNAR_TERMS_DONE_FLAG); 00684 } /* End of for(;;) */ 00685 00686 sse = sse+se; 00687 ssi = ssi+si; 00688 ssl = ssl+sl; 00689 ssg = ssg+sgh-deep_arg->cosio/deep_arg->sinio*sh; 00690 ssh = ssh+sh/deep_arg->sinio; 00691 00692 /* Geopotential resonance initialization for 12 hour orbits */ 00693 ClearFlag(RESONANCE_FLAG); 00694 ClearFlag(SYNCHRONOUS_FLAG); 00695 00696 if( !((xnq < 0.0052359877) && (xnq > 0.0034906585)) ) 00697 { 00698 if( (xnq < 0.00826) || (xnq > 0.00924) ) return; 00699 if (eq < 0.5) return; 00700 SetFlag(RESONANCE_FLAG); 00701 eoc = eq*deep_arg->eosq; 00702 g201 = -0.306-(eq-0.64)*0.440; 00703 if (eq <= 0.65) 00704 { 00705 g211 = 3.616-13.247*eq+16.290*deep_arg->eosq; 00706 g310 = -19.302+117.390*eq-228.419* 00707 deep_arg->eosq+156.591*eoc; 00708 g322 = -18.9068+109.7927*eq-214.6334* 00709 deep_arg->eosq+146.5816*eoc; 00710 g410 = -41.122+242.694*eq-471.094* 00711 deep_arg->eosq+313.953*eoc; 00712 g422 = -146.407+841.880*eq-1629.014* 00713 deep_arg->eosq+1083.435*eoc; 00714 g520 = -532.114+3017.977*eq-5740* 00715 deep_arg->eosq+3708.276*eoc; 00716 } 00717 else 00718 { 00719 g211 = -72.099+331.819*eq-508.738* 00720 deep_arg->eosq+266.724*eoc; 00721 g310 = -346.844+1582.851*eq-2415.925* 00722 deep_arg->eosq+1246.113*eoc; 00723 g322 = -342.585+1554.908*eq-2366.899* 00724 deep_arg->eosq+1215.972*eoc; 00725 g410 = -1052.797+4758.686*eq-7193.992* 00726 deep_arg->eosq+3651.957*eoc; 00727 g422 = -3581.69+16178.11*eq-24462.77* 00728 deep_arg->eosq+ 12422.52*eoc; 00729 if (eq <= 0.715) 00730 g520 = 1464.74-4664.75*eq+3763.64*deep_arg->eosq; 00731 else 00732 g520 = -5149.66+29936.92*eq-54087.36* 00733 deep_arg->eosq+31324.56*eoc; 00734 } /* End if (eq <= 0.65) */ 00735 00736 if (eq < 0.7) 00737 { 00738 g533 = -919.2277+4988.61*eq-9064.77* 00739 deep_arg->eosq+5542.21*eoc; 00740 g521 = -822.71072+4568.6173*eq-8491.4146* 00741 deep_arg->eosq+5337.524*eoc; 00742 g532 = -853.666+4690.25*eq-8624.77* 00743 deep_arg->eosq+ 5341.4*eoc; 00744 } 00745 else 00746 { 00747 g533 = -37995.78+161616.52*eq-229838.2* 00748 deep_arg->eosq+109377.94*eoc; 00749 g521 = -51752.104+218913.95*eq-309468.16* 00750 deep_arg->eosq+146349.42*eoc; 00751 g532 = -40023.88+170470.89*eq-242699.48* 00752 deep_arg->eosq+115605.82*eoc; 00753 } /* End if (eq <= 0.7) */ 00754 00755 sini2 = deep_arg->sinio*deep_arg->sinio; 00756 f220 = 0.75*(1+2*deep_arg->cosio+deep_arg->theta2); 00757 f221 = 1.5*sini2; 00758 f321 = 1.875*deep_arg->sinio*(1-2*\ 00759 deep_arg->cosio-3*deep_arg->theta2); 00760 f322 = -1.875*deep_arg->sinio*(1+2* 00761 deep_arg->cosio-3*deep_arg->theta2); 00762 f441 = 35*sini2*f220; 00763 f442 = 39.3750*sini2*sini2; 00764 f522 = 9.84375*deep_arg->sinio*(sini2*(1-2*deep_arg->cosio-5* 00765 deep_arg->theta2)+0.33333333*(-2+4*deep_arg->cosio+ 00766 6*deep_arg->theta2)); 00767 f523 = deep_arg->sinio*(4.92187512*sini2*(-2-4* 00768 deep_arg->cosio+10*deep_arg->theta2)+6.56250012 00769 *(1+2*deep_arg->cosio-3*deep_arg->theta2)); 00770 f542 = 29.53125*deep_arg->sinio*(2-8* 00771 deep_arg->cosio+deep_arg->theta2* 00772 (-12+8*deep_arg->cosio+10*deep_arg->theta2)); 00773 f543 = 29.53125*deep_arg->sinio*(-2-8*deep_arg->cosio+ 00774 deep_arg->theta2*(12+8*deep_arg->cosio-10* 00775 deep_arg->theta2)); 00776 xno2 = xnq*xnq; 00777 ainv2 = aqnv*aqnv; 00778 temp1 = 3*xno2*ainv2; 00779 temp = temp1*root22; 00780 d2201 = temp*f220*g201; 00781 d2211 = temp*f221*g211; 00782 temp1 = temp1*aqnv; 00783 temp = temp1*root32; 00784 d3210 = temp*f321*g310; 00785 d3222 = temp*f322*g322; 00786 temp1 = temp1*aqnv; 00787 temp = 2*temp1*root44; 00788 d4410 = temp*f441*g410; 00789 d4422 = temp*f442*g422; 00790 temp1 = temp1*aqnv; 00791 temp = temp1*root52; 00792 d5220 = temp*f522*g520; 00793 d5232 = temp*f523*g532; 00794 temp = 2*temp1*root54; 00795 d5421 = temp*f542*g521; 00796 d5433 = temp*f543*g533; 00797 xlamo = xmao+tle->xnodeo+tle->xnodeo-thgr-thgr; 00798 bfact = deep_arg->xmdot+deep_arg->xnodot+ 00799 deep_arg->xnodot-thdt-thdt; 00800 bfact = bfact+ssl+ssh+ssh; 00801 } /* if( !(xnq < 0.0052359877) && (xnq > 0.0034906585) ) */ 00802 else 00803 { 00804 SetFlag(RESONANCE_FLAG); 00805 SetFlag(SYNCHRONOUS_FLAG); 00806 /* Synchronous resonance terms initialization */ 00807 g200 = 1+deep_arg->eosq*(-2.5+0.8125*deep_arg->eosq); 00808 g310 = 1+2*deep_arg->eosq; 00809 g300 = 1+deep_arg->eosq*(-6+6.60937*deep_arg->eosq); 00810 f220 = 0.75*(1+deep_arg->cosio)*(1+deep_arg->cosio); 00811 f311 = 0.9375*deep_arg->sinio*deep_arg->sinio* 00812 (1+3*deep_arg->cosio)-0.75*(1+deep_arg->cosio); 00813 f330 = 1+deep_arg->cosio; 00814 f330 = 1.875*f330*f330*f330; 00815 del1 = 3*xnq*xnq*aqnv*aqnv; 00816 del2 = 2*del1*f220*g200*q22; 00817 del3 = 3*del1*f330*g300*q33*aqnv; 00818 del1 = del1*f311*g310*q31*aqnv; 00819 fasx2 = 0.13130908; 00820 fasx4 = 2.8843198; 00821 fasx6 = 0.37448087; 00822 xlamo = xmao+tle->xnodeo+tle->omegao-thgr; 00823 bfact = deep_arg->xmdot+xpidot-thdt; 00824 bfact = bfact+ssl+ssg+ssh; 00825 } /* End if( !(xnq < 0.0052359877) && (xnq > 0.0034906585) ) */ 00826 00827 xfact = bfact-xnq; 00828 00829 /* Initialize integrator */ 00830 xli = xlamo; 00831 xni = xnq; 00832 atime = 0; 00833 stepp = 720; 00834 stepn = -720; 00835 step2 = 259200; 00836 /* End case dpinit: */ 00837 return; 00838 00839 case dpsec: /* Entrance for deep space secular effects */ 00840 deep_arg->xll = deep_arg->xll+ssl*deep_arg->t; 00841 deep_arg->omgadf = deep_arg->omgadf+ssg*deep_arg->t; 00842 deep_arg->xnode = deep_arg->xnode+ssh*deep_arg->t; 00843 deep_arg->em = tle->eo+sse*deep_arg->t; 00844 deep_arg->xinc = tle->xincl+ssi*deep_arg->t; 00845 if (deep_arg->xinc < 0) 00846 { 00847 deep_arg->xinc = -deep_arg->xinc; 00848 deep_arg->xnode = deep_arg->xnode + pi; 00849 deep_arg->omgadf = deep_arg->omgadf-pi; 00850 } 00851 if( isFlagClear(RESONANCE_FLAG) ) return; 00852 00853 do 00854 { 00855 if( (atime == 0) || 00856 ((deep_arg->t >= 0) && (atime < 0)) || 00857 ((deep_arg->t < 0) && (atime >= 0)) ) 00858 { 00859 /* Epoch restart */ 00860 if( deep_arg->t >= 0 ) 00861 delt = stepp; 00862 else 00863 delt = stepn; 00864 00865 atime = 0; 00866 xni = xnq; 00867 xli = xlamo; 00868 } 00869 else 00870 { 00871 if( fabs(deep_arg->t) >= fabs(atime) ) 00872 { 00873 if ( deep_arg->t > 0 ) 00874 delt = stepp; 00875 else 00876 delt = stepn; 00877 } 00878 } 00879 00880 do 00881 { 00882 if ( fabs(deep_arg->t-atime) >= stepp ) 00883 { 00884 SetFlag(DO_LOOP_FLAG); 00885 ClearFlag(EPOCH_RESTART_FLAG); 00886 } 00887 else 00888 { 00889 ft = deep_arg->t-atime; 00890 ClearFlag(DO_LOOP_FLAG); 00891 } 00892 00893 if( fabs(deep_arg->t) < fabs(atime) ) 00894 { 00895 if (deep_arg->t >= 0) 00896 delt = stepn; 00897 else 00898 delt = stepp; 00899 SetFlag(DO_LOOP_FLAG | EPOCH_RESTART_FLAG); 00900 } 00901 00902 /* Dot terms calculated */ 00903 if( isFlagSet(SYNCHRONOUS_FLAG) ) 00904 { 00905 xndot = del1*sin(xli-fasx2)+del2*sin(2*(xli-fasx4)) 00906 +del3*sin(3*(xli-fasx6)); 00907 xnddt = del1*cos(xli-fasx2)+2*del2*cos(2*(xli-fasx4)) 00908 +3*del3*cos(3*(xli-fasx6)); 00909 } 00910 else 00911 { 00912 xomi = omegaq+deep_arg->omgdot*atime; 00913 x2omi = xomi+xomi; 00914 x2li = xli+xli; 00915 xndot = d2201*sin(x2omi+xli-g22) 00916 +d2211*sin(xli-g22) 00917 +d3210*sin(xomi+xli-g32) 00918 +d3222*sin(-xomi+xli-g32) 00919 +d4410*sin(x2omi+x2li-g44) 00920 +d4422*sin(x2li-g44) 00921 +d5220*sin(xomi+xli-g52) 00922 +d5232*sin(-xomi+xli-g52) 00923 +d5421*sin(xomi+x2li-g54) 00924 +d5433*sin(-xomi+x2li-g54); 00925 xnddt = d2201*cos(x2omi+xli-g22) 00926 +d2211*cos(xli-g22) 00927 +d3210*cos(xomi+xli-g32) 00928 +d3222*cos(-xomi+xli-g32) 00929 +d5220*cos(xomi+xli-g52) 00930 +d5232*cos(-xomi+xli-g52) 00931 +2*(d4410*cos(x2omi+x2li-g44) 00932 +d4422*cos(x2li-g44) 00933 +d5421*cos(xomi+x2li-g54) 00934 +d5433*cos(-xomi+x2li-g54)); 00935 } /* End of if (isFlagSet(SYNCHRONOUS_FLAG)) */ 00936 00937 xldot = xni+xfact; 00938 xnddt = xnddt*xldot; 00939 00940 if(isFlagSet(DO_LOOP_FLAG)) 00941 { 00942 xli = xli+xldot*delt+xndot*step2; 00943 xni = xni+xndot*delt+xnddt*step2; 00944 atime = atime+delt; 00945 } 00946 } 00947 while(isFlagSet(DO_LOOP_FLAG) && isFlagClear(EPOCH_RESTART_FLAG)); 00948 } 00949 while(isFlagSet(DO_LOOP_FLAG) && isFlagSet(EPOCH_RESTART_FLAG)); 00950 00951 deep_arg->xn = xni+xndot*ft+xnddt*ft*ft*0.5; 00952 xl = xli+xldot*ft+xndot*ft*ft*0.5; 00953 temp = -deep_arg->xnode+thgr+deep_arg->t*thdt; 00954 00955 if (isFlagClear(SYNCHRONOUS_FLAG)) 00956 deep_arg->xll = xl+temp+temp; 00957 else 00958 deep_arg->xll = xl-deep_arg->omgadf+temp; 00959 00960 return; 00961 /*End case dpsec: */ 00962 00963 case dpper: /* Entrance for lunar-solar periodics */ 00964 sinis = sin(deep_arg->xinc); 00965 cosis = cos(deep_arg->xinc); 00966 if (fabs(savtsn-deep_arg->t) >= 30) 00967 { 00968 savtsn = deep_arg->t; 00969 zm = zmos+zns*deep_arg->t; 00970 zf = zm+2*zes*sin(zm); 00971 sinzf = sin(zf); 00972 f2 = 0.5*sinzf*sinzf-0.25; 00973 f3 = -0.5*sinzf*cos(zf); 00974 ses = se2*f2+se3*f3; 00975 sis = si2*f2+si3*f3; 00976 sls = sl2*f2+sl3*f3+sl4*sinzf; 00977 sghs = sgh2*f2+sgh3*f3+sgh4*sinzf; 00978 shs = sh2*f2+sh3*f3; 00979 zm = zmol+znl*deep_arg->t; 00980 zf = zm+2*zel*sin(zm); 00981 sinzf = sin(zf); 00982 f2 = 0.5*sinzf*sinzf-0.25; 00983 f3 = -0.5*sinzf*cos(zf); 00984 sel = ee2*f2+e3*f3; 00985 sil = xi2*f2+xi3*f3; 00986 sll = xl2*f2+xl3*f3+xl4*sinzf; 00987 sghl = xgh2*f2+xgh3*f3+xgh4*sinzf; 00988 sh1 = xh2*f2+xh3*f3; 00989 pe = ses+sel; 00990 pinc = sis+sil; 00991 pl = sls+sll; 00992 } 00993 00994 pgh = sghs+sghl; 00995 ph = shs+sh1; 00996 deep_arg->xinc = deep_arg->xinc+pinc; 00997 deep_arg->em = deep_arg->em+pe; 00998 00999 if (xqncl >= 0.2) 01000 { 01001 /* Apply periodics directly */ 01002 ph = ph/deep_arg->sinio; 01003 pgh = pgh-deep_arg->cosio*ph; 01004 deep_arg->omgadf = deep_arg->omgadf+pgh; 01005 deep_arg->xnode = deep_arg->xnode+ph; 01006 deep_arg->xll = deep_arg->xll+pl; 01007 } 01008 else 01009 { 01010 /* Apply periodics with Lyddane modification */ 01011 sinok = sin(deep_arg->xnode); 01012 cosok = cos(deep_arg->xnode); 01013 alfdp = sinis*sinok; 01014 betdp = sinis*cosok; 01015 dalf = ph*cosok+pinc*cosis*sinok; 01016 dbet = -ph*sinok+pinc*cosis*cosok; 01017 alfdp = alfdp+dalf; 01018 betdp = betdp+dbet; 01019 deep_arg->xnode = FMod2p(deep_arg->xnode); 01020 xls = deep_arg->xll+deep_arg->omgadf+cosis*deep_arg->xnode; 01021 dls = pl+pgh-pinc*deep_arg->xnode*sinis; 01022 xls = xls+dls; 01023 xnoh = deep_arg->xnode; 01024 deep_arg->xnode = AcTan(alfdp,betdp); 01025 01026 /* This is a patch to Lyddane modification */ 01027 /* suggested by Rob Matson. */ 01028 if(fabs(xnoh-deep_arg->xnode) > pi) 01029 { 01030 if(deep_arg->xnode < xnoh) 01031 deep_arg->xnode +=twopi; 01032 else 01033 deep_arg->xnode -=twopi; 01034 } 01035 01036 deep_arg->xll = deep_arg->xll+pl; 01037 deep_arg->omgadf = xls-deep_arg->xll-cos(deep_arg->xinc)* 01038 deep_arg->xnode; 01039 } /* End case dpper: */ 01040 return; 01041 01042 } /* End switch(ientry) */ 01043 01044 } /* End of Deep() */ 01045 01046 /*------------------------------------------------------------------*/ 01047 01048 /* Functions for testing and setting/clearing flags */ 01049 01050 /* An int variable holding the single-bit flags */ 01051 static int Flags = 0; 01052 01053 int 01054 isFlagSet(int flag) 01055 { 01056 return (Flags & flag); 01057 } 01058 01059 int 01060 isFlagClear(int flag) 01061 { 01062 return (~Flags & flag); 01063 } 01064 01065 void 01066 SetFlag(int flag) 01067 { 01068 Flags |= flag; 01069 } 01070 01071 void 01072 ClearFlag(int flag) 01073 { 01074 Flags &= ~flag; 01075 } 01076 01077 /*------------------------------------------------------------------*/
Generated on Tue Jul 12 2022 18:05:35 by
1.7.2