1/*
2 * RVOSimulator2d.cpp
3 * RVO2 Library
4 *
5 * Copyright 2008 University of North Carolina at Chapel Hill
6 *
7 * Licensed under the Apache License, Version 2.0 (the "License");
8 * you may not use this file except in compliance with the License.
9 * You may obtain a copy of the License at
10 *
11 * http://www.apache.org/licenses/LICENSE-2.0
12 *
13 * Unless required by applicable law or agreed to in writing, software
14 * distributed under the License is distributed on an "AS IS" BASIS,
15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
16 * See the License for the specific language governing permissions and
17 * limitations under the License.
18 *
19 * Please send all bug reports to <geom@cs.unc.edu>.
20 *
21 * The authors may be contacted via:
22 *
23 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
24 * Dept. of Computer Science
25 * 201 S. Columbia St.
26 * Frederick P. Brooks, Jr. Computer Science Bldg.
27 * Chapel Hill, N.C. 27599-3175
28 * United States of America
29 *
30 * <http://gamma.cs.unc.edu/RVO2/>
31 */
32
33#include "RVOSimulator2d.h"
34
35#include "Agent2d.h"
36#include "KdTree2d.h"
37#include "Obstacle2d.h"
38
39#ifdef _OPENMP
40#include <omp.h>
41#endif
42
43namespace RVO2D {
44 RVOSimulator2D::RVOSimulator2D() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f)
45 {
46 kdTree_ = new KdTree2D(this);
47 }
48
49 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)
50 {
51 kdTree_ = new KdTree2D(this);
52 defaultAgent_ = new Agent2D();
53
54 defaultAgent_->maxNeighbors_ = maxNeighbors;
55 defaultAgent_->maxSpeed_ = maxSpeed;
56 defaultAgent_->neighborDist_ = neighborDist;
57 defaultAgent_->radius_ = radius;
58 defaultAgent_->timeHorizon_ = timeHorizon;
59 defaultAgent_->timeHorizonObst_ = timeHorizonObst;
60 defaultAgent_->velocity_ = velocity;
61 }
62
63 RVOSimulator2D::~RVOSimulator2D()
64 {
65 if (defaultAgent_ != NULL) {
66 delete defaultAgent_;
67 }
68
69 for (size_t i = 0; i < agents_.size(); ++i) {
70 delete agents_[i];
71 }
72
73 for (size_t i = 0; i < obstacles_.size(); ++i) {
74 delete obstacles_[i];
75 }
76
77 delete kdTree_;
78 }
79
80 size_t RVOSimulator2D::addAgent(const Vector2 &position)
81 {
82 if (defaultAgent_ == NULL) {
83 return RVO2D_ERROR;
84 }
85
86 Agent2D *agent = new Agent2D();
87
88 agent->position_ = position;
89 agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
90 agent->maxSpeed_ = defaultAgent_->maxSpeed_;
91 agent->neighborDist_ = defaultAgent_->neighborDist_;
92 agent->radius_ = defaultAgent_->radius_;
93 agent->timeHorizon_ = defaultAgent_->timeHorizon_;
94 agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_;
95 agent->velocity_ = defaultAgent_->velocity_;
96
97 agent->id_ = agents_.size();
98
99 agents_.push_back(agent);
100
101 return agents_.size() - 1;
102 }
103
104 size_t RVOSimulator2D::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
105 {
106 Agent2D *agent = new Agent2D();
107
108 agent->position_ = position;
109 agent->maxNeighbors_ = maxNeighbors;
110 agent->maxSpeed_ = maxSpeed;
111 agent->neighborDist_ = neighborDist;
112 agent->radius_ = radius;
113 agent->timeHorizon_ = timeHorizon;
114 agent->timeHorizonObst_ = timeHorizonObst;
115 agent->velocity_ = velocity;
116
117 agent->id_ = agents_.size();
118
119 agents_.push_back(agent);
120
121 return agents_.size() - 1;
122 }
123
124 size_t RVOSimulator2D::addObstacle(const std::vector<Vector2> &vertices)
125 {
126 if (vertices.size() < 2) {
127 return RVO2D_ERROR;
128 }
129
130 const size_t obstacleNo = obstacles_.size();
131
132 for (size_t i = 0; i < vertices.size(); ++i) {
133 Obstacle2D *obstacle = new Obstacle2D();
134 obstacle->point_ = vertices[i];
135
136 if (i != 0) {
137 obstacle->prevObstacle_ = obstacles_.back();
138 obstacle->prevObstacle_->nextObstacle_ = obstacle;
139 }
140
141 if (i == vertices.size() - 1) {
142 obstacle->nextObstacle_ = obstacles_[obstacleNo];
143 obstacle->nextObstacle_->prevObstacle_ = obstacle;
144 }
145
146 obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]);
147
148 if (vertices.size() == 2) {
149 obstacle->isConvex_ = true;
150 }
151 else {
152 obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
153 }
154
155 obstacle->id_ = obstacles_.size();
156
157 obstacles_.push_back(obstacle);
158 }
159
160 return obstacleNo;
161 }
162
163 void RVOSimulator2D::doStep()
164 {
165 kdTree_->buildAgentTree(agents_);
166
167 for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
168 agents_[i]->computeNeighbors(this);
169 agents_[i]->computeNewVelocity(this);
170 }
171
172 for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
173 agents_[i]->update(this);
174 }
175
176 globalTime_ += timeStep_;
177 }
178
179 size_t RVOSimulator2D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
180 {
181 return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
182 }
183
184 size_t RVOSimulator2D::getAgentMaxNeighbors(size_t agentNo) const
185 {
186 return agents_[agentNo]->maxNeighbors_;
187 }
188
189 float RVOSimulator2D::getAgentMaxSpeed(size_t agentNo) const
190 {
191 return agents_[agentNo]->maxSpeed_;
192 }
193
194 float RVOSimulator2D::getAgentNeighborDist(size_t agentNo) const
195 {
196 return agents_[agentNo]->neighborDist_;
197 }
198
199 size_t RVOSimulator2D::getAgentNumAgentNeighbors(size_t agentNo) const
200 {
201 return agents_[agentNo]->agentNeighbors_.size();
202 }
203
204 size_t RVOSimulator2D::getAgentNumObstacleNeighbors(size_t agentNo) const
205 {
206 return agents_[agentNo]->obstacleNeighbors_.size();
207 }
208
209 size_t RVOSimulator2D::getAgentNumORCALines(size_t agentNo) const
210 {
211 return agents_[agentNo]->orcaLines_.size();
212 }
213
214 size_t RVOSimulator2D::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const
215 {
216 return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_;
217 }
218
219 const Line &RVOSimulator2D::getAgentORCALine(size_t agentNo, size_t lineNo) const
220 {
221 return agents_[agentNo]->orcaLines_[lineNo];
222 }
223
224 const Vector2 &RVOSimulator2D::getAgentPosition(size_t agentNo) const
225 {
226 return agents_[agentNo]->position_;
227 }
228
229 const Vector2 &RVOSimulator2D::getAgentPrefVelocity(size_t agentNo) const
230 {
231 return agents_[agentNo]->prefVelocity_;
232 }
233
234 float RVOSimulator2D::getAgentRadius(size_t agentNo) const
235 {
236 return agents_[agentNo]->radius_;
237 }
238
239 float RVOSimulator2D::getAgentTimeHorizon(size_t agentNo) const
240 {
241 return agents_[agentNo]->timeHorizon_;
242 }
243
244 float RVOSimulator2D::getAgentTimeHorizonObst(size_t agentNo) const
245 {
246 return agents_[agentNo]->timeHorizonObst_;
247 }
248
249 const Vector2 &RVOSimulator2D::getAgentVelocity(size_t agentNo) const
250 {
251 return agents_[agentNo]->velocity_;
252 }
253
254 float RVOSimulator2D::getGlobalTime() const
255 {
256 return globalTime_;
257 }
258
259 size_t RVOSimulator2D::getNumAgents() const
260 {
261 return agents_.size();
262 }
263
264 size_t RVOSimulator2D::getNumObstacleVertices() const
265 {
266 return obstacles_.size();
267 }
268
269 const Vector2 &RVOSimulator2D::getObstacleVertex(size_t vertexNo) const
270 {
271 return obstacles_[vertexNo]->point_;
272 }
273
274 size_t RVOSimulator2D::getNextObstacleVertexNo(size_t vertexNo) const
275 {
276 return obstacles_[vertexNo]->nextObstacle_->id_;
277 }
278
279 size_t RVOSimulator2D::getPrevObstacleVertexNo(size_t vertexNo) const
280 {
281 return obstacles_[vertexNo]->prevObstacle_->id_;
282 }
283
284 float RVOSimulator2D::getTimeStep() const
285 {
286 return timeStep_;
287 }
288
289 void RVOSimulator2D::processObstacles()
290 {
291 kdTree_->buildObstacleTree(obstacles_);
292 }
293
294 bool RVOSimulator2D::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const
295 {
296 return kdTree_->queryVisibility(point1, point2, radius);
297 }
298
299 void RVOSimulator2D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
300 {
301 if (defaultAgent_ == NULL) {
302 defaultAgent_ = new Agent2D();
303 }
304
305 defaultAgent_->maxNeighbors_ = maxNeighbors;
306 defaultAgent_->maxSpeed_ = maxSpeed;
307 defaultAgent_->neighborDist_ = neighborDist;
308 defaultAgent_->radius_ = radius;
309 defaultAgent_->timeHorizon_ = timeHorizon;
310 defaultAgent_->timeHorizonObst_ = timeHorizonObst;
311 defaultAgent_->velocity_ = velocity;
312 }
313
314 void RVOSimulator2D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
315 {
316 agents_[agentNo]->maxNeighbors_ = maxNeighbors;
317 }
318
319 void RVOSimulator2D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
320 {
321 agents_[agentNo]->maxSpeed_ = maxSpeed;
322 }
323
324 void RVOSimulator2D::setAgentNeighborDist(size_t agentNo, float neighborDist)
325 {
326 agents_[agentNo]->neighborDist_ = neighborDist;
327 }
328
329 void RVOSimulator2D::setAgentPosition(size_t agentNo, const Vector2 &position)
330 {
331 agents_[agentNo]->position_ = position;
332 }
333
334 void RVOSimulator2D::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity)
335 {
336 agents_[agentNo]->prefVelocity_ = prefVelocity;
337 }
338
339 void RVOSimulator2D::setAgentRadius(size_t agentNo, float radius)
340 {
341 agents_[agentNo]->radius_ = radius;
342 }
343
344 void RVOSimulator2D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
345 {
346 agents_[agentNo]->timeHorizon_ = timeHorizon;
347 }
348
349 void RVOSimulator2D::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst)
350 {
351 agents_[agentNo]->timeHorizonObst_ = timeHorizonObst;
352 }
353
354 void RVOSimulator2D::setAgentVelocity(size_t agentNo, const Vector2 &velocity)
355 {
356 agents_[agentNo]->velocity_ = velocity;
357 }
358
359 void RVOSimulator2D::setTimeStep(float timeStep)
360 {
361 timeStep_ = timeStep;
362 }
363}
364