+/*

+ * RVOSimulator2d.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 "RVOSimulator2d.h"

+

+#include "Agent2d.h"

+#include "KdTree2d.h"

+#include "Obstacle2d.h"

+

+#ifdef _OPENMP

+#include <omp.h>

+#endif

+

+namespace RVO2D {

+ RVOSimulator2D::RVOSimulator2D() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f)

+ {

+ kdTree_ = new KdTree2D(this);

+ }

+

+ RVOSimulator2D::RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(timeStep)

+ {

+ kdTree_ = new KdTree2D(this);

+ defaultAgent_ = new Agent2D();

+

+ defaultAgent_->maxNeighbors_ = maxNeighbors;

+ defaultAgent_->maxSpeed_ = maxSpeed;

+ defaultAgent_->neighborDist_ = neighborDist;

+ defaultAgent_->radius_ = radius;

+ defaultAgent_->timeHorizon_ = timeHorizon;

+ defaultAgent_->timeHorizonObst_ = timeHorizonObst;

+ defaultAgent_->velocity_ = velocity;

+ }

+

+ RVOSimulator2D::~RVOSimulator2D()

+ {

+ 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 RVOSimulator2D::addAgent(const Vector2 &position)

+ {

+ if (defaultAgent_ == NULL) {

+ return RVO2D_ERROR;

+ }

+

+ Agent2D *agent = new Agent2D();

+

+ 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_;

+

+ agent->id_ = agents_.size();

+

+ agents_.push_back(agent);

+

+ return agents_.size() - 1;

+ }

+

+ size_t RVOSimulator2D::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)

+ {

+ Agent2D *agent = new Agent2D();

+

+ agent->position_ = position;

+ agent->maxNeighbors_ = maxNeighbors;

+ agent->maxSpeed_ = maxSpeed;

+ agent->neighborDist_ = neighborDist;

+ agent->radius_ = radius;

+ agent->timeHorizon_ = timeHorizon;

+ agent->timeHorizonObst_ = timeHorizonObst;

+ agent->velocity_ = velocity;

+

+ agent->id_ = agents_.size();

+

+ agents_.push_back(agent);

+

+ return agents_.size() - 1;

+ }

+

+ size_t RVOSimulator2D::addObstacle(const std::vector<Vector2> &vertices)

+ {

+ if (vertices.size() < 2) {

+ return RVO2D_ERROR;

+ }

+

+ const size_t obstacleNo = obstacles_.size();

+

+ for (size_t i = 0; i < vertices.size(); ++i) {

+ Obstacle2D *obstacle = new Obstacle2D();

+ 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.0f);

+ }

+

+ obstacle->id_ = obstacles_.size();

+

+ obstacles_.push_back(obstacle);

+ }

+

+ return obstacleNo;

+ }

+

+ void RVOSimulator2D::doStep()

+ {

+ kdTree_->buildAgentTree(agents_);

+

+ for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {

+ agents_[i]->computeNeighbors(this);

+ agents_[i]->computeNewVelocity(this);

+ }

+

+ for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {

+ agents_[i]->update(this);

+ }

+

+ globalTime_ += timeStep_;

+ }

+

+ size_t RVOSimulator2D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const

+ {

+ return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;

+ }

+

+ size_t RVOSimulator2D::getAgentMaxNeighbors(size_t agentNo) const

+ {

+ return agents_[agentNo]->maxNeighbors_;

+ }

+

+ float RVOSimulator2D::getAgentMaxSpeed(size_t agentNo) const

+ {

+ return agents_[agentNo]->maxSpeed_;

+ }

+

+ float RVOSimulator2D::getAgentNeighborDist(size_t agentNo) const

+ {

+ return agents_[agentNo]->neighborDist_;

+ }

+

+ size_t RVOSimulator2D::getAgentNumAgentNeighbors(size_t agentNo) const

+ {

+ return agents_[agentNo]->agentNeighbors_.size();

+ }

+

+ size_t RVOSimulator2D::getAgentNumObstacleNeighbors(size_t agentNo) const

+ {

+ return agents_[agentNo]->obstacleNeighbors_.size();

+ }

+

+ size_t RVOSimulator2D::getAgentNumORCALines(size_t agentNo) const

+ {

+ return agents_[agentNo]->orcaLines_.size();

+ }

+

+ size_t RVOSimulator2D::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const

+ {

+ return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_;

+ }

+

+ const Line &RVOSimulator2D::getAgentORCALine(size_t agentNo, size_t lineNo) const

+ {

+ return agents_[agentNo]->orcaLines_[lineNo];

+ }

+

+ const Vector2 &RVOSimulator2D::getAgentPosition(size_t agentNo) const

+ {

+ return agents_[agentNo]->position_;

+ }

+

+ const Vector2 &RVOSimulator2D::getAgentPrefVelocity(size_t agentNo) const

+ {

+ return agents_[agentNo]->prefVelocity_;

+ }

+

+ float RVOSimulator2D::getAgentRadius(size_t agentNo) const

+ {

+ return agents_[agentNo]->radius_;

+ }

+

+ float RVOSimulator2D::getAgentTimeHorizon(size_t agentNo) const

+ {

+ return agents_[agentNo]->timeHorizon_;

+ }

+

+ float RVOSimulator2D::getAgentTimeHorizonObst(size_t agentNo) const

+ {

+ return agents_[agentNo]->timeHorizonObst_;

+ }

+

+ const Vector2 &RVOSimulator2D::getAgentVelocity(size_t agentNo) const

+ {

+ return agents_[agentNo]->velocity_;

+ }

+

+ float RVOSimulator2D::getGlobalTime() const

+ {

+ return globalTime_;

+ }

+

+ size_t RVOSimulator2D::getNumAgents() const

+ {

+ return agents_.size();

+ }

+

+ size_t RVOSimulator2D::getNumObstacleVertices() const

+ {

+ return obstacles_.size();

+ }

+

+ const Vector2 &RVOSimulator2D::getObstacleVertex(size_t vertexNo) const

+ {

+ return obstacles_[vertexNo]->point_;

+ }

+

+ size_t RVOSimulator2D::getNextObstacleVertexNo(size_t vertexNo) const

+ {

+ return obstacles_[vertexNo]->nextObstacle_->id_;

+ }

+

+ size_t RVOSimulator2D::getPrevObstacleVertexNo(size_t vertexNo) const

+ {

+ return obstacles_[vertexNo]->prevObstacle_->id_;

+ }

+

+ float RVOSimulator2D::getTimeStep() const

+ {

+ return timeStep_;

+ }

+

+ void RVOSimulator2D::processObstacles()

+ {

+ kdTree_->buildObstacleTree(obstacles_);

+ }

+

+ bool RVOSimulator2D::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const

+ {

+ return kdTree_->queryVisibility(point1, point2, radius);

+ }

+

+ void RVOSimulator2D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)

+ {

+ if (defaultAgent_ == NULL) {

+ defaultAgent_ = new Agent2D();

+ }

+

+ defaultAgent_->maxNeighbors_ = maxNeighbors;

+ defaultAgent_->maxSpeed_ = maxSpeed;

+ defaultAgent_->neighborDist_ = neighborDist;

+ defaultAgent_->radius_ = radius;

+ defaultAgent_->timeHorizon_ = timeHorizon;

+ defaultAgent_->timeHorizonObst_ = timeHorizonObst;

+ defaultAgent_->velocity_ = velocity;

+ }

+

+ void RVOSimulator2D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)

+ {

+ agents_[agentNo]->maxNeighbors_ = maxNeighbors;

+ }

+

+ void RVOSimulator2D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)

+ {

+ agents_[agentNo]->maxSpeed_ = maxSpeed;

+ }

+

+ void RVOSimulator2D::setAgentNeighborDist(size_t agentNo, float neighborDist)

+ {

+ agents_[agentNo]->neighborDist_ = neighborDist;

+ }

+

+ void RVOSimulator2D::setAgentPosition(size_t agentNo, const Vector2 &position)

+ {

+ agents_[agentNo]->position_ = position;

+ }

+

+ void RVOSimulator2D::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity)

+ {

+ agents_[agentNo]->prefVelocity_ = prefVelocity;

+ }

+

+ void RVOSimulator2D::setAgentRadius(size_t agentNo, float radius)

+ {

+ agents_[agentNo]->radius_ = radius;

+ }

+

+ void RVOSimulator2D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)

+ {

+ agents_[agentNo]->timeHorizon_ = timeHorizon;

+ }

+

+ void RVOSimulator2D::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst)

+ {

+ agents_[agentNo]->timeHorizonObst_ = timeHorizonObst;

+ }

+

+ void RVOSimulator2D::setAgentVelocity(size_t agentNo, const Vector2 &velocity)

+ {

+ agents_[agentNo]->velocity_ = velocity;

+ }

+

+ void RVOSimulator2D::setTimeStep(float timeStep)

+ {

+ timeStep_ = timeStep;

+ }

+}