private void drawInfo(OutSimEventArgs e)
{
var mpos = e.Pos.ToLfsFloatV();
var vel = e.Vel.Length() * 3.6f;
var headd = e.Heading * 180f / Math.PI;
BTN(1, 10, 20, 20, 5, "^3x: " + mpos.X.ToString("0.00"));
BTN(2, 10, 25, 20, 5, "^3y: " + mpos.Y.ToString("0.00"));
BTN(3, 10, 30, 20, 5, "^2spd: " + vel.ToString("0.00"));
BTN(4, 10, 35, 20, 5, "^1head: " + headd.ToString("0.0"));
}
private void Outsim_PacketReceived(object sender, OutSimEventArgs e)
{
drawInfo(e);
var PosX = e.Pos.X / 65535.0;
var PosY = e.Pos.Y / 65535.0;
var PosZ = e.Pos.Z / 65535.0;
var vel = e.Vel.Length() * 3.6f;
//insim.Send(new InSimDotNet.Packets.IS_MSL() {Msg = $"vel: {vel.ToString("0.0")} pos: {PosX.ToString("0.0")} {PosY.ToString("0.0")} {PosZ.ToString("0.0")}" });
var cpos = Cursor.Position;
var mpos = e.Pos.ToLfsFloatV();
var heading = (float)(e.Heading * 180f / Math.PI);
var carangle = (float)(270 + (Math.Atan2(e.Vel.Y, e.Vel.X) * 180f / Math.PI)) % 360;
carangle = confine360(carangle);
var drift = heading - carangle;
drift = confine360(drift);
if (vel < 20)
{
drift = 0;
}
//var closestD = Math.Sqrt(currentPath.Min(x => x.Dist2(mpos)));
//if((DateTime.Now - lastRandomed).TotalSeconds > 300 && pathIndex >= currentPath.Count - 5)
//{
// Random r = new Random();
// var p = paths.paths[r.Next(paths.paths.Count)];
// currentPath = p.Waypoints;
// MSL("^7Random path: ^3" + p.Name);
// initPhase = true;
// lastRandomed = DateTime.Now;
//}
if (initPhase)
{
//var closestD = Math.Sqrt(currentPath.Min(x => x.Dist2(mpos)));
pathIndex = currentPath.IndexOf(currentPath.MinBy(x => x.Dist2(mpos)));
initPhase = false;
}
var target = currentPath[pathIndex];
var tdist = target.Dist(mpos);
while (tdist <= 5)
{
pathIndex = (pathIndex + 1) % currentPath.Count;
target = currentPath[pathIndex];
tdist = target.Dist(mpos);
}
var tgan = (270 + (180f * Math.Atan2(target.Y - mpos.Y, target.X - mpos.X) / Math.PI)) % 360;
tgan = confine360((float)tgan);
var diff = (float)(tgan - heading);
diff = confine360(diff);
//if (diff < -180) diff += 360;
//if (diff > 180) diff -= 360;
var turn = 50 - 50 * (diff / 30f);
turn = Math.Min(100, Math.Max(0, turn));
turn += drift * 0.6f;
turn = Math.Min(110, Math.Max(-10, turn));
turn += e.AngVel.Z * 3;
turn = Math.Min(100, Math.Max(0, turn));
float tspeed = target.Z * speedmod / 100f;
BTN(10, 40, 25, 30, 5, "Closest: ^7" + tdist.ToString("0.000"));
BTN(11, 40, 30, 30, 5, "^1target: " + tgan.ToString("0.0"));
BTN(12, 40, 35, 30, 5, "^3diff: " + diff.ToString("0.0"));
BTN(13, 40, 40, 30, 5, "^7turn: " + turn.ToString("0.0"));
BTN(14, 40, 45, 30, 5, "^2tspeed: " + tspeed.ToString("0.0"));
BTN(5, 10, 40, 20, 5, "^7carangle: " + carangle.ToString("0.0"));
BTN(6, 10, 45, 20, 5, "^7drift: " + drift.ToString("0.0"));
if (control == 0)
{
joystick.SetDevAxis(hDev, 1, cpos.X / 19.2f);
joystick.SetDevAxis(hDev, 2, 100 - cpos.Y / 10.8f);
}
else if (control == 1)
{
var r = new Random().NextDouble();
var thr = (tspeed - vel) / 0.2f + (randomSpeed ? (float)r : 0);
//joystick.SetDevAxis(hDev, 2, vel < 60 ? 0 : 50);
joystick.SetDevAxis(hDev, 2, 50 + thr);
joystick.SetDevAxis(hDev, 1, (float)turn);
}
}