text: /*
   * RVOSimulator.cpp
   * RVO2 Library
   *
   * Copyright 2008 University of North Carolina at Chapel Hill
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   *     http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   *
   * Please send all bug reports to <geom@cs.unc.edu>.
   *
   * The authors may be contacted via:
   *
   * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
   * Dept. of Computer Science
   * 201 S. Columbia St.
   * Frederick P. Brooks, Jr. Computer Science Bldg.
   * Chapel Hill, N.C. 27599-3175
   * United States of America
   *
   * <http://gamma.cs.unc.edu/RVO2/>
   */
  
  #include "RVOSimulator.h"
  
  #include "Agent.h"
  #include "KdTree.h"
  #include "Obstacle.h"
  
  #ifdef _OPENMP
  #include <omp.h>
  #endif
  
  namespace RVO {
      RVOSimulator::RVOSimulator() : defaultAgent_(NULL), globalTime_(0.0), kdTree_(NULL), timeStep_(0.0)
      {
          kdTree_ = new KdTree(this);
      }
  
      RVOSimulator::RVOSimulator(double timeStep, double neighborDist, size_t maxNeighbors, double timeHorizon, double timeHorizonObst, double radius, double maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0), kdTree_(NULL), timeStep_(timeStep)
      {
          kdTree_ = new KdTree(this);
          defaultAgent_ = new Agent(this);
  
          defaultAgent_->maxNeighbors_ = maxNeighbors;
          defaultAgent_->maxSpeed_ = maxSpeed;
          defaultAgent_->neighborDist_ = neighborDist;
          defaultAgent_->radius_ = radius;
          defaultAgent_->timeHorizon_ = timeHorizon;
          defaultAgent_->timeHorizonObst_ = timeHorizonObst;
          defaultAgent_->velocity_ = velocity;
      }
  
      RVOSimulator::~RVOSimulator()
      {
          if (defaultAgent_ != NULL) {
              delete defaultAgent_;
          }
  
          for (size_t i = 0; i < agents_.size(); ++i) {
              delete agents_[i];
          }
  
          for (size_t i = 0; i < obstacles_.size(); ++i) {
              delete obstacles_[i];
          }
  
          delete kdTree_;
      }
  
      size_t RVOSimulator::addAgent(const Vector2 &position)
      {
          if (defaultAgent_ == NULL) {
              return RVO_ERROR;
          }
          size_t agentNo = RVO_ERROR;
          Agent *agent = nullptr;
          if (freeAgents_.empty())
          {
              agent = new Agent(this);
              agent->id_ = agents_.size();
              agentNo = agents_.size();
              agents_.push_back(agent);
          }
          else
          {
              agentNo = freeAgents_.back();
              freeAgents_.pop_back();
              agent = agents_.at(agentNo);
          }
          
          agent->position_ = position;
          agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
          agent->maxSpeed_ = defaultAgent_->maxSpeed_;
          agent->neighborDist_ = defaultAgent_->neighborDist_;
          agent->radius_ = defaultAgent_->radius_;
          agent->timeHorizon_ = defaultAgent_->timeHorizon_;
          agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_;
          agent->velocity_ = defaultAgent_->velocity_;
  
          return agentNo;
      }
  
      size_t RVOSimulator::addAgent(const Vector2 &position, double neighborDist, size_t maxNeighbors, double timeHorizon, double timeHorizonObst, double radius, double maxSpeed, const Vector2 &velocity)
      {
          size_t agentNo = RVO_ERROR;
          Agent *agent = nullptr;
          if (freeAgents_.empty())
          {
              agent = new Agent(this);
              agent->id_ = agents_.size();
              agentNo = agents_.size();
              agents_.push_back(agent);
          }
          else
          {
              agentNo = freeAgents_.back();
              freeAgents_.pop_back();
              agent = agents_.at(agentNo);
          }
  
  
  
          agent->position_ = position;
          agent->maxNeighbors_ = maxNeighbors;
          agent->maxSpeed_ = maxSpeed;
          agent->neighborDist_ = neighborDist;
          agent->radius_ = radius;
          agent->timeHorizon_ = timeHorizon;
          agent->timeHorizonObst_ = timeHorizonObst;
          agent->velocity_ = velocity;
  
          return agentNo;
      }
      void RVOSimulator::removeAgent(size_t agentNo)
      {
          if (agentNo < agents_.size())
          {
              if (std::find_if(freeAgents_.begin(), freeAgents_.end(), [agentNo](size_t No) {return No == agentNo; }) != freeAgents_.end())
              {
                  return;
              }
              freeAgents_.push_back(agentNo);
              agents_.at(agentNo)->position_ = Vector2(1E120*agentNo, 1E120*agentNo);
              agents_.at(agentNo)->prefVelocity_ = Vector2(0, 0);
          }
      }
  
      size_t RVOSimulator::addObstacle(const std::vector<Vector2> &vertices)
      {
          if (vertices.size() < 2) {
              return RVO_ERROR;
          }
  
          const size_t obstacleNo = obstacles_.size();
  
          for (size_t i = 0; i < vertices.size(); ++i) {
              Obstacle *obstacle = new Obstacle();
              obstacle->point_ = vertices[i];
  
              if (i != 0) {
                  obstacle->prevObstacle_ = obstacles_.back();
                  obstacle->prevObstacle_->nextObstacle_ = obstacle;
              }
  
              if (i == vertices.size() - 1) {
                  obstacle->nextObstacle_ = obstacles_[obstacleNo];
                  obstacle->nextObstacle_->prevObstacle_ = obstacle;
              }
  
              obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]);
  
              if (vertices.size() == 2) {
                  obstacle->isConvex_ = true;
              }
              else {
                  obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0);
              }
  
              obstacle->id_ = obstacles_.size();
  
              obstacles_.push_back(obstacle);
          }
  
          return obstacleNo;
      }
      void RVOSimulator::cleanObstacle()
      {
          for (size_t i = 0; i < obstacles_.size(); ++i) {
              delete obstacles_[i];
          }
          obstacles_.clear();
      }
      void RVOSimulator::doStep()
      {
          kdTree_->buildAgentTree();
  
  #ifdef _OPENMP
  #pragma omp parallel for
  #endif
          for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
              agents_[i]->computeNeighbors();
              agents_[i]->computeNewVelocity();
          }
  
  #ifdef _OPENMP
  #pragma omp parallel for
  #endif
          for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
              agents_[i]->update();
          }
  
          globalTime_ += timeStep_;
      }
  
      size_t RVOSimulator::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
      {
          return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
      }
  
      size_t RVOSimulator::getAgentMaxNeighbors(size_t agentNo) const
      {
          return agents_[agentNo]->maxNeighbors_;
      }
  
      double RVOSimulator::getAgentMaxSpeed(size_t agentNo) const
      {
          return agents_[agentNo]->maxSpeed_;
      }
  
      double RVOSimulator::getAgentNeighborDist(size_t agentNo) const
      {
          return agents_[agentNo]->neighborDist_;
      }
  
      size_t RVOSimulator::getAgentNumAgentNeighbors(size_t agentNo) const
      {
          return agents_[agentNo]->agentNeighbors_.size();
      }
  
      size_t RVOSimulator::getAgentNumObstacleNeighbors(size_t agentNo) const
      {
          return agents_[agentNo]->obstacleNeighbors_.size();
      }
  
      size_t RVOSimulator::getAgentNumORCALines(size_t agentNo) const
      {
          return agents_[agentNo]->orcaLines_.size();
      }
  
      size_t RVOSimulator::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const
      {
          return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_;
      }
  
      const Line &RVOSimulator::getAgentORCALine(size_t agentNo, size_t lineNo) const
      {
          return agents_[agentNo]->orcaLines_[lineNo];
      }
  
      const Vector2 &RVOSimulator::getAgentPosition(size_t agentNo) const
      {
          return agents_[agentNo]->position_;
      }
  
      const Vector2 &RVOSimulator::getAgentPrefVelocity(size_t agentNo) const
      {
          return agents_[agentNo]->prefVelocity_;
      }
  
      double RVOSimulator::getAgentRadius(size_t agentNo) const
      {
          return agents_[agentNo]->radius_;
      }
  
      double RVOSimulator::getAgentTimeHorizon(size_t agentNo) const
      {
          return agents_[agentNo]->timeHorizon_;
      }
  
      double RVOSimulator::getAgentTimeHorizonObst(size_t agentNo) const
      {
          return agents_[agentNo]->timeHorizonObst_;
      }
  
      const Vector2 &RVOSimulator::getAgentVelocity(size_t agentNo) const
      {
          return agents_[agentNo]->velocity_;
      }
  
      double RVOSimulator::getGlobalTime() const
      {
          return globalTime_;
      }
  
      size_t RVOSimulator::getNumAgents() const
      {
          return agents_.size();
      }
  
      size_t RVOSimulator::getNumObstacleVertices() const
      {
          return obstacles_.size();
      }
  
      const Vector2 &RVOSimulator::getObstacleVertex(size_t vertexNo) const
      {
          return obstacles_[vertexNo]->point_;
      }
  
      size_t RVOSimulator::getNextObstacleVertexNo(size_t vertexNo) const
      {
          return obstacles_[vertexNo]->nextObstacle_->id_;
      }
  
      size_t RVOSimulator::getPrevObstacleVertexNo(size_t vertexNo) const
      {
          return obstacles_[vertexNo]->prevObstacle_->id_;
      }
  
      double RVOSimulator::getTimeStep() const
      {
          return timeStep_;
      }
  
      void RVOSimulator::processObstacles()
      {
          kdTree_->buildObstacleTree();
      }
  
      bool RVOSimulator::queryVisibility(const Vector2 &point1, const Vector2 &point2, double radius) const
      {
          return kdTree_->queryVisibility(point1, point2, radius);
      }
  
      void RVOSimulator::setAgentDefaults(double neighborDist, size_t maxNeighbors, double timeHorizon, double timeHorizonObst, double radius, double maxSpeed, const Vector2 &velocity)
      {
          if (defaultAgent_ == NULL) {
              defaultAgent_ = new Agent(this);
          }
  
          defaultAgent_->maxNeighbors_ = maxNeighbors;
          defaultAgent_->maxSpeed_ = maxSpeed;
          defaultAgent_->neighborDist_ = neighborDist;
          defaultAgent_->radius_ = radius;
          defaultAgent_->timeHorizon_ = timeHorizon;
          defaultAgent_->timeHorizonObst_ = timeHorizonObst;
          defaultAgent_->velocity_ = velocity;
      }
  
      void RVOSimulator::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
      {
          agents_[agentNo]->maxNeighbors_ = maxNeighbors;
      }
  
      void RVOSimulator::setAgentMaxSpeed(size_t agentNo, double maxSpeed)
      {
          agents_[agentNo]->maxSpeed_ = maxSpeed;
      }
  
      void RVOSimulator::setAgentNeighborDist(size_t agentNo, double neighborDist)
      {
          agents_[agentNo]->neighborDist_ = neighborDist;
      }
  
      void RVOSimulator::setAgentPosition(size_t agentNo, const Vector2 &position)
      {
          agents_[agentNo]->position_ = position;
      }
  
      void RVOSimulator::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity)
      {
          agents_[agentNo]->prefVelocity_ = prefVelocity;
      }
      double RVOSimulator::getAgentRadius(size_t agentNo)
      {
          return agents_[agentNo]->radius_;
      }
  
      void RVOSimulator::setAgentRadius(size_t agentNo, double radius)
      {
          agents_[agentNo]->radius_ = radius;
      }
  
      void RVOSimulator::setAgentTimeHorizon(size_t agentNo, double timeHorizon)
      {
          agents_[agentNo]->timeHorizon_ = timeHorizon;
      }
  
      void RVOSimulator::setAgentTimeHorizonObst(size_t agentNo, double timeHorizonObst)
      {
          agents_[agentNo]->timeHorizonObst_ = timeHorizonObst;
      }
  
      void RVOSimulator::setAgentVelocity(size_t agentNo, const Vector2 &velocity)
      {
          agents_[agentNo]->velocity_ = velocity;
      }
  
      void RVOSimulator::setTimeStep(double timeStep)
      {
          timeStep_ = timeStep;
      }
  }
  