private void runRobot(int id)
{
CollisionFreeVelocityGenerator col = robots[id];
double distance = double.MaxValue;
addObstacle(col);
while (distance > 0.01)
{
State currentRobotState = AllRobotStates[id];
Vector2 currentVelocity;
Vector2 gole = goals[id];
currentVelocity = getPreferredVelocities(id, currentRobotState.location);
currentVelocity = col.GetVelocityCollisionFree(currentRobotState.location, currentVelocity);
currentRobotState.location += currentVelocity * col.timeStep;
currentRobotState.velocity = currentVelocity;
System.Threading.Thread.Sleep(timeStamp);
// System.Console.WriteLine("R: {0} X: {1} Y: {2}",id, currentRobotState.location.x(), currentRobotState.location.y());
// System.Console.WriteLine("R: {0} X: {1} Y: {2}", id, currentVelocity.x(), currentVelocity.y());
distance = getDistance(gole, currentRobotState.location);
}
}
private void addObstacle(CollisionFreeVelocityGenerator col)
{
List <IList <Vector2> > obst = GetObstacles();
foreach (var item in obst)
{
col.addObstacle(item);
}
}
private void createRobot(int id, Vector2 location, Vector2 gole)
{
Thread thr;
CollisionFreeVelocityGenerator col;
int robotId;
State state;
robotId = id;
state = new State();
state.location = location;
state.velocity = new Vector2(0, 0);
state.robotId = id;
AllRobotStates.TryAdd(robotId, state);
col = new CollisionFreeVelocityGenerator(robotId);
thr = new Thread(() => runRobot(robotId));
robots.TryAdd(robotId, col);
robotThr.Add(thr);
goals.Add(gole);
}
