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