private static double[] obstacleFieldErrorGradient(IObstaclePositionProvider ops, GridCarModelState state, int time)
{
//C = sum((1/d(X) - 1/d(0))^2)
//dC/dy_x =...
//dC/dy_y =...
double ksi = 0.1;
double disterr = 0;
double orxerr = 0;
double oryerr = 0;
List <ObstacleState> obstacles = ops.GetObstacleStates(0);
foreach (ObstacleState obst in obstacles)//cel az origo, tehat az origohoz relativak az akadalyok, origo felfele nez
{
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, MAX_NEURON_VALUE);
double a = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE);
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));
double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE);
double dist = obstdist - r;
if (dist <= 0.0001)
{
dist = 0.0001;
}
double err = 1 / (2 * (dist * dist * dist));
disterr += -(AA + 2 * a) * err;
double angerr = (-2 * a * d * Math.Sin(ang)) * err;
orxerr += -Math.Sin(state.TargetAngle) * angerr;
oryerr += Math.Cos(state.TargetAngle) * angerr;
}
if (obstacles.Count > 0)
{
disterr /= obstacles.Count;
orxerr /= obstacles.Count;
oryerr /= obstacles.Count;
}
return(new double[] { -ksi * disterr, -ksi * orxerr, -ksi * oryerr });
}
//private double obstacleFieldError(GridCarModelState state)
//{
// double err = 0;
// List<ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1);
// foreach (ObstacleState obst in obstacles)
// {
// double d = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
// double x = Math.Cos(state.TargetAngle - state.TargetFinishAngle) * d;
// double y = Math.Sin(state.TargetAngle - state.TargetFinishAngle) * d;
// double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
// double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
// double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0));
// err += Math.Pow(1 / dist - 1 / obst.radius, 2);
// }
// return err;
//}
private static PolarGrid obstaclePolarGrid(IObstaclePositionProvider ops, GridCarModelState state)
{
List <ObstacleState> obstacles = ops.GetObstacleStates(0);
PolarGrid pg = new PolarGrid();
foreach (ObstacleState obst in obstacles)
{
GridObstacleState gos = GridObstacleState.FromObstacleState(obst, state);
pg.AddObstacle(gos);
}
return(pg);
}
private double[] obstacleFieldErrorGradient(CarModelState state, int time)
{
//C = sum((1/d(X) - 1/d(0))^2)
//dC/dy_x =...
//dC/dy_y =...
double ksi = 0.0001;
double errX = 0;
double errY = 0;
List <ObstacleState> obstacles = obstacleProvider.GetObstacleStates(time);
foreach (ObstacleState obst in obstacles)
{
double x = ComMath.Normal(state.Position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double y = ComMath.Normal(state.Position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, CarModelState.MIN_POS_Y, CarModelState.MAX_POS_Y, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0)) - r;
if (dist <= 0.0001)
{
dist = 0.0001;
}
double err = (1 / (dist * dist * dist));
errX += err * (x - x0);
errY += err * (y - y0);
}
if (obstacles.Count > 0)
{
errX /= obstacles.Count;
errY /= obstacles.Count;
}
return(new double[] { ksi *errX, ksi *errY, 0, 0 });
}
private static double[] obstacleFieldErrorGradient(IObstaclePositionProvider ops, GridCarModelState state, int time)
{
//C = sum((1/d(X) - 1/d(0))^2)
//dC/dy_x =...
//dC/dy_y =...
double ksi = 0.1;
double disterr = 0;
double orxerr = 0;
double oryerr = 0;
List<ObstacleState> obstacles = ops.GetObstacleStates(0);
foreach (ObstacleState obst in obstacles)//cel az origo, tehat az origohoz relativak az akadalyok, origo felfele nez
{
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, MAX_NEURON_VALUE);
double a = ComMath.Normal(state.TargetDist , GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE);
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));
double r = ComMath.Normal(obst.radius + CarModel.SHAFT_LENGTH / 2, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, 0, MAX_NEURON_VALUE);
double dist = obstdist - r;
if (dist <= 0.0001) dist = 0.0001;
double err = 1 / (2 * (dist * dist * dist));
disterr += -(AA + 2 * a) * err;
double angerr = (-2 * a * d * Math.Sin(ang)) * err;
orxerr += -Math.Sin(state.TargetAngle) * angerr;
oryerr += Math.Cos(state.TargetAngle) * angerr;
}
if (obstacles.Count > 0)
{
disterr /= obstacles.Count;
orxerr /= obstacles.Count;
oryerr /= obstacles.Count;
}
return new double[] { -ksi * disterr, -ksi * orxerr, -ksi * oryerr };
}
//private double obstacleFieldError(GridCarModelState state)
//{
// double err = 0;
// List<ObstacleState> obstacles = obstacleProvider.GetObstacleStates(1);
// foreach (ObstacleState obst in obstacles)
// {
// double d = ComMath.Normal(state.TargetDist, GridCarModelState.MIN_DIST, GridCarModelState.MAX_DIST, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
// double x = Math.Cos(state.TargetAngle - state.TargetFinishAngle) * d;
// double y = Math.Sin(state.TargetAngle - state.TargetFinishAngle) * d;
// double x0 = ComMath.Normal(obst.pp.position.X, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
// double y0 = ComMath.Normal(obst.pp.position.Y, CarModelState.MIN_POS_X, CarModelState.MAX_POS_X, MIN_NEURON_VALUE, MAX_NEURON_VALUE);
// double dist = Math.Sqrt((x - x0) * (x - x0) + (y - y0) * (y - y0));
// err += Math.Pow(1 / dist - 1 / obst.radius, 2);
// }
// return err;
//}
private static PolarGrid obstaclePolarGrid(IObstaclePositionProvider ops, GridCarModelState state)
{
List<ObstacleState> obstacles = ops.GetObstacleStates(0);
PolarGrid pg = new PolarGrid();
foreach (ObstacleState obst in obstacles)
{
GridObstacleState gos = GridObstacleState.FromObstacleState(obst, state);
pg.AddObstacle(gos);
}
return pg;
}
