Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- void guidanceSimulation()
- {
- vt1 = (-vt) * Math.Cos(beta);
- vt2 = vt * Math.Sin(beta*Math.PI/180);
- he = hedeg / 57.3;
- rtm1 = rt1 - rm1;
- rtm2 = rt2 - rm2;
- rtm = Math.Sqrt(rtm1 * rtm1 + rtm2 * rtm2);
- xlam = Math.Atan2(rtm2, rtm1);
- xlead = Math.Asin((vt * Math.Sin(beta*Math.PI/180 + xlam * Math.PI / 180)) / vm);
- thet = xlam + xlead;
- vm1 = vm * Math.Cos(thet * Math.PI / 180 + he * Math.PI / 180);
- vm2 = vm * Math.Sin(thet * Math.PI / 180 + he * Math.PI / 180);
- vtm1 = vt1 - vm1;
- vtm2 = vt2 - vm2;
- vc = -(rtm1 * vtm1 + rtm2 * vtm2) / rtm;
- xnc = xnp * vc * xlam;
- am1 = (-xnc) * Math.Sin(xlam * Math.PI / 180);
- am2 = (xnc) * Math.Cos(xlam * Math.PI / 180);
- betad = (xnt) / vt;
- beta += Time.deltaTime * betad;
- vm1 += Time.deltaTime * am1;
- vm2 += Time.deltaTime * am2;
- rm1 += Time.deltaTime * vm1;
- rm2 += Time.deltaTime * vm2;
- #region
- //while (vc >= 0)
- //{
- // if (rtm < 1000)
- // h = 0.0002;
- // else
- // h = 0.01;
- // betaold = beta;
- // rt1old = rt1;
- // rt2old = rt2;
- // rm1old = rm1;
- // rm2old = rm2;
- // vm1old = vm1;
- // vm2old = vm2;
- // int step = 1;
- // int flag = 0;
- // while(step <= 1)
- // {
- // if(flag == 1)
- // {
- // step = 2;
- // beta = beta + h * betad;
- // rt1 = rt1 + h * vt1;
- // rt2 = rt2 + h * vt2;
- // rm1 = rm1 + h * vm1;
- // rm2 = rm2 + h * vm2;
- // vm1 = vm1 + h * am1;
- // vm2 = vm2 + h * am2;
- // t = t + h;
- // }
- // rtm1 = rt1 - rm1;
- // rtm2 = rt2 - rm2;
- // rtm = Math.Sqrt(rtm1 * rtm1 + rtm2 * rtm2);
- // vtm1 = vt1 - vm1;
- // vtm2 = vt2 - vm2;
- // vc = -(rtm1 * vtm1 + rtm2 * vtm2) / rtm;
- // xlam = Math.Atan2(rtm2, rtm1);
- // xlamd = (rtm1 * vtm2 - rtm2 * vtm1) / (rtm * rtm);
- // //double XNC1 = ( (xnp + 20000000 * xlamd * xlamd) / vc) * vc * xlamd;
- // am1 = -xnc * Math.Sin(xlam);
- // am2 = xnc * Math.Cos(xlam);
- // vt1 = -vt * Math.Cos(beta);
- // vt2 = vt * Math.Sin(beta);
- // betad = xnt / vt;
- // flag = 1;
- // }
- // flag = 0;
- // beta = .5 * (betaold + beta + h * betad);
- // rt1 = .5 * (rt1old + rt1 + h * vt1);
- // rt2 = .5 * (rt2old + rt2 + h * vt2);
- // rm1 = .5 * (rm1old + rm1 + h * vm1);
- // rm2 = .5 * (rm2old + rm2 + h * vm2);
- // vm1 = .5 * (vm1old + vm1 + h * am1);
- // vm2 = .5 * (vm2old + vm2 + h * am2);
- // s = s + h;
- // if (s >= 0.09999)
- // s = 0;
- //}
- #endregion
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement