public static void FillArrow(Graphics g, int x0, int y0, int x1, int y1, double h, double w)
{
Segment3D seg = new Segment3D(x0, y0, 0.0D, x1, y1, 0.0D);
double len = seg.SegLength();
double x = seg.p1.x - seg.p0.x;
double y = seg.p1.y - seg.p0.y;
double angle;
Vector3D p2;
if (len > 0.0D)
{
angle = -System.Math.Atan2(x, y);
p2 = seg.LinearIntarp((len - h) / len);
}
else
{
angle = 0.0D;
p2 = new Vector3D(seg.p0);
p2.y += h;
}
//g.DrawLine((int)(seg.p0.x + 0.5D), (int)(seg.p0.y + 0.5D), (int)(p2.x + 0.5D), (int)(p2.y + 0.5D));
smat.SetSMat(w, h, 0.0D);
rmat.SetRzMat(angle);
tmat.SetTMat(p2.x, p2.y, p2.z);
Matrix44 mat = smat.MultMat(rmat).MultMat(tmat);
Polygon3D arw = triangle.Transform(mat);
//g.FillPolygon(new SolidBrush(arw.IxArray(), arw.IyArray(), 3);
}
internal void Calc_dynamics(int lr, AirPlane ap, double dt)
{
n_fv.SetVec(0.0D, 0.0D, 0.0D);
n_tv.SetVec(0.0D, 0.0D, 0.0D);
f_fv.SetVec(0.0D, 0.0D, 0.0D);
f_tv.SetVec(0.0D, 0.0D, 0.0D);
a_fv.SetVec(0.0D, 0.0D, 0.0D);
a_tv.SetVec(0.0D, 0.0D, 0.0D);
if (flag == 0)
{
return;
}
p0g.SetVec(AirPlane.Get_point(p0_base, lr).Sub(ap.inp.cg));
p0w.SetVec(AirPlane.Get_point(p0_base, lr).R2l().MultMat(ap.pMotion.owm).L2r());
vd = Dynamics.VWithRot(ap.pMotion.vc, ap.pMotion.omega, p0g);
if ((p0w.z > 0.0D) && (delta == 1.0D))
{
flag_land = 1;
}
else
{
flag_land = 0;
}
stroke_b = stroke;
pw.SetVec(p0w.x, p0w.y, 0.0D);
if (flag_land == 1)
{
pg = pw.R2l().MultMat(ap.pMotion.wom).L2r();
stroke = p0w.z;
}
else
{
pg.SetVec(p0g);
stroke = 0.0D;
}
d_stroke = ((stroke - stroke_b) / dt);
if (delta == 1.0D)
{
if (flag_land == 1)
{
fz = (-stroke * k_sus - d_stroke * c_sus);
if (fz > 0.0D)
{
fz = 0.0D;
}
}
else
{
fz = 0.0D;
}
n_fv = ap.pMotion.dg.SclProd(fz);
n_tv = Dynamics.Torque(pg, n_fv);
Matrix44 otm = new Matrix44();
Matrix44 tom = new Matrix44();
Matrix44 rxmat = new Matrix44();
Matrix44 rymat = new Matrix44();
rymat.SetRyMat(ap.pMotion.pitch.GetValue());
rxmat.SetRyMat(ap.pMotion.roll.GetValue());
tom = rxmat.MultMat(rymat);
rymat.SetRyMat(-ap.pMotion.pitch.GetValue());
rxmat.SetRyMat(-ap.pMotion.roll.GetValue());
otm = rymat.MultMat(rxmat);
tire.Update(vd.MultMat(otm), -fz, 1.0D, brake);
if (flag_land == 1)
{
f_fv.SetVec(tire.fx, tire.fy, 0.0D);
f_fv = f_fv.MultMat(tom);
f_tv = f_tv.Add(Dynamics.Torque(pg, f_fv));
}
}
pa.SetVec(AirPlane.Get_point(sa.LinearIntarp(delta), lr).Sub(ap.inp.cg));
vd = Dynamics.VWithRot(ap.pMotion.vc, ap.pMotion.omega, pa);
vpa = vd.Length();
q = (0.5D * ap.atmos.rho * vpa * vpa);
if (delta != 0.0D)
{
d = (q * cd_s);
a_fv = vd.NmlVec().SclProd(-d);
a_tv = a_tv.Add(Dynamics.Torque(pa, a_fv));
}
}