private void initThred() { int RobotID = 0; for (int i = 0; i < robotStartPos.Count; i++) { SimRVO3 sim = new SimRVO3(robotEndPos[i], RobotID, obst); rvo.Add(sim); State state = new State(); state.location = robotStartPos[i]; state.velocity = new Vector2(0, 0); state.robotId = RobotID; AllRobotStates.TryAdd(RobotID, state); RobotID++; thr.Add(new Thread(() => run(sim))); } working = true; }
private void run(SimRVO3 sim) { int robotID = sim.RobotID; int loop = 0; while (working) { if (loop > 13) { int llll = 00; } State currentState = AllRobotStates[robotID]; List <State> AllState = new List <State>(); foreach (var item in AllRobotStates.Values) { AllState.Add(item); } Vector2 currentVelocity = sim.compute(AllState, currentState.location); currentState.location += currentVelocity * sim.timeStep; currentState.velocity = currentVelocity; Console.WriteLine(string.Format("{3}&{0}&{1}&{2}&{4}&{5}&", robotID, currentVelocity.x(), currentVelocity.y(), loop, currentState.location.x(), currentState.location.y())); if (sim.IsCollide()) { int i = 4343; } loop++; System.Threading.Thread.Sleep(50); // System.Threading.Thread.Sleep(500); } }