public Vector2 GetVelocityCollisionFree(Vector2 currentlocation, Vector2 currentVelocity) //
{
IList <Agent> tmpAgents = getAgents(); //
currentAgent = createAgent(robotId);
if (tmpAgents.Count > 0)
{
float rangeSq = RVOMath.sqr(timeHorizonObst * maxSpeed + radius);
IList <Obstacle> tmpObstacle = getObstacles(currentlocation, rangeSq);
currentAgent.position_ = currentlocation; //
currentAgent.prefVelocity_ = currentVelocity; //
kdTree_ = new KdTree();
kdTree_.buildAgentTree(tmpAgents);
currentAgent.computeNeighbors(kdTree_, tmpObstacle); //
currentAgent.computeNewVelocity(); //
return(currentAgent.newVelocity_); //
}
else
{
return(new Vector2(0, 0));
}
}
/**
* <summary>Performs a simulation step and updates the two-dimensional
* position and two-dimensional velocity of each agent.</summary>
*
* <returns>The global time after the simulation step.</returns>
*/
public KInt doStep()
{
if (isError)
{
return(globalTime_);
}
try
{
updateDeleteAgent();
if (!singletonMode)
{
if (workers_ == null)
{
workers_ = new Worker[numWorkers_];
doneEvents_ = new ManualResetEvent[workers_.Length];
workerAgentCount_ = getNumAgents();
for (int block = 0; block < workers_.Length; ++block)
{
doneEvents_[block] = new ManualResetEvent(false);
workers_[block] = new Worker(block * getNumAgents() / workers_.Length, (block + 1) * getNumAgents() / workers_.Length, doneEvents_[block]);
}
}
if (workerAgentCount_ != getNumAgents())
{
workerAgentCount_ = getNumAgents();
for (int block = 0; block < workers_.Length; ++block)
{
workers_[block].config(block * getNumAgents() / workers_.Length, (block + 1) * getNumAgents() / workers_.Length);
}
}
}
kdTree_.buildAgentTree();
if (!singletonMode)
{
for (int block = 0; block < workers_.Length; ++block)
{
doneEvents_[block].Reset();
ThreadPool.QueueUserWorkItem(workers_[block].step);
}
WaitHandle.WaitAll(doneEvents_);
for (int block = 0; block < workers_.Length; ++block)
{
doneEvents_[block].Reset();
ThreadPool.QueueUserWorkItem(workers_[block].update);
}
WaitHandle.WaitAll(doneEvents_);
}
else
{
for (int index = 0; index < getNumAgents(); ++index)
{
agents_[index].computeNeighbors();
agents_[index].computeNewVelocity();
}
for (int index = 0; index < getNumAgents(); ++index)
{
agents_[index].update();
}
}
globalTime_ += timeStep_;
return(globalTime_);
}
catch (Exception ex)
{
//LogMgr.LogError(ex);
this.isError = true;
return(globalTime_);
}
}