public float DistanceTo(Ets2Point pt)
{
if (pt == null) return float.MaxValue;
var dx = pt.X - X;
var dy = pt.Z - Z;
var dst = (float) Math.Sqrt(dx * dx + dy * dy);
return dst;
}
public Ets2NavigationRoute(Ets2Item start, Ets2Item end, Ets2Point from, Ets2Point to, Ets2Mapper mapper)
{
Start = start;
End = end;
From = from;
To = to;
Mapper = mapper;
if (Start != End)
ThreadPool.QueueUserWorkItem(new WaitCallback(FindRoute));
}
public Ets2NavigationRoute(Ets2Item start, Ets2Item end, Ets2Point from, Ets2Point to, Ets2Mapper mapper)
{
Start = start;
End = end;
From = from;
To = to;
Mapper = mapper;
if (Start != End)
{
ThreadPool.QueueUserWorkItem(new WaitCallback(FindRoute));
}
}
public float DistanceTo(Ets2Point pt)
{
if (pt == null)
{
return(float.MaxValue);
}
var dx = pt.X - X;
var dy = pt.Z - Z;
var dst = (float)Math.Sqrt(dx * dx + dy * dy);
return(dst);
}
/// <summary>
/// Generate road curves for a specific lane. The curve is generated with [steps]
/// nodes and positioned left or right from the road's middle point.
/// Additionally, each extra lane is shifted 4.5 game units outward.
/// </summary>
/// <param name="steps"></param>
/// <param name="leftlane"></param>
/// <param name="lane"></param>
/// <returns></returns>
public IEnumerable <Ets2Point> GenerateRoadCurve(int steps, bool leftlane, int lane)
{
var ps = new Ets2Point[steps];
var sx = StartNode.X;
var ex = EndNode.X;
var sz = StartNode.Z;
var ez = EndNode.Z;
if (steps == 2)
{
sx += (float)Math.Sin(-StartNode.Yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
sz += (float)Math.Cos(-StartNode.Yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
ex += (float)Math.Sin(-EndNode.Yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
ez += (float)Math.Cos(-EndNode.Yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
ps[0] = new Ets2Point(sx, 0, sz, StartNode.Yaw);
ps[1] = new Ets2Point(ex, 0, ez, EndNode.Yaw);
return(ps);
}
var radius = (float)Math.Sqrt((sx - ex) * (sx - ex) + (sz - ez) * (sz - ez));
var tangentSX = (float)Math.Cos(-StartNode.Yaw) * radius;
var tangentEX = (float)Math.Cos(-EndNode.Yaw) * radius;
var tangentSZ = (float)Math.Sin(-StartNode.Yaw) * radius;
var tangentEZ = (float)Math.Sin(-EndNode.Yaw) * radius;
for (int k = 0; k < steps; k++)
{
var s = (float)k / (float)(steps - 1);
var x = (float)Ets2CurveHelper.Hermite(s, sx, ex, tangentSX, tangentEX);
var z = (float)Ets2CurveHelper.Hermite(s, sz, ez, tangentSZ, tangentEZ);
var tx = (float)Ets2CurveHelper.HermiteTangent(s, sx, ex, tangentSX, tangentEX);
var ty = (float)Ets2CurveHelper.HermiteTangent(s, sz, ez, tangentSZ, tangentEZ);
var yaw = (float)Math.Atan2(ty, tx);
x += (float)Math.Sin(-yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
z += (float)Math.Cos(-yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
ps[k] = new Ets2Point(x, 0, z, yaw);
}
return(ps);
}
public IEnumerable <Ets2Point> GeneratePolygonForRoute(Ets2PrefabRoute route, Ets2Node node, int nodeOr)
{
List <Ets2Point> p = new List <Ets2Point>();
if (route == null || route.Route == null)
{
return(p);
}
/*
* yaw -= this.Nodes[nodeOr].Yaw;
* yaw += Math.PI/2;
*/
var xOr = node.X;
var yOr = node.Z;
var yaw = node.Yaw - this.Nodes[nodeOr].Yaw + Math.PI / 2;
foreach (var curve in route.Route)
{
var srx = curve.StartX - this.Nodes[nodeOr].X;
var erx = curve.EndX - this.Nodes[nodeOr].X;
var sry = curve.StartZ - this.Nodes[nodeOr].Z;
var ery = curve.EndZ - this.Nodes[nodeOr].Z;
var sr = (float)Math.Sqrt(srx * srx + sry * sry);
var er = (float)Math.Sqrt(erx * erx + ery * ery);
var ans = yaw - Math.Atan2(sry, srx);
var ane = yaw - Math.Atan2(ery, erx);
var sx = xOr - sr * (float)Math.Sin(ans);
var ex = xOr - er * (float)Math.Sin(ane);
var sy = yOr - sr * (float)Math.Cos(ans);
var ey = yOr - er * (float)Math.Cos(ane);
// tmp
var ps = new Ets2Point[2];
ps[0] = new Ets2Point(sx, node.Y, sy, (float)ans);
ps[1] = new Ets2Point(ex, node.Y, ey, (float)ane);
p.AddRange(ps);
}
return(p);
}
public IEnumerable<Ets2Point> GeneratePolygonForRoute(Ets2PrefabRoute route, Ets2Node node, int nodeOr)
{
List<Ets2Point> p = new List<Ets2Point>();
if (route == null || route.Route == null)
return p;
/*
yaw -= this.Nodes[nodeOr].Yaw;
yaw += Math.PI/2;
*/
var xOr = node.X;
var yOr = node.Z;
var yaw = node.Yaw - this.Nodes[nodeOr].Yaw + Math.PI / 2;
foreach (var curve in route.Route)
{
var srx = curve.StartX - this.Nodes[nodeOr].X;
var erx = curve.EndX - this.Nodes[nodeOr].X;
var sry = curve.StartZ - this.Nodes[nodeOr].Z;
var ery = curve.EndZ - this.Nodes[nodeOr].Z;
var sr = (float) Math.Sqrt(srx*srx + sry*sry);
var er = (float) Math.Sqrt(erx*erx + ery*ery);
var ans = yaw - Math.Atan2(sry, srx);
var ane = yaw - Math.Atan2(ery, erx);
var sx = xOr - sr*(float) Math.Sin(ans);
var ex = xOr - er*(float) Math.Sin(ane);
var sy = yOr - sr*(float) Math.Cos(ans);
var ey = yOr - er*(float) Math.Cos(ane);
// tmp
var ps = new Ets2Point[2];
ps[0] = new Ets2Point(sx, node.Y, sy,(float) ans);
ps[1] = new Ets2Point(ex, node.Y, ey, (float)ane);
p.AddRange(ps);
}
return p;
}
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 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);
}
}
public bool CloseTo(Ets2Point pt)
{
return DistanceTo(pt) <= 2f;
}
/// <summary>
/// Generate road curves for a specific lane. The curve is generated with [steps]
/// nodes and positioned left or right from the road's middle point.
/// Additionally, each extra lane is shifted 4.5 game units outward.
/// </summary>
/// <param name="steps"></param>
/// <param name="leftlane"></param>
/// <param name="lane"></param>
/// <returns></returns>
public IEnumerable<Ets2Point> GenerateRoadCurve(int steps, bool leftlane, int lane)
{
var ps = new Ets2Point[steps];
var sx = StartNode.X;
var ex = EndNode.X;
var sz = StartNode.Z;
var ez = EndNode.Z;
if (steps == 2)
{
sx += (float)Math.Sin(-StartNode.Yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
sz += (float)Math.Cos(-StartNode.Yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
ex += (float)Math.Sin(-EndNode.Yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
ez += (float)Math.Cos(-EndNode.Yaw) * (leftlane ? -1 : 1) * (RoadLook.Offset + (0.5f + lane) * 4.5f);
ps[0] = new Ets2Point(sx, 0, sz, StartNode.Yaw);
ps[1] = new Ets2Point(ex, 0, ez, EndNode.Yaw);
return ps;
}
var radius = (float)Math.Sqrt((sx - ex) * (sx - ex) + (sz - ez) * (sz - ez));
var tangentSX = (float)Math.Cos(-StartNode.Yaw) * radius;
var tangentEX = (float)Math.Cos(-EndNode.Yaw) * radius;
var tangentSZ = (float)Math.Sin(-StartNode.Yaw) * radius;
var tangentEZ = (float)Math.Sin(-EndNode.Yaw) * radius;
for (int k = 0; k < steps; k++)
{
var s = (float) k/(float) (steps - 1);
var x = (float) Ets2CurveHelper.Hermite(s, sx, ex, tangentSX, tangentEX);
var z = (float) Ets2CurveHelper.Hermite(s, sz, ez, tangentSZ, tangentEZ);
var tx = (float) Ets2CurveHelper.HermiteTangent(s, sx, ex, tangentSX, tangentEX);
var ty = (float) Ets2CurveHelper.HermiteTangent(s, sz, ez, tangentSZ, tangentEZ);
var yaw = (float) Math.Atan2(ty, tx);
x += (float) Math.Sin(-yaw)*(leftlane ? -1 : 1)*(RoadLook.Offset + (0.5f + lane)*4.5f);
z += (float) Math.Cos(-yaw)*(leftlane ? -1 : 1)*(RoadLook.Offset + (0.5f + lane)*4.5f);
ps[k] = new Ets2Point(x, 0, z, yaw);
}
return ps;
}
public bool CloseTo(Ets2Point pt)
{
return(DistanceTo(pt) <= 2f);
}
