#include "StdAfx.h"
#include "Traj_wreckage.h"
#include <iostream>
#include <string>
#include "ADGfunction.h"
using namespace std;
Traj_wreckage::Traj_wreckage(void)
{
}
Traj_wreckage::~Traj_wreckage(void)
{
}
void Traj_wreckage::trajectory_WreckageS1(double state[8])
{
// 将传递的状态数组复制到类成员x0
dh = dhconst; //将全局常量dhconst的值赋给dh(龙格库塔积分的步长)
for (int i = 0; i < 7; i++) {
x0[i] = state[i];
}
phase = state[7];
flag = 0;
while (!flag) {
w_fnRK();
if (Height < 10)
flag = 1;
//cout << Height << endl;
}
//fpCombined = fopen(".\\OUTPUT\\impactpoint.csv", "w");
//fprintf(fpCombined, "%10.12f,%10.12f\n", Lamda*RTD, BWei*RTD);
cout << Lamda*RTD << " "<< BWei * RTD << endl;
}
void Traj_wreckage::trajectory_Wreckagefairing(double state[8])
{
dh = dhconst; //将全局常量dhconst的值赋给dh(龙格库塔积分的步长)
for (int i = 0; i < 7; i++) {
x0[i] = state[i];
}
phase = state[7];
flag = 0;
while (!flag) {
w_fnRK();
if (Height < 10)
flag = 1;
//cout << Height << endl;
}
//fprintf(fpCombined, "%10.12f,%10.12f\n", Lamda*RTD, BWei*RTD);
cout << Lamda * RTD << " " << BWei * RTD << endl;
cin.get();
}
//**************************************************************************
// 函数名称: m_fnRK
// 函数说明: 龙门库塔积分函数
//
// 输入参数:
//
// 输出参数:
//
void Traj_wreckage::w_fnRK()
{
double K1[NumZD], K2[NumZD], K3[NumZD], K4[NumZD];
int iRK;
//========第一阶======//
w_fnDynamic();
for (iRK = 0; iRK < NumZD; iRK++)
{
K1[iRK] = dx[iRK];
x0[iRK] = x0[iRK] + dh * K1[iRK] / 2;
}
//========第二阶======//
w_fnDynamic();
for (iRK = 0; iRK < NumZD; iRK++)
{
x0[iRK] = x0[iRK] - dh * K1[iRK] / 2;
K2[iRK] = dx[iRK];
x0[iRK] = x0[iRK] + dh * K2[iRK] / 2;
}
//========第三阶======//
w_fnDynamic();
for (iRK = 0; iRK < NumZD; iRK++)
{
x0[iRK] = x0[iRK] - dh * K2[iRK] / 2;
K3[iRK] = dx[iRK];
x0[iRK] = x0[iRK] + dh * K3[iRK];
}
//========第四阶======//
w_fnDynamic();
for (iRK = 0; iRK < NumZD; iRK++)
{
K4[iRK] = dx[iRK];
x0[iRK] = x0[iRK] - dh * K3[iRK];
}
//获得最新的状态量
for (iRK = 0; iRK < NumZD; iRK++)
{
x0[iRK] = x0[iRK] + dh * (K1[iRK] + 2 * K2[iRK] + 2 * K3[iRK] + K4[iRK]) / 6.0;
}
w_fnDynamic();
}
//**************************************************************************
// 函数名称: m_fnDynamic
// 函数说明: 在发射坐标系建立火箭的动力学模型
//
// 输入参数:
//
// 输出参数:
void Traj_wreckage::w_fnDynamic()
{
//*******************************************************//
t = x0[0];
x = x0[1];
y = x0[2];
z = x0[3];
vx = x0[4];
vy = x0[5];
vz = x0[6];
//质量//
if (phase == 1)
MassRoc = mass1 - mass2 - massfairing - dm1 * TimeP1;
else
MassRoc = 0.5*massfairing;
//发射场地心纬度//
Phi0 = atan(tan(B0)*pow(bEarth / aEarth, 2));
//发射场位置距离地心的距离//
double R0norm;
R0norm = aEarth * bEarth / pow(pow(aEarth*sin(Phi0), 2) + pow(bEarth*cos(Phi0), 2), 0.5);
//发射场地心矢径//
R0[0] = R0norm * (-sin(B0 - Phi0)*cos(A0));
R0[1] = R0norm * cos(B0 - Phi0);
R0[2] = R0norm * sin(B0 - Phi0)*sin(A0);
//地球自转角速度矢量//
W[0] = WEarth * cos(B0)*cos(A0);
W[1] = WEarth * sin(B0);
W[2] = WEarth * (-cos(B0)*sin(A0));
//火箭速度矢量//
double V[3];
V[0] = vx;
V[1] = vy;
V[2] = vz;
//火箭速度模//
Vnorm = p_mathpri.m_qiumo(V);
//火箭相对发射场的位置矢量//
double rou[3];
rou[0] = x;
rou[1] = y;
rou[2] = z;
//火箭相对地心的位置矢量//
double R[3];
R[0] = rou[0] + R0[0];
R[1] = rou[1] + R0[1];
R[2] = rou[2] + R0[2];
//火箭相对地心的位置矢量的模//
double Rnorm;
Rnorm = p_mathpri.m_qiumo(R);
//地心临时坐标系的速度和位置矢量//
double R1[3], V1[3];
R1[0] = cos(B0)*cos(A0)*R[0] + sin(B0)*R[1] - cos(B0)*sin(A0)*R[2];
R1[1] = -sin(B0)*cos(A0)*R[0] + cos(B0)*R[1] + sin(B0)*sin(A0)*R[2];
R1[2] = sin(A0)*R[0] + cos(A0)*R[2];
V1[0] = cos(B0)*cos(A0)*V[0] + sin(B0)*V[1] - cos(B0)*sin(A0)*V[2];
V1[1] = -sin(B0)*cos(A0)*V[0] + cos(B0)*V[1] + sin(B0)*sin(A0)*V[2];
V1[2] = sin(A0)*V[0] + cos(A0)*V[2];
//经度//
double DLamda;
if (R1[1] > 0) DLamda = atan(R1[2] / R1[1]);
else DLamda = pi + atan(R1[2] / R1[1]);
Lamda = Lamda0 + DLamda;
//地心纬度//
PhiWei = asin(p_mathpri.m_diancheng(R, W) / Rnorm / WEarth);
//地理纬度//
BWei = atan(tan(PhiWei)*aEarth*aEarth / bEarth / bEarth);
//射程//
Range = acos(p_mathpri.m_diancheng(R0, R) / R0norm / Rnorm)*6378.0;
//地球半径//
double REarthnorm;
REarthnorm = aEarth * bEarth / pow(pow(aEarth*sin(PhiWei), 2) + pow(bEarth*cos(PhiWei), 2), 0.5);
//高度//
Height = Rnorm - REarthnorm;
//大气模型//
w_atmosphere(Height);
//动压//
q = 0.5*midu*pow(Vnorm, 2);
//马赫数//
ma = Vnorm / A;
//北天东坐标系速度//
double V_NBE[3];
V_NBE[0] = cos(PhiWei)*V1[0] - sin(PhiWei)*cos(DLamda)*V1[1] - sin(PhiWei)*sin(DLamda)*V1[2];
V_NBE[1] = sin(PhiWei)*V1[0] + cos(PhiWei)*cos(DLamda)*V1[1] + cos(PhiWei)*sin(DLamda)*V1[2];
V_NBE[2] = -sin(DLamda)*V1[1] + cos(DLamda)*V1[2];
//当地弹道倾角//
thetaK = atan(V_NBE[1] / pow(V_NBE[0] * V_NBE[0] + V_NBE[2] * V_NBE[2], 0.5));
//地球固联坐标系的位置和速度//
double Rearthfix[3], Vearthfix[3];
p_mathpri.m_TransFaShe2Efix(R);
Rearthfix[0] = p_mathpri.Vec_Efix[0];
Rearthfix[1] = p_mathpri.Vec_Efix[1];
Rearthfix[2] = p_mathpri.Vec_Efix[2];
p_mathpri.m_TransFaShe2Efix(V);
Vearthfix[0] = p_mathpri.Vec_Efix[0];
Vearthfix[1] = p_mathpri.Vec_Efix[1];
Vearthfix[2] = p_mathpri.Vec_Efix[2];
//地球惯性坐标系的位置//
p_mathpri.m_TransEfix2Einertia(Rearthfix, x0[0]);
RearthI[0] = p_mathpri.Vec_Ei[0];
RearthI[1] = p_mathpri.Vec_Ei[1];
RearthI[2] = p_mathpri.Vec_Ei[2];
//地球惯性坐标系的速度//
double Wearth[3];
Wearth[0] = 0.0;
Wearth[1] = 0.0;
Wearth[2] = WEarth;
p_mathpri.m_chacheng(Wearth, Rearthfix);
VearthI[0] = Vearthfix[0] + p_mathpri.Resul_chacheng[0];
VearthI[1] = Vearthfix[1] + p_mathpri.Resul_chacheng[1];
VearthI[2] = Vearthfix[2] + p_mathpri.Resul_chacheng[2];
p_mathpri.m_TransEfix2Einertia(VearthI, x0[0]);
VearthI[0] = p_mathpri.Vec_Ei[0];
VearthI[1] = p_mathpri.Vec_Ei[1];
VearthI[2] = p_mathpri.Vec_Ei[2];
VEInorm = p_mathpri.m_qiumo(VearthI);
//轨道六根数//
p_mathpri.m_PV2COE(RearthI, VearthI);
//速度角//
if (vx > 0)
theta = atan(vy / vx);
else if (vx < 0)
theta = pi + atan(vy / vx);
else
{
if (vy >= 0)
theta = pi / 2;
else
theta = -pi / 2;
}
theta = fmod(theta, 2 * pi);
if (theta > pi) theta -= 2 * pi;
if (theta < -pi) theta += 2 * pi;
sigma = -asin(vz / Vnorm);
upsilon = 0.0;
//程序角//
//PhiT = m_fnProAngle(t, theta, W[2]);
//PsiT = 0.0;
//GammaT = 0.0;
//姿态角//
//phi = PhiT - W[2] * t;
//psi = PsiT - W[1] * t*cos(phi) + W[0] * t*sin(phi);
//gamma = GammaT - W[1] * t*sin(phi) - W[0] * t*cos(phi);
//攻角、侧滑角//
//beta = asin(cos(theta - phi)*cos(sigma)*sin(psi)*cos(gamma) - sin(theta - phi)*cos(sigma)*sin(gamma) - sin(sigma)*cos(psi)*cos(gamma));
//alpha = asin((cos(theta - phi)*cos(sigma)*sin(psi)*sin(gamma) + sin(theta - phi)*cos(sigma)*cos(gamma) - sin(sigma)*cos(psi)*sin(gamma)) / -cos(beta));
//补充风攻角和风侧滑角
alphaw = 0;
betaw = 0;
alpha = alphaw;
beta = betaw;
phi = alpha + theta;
psi = beta + sigma;
alphaHe = acos(cos(alpha)*cos(beta));
//空气动力//
if (ma < 6) {
Ca = p_mathpri.lookup2D(p_mathpri.m_jueduizhi(alphaHe)*RTD, ma, Num_Alp1, Num_Ma1, Ca1, Alp1, Ma1);
Cn = p_mathpri.lookup2D(p_mathpri.m_jueduizhi(alpha)*RTD, ma, Num_Alp1, Num_Ma1, Cn1, Alp1, Ma1)*p_mathpri.m_zhengfu(alpha);
Cz = 0.0;
}
else
{
Ca = 0;
Cn = 0;
Cz = 0;
}
//else
//{
// Ca = 0.0;
// Cn = 0.0;
// Cz = 0.0;
//}
A_AirDT[0] = -2*Ca * q*Sm / MassRoc;
A_AirDT[1] = Cn * q*Sm / MassRoc;
A_AirDT[2] = Cz * q*Sm / MassRoc;
p_mathpri.m_TransJianti2FaShe(A_AirDT, phi, psi, gamma);
A_AirFS[0] = p_mathpri.Vec_Fashe[0];
A_AirFS[1] = p_mathpri.Vec_Fashe[1];
A_AirFS[2] = p_mathpri.Vec_Fashe[2];
//发动机推力//
//if (phase == 1)
// F_Engine[0] = F1 + Se1 * (1 - P) * 101325;
//else if (phase == 2 || phase == 3)
// F_Engine[0] = F2;
//else if (phase == 11 || phase == 12)
// F_Engine[0] = 0.0;
//F_Engine[1] = 0.0;
//F_Engine[2] = 0.0;
//A_EngDT[0] = F_Engine[0] / MassRoc;
//A_EngDT[1] = F_Engine[1] / MassRoc;
//A_EngDT[2] = F_Engine[2] / MassRoc;
//p_mathpri.m_TransJianti2FaShe(A_EngDT, phi, psi, gamma);
//A_EngFS[0] = p_mathpri.Vec_Fashe[0];
//A_EngFS[1] = p_mathpri.Vec_Fashe[1];
//A_EngFS[2] = p_mathpri.Vec_Fashe[2];
//视加速度
//dW22[0] = (A_AirDT[0] + A_EngDT[0]);
//dW22[1] = (A_AirDT[1] + A_EngDT[1]);
//dW22[2] = (A_AirDT[2] + A_EngDT[2]);
dW22[0] = (A_AirDT[0]);
dW22[1] = (A_AirDT[1]);
dW22[2] = (A_AirDT[2]);
//重力加速度//
g_r = -(MuEarth / pow(Rnorm, 2))*(1 + J2 * pow(aEarth / Rnorm, 2)*(1 - 5 * pow(sin(PhiWei), 2)));
g_we = -2 * (MuEarth / pow(Rnorm, 2))*J2*pow(aEarth / Rnorm, 2)*sin(PhiWei);
g[0] = g_r / Rnorm * R[0] + g_we / WEarth * W[0];
g[1] = g_r / Rnorm * R[1] + g_we / WEarth * W[1];
g[2] = g_r / Rnorm * R[2] + g_we / WEarth * W[2];
double gDT[3];//箭体坐标系
p_mathpri.m_TransFaShe2Jianti(g, phi, psi, gamma);
gDT[0] = p_mathpri.Vec_Jianti[0];
gDT[1] = p_mathpri.Vec_Jianti[1];
gDT[2] = p_mathpri.Vec_Jianti[2];
//离心惯性力加速度//
double Cross1[3];
p_mathpri.m_chacheng(W, R);
Cross1[0] = p_mathpri.Resul_chacheng[0];
Cross1[1] = p_mathpri.Resul_chacheng[1];
Cross1[2] = p_mathpri.Resul_chacheng[2];
p_mathpri.m_chacheng(W, Cross1);
A_li[0] = -p_mathpri.Resul_chacheng[0];
A_li[1] = -p_mathpri.Resul_chacheng[1];
A_li[2] = -p_mathpri.Resul_chacheng[2];
//科氏惯性力加速度//
p_mathpri.m_chacheng(W, V);
A_k[0] = -2 * p_mathpri.Resul_chacheng[0];
A_k[1] = -2 * p_mathpri.Resul_chacheng[1];
A_k[2] = -2 * p_mathpri.Resul_chacheng[2];
//控制力加速度//
double A_control[3];
A_control[0] = 0.0;
A_control[1] = 0.0;
A_control[2] = 0.0;
double A_DT[3];//箭体坐标系加速度
//A_DT[0] = A_AirDT[0] + A_EngDT[0] + gDT[0];
//A_DT[1] = A_AirDT[1] + A_EngDT[1] + gDT[1];
//A_DT[2] = A_AirDT[2] + A_EngDT[2] + gDT[2];
A_DT[0] = A_AirDT[0] + gDT[0];
A_DT[1] = A_AirDT[1] + gDT[1];
A_DT[2] = A_AirDT[2] + gDT[2];
//nx = (A_AirDT[0] + A_EngDT[0]) / 9.81;
//nz = sqrt(pow(A_EngDT[1] + A_AirDT[1], 2) + pow(A_EngDT[2] + A_AirDT[2], 2)) / 9.81;
nx = (A_AirDT[0]) / 9.81;
nz = sqrt(pow(A_AirDT[1], 2) + pow(A_AirDT[2], 2)) / 9.81;
dx[0] = 1;
dx[1] = vx;
dx[2] = vy;
dx[3] = vz;
//dx[4] = A_EngFS[0] + A_AirFS[0] + g[0] + A_li[0] + A_k[0] + A_control[0];
//dx[5] = A_EngFS[1] + A_AirFS[1] + g[1] + A_li[1] + A_k[1] + A_control[1];
//dx[6] = A_EngFS[2] + A_AirFS[2] + g[2] + A_li[2] + A_k[2] + A_control[2];
dx[4] = A_AirFS[0] + g[0] + A_li[0] + A_k[0] + A_control[0];
dx[5] = A_AirFS[1] + g[1] + A_li[1] + A_k[1] + A_control[1];
dx[6] = A_AirFS[2] + g[2] + A_li[2] + A_k[2] + A_control[2];
}
//**************************************************************************
// 函数名称: w_atmosphere
// 函数说明: 大气模型
//
// 输入参数:
// H 地势高度
//
// 输出参数:
// P 大气压强
// midu 大气密度
// A 声速
void Traj_wreckage::w_atmosphere(double H)
{
H = H / 1000;//地势高度
double Z;//几何高度
double W;
Z = H * 6356.766 / (6356.766 - H);
if (Z <= 11.0191&&Z >= 0)
{
W = 1 - H / 44.3308;
T = 288.15*W;
P = pow(W, 5.2559);
midu = pow(W, 4.2559);
}
else if (Z > 11.0191&&Z <= 20.0631)
{
W = exp((14.9647 - H) / 6.3416);
T = 216.650;
P = 0.11953*W;
midu = 0.15898*W;
}
else if (Z > 20.0631&&Z <= 32.1619)
{
W = 1 + (H - 24.9021) / 221.552;
T = 221.552*W;
P = 2.5158e-2*pow(W, -34.1629);
midu = 3.2722e-2*pow(W, -35.1629);
}
else if (Z > 32.1619&&Z <= 47.3501)
{
W = 1 + (H - 39.7499) / 89.4107;
T = 250.350*W;
P = 2.8338e-3*pow(W, -12.2011);
midu = 3.2618e-3*pow(W, -13.2011);
}
else if (Z > 47.3501&&Z <= 51.4125)
{
W = exp((48.6252 - H) / 7.9223);
T = 270.650;
P = 8.9155e-4*W;
midu = 9.4920e-4*W;
}
else if (Z > 51.4125&&Z <= 71.8020)
{
W = 1 - (H - 59.4390) / 88.2218;
T = 247.021*W;
P = 2.1671e-4*pow(W, 12.2011);
midu = 2.5280e-4*pow(W, 11.2011);
}
else if (Z > 71.8020&&Z < 86.0)
{
W = 1 - (H - 78.0303) / 100.2950;
T = 200.590*W;
P = 1.2274e-5*pow(W, 17.0816);
midu = 1.7632e-5*pow(W, 16.0816);
}
else if (Z >= 86.0 && Z <= 91.0)
{
W = exp((87.2848 - H) / 5.47);
T = 186.87;
P = (2.2730 + 1.042e-3*H)*0.000001*W;
midu = 3.6411e-6*W;
}
else
{
P = 0;
midu = 0;
T = 186.8700;
}
midu = midu * 1.225;
A = 20.0468*pow(T, 0.5);
}
//**************************************************************************
// 函数名称: w_fnLoadData
// 函数说明: 载入仿真需要的参数
//
// 输入参数:
//
// 输出参数: 气动参数
void Traj_wreckage::w_fnLoadData()
{
int i, j;
//气动参数
//Ca1基本量
FILE *fpCa1;
if ((fpCa1 = fopen(".\\INPUT\\Ca.txt", "r")) == NULL)
{
printf("error!");
}
Num_Alp1 = 4;
Num_Ma1 = 11;
for (i = 0; i < Num_Ma1; i++)
{
fscanf(fpCa1, "%lf", &Ma1[i]);
}
for (i = 0; i < Num_Alp1; i++)
{
fscanf(fpCa1, "%lf", &Alp1[i]);
for (j = 0; j < Num_Ma1; j++)
{
fscanf(fpCa1, "%lf", &Ca1[j + i * Num_Ma1]);
}
}
fclose(fpCa1);
//Cn1 基本量
FILE *fpCn1;
fpCn1 = fopen(".\\INPUT\\Cn.txt", "r");
for (i = 0; i < Num_Ma1; i++)
{
fscanf(fpCn1, "%lf", &Ma1[i]);
}
for (i = 0; i < Num_Alp1; i++)
{
fscanf(fpCn1, "%lf", &Alp1[i]);
for (j = 0; j < Num_Ma1; j++)
{
fscanf(fpCn1, "%lf", &Cn1[j + i * Num_Ma1]);
}
}
fclose(fpCn1);
} 帮我把他转写成一个matlab 程序
最新发布