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))));
}
