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