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