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);            
 }
Beispiel #3
0
        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);
                            }
                        }
                    }
                }
            }
        }