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);
}
}
}
}
}
public void newObstacle(object sender, EventArgs e)
{
ObstacleModel om = new ObstacleModel(contextMenuLocation, 30);
lock (obstacles)
{
obstacles.Add(om);
}
dragables.Add(om);
}
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);
}
}
}
}
}
}