Beispiel #1
0
        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;
        }
Beispiel #2
0
        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));
            }
        }
Beispiel #4
0
        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);
        }
Beispiel #5
0
        /// <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);
        }
Beispiel #6
0
        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);
        }
Beispiel #7
0
        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;
        }
Beispiel #8
0
        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;
        }
Beispiel #9
0
        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);
            }
        }
Beispiel #10
0
 public bool CloseTo(Ets2Point pt)
 {
     return DistanceTo(pt) <= 2f;
 }
Beispiel #11
0
        /// <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;
        }
Beispiel #12
0
 public bool CloseTo(Ets2Point pt)
 {
     return(DistanceTo(pt) <= 2f);
 }