rendered paste body#include <stdio.h>
#include <math.h>
#include "PitchBlkOut.h"
#define N_Max 100
void Search(int stages, double Weight[], double Tol[], double Pert[], double Xfin[]);
void OneRun(int stages, double Weight[], double X[]);
void RK4_System(int n, double h, double t, double X[], int nsteps);
void XP_System(int Neq, double T, double X[], double DY[]);
void output(int j, double T, double Y[]);
double Celerity14(double temp);
double Gravity14(double alt);
double Pressure14(double altitude);
double Temperature14(double alt);
void Gauss(int n, double a[][3], double x[], double b[]);
double do_lagrange(double pairs[][2], double x, int k, int num_pairs);
int print_control, n_stage;
double Vrt, Pang, Pend, Az, time, Pressure, Temperature, Mach, Celerity, Q, Fa, start_lat, start_long, start_ht, velocity, g;
double Thrust[5], FlowRate[5], XSect[5], CD[5], COT[5], TI[5], F[3], G[3], C[3];
int i, j, k;
double F1S, F1P1, F1P2, F1P3, F2S, F2P1, F2P2, F2P3, F3S, F3P1, F3P2, F3P3;
double I[3], B[3], X[7], Guess[3];
double A[3][3];
void Search(int stages, double Weight[], double Tol[], double Pert[], double Xfin[])
{
double temp_P_control=print_control;
print_control=0;
Guess[0] = Az;
Guess[1] = Pang;
Guess[2] = Thrust[0];
for (i = 0; i < 8; i++)
{
printf("\nIteration %d, Run 1\n", i);
for (j = 0; j < 6; j++)
{
X[j] = 0.0;
}
X[2] = start_ht;
Az = Guess[0];
Pang = Guess[1];
Thrust[0] = Guess[2];
printf("\nAZ: %lf PANG: %lf Thrust: %lf\n", Az, Pang, Thrust[0]);
OneRun(stages, Weight, X);
if (fabs(X[0] - Xfin[0]) < Tol[0] && fabs(X[1] - Xfin[1]) < Tol[1] && fabs(X[2] - Xfin[2]) < Tol[2])
{
break;
}
F1S = X[0];
F2S = X[1];
F3S = X[2];
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
for (j = 0; j < 6; j++)
{
X[j] = 0.0;
}
X[2] = start_ht;
Az += (Pert[0] * Az);
printf("\nIteration %d, Run 2\n", i);
OneRun(stages, Weight, X);
F1P1 = X[0];
F2P1 = X[1];
F3P1 = X[2];
//////////////////////////////////////////////////////////////////////////////////////////////////
for (j = 0; j < 6; j++)
{
X[j] = 0.0;
}
X[2] = start_ht;
Az = Guess[0];
Pang += (Pert[1] * Pang);
printf("\nIteration %d, Run 3\n", i);
OneRun(stages, Weight, X);
F1P2 = X[0];
F2P2 = X[1];
F3P2 = X[2];
////////////////////////////////////////////////////////////////////////////////////////////////////////////////
for (j = 0; j < 6; j++)
{
X[j] = 0.0;
}
X[2] = start_ht;
Pang = Guess[1];
Thrust[0] += (Pert[2] * Thrust[0]);
printf("\nIteration %d, Run 4\n", i);
OneRun(stages, Weight, X);
F1P3 = X[0];
F2P3 = X[1];
F3P3 = X[2];
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
A[0][0] = (F1P1 - F1S) / (Pert[0] * Guess[0]);
A[0][1] = (F1P2 - F1S) / (Pert[1] * Guess[1]);
A[0][2] = (F1P3 - F1S) / (Pert[2] * Guess[2]);
A[1][0] = (F2P1 - F2S) / (Pert[0] * Guess[0]);
A[1][1] = (F2P2 - F2S) / (Pert[1] * Guess[1]);
A[1][2] = (F2P3 - F2S) / (Pert[2] * Guess[2]);
A[2][0] = (F3P1 - F3S) / (Pert[0] * Guess[0]);
A[2][1] = (F3P2 - F3S) / (Pert[1] * Guess[1]);
A[2][2] = (F3P3 - F3S) / (Pert[2] * Guess[2]);
B[0] = Xfin[0] - F1S;
B[1] = Xfin[1] - F2S;
B[2] = Xfin[2] - F3S;
Gauss(3, A, I, B);
for (j = 0; j < 3; j++)
{
Guess[j] += I[j];
}
}
print_control=temp_P_control;
for (j = 0; j < 6; j++)
{
X[j] = 0.0;
}
X[2] = start_ht;
Az = Guess[0];
Pang = Guess[1];
Thrust[0] = Guess[2];
OneRun(stages, Weight, X);
printf("\nFinal Check: ");
printf("\nX = %lf, Y = %lf, Z = %lf\n", X[0], X[1], X[2]);
}
void output(int j, double T, double Y[])
{
int i;
double met_long, met_lat;
met_lat = start_lat * 1852.0 * 60.0;
met_long = start_long * 1852.0 * 60.0;
if(print_control)
{
BLKOUT(1, print_control, time, Y, C, F, G, Pang, Vrt, Pend, Az, -FlowRate[n_stage], XSect[n_stage], Thrust[n_stage], g, Pressure,
Temperature, Mach, Celerity, Q, CD[n_stage], Fa, (met_lat + Y[0]) / (1852.0 * 60.0),
(met_long + Y[1]) / (1852.0 * 60.0),
sqrt(pow(Y[0], 2.0) + pow(Y[1], 2.0)) / 1852.0, velocity, (Thrust[n_stage] - Fa) / (9.80665 * Y[6]),
COT[n_stage], TI[n_stage]);
}
}
void OneRun(int stages, double Weight[], double X[])
{
int nsteps, i;
time = 0.0;
for (i = 0; i < stages; i++)
{
nsteps = (int) COT[i] / TI[i];
n_stage = i;
X[6] = Weight[i];
RK4_System(7, TI[i], time, X, nsteps);
}
}
void RK4_System(int n, double h, double t, double X[], int nsteps)
{
double Y[n];
double K[4][n];
int i, j;
output(0, 0.0, X);
for (j = 0; j < nsteps; j++)
{
XP_System(n, t, X, K[0]);
for (i = 0; i < n; i++)
{
Y[i] = X[i] + 0.5 * h * K[0][i];
}
XP_System(n, t + h/2, Y, K[1]);
for (i = 0; i < n; i++)
{
Y[i] = X[i] + 0.5 * h * K[1][i];
}
XP_System(n, t + h/2, Y, K[2]);
for (i = 0; i < n; i++)
{
Y[i] = X[i] + h * K[2][i];
}
XP_System(n, t + h, Y, K[3]);
for (i = 0; i < n; i++)
{
X[i] += (1.0/6.0) * h * (K[0][i] + (2 * K[1][i]) + (2 * K[2][i]) + K[3][i]);
}
t += h;
time += h;
output(j, t, X);
}
}
void XP_System(int Neq, double T, double Y[], double DY[])
{
int i;
double alt;
double Fg,Ft,Fa;
alt=Y[2];
velocity = sqrt(pow(Y[3], 2.0) + pow(Y[4], 2.0) + pow(Y[5], 2.0));
if (velocity != 0.0)
{
C[0] = Y[3] / velocity;
C[1] = Y[4] / velocity;
C[2] = Y[5] / velocity;
}
else
{
C[0] = 0.0;
C[1] = 0.0;
C[2] = 0.0;
}
PITCH(T, Az, Pang, Vrt, Pend, F, C);
Pressure = Pressure14(X[2]);
Temperature = Temperature14(X[2]);
Celerity = Celerity14(Temperature);
Mach = velocity / Celerity;
Q = 0.7 * Pressure * pow(Mach, 2.0);
Fa = CD[n_stage] * Q * XSect[n_stage];
g = Gravity14(X[2]);
DY[0] = Y[3];
DY[1] = Y[4];
DY[2] = Y[5];
DY[3] = ((Thrust[n_stage] * F[0]) - (Fa * C[0])) / Y[6];
DY[4] = ((Thrust[n_stage] * F[1]) - (Fa * C[1])) / Y[6];
DY[5] = (((Thrust[n_stage] * F[2]) - (Fa * C[2])) / Y[6]) - g;
DY[6] = -FlowRate[n_stage];
}
double do_lagrange(double pairs[][2], double x, int k, int num_pairs)
{
int i,j,x_begin,x_end,x_middle;
int x_half, k_count;
double partial,result;
partial = result = 0.0;
x_half = k / 2;
k_count = k;
for(i = 0; i < num_pairs; i++)
if(pairs[i][0] >= x) break;
x_middle = i;
if(x_middle < 0) x_middle = 0;
x_begin = x_end = 0;
x_begin = x_middle - x_half;
x_end = x_middle + x_half;
if(x_begin < 0) {
x_begin = 0;
k_count -= (x_end - x_middle);
k_count -= (x_middle - x_begin);
x_end+=k_count;
}
else if(x_end >= num_pairs) {
x_end = num_pairs - 1;
k_count -= (x_end - x_middle);
k_count -= (x_middle - x_begin);
x_end+=k_count;
} else {
k_count -= (x_end - x_middle);
k_count -= (x_middle - x_begin);
x_end+=k_count;
if(x_end >= num_pairs)
{
x_end -= k_count;
x_begin -= k_count;
}
}
result = 0.0;
for(i = x_begin; i < x_end; i++) {
partial = pairs[i][1];
for(j = x_begin; j < x_end; j++) {
if(i != j) {
partial *= (x - pairs[j][0]) / (pairs[i][0] - pairs[j][0]);
}
}
result += partial;
}
return result;
}
double Pressure14(double altitude)
{
double x, evaluate;
double A[N_Max];
int i,j;
x = altitude / 1000.0;
if(x > 65.0)
{
return 0.0;
}
else if(x < 25.0)
{
A [ 0 ] = -9.666667e-04;
A [ 1 ] = 7.833333e-02;
A [ 2 ] = -2.379167e+00;
A [ 3 ] = 3.429167e+01;
A [ 4 ] = -2.511667e+02;
A [ 5 ] = 1.000000e+03;
evaluate=0.0;
for(i=5; i>=0; i--)
{
evaluate+=A[i] * pow(x, (double)(5-i));
}
return evaluate * 1000.0 * 0.010197162;
}
else if(x > 0.0)
{
A [ 0 ] = -2.083333e-06;
A [ 1 ] = 5.208333e-04;
A [ 2 ] = -5.104167e-02;
A [ 3 ] = 2.472917e+00;
A [ 4 ] = -6.112500e+01;
A [ 5 ] = 6.650000e+02;
evaluate = 0.0;
for(j=5; j>=0; j--)
{
evaluate+=A[j] * pow(x, (double)(5-j));
}
return evaluate * 1000.0 * 0.010197162;
}
}
double Temperature14(double alt)
{
double pairs[15][2];
double result;
alt /= 1000.0;
pairs[0][0] = 0.0; pairs[0][1] = 290.0;
pairs[1][0] = 5.0; pairs[1][1] = 240.0;
pairs[2][0] = 10.0; pairs[2][1] = 220.0;
pairs[3][0] = 15.0; pairs[3][1] = 225.0;
pairs[4][0] = 20.0; pairs[4][1] = 230.0;
pairs[5][0] = 25.0; pairs[5][1] = 245.0;
pairs[6][0] = 30.0; pairs[6][1] = 255.0;
pairs[7][0] = 35.0; pairs[7][1] = 265.0;
pairs[8][0] = 40.0; pairs[8][1] = 270.0;
pairs[9][0] = 45.0; pairs[9][1] = 275.0;
pairs[10][0] = 50.0; pairs[10][1] = 270.0;
pairs[11][0] = 55.0; pairs[11][1] = 255.0;
pairs[12][0] = 60.0; pairs[12][1] = 235.0;
pairs[13][0] = 65.0; pairs[13][1] = 215.0;
pairs[14][0] = 70.0; pairs[14][1] = 200.0;
if(alt > 65.0) result = 180.0;
else result = do_lagrange(pairs,alt,4,15);
return result;
}
double Celerity14(double temp)
{
double result;
result = pow((1.4 * (8314.32 / 28.9644)) * temp,0.5);
return (temp == 180.0 ? 1000.0 : result);
}
double Gravity14(double alt)
{
double GM = 3.98622 * pow(10.0,14.0);
double r0 = 6.378178 * pow(10.0,6.0);
return GM/pow(r0 + alt,2.0);
}
void Gauss(int n, double a[][3], double x[], double b[])
{
int L[10];
double s[10];
int i, j, k, temp;
double r, rmax, smax, xmult, sum;
for (i = 0; i < n; i++)
{
L[i] = i;
smax = 0.0;
for (j = 0; j < n; j++)
{
if (fabs(a[i][j]) > smax)
smax = fabs(a[i][j]);
}
s[i] = smax;
}
for (k = 0; k < n - 1; k++)
{
rmax = 0.0;
for (i = k; i < n; i++)
{
r = fabs(a[L[i]][k] / s[L[i]]);
if (r > rmax)
{
rmax = r;
j = i;
}
}
temp = L[j];
L[j] = L[k];
L[k] = temp;
for (i = k + 1; i < n; i++)
{
xmult = a[L[i]][k] / a[L[k]][k];
a[L[i]][k] = xmult;
for (j = k + 1; j < n; j++)
a[L[i]][j] = a[L[i]][j] - (xmult * a[L[k]][j]);
}
}
for (k = 0; k < n - 1; k++)
{
for (i = k + 1; i < n; i++)
b[L[i]] = b[L[i]] - (a[L[i]][k] * b[L[k]]);
}
x[n - 1] = b[L[n - 1]] / a[L[n - 1]][n - 1];
for(i = n - 2; i >= 0; i--)
{
sum = b[L[i]];
for (j = i + 1; j < n; j++)
sum = sum - (a[L[i]][j] * x[j]);
x[i] = sum / a[L[i]][i];
}
}
int main()
{
int closed, i, stages;
double Weight[5], Pert[3], Tol[3], Xfin[3];
printf("Kwang-kook, Kim(Shine) NN:14 Numerical Analysis for CS407\n");
scanf("%d %d %lf %lf %lf", &stages, &print_control, &start_lat, &start_long, &start_ht);
for (i = 0; i < stages; i++)
scanf("%lf %lf %lf %lf %lf %lf %lf", &Thrust[i], &Weight[i], &FlowRate[i], &XSect[i], &CD[i], &COT[i],
&TI[i]);
scanf("%lf %lf %lf %lf", &Vrt, &Pang, &Pend, &Az);
for (i = 0; i < 3; i++)
scanf("%lf %lf %lf", &Xfin[i], &Pert[i], &Tol[i]);
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Echo Input of Data
printf("\nControl values:\n");
printf(" Number of stages:%d\n",stages);
printf(" Print control: %d\n",print_control);
printf("Launch Point\n");
printf(" Latitude:%lf\n",start_lat);
printf(" Longitude:%lf\n",start_long);
printf(" Height:%lf\n",start_ht);
for(i = 0; i < stages; i++)
{
printf("\nThe vehicle:\n",i);
printf(" Thrust:%lf\n",Thrust[i]);
printf(" Weight:%lf\n",Weight[i]);
printf(" Flow:%lf\n",FlowRate[i]);
printf(" Cross-sectional area:%lf\n",XSect[i]);
printf(" Drag coefficient:%lf\n",CD[i]);
printf(" Cut-off time:%lf\n",COT[i]);
printf(" Step size:%lf\n",TI[i]);
}
printf("\nPitch program:\n");
printf(" Vertical rise time:%lf\n",Vrt);
printf(" Pitch angle:%lf\n",Pang);
printf(" Pitch Program end time:%lf\n",Pend);
printf(" Launch azimuth:%lf\n",Az);
for(i = 0; i < stages; i++)
{
printf("\n");
printf(" Desired value:%lf\n",Xfin[i]);
printf(" Perturbation:%lf\n",Pert[i]);
printf(" tolerance:%lf\n",Tol[i]);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
for (i = 0; i < stages; i++)
{
Thrust[i] *= 0.4535924;
Weight[i] *= 0.04625455;
FlowRate[i] *= 0.04625455;
XSect[i] *= pow(0.3048, 2.0);
}
G[0] = 0.0;
G[1] = 0.0;
G[2] = -1.0;
Search(stages, Weight, Tol, Pert, Xfin);
printf("\nAzimuth = %lf, Pitch angle = %lf, Thrust = %lf\n", Az, Pang, Thrust[0]);
return 0;
}