1/*
2 * RVOSimulator.cpp
3 * RVO2-3D 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 "RVOSimulator3d.h"
34
35#ifdef _OPENMP
36#include <omp.h>
37#endif
38
39#include "Agent3d.h"
40#include "KdTree3d.h"
41
42namespace RVO3D {
43 RVOSimulator3D::RVOSimulator3D() : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(0.0f)
44 {
45 kdTree_ = new KdTree3D(this);
46 }
47
48 RVOSimulator3D::RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity) : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(timeStep)
49 {
50 kdTree_ = new KdTree3D(this);
51 defaultAgent_ = new Agent3D();
52
53 defaultAgent_->maxNeighbors_ = maxNeighbors;
54 defaultAgent_->maxSpeed_ = maxSpeed;
55 defaultAgent_->neighborDist_ = neighborDist;
56 defaultAgent_->radius_ = radius;
57 defaultAgent_->timeHorizon_ = timeHorizon;
58 defaultAgent_->velocity_ = velocity;
59 }
60
61 RVOSimulator3D::~RVOSimulator3D()
62 {
63 if (defaultAgent_ != NULL) {
64 delete defaultAgent_;
65 }
66
67 for (size_t i = 0; i < agents_.size(); ++i) {
68 delete agents_[i];
69 }
70
71 if (kdTree_ != NULL) {
72 delete kdTree_;
73 }
74 }
75
76 size_t RVOSimulator3D::getAgentNumAgentNeighbors(size_t agentNo) const
77 {
78 return agents_[agentNo]->agentNeighbors_.size();
79 }
80
81 size_t RVOSimulator3D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
82 {
83 return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
84 }
85
86 size_t RVOSimulator3D::getAgentNumORCAPlanes(size_t agentNo) const
87 {
88 return agents_[agentNo]->orcaPlanes_.size();
89 }
90
91 const Plane &RVOSimulator3D::getAgentORCAPlane(size_t agentNo, size_t planeNo) const
92 {
93 return agents_[agentNo]->orcaPlanes_[planeNo];
94 }
95
96 void RVOSimulator3D::removeAgent(size_t agentNo)
97 {
98 delete agents_[agentNo];
99 agents_[agentNo] = agents_.back();
100 agents_.pop_back();
101 }
102
103 size_t RVOSimulator3D::addAgent(const Vector3 &position)
104 {
105 if (defaultAgent_ == NULL) {
106 return RVO3D_ERROR;
107 }
108
109 Agent3D *agent = new Agent3D();
110
111 agent->position_ = position;
112 agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
113 agent->maxSpeed_ = defaultAgent_->maxSpeed_;
114 agent->neighborDist_ = defaultAgent_->neighborDist_;
115 agent->radius_ = defaultAgent_->radius_;
116 agent->timeHorizon_ = defaultAgent_->timeHorizon_;
117 agent->velocity_ = defaultAgent_->velocity_;
118
119 agent->id_ = agents_.size();
120
121 agents_.push_back(agent);
122
123 return agents_.size() - 1;
124 }
125
126 size_t RVOSimulator3D::addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
127 {
128 Agent3D *agent = new Agent3D();
129
130 agent->position_ = position;
131 agent->maxNeighbors_ = maxNeighbors;
132 agent->maxSpeed_ = maxSpeed;
133 agent->neighborDist_ = neighborDist;
134 agent->radius_ = radius;
135 agent->timeHorizon_ = timeHorizon;
136 agent->velocity_ = velocity;
137
138 agent->id_ = agents_.size();
139
140 agents_.push_back(agent);
141
142 return agents_.size() - 1;
143 }
144
145 void RVOSimulator3D::doStep()
146 {
147 kdTree_->buildAgentTree(agents_);
148
149 for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
150 agents_[i]->computeNeighbors(this);
151 agents_[i]->computeNewVelocity(this);
152 }
153
154 for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
155 agents_[i]->update(this);
156 }
157
158 globalTime_ += timeStep_;
159 }
160
161 size_t RVOSimulator3D::getAgentMaxNeighbors(size_t agentNo) const
162 {
163 return agents_[agentNo]->maxNeighbors_;
164 }
165
166 float RVOSimulator3D::getAgentMaxSpeed(size_t agentNo) const
167 {
168 return agents_[agentNo]->maxSpeed_;
169 }
170
171 float RVOSimulator3D::getAgentNeighborDist(size_t agentNo) const
172 {
173 return agents_[agentNo]->neighborDist_;
174 }
175
176 const Vector3 &RVOSimulator3D::getAgentPosition(size_t agentNo) const
177 {
178 return agents_[agentNo]->position_;
179 }
180
181 const Vector3 &RVOSimulator3D::getAgentPrefVelocity(size_t agentNo) const
182 {
183 return agents_[agentNo]->prefVelocity_;
184 }
185
186 float RVOSimulator3D::getAgentRadius(size_t agentNo) const
187 {
188 return agents_[agentNo]->radius_;
189 }
190
191 float RVOSimulator3D::getAgentTimeHorizon(size_t agentNo) const
192 {
193 return agents_[agentNo]->timeHorizon_;
194 }
195
196 const Vector3 &RVOSimulator3D::getAgentVelocity(size_t agentNo) const
197 {
198 return agents_[agentNo]->velocity_;
199 }
200
201 float RVOSimulator3D::getGlobalTime() const
202 {
203 return globalTime_;
204 }
205
206 size_t RVOSimulator3D::getNumAgents() const
207 {
208 return agents_.size();
209 }
210
211 float RVOSimulator3D::getTimeStep() const
212 {
213 return timeStep_;
214 }
215
216 void RVOSimulator3D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
217 {
218 if (defaultAgent_ == NULL) {
219 defaultAgent_ = new Agent3D();
220 }
221
222 defaultAgent_->maxNeighbors_ = maxNeighbors;
223 defaultAgent_->maxSpeed_ = maxSpeed;
224 defaultAgent_->neighborDist_ = neighborDist;
225 defaultAgent_->radius_ = radius;
226 defaultAgent_->timeHorizon_ = timeHorizon;
227 defaultAgent_->velocity_ = velocity;
228 }
229
230 void RVOSimulator3D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
231 {
232 agents_[agentNo]->maxNeighbors_ = maxNeighbors;
233 }
234
235 void RVOSimulator3D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
236 {
237 agents_[agentNo]->maxSpeed_ = maxSpeed;
238 }
239
240 void RVOSimulator3D::setAgentNeighborDist(size_t agentNo, float neighborDist)
241 {
242 agents_[agentNo]->neighborDist_ = neighborDist;
243 }
244
245 void RVOSimulator3D::setAgentPosition(size_t agentNo, const Vector3 &position)
246 {
247 agents_[agentNo]->position_ = position;
248 }
249
250 void RVOSimulator3D::setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity)
251 {
252 agents_[agentNo]->prefVelocity_ = prefVelocity;
253 }
254
255 void RVOSimulator3D::setAgentRadius(size_t agentNo, float radius)
256 {
257 agents_[agentNo]->radius_ = radius;
258 }
259
260 void RVOSimulator3D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
261 {
262 agents_[agentNo]->timeHorizon_ = timeHorizon;
263 }
264
265 void RVOSimulator3D::setAgentVelocity(size_t agentNo, const Vector3 &velocity)
266 {
267 agents_[agentNo]->velocity_ = velocity;
268 }
269
270 void RVOSimulator3D::setTimeStep(float timeStep)
271 {
272 timeStep_ = timeStep;
273 }
274}
275