private void FindNewSegments(Ets2NavigationRoute route, Ets2Point me)
{
if (route == null || route.Segments == null)
{
return;
}
var segs = new List <Ets2NavigationSegment>();
var dstLimit = 1250;
rescan:
foreach (var seg in route.Segments)
{
var dstEntry = seg.Entry.Point.DistanceTo(me) < dstLimit;
var dstExit = seg.Exit.Point.DistanceTo(me) < dstLimit;
if (dstEntry || dstExit)
{
segs.Add(seg);
}
}
if (!segs.Any() && dstLimit == 1250)
{
dstLimit = 5000;
goto rescan;
}
NearbySegments = segs;
}
public Ets2MapDemo()
{
var projectMap = @"C:\Projects\Software\ets2-map\";
map = new Ets2Mapper(
projectMap + @"SCS\europe\",
projectMap + @"SCS\prefab",
projectMap + @"SCS\LUT1.19",
projectMap + @"LUT\LUT1.19");
map.Parse(true);
render = new MapRenderer(map, new SimpleMapPalette());
InitializeComponent();
SetStyle(ControlStyles.UserPaint, true);
SetStyle(ControlStyles.OptimizedDoubleBuffer, true);
SetStyle(ControlStyles.AllPaintingInWmPaint, true);
refresh = new Timer();
refresh.Interval = 250;
refresh.Tick += (sender, args) => Invalidate();
refresh.Start();
// Panning around
MouseDown += (s, e) => dragPoint = e.Location;
MouseUp += (s, e) => dragPoint = null;
MouseMove += (s, e) =>
{
if (dragPoint.HasValue)
{
var spd = mapScale/Math.Max(this.Width, this.Height);
location = new Ets2Point(location.X - (e.X - dragPoint.Value.X)*spd,
0,
location.Z - (e.Y - dragPoint.Value.Y)*spd,
0);
dragPoint = e.Location;
}
};
// Zooming in
MouseWheel += Ets2MapDemo_MouseWheel;
// Navigation
MouseDoubleClick += Ets2MapDemo_MouseDoubleClick;
Resize += Ets2MapDemo_Resize;
}
protected override void OnPaint(PaintEventArgs e)
{
if (navigatePoint != null)
{
route = map.NavigateTo(location, navigatePoint);
navigatePoint = null;
}
if (route != null && route.Loading == false)
{
render.SetNavigation(route);
}
render.Render(e.Graphics, e.ClipRectangle, mapScale, location);
base.OnPaint(e);
}
protected override void OnPaint(PaintEventArgs e)
{
if (navigatePoint != null)
{
route = map.NavigateTo(location, navigatePoint);
navigatePoint = null;
}
if (route != null && route.Loading == false)
render.SetNavigation(route);
render.Render(e.Graphics, e.ClipRectangle, mapScale, location);
base.OnPaint(e);
}
private void Ets2MapDemo_MouseDoubleClick(object sender, MouseEventArgs e)
{
navigatePoint = render.CalculatePointFromMap(e.X, e.Y);
}
private void Ets2MapDemo_MouseDoubleClick(object sender, MouseEventArgs e)
{
navigatePoint = render.CalculatePointFromMap(e.X, e.Y);
}
public Ets2MapDemo()
{
// Set location based on game
switch (Game)
{
case GAME.ETS2:
location = new Ets2Point(0, 0, 0, 0);
break;
case GAME.ATS:
location = new Ets2Point(-100000, 0, 17000, 0);
break;
}
// Get current folder and remove "\Ets2Map\Ets2Map.Demo\bin\[Debug|Release]"
var projectFolder = Directory.GetCurrentDirectory();
for (int i = 0; i < 4; i++)
{
projectFolder = projectFolder.Substring(0, projectFolder.LastIndexOf("\\"));
}
// Load game specific folder
var mapFilesFolder = projectFolder + "\\" + (Game == GAME.ETS2 ? "europe" : "usa");
map = new Ets2Mapper(
mapFilesFolder + @"\SCS\map\",
mapFilesFolder + @"\SCS\prefab\",
mapFilesFolder + @"\SCS\LUT\",
mapFilesFolder + @"\LUT\");
map.Parse(true);
render = new MapRenderer(map, new SimpleMapPalette());
InitializeComponent();
SetStyle(ControlStyles.UserPaint, true);
SetStyle(ControlStyles.OptimizedDoubleBuffer, true);
SetStyle(ControlStyles.AllPaintingInWmPaint, true);
refresh = new Timer();
refresh.Interval = 250;
refresh.Tick += (sender, args) => Invalidate();
refresh.Start();
// Panning around
MouseDown += (s, e) => dragPoint = e.Location;
MouseUp += (s, e) => dragPoint = null;
MouseMove += (s, e) => {
if (dragPoint.HasValue)
{
var spd = mapScale / Math.Max(this.Width, this.Height);
location = new Ets2Point(location.X - (e.X - dragPoint.Value.X) * spd,
0,
location.Z - (e.Y - dragPoint.Value.Y) * spd,
0);
dragPoint = e.Location;
}
};
// Zooming in
MouseWheel += Ets2MapDemo_MouseWheel;
// Navigation
MouseDoubleClick += Ets2MapDemo_MouseDoubleClick;
Resize += Ets2MapDemo_Resize;
}
public void TickTelemetry(IDataMiner data)
{
speed = data.Telemetry.Speed;
if (true)
{
var ets2Tel = (Ets2DataMiner)data;
var x = ets2Tel.MyTelemetry.Physics.CoordinateX;
var z = ets2Tel.MyTelemetry.Physics.CoordinateZ;
var yaw = 2 * Math.PI * (ets2Tel.MyTelemetry.Physics.RotationX);
var lah = 1.5f + ets2Tel.MyTelemetry.Physics.SpeedKmh / 100.0f * 7.5f;
x += (float)Math.Sin(yaw) * -lah;
z += (float)Math.Cos(yaw) * -lah;
var me = new Ets2Point(x, 0, z, (float)yaw);
lookPoint = new PointF(x, z);
// Get map
var map = Main.LoadedMap;
var route = dlMap.Route;
if (map == null || route == null)
{
Active = false;
return;
}
bool firstTry = true;
Ets2NavigationSegment activeSegment = default(Ets2NavigationSegment);
var activeSegmentOption = default(Ets2NavigationSegment.Ets2NavigationSegmentOption);
float dist = float.MaxValue;
rescanSegment:
// Find closest segment
for (int segI = 0; segI < NearbySegments.Count; segI++)
{
var seg = NearbySegments[segI];
if (seg == null)
{
continue;
}
if (!seg.Solutions.Any())
{
continue;
}
foreach (var sol in seg.Solutions)
{
if (sol.HiResPoints == null || !sol.HiResPoints.Any())
{
NearbySegments[segI].GenerateHiRes(sol);
}
var dst = sol.HiResPoints.Min(k => k.DistanceTo(me));
if (dist > dst)
{
dist = dst;
activeSegment = NearbySegments[segI];
activeSegmentOption = sol;
}
}
}
if (!NearbySegments.Any(k => k != null) || dist > 5)
{
FindNewSegments(route, me);
if (firstTry)
{
firstTry = false;
goto rescanSegment;
}
else
{
//beep.Play();
//Active = false;
//return;
}
}
if (activeSegmentOption == null)
{
return;
}
var lineDistanceError = 0.0;
var angleDistancError = 0.0;
Ets2Point bestPoint = default(Ets2Point);
Ets2Point bestPointP1 = default(Ets2Point);
double bestDistance = double.MaxValue;
for (var k = 0; k < activeSegmentOption.HiResPoints.Count; k++)
{
var distance = me.DistanceTo(activeSegmentOption.HiResPoints[k]);
if (bestDistance > Math.Abs(distance))
{
bestDistance = Math.Abs(distance);
if (k + 1 == activeSegmentOption.HiResPoints.Count)
{
bestPoint = activeSegmentOption.HiResPoints[k - 1];
bestPointP1 = activeSegmentOption.HiResPoints[k];
}
else
{
bestPoint = activeSegmentOption.HiResPoints[k];
var m = k;
do
{
m++;
if (m >= activeSegmentOption.HiResPoints.Count)
{
break;
}
bestPointP1 = activeSegmentOption.HiResPoints[m];
} while (bestPoint.DistanceTo(bestPointP1) < 0.1f && m + 1 < activeSegmentOption.HiResPoints.Count);
}
}
}
var min = activeSegmentOption.HiResPoints.Min(k => k.DistanceTo(me));
if (bestPoint == null)
{
return;
}
var lx1 = bestPoint.X - Math.Sin(-bestPoint.Heading) * 5;
var lz1 = bestPoint.Z - Math.Cos(-bestPoint.Heading) * 5;
var lx2 = bestPoint.X + Math.Sin(-bestPoint.Heading) * 5;
var lz2 = bestPoint.Z + Math.Cos(-bestPoint.Heading) * 5;
lx2 = bestPoint.X;
lx1 = bestPointP1.X;
lz2 = bestPoint.Z;
lz1 = bestPointP1.Z;
var px1 = me.X - lx1;
var pz1 = me.Z - lz1;
var px2 = lz2 - lz1;
var pz2 = -(lx2 - lx1);
var qwer = Math.Sqrt(px2 * px2 + pz2 * pz2);
Console.WriteLine(qwer);
// Reference to top (otherwise 90deg offset) - CCW
yawRoad = activeSegment.Type == Ets2NavigationSegmentType.Road ? -bestPoint.Heading + Math.PI / 2 : bestPoint.Heading - Math.PI / 2;
hook = bestPoint;
lineDistanceError = (px1 * px2 + pz1 * pz2) / Math.Sqrt(px2 * px2 + pz2 * pz2);
angleDistancError = yaw - yawRoad;
angleDistancError = angleDistancError % (Math.PI * 2);
//lineDistanceError = -lineDistanceError;
if (lineDistanceError > 7)
{
lineDistanceError = 7;
}
if (lineDistanceError < -7)
{
lineDistanceError = -7;
}
//if (Math.Abs(angleDistancError) < Math.PI/4) lineDistanceError = -lineDistanceError;
Console.WriteLine(lineDistanceError.ToString("0.00m") + " | " + angleDistancError.ToString("0.000rad"));
var gain = 2.5f + ets2Tel.Telemetry.Speed / 2.5f;
SteerAngle = 0.5f - lineDistanceError / gain;// - angleDistancError * 0.1f;
//Debug.WriteLine(lineDistanceError + "px error / " + angleDistancError + " angle error / " + SteerAngle);
}
}