fgmsis.cpp

6 DOF Missle Simulation

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void MSIS::spline (double *x, double *y, int n, double yp1, double ypn, double *y2)
{
/*       CALCULATE 2ND DERIVATIVES OF CUBIC SPLINE INTERP FUNCTION
 *       ADAPTED FROM NUMERICAL RECIPES BY PRESS ET AL
 *       X,Y: ARRAYS OF TABULATED FUNCTION IN ASCENDING ORDER BY X
 *       N: SIZE OF ARRAYS X,Y
 *       YP1,YPN: SPECIFIED DERIVATIVES AT X[0] AND X[N-1]; VALUES
 *                >= 1E30 SIGNAL SIGNAL SECOND DERIVATIVE ZERO
 *       Y2: OUTPUT ARRAY OF SECOND DERIVATIVES
 */
  double *u;
  double sig, p, qn, un;
  int i, k;
  u=(double*)malloc(sizeof(double)*n);
  if (u==NULL) {
    printf("Out Of Memory in spline - ERROR");
    return;
  }
  if (yp1>0.99E30) {
    y2[0]=0;
    u[0]=0;
  } else {
    y2[0]=-0.5;
    u[0]=(3.0/(x[1]-x[0]))*((y[1]-y[0])/(x[1]-x[0])-yp1);
  }
  for (i=1;i<(n-1);i++) {
    sig = (x[i]-x[i-1])/(x[i+1] - x[i-1]);
    p = sig * y2[i-1] + 2.0;
    y2[i] = (sig - 1.0) / p;
    u[i] = (6.0 * ((y[i+1] - y[i])/(x[i+1] - x[i]) -(y[i] - y[i-1]) / (x[i] - x[i-1]))/(x[i+1] - x[i-1]) - sig * u[i-1])/p;
  }
  if (ypn>0.99E30) {
    qn = 0;
    un = 0;
  } else {
    qn = 0.5;
    un = (3.0 / (x[n-1] - x[n-2])) * (ypn - (y[n-1] - y[n-2])/(x[n-1] - x[n-2]));
  }
  y2[n-1] = (un - qn * u[n-2]) / (qn * y2[n-2] + 1.0);
  for (k=n-2;k>=0;k--)
    y2[k] = y2[k] * y2[k+1] + u[k];

  free(u);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double MSIS::zeta(double zz, double zl)
{
  return ((zz-zl)*(re+zl)/(re+zz));
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double MSIS::densm(double alt, double d0, double xm, double *tz, int mn3,
                     double *zn3, double *tn3, double *tgn3, int mn2, double *zn2,
                     double *tn2, double *tgn2)
{
/*      Calculate Temperature and Density Profiles for lower atmos.  */
  double xs[10], ys[10], y2out[10];
  double rgas = 831.4;
  double z, z1, z2, t1, t2, zg, zgdif;
  double yd1, yd2;
  double x, y, yi;
  double expl, gamm, glb;
  double densm_tmp;
  int mn;
  int k;
  densm_tmp=d0;
  if (alt>zn2[0]) {
    if (xm==0.0)
      return *tz;
    else
      return d0;
  }

  /* STRATOSPHERE/MESOSPHERE TEMPERATURE */
  if (alt>zn2[mn2-1])
    z=alt;
  else
    z=zn2[mn2-1];
  mn=mn2;
  z1=zn2[0];
  z2=zn2[mn-1];
  t1=tn2[0];
  t2=tn2[mn-1];
  zg = zeta(z, z1);
  zgdif = zeta(z2, z1);

  /* set up spline nodes */
  for (k=0;k<mn;k++) {
    xs[k]=zeta(zn2[k],z1)/zgdif;
    ys[k]=1.0 / tn2[k];
  }
  yd1=-tgn2[0] / (t1*t1) * zgdif;
  yd2=-tgn2[1] / (t2*t2) * zgdif * (pow(((re+z2)/(re+z1)),2.0));

  /* calculate spline coefficients */
  spline (xs, ys, mn, yd1, yd2, y2out);
  x = zg/zgdif;
  splint (xs, ys, y2out, mn, x, &y);

  /* temperature at altitude */
  *tz = 1.0 / y;
  if (xm!=0.0) {
    /* calaculate stratosphere / mesospehere density */
    glb = gsurf / (pow((1.0 + z1/re),2.0));
    gamm = xm * glb * zgdif / rgas;

    /* Integrate temperature profile */
    splini(xs, ys, y2out, mn, x, &yi);
    expl=gamm*yi;
    if (expl>50.0)
      expl=50.0;

    /* Density at altitude */
    densm_tmp = densm_tmp * (t1 / *tz) * exp(-expl);
  }

  if (alt>zn3[0]) {
    if (xm==0.0)
      return *tz;
    else
      return densm_tmp;
  }

  /* troposhere / stratosphere temperature */
  z = alt;
  mn = mn3;
  z1=zn3[0];
  z2=zn3[mn-1];
  t1=tn3[0];
  t2=tn3[mn-1];
  zg=zeta(z,z1);
  zgdif=zeta(z2,z1);

  /* set up spline nodes */
  for (k=0;k<mn;k++) {
    xs[k] = zeta(zn3[k],z1) / zgdif;
    ys[k] = 1.0 / tn3[k];
  }
  yd1=-tgn3[0] / (t1*t1) * zgdif;
  yd2=-tgn3[1] / (t2*t2) * zgdif * (pow(((re+z2)/(re+z1)),2.0));

  /* calculate spline coefficients */
  spline (xs, ys, mn, yd1, yd2, y2out);
  x = zg/zgdif;
  splint (xs, ys, y2out, mn, x, &y);

  /* temperature at altitude */
  *tz = 1.0 / y;
  if (xm!=0.0) {
    /* calaculate tropospheric / stratosphere density */
    glb = gsurf / (pow((1.0 + z1/re),2.0));
    gamm = xm * glb * zgdif / rgas;

    /* Integrate temperature profile */
    splini(xs, ys, y2out, mn, x, &yi);
    expl=gamm*yi;
    if (expl>50.0)
      expl=50.0;

    /* Density at altitude */
    densm_tmp = densm_tmp * (t1 / *tz) * exp(-expl);
  }
  if (xm==0.0)
    return *tz;
  else
    return densm_tmp;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double MSIS::densu(double alt, double dlb, double tinf, double tlb, double xm,
                     double alpha, double *tz, double zlb, double s2, int mn1,
                     double *zn1, double *tn1, double *tgn1)
{
/*      Calculate Temperature and Density Profiles for MSIS models
 *      New lower thermo polynomial
 */
  double yd2, yd1, x=0.0, y=0.0;
  double rgas=831.4;
  double densu_temp=1.0;
  double za, z, zg2, tt, ta=0.0;
  double dta, z1=0.0, z2, t1=0.0, t2, zg, zgdif=0.0;
  int mn=0;
  int k;
  double glb;
  double expl;
  double yi;
  double densa;
  double gamma, gamm;
  double xs[5], ys[5], y2out[5];
  /* joining altitudes of Bates and spline */
  za=zn1[0];
  if (alt>za)
    z=alt;
  else
    z=za;

  /* geopotential altitude difference from ZLB */
  zg2 = zeta(z, zlb);

  /* Bates temperature */
  tt = tinf - (tinf - tlb) * exp(-s2*zg2);
  ta = tt;
  *tz = tt;
  densu_temp = *tz;

  if (alt<za) {
    /* calculate temperature below ZA
     * temperature gradient at ZA from Bates profile */
    dta = (tinf - ta) * s2 * pow(((re+zlb)/(re+za)),2.0);
    tgn1[0]=dta;
    tn1[0]=ta;
    if (alt>zn1[mn1-1])
      z=alt;
    else
      z=zn1[mn1-1];
    mn=mn1;
    z1=zn1[0];
    z2=zn1[mn-1];
    t1=tn1[0];
    t2=tn1[mn-1];
    /* geopotental difference from z1 */
    zg = zeta (z, z1);
    zgdif = zeta(z2, z1);
    /* set up spline nodes */
    for (k=0;k<mn;k++) {
      xs[k] = zeta(zn1[k], z1) / zgdif;
      ys[k] = 1.0 / tn1[k];
    }
    /* end node derivatives */
    yd1 = -tgn1[0] / (t1*t1) * zgdif;
    yd2 = -tgn1[1] / (t2*t2) * zgdif * pow(((re+z2)/(re+z1)),2.0);
    /* calculate spline coefficients */
    spline (xs, ys, mn, yd1, yd2, y2out);
    x = zg / zgdif;
    splint (xs, ys, y2out, mn, x, &y);
    /* temperature at altitude */
    *tz = 1.0 / y;
    densu_temp = *tz;
  }
  if (xm==0)
    return densu_temp;

  /* calculate density above za */
  glb = gsurf / pow((1.0 + zlb/re),2.0);
  gamma = xm * glb / (s2 * rgas * tinf);
  expl = exp(-s2 * gamma * zg2);
  if (expl>50.0)
      expl=50.0;
  if (tt<=0)
    expl=50.0;

  /* density at altitude */
  densa = dlb * pow((tlb/tt),((1.0+alpha+gamma))) * expl;
  densu_temp=densa;
  if (alt>=za)
    return densu_temp;

  /* calculate density below za */
  glb = gsurf / pow((1.0 + z1/re),2.0);
  gamm = xm * glb * zgdif / rgas;

  /* integrate spline temperatures */
  splini (xs, ys, y2out, mn, x, &yi);
  expl = gamm * yi;
  if (expl>50.0)
    expl=50.0;
  if (*tz<=0)
    expl=50.0;

  /* density at altitude */
  densu_temp = densu_temp * pow ((t1 / *tz),(1.0 + alpha)) * exp(-expl);
  return densu_temp;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

/*    3hr Magnetic activity functions */
/*    Eq. A24d */
double MSIS::g0(double a, double *p)
{
  return (a - 4.0 + (p[25] - 1.0) * (a - 4.0 + (exp(-sqrt(p[24]*p[24]) *
                (a - 4.0)) - 1.0) / sqrt(p[24]*p[24])));
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

/*    Eq. A24c */
double MSIS::sumex(double ex)
{
  return (1.0 + (1.0 - pow(ex,19.0)) / (1.0 - ex) * pow(ex,0.5));
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

/*    Eq. A24a */
double MSIS::sg0(double ex, double *p, double *ap)
{
  return (g0(ap[1],p) + (g0(ap[2],p)*ex + g0(ap[3],p)*ex*ex +
                g0(ap[4],p)*pow(ex,3.0)  + (g0(ap[5],p)*pow(ex,4.0) +
                g0(ap[6],p)*pow(ex,12.0))*(1.0-pow(ex,8.0))/(1.0-ex)))/sumex(ex);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double MSIS::globe7(double *p, struct nrlmsise_input *input,
                      struct nrlmsise_flags *flags)
{
/*       CALCULATE G(L) FUNCTION
 *       Upper Thermosphere Parameters */
  double t[15];
  int i,j;
  int sw9=1;
  double apd;
  double xlong;
  double tloc;
  double c, s, c2, c4, s2;
  double sr = 7.2722E-5;
  double dgtr = 1.74533E-2;
  double dr = 1.72142E-2;
  double hr = 0.2618;
  double cd32, cd18, cd14, cd39;
  double p32, p18, p14, p39;
  double df, dfa;
  double f1, f2;
  double tinf;
  struct ap_array *ap;

  tloc=input->lst;
  for (j=0;j<14;j++)
    t[j]=0;
  if (flags->sw[9]>0)
    sw9=1;
  else if (flags->sw[9]<0)
    sw9=-1;
  xlong = input->g_long;

  /* calculate legendre polynomials */
  c = sin(input->g_lat * dgtr);
  s = cos(input->g_lat * dgtr);
  c2 = c*c;
  c4 = c2*c2;
  s2 = s*s;

  plg[0][1] = c;
  plg[0][2] = 0.5*(3.0*c2 -1.0);
  plg[0][3] = 0.5*(5.0*c*c2-3.0*c);
  plg[0][4] = (35.0*c4 - 30.0*c2 + 3.0)/8.0;
  plg[0][5] = (63.0*c2*c2*c - 70.0*c2*c + 15.0*c)/8.0;
  plg[0][6] = (11.0*c*plg[0][5] - 5.0*plg[0][4])/6.0;
/*      plg[0][7] = (13.0*c*plg[0][6] - 6.0*plg[0][5])/7.0; */
  plg[1][1] = s;
  plg[1][2] = 3.0*c*s;
  plg[1][3] = 1.5*(5.0*c2-1.0)*s;
  plg[1][4] = 2.5*(7.0*c2*c-3.0*c)*s;
  plg[1][5] = 1.875*(21.0*c4 - 14.0*c2 +1.0)*s;
  plg[1][6] = (11.0*c*plg[1][5]-6.0*plg[1][4])/5.0;
/*      plg[1][7] = (13.0*c*plg[1][6]-7.0*plg[1][5])/6.0; */
/*      plg[1][8] = (15.0*c*plg[1][7]-8.0*plg[1][6])/7.0; */
  plg[2][2] = 3.0*s2;
  plg[2][3] = 15.0*s2*c;
  plg[2][4] = 7.5*(7.0*c2 -1.0)*s2;
  plg[2][5] = 3.0*c*plg[2][4]-2.0*plg[2][3];
  plg[2][6] =(11.0*c*plg[2][5]-7.0*plg[2][4])/4.0;
  plg[2][7] =(13.0*c*plg[2][6]-8.0*plg[2][5])/5.0;
  plg[3][3] = 15.0*s2*s;
  plg[3][4] = 105.0*s2*s*c;
  plg[3][5] =(9.0*c*plg[3][4]-7.*plg[3][3])/2.0;
  plg[3][6] =(11.0*c*plg[3][5]-8.*plg[3][4])/3.0;

  if (!(((flags->sw[7]==0)&&(flags->sw[8]==0))&&(flags->sw[14]==0))) {
    stloc = sin(hr*tloc);
    ctloc = cos(hr*tloc);
    s2tloc = sin(2.0*hr*tloc);
    c2tloc = cos(2.0*hr*tloc);
    s3tloc = sin(3.0*hr*tloc);
    c3tloc = cos(3.0*hr*tloc);
  }

  cd32 = cos(dr*(input->doy-p[31]));
  cd18 = cos(2.0*dr*(input->doy-p[17]));
  cd14 = cos(dr*(input->doy-p[13]));
  cd39 = cos(2.0*dr*(input->doy-p[38]));
  p32=p[31];
  p18=p[17];
  p14=p[13];
  p39=p[38];

  /* F10.7 EFFECT */
  df = input->f107 - input->f107A;
  dfa = input->f107A - 150.0;
  t[0] =  p[19]*df*(1.0+p[59]*dfa) + p[20]*df*df + p[21]*dfa + p[29]*pow(dfa,2.0);
  f1 = 1.0 + (p[47]*dfa +p[19]*df+p[20]*df*df)*flags->swc[1];
  f2 = 1.0 + (p[49]*dfa+p[19]*df+p[20]*df*df)*flags->swc[1];

  /*  TIME INDEPENDENT */
  t[1] = (p[1]*plg[0][2]+ p[2]*plg[0][4]+p[22]*plg[0][6]) +
        (p[14]*plg[0][2])*dfa*flags->swc[1] +p[26]*plg[0][1];

  /*  SYMMETRICAL ANNUAL */
  t[2] = p[18]*cd32;

  /*  SYMMETRICAL SEMIANNUAL */
  t[3] = (p[15]+p[16]*plg[0][2])*cd18;

  /*  ASYMMETRICAL ANNUAL */
  t[4] =  f1*(p[9]*plg[0][1]+p[10]*plg[0][3])*cd14;

  /*  ASYMMETRICAL SEMIANNUAL */
  t[5] =    p[37]*plg[0][1]*cd39;

        /* DIURNAL */