364 lines
9.3 KiB
C++
364 lines
9.3 KiB
C++
/*
|
|
* 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;
|
|
}
|
|
}
|