internal void Calc_dynamics(int lr, ControlPlane cw, AirPlane ap, Vector3D dv, double k_q, double k_S)
{
Matrix44 ma = new Matrix44();
Matrix44 mtemp = new Matrix44();
Vector3D vtemp = new Vector3D();
fv.SetVec(0.0D, 0.0D, 0.0D);
tv.SetVec(0.0D, 0.0D, 0.0D);
ac = AirPlane.Get_point(ac_base, lr).Sub(ap.inp.cg);
dvv = ap.pMotion.omega.CrsProd(ac);
vd = ap.pMotion.vc.Add(dvv);
vd = vd.Add(dv);
v = vd.Length();
vd = vd.NmlVec().SclProd(v * k_q);
v = vd.Length();
s2_dash = (s2 * k_S);
m = (v / ap.atmos.a);
q = (0.5D * v * v * ap.atmos.rho);
Vector3D kdd = new Vector3D();
Vector3D idd = new Vector3D();
Vector3D jdd = new Vector3D();
if ((cw != null) && (cw.type == 2))
{
ma.SetRyMat(cw.delta);
mtemp.SetRzMat(cw.lamda_h);
ma = ma.MultMat(mtemp);
if (lr == 1)
{
mtemp.SetRxMat(-cw.gamma_h);
}
else
{
mtemp.SetRxMat(-cw.gamma_h);
}
ma = ma.MultMat(mtemp);
idd = id.MultMat(ma);
kdd = kd.MultMat(ma);
}
else
{
idd = new Vector3D(id);
jdd = new Vector3D(jd);
kdd = new Vector3D(kd);
}
mtemp.Set3ColVec(idd, jdd, kdd);
vdw = vd.MultMat(mtemp);
alpha = Jp.Maker1.Sim.Tools.Tool.CalcAlpha(vdw);
beta = Jp.Maker1.Sim.Tools.Tool.CalcBeta(vdw);
ar = wing.AspectRatio(beta);
are = (ar * wing.K_ground_effect(beta,
ap.pMotion.wpos.y));
lamda_dash = SweepbackAngle(wing.hv_arrangement, lr,
beta);
Calc_cla();
delta_cl_flap = 0.0D;
delta_cd_flap = 0.0D;
delta_cmac_flap = 0.0D;
delta_alpha = 0.0D;
k_a = 1.0D;
if ((cw != null) && ((cw.type == 1) || (cw.type == 1)))
{
cw.beta_f = Flap.Beta_f(lr, wing.hv_arrangement, cw.lamda_h, beta);
cw.delta_dash = Flap.Delta_dash(cw.delta, cw.beta_f);
if (Math.Abs(cw.beta_f) < 1.570796326794897D)
{
delta_alpha = (cw.f_lamda1 * cw.delta_dash);
delta_cmac_flap = cw.f_cmac;
}
else
{
delta_alpha = (cw.b_lamda1 * cw.delta_dash);
delta_cmac_flap = cw.b_cmac;
}
k_a = Flap.K_cla(cw.delta_dash, cw.cf_c);
delta_cl_flap = (k_a * cla * delta_alpha);
delta_cmac_flap *= (alpha + delta_alpha - alpha0);
}
vtemp.SetVec(0.0D, 0.0D, -1.0D);
ma.SetRyMat(-alpha);
mtemp.SetRzMat(beta);
ma = ma.MultMat(mtemp);
mtemp.Set3RowVec(idd, jdd, kdd);
ma = ma.MultMat(mtemp);
lu = vtemp.MultMat(ma);
du = vd.SclProd(-1.0D).NmlVec();
alpha += delta_alpha;
cla *= k_a;
Calc_cl_cd();
l = (q * cl * s2_dash);
cdi = (cl * cl / (Math.PI * are * ew));
cd += cdi;
cd += delta_cd_flap;
d = (q * cd * s2_dash);
lf = Dynamics.Force(l, lu);
lt = Dynamics.Torque(ac, lf);
df = Dynamics.Force(d, du);
dt = Dynamics.Torque(ac, df);
cmac = cmac0;
cmac += delta_cmac_flap;
mac = (q * s2_dash * cmac);
Vector3D mt_0 = new Vector3D(0.0D, mac, 0.0D);
fv = lf.Add(df);
tv = lt.Add(dt).Add(mt_0);
}
