1/*
2 * RVOSimulator2d.h
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#ifndef RVO2D_RVO_SIMULATOR_H_
34#define RVO2D_RVO_SIMULATOR_H_
35
36/**
37 * \file RVOSimulator2d.h
38 * \brief Contains the RVOSimulator2D class.
39 */
40
41#include <cstddef>
42#include <limits>
43#include <vector>
44
45#include "Vector2.h"
46
47namespace RVO2D {
48 /**
49 * \brief Error value.
50 *
51 * A value equal to the largest unsigned integer that is returned in case
52 * of an error by functions in RVO2D::RVOSimulator2D.
53 */
54 const size_t RVO2D_ERROR = std::numeric_limits<size_t>::max();
55
56 /**
57 * \brief Defines a directed line.
58 */
59 class Line {
60 public:
61 /**
62 * \brief A point on the directed line.
63 */
64 Vector2 point;
65
66 /**
67 * \brief The direction of the directed line.
68 */
69 Vector2 direction;
70 };
71
72 class Agent2D;
73 class KdTree2D;
74 class Obstacle2D;
75
76 /**
77 * \brief Defines the simulation.
78 *
79 * The main class of the library that contains all simulation functionality.
80 */
81 class RVOSimulator2D {
82 public:
83 /**
84 * \brief Constructs a simulator instance.
85 */
86 RVOSimulator2D();
87
88 /**
89 * \brief Constructs a simulator instance and sets the default
90 * properties for any new agent that is added.
91 * \param timeStep The time step of the simulation.
92 * Must be positive.
93 * \param neighborDist The default maximum distance (center point
94 * to center point) to other agents a new agent
95 * takes into account in the navigation. The
96 * larger this number, the longer he running
97 * time of the simulation. If the number is too
98 * low, the simulation will not be safe. Must be
99 * non-negative.
100 * \param maxNeighbors The default maximum number of other agents a
101 * new agent takes into account in the
102 * navigation. The larger this number, the
103 * longer the running time of the simulation.
104 * If the number is too low, the simulation
105 * will not be safe.
106 * \param timeHorizon The default minimal amount of time for which
107 * a new agent's velocities that are computed
108 * by the simulation are safe with respect to
109 * other agents. The larger this number, the
110 * sooner an agent will respond to the presence
111 * of other agents, but the less freedom the
112 * agent has in choosing its velocities.
113 * Must be positive.
114 * \param timeHorizonObst The default minimal amount of time for which
115 * a new agent's velocities that are computed
116 * by the simulation are safe with respect to
117 * obstacles. The larger this number, the
118 * sooner an agent will respond to the presence
119 * of obstacles, but the less freedom the agent
120 * has in choosing its velocities.
121 * Must be positive.
122 * \param radius The default radius of a new agent.
123 * Must be non-negative.
124 * \param maxSpeed The default maximum speed of a new agent.
125 * Must be non-negative.
126 * \param velocity The default initial two-dimensional linear
127 * velocity of a new agent (optional).
128 */
129 RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors,
130 float timeHorizon, float timeHorizonObst, float radius,
131 float maxSpeed, const Vector2 &velocity = Vector2());
132
133 /**
134 * \brief Destroys this simulator instance.
135 */
136 ~RVOSimulator2D();
137
138 /**
139 * \brief Adds a new agent with default properties to the
140 * simulation.
141 * \param position The two-dimensional starting position of
142 * this agent.
143 * \return The number of the agent, or RVO2D::RVO2D_ERROR when the agent
144 * defaults have not been set.
145 */
146 size_t addAgent(const Vector2 &position);
147
148 /**
149 * \brief Adds a new agent to the simulation.
150 * \param position The two-dimensional starting position of
151 * this agent.
152 * \param neighborDist The maximum distance (center point to
153 * center point) to other agents this agent
154 * takes into account in the navigation. The
155 * larger this number, the longer the running
156 * time of the simulation. If the number is too
157 * low, the simulation will not be safe.
158 * Must be non-negative.
159 * \param maxNeighbors The maximum number of other agents this
160 * agent takes into account in the navigation.
161 * The larger this number, the longer the
162 * running time of the simulation. If the
163 * number is too low, the simulation will not
164 * be safe.
165 * \param timeHorizon The minimal amount of time for which this
166 * agent's velocities that are computed by the
167 * simulation are safe with respect to other
168 * agents. The larger this number, the sooner
169 * this agent will respond to the presence of
170 * other agents, but the less freedom this
171 * agent has in choosing its velocities.
172 * Must be positive.
173 * \param timeHorizonObst The minimal amount of time for which this
174 * agent's velocities that are computed by the
175 * simulation are safe with respect to
176 * obstacles. The larger this number, the
177 * sooner this agent will respond to the
178 * presence of obstacles, but the less freedom
179 * this agent has in choosing its velocities.
180 * Must be positive.
181 * \param radius The radius of this agent.
182 * Must be non-negative.
183 * \param maxSpeed The maximum speed of this agent.
184 * Must be non-negative.
185 * \param velocity The initial two-dimensional linear velocity
186 * of this agent (optional).
187 * \return The number of the agent.
188 */
189 size_t addAgent(const Vector2 &position, float neighborDist,
190 size_t maxNeighbors, float timeHorizon,
191 float timeHorizonObst, float radius, float maxSpeed,
192 const Vector2 &velocity = Vector2());
193
194 /**
195 * \brief Adds a new obstacle to the simulation.
196 * \param vertices List of the vertices of the polygonal
197 * obstacle in counterclockwise order.
198 * \return The number of the first vertex of the obstacle,
199 * or RVO2D::RVO2D_ERROR when the number of vertices is less than two.
200 * \note To add a "negative" obstacle, e.g. a bounding polygon around
201 * the environment, the vertices should be listed in clockwise
202 * order.
203 */
204 size_t addObstacle(const std::vector<Vector2> &vertices);
205
206 /**
207 * \brief Lets the simulator perform a simulation step and updates the
208 * two-dimensional position and two-dimensional velocity of
209 * each agent.
210 */
211 void doStep();
212
213 /**
214 * \brief Returns the specified agent neighbor of the specified
215 * agent.
216 * \param agentNo The number of the agent whose agent
217 * neighbor is to be retrieved.
218 * \param neighborNo The number of the agent neighbor to be
219 * retrieved.
220 * \return The number of the neighboring agent.
221 */
222 size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const;
223
224 /**
225 * \brief Returns the maximum neighbor count of a specified agent.
226 * \param agentNo The number of the agent whose maximum
227 * neighbor count is to be retrieved.
228 * \return The present maximum neighbor count of the agent.
229 */
230 size_t getAgentMaxNeighbors(size_t agentNo) const;
231
232 /**
233 * \brief Returns the maximum speed of a specified agent.
234 * \param agentNo The number of the agent whose maximum speed
235 * is to be retrieved.
236 * \return The present maximum speed of the agent.
237 */
238 float getAgentMaxSpeed(size_t agentNo) const;
239
240 /**
241 * \brief Returns the maximum neighbor distance of a specified
242 * agent.
243 * \param agentNo The number of the agent whose maximum
244 * neighbor distance is to be retrieved.
245 * \return The present maximum neighbor distance of the agent.
246 */
247 float getAgentNeighborDist(size_t agentNo) const;
248
249 /**
250 * \brief Returns the count of agent neighbors taken into account to
251 * compute the current velocity for the specified agent.
252 * \param agentNo The number of the agent whose count of agent
253 * neighbors is to be retrieved.
254 * \return The count of agent neighbors taken into account to compute
255 * the current velocity for the specified agent.
256 */
257 size_t getAgentNumAgentNeighbors(size_t agentNo) const;
258
259 /**
260 * \brief Returns the count of obstacle neighbors taken into account
261 * to compute the current velocity for the specified agent.
262 * \param agentNo The number of the agent whose count of
263 * obstacle neighbors is to be retrieved.
264 * \return The count of obstacle neighbors taken into account to
265 * compute the current velocity for the specified agent.
266 */
267 size_t getAgentNumObstacleNeighbors(size_t agentNo) const;
268
269
270 /**
271 * \brief Returns the count of ORCA constraints used to compute
272 * the current velocity for the specified agent.
273 * \param agentNo The number of the agent whose count of ORCA
274 * constraints is to be retrieved.
275 * \return The count of ORCA constraints used to compute the current
276 * velocity for the specified agent.
277 */
278 size_t getAgentNumORCALines(size_t agentNo) const;
279
280 /**
281 * \brief Returns the specified obstacle neighbor of the specified
282 * agent.
283 * \param agentNo The number of the agent whose obstacle
284 * neighbor is to be retrieved.
285 * \param neighborNo The number of the obstacle neighbor to be
286 * retrieved.
287 * \return The number of the first vertex of the neighboring obstacle
288 * edge.
289 */
290 size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const;
291
292 /**
293 * \brief Returns the specified ORCA constraint of the specified
294 * agent.
295 * \param agentNo The number of the agent whose ORCA
296 * constraint is to be retrieved.
297 * \param lineNo The number of the ORCA constraint to be
298 * retrieved.
299 * \return A line representing the specified ORCA constraint.
300 * \note The halfplane to the left of the line is the region of
301 * permissible velocities with respect to the specified
302 * ORCA constraint.
303 */
304 const Line &getAgentORCALine(size_t agentNo, size_t lineNo) const;
305
306 /**
307 * \brief Returns the two-dimensional position of a specified
308 * agent.
309 * \param agentNo The number of the agent whose
310 * two-dimensional position is to be retrieved.
311 * \return The present two-dimensional position of the (center of the)
312 * agent.
313 */
314 const Vector2 &getAgentPosition(size_t agentNo) const;
315
316 /**
317 * \brief Returns the two-dimensional preferred velocity of a
318 * specified agent.
319 * \param agentNo The number of the agent whose
320 * two-dimensional preferred velocity is to be
321 * retrieved.
322 * \return The present two-dimensional preferred velocity of the agent.
323 */
324 const Vector2 &getAgentPrefVelocity(size_t agentNo) const;
325
326 /**
327 * \brief Returns the radius of a specified agent.
328 * \param agentNo The number of the agent whose radius is to
329 * be retrieved.
330 * \return The present radius of the agent.
331 */
332 float getAgentRadius(size_t agentNo) const;
333
334 /**
335 * \brief Returns the time horizon of a specified agent.
336 * \param agentNo The number of the agent whose time horizon
337 * is to be retrieved.
338 * \return The present time horizon of the agent.
339 */
340 float getAgentTimeHorizon(size_t agentNo) const;
341
342 /**
343 * \brief Returns the time horizon with respect to obstacles of a
344 * specified agent.
345 * \param agentNo The number of the agent whose time horizon
346 * with respect to obstacles is to be
347 * retrieved.
348 * \return The present time horizon with respect to obstacles of the
349 * agent.
350 */
351 float getAgentTimeHorizonObst(size_t agentNo) const;
352
353 /**
354 * \brief Returns the two-dimensional linear velocity of a
355 * specified agent.
356 * \param agentNo The number of the agent whose
357 * two-dimensional linear velocity is to be
358 * retrieved.
359 * \return The present two-dimensional linear velocity of the agent.
360 */
361 const Vector2 &getAgentVelocity(size_t agentNo) const;
362
363 /**
364 * \brief Returns the global time of the simulation.
365 * \return The present global time of the simulation (zero initially).
366 */
367 float getGlobalTime() const;
368
369 /**
370 * \brief Returns the count of agents in the simulation.
371 * \return The count of agents in the simulation.
372 */
373 size_t getNumAgents() const;
374
375 /**
376 * \brief Returns the count of obstacle vertices in the simulation.
377 * \return The count of obstacle vertices in the simulation.
378 */
379 size_t getNumObstacleVertices() const;
380
381 /**
382 * \brief Returns the two-dimensional position of a specified obstacle
383 * vertex.
384 * \param vertexNo The number of the obstacle vertex to be
385 * retrieved.
386 * \return The two-dimensional position of the specified obstacle
387 * vertex.
388 */
389 const Vector2 &getObstacleVertex(size_t vertexNo) const;
390
391 /**
392 * \brief Returns the number of the obstacle vertex succeeding the
393 * specified obstacle vertex in its polygon.
394 * \param vertexNo The number of the obstacle vertex whose
395 * successor is to be retrieved.
396 * \return The number of the obstacle vertex succeeding the specified
397 * obstacle vertex in its polygon.
398 */
399 size_t getNextObstacleVertexNo(size_t vertexNo) const;
400
401 /**
402 * \brief Returns the number of the obstacle vertex preceding the
403 * specified obstacle vertex in its polygon.
404 * \param vertexNo The number of the obstacle vertex whose
405 * predecessor is to be retrieved.
406 * \return The number of the obstacle vertex preceding the specified
407 * obstacle vertex in its polygon.
408 */
409 size_t getPrevObstacleVertexNo(size_t vertexNo) const;
410
411 /**
412 * \brief Returns the time step of the simulation.
413 * \return The present time step of the simulation.
414 */
415 float getTimeStep() const;
416
417 /**
418 * \brief Processes the obstacles that have been added so that they
419 * are accounted for in the simulation.
420 * \note Obstacles added to the simulation after this function has
421 * been called are not accounted for in the simulation.
422 */
423 void processObstacles();
424
425 /**
426 * \brief Performs a visibility query between the two specified
427 * points with respect to the obstacles
428 * \param point1 The first point of the query.
429 * \param point2 The second point of the query.
430 * \param radius The minimal distance between the line
431 * connecting the two points and the obstacles
432 * in order for the points to be mutually
433 * visible (optional). Must be non-negative.
434 * \return A boolean specifying whether the two points are mutually
435 * visible. Returns true when the obstacles have not been
436 * processed.
437 */
438 bool queryVisibility(const Vector2 &point1, const Vector2 &point2,
439 float radius = 0.0f) const;
440
441 /**
442 * \brief Sets the default properties for any new agent that is
443 * added.
444 * \param neighborDist The default maximum distance (center point
445 * to center point) to other agents a new agent
446 * takes into account in the navigation. The
447 * larger this number, the longer he running
448 * time of the simulation. If the number is too
449 * low, the simulation will not be safe.
450 * Must be non-negative.
451 * \param maxNeighbors The default maximum number of other agents a
452 * new agent takes into account in the
453 * navigation. The larger this number, the
454 * longer the running time of the simulation.
455 * If the number is too low, the simulation
456 * will not be safe.
457 * \param timeHorizon The default minimal amount of time for which
458 * a new agent's velocities that are computed
459 * by the simulation are safe with respect to
460 * other agents. The larger this number, the
461 * sooner an agent will respond to the presence
462 * of other agents, but the less freedom the
463 * agent has in choosing its velocities.
464 * Must be positive.
465 * \param timeHorizonObst The default minimal amount of time for which
466 * a new agent's velocities that are computed
467 * by the simulation are safe with respect to
468 * obstacles. The larger this number, the
469 * sooner an agent will respond to the presence
470 * of obstacles, but the less freedom the agent
471 * has in choosing its velocities.
472 * Must be positive.
473 * \param radius The default radius of a new agent.
474 * Must be non-negative.
475 * \param maxSpeed The default maximum speed of a new agent.
476 * Must be non-negative.
477 * \param velocity The default initial two-dimensional linear
478 * velocity of a new agent (optional).
479 */
480 void setAgentDefaults(float neighborDist, size_t maxNeighbors,
481 float timeHorizon, float timeHorizonObst,
482 float radius, float maxSpeed,
483 const Vector2 &velocity = Vector2());
484
485 /**
486 * \brief Sets the maximum neighbor count of a specified agent.
487 * \param agentNo The number of the agent whose maximum
488 * neighbor count is to be modified.
489 * \param maxNeighbors The replacement maximum neighbor count.
490 */
491 void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors);
492
493 /**
494 * \brief Sets the maximum speed of a specified agent.
495 * \param agentNo The number of the agent whose maximum speed
496 * is to be modified.
497 * \param maxSpeed The replacement maximum speed. Must be
498 * non-negative.
499 */
500 void setAgentMaxSpeed(size_t agentNo, float maxSpeed);
501
502 /**
503 * \brief Sets the maximum neighbor distance of a specified agent.
504 * \param agentNo The number of the agent whose maximum
505 * neighbor distance is to be modified.
506 * \param neighborDist The replacement maximum neighbor distance.
507 * Must be non-negative.
508 */
509 void setAgentNeighborDist(size_t agentNo, float neighborDist);
510
511 /**
512 * \brief Sets the two-dimensional position of a specified agent.
513 * \param agentNo The number of the agent whose
514 * two-dimensional position is to be modified.
515 * \param position The replacement of the two-dimensional
516 * position.
517 */
518 void setAgentPosition(size_t agentNo, const Vector2 &position);
519
520 /**
521 * \brief Sets the two-dimensional preferred velocity of a
522 * specified agent.
523 * \param agentNo The number of the agent whose
524 * two-dimensional preferred velocity is to be
525 * modified.
526 * \param prefVelocity The replacement of the two-dimensional
527 * preferred velocity.
528 */
529 void setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity);
530
531 /**
532 * \brief Sets the radius of a specified agent.
533 * \param agentNo The number of the agent whose radius is to
534 * be modified.
535 * \param radius The replacement radius.
536 * Must be non-negative.
537 */
538 void setAgentRadius(size_t agentNo, float radius);
539
540 /**
541 * \brief Sets the time horizon of a specified agent with respect
542 * to other agents.
543 * \param agentNo The number of the agent whose time horizon
544 * is to be modified.
545 * \param timeHorizon The replacement time horizon with respect
546 * to other agents. Must be positive.
547 */
548 void setAgentTimeHorizon(size_t agentNo, float timeHorizon);
549
550 /**
551 * \brief Sets the time horizon of a specified agent with respect
552 * to obstacles.
553 * \param agentNo The number of the agent whose time horizon
554 * with respect to obstacles is to be modified.
555 * \param timeHorizonObst The replacement time horizon with respect to
556 * obstacles. Must be positive.
557 */
558 void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst);
559
560 /**
561 * \brief Sets the two-dimensional linear velocity of a specified
562 * agent.
563 * \param agentNo The number of the agent whose
564 * two-dimensional linear velocity is to be
565 * modified.
566 * \param velocity The replacement two-dimensional linear
567 * velocity.
568 */
569 void setAgentVelocity(size_t agentNo, const Vector2 &velocity);
570
571 /**
572 * \brief Sets the time step of the simulation.
573 * \param timeStep The time step of the simulation.
574 * Must be positive.
575 */
576 void setTimeStep(float timeStep);
577
578 public:
579 std::vector<Agent2D *> agents_;
580 Agent2D *defaultAgent_;
581 float globalTime_;
582 KdTree2D *kdTree_;
583 std::vector<Obstacle2D *> obstacles_;
584 float timeStep_;
585
586 friend class Agent2D;
587 friend class KdTree2D;
588 friend class Obstacle2D;
589 };
590}
591
592#endif /* RVO2D_RVO_SIMULATOR_H_ */
593