private void timer1_Tick(object sender, EventArgs e)
{
Bitmap frame = GenerateImage();
pictureBox1.Image = frame;
List <Shape> shapes = mf.ProcessFrame(frame);
Graphics g = Graphics.FromImage(frame);
foreach (Shape shape in shapes)
{
Matrix m = g.Transform;
g.TranslateTransform(shape.pos.X, shape.pos.Y);
g.RotateTransform((float)(-shape.rot * 180 / Math.PI));
g.ScaleTransform((float)(shape.scale * 0.75), (float)(shape.scale * 0.75));
g.Transform = m;
g.TranslateTransform(shape.pos.X, shape.pos.Y);
g.RotateTransform((float)(-shape.rot * 180 / Math.PI));
g.ScaleTransform((float)(shape.scale * 1.5), (float)(shape.scale * 1.5));
g.DrawPolygon(new Pen(Color.Red), shape.contour.ToArray());
g.Transform = m;
}
g.Flush();
g = Graphics.FromImage(contour);
g.TranslateTransform(50, 50);
g.DrawPolygon(new Pen(Color.Red), shapes[0].contour.ToArray());
g.Flush();
pictureBox2.Image = contour;
this.Invalidate();
StringBuilder line = new StringBuilder();
foreach (Point p in shapes[0].contour)
{
line.Append(p.X);
line.Append(",");
line.Append(p.Y);
line.Append(",");
}
line.Append("\r\n");
byte[] data = Encoding.ASCII.GetBytes(line.ToString());
f.Write(data, 0, data.Length);
}
private void RunMarkerFinder()
{
if (mf != null)
{
Bitmap frame = camera.GetBitmap();
List <Shape> shapes = null;
shapes = mf.ProcessFrame(frame);
bool carFound = false;
bool finishFound = false;
// finish
currentObstacleStates = new List <ObstacleState>();
foreach (Shape s in shapes)
{
if ((s.index == 0) && (s.scale > 0.17) && (s.scale < 0.22))
{
currentCarModelState = new CarModelState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
-Math.PI / 2 - s.rot);
carFound = true;
}
else if ((s.index == 1) && (s.scale > 0.16) && (s.scale < 0.20))
{
currentFinishState = new FinishState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
-Math.PI / 2 - s.rot);
finishPredictor.AddPoint(currentFinishState.Position, currentFinishState.Orientation);
finishFound = true;
}
else
{
currentObstacleStates.Add(new ObstacleState(new PointD(ComMath.Normal(s.pos.X, 0, frame.Width, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X),
ComMath.Normal(s.pos.Y, 0, frame.Height, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y)),
s.scale * 500));
//ide kell egy robusztus alg
//ha zaj jon, akkor ne vegye be
}
}
lock (obstacles)
{
List <ObstacleState> curObState = new List <ObstacleState>(currentObstacleStates);
foreach (ObstacleModel ops in obstacles)
{
List <PointD> list = ops.state.pp.PredictNextPositions(1);
ObstacleState osw = null;
PointD pd;
if (list != null)
{
pd = list[0];
}
else
{
pd = ops.state.pp.position;
}
double mindist = double.MaxValue;
foreach (ObstacleState os in curObState)
{
double dist = (os.pp.position.X - pd.X) * (os.pp.position.X - pd.X) + (os.pp.position.Y - pd.Y) * (os.pp.position.Y - pd.Y);
if (dist < mindist)
{
mindist = dist;
osw = os;
}
}
if (osw != null)
{
lock (ops)
{
ops.state.pp.AddNewPosition(osw.pp.position);
}
curObState.Remove(osw);
}
else
{
//a predikcioval leptetjuk tovabb
lock (ops)
{
ops.state.pp.AddNewPosition(pd);
}
}
}
foreach (ObstacleState os in curObState)
{
lock (obstacles)
{
ObstacleModel om = new ObstacleModel(os.pp.position, os.radius);
om.SetSelectedState(1, 0);
if (obstacles.Count < 1)
{
obstacles.Add(om);
}
}
}
}
}
}