public static ExtrinsicCameraParameters Calibrate(PointF[] locals, MCvPoint3D32f[] globals, Size pattern) { PointF[] pl = new PointF[] { locals[0 + (pattern.Height -1) * pattern.Width], locals[0 + 0 * pattern.Width], locals[(pattern.Width-1) + (pattern.Height -1) * pattern.Width], }; MCvPoint3D32f[] pg = new MCvPoint3D32f[] { globals[0 + (pattern.Height -1) * pattern.Width], globals[0 + 0 * pattern.Width], globals[(pattern.Width-1) + (pattern.Height -1) * pattern.Width], }; //f2 = (p1 - p0).Normalize(1); //f1 = (p2 - p0).Normalize(1); //f1 = (f1 - (f1.DotProduct(f2) * f2)).Normalize(1); //f3 = new DenseVector(new double[] { f1[1] * f2[2] - f1[2] * f2[1], f1[2] * f2[0] - f1[0] * f2[2], f1[0] * f2[1] - f1[1] * f2[0] }); //f3 = f3.Normalize(1); var plv = pl.Select(row => new DenseVector(new double[] { row.X, row.Y })).ToArray(); return null; }
public PolyRect(PointF[] points) { var rect = PolygonMath.GetBoundingBox(points); this.SetBounds(rect.X, rect.Y, rect.Width, rect.Height); // PolygonMath.IsUnrotated can tell us that the points follow a // particular pattern, but in order to represent 90-degree rotations // and flips, we want to consider them to be *non-rectangle* points. // Therefore, we use a much more strict definition: there can only // be 4 points, and they must be in the canonical order. var right = this.x + this.width; var bottom = this.y + this.height; this.rect = points.Length == 4 && points[0].X == this.x && points[0].Y == this.y && points[1].X == right && points[1].Y == this.y && points[2].X == right && points[2].Y == bottom && points[3].X == this.x && points[3].Y == bottom; this.points = points.Select(p => new float[] { p.X, p.Y }).ToArray(); }
protected override void OnPaint(PaintEventArgs e) { if (fitToSize) { // Measure size (this is with border) var w = e.ClipRectangle.Width; var h = e.ClipRectangle.Height; var dw = targetW - w; var dh = targetH - h; this.Size = new Size(this.Width + dw, this.Height + dh); fitToSize = false; } var g = e.Graphics; g.FillRectangle(Brushes.Black, e.ClipRectangle); if (data == null) { g.DrawString("NO ETS2 DATA MINER ACTIVE", new Font("Arial", 24.0f), Brushes.White, 5, 5); return; } // Render map float scale = 150; // p-p dlMap.RenderMap(e.ClipRectangle, g, true, ref scale); // ME: g.DrawEllipse(new Pen(meColor, 5.0f), targetW/2 - 3, targetH/2 - 3, 6, 6); var meHeading = Math.PI - data.MyTelemetry.Physics.RotationX*-2*Math.PI; var meHeadingRadius = 25; g.DrawLine(new Pen(meColor, 3.0f), targetW/2, targetH/2, targetW/2 + (float) Math.Sin(meHeading)*meHeadingRadius, targetH/2 + (float) Math.Cos(meHeading)*meHeadingRadius); var centerX = data.MyTelemetry.Physics.CoordinateX; var centerY = data.MyTelemetry.Physics.CoordinateY; var centerZ = data.MyTelemetry.Physics.CoordinateZ; var steeringAngle = 35.0/180*Math.PI*data.MyTelemetry.Controls.GameSteer; var wheelBase = 4; var saTan = (float) Math.Tan(Math.Abs(steeringAngle)); var steerRadius = wheelBase/saTan; var steerCircumfere = steerRadius*2*Math.PI; var steerRadiusSc = steerRadius/scale*targetW; var offx = Math.Sin(meHeading); if (false && float.IsNaN(steerRadiusSc) == false && float.IsInfinity(steerRadiusSc) == false && Math.Abs(steerRadius) < 30000) { if (steeringAngle < 0) // left { var corrAngle = meHeading + Math.PI/2; g.DrawArc(new Pen(Brushes.Aqua, 2.0f), targetW/2 - steerRadiusSc/2 - (float) Math.Sin(corrAngle)*steerRadiusSc/2, targetH/2 - steerRadiusSc/2 - (float) Math.Cos(corrAngle)*steerRadiusSc/2, steerRadiusSc, steerRadiusSc, (float) (meHeading/Math.PI*180 + 90), 360); } else { var corrAngle = meHeading + Math.PI/2; g.DrawArc(new Pen(Brushes.Aqua, 2.0f), targetW/2 - steerRadiusSc/2 + (float) Math.Sin(corrAngle)*steerRadiusSc/2, targetH/2 - steerRadiusSc/2 + (float) Math.Cos(corrAngle)*steerRadiusSc/2, steerRadiusSc, steerRadiusSc, (float) (meHeading/Math.PI*180 + 90), 360); } /*g.DrawEllipse(new Pen(Brushes.Turquoise, 2.0f), targetW / 2 - (float)Math.Sin(meHeading) * steerRadiusSc, targetH / 2 - (float)Math.Cos(meHeading) * steerRadiusSc, steerRadiusSc, steerRadiusSc);*/ } // Track this vehicle along curve: var mX = data.MyTelemetry.Physics.CoordinateX; var mY = data.MyTelemetry.Physics.CoordinateZ; var px = 0.0f; var py = 0.0f; var step = 0.05f; var heading = meHeading + Math.PI/2; foreach (var car in data.Cars) { car.Tracked = false; car.Tick(); } var pwr = Main.Drivetrain.CalculatePower(data.MyTelemetry.Drivetrain.EngineRpm, data.MyTelemetry.Controls.GameThrottle); var scanDistance = 2.0f + Math.Pow(1 - Math.Abs(data.MyTelemetry.Controls.GameSteer), 64)*3.5f; for (float ts = 0.0f; ts < scanDistance && !data.Cars.Any(x => x.Tracked); ts += step) { // Interpolate the steer radius var ds = data.MyTelemetry.Drivetrain.Speed*ts; var spd = Math.Max(10, data.MyTelemetry.Drivetrain.Speed); // always scan at minimum of 36kmh var da = spd/steerCircumfere*2*Math.PI*step; var dix = Math.Sin(heading) - Math.Sin(heading + da); var diy = Math.Cos(heading) - Math.Cos(heading + da); if (steeringAngle < 0) heading -= da; else heading += da; px += (float) dix*steerRadius/2; py += (float) diy*steerRadius/2; // Rotated polygon var carL = 12.0f; var carW = 2.5f; var hg = -heading; // PointF[] poly = new PointF[] { new PointF(mX + px + carL/2*(float) Math.Cos(hg) - carW/2*(float) Math.Sin(hg), mY + py + carL/2*(float) Math.Sin(hg) + carW/2*(float) Math.Cos(hg)), new PointF(mX + px - carL/2*(float) Math.Cos(hg) - carW/2*(float) Math.Sin(hg), mY + py - carL/2*(float) Math.Sin(hg) + carW/2*(float) Math.Cos(hg)), new PointF(mX + px - carL/2*(float) Math.Cos(hg) + carW/2*(float) Math.Sin(hg), mY + py - carL/2*(float) Math.Sin(hg) - carW/2*(float) Math.Cos(hg)), new PointF(mX + px + carL/2*(float) Math.Cos(hg) + carW/2*(float) Math.Sin(hg), mY + py + carL/2*(float) Math.Sin(hg) - carW/2*(float) Math.Cos(hg)), }; foreach (var car in data.Cars) { if (car.Valid && IsPolygonsIntersecting(car.Box, poly)) { car.Tracked = true; break; } } var drx = targetW/2 + px/scale*targetW; var dry = targetH/2 + py/scale*targetH; var polyToDraw = poly.Select( x => new PointF(targetW/2 + (x.X - mX)/2/scale*targetW, targetH/2 + (x.Y - mY)/2/scale*targetH)) .ToArray(); g.FillPolygon(Brushes.Tomato, polyToDraw); //g.DrawLine(new Pen(Color.DarkSalmon, 5.0f), drx, dry, drx + 1, dry); } g.FillRectangle(new SolidBrush(Color.FromArgb(25, 25, 25)), 0, 0, targetW, 32); var trafficColor = Brushes.BlueViolet; foreach (var car in data.Cars) { if (!car.Valid) continue; var x = targetW/2 + (car.X - centerX)/2/scale*targetW; var y = targetH/2 + (car.Z - centerZ)/2/scale*targetH; var sz = 10*scale; if (sz > 10) sz = 10; var of = sz/2; var dx = car.X - centerX; var dy = car.Z - centerZ; var dv = data.MyTelemetry.Drivetrain.Speed - car.Speed; // m/s var distance = (float) Math.Sqrt(dx*dx + dy*dy) - 12; if (distance < 0.1f) distance = 0.1f; var tti = dv < 0 ? -1.0f : distance/dv; var tc = trafficColor; if (car.Tracked) { g.DrawString("Track #" + car.ID + " SPD " + Math.Round(car.Speed*3.6) + "km/h (d=" + Math.Round(car.Speed*3.6 - data.MyTelemetry.Drivetrain.SpeedKmh,1) + "); distance " + Math.Round(distance, 1) + "m TTI " + ((tti==-1.0f)?"never":Math.Round(tti, 2) + "s"), new Font("Arial", 11.0f, FontStyle.Bold), Brushes.White, 5, 5); car.Distance = distance; car.TTI = tti; TrackedCar = car; tc = Brushes.Turquoise; if (tti < 10 && tti > 0) tc = Brushes.CadetBlue; if (tti < 5 && tti > 0) tc = Brushes.DarkSalmon; if (tti < 2 && tti > 0) tc = Brushes.HotPink; } if (car.Box == null) continue; var polyToDraw = car.Box.Select( d => new PointF(targetW/2 + (d.X - centerX)/2/scale*targetW, targetH/2 + (d.Y - centerZ)/2/scale*targetH)).ToArray(); g.FillPolygon(tc, polyToDraw); //g.DrawEllipse(new Pen(tc, sz), x - of, y - of, sz, sz); /*if (tti == -1337.0f) g.DrawString(":-)", new Font("Arial", 7.0f), Brushes.White, x-8,y-4); else g.DrawString(Math.Round(tti,1).ToString(), new Font("Arial", 7.0f), Brushes.White, x-8,y-4);*/ } g.FillRectangle(new SolidBrush(Color.FromArgb(25, 25, 25)), 0, targetH - 32, targetW, targetH); g.DrawString("Looking " + Math.Round(scanDistance, 2) + "s ahead SPD " + Math.Round(data.MyTelemetry.Drivetrain.SpeedKmh) + "kmh PWR " + Math.Round(pwr) + "hp THR " + Math.Round(data.MyTelemetry.Controls.GameThrottle * 100, 1) + "% BKR " + Math.Round(data.MyTelemetry.Controls.GameBrake * 100, 2) + "%", new Font("Arial", 11.0f, FontStyle.Bold), Brushes.White, 2, targetH - 25); if (data.Cars.Any(x => x.Tracked) == false) { g.DrawString("No car being tracked (followed)", new Font("Arial", 12.0f, FontStyle.Bold), Brushes.White, 5, 5); TrackedCar = null; } }
public PointF[] OffsetPoints(PointF[] Points) { return Points.Select(p => PointF.Subtract(p, Offset)).ToArray(); }
public static PointF[] PhineTune(Projector projector, Camera camera, PointF[] cameraCorners, PointF[] rough, int subdiv) { var color = Color.Green; projector.DrawBackground(Color.Black); var nolight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; projector.DrawBackground(color); var fulllight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; Func<int, bool, Image<Gray, byte>[]> takePics = (step, vertical) => { Image<Gray, byte>[] pics = new Image<Gray, byte>[3]; projector.DrawBackground(Color.Black); camera.TakePicture(5).Dispose(); projector.DrawPhaseMod(step, (float)(-2.0f * Math.PI / 3.0f), vertical, color); pics[0] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; projector.DrawBackground(Color.Black); camera.TakePicture(5).Dispose(); projector.DrawPhaseMod(step, 0f, vertical, color); pics[1] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; projector.DrawBackground(Color.Black); camera.TakePicture(5).Dispose(); projector.DrawPhaseMod(step, (float)(2.0f * Math.PI / 3.0f), vertical, color); pics[2] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; return pics; }; Func<Image<Gray, byte>[], double[]> itop = (pics) => { return cameraCorners.Select(c => { var mini = nolight[(int)c.Y, (int)c.X].Intensity; var maxi = fulllight[(int)c.Y, (int)c.X].Intensity; var i1 = pics[0][(int)c.Y, (int)c.X].Intensity; i1 = (i1 - mini) / (maxi - mini); var i2 = pics[1][(int)c.Y, (int)c.X].Intensity; i2 = (i2 - mini) / (maxi - mini); var i3 = pics[2][(int)c.Y, (int)c.X].Intensity; i3 = (i3 - mini) / (maxi - mini); return PhaseModulation.IntensityToPhase(i1, i2, i3); }).ToArray(); }; int w = projector.Size.Width; int h = projector.Size.Height; var xphs = itop(takePics(subdiv, true)); var yphs = itop(takePics(subdiv, false)); var ids = new int[cameraCorners.Length]; int idx = 0; ids = ids.Select(i => idx++).ToArray(); return ids.Select(i => { var v = rough[i]; var denom = (double)subdiv; var xph = xphs[i] * (w / denom); var yph = yphs[i] * (h / denom); var phsx = Math.Floor(v.X / denom) * denom; var phsy = Math.Floor(v.Y / denom) * denom; double apx = v.X / w; double phx = xphs[i]; apx = Math.Floor(apx / (1.0 / denom)) * (1.0 / denom) + (phx < 1 ? phx / denom : 0); apx = Math.Round(apx, 5) * w; double apy = v.Y / h; double phy = yphs[i]; apy = Math.Floor(apy / (1.0 / denom)) * (1.0 / denom) + (phy < 1 ? phy / denom : 0); apy = Math.Round(apy, 5) * h; return new PointF((float)(apx), (float)(apy)); }).ToArray(); }
public static PointF[] PhaseCalib(Projector projector, Camera camera, PointF[] cameraCorners, int steps = 7) { var color = Color.Green; projector.DrawBackground(Color.Black); var nolight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; projector.DrawBackground(color); var fulllight = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; Func<int, bool, Image<Gray, byte>[]> takePics = (step, vertical) => { Image<Gray, byte>[] pics = new Image<Gray, byte>[3]; projector.DrawBackground(Color.Black); camera.TakePicture(5).Dispose(); projector.DrawPhaseMod(step, (float)(-2.0f * Math.PI / 3.0f), vertical, color); pics[0] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; projector.DrawBackground(Color.Black); camera.TakePicture(5).Dispose(); projector.DrawPhaseMod(step, 0f, vertical, color); pics[1] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; projector.DrawBackground(Color.Black); camera.TakePicture(5).Dispose(); projector.DrawPhaseMod(step, (float)(2.0f * Math.PI / 3.0f), vertical, color); pics[2] = new Image<Bgr, byte>(camera.TakePicture(2)).Split()[1]; return pics; }; Func<Image<Gray, byte>[], double[]> itop = (pics) => { return cameraCorners.Select(c => { var mini = nolight[(int)c.Y, (int)c.X].Intensity; var maxi = fulllight[(int)c.Y, (int)c.X].Intensity; var i1 = pics[0][(int)c.Y, (int)c.X].Intensity; i1 = (i1 - mini) / (i1 - maxi); var i2 = pics[1][(int)c.Y, (int)c.X].Intensity; i2 = (i2 - mini) / (i2 - maxi); var i3 = pics[2][(int)c.Y, (int)c.X].Intensity; i3 = (i3 - mini) / (i3 - maxi); return PhaseModulation.IntensityToPhase(i1, i2, i3); }).ToArray(); }; int w = projector.Size.Width; int h = projector.Size.Height; int subdiv = 1; int[] xs = new int[cameraCorners.Length]; int[] ys = new int[cameraCorners.Length]; int[] ids = Range.OfInts(cameraCorners.Length).ToArray(); for (int i = 0; i < steps; i++) { var xpics = takePics(subdiv, true); var xphs = itop(xpics); var xh = ids.Select(id => xphs[id] > 0.5).ToArray(); var qd = QuickDraw.Start(xpics[1].Bitmap); var thrash = ids.Select(id => { qd.Color(xh[id] ? Color.White : Color.Gray); qd.DrawPoint(cameraCorners[id].X, cameraCorners[id].Y, 5); return id; }).ToArray(); qd.Finish(); DebugWindow.DrawBitmap(xpics[1].Bitmap); var ypics = takePics(subdiv, false); var yphs = itop(ypics); var yh = ids.Select(id => yphs[id] > 0.5).ToArray(); qd = QuickDraw.Start(ypics[1].Bitmap); thrash = ids.Select(id => { qd.Color(yh[id] ? Color.White : Color.Gray); qd.DrawPoint(cameraCorners[id].X, cameraCorners[id].Y, 5); return id; }).ToArray(); qd.Finish(); DebugWindow.DrawBitmap(ypics[1].Bitmap); xs = ids.Select(id => (xs[id] << 1) | (xh[id] ? 1 : 0)).ToArray(); ys = ids.Select(id => (ys[id] << 1) | (yh[id] ? 1 : 0)).ToArray(); subdiv = subdiv << 1; } var fxs = ids.Select(id => ((double)xs[id] / (double)subdiv) * w).ToArray(); var fys = ids.Select(id => ((double)ys[id] / (double)subdiv) * h).ToArray(); return fxs.Zip(fys, (x,y) => new PointF((float)x,(float)y)).ToArray(); }
public static Func<PointF[], PointF[]> FindHomography(PointF[] ccs, PointF[] pcs) { var hg = Emgu.CV.CameraCalibration.FindHomography(ccs, pcs, HOMOGRAPHY_METHOD.DEFAULT, 2); var t = ccs.Select(c => new PointF(c.X, c.Y)).ToArray(); hg.ProjectPoints(t); var tes = pcs.Zip(t, (a, b) => (b.X - a.X) * (b.X - a.X) + (b.Y - a.Y) * (b.Y - a.Y)).ToArray(); var sum = tes.Sum(); return (ps) => { var psc = ps.Select(p => new PointF(p.X, p.Y)).ToArray(); hg.ProjectPoints(psc); return psc; }; }
public static CalibrationResult CalibrateProjector(Projector projector, Camera camera, Size pattern, CalibrationResult cameraCalib, PointF[][] cacalibdata, Size cameraPattern, float checkerboardSize) { List<PointF[]> cameraCorners = new List<PointF[]>(); List<PointF[]> projectorCorners = new List<PointF[]>(); var cpattern = new Size(pattern.Width - 1, pattern.Height - 1); int steps = 15; double rotx = 0, roty = 0, rotz = 0; for (int i = 0; i < steps; i++) { var di = (double)i / (double)steps; rotx = 0; roty = Math.Sin(di * Math.PI / 2) * 0.8; rotz = Math.Sin(di * Math.PI / 2) * 0.6; var pcs = projector.DrawCheckerboard(pattern, rotx, roty, rotz, 0.5); var img = camera.TakePicture(3); var ccs = GetCameraCorners(img, cpattern, false); if (ccs != null) { projectorCorners.Add(pcs); cameraCorners.Add(ccs); var withCorners = camera.TakePicture(0); if (DebugWindow != null) { QuickDraw.Start(withCorners) .Color(Color.White) .DrawPoint(ccs, 5) .Finish(); DebugWindow.DrawBitmap(withCorners); } } } //for (int i = 0; i < steps; i++) //{ // var di = (double)i / (double)steps; // rotx += 0.04; // roty *= 0.90; // var pcs = projector.DrawCheckerboard(pattern, // rotx, // roty, // rotz, 0.7); // var ccs = GetCameraCorners(camera, cpattern, false); // if (ccs != null) // { // projectorCorners.Add(pcs); // cameraCorners.Add(ccs); // var withCorners = camera.TakePicture(0); // if (DebugWindow != null) // { // QuickDraw.Start(withCorners) // .Color(Color.White) // .DrawPoint(ccs, 5) // .Finish(); // DebugWindow.DrawBitmap(withCorners); // } // } //} var hm = CreateHomography(cameraCorners.ToArray(), projectorCorners.ToArray()); var globals = GenerateCheckerBoard(cameraPattern, checkerboardSize, 0); var globalCorners = cacalibdata.Select(row => globals).ToArray(); var proj = cacalibdata.Select(cs => cs.Select(c => new PointF(c.X, c.Y)).ToArray()).ToArray(); foreach (var p in proj) hm.ProjectPoints(p); IntrinsicCameraParameters projIntrin = new IntrinsicCameraParameters(); ExtrinsicCameraParameters[] projExtrins; Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, proj, projector.Size, projIntrin, Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_RATIONAL_MODEL, out projExtrins); return new CalibrationResult() { Intrinsic = projIntrin, Extrinsic = projExtrins.First() }; }
public static CalibrationResult CalibrateProjector(Projector projector, PointF[][] cacalibdata, PointF[][] cameraCorners, PointF[][] projectorCorners, Size cameraPattern, float checkerboardSize) { var hm = CreateHomography(cameraCorners, projectorCorners); var globals = GenerateCheckerBoard(cameraPattern, checkerboardSize, 0); var globalCorners = cacalibdata.Select(row => globals).ToArray(); var proj = cacalibdata.Select(cs => cs.Select(c => new PointF(c.X, c.Y)).ToArray()).ToArray(); foreach (var p in proj) hm.ProjectPoints(p); IntrinsicCameraParameters projIntrin = new IntrinsicCameraParameters(); ExtrinsicCameraParameters[] projExtrins; Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, proj, projector.Size, projIntrin, Emgu.CV.CvEnum.CALIB_TYPE.CV_CALIB_ZERO_TANGENT_DIST | CALIB_TYPE.CV_CALIB_FIX_K3, out projExtrins); return new CalibrationResult() { Intrinsic = projIntrin, Extrinsic = projExtrins.First() }; }
public static CalibrationResult CalibrateCamera(PointF[][] cameraCorners, Size cameraSize, Size pattern, float checkerBoardSize) { var globals = GenerateCheckerBoard(pattern, checkerBoardSize, 0); var globalCorners = cameraCorners.Select(row => globals).ToArray(); var intrinsic = new IntrinsicCameraParameters(); ExtrinsicCameraParameters[] cameraExtrinsicsArray; Emgu.CV.CameraCalibration.CalibrateCamera(globalCorners, cameraCorners, cameraSize, intrinsic, CALIB_TYPE.CV_CALIB_RATIONAL_MODEL, out cameraExtrinsicsArray); var extrinsic = cameraExtrinsicsArray.First(); return new CalibrationResult() { Intrinsic = intrinsic, Extrinsic = extrinsic }; }
public static int RenderMap(Rectangle clip, Graphics g, bool dedicated, ref float scale) { var ets2Tel = (Main.Data.Active == null) ? default(Ets2Telemetry) : ((Ets2DataMiner)Main.Data.Active).MyTelemetry; // Search the map var map = FrmMain.Ets2Map; g.FillRectangle(map.Loading ? Brushes.DarkOrange : Brushes.Black, clip); g.SmoothingMode = mapScale < 1000 ? SmoothingMode.AntiAlias : SmoothingMode.HighSpeed; g.InterpolationMode = InterpolationMode.NearestNeighbor; g.PixelOffsetMode = PixelOffsetMode.None; float tx = 0.0f; float ty = 0.0f; float baseScale = 0.0f; if (Main.Data.Active == null || (dedicated && locationOverride)) { tx = location.X; ty = location.Y; baseScale = mapScale; } else { var d = GetLivePoint(); tx = d.Item1.X; ty = d.Item1.Y; baseScale = d.Item2; } var totalX = 0.0f; var totalY = 0.0f; // Input scale value is max scale var maxScale = scale; if (baseScale > maxScale) baseScale = scale; scale = baseScale; if (clip.Width > clip.Height) { totalX = baseScale; totalY = (int)(baseScale * (float)clip.Height / clip.Width); } else { totalY = baseScale; totalX = (int)(baseScale * (float)clip.Width / clip.Height); } var startX = clip.X + tx - totalX; var endX = clip.X + tx + totalX; var startY = clip.Y + ty - totalY; var endY = clip.Y + ty + totalY; var scaleX = clip.Width / (endX - startX); var scaleY = clip.Height / (endY - startY); if (float.IsInfinity(scaleX) || float.IsNaN(scaleX)) scaleX = clip.Width; if (float.IsInfinity(scaleY) || float.IsNaN(scaleY)) scaleY = clip.Height; var nodesNearby = map.Nodes.Values.Where( x => x.X >= startX - 1500 && x.X <= endX + 1500 && x.Z >= startY -1500 && x.Z <= endY + 1500); var itemsNearby = nodesNearby.SelectMany(x => x.GetItems()).Where(x => x.HideUI == false).ToList(); var roads = itemsNearby.Where(x => x.Type == Ets2ItemType.Road); var prefabs = itemsNearby.Where(x => x.Type == Ets2ItemType.Prefab); var gpsPen = new Pen(Brushes.MediumPurple, 22*scaleX); var localPen = new Pen(Brushes.Orange, 7.5f * scaleX); var prefabLane = new Pen(Brushes.Yellow, 3 * scaleX); var expressPen = new Pen(Brushes.Yellow, 19 * scaleX); var highwayPen = new Pen(Brushes.Red, 22*scaleX); List<ulong> nodesPassed = new List<ulong>(); List<List<PointF>> roadPoints = new List<List<PointF>>(); var nodesToFollow = prefabs.SelectMany(x => x.NodesList.Values).Distinct(); // Gather all prefabs, and issue a drawing command foreach (var node in nodesToFollow) { if (node == null) continue; bool isHighway = false; bool isExpress = false; bool isLocal = false; // Nodes from prefab are always like: // Prefab = Forward // Road=backward var road = node.ForwardItem != null && node.ForwardItem.Type == Ets2ItemType.Prefab ? node.BackwardItem : node.ForwardItem; var roadStart = road; isHighway = road == null || road.RoadLook == null ? false : road.RoadLook.IsHighway; isExpress = road == null || road.RoadLook == null ? false : road.RoadLook.IsExpress; isLocal = road == null || road.RoadLook == null ? false : road.RoadLook.IsLocal; var fw = node.ForwardItem != null && node.ForwardItem.Type == Ets2ItemType.Road; if (road == null) { // DEAD END //Console.WriteLine("Dead-end from prefab.."); continue; } var roadChain = new List<Ets2Item>(); // Start drawing at start road if (fw) { do { roadChain.Add(road); road = road.EndNode == null ? null : road.EndNode.ForwardItem; } while (road != null && road.Type == Ets2ItemType.Road); } else { do { roadChain.Add(road); road = road.StartNode == null ? null : road.StartNode.BackwardItem; } while (road != null && road.Type == Ets2ItemType.Road); } if (!fw) roadChain.Reverse(); foreach (var n in roadChain.Where(x => x.HideUI == false)) { n.GenerateRoadPolygon(64); } var pen = isHighway ? highwayPen : isExpress ? expressPen : localPen; var roadPoly = roadChain.Where(x => x.HideUI == false) .SelectMany(x => x.RoadPolygons) .Select(x => new PointF((x.X - startX)*scaleX, (x.Y - startY)*scaleY)); if (roadPoly.Any()) { g.DrawLines(pen, roadPoly.ToArray()); } } // Draw GPS routes if any if (route != null && route.Segments != null && route.Segments.Any()) { foreach (var seg in route.Segments) { if (seg.Solutions.Any()) { foreach (var opt in seg.Solutions) { var pt = opt.Points.Select(x => new PointF((x.X - startX) * scaleX, (x.Z - startY) * scaleY)); g.DrawLines(new Pen(Color.SpringGreen, 5.0f), pt.ToArray()); } } else { foreach (var opt in seg.Options) { var pt = opt.Points.Select(x => new PointF((x.X - startX) * scaleX, (x.Z - startY) * scaleY)); g.DrawLines(new Pen(Color.LightSkyBlue, 5.0f), pt.ToArray()); } } } } if (LaneAssistance.hook != null) { var d = new PointF((LaneAssistance.hook.X - startX) * scaleX, (LaneAssistance.hook.Z - startY) * scaleY); g.FillEllipse(Brushes.GreenYellow, d.X - 5, d.Y - 5, 10, 10); var d2 = new PointF(d.X + (float)Math.Sin(LaneAssistance.yawRoad) * 25, d.Y + (float)Math.Cos(LaneAssistance.yawRoad) * 25); g.DrawLine(new Pen(Color.GreenYellow, 3.0f), d, d2); } if (LaneAssistance.lookPoint != null) { var d = new PointF((LaneAssistance.lookPoint.X - startX) * scaleX, (LaneAssistance.lookPoint.Y - startY) * scaleY); g.FillEllipse(Brushes.Pink, d.X - 5, d.Y - 5, 10, 10); } // Cities? var cityFont = new Font("Arial", 10.0f); foreach (var cities in itemsNearby.Where(x => x.Type == Ets2ItemType.City && x.StartNode!=null)) { var centerX = cities.StartNode.X; var centerY = cities.StartNode.Z; var mapX = (centerX - startX)*scaleX; var mapY = (centerY - startY)*scaleY; // g.DrawString(cities.City, cityFont, Brushes.White, mapX,mapY); } // Draw all prefab curves foreach (var prefabItem in prefabs.Where(x => x.Prefab != null && x.HideUI == false).Distinct()) { if (prefabItem.Prefab.Company != null) { var nx = prefabItem.NodesList.FirstOrDefault().Value.X; var ny = prefabItem.NodesList.FirstOrDefault().Value.Z; var companyRect = new PointF[] { new PointF(nx - prefabItem.Prefab.Company.MinX, ny - prefabItem.Prefab.Company.MinY), new PointF(nx - prefabItem.Prefab.Company.MinX, ny + prefabItem.Prefab.Company.MaxY), new PointF(nx + prefabItem.Prefab.Company.MaxX, ny + prefabItem.Prefab.Company.MaxY), new PointF(nx + prefabItem.Prefab.Company.MaxX, ny - prefabItem.Prefab.Company.MinY), }; var offsetPoly = companyRect.Select(x => new PointF((x.X - startX) * scaleX, (x.Y - startY) * scaleY)).ToArray(); //g.FillPolygon(Brushes.Orange, offsetPoly); } else { // Then it's likely a road prefab. //prefab.Origin = 0; // TODO: find origin var originNode = prefabItem.NodesList.FirstOrDefault().Value; if (originNode != null) { foreach ( var poly in prefabItem.Prefab.GeneratePolygonCurves(originNode, prefabItem.Origin)) { var offsetPoly = poly.Select(x => new PointF((x.X - startX) * scaleX, (x.Y - startY) * scaleY)).ToArray(); var p = new Pen(prefabLane.Color, 1.0f); g.DrawLines(p, offsetPoly); } } } } var rotation = ets2Tel == null ? 0 : ets2Tel.Physics.RotationX*2*Math.PI; //g.FillEllipse(Brushes.Turquoise, scaleX*totalX-5, scaleY*totalY-5,10,10); g.DrawLine(new Pen(Brushes.Cyan, 5.0f), scaleX*totalX + (float) Math.Sin(rotation)*localPen.Width*2, scaleY*totalY + (float) Math.Cos(rotation)*localPen.Width*2, scaleX*totalX, scaleY*totalY); if (dedicated && ProcessDoubleClick) { ProcessDoubleClick = false; // Calculate the coordinate var clkX = 2 * (-0.5f + DoubleClickPoint.X / (float)clip.Width) * totalX + tx; var clkY = 2 * (-0.5f + DoubleClickPoint.Y / (float)clip.Height) * totalY + ty; var currentLocation = GetLivePoint(); // Navigate to this point route = map.NavigateTo(currentLocation.Item1, new PointF(clkX, clkY)); } // At 50 speed: 20fps // At 100 speed: 2fps var fps = 15.0f; if (ets2Tel != null && ets2Tel.Drivetrain.SpeedKmh > 50) fps -= (ets2Tel.Drivetrain.SpeedKmh - 50) * 0.43f; // 2.777spd = -1fps if (fps < 2) fps = 2; var interval = 1000.0f/fps; return (int) interval; }
public OctavePlot(PointF[] points) { dataX = Embrace(String.Join(",", points.Select(d => d.X.ToString(cul)))); dataY = Embrace(String.Join(",", points.Select(d => d.Y.ToString(cul)))); }