public static GridObstacleState FromObstacleState(ObstacleState obst, GridCarModelState state)
{
double d = ComMath.Normal(Math.Sqrt(obst.pp.position.X * obst.pp.position.X + obst.pp.position.Y * obst.pp.position.Y), GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1);
double a = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, 1);
double ang = Math.PI - (Math.Atan2(obst.pp.position.Y, obst.pp.position.X) + Math.PI) + state.TargetAngle - state.TargetFinishAngle;
if (ang > Math.PI) ang -= 2 * Math.PI;
if (ang < -Math.PI) ang += 2 * Math.PI;
double AA = -2 * d * Math.Cos(ang);
double BB = d * d;
double obstdist = Math.Sqrt(a * a + BB + AA * a);
double obstang = state.TargetAngle + Math.Sign(ang) * Math.Acos((a * a + obstdist * obstdist - d * d) / (2 * a * obstdist));
GridObstacleState gos = new GridObstacleState(obstdist, obstang, obst.radius);
return gos;
}
public ObstacleModel(Point positionInPixel, double radiusInPixel)
{
state = new ObstacleState();
SetPosition(positionInPixel,false);
state.radius = radiusInPixel * CarModel.MM_PER_PIXEL;
}
public ObstacleModel(PointD position, double radius)
{
state = new ObstacleState(position, radius);
}
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 ObstacleModel(Point positionInPixel, double radiusInPixel)
{
state = new ObstacleState();
SetPosition(positionInPixel, false);
state.radius = radiusInPixel * CarModel.MM_PER_PIXEL;
}
public ObstacleModel(PointD position, double radius)
{
state = new ObstacleState(position, radius);
}
