diff options
Diffstat (limited to 'thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp')
-rw-r--r-- | thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp | 363 |
1 files changed, 0 insertions, 363 deletions
diff --git a/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp b/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp deleted file mode 100644 index 9fb1555ebc..0000000000 --- a/thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp +++ /dev/null @@ -1,363 +0,0 @@ -/* - * 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; - } -} |