0000#include <stdio.h>
0001#include <math.h>
0002#include <orbit.h>
0003
0004
0005/*************************************************************/
0006/*  vector external multiplication       */
0007/*************************************************************/
0008void vecXmul(double *a,double *b,double *c)
0009{
0010 c[0]=a[1]*b[2]-a[2]*b[1];
0011 c[1]=a[2]*b[0]-a[0]*b[2];
0012 c[2]=a[0]*b[1]-a[1]*b[0];
0013}
0014
0015/************************************************************/
0016/*  vector internal multiplication      */
0017/************************************************************/
0018double vecImul(double *a,double *b)
0019{
0020 return((double)(a[0]*b[0]+a[1]*b[1]+a[2]*b[2]));
0021}
0022
0023/************************************************************/
0024/*  angle between two vectors       */
0025/************************************************************/
0026double vecAngSimp(double *a,double *b)
0027{
0028 double aa,bb;
0029 aa=vecNorm(a);
0030 bb=vecNorm(b);
0031 bb=vecImul(a,b)/(aa*bb);
0032// aa=sqrt(1.-bb*bb);
0033// return( (double)(atan2(aa,bb)));
0034 return( (double)acos(bb));
0035}
0036
0037/************************************************************/
0038/*  angle between two vectors       */
0039/************************************************************/
0040double vecAngDire(double *a,double *b,double *c)
0041{
0042 double ang;
0043 double cc[3];
0044 ang=vecAngSimp(a,b);
0045 vecXmul(a,b,cc);
0046 if(vecImul(cc,c)>0.) return(ang);
0047 else return((double)(6.283185306-ang));
0048}
0049
0050/************************************************************/
0051/*  vector conversion         */
0052/************************************************************/
0053void vecCnv(double a[][3],double *b,double *c)
0054{
0055 short i,j;
0056 for(i=0;i<3;i++){
0057  c[i]=0.;
0058  for(j=0;j<3;j++) c[i]+=a[i][j]*b[j];
0059 }
0060}
0061
0062void ele2vec(double x,double y,double z,double *a)
0063{
0064 a[0]=x;
0065 a[1]=y;
0066 a[2]=z;
0067}
0068
0069void vec2ele(double *a,double *x,double *y,double *z)
0070{
0071 *x=a[0];
0072 *y=a[1];
0073 *z=a[2];
0074}
0075
0076/********************************************************/
0077/*  absolute value of a vector      */
0078/********************************************************/
0079double vecNorm(double *c)
0080{
0081 return((double)sqrt(c[0]*c[0]+c[1]*c[1]+c[2]*c[2]));
0082}
0083
0084/************************************************************/
0085/*  mm.ddyyyy(day1) & hh.mmss(hr) vs. ref. day(day2) */
0086/*  mode=0 : day1 to day2 mode=1 : day2 to day1  */
0087/************************************************************/
0088void date(DateTimePtr day1,double *day2,Boolean left2right)
0089{
0090 double dy1,h,dy2;
0091 double dmo,day3,ddy,dyr,dhr,dhr1,dmn;
0092 
0093 if(left2right){
0094  dmo=day1->mo;
0095  ddy=day1->dd;
0096  dyr=day1->yyyy;
0097  dhr=day1->hh;
0098  dmn=day1->mi;
0099  dhr1=day1->ss;
0100  dhr=dhr+dmn/60.+dhr1/3600.;
0101  if(dmo<2.1){
0102   dyr=dyr-1.;
0103   dmo=dmo+13.;
0104  }
0105  else dmo=dmo+1.;
0106  *day2=floor(365.25*dyr)+floor(30.6001*dmo)+ddy+1720982.
0107    +dhr/24.-2433283.;
0108 }
0109 else{
0110  dy2=*day2;
0111  day3=floor(dy2);
0112  h=dy2-day3;
0113  day3=day3+2433283.-1720982;
0114  dyr=floor((day3-122.1)/365.25);
0115  dmo=floor((day3-floor(365.25*dyr))/30.6001);
0116  ddy=day3-floor(365.25*dyr)-floor(30.6001*dmo);
0117  dmo=dmo-1.;
0118  if(dmo>12.5) dmo=dmo-12.;
0119  if(dmo<2.5) dyr=dyr+1.;
0120  day1->yyyy=dyr;
0121  day1->mo=dmo;
0122  day1->dd=ddy;
0123  day3=h*24.;
0124  h=floor(day3);
0125  day3=(day3-h)*60.;
0126  dmn=floor(day3);
0127  day3=(day3-dmn)*60.;
0128  h=h+0.0001*day3;
0129  day1->hh=h;
0130  day1->mi=dmn;
0131  day1->ss=day3;
0132 }
0133}
0134  
0135/************************************************************/
0136/*  time of the day          */
0137/************************************************************/
0138double tod(double day)
0139{
0140 double thet;
0141 
0142 thet=fmod(day*360.985,360.);
0143 thet=fmod(0.29015e-12*day*day+6.47346e-4*day+thet
0144   +100.0755425L,360.);
0145 thet*=0.017453293;
0146 return(thet);
0147}
0148
0149/************************************************************/
0150/*  true anomaly to mean anomaly      */
0151/************************************************************/
0152double f2m(double f,double e)
0153{
0154 double E,M;
0155 
0156 E=f2e(f,e);
0157 M=e2m(E,e);
0158 return(M);
0159}
0160
0161/************************************************************/
0162/*  true anomaly to eccentric anomaly     */
0163/************************************************************/
0164double f2e(double f,double e)
0165{
0166 double E;
0167 E=2.*atan(sqrt((1.-e)/(1.+e))*tan(0.5*f));
0168 return(E);
0169}
0170
0171/************************************************************/
0172/*  eccentric anomaly to mean anomaly     */
0173/************************************************************/
0174double e2m(double E,double e)
0175{
0176 double M;
0177 M=E-e*sin(E);
0178 return(M);
0179}
0180
0181/************************************************************/
0182/*  eccentric anomaly to true anomaly     */
0183/************************************************************/
0184double e2f(double E,double e)
0185{
0186 double f;
0187 f=2.*atan(sqrt( (1.+e)/(1.-e) )*tan(0.5*E));
0188 return(f);
0189}
0190
0191/************************************************************/
0192/*  mean anomaly to eccentric anomaly     */
0193/************************************************************/
0194double m2e(double M,double e)
0195{
0196 double E;
0197 short i;
0198 
0199 E=M;
0200 for(i=0;i<14;i++) E=M+e*sin(E);
0201 return(E);
0202}
0203
0204/************************************************************/
0205/*  mean anomaly to true anomaly      */
0206/************************************************************/
0207double m2f(double M,double e)
0208{
0209 double f,E;
0210 E=m2e(M,e);
0211 f=e2f(E,e);
0212 return(f);
0213}
0214
0215/************************************************/
0216/* LV and Six conversion      */
0217/************************************************/
0218void LVSix(LVElemPtr a,SixElemPtr b,Boolean left2right)
0219{
0220 double c[3][3],vdash[3],v0[3],ha[3],hb[3];
0221 double at,on,h,v,az,el,cf1,hh,di,omegal,dr;
0222 double mu,dx,dy,de,df0,da,omegas,day1,dthet;
0223 double hb1,M;
0224 
0225 double aa[3],bb[3],cc[3];
0226
0227 
0228 if(left2right){
0229  at=a->lat;
0230  on=a->lon;
0231  h=a->h;
0232  v=a->v;
0233  az=a->az;
0234  el=a->el;
0235 
0236  cf1=0.01745329251;
0237  at*=cf1;
0238  on*=cf1;
0239  az*=cf1;
0240  el*=cf1;
0241  c[0][0]=-sin(on); // local coordinate pointing east
0242  c[1][0]=cos(on);
0243  c[2][0]=0.;
0244  c[0][2]=cos(at)*cos(on); // local coordinate pointing up
0245  c[1][2]=cos(at)*sin(on);
0246  c[2][2]=sin(at);
0247  ele2vec(c[0][2],c[1][2],c[2][2],aa); // radial vector
0248  ele2vec(c[0][0],c[1][0],c[2][0],bb); // east vector
0249  vecXmul(aa,bb,cc);      // north vector
0250  vec2ele(cc,&c[0][1],&c[1][1],&c[2][1]); // local coordinate at separation point
0251  vdash[0]=v*cos(el)*sin(az);  // east
0252  vdash[1]=v*cos(el)*cos(az);  // north
0253  vdash[2]=v*sin(el);    // up
0254  vecCnv(c,vdash,v0);    // v0 in geocentric coordinate
0255  vecXmul(aa,v0,ha); // moment of velocity vector
0256  hh=vecNorm(ha);
0257  ha[0]/=hh;
0258  ha[1]/=hh;
0259  ha[2]/=hh;
0260  di=sqrt(ha[0]*ha[0]+ha[1]*ha[1]);
0261  di=atan2(di,ha[2]);      // inclination in radian
0262  omegal=atan2(ha[1],ha[0])+1.5707963267; // large omega
0263  dr=6378142.+h;
0264  hh=dr*hh;    // moment of velocity
0265  hb1=vecImul(aa,v0);  // aa=radial vector
0266  mu=3.986009e14;
0267  dy=hh*hb1;
0268  dx=hh*hh/dr-mu;
0269  de=sqrt(dx*dx+dy*dy)/mu;    // eccentricity
0270  df0=atan2(dy,dx);      // true anomaly
0271  da=hh*hh/(mu*(1.-de*de));
0272  
0273  ele2vec(0.,0.,1.,aa);
0274  vecXmul(aa,ha,hb);
0275  ele2vec(c[0][2],c[1][2],c[2][2],aa); // radial vector
0276  omegas=vecAngDire(hb,aa,ha);
0277  omegas=omegas-df0;
0278  date((DateTimePtr) a,(double *)b,TRUE);
0279  b->epoc+=a->elapsed/86400.;
0280  dthet=tod(b->epoc); // epoc (day from 1950.1.1)
0281  omegal=omegal+dthet;    // large omega
0282  M=f2m(df0,de);
0283  b->e=de;  // eccentricity
0284  b->M=M;  // mean anomaly (rad)
0285  b->om=omegas;  // small omega (rad)
0286  b->i=di;  // inclination (rad)
0287  b->Om=omegal;  // large omega (rad)
0288  b->a=da;  // semi-major axis (m)
0289 }
0290 else{
0291  date((DateTimePtr)a,(double *)b,FALSE);
0292  // to be added later
0293 }
0294}
0295
0296/************************************************************/
0297/*  6 elements vs. cartesian       */
0298/*               */
0299/************************************************************/
0300void SixCart(SixElemPtr a,CartElemPtr b,Boolean left2right)
0301{
0302 double E,x,y,sinlm,coslm,sinsm,cossm,sini,cosi;
0303 double r,mu,vx,vy,ha,r1,f;
0304 double h[3],d[3];
0305 
0306 if(left2right){
0307  E=m2e(a->M,a->e);
0308  x=(a->a)*(cos(E)-(a->e));
0309  y=(a->a)*sqrt(1.-(a->e)*(a->e))*sin(E);
0310  
0311  sinlm=sin(a->Om);
0312  coslm=cos(a->Om);
0313  sinsm=sin(a->om);
0314  cossm=cos(a->om);
0315  sini=sin(a->i);
0316  cosi=cos(a->i);
0317  
0318  b->p[0]=(coslm*cossm-sinlm*sinsm*cosi)*x
0319    +(-coslm*sinsm-sinlm*cossm*cosi)*y;
0320  b->p[1]=(sinlm*cossm+coslm*sinsm*cosi)*x
0321    +(-sinlm*sinsm+coslm*cossm*cosi)*y;
0322  b->p[2]=sinsm*sini*x+cossm*sini*y;
0323  
0324  r=vecNorm((double *)(b->p));
0325  mu=3.986009e14;
0326  
0327  vx=-(sqrt(mu*(a->a))/r)*sin(E);
0328  vy=(sqrt(mu*(a->a)*(1.-(a->e)*(a->e)))/r)*cos(E);
0329
0330  b->v[0]=(coslm*cossm-sinlm*sinsm*cosi)*vx
0331    +(-coslm*sinsm-sinlm*cossm*cosi)*vy;
0332  b->v[1]=(sinlm*cossm+coslm*sinsm*cosi)*vx
0333    +(-sinlm*sinsm+coslm*cossm*cosi)*vy;
0334  b->v[2]=sinsm*sini*vx+cossm*sini*vy;
0335  
0336 }
0337 else{
0338  if(b->p[2]==0.&&b->v[2]==0.){
0339   h[2]=(b->p[0])*(b->v[1])-(b->p[1])*(b->v[0]);
0340   ha=h[2];
0341   a->Om=0.;
0342   if(h[2]>0.)
0343    (a->i)=0.;
0344   else
0345    (a->i)=3.14159265;
0346  }
0347  else{
0348   h[0]=(b->p[1])*(b->v[2])-(b->p[2])*(b->v[1]);
0349   h[1]=(b->p[2])*(b->v[0])-(b->p[0])*(b->v[2]);
0350   h[2]=(b->p[0])*(b->v[1])-(b->p[1])*(b->v[0]);
0351   
0352   ha=vecNorm(h);
0353   a->i=acos(h[2]/ha);
0354   x=-h[1];
0355   y=h[0];
0356   a->Om=atan2(y,x);
0357  }
0358
0359  r=vecNorm(b->p);
0360  mu=3.986009e14;
0361  x=ha*ha/r-mu;
0362  y=ha*vecImul(b->p,b->v)/r;
0363  r1=sqrt(x*x+y*y);
0364  if(r1==0.){
0365   a->e=0.0;
0366   a->om=0.0;
0367   d[0]=cos(a->Om);
0368   d[1]=sin(a->Om);
0369   d[2]=0.;
0370   a->M=vecAngDire(d,b->p,h);
0371  }
0372  else {
0373   f=atan2(y,x);
0374   a->e=sqrt(x*x+y*y)/mu;
0375   a->M=f2m(f,a->e);
0376   a->a=ha*ha/(mu*(1.-(a->e)*(a->e)));
0377   d[0]=-h[1];
0378   d[1]=h[0];
0379   d[2]=0.;
0380   a->om=vecAngDire(d,b->p,h)-f;
0381   if(a->om<0.) a->om+=6.283185306;
0382  }
0383 }
0384}