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