$projectname: $title

20 | 21 | -------------------------------------------------------------------------------- /example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rvo2 4 | 5 | sim = rvo2.PyRVOSimulator(1/60., 1.5, 5, 1.5, 2, 0.4, 2) 6 | 7 | # Pass either just the position (the other parameters then use 8 | # the default values passed to the PyRVOSimulator constructor), 9 | # or pass all available parameters. 10 | a0 = sim.addAgent((0, 0)) 11 | a1 = sim.addAgent((1, 0)) 12 | a2 = sim.addAgent((1, 1)) 13 | a3 = sim.addAgent((0, 1), 1.5, 5, 1.5, 2, 0.4, 2, (0, 0)) 14 | 15 | # Obstacles are also supported. 16 | o1 = sim.addObstacle([(0.1, 0.1), (-0.1, 0.1), (-0.1, -0.1)]) 17 | sim.processObstacles() 18 | 19 | sim.setAgentPrefVelocity(a0, (1, 1)) 20 | sim.setAgentPrefVelocity(a1, (-1, 1)) 21 | sim.setAgentPrefVelocity(a2, (-1, -1)) 22 | sim.setAgentPrefVelocity(a3, (1, -1)) 23 | 24 | print('Simulation has %i agents and %i obstacle vertices in it.' % 25 | (sim.getNumAgents(), sim.getNumObstacleVertices())) 26 | 27 | print('Running simulation') 28 | 29 | for step in range(20): 30 | sim.doStep() 31 | 32 | positions = ['(%5.3f, %5.3f)' % sim.getAgentPosition(agent_no) 33 | for agent_no in (a0, a1, a2, a3)] 34 | print('step=%2i t=%.3f %s' % (step, sim.getGlobalTime(), ' '.join(positions))) 35 | 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # CMakeLists.txt 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 . 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 | # 31 | # 32 | 33 | cmake_minimum_required(VERSION 2.8) 34 | project(RVO) 35 | 36 | include(CTest) 37 | 38 | add_subdirectory(src) 39 | add_subdirectory(examples) 40 | 41 | include(CPack) 42 | -------------------------------------------------------------------------------- /src/Obstacle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Obstacle.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 . 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 | * 31 | */ 32 | 33 | #include "Obstacle.h" 34 | #include "RVOSimulator.h" 35 | 36 | namespace RVO { 37 | Obstacle::Obstacle() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { } 38 | } 39 | -------------------------------------------------------------------------------- /CTestConfig.cmake: -------------------------------------------------------------------------------- 1 | # 2 | # CTestConfig.cmake 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 . 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 | # 31 | # 32 | 33 | set(CTEST_PROJECT_NAME "RVO2") 34 | set(CTEST_NIGHTLY_START_TIME "01:00:00 UTC") 35 | 36 | set(CTEST_DROP_METHOD "http") 37 | set(CTEST_DROP_SITE "my.cdash.org") 38 | set(CTEST_DROP_LOCATION "/submit.php?project=RVO2") 39 | set(CTEST_DROP_SITE_CDASH TRUE) 40 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # examples/CMakeLists.txt 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 . 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 | # 31 | # 32 | 33 | include_directories(${RVO_SOURCE_DIR}/src) 34 | 35 | add_executable(Blocks Blocks.cpp) 36 | target_link_libraries(Blocks RVO) 37 | add_test(Blocks Blocks) 38 | 39 | add_executable(Circle Circle.cpp) 40 | target_link_libraries(Circle RVO) 41 | add_test(Circle Circle) 42 | 43 | add_executable(Roadmap Roadmap.cpp) 44 | target_link_libraries(Roadmap RVO) 45 | add_test(Roadmap Roadmap) 46 | 47 | install(TARGETS Blocks Circle Roadmap DESTINATION bin) 48 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # src/CMakeLists.txt 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 . 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 | # 31 | # 32 | 33 | set(RVO_HEADERS 34 | RVO.h 35 | RVOSimulator.h 36 | Vector2.h) 37 | 38 | set(RVO_SOURCES 39 | Agent.cpp 40 | Agent.h 41 | Definitions.h 42 | KdTree.cpp 43 | KdTree.h 44 | Obstacle.cpp 45 | Obstacle.h 46 | RVOSimulator.cpp) 47 | 48 | add_library(RVO ${RVO_HEADERS} ${RVO_SOURCES}) 49 | 50 | if(WIN32) 51 | set_target_properties(RVO PROPERTIES COMPILE_DEFINITIONS NOMINMAX) 52 | endif() 53 | 54 | install(FILES ${RVO_HEADERS} DESTINATION include) 55 | install(TARGETS RVO DESTINATION lib) 56 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, Extension 2 | from setuptools.command.build_ext import build_ext as _build_ext 3 | from Cython.Build import cythonize 4 | 5 | 6 | class BuildRvo2Ext(_build_ext): 7 | """Builds RVO2 before our module.""" 8 | 9 | def run(self): 10 | # Build RVO2 11 | import os 12 | import os.path 13 | import subprocess 14 | 15 | build_dir = os.path.abspath('build/RVO2') 16 | if not os.path.exists(build_dir): 17 | os.makedirs(build_dir) 18 | subprocess.check_call(['cmake', '../..', '-DCMAKE_CXX_FLAGS=-fPIC'], 19 | cwd=build_dir) 20 | subprocess.check_call(['cmake', '--build', '.'], cwd=build_dir) 21 | 22 | _build_ext.run(self) 23 | 24 | 25 | extensions = [ 26 | Extension('rvo2', ['src/*.pyx'], 27 | include_dirs=['src'], 28 | libraries=['RVO'], 29 | library_dirs=['build/RVO2/src'], 30 | extra_compile_args=['-fPIC']), 31 | ] 32 | 33 | setup( 34 | name="pyrvo2", 35 | ext_modules=cythonize(extensions), 36 | cmdclass={'build_ext': BuildRvo2Ext}, 37 | classifiers=[ 38 | 'Development Status :: 5 - Production/Stable', 39 | 'Intended Audience :: Developers', 40 | 'Intended Audience :: Education', 41 | 'Intended Audience :: Information Technology', 42 | 'Operating System :: OS Independent', 43 | 'Programming Language :: Python', 44 | 'Programming Language :: Python :: 2.7', 45 | 'Programming Language :: Python :: 3', 46 | 'Programming Language :: Python :: 3.4', 47 | 'Programming Language :: Cython', 48 | 'Topic :: Games/Entertainment :: Simulation', 49 | 'Topic :: Software Development :: Libraries :: Python Modules', 50 | ], 51 | ) 52 | -------------------------------------------------------------------------------- /src/Obstacle.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Obstacle.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 . 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 | * 31 | */ 32 | 33 | #ifndef RVO_OBSTACLE_H_ 34 | #define RVO_OBSTACLE_H_ 35 | 36 | /** 37 | * \file Obstacle.h 38 | * \brief Contains the Obstacle class. 39 | */ 40 | 41 | #include "Definitions.h" 42 | 43 | namespace RVO { 44 | /** 45 | * \brief Defines static obstacles in the simulation. 46 | */ 47 | class Obstacle { 48 | private: 49 | /** 50 | * \brief Constructs a static obstacle instance. 51 | */ 52 | Obstacle(); 53 | 54 | bool isConvex_; 55 | Obstacle *nextObstacle_; 56 | Vector2 point_; 57 | Obstacle *prevObstacle_; 58 | Vector2 unitDir_; 59 | 60 | size_t id_; 61 | 62 | friend class Agent; 63 | friend class KdTree; 64 | friend class RVOSimulator; 65 | }; 66 | } 67 | 68 | #endif /* RVO_OBSTACLE_H_ */ 69 | -------------------------------------------------------------------------------- /src/Definitions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Definitions.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 . 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 | * 31 | */ 32 | 33 | #ifndef RVO_DEFINITIONS_H_ 34 | #define RVO_DEFINITIONS_H_ 35 | 36 | /** 37 | * \file Definitions.h 38 | * \brief Contains functions and constants used in multiple classes. 39 | */ 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include "Vector2.h" 48 | 49 | /** 50 | * \brief A sufficiently small positive number. 51 | */ 52 | const float RVO_EPSILON = 0.00001f; 53 | 54 | namespace RVO { 55 | class Agent; 56 | class Obstacle; 57 | class RVOSimulator; 58 | 59 | /** 60 | * \brief Computes the squared distance from a line segment with the 61 | * specified endpoints to a specified point. 62 | * \param a The first endpoint of the line segment. 63 | * \param b The second endpoint of the line segment. 64 | * \param c The point to which the squared distance is to 65 | * be calculated. 66 | * \return The squared distance from the line segment to the point. 67 | */ 68 | inline float distSqPointLineSegment(const Vector2 &a, const Vector2 &b, 69 | const Vector2 &c) 70 | { 71 | const float r = ((c - a) * (b - a)) / absSq(b - a); 72 | 73 | if (r < 0.0f) { 74 | return absSq(c - a); 75 | } 76 | else if (r > 1.0f) { 77 | return absSq(c - b); 78 | } 79 | else { 80 | return absSq(c - (a + r * (b - a))); 81 | } 82 | } 83 | 84 | /** 85 | * \brief Computes the signed distance from a line connecting the 86 | * specified points to a specified point. 87 | * \param a The first point on the line. 88 | * \param b The second point on the line. 89 | * \param c The point to which the signed distance is to 90 | * be calculated. 91 | * \return Positive when the point c lies to the left of the line ab. 92 | */ 93 | inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c) 94 | { 95 | return det(a - c, b - a); 96 | } 97 | 98 | /** 99 | * \brief Computes the square of a float. 100 | * \param a The float to be squared. 101 | * \return The square of the float. 102 | */ 103 | inline float sqr(float a) 104 | { 105 | return a * a; 106 | } 107 | } 108 | 109 | #endif /* RVO_DEFINITIONS_H_ */ 110 | -------------------------------------------------------------------------------- /examples/Circle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Circle.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 . 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 | * 31 | */ 32 | 33 | /* 34 | * Example file showing a demo with 250 agents initially positioned evenly 35 | * distributed on a circle attempting to move to the antipodal position on the 36 | * circle. 37 | */ 38 | 39 | #ifndef RVO_OUTPUT_TIME_AND_POSITIONS 40 | #define RVO_OUTPUT_TIME_AND_POSITIONS 1 41 | #endif 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | #if RVO_OUTPUT_TIME_AND_POSITIONS 48 | #include 49 | #endif 50 | 51 | #if _OPENMP 52 | #include 53 | #endif 54 | 55 | #include 56 | 57 | #ifndef M_PI 58 | const float M_PI = 3.14159265358979323846f; 59 | #endif 60 | 61 | /* Store the goals of the agents. */ 62 | std::vector goals; 63 | 64 | void setupScenario(RVO::RVOSimulator *sim) 65 | { 66 | /* Specify the global time step of the simulation. */ 67 | sim->setTimeStep(0.25f); 68 | 69 | /* Specify the default parameters for agents that are subsequently added. */ 70 | sim->setAgentDefaults(15.0f, 10, 10.0f, 10.0f, 1.5f, 2.0f); 71 | 72 | /* 73 | * Add agents, specifying their start position, and store their goals on the 74 | * opposite side of the environment. 75 | */ 76 | for (size_t i = 0; i < 250; ++i) { 77 | sim->addAgent(200.0f * 78 | RVO::Vector2(std::cos(i * 2.0f * M_PI / 250.0f), 79 | std::sin(i * 2.0f * M_PI / 250.0f))); 80 | goals.push_back(-sim->getAgentPosition(i)); 81 | } 82 | } 83 | 84 | #if RVO_OUTPUT_TIME_AND_POSITIONS 85 | void updateVisualization(RVO::RVOSimulator *sim) 86 | { 87 | /* Output the current global time. */ 88 | std::cout << sim->getGlobalTime(); 89 | 90 | /* Output the current position of all the agents. */ 91 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 92 | std::cout << " " << sim->getAgentPosition(i); 93 | } 94 | 95 | std::cout << std::endl; 96 | } 97 | #endif 98 | 99 | void setPreferredVelocities(RVO::RVOSimulator *sim) 100 | { 101 | /* 102 | * Set the preferred velocity to be a vector of unit magnitude (speed) in the 103 | * direction of the goal. 104 | */ 105 | #ifdef _OPENMP 106 | #pragma omp parallel for 107 | #endif 108 | for (int i = 0; i < static_cast(sim->getNumAgents()); ++i) { 109 | RVO::Vector2 goalVector = goals[i] - sim->getAgentPosition(i); 110 | 111 | if (RVO::absSq(goalVector) > 1.0f) { 112 | goalVector = RVO::normalize(goalVector); 113 | } 114 | 115 | sim->setAgentPrefVelocity(i, goalVector); 116 | } 117 | } 118 | 119 | bool reachedGoal(RVO::RVOSimulator *sim) 120 | { 121 | /* Check if all agents have reached their goals. */ 122 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 123 | if (RVO::absSq(sim->getAgentPosition(i) - goals[i]) > sim->getAgentRadius(i) * sim->getAgentRadius(i)) { 124 | return false; 125 | } 126 | } 127 | 128 | return true; 129 | } 130 | 131 | int main() 132 | { 133 | /* Create a new simulator instance. */ 134 | RVO::RVOSimulator *sim = new RVO::RVOSimulator(); 135 | 136 | /* Set up the scenario. */ 137 | setupScenario(sim); 138 | 139 | /* Perform (and manipulate) the simulation. */ 140 | do { 141 | #if RVO_OUTPUT_TIME_AND_POSITIONS 142 | updateVisualization(sim); 143 | #endif 144 | setPreferredVelocities(sim); 145 | sim->doStep(); 146 | } 147 | while (!reachedGoal(sim)); 148 | 149 | delete sim; 150 | 151 | return 0; 152 | } 153 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Python bindings for Optimal Reciprocal Collision Avoidance 2 | ========================================================== 3 | 4 | This repository contains the RVO2 framework, as described below, along with 5 | [Cython](http://cython.org/)-based Python bindings. Its home is 6 | [GitHub](https://github.com/sybrenstuvel/Python-RVO2). New updates are released 7 | there. There are no explicit version numbers -- all commits on the master 8 | branch are supposed to be stable. 9 | 10 | 11 | Building & installing 12 | ---------------------- 13 | 14 | Building requires [CMake](http://cmake.org/) and [Cython](http://cython.org/) to be installed. 15 | Run `pip install -r requirements.txt` to install the tested version of Cython, or run 16 | `pip install Cython` to install the latest version. 17 | 18 | Run `python setup.py build` to build, and `python setup.py install` to install. 19 | Alternatively, if you want an in-place build that puts the compiled library right in 20 | the current directory, run `python setup.py build_ext --inplace` 21 | 22 | Only tested with Python 2.7, 3.4, and 3.6 on Ubuntu Linux. The setup.py script uses CMake to build 23 | the RVO2 library itself, before building the Python wrappers. If you have success (or failure) 24 | stories, please share them! 25 | 26 | To build on Mac OSX, give an `export MACOSX_DEPLOYMENT_TARGET=10.xx` command first, before 27 | running `python setup.py build`. Replace `10.xx` with your version of OSX, for example `10.11`. 28 | 29 | Differences with the C++ version 30 | -------------------------------- 31 | 32 | The `Vector2` and `Line` classes from the RVO2 library are _not_ wrapped. Instead, 33 | vectors are passed as tuples `(x, y)` from/to Python. Lines are passed as tuples 34 | `(point x, point y, direction x, direction y)`. 35 | 36 | 37 | Example code 38 | ------------ 39 | 40 | ```python 41 | #!/usr/bin/env python 42 | 43 | import rvo2 44 | 45 | sim = rvo2.PyRVOSimulator(1/60., 1.5, 5, 1.5, 2, 0.4, 2) 46 | 47 | # Pass either just the position (the other parameters then use 48 | # the default values passed to the PyRVOSimulator constructor), 49 | # or pass all available parameters. 50 | a0 = sim.addAgent((0, 0)) 51 | a1 = sim.addAgent((1, 0)) 52 | a2 = sim.addAgent((1, 1)) 53 | a3 = sim.addAgent((0, 1), 1.5, 5, 1.5, 2, 0.4, 2, (0, 0)) 54 | 55 | # Obstacles are also supported. 56 | o1 = sim.addObstacle([(0.1, 0.1), (-0.1, 0.1), (-0.1, -0.1)]) 57 | sim.processObstacles() 58 | 59 | sim.setAgentPrefVelocity(a0, (1, 1)) 60 | sim.setAgentPrefVelocity(a1, (-1, 1)) 61 | sim.setAgentPrefVelocity(a2, (-1, -1)) 62 | sim.setAgentPrefVelocity(a3, (1, -1)) 63 | 64 | print('Simulation has %i agents and %i obstacle vertices in it.' % 65 | (sim.getNumAgents(), sim.getNumObstacleVertices())) 66 | 67 | print('Running simulation') 68 | 69 | for step in range(20): 70 | sim.doStep() 71 | 72 | positions = ['(%5.3f, %5.3f)' % sim.getAgentPosition(agent_no) 73 | for agent_no in (a0, a1, a2, a3)] 74 | print('step=%2i t=%.3f %s' % (step, sim.getGlobalTime(), ' '.join(positions))) 75 | ``` 76 | 77 | 78 | Threading support 79 | -------------------------------- 80 | 81 | Calling Python-RVO2 from multiple threads has not been tested. However, code that 82 | may take longer to run (`doStep()`, `processObstacles()` and `queryVisibility(...)`) 83 | release the Global Interpreter Lock (GIL) so that other Python threads can run while 84 | RVO2 is processing. 85 | 86 | 87 | Optimal Reciprocal Collision Avoidance 88 | ====================================== 89 | 90 | 91 | 92 | [![Build Status](https://travis-ci.org/snape/RVO2.svg?branch=master)](https://travis-ci.org/snape/RVO2) 93 | [![Build status](https://ci.appveyor.com/api/projects/status/0nyp7y4di8x1gh9o/branch/master?svg=true)](https://ci.appveyor.com/project/snape/rvo2) 94 | 95 | Copyright 2008 University of North Carolina at Chapel Hill 96 | 97 | Licensed under the Apache License, Version 2.0 (the "License"); 98 | you may not use this file except in compliance with the License. 99 | You may obtain a copy of the License at 100 | 101 | 102 | 103 | Unless required by applicable law or agreed to in writing, software 104 | distributed under the License is distributed on an "AS IS" BASIS, 105 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 106 | See the License for the specific language governing permissions and 107 | limitations under the License. 108 | 109 | Please send all bug reports for the Python wrapper to 110 | [Python-RVO2](https://github.com/sybrenstuvel/Python-RVO2), and bug 111 | report for the RVO2 library itself to [geom@cs.unc.edu](mailto:geom@cs.unc.edu). 112 | 113 | The RVO2 authors may be contacted via: 114 | 115 | Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and Dinesh Manocha 116 | Dept. of Computer Science 117 | 201 S. Columbia St. 118 | Frederick P. Brooks, Jr. Computer Science Bldg. 119 | Chapel Hill, N.C. 27599-3175 120 | United States of America 121 | -------------------------------------------------------------------------------- /src/Agent.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Agent.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 . 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 | * 31 | */ 32 | 33 | #ifndef RVO_AGENT_H_ 34 | #define RVO_AGENT_H_ 35 | 36 | /** 37 | * \file Agent.h 38 | * \brief Contains the Agent class. 39 | */ 40 | 41 | #include "Definitions.h" 42 | #include "RVOSimulator.h" 43 | 44 | namespace RVO { 45 | /** 46 | * \brief Defines an agent in the simulation. 47 | */ 48 | class Agent { 49 | private: 50 | /** 51 | * \brief Constructs an agent instance. 52 | * \param sim The simulator instance. 53 | */ 54 | explicit Agent(RVOSimulator *sim); 55 | 56 | /** 57 | * \brief Computes the neighbors of this agent. 58 | */ 59 | void computeNeighbors(); 60 | 61 | /** 62 | * \brief Computes the new velocity of this agent. 63 | */ 64 | void computeNewVelocity(); 65 | 66 | /** 67 | * \brief Inserts an agent neighbor into the set of neighbors of 68 | * this agent. 69 | * \param agent A pointer to the agent to be inserted. 70 | * \param rangeSq The squared range around this agent. 71 | */ 72 | void insertAgentNeighbor(const Agent *agent, float &rangeSq); 73 | 74 | /** 75 | * \brief Inserts a static obstacle neighbor into the set of neighbors 76 | * of this agent. 77 | * \param obstacle The number of the static obstacle to be 78 | * inserted. 79 | * \param rangeSq The squared range around this agent. 80 | */ 81 | void insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq); 82 | 83 | /** 84 | * \brief Updates the two-dimensional position and two-dimensional 85 | * velocity of this agent. 86 | */ 87 | void update(); 88 | 89 | std::vector > agentNeighbors_; 90 | size_t maxNeighbors_; 91 | float maxSpeed_; 92 | float neighborDist_; 93 | Vector2 newVelocity_; 94 | std::vector > obstacleNeighbors_; 95 | std::vector orcaLines_; 96 | Vector2 position_; 97 | Vector2 prefVelocity_; 98 | float radius_; 99 | RVOSimulator *sim_; 100 | float timeHorizon_; 101 | float timeHorizonObst_; 102 | Vector2 velocity_; 103 | 104 | size_t id_; 105 | 106 | friend class KdTree; 107 | friend class RVOSimulator; 108 | }; 109 | 110 | /** 111 | * \relates Agent 112 | * \brief Solves a one-dimensional linear program on a specified line 113 | * subject to linear constraints defined by lines and a circular 114 | * constraint. 115 | * \param lines Lines defining the linear constraints. 116 | * \param lineNo The specified line constraint. 117 | * \param radius The radius of the circular constraint. 118 | * \param optVelocity The optimization velocity. 119 | * \param directionOpt True if the direction should be optimized. 120 | * \param result A reference to the result of the linear program. 121 | * \return True if successful. 122 | */ 123 | bool linearProgram1(const std::vector &lines, size_t lineNo, 124 | float radius, const Vector2 &optVelocity, 125 | bool directionOpt, Vector2 &result); 126 | 127 | /** 128 | * \relates Agent 129 | * \brief Solves a two-dimensional linear program subject to linear 130 | * constraints defined by lines and a circular constraint. 131 | * \param lines Lines defining the linear constraints. 132 | * \param radius The radius of the circular constraint. 133 | * \param optVelocity The optimization velocity. 134 | * \param directionOpt True if the direction should be optimized. 135 | * \param result A reference to the result of the linear program. 136 | * \return The number of the line it fails on, and the number of lines if successful. 137 | */ 138 | size_t linearProgram2(const std::vector &lines, float radius, 139 | const Vector2 &optVelocity, bool directionOpt, 140 | Vector2 &result); 141 | 142 | /** 143 | * \relates Agent 144 | * \brief Solves a two-dimensional linear program subject to linear 145 | * constraints defined by lines and a circular constraint. 146 | * \param lines Lines defining the linear constraints. 147 | * \param numObstLines Count of obstacle lines. 148 | * \param beginLine The line on which the 2-d linear program failed. 149 | * \param radius The radius of the circular constraint. 150 | * \param result A reference to the result of the linear program. 151 | */ 152 | void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, 153 | float radius, Vector2 &result); 154 | } 155 | 156 | #endif /* RVO_AGENT_H_ */ 157 | -------------------------------------------------------------------------------- /src/KdTree.h: -------------------------------------------------------------------------------- 1 | /* 2 | * KdTree.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 . 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 | * 31 | */ 32 | 33 | #ifndef RVO_KD_TREE_H_ 34 | #define RVO_KD_TREE_H_ 35 | 36 | /** 37 | * \file KdTree.h 38 | * \brief Contains the KdTree class. 39 | */ 40 | 41 | #include "Definitions.h" 42 | 43 | namespace RVO { 44 | /** 45 | * \brief Defines kd-trees for agents and static obstacles in the 46 | * simulation. 47 | */ 48 | class KdTree { 49 | private: 50 | /** 51 | * \brief Defines an agent kd-tree node. 52 | */ 53 | class AgentTreeNode { 54 | public: 55 | /** 56 | * \brief The beginning node number. 57 | */ 58 | size_t begin; 59 | 60 | /** 61 | * \brief The ending node number. 62 | */ 63 | size_t end; 64 | 65 | /** 66 | * \brief The left node number. 67 | */ 68 | size_t left; 69 | 70 | /** 71 | * \brief The maximum x-coordinate. 72 | */ 73 | float maxX; 74 | 75 | /** 76 | * \brief The maximum y-coordinate. 77 | */ 78 | float maxY; 79 | 80 | /** 81 | * \brief The minimum x-coordinate. 82 | */ 83 | float minX; 84 | 85 | /** 86 | * \brief The minimum y-coordinate. 87 | */ 88 | float minY; 89 | 90 | /** 91 | * \brief The right node number. 92 | */ 93 | size_t right; 94 | }; 95 | 96 | /** 97 | * \brief Defines an obstacle kd-tree node. 98 | */ 99 | class ObstacleTreeNode { 100 | public: 101 | /** 102 | * \brief The left obstacle tree node. 103 | */ 104 | ObstacleTreeNode *left; 105 | 106 | /** 107 | * \brief The obstacle number. 108 | */ 109 | const Obstacle *obstacle; 110 | 111 | /** 112 | * \brief The right obstacle tree node. 113 | */ 114 | ObstacleTreeNode *right; 115 | }; 116 | 117 | /** 118 | * \brief Constructs a kd-tree instance. 119 | * \param sim The simulator instance. 120 | */ 121 | explicit KdTree(RVOSimulator *sim); 122 | 123 | /** 124 | * \brief Destroys this kd-tree instance. 125 | */ 126 | ~KdTree(); 127 | 128 | /** 129 | * \brief Builds an agent kd-tree. 130 | */ 131 | void buildAgentTree(); 132 | 133 | void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); 134 | 135 | /** 136 | * \brief Builds an obstacle kd-tree. 137 | */ 138 | void buildObstacleTree(); 139 | 140 | ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector & 141 | obstacles); 142 | 143 | /** 144 | * \brief Computes the agent neighbors of the specified agent. 145 | * \param agent A pointer to the agent for which agent 146 | * neighbors are to be computed. 147 | * \param rangeSq The squared range around the agent. 148 | */ 149 | void computeAgentNeighbors(Agent *agent, float &rangeSq) const; 150 | 151 | /** 152 | * \brief Computes the obstacle neighbors of the specified agent. 153 | * \param agent A pointer to the agent for which obstacle 154 | * neighbors are to be computed. 155 | * \param rangeSq The squared range around the agent. 156 | */ 157 | void computeObstacleNeighbors(Agent *agent, float rangeSq) const; 158 | 159 | /** 160 | * \brief Deletes the specified obstacle tree node. 161 | * \param node A pointer to the obstacle tree node to be 162 | * deleted. 163 | */ 164 | void deleteObstacleTree(ObstacleTreeNode *node); 165 | 166 | void queryAgentTreeRecursive(Agent *agent, float &rangeSq, 167 | size_t node) const; 168 | 169 | void queryObstacleTreeRecursive(Agent *agent, float rangeSq, 170 | const ObstacleTreeNode *node) const; 171 | 172 | /** 173 | * \brief Queries the visibility between two points within a 174 | * specified radius. 175 | * \param q1 The first point between which visibility is 176 | * to be tested. 177 | * \param q2 The second point between which visibility is 178 | * to be tested. 179 | * \param radius The radius within which visibility is to be 180 | * tested. 181 | * \return True if q1 and q2 are mutually visible within the radius; 182 | * false otherwise. 183 | */ 184 | bool queryVisibility(const Vector2 &q1, const Vector2 &q2, 185 | float radius) const; 186 | 187 | bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, 188 | float radius, 189 | const ObstacleTreeNode *node) const; 190 | 191 | std::vector agents_; 192 | std::vector agentTree_; 193 | ObstacleTreeNode *obstacleTree_; 194 | RVOSimulator *sim_; 195 | 196 | static const size_t MAX_LEAF_SIZE = 10; 197 | 198 | friend class Agent; 199 | friend class RVOSimulator; 200 | }; 201 | } 202 | 203 | #endif /* RVO_KD_TREE_H_ */ 204 | -------------------------------------------------------------------------------- /examples/Blocks.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Blocks.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 . 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 | * 31 | */ 32 | 33 | /* 34 | * Example file showing a demo with 100 agents split in four groups initially 35 | * positioned in four corners of the environment. Each agent attempts to move to 36 | * other side of the environment through a narrow passage generated by four 37 | * obstacles. There is no roadmap to guide the agents around the obstacles. 38 | */ 39 | 40 | #ifndef RVO_OUTPUT_TIME_AND_POSITIONS 41 | #define RVO_OUTPUT_TIME_AND_POSITIONS 1 42 | #endif 43 | 44 | #ifndef RVO_SEED_RANDOM_NUMBER_GENERATOR 45 | #define RVO_SEED_RANDOM_NUMBER_GENERATOR 1 46 | #endif 47 | 48 | #include 49 | #include 50 | 51 | #include 52 | 53 | #if RVO_OUTPUT_TIME_AND_POSITIONS 54 | #include 55 | #endif 56 | 57 | #if RVO_SEED_RANDOM_NUMBER_GENERATOR 58 | #include 59 | #endif 60 | 61 | #if _OPENMP 62 | #include 63 | #endif 64 | 65 | #include 66 | 67 | #ifndef M_PI 68 | const float M_PI = 3.14159265358979323846f; 69 | #endif 70 | 71 | /* Store the goals of the agents. */ 72 | std::vector goals; 73 | 74 | void setupScenario(RVO::RVOSimulator *sim) 75 | { 76 | #if RVO_SEED_RANDOM_NUMBER_GENERATOR 77 | std::srand(static_cast(std::time(NULL))); 78 | #endif 79 | 80 | /* Specify the global time step of the simulation. */ 81 | sim->setTimeStep(0.25f); 82 | 83 | /* Specify the default parameters for agents that are subsequently added. */ 84 | sim->setAgentDefaults(15.0f, 10, 5.0f, 5.0f, 2.0f, 2.0f); 85 | 86 | /* 87 | * Add agents, specifying their start position, and store their goals on the 88 | * opposite side of the environment. 89 | */ 90 | for (size_t i = 0; i < 5; ++i) { 91 | for (size_t j = 0; j < 5; ++j) { 92 | sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, 55.0f + j * 10.0f)); 93 | goals.push_back(RVO::Vector2(-75.0f, -75.0f)); 94 | 95 | sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, 55.0f + j * 10.0f)); 96 | goals.push_back(RVO::Vector2(75.0f, -75.0f)); 97 | 98 | sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, -55.0f - j * 10.0f)); 99 | goals.push_back(RVO::Vector2(-75.0f, 75.0f)); 100 | 101 | sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, -55.0f - j * 10.0f)); 102 | goals.push_back(RVO::Vector2(75.0f, 75.0f)); 103 | } 104 | } 105 | 106 | /* 107 | * Add (polygonal) obstacles, specifying their vertices in counterclockwise 108 | * order. 109 | */ 110 | std::vector obstacle1, obstacle2, obstacle3, obstacle4; 111 | 112 | obstacle1.push_back(RVO::Vector2(-10.0f, 40.0f)); 113 | obstacle1.push_back(RVO::Vector2(-40.0f, 40.0f)); 114 | obstacle1.push_back(RVO::Vector2(-40.0f, 10.0f)); 115 | obstacle1.push_back(RVO::Vector2(-10.0f, 10.0f)); 116 | 117 | obstacle2.push_back(RVO::Vector2(10.0f, 40.0f)); 118 | obstacle2.push_back(RVO::Vector2(10.0f, 10.0f)); 119 | obstacle2.push_back(RVO::Vector2(40.0f, 10.0f)); 120 | obstacle2.push_back(RVO::Vector2(40.0f, 40.0f)); 121 | 122 | obstacle3.push_back(RVO::Vector2(10.0f, -40.0f)); 123 | obstacle3.push_back(RVO::Vector2(40.0f, -40.0f)); 124 | obstacle3.push_back(RVO::Vector2(40.0f, -10.0f)); 125 | obstacle3.push_back(RVO::Vector2(10.0f, -10.0f)); 126 | 127 | obstacle4.push_back(RVO::Vector2(-10.0f, -40.0f)); 128 | obstacle4.push_back(RVO::Vector2(-10.0f, -10.0f)); 129 | obstacle4.push_back(RVO::Vector2(-40.0f, -10.0f)); 130 | obstacle4.push_back(RVO::Vector2(-40.0f, -40.0f)); 131 | 132 | sim->addObstacle(obstacle1); 133 | sim->addObstacle(obstacle2); 134 | sim->addObstacle(obstacle3); 135 | sim->addObstacle(obstacle4); 136 | 137 | /* Process the obstacles so that they are accounted for in the simulation. */ 138 | sim->processObstacles(); 139 | } 140 | 141 | #if RVO_OUTPUT_TIME_AND_POSITIONS 142 | void updateVisualization(RVO::RVOSimulator *sim) 143 | { 144 | /* Output the current global time. */ 145 | std::cout << sim->getGlobalTime(); 146 | 147 | /* Output the current position of all the agents. */ 148 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 149 | std::cout << " " << sim->getAgentPosition(i); 150 | } 151 | 152 | std::cout << std::endl; 153 | } 154 | #endif 155 | 156 | void setPreferredVelocities(RVO::RVOSimulator *sim) 157 | { 158 | /* 159 | * Set the preferred velocity to be a vector of unit magnitude (speed) in the 160 | * direction of the goal. 161 | */ 162 | #ifdef _OPENMP 163 | #pragma omp parallel for 164 | #endif 165 | for (int i = 0; i < static_cast(sim->getNumAgents()); ++i) { 166 | RVO::Vector2 goalVector = goals[i] - sim->getAgentPosition(i); 167 | 168 | if (RVO::absSq(goalVector) > 1.0f) { 169 | goalVector = RVO::normalize(goalVector); 170 | } 171 | 172 | sim->setAgentPrefVelocity(i, goalVector); 173 | 174 | /* 175 | * Perturb a little to avoid deadlocks due to perfect symmetry. 176 | */ 177 | float angle = std::rand() * 2.0f * M_PI / RAND_MAX; 178 | float dist = std::rand() * 0.0001f / RAND_MAX; 179 | 180 | sim->setAgentPrefVelocity(i, sim->getAgentPrefVelocity(i) + 181 | dist * RVO::Vector2(std::cos(angle), std::sin(angle))); 182 | } 183 | } 184 | 185 | bool reachedGoal(RVO::RVOSimulator *sim) 186 | { 187 | /* Check if all agents have reached their goals. */ 188 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 189 | if (RVO::absSq(sim->getAgentPosition(i) - goals[i]) > 20.0f * 20.0f) { 190 | return false; 191 | } 192 | } 193 | 194 | return true; 195 | } 196 | 197 | int main() 198 | { 199 | /* Create a new simulator instance. */ 200 | RVO::RVOSimulator *sim = new RVO::RVOSimulator(); 201 | 202 | /* Set up the scenario. */ 203 | setupScenario(sim); 204 | 205 | /* Perform (and manipulate) the simulation. */ 206 | do { 207 | #if RVO_OUTPUT_TIME_AND_POSITIONS 208 | updateVisualization(sim); 209 | #endif 210 | setPreferredVelocities(sim); 211 | sim->doStep(); 212 | } 213 | while (!reachedGoal(sim)); 214 | 215 | delete sim; 216 | 217 | return 0; 218 | } 219 | -------------------------------------------------------------------------------- /src/RVOSimulator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RVOSimulator.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 . 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 | * 31 | */ 32 | 33 | #include "RVOSimulator.h" 34 | 35 | #include "Agent.h" 36 | #include "KdTree.h" 37 | #include "Obstacle.h" 38 | 39 | #ifdef _OPENMP 40 | #include 41 | #endif 42 | 43 | namespace RVO { 44 | RVOSimulator::RVOSimulator() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f) 45 | { 46 | kdTree_ = new KdTree(this); 47 | } 48 | 49 | RVOSimulator::RVOSimulator(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 KdTree(this); 52 | defaultAgent_ = new Agent(this); 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 | RVOSimulator::~RVOSimulator() 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 RVOSimulator::addAgent(const Vector2 &position) 81 | { 82 | if (defaultAgent_ == NULL) { 83 | return RVO_ERROR; 84 | } 85 | 86 | Agent *agent = new Agent(this); 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 RVOSimulator::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) 105 | { 106 | Agent *agent = new Agent(this); 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 RVOSimulator::addObstacle(const std::vector &vertices) 125 | { 126 | if (vertices.size() < 2) { 127 | return RVO_ERROR; 128 | } 129 | 130 | const size_t obstacleNo = obstacles_.size(); 131 | 132 | for (size_t i = 0; i < vertices.size(); ++i) { 133 | Obstacle *obstacle = new Obstacle(); 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 RVOSimulator::doStep() 164 | { 165 | kdTree_->buildAgentTree(); 166 | 167 | #ifdef _OPENMP 168 | #pragma omp parallel for 169 | #endif 170 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 171 | agents_[i]->computeNeighbors(); 172 | agents_[i]->computeNewVelocity(); 173 | } 174 | 175 | #ifdef _OPENMP 176 | #pragma omp parallel for 177 | #endif 178 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 179 | agents_[i]->update(); 180 | } 181 | 182 | globalTime_ += timeStep_; 183 | } 184 | 185 | size_t RVOSimulator::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const 186 | { 187 | return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_; 188 | } 189 | 190 | size_t RVOSimulator::getAgentMaxNeighbors(size_t agentNo) const 191 | { 192 | return agents_[agentNo]->maxNeighbors_; 193 | } 194 | 195 | float RVOSimulator::getAgentMaxSpeed(size_t agentNo) const 196 | { 197 | return agents_[agentNo]->maxSpeed_; 198 | } 199 | 200 | float RVOSimulator::getAgentNeighborDist(size_t agentNo) const 201 | { 202 | return agents_[agentNo]->neighborDist_; 203 | } 204 | 205 | size_t RVOSimulator::getAgentNumAgentNeighbors(size_t agentNo) const 206 | { 207 | return agents_[agentNo]->agentNeighbors_.size(); 208 | } 209 | 210 | size_t RVOSimulator::getAgentNumObstacleNeighbors(size_t agentNo) const 211 | { 212 | return agents_[agentNo]->obstacleNeighbors_.size(); 213 | } 214 | 215 | size_t RVOSimulator::getAgentNumORCALines(size_t agentNo) const 216 | { 217 | return agents_[agentNo]->orcaLines_.size(); 218 | } 219 | 220 | size_t RVOSimulator::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const 221 | { 222 | return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_; 223 | } 224 | 225 | const Line &RVOSimulator::getAgentORCALine(size_t agentNo, size_t lineNo) const 226 | { 227 | return agents_[agentNo]->orcaLines_[lineNo]; 228 | } 229 | 230 | const Vector2 &RVOSimulator::getAgentPosition(size_t agentNo) const 231 | { 232 | return agents_[agentNo]->position_; 233 | } 234 | 235 | const Vector2 &RVOSimulator::getAgentPrefVelocity(size_t agentNo) const 236 | { 237 | return agents_[agentNo]->prefVelocity_; 238 | } 239 | 240 | float RVOSimulator::getAgentRadius(size_t agentNo) const 241 | { 242 | return agents_[agentNo]->radius_; 243 | } 244 | 245 | float RVOSimulator::getAgentTimeHorizon(size_t agentNo) const 246 | { 247 | return agents_[agentNo]->timeHorizon_; 248 | } 249 | 250 | float RVOSimulator::getAgentTimeHorizonObst(size_t agentNo) const 251 | { 252 | return agents_[agentNo]->timeHorizonObst_; 253 | } 254 | 255 | const Vector2 &RVOSimulator::getAgentVelocity(size_t agentNo) const 256 | { 257 | return agents_[agentNo]->velocity_; 258 | } 259 | 260 | float RVOSimulator::getGlobalTime() const 261 | { 262 | return globalTime_; 263 | } 264 | 265 | size_t RVOSimulator::getNumAgents() const 266 | { 267 | return agents_.size(); 268 | } 269 | 270 | size_t RVOSimulator::getNumObstacleVertices() const 271 | { 272 | return obstacles_.size(); 273 | } 274 | 275 | const Vector2 &RVOSimulator::getObstacleVertex(size_t vertexNo) const 276 | { 277 | return obstacles_[vertexNo]->point_; 278 | } 279 | 280 | size_t RVOSimulator::getNextObstacleVertexNo(size_t vertexNo) const 281 | { 282 | return obstacles_[vertexNo]->nextObstacle_->id_; 283 | } 284 | 285 | size_t RVOSimulator::getPrevObstacleVertexNo(size_t vertexNo) const 286 | { 287 | return obstacles_[vertexNo]->prevObstacle_->id_; 288 | } 289 | 290 | float RVOSimulator::getTimeStep() const 291 | { 292 | return timeStep_; 293 | } 294 | 295 | void RVOSimulator::processObstacles() 296 | { 297 | kdTree_->buildObstacleTree(); 298 | } 299 | 300 | bool RVOSimulator::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const 301 | { 302 | return kdTree_->queryVisibility(point1, point2, radius); 303 | } 304 | 305 | void RVOSimulator::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) 306 | { 307 | if (defaultAgent_ == NULL) { 308 | defaultAgent_ = new Agent(this); 309 | } 310 | 311 | defaultAgent_->maxNeighbors_ = maxNeighbors; 312 | defaultAgent_->maxSpeed_ = maxSpeed; 313 | defaultAgent_->neighborDist_ = neighborDist; 314 | defaultAgent_->radius_ = radius; 315 | defaultAgent_->timeHorizon_ = timeHorizon; 316 | defaultAgent_->timeHorizonObst_ = timeHorizonObst; 317 | defaultAgent_->velocity_ = velocity; 318 | } 319 | 320 | void RVOSimulator::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) 321 | { 322 | agents_[agentNo]->maxNeighbors_ = maxNeighbors; 323 | } 324 | 325 | void RVOSimulator::setAgentMaxSpeed(size_t agentNo, float maxSpeed) 326 | { 327 | agents_[agentNo]->maxSpeed_ = maxSpeed; 328 | } 329 | 330 | void RVOSimulator::setAgentNeighborDist(size_t agentNo, float neighborDist) 331 | { 332 | agents_[agentNo]->neighborDist_ = neighborDist; 333 | } 334 | 335 | void RVOSimulator::setAgentPosition(size_t agentNo, const Vector2 &position) 336 | { 337 | agents_[agentNo]->position_ = position; 338 | } 339 | 340 | void RVOSimulator::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity) 341 | { 342 | agents_[agentNo]->prefVelocity_ = prefVelocity; 343 | } 344 | 345 | void RVOSimulator::setAgentRadius(size_t agentNo, float radius) 346 | { 347 | agents_[agentNo]->radius_ = radius; 348 | } 349 | 350 | void RVOSimulator::setAgentTimeHorizon(size_t agentNo, float timeHorizon) 351 | { 352 | agents_[agentNo]->timeHorizon_ = timeHorizon; 353 | } 354 | 355 | void RVOSimulator::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst) 356 | { 357 | agents_[agentNo]->timeHorizonObst_ = timeHorizonObst; 358 | } 359 | 360 | void RVOSimulator::setAgentVelocity(size_t agentNo, const Vector2 &velocity) 361 | { 362 | agents_[agentNo]->velocity_ = velocity; 363 | } 364 | 365 | void RVOSimulator::setTimeStep(float timeStep) 366 | { 367 | timeStep_ = timeStep; 368 | } 369 | } 370 | -------------------------------------------------------------------------------- /src/rvo2.pyx: -------------------------------------------------------------------------------- 1 | # distutils: language = c++ 2 | from libcpp.vector cimport vector 3 | from libcpp cimport bool 4 | 5 | 6 | cdef extern from "Vector2.h" namespace "RVO": 7 | cdef cppclass Vector2: 8 | Vector2() except + 9 | Vector2(float x, float y) except + 10 | float x() const 11 | float y() const 12 | 13 | 14 | cdef extern from "RVOSimulator.h" namespace "RVO": 15 | cdef const size_t RVO_ERROR 16 | 17 | 18 | cdef extern from "RVOSimulator.h" namespace "RVO": 19 | cdef cppclass Line: 20 | Vector2 point 21 | Vector2 direction 22 | 23 | 24 | cdef extern from "RVOSimulator.h" namespace "RVO": 25 | cdef cppclass RVOSimulator: 26 | RVOSimulator() 27 | RVOSimulator(float timeStep, float neighborDist, size_t maxNeighbors, 28 | float timeHorizon, float timeHorizonObst, float radius, 29 | float maxSpeed, const Vector2 & velocity) 30 | size_t addAgent(const Vector2 & position) 31 | size_t addAgent(const Vector2 & position, float neighborDist, 32 | size_t maxNeighbors, float timeHorizon, 33 | float timeHorizonObst, float radius, float maxSpeed, 34 | const Vector2 & velocity) 35 | size_t addObstacle(const vector[Vector2] & vertices) 36 | void doStep() nogil 37 | size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const 38 | size_t getAgentMaxNeighbors(size_t agentNo) const 39 | float getAgentMaxSpeed(size_t agentNo) const 40 | float getAgentNeighborDist(size_t agentNo) const 41 | size_t getAgentNumAgentNeighbors(size_t agentNo) const 42 | size_t getAgentNumObstacleNeighbors(size_t agentNo) const 43 | size_t getAgentNumORCALines(size_t agentNo) const 44 | size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const 45 | const Line & getAgentORCALine(size_t agentNo, size_t lineNo) const 46 | const Vector2 & getAgentPosition(size_t agentNo) const 47 | const Vector2 & getAgentPrefVelocity(size_t agentNo) const 48 | float getAgentRadius(size_t agentNo) const 49 | float getAgentTimeHorizon(size_t agentNo) const 50 | float getAgentTimeHorizonObst(size_t agentNo) const 51 | const Vector2 & getAgentVelocity(size_t agentNo) const 52 | float getGlobalTime() const 53 | size_t getNumAgents() const 54 | size_t getNumObstacleVertices() const 55 | const Vector2 & getObstacleVertex(size_t vertexNo) const 56 | size_t getNextObstacleVertexNo(size_t vertexNo) const 57 | size_t getPrevObstacleVertexNo(size_t vertexNo) const 58 | float getTimeStep() const 59 | void processObstacles() nogil 60 | bool queryVisibility(const Vector2 & point1, const Vector2 & point2, 61 | float radius) nogil const 62 | void setAgentDefaults(float neighborDist, size_t maxNeighbors, 63 | float timeHorizon, float timeHorizonObst, 64 | float radius, float maxSpeed, 65 | const Vector2 & velocity) 66 | void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) 67 | void setAgentMaxSpeed(size_t agentNo, float maxSpeed) 68 | void setAgentNeighborDist(size_t agentNo, float neighborDist) 69 | void setAgentPosition(size_t agentNo, const Vector2 & position) 70 | void setAgentPrefVelocity(size_t agentNo, const Vector2 & prefVelocity) 71 | void setAgentRadius(size_t agentNo, float radius) 72 | void setAgentTimeHorizon(size_t agentNo, float timeHorizon) 73 | void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst) 74 | void setAgentVelocity(size_t agentNo, const Vector2 & velocity) 75 | void setTimeStep(float timeStep) 76 | 77 | 78 | cdef class PyRVOSimulator: 79 | cdef RVOSimulator *thisptr 80 | 81 | def __cinit__(self, float timeStep, float neighborDist, size_t maxNeighbors, 82 | float timeHorizon, float timeHorizonObst, float radius, 83 | float maxSpeed, tuple velocity=(0, 0)): 84 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 85 | 86 | self.thisptr = new RVOSimulator(timeStep, neighborDist, maxNeighbors, 87 | timeHorizon, timeHorizonObst, radius, 88 | maxSpeed, c_velocity) 89 | 90 | def __dealloc__(self): 91 | del self.thisptr 92 | 93 | def addAgent(self, tuple pos, neighborDist=None, 94 | maxNeighbors=None, timeHorizon=None, 95 | timeHorizonObst=None, radius=None, maxSpeed=None, 96 | velocity=None): 97 | cdef Vector2 c_pos = Vector2(pos[0], pos[1]) 98 | cdef Vector2 c_velocity 99 | 100 | if neighborDist is not None and velocity is None: 101 | raise ValueError("Either pass only 'pos', or pass all parameters.") 102 | 103 | if neighborDist is None: 104 | agent_nr = self.thisptr.addAgent(c_pos) 105 | else: 106 | c_velocity = Vector2(velocity[0], velocity[1]) 107 | agent_nr = self.thisptr.addAgent(c_pos, neighborDist, 108 | maxNeighbors, timeHorizon, 109 | timeHorizonObst, radius, maxSpeed, 110 | c_velocity) 111 | 112 | if agent_nr == RVO_ERROR: 113 | raise RuntimeError('Error adding agent to RVO simulation') 114 | 115 | return agent_nr 116 | 117 | def addObstacle(self, list vertices): 118 | cdef vector[Vector2] c_vertices 119 | 120 | for x, y in vertices: 121 | c_vertices.push_back(Vector2(x, y)) 122 | 123 | obstacle_nr = self.thisptr.addObstacle(c_vertices) 124 | if obstacle_nr == RVO_ERROR: 125 | raise RuntimeError('Error adding obstacle to RVO simulation') 126 | return obstacle_nr 127 | 128 | def doStep(self): 129 | with nogil: 130 | self.thisptr.doStep() 131 | 132 | def getAgentAgentNeighbor(self, size_t agent_no, size_t neighbor_no): 133 | return self.thisptr.getAgentAgentNeighbor(agent_no, neighbor_no) 134 | def getAgentMaxNeighbors(self, size_t agent_no): 135 | return self.thisptr.getAgentMaxNeighbors(agent_no) 136 | def getAgentMaxSpeed(self, size_t agent_no): 137 | return self.thisptr.getAgentMaxSpeed(agent_no) 138 | def getAgentNeighborDist(self, size_t agent_no): 139 | return self.thisptr.getAgentNeighborDist(agent_no) 140 | def getAgentNumAgentNeighbors(self, size_t agent_no): 141 | return self.thisptr.getAgentNumAgentNeighbors(agent_no) 142 | def getAgentNumObstacleNeighbors(self, size_t agent_no): 143 | return self.thisptr.getAgentNumObstacleNeighbors(agent_no) 144 | def getAgentNumORCALines(self, size_t agent_no): 145 | return self.thisptr.getAgentNumORCALines(agent_no) 146 | def getAgentObstacleNeighbor(self, size_t agent_no, size_t obstacle_no): 147 | return self.thisptr.getAgentObstacleNeighbor(agent_no, obstacle_no) 148 | def getAgentORCALine(self, size_t agent_no, size_t line_no): 149 | cdef Line line = self.thisptr.getAgentORCALine(agent_no, line_no) 150 | return line.point.x(), line.point.y(), line.direction.x(), line.direction.y() 151 | def getAgentPosition(self, size_t agent_no): 152 | cdef Vector2 pos = self.thisptr.getAgentPosition(agent_no) 153 | return pos.x(), pos.y() 154 | def getAgentPrefVelocity(self, size_t agent_no): 155 | cdef Vector2 velocity = self.thisptr.getAgentPrefVelocity(agent_no) 156 | return velocity.x(), velocity.y() 157 | def getAgentRadius(self, size_t agent_no): 158 | return self.thisptr.getAgentRadius(agent_no) 159 | def getAgentTimeHorizon(self, size_t agent_no): 160 | return self.thisptr.getAgentTimeHorizon(agent_no) 161 | def getAgentTimeHorizonObst(self, size_t agent_no): 162 | return self.thisptr.getAgentTimeHorizonObst(agent_no) 163 | def getAgentVelocity(self, size_t agent_no): 164 | cdef Vector2 velocity = self.thisptr.getAgentVelocity(agent_no) 165 | return velocity.x(), velocity.y() 166 | def getGlobalTime(self): 167 | return self.thisptr.getGlobalTime() 168 | def getNumAgents(self): 169 | return self.thisptr.getNumAgents() 170 | def getNumObstacleVertices(self): 171 | return self.thisptr.getNumObstacleVertices() 172 | def getObstacleVertex(self, size_t vertex_no): 173 | cdef Vector2 pos = self.thisptr.getObstacleVertex(vertex_no) 174 | return pos.x(), pos.y() 175 | def getNextObstacleVertexNo(self, size_t vertex_no): 176 | return self.thisptr.getNextObstacleVertexNo(vertex_no) 177 | def getPrevObstacleVertexNo(self, size_t vertex_no): 178 | return self.thisptr.getPrevObstacleVertexNo(vertex_no) 179 | def getTimeStep(self): 180 | return self.thisptr.getTimeStep() 181 | 182 | def processObstacles(self): 183 | with nogil: 184 | self.thisptr.processObstacles() 185 | 186 | def queryVisibility(self, tuple point1, tuple point2, float radius=0.0): 187 | cdef Vector2 c_point1 = Vector2(point1[0], point1[1]) 188 | cdef Vector2 c_point2 = Vector2(point2[0], point2[1]) 189 | cdef bool visible 190 | 191 | with nogil: 192 | visible = self.thisptr.queryVisibility(c_point1, c_point2, radius) 193 | 194 | return visible 195 | 196 | def setAgentDefaults(self, float neighbor_dist, size_t max_neighbors, float time_horizon, 197 | float time_horizon_obst, float radius, float max_speed, 198 | tuple velocity=(0, 0)): 199 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 200 | self.thisptr.setAgentDefaults(neighbor_dist, max_neighbors, time_horizon, 201 | time_horizon_obst, radius, max_speed, c_velocity) 202 | 203 | def setAgentMaxNeighbors(self, size_t agent_no, size_t max_neighbors): 204 | self.thisptr.setAgentMaxNeighbors(agent_no, max_neighbors) 205 | def setAgentMaxSpeed(self, size_t agent_no, float max_speed): 206 | self.thisptr.setAgentMaxSpeed(agent_no, max_speed) 207 | def setAgentNeighborDist(self, size_t agent_no, float neighbor_dist): 208 | self.thisptr.setAgentNeighborDist(agent_no, neighbor_dist) 209 | def setAgentNeighborDist(self, size_t agent_no, float neighbor_dist): 210 | self.thisptr.setAgentNeighborDist(agent_no, neighbor_dist) 211 | def setAgentPosition(self, size_t agent_no, tuple position): 212 | cdef Vector2 c_pos = Vector2(position[0], position[1]) 213 | self.thisptr.setAgentPosition(agent_no, c_pos) 214 | def setAgentPrefVelocity(self, size_t agent_no, tuple velocity): 215 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 216 | self.thisptr.setAgentPrefVelocity(agent_no, c_velocity) 217 | def setAgentRadius(self, size_t agent_no, float radius): 218 | self.thisptr.setAgentRadius(agent_no, radius) 219 | def setAgentTimeHorizon(self, size_t agent_no, float time_horizon): 220 | self.thisptr.setAgentTimeHorizon(agent_no, time_horizon) 221 | def setAgentTimeHorizonObst(self, size_t agent_no, float timeHorizonObst): 222 | self.thisptr.setAgentTimeHorizonObst(agent_no, timeHorizonObst) 223 | def setAgentVelocity(self, size_t agent_no, tuple velocity): 224 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 225 | self.thisptr.setAgentVelocity(agent_no, c_velocity) 226 | def setTimeStep(self, float time_step): 227 | self.thisptr.setTimeStep(time_step) 228 | -------------------------------------------------------------------------------- /examples/Roadmap.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Roadmap.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 . 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 | * 31 | */ 32 | 33 | /* 34 | * Example file showing a demo with 100 agents split in four groups initially 35 | * positioned in four corners of the environment. Each agent attempts to move to 36 | * other side of the environment through a narrow passage generated by four 37 | * obstacles. There is a roadmap to guide the agents around the obstacles. 38 | */ 39 | 40 | #ifndef RVO_OUTPUT_TIME_AND_POSITIONS 41 | #define RVO_OUTPUT_TIME_AND_POSITIONS 1 42 | #endif 43 | 44 | #ifndef RVO_SEED_RANDOM_NUMBER_GENERATOR 45 | #define RVO_SEED_RANDOM_NUMBER_GENERATOR 1 46 | #endif 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #if RVO_OUTPUT_TIME_AND_POSITIONS 54 | #include 55 | #endif 56 | 57 | #if RVO_SEED_RANDOM_NUMBER_GENERATOR 58 | #include 59 | #endif 60 | 61 | #if _OPENMP 62 | #include 63 | #endif 64 | 65 | #include 66 | 67 | #ifndef M_PI 68 | const float M_PI = 3.14159265358979323846f; 69 | #endif 70 | 71 | class RoadmapVertex { 72 | public: 73 | RVO::Vector2 position; 74 | std::vector neighbors; 75 | std::vector distToGoal; 76 | }; 77 | 78 | /* Store the roadmap. */ 79 | std::vector roadmap; 80 | 81 | /* Store the goals of the agents. */ 82 | std::vector goals; 83 | 84 | void setupScenario(RVO::RVOSimulator *sim) 85 | { 86 | #if RVO_SEED_RANDOM_NUMBER_GENERATOR 87 | std::srand(static_cast(std::time(NULL))); 88 | #endif 89 | 90 | /* Specify the global time step of the simulation. */ 91 | sim->setTimeStep(0.25f); 92 | 93 | /* 94 | * Add (polygonal) obstacles, specifying their vertices in counterclockwise 95 | * order. 96 | */ 97 | std::vector obstacle1, obstacle2, obstacle3, obstacle4; 98 | 99 | obstacle1.push_back(RVO::Vector2(-10.0f, 40.0f)); 100 | obstacle1.push_back(RVO::Vector2(-40.0f, 40.0f)); 101 | obstacle1.push_back(RVO::Vector2(-40.0f, 10.0f)); 102 | obstacle1.push_back(RVO::Vector2(-10.0f, 10.0f)); 103 | 104 | obstacle2.push_back(RVO::Vector2(10.0f, 40.0f)); 105 | obstacle2.push_back(RVO::Vector2(10.0f, 10.0f)); 106 | obstacle2.push_back(RVO::Vector2(40.0f, 10.0f)); 107 | obstacle2.push_back(RVO::Vector2(40.0f, 40.0f)); 108 | 109 | obstacle3.push_back(RVO::Vector2(10.0f, -40.0f)); 110 | obstacle3.push_back(RVO::Vector2(40.0f, -40.0f)); 111 | obstacle3.push_back(RVO::Vector2(40.0f, -10.0f)); 112 | obstacle3.push_back(RVO::Vector2(10.0f, -10.0f)); 113 | 114 | obstacle4.push_back(RVO::Vector2(-10.0f, -40.0f)); 115 | obstacle4.push_back(RVO::Vector2(-10.0f, -10.0f)); 116 | obstacle4.push_back(RVO::Vector2(-40.0f, -10.0f)); 117 | obstacle4.push_back(RVO::Vector2(-40.0f, -40.0f)); 118 | 119 | sim->addObstacle(obstacle1); 120 | sim->addObstacle(obstacle2); 121 | sim->addObstacle(obstacle3); 122 | sim->addObstacle(obstacle4); 123 | 124 | /* Process the obstacles so that they are accounted for in the simulation. */ 125 | sim->processObstacles(); 126 | 127 | /* Add roadmap vertices. */ 128 | RoadmapVertex v; 129 | 130 | /* Add the goal positions of agents. */ 131 | v.position = RVO::Vector2(-75.0f, -75.0f); 132 | roadmap.push_back(v); 133 | v.position = RVO::Vector2(75.0f, -75.0f); 134 | roadmap.push_back(v); 135 | v.position = RVO::Vector2(-75.0f, 75.0f); 136 | roadmap.push_back(v); 137 | v.position = RVO::Vector2(75.0f, 75.0f); 138 | roadmap.push_back(v); 139 | 140 | /* Add roadmap vertices around the obstacles. */ 141 | v.position = RVO::Vector2(-42.0f, -42.0f); 142 | roadmap.push_back(v); 143 | v.position = RVO::Vector2(-42.0f, -8.0f); 144 | roadmap.push_back(v); 145 | v.position = RVO::Vector2(-42.0f, 8.0f); 146 | roadmap.push_back(v); 147 | v.position = RVO::Vector2(-42.0f, 42.0f); 148 | roadmap.push_back(v); 149 | v.position = RVO::Vector2(-8.0f, -42.0f); 150 | roadmap.push_back(v); 151 | v.position = RVO::Vector2(-8.0f, -8.0f); 152 | roadmap.push_back(v); 153 | v.position = RVO::Vector2(-8.0f, 8.0f); 154 | roadmap.push_back(v); 155 | v.position = RVO::Vector2(-8.0f, 42.0f); 156 | roadmap.push_back(v); 157 | v.position = RVO::Vector2(8.0f, -42.0f); 158 | roadmap.push_back(v); 159 | v.position = RVO::Vector2(8.0f, -8.0f); 160 | roadmap.push_back(v); 161 | v.position = RVO::Vector2(8.0f, 8.0f); 162 | roadmap.push_back(v); 163 | v.position = RVO::Vector2(8.0f, 42.0f); 164 | roadmap.push_back(v); 165 | v.position = RVO::Vector2(42.0f, -42.0f); 166 | roadmap.push_back(v); 167 | v.position = RVO::Vector2(42.0f, -8.0f); 168 | roadmap.push_back(v); 169 | v.position = RVO::Vector2(42.0f, 8.0f); 170 | roadmap.push_back(v); 171 | v.position = RVO::Vector2(42.0f, 42.0f); 172 | roadmap.push_back(v); 173 | 174 | /* Specify the default parameters for agents that are subsequently added. */ 175 | sim->setAgentDefaults(15.0f, 10, 5.0f, 5.0f, 2.0f, 2.0f); 176 | 177 | /* 178 | * Add agents, specifying their start position, and store goals on the 179 | * opposite side of the environment (roadmap vertices). 180 | */ 181 | for (size_t i = 0; i < 5; ++i) { 182 | for (size_t j = 0; j < 5; ++j) { 183 | sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, 55.0f + j * 10.0f)); 184 | goals.push_back(0); 185 | 186 | sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, 55.0f + j * 10.0f)); 187 | goals.push_back(1); 188 | 189 | sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, -55.0f - j * 10.0f)); 190 | goals.push_back(2); 191 | 192 | sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, -55.0f - j * 10.0f)); 193 | goals.push_back(3); 194 | } 195 | } 196 | } 197 | 198 | #if RVO_OUTPUT_TIME_AND_POSITIONS 199 | void updateVisualization(RVO::RVOSimulator *sim) 200 | { 201 | /* Output the current global time. */ 202 | std::cout << sim->getGlobalTime(); 203 | 204 | /* Output the current position of all the agents. */ 205 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 206 | std::cout << " " << sim->getAgentPosition(i); 207 | } 208 | 209 | std::cout << std::endl; 210 | } 211 | #endif 212 | 213 | void buildRoadmap(RVO::RVOSimulator *sim) 214 | { 215 | /* Connect the roadmap vertices by edges if mutually visible. */ 216 | #ifdef _OPENMP 217 | #pragma omp parallel for 218 | #endif 219 | for (int i = 0; i < static_cast(roadmap.size()); ++i) { 220 | for (int j = 0; j < static_cast(roadmap.size()); ++j) { 221 | if (sim->queryVisibility(roadmap[i].position, roadmap[j].position, sim->getAgentRadius(0))) { 222 | roadmap[i].neighbors.push_back(j); 223 | } 224 | } 225 | 226 | /* 227 | * Initialize the distance to each of the four goal vertices at infinity 228 | * (9e9f). 229 | */ 230 | roadmap[i].distToGoal.resize(4, 9e9f); 231 | } 232 | 233 | /* 234 | * Compute the distance to each of the four goals (the first four vertices) 235 | * for all vertices using Dijkstra's algorithm. 236 | */ 237 | #ifdef _OPENMP 238 | #pragma omp parallel for 239 | #endif 240 | for (int i = 0; i < 4; ++i) { 241 | std::multimap Q; 242 | std::vector::iterator> posInQ(roadmap.size(), Q.end()); 243 | 244 | roadmap[i].distToGoal[i] = 0.0f; 245 | posInQ[i] = Q.insert(std::make_pair(0.0f, i)); 246 | 247 | while (!Q.empty()) { 248 | const int u = Q.begin()->second; 249 | Q.erase(Q.begin()); 250 | posInQ[u] = Q.end(); 251 | 252 | for (int j = 0; j < static_cast(roadmap[u].neighbors.size()); ++j) { 253 | const int v = roadmap[u].neighbors[j]; 254 | const float dist_uv = RVO::abs(roadmap[v].position - roadmap[u].position); 255 | 256 | if (roadmap[v].distToGoal[i] > roadmap[u].distToGoal[i] + dist_uv) { 257 | roadmap[v].distToGoal[i] = roadmap[u].distToGoal[i] + dist_uv; 258 | 259 | if (posInQ[v] == Q.end()) { 260 | posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v)); 261 | } 262 | else { 263 | Q.erase(posInQ[v]); 264 | posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v)); 265 | } 266 | } 267 | } 268 | } 269 | } 270 | } 271 | 272 | void setPreferredVelocities(RVO::RVOSimulator *sim) 273 | { 274 | /* 275 | * Set the preferred velocity to be a vector of unit magnitude (speed) in the 276 | * direction of the visible roadmap vertex that is on the shortest path to the 277 | * goal. 278 | */ 279 | #ifdef _OPENMP 280 | #pragma omp parallel for 281 | #endif 282 | for (int i = 0; i < static_cast(sim->getNumAgents()); ++i) { 283 | float minDist = 9e9f; 284 | int minVertex = -1; 285 | 286 | for (int j = 0; j < static_cast(roadmap.size()); ++j) { 287 | if (RVO::abs(roadmap[j].position - sim->getAgentPosition(i)) + roadmap[j].distToGoal[goals[i]] < minDist && 288 | sim->queryVisibility(sim->getAgentPosition(i), roadmap[j].position, sim->getAgentRadius(i))) { 289 | 290 | minDist = RVO::abs(roadmap[j].position - sim->getAgentPosition(i)) + roadmap[j].distToGoal[goals[i]]; 291 | minVertex = j; 292 | } 293 | } 294 | 295 | if (minVertex == -1) { 296 | /* No roadmap vertex is visible; should not happen. */ 297 | sim->setAgentPrefVelocity(i, RVO::Vector2(0, 0)); 298 | } 299 | else { 300 | if (RVO::absSq(roadmap[minVertex].position - 301 | sim->getAgentPosition(i)) == 0.0f) { 302 | if (minVertex == goals[i]) { 303 | sim->setAgentPrefVelocity(i, RVO::Vector2()); 304 | } 305 | else { 306 | sim->setAgentPrefVelocity(i, RVO::normalize(roadmap[goals[i]].position - sim->getAgentPosition(i))); 307 | } 308 | } 309 | else { 310 | sim->setAgentPrefVelocity(i, RVO::normalize(roadmap[minVertex].position - sim->getAgentPosition(i))); 311 | } 312 | } 313 | 314 | /* 315 | * Perturb a little to avoid deadlocks due to perfect symmetry. 316 | */ 317 | float angle = std::rand() * 2.0f * M_PI / RAND_MAX; 318 | float dist = std::rand() * 0.0001f / RAND_MAX; 319 | 320 | sim->setAgentPrefVelocity(i, sim->getAgentPrefVelocity(i) + 321 | dist * RVO::Vector2(std::cos(angle), std::sin(angle))); 322 | } 323 | } 324 | 325 | bool reachedGoal(RVO::RVOSimulator *sim) 326 | { 327 | /* Check if all agents have reached their goals. */ 328 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 329 | if (RVO::absSq(sim->getAgentPosition(i) - roadmap[goals[i]].position) > 20.0f * 20.0f) { 330 | return false; 331 | } 332 | } 333 | 334 | return true; 335 | } 336 | 337 | int main() 338 | { 339 | /* Create a new simulator instance. */ 340 | RVO::RVOSimulator *sim = new RVO::RVOSimulator(); 341 | 342 | /* Set up the scenario. */ 343 | setupScenario(sim); 344 | 345 | /* Build the roadmap. */ 346 | buildRoadmap(sim); 347 | 348 | /* Perform (and manipulate) the simulation. */ 349 | do { 350 | #if RVO_OUTPUT_TIME_AND_POSITIONS 351 | updateVisualization(sim); 352 | #endif 353 | setPreferredVelocities(sim); 354 | sim->doStep(); 355 | } 356 | while (!reachedGoal(sim)); 357 | 358 | delete sim; 359 | 360 | return 0; 361 | } 362 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /src/Vector2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Vector2.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 . 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 | * 31 | */ 32 | 33 | #ifndef RVO_VECTOR2_H_ 34 | #define RVO_VECTOR2_H_ 35 | 36 | /** 37 | * \file Vector2.h 38 | * \brief Contains the Vector2 class. 39 | */ 40 | 41 | #include 42 | #include 43 | 44 | namespace RVO { 45 | /** 46 | * \brief Defines a two-dimensional vector. 47 | */ 48 | class Vector2 { 49 | public: 50 | /** 51 | * \brief Constructs and initializes a two-dimensional vector instance 52 | * to (0.0, 0.0). 53 | */ 54 | inline Vector2() : x_(0.0f), y_(0.0f) { } 55 | 56 | /** 57 | * \brief Constructs and initializes a two-dimensional vector from 58 | * the specified xy-coordinates. 59 | * \param x The x-coordinate of the two-dimensional 60 | * vector. 61 | * \param y The y-coordinate of the two-dimensional 62 | * vector. 63 | */ 64 | inline Vector2(float x, float y) : x_(x), y_(y) { } 65 | 66 | /** 67 | * \brief Returns the x-coordinate of this two-dimensional vector. 68 | * \return The x-coordinate of the two-dimensional vector. 69 | */ 70 | inline float x() const { return x_; } 71 | 72 | /** 73 | * \brief Returns the y-coordinate of this two-dimensional vector. 74 | * \return The y-coordinate of the two-dimensional vector. 75 | */ 76 | inline float y() const { return y_; } 77 | 78 | /** 79 | * \brief Computes the negation of this two-dimensional vector. 80 | * \return The negation of this two-dimensional vector. 81 | */ 82 | inline Vector2 operator-() const 83 | { 84 | return Vector2(-x_, -y_); 85 | } 86 | 87 | /** 88 | * \brief Computes the dot product of this two-dimensional vector with 89 | * the specified two-dimensional vector. 90 | * \param vector The two-dimensional vector with which the 91 | * dot product should be computed. 92 | * \return The dot product of this two-dimensional vector with a 93 | * specified two-dimensional vector. 94 | */ 95 | inline float operator*(const Vector2 &vector) const 96 | { 97 | return x_ * vector.x() + y_ * vector.y(); 98 | } 99 | 100 | /** 101 | * \brief Computes the scalar multiplication of this 102 | * two-dimensional vector with the specified scalar value. 103 | * \param s The scalar value with which the scalar 104 | * multiplication should be computed. 105 | * \return The scalar multiplication of this two-dimensional vector 106 | * with a specified scalar value. 107 | */ 108 | inline Vector2 operator*(float s) const 109 | { 110 | return Vector2(x_ * s, y_ * s); 111 | } 112 | 113 | /** 114 | * \brief Computes the scalar division of this two-dimensional vector 115 | * with the specified scalar value. 116 | * \param s The scalar value with which the scalar 117 | * division should be computed. 118 | * \return The scalar division of this two-dimensional vector with a 119 | * specified scalar value. 120 | */ 121 | inline Vector2 operator/(float s) const 122 | { 123 | const float invS = 1.0f / s; 124 | 125 | return Vector2(x_ * invS, y_ * invS); 126 | } 127 | 128 | /** 129 | * \brief Computes the vector sum of this two-dimensional vector with 130 | * the specified two-dimensional vector. 131 | * \param vector The two-dimensional vector with which the 132 | * vector sum should be computed. 133 | * \return The vector sum of this two-dimensional vector with a 134 | * specified two-dimensional vector. 135 | */ 136 | inline Vector2 operator+(const Vector2 &vector) const 137 | { 138 | return Vector2(x_ + vector.x(), y_ + vector.y()); 139 | } 140 | 141 | /** 142 | * \brief Computes the vector difference of this two-dimensional 143 | * vector with the specified two-dimensional vector. 144 | * \param vector The two-dimensional vector with which the 145 | * vector difference should be computed. 146 | * \return The vector difference of this two-dimensional vector with a 147 | * specified two-dimensional vector. 148 | */ 149 | inline Vector2 operator-(const Vector2 &vector) const 150 | { 151 | return Vector2(x_ - vector.x(), y_ - vector.y()); 152 | } 153 | 154 | /** 155 | * \brief Tests this two-dimensional vector for equality with the 156 | * specified two-dimensional vector. 157 | * \param vector The two-dimensional vector with which to 158 | * test for equality. 159 | * \return True if the two-dimensional vectors are equal. 160 | */ 161 | inline bool operator==(const Vector2 &vector) const 162 | { 163 | return x_ == vector.x() && y_ == vector.y(); 164 | } 165 | 166 | /** 167 | * \brief Tests this two-dimensional vector for inequality with the 168 | * specified two-dimensional vector. 169 | * \param vector The two-dimensional vector with which to 170 | * test for inequality. 171 | * \return True if the two-dimensional vectors are not equal. 172 | */ 173 | inline bool operator!=(const Vector2 &vector) const 174 | { 175 | return x_ != vector.x() || y_ != vector.y(); 176 | } 177 | 178 | /** 179 | * \brief Sets the value of this two-dimensional vector to the scalar 180 | * multiplication of itself with the specified scalar value. 181 | * \param s The scalar value with which the scalar 182 | * multiplication should be computed. 183 | * \return A reference to this two-dimensional vector. 184 | */ 185 | inline Vector2 &operator*=(float s) 186 | { 187 | x_ *= s; 188 | y_ *= s; 189 | 190 | return *this; 191 | } 192 | 193 | /** 194 | * \brief Sets the value of this two-dimensional vector to the scalar 195 | * division of itself with the specified scalar value. 196 | * \param s The scalar value with which the scalar 197 | * division should be computed. 198 | * \return A reference to this two-dimensional vector. 199 | */ 200 | inline Vector2 &operator/=(float s) 201 | { 202 | const float invS = 1.0f / s; 203 | x_ *= invS; 204 | y_ *= invS; 205 | 206 | return *this; 207 | } 208 | 209 | /** 210 | * \brief Sets the value of this two-dimensional vector to the vector 211 | * sum of itself with the specified two-dimensional vector. 212 | * \param vector The two-dimensional vector with which the 213 | * vector sum should be computed. 214 | * \return A reference to this two-dimensional vector. 215 | */ 216 | inline Vector2 &operator+=(const Vector2 &vector) 217 | { 218 | x_ += vector.x(); 219 | y_ += vector.y(); 220 | 221 | return *this; 222 | } 223 | 224 | /** 225 | * \brief Sets the value of this two-dimensional vector to the vector 226 | * difference of itself with the specified two-dimensional 227 | * vector. 228 | * \param vector The two-dimensional vector with which the 229 | * vector difference should be computed. 230 | * \return A reference to this two-dimensional vector. 231 | */ 232 | inline Vector2 &operator-=(const Vector2 &vector) 233 | { 234 | x_ -= vector.x(); 235 | y_ -= vector.y(); 236 | 237 | return *this; 238 | } 239 | 240 | private: 241 | float x_; 242 | float y_; 243 | }; 244 | 245 | /** 246 | * \relates Vector2 247 | * \brief Computes the scalar multiplication of the specified 248 | * two-dimensional vector with the specified scalar value. 249 | * \param s The scalar value with which the scalar 250 | * multiplication should be computed. 251 | * \param vector The two-dimensional vector with which the scalar 252 | * multiplication should be computed. 253 | * \return The scalar multiplication of the two-dimensional vector with the 254 | * scalar value. 255 | */ 256 | inline Vector2 operator*(float s, const Vector2 &vector) 257 | { 258 | return Vector2(s * vector.x(), s * vector.y()); 259 | } 260 | 261 | /** 262 | * \relates Vector2 263 | * \brief Inserts the specified two-dimensional vector into the specified 264 | * output stream. 265 | * \param os The output stream into which the two-dimensional 266 | * vector should be inserted. 267 | * \param vector The two-dimensional vector which to insert into 268 | * the output stream. 269 | * \return A reference to the output stream. 270 | */ 271 | inline std::ostream &operator<<(std::ostream &os, const Vector2 &vector) 272 | { 273 | os << "(" << vector.x() << "," << vector.y() << ")"; 274 | 275 | return os; 276 | } 277 | 278 | /** 279 | * \relates Vector2 280 | * \brief Computes the length of a specified two-dimensional vector. 281 | * \param vector The two-dimensional vector whose length is to be 282 | * computed. 283 | * \return The length of the two-dimensional vector. 284 | */ 285 | inline float abs(const Vector2 &vector) 286 | { 287 | return std::sqrt(vector * vector); 288 | } 289 | 290 | /** 291 | * \relates Vector2 292 | * \brief Computes the squared length of a specified two-dimensional 293 | * vector. 294 | * \param vector The two-dimensional vector whose squared length 295 | * is to be computed. 296 | * \return The squared length of the two-dimensional vector. 297 | */ 298 | inline float absSq(const Vector2 &vector) 299 | { 300 | return vector * vector; 301 | } 302 | 303 | /** 304 | * \relates Vector2 305 | * \brief Computes the determinant of a two-dimensional square matrix with 306 | * rows consisting of the specified two-dimensional vectors. 307 | * \param vector1 The top row of the two-dimensional square 308 | * matrix. 309 | * \param vector2 The bottom row of the two-dimensional square 310 | * matrix. 311 | * \return The determinant of the two-dimensional square matrix. 312 | */ 313 | inline float det(const Vector2 &vector1, const Vector2 &vector2) 314 | { 315 | return vector1.x() * vector2.y() - vector1.y() * vector2.x(); 316 | } 317 | 318 | /** 319 | * \relates Vector2 320 | * \brief Computes the normalization of the specified two-dimensional 321 | * vector. 322 | * \param vector The two-dimensional vector whose normalization 323 | * is to be computed. 324 | * \return The normalization of the two-dimensional vector. 325 | */ 326 | inline Vector2 normalize(const Vector2 &vector) 327 | { 328 | return vector / abs(vector); 329 | } 330 | } 331 | 332 | #endif /* RVO_VECTOR2_H_ */ 333 | -------------------------------------------------------------------------------- /doc/Doxyfile: -------------------------------------------------------------------------------- 1 | # Doxyfile 1.8.11 2 | 3 | #--------------------------------------------------------------------------- 4 | # Project related configuration options 5 | #--------------------------------------------------------------------------- 6 | DOXYFILE_ENCODING = UTF-8 7 | PROJECT_NAME = "RVO2 Library" 8 | PROJECT_NUMBER = 2.0.2 9 | PROJECT_BRIEF = "Optimal Reciprocal Collision Avoidance" 10 | PROJECT_LOGO = 11 | OUTPUT_DIRECTORY = 12 | CREATE_SUBDIRS = NO 13 | ALLOW_UNICODE_NAMES = NO 14 | OUTPUT_LANGUAGE = English 15 | BRIEF_MEMBER_DESC = YES 16 | REPEAT_BRIEF = YES 17 | ABBREVIATE_BRIEF = 18 | ALWAYS_DETAILED_SEC = NO 19 | INLINE_INHERITED_MEMB = NO 20 | FULL_PATH_NAMES = YES 21 | STRIP_FROM_PATH = ../src 22 | STRIP_FROM_INC_PATH = 23 | SHORT_NAMES = NO 24 | JAVADOC_AUTOBRIEF = NO 25 | QT_AUTOBRIEF = NO 26 | MULTILINE_CPP_IS_BRIEF = NO 27 | INHERIT_DOCS = YES 28 | SEPARATE_MEMBER_PAGES = NO 29 | TAB_SIZE = 4 30 | ALIASES = 31 | TCL_SUBST = 32 | OPTIMIZE_OUTPUT_FOR_C = NO 33 | OPTIMIZE_OUTPUT_JAVA = NO 34 | OPTIMIZE_FOR_FORTRAN = NO 35 | OPTIMIZE_OUTPUT_VHDL = NO 36 | EXTENSION_MAPPING = 37 | MARKDOWN_SUPPORT = YES 38 | AUTOLINK_SUPPORT = YES 39 | BUILTIN_STL_SUPPORT = YES 40 | CPP_CLI_SUPPORT = NO 41 | SIP_SUPPORT = NO 42 | IDL_PROPERTY_SUPPORT = YES 43 | DISTRIBUTE_GROUP_DOC = NO 44 | GROUP_NESTED_COMPOUNDS = NO 45 | SUBGROUPING = YES 46 | INLINE_GROUPED_CLASSES = NO 47 | INLINE_SIMPLE_STRUCTS = NO 48 | TYPEDEF_HIDES_STRUCT = NO 49 | LOOKUP_CACHE_SIZE = 0 50 | #--------------------------------------------------------------------------- 51 | # Build related configuration options 52 | #--------------------------------------------------------------------------- 53 | EXTRACT_ALL = YES 54 | EXTRACT_PRIVATE = NO 55 | EXTRACT_PACKAGE = NO 56 | EXTRACT_STATIC = NO 57 | EXTRACT_LOCAL_CLASSES = YES 58 | EXTRACT_LOCAL_METHODS = NO 59 | EXTRACT_ANON_NSPACES = NO 60 | HIDE_UNDOC_MEMBERS = NO 61 | HIDE_UNDOC_CLASSES = NO 62 | HIDE_FRIEND_COMPOUNDS = NO 63 | HIDE_IN_BODY_DOCS = NO 64 | INTERNAL_DOCS = NO 65 | CASE_SENSE_NAMES = NO 66 | HIDE_SCOPE_NAMES = NO 67 | HIDE_COMPOUND_REFERENCE= NO 68 | SHOW_INCLUDE_FILES = YES 69 | SHOW_GROUPED_MEMB_INC = NO 70 | FORCE_LOCAL_INCLUDES = NO 71 | INLINE_INFO = YES 72 | SORT_MEMBER_DOCS = YES 73 | SORT_BRIEF_DOCS = YES 74 | SORT_MEMBERS_CTORS_1ST = YES 75 | SORT_GROUP_NAMES = NO 76 | SORT_BY_SCOPE_NAME = NO 77 | STRICT_PROTO_MATCHING = NO 78 | GENERATE_TODOLIST = YES 79 | GENERATE_TESTLIST = YES 80 | GENERATE_BUGLIST = YES 81 | GENERATE_DEPRECATEDLIST= YES 82 | ENABLED_SECTIONS = 83 | MAX_INITIALIZER_LINES = 30 84 | SHOW_USED_FILES = YES 85 | SHOW_FILES = YES 86 | SHOW_NAMESPACES = YES 87 | FILE_VERSION_FILTER = 88 | LAYOUT_FILE = 89 | CITE_BIB_FILES = 90 | #--------------------------------------------------------------------------- 91 | # Configuration options related to warning and progress messages 92 | #--------------------------------------------------------------------------- 93 | QUIET = NO 94 | WARNINGS = YES 95 | WARN_IF_UNDOCUMENTED = YES 96 | WARN_IF_DOC_ERROR = YES 97 | WARN_NO_PARAMDOC = NO 98 | WARN_AS_ERROR = NO 99 | WARN_FORMAT = "$file:$line: $text" 100 | WARN_LOGFILE = 101 | #--------------------------------------------------------------------------- 102 | # Configuration options related to the input files 103 | #--------------------------------------------------------------------------- 104 | INPUT = ../src 105 | INPUT_ENCODING = UTF-8 106 | FILE_PATTERNS = RVO.h \ 107 | RVOSimulator.h \ 108 | Vector2.h 109 | RECURSIVE = NO 110 | EXCLUDE = 111 | EXCLUDE_SYMLINKS = NO 112 | EXCLUDE_PATTERNS = 113 | EXCLUDE_SYMBOLS = 114 | EXAMPLE_PATH = ../examples 115 | EXAMPLE_PATTERNS = Blocks.cpp \ 116 | Circle.cpp \ 117 | Roadmap.cpp 118 | EXAMPLE_RECURSIVE = NO 119 | IMAGE_PATH = 120 | INPUT_FILTER = 121 | FILTER_PATTERNS = 122 | FILTER_SOURCE_FILES = NO 123 | FILTER_SOURCE_PATTERNS = 124 | USE_MDFILE_AS_MAINPAGE = 125 | #--------------------------------------------------------------------------- 126 | # Configuration options related to source browsing 127 | #--------------------------------------------------------------------------- 128 | SOURCE_BROWSER = NO 129 | INLINE_SOURCES = NO 130 | STRIP_CODE_COMMENTS = YES 131 | REFERENCED_BY_RELATION = NO 132 | REFERENCES_RELATION = NO 133 | REFERENCES_LINK_SOURCE = YES 134 | SOURCE_TOOLTIPS = YES 135 | USE_HTAGS = NO 136 | VERBATIM_HEADERS = YES 137 | CLANG_ASSISTED_PARSING = NO 138 | CLANG_OPTIONS = 139 | #--------------------------------------------------------------------------- 140 | # Configuration options related to the alphabetical class index 141 | #--------------------------------------------------------------------------- 142 | ALPHABETICAL_INDEX = YES 143 | COLS_IN_ALPHA_INDEX = 5 144 | IGNORE_PREFIX = 145 | #--------------------------------------------------------------------------- 146 | # Configuration options related to the HTML output 147 | #--------------------------------------------------------------------------- 148 | GENERATE_HTML = YES 149 | HTML_OUTPUT = html 150 | HTML_FILE_EXTENSION = .html 151 | HTML_HEADER = header.html 152 | HTML_FOOTER = footer.html 153 | HTML_STYLESHEET = 154 | HTML_EXTRA_STYLESHEET = stylesheet.css 155 | HTML_EXTRA_FILES = 156 | HTML_COLORSTYLE_HUE = 220 157 | HTML_COLORSTYLE_SAT = 100 158 | HTML_COLORSTYLE_GAMMA = 80 159 | HTML_TIMESTAMP = NO 160 | HTML_DYNAMIC_SECTIONS = NO 161 | HTML_INDEX_NUM_ENTRIES = 100 162 | GENERATE_DOCSET = NO 163 | DOCSET_FEEDNAME = "RVO2 Library" 164 | DOCSET_BUNDLE_ID = edu.unc.cs.gamma.rvo2 165 | DOCSET_PUBLISHER_ID = edu.unc.cs.gamma.documentation 166 | DOCSET_PUBLISHER_NAME = Publisher 167 | GENERATE_HTMLHELP = NO 168 | CHM_FILE = 169 | HHC_LOCATION = 170 | GENERATE_CHI = NO 171 | CHM_INDEX_ENCODING = 172 | BINARY_TOC = NO 173 | TOC_EXPAND = NO 174 | GENERATE_QHP = NO 175 | QCH_FILE = 176 | QHP_NAMESPACE = edu.unc.cs.gamma.rvo2 177 | QHP_VIRTUAL_FOLDER = doc 178 | QHP_CUST_FILTER_NAME = 179 | QHP_CUST_FILTER_ATTRS = 180 | QHP_SECT_FILTER_ATTRS = 181 | QHG_LOCATION = 182 | GENERATE_ECLIPSEHELP = NO 183 | ECLIPSE_DOC_ID = edu.unc.cs.gamma.rvo2 184 | DISABLE_INDEX = NO 185 | GENERATE_TREEVIEW = NO 186 | ENUM_VALUES_PER_LINE = 4 187 | TREEVIEW_WIDTH = 250 188 | EXT_LINKS_IN_WINDOW = NO 189 | FORMULA_FONTSIZE = 10 190 | FORMULA_TRANSPARENT = YES 191 | USE_MATHJAX = NO 192 | MATHJAX_FORMAT = HTML-CSS 193 | MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest 194 | MATHJAX_EXTENSIONS = 195 | MATHJAX_CODEFILE = 196 | SEARCHENGINE = NO 197 | SERVER_BASED_SEARCH = NO 198 | EXTERNAL_SEARCH = NO 199 | SEARCHENGINE_URL = 200 | SEARCHDATA_FILE = searchdata.xml 201 | EXTERNAL_SEARCH_ID = 202 | EXTRA_SEARCH_MAPPINGS = 203 | #--------------------------------------------------------------------------- 204 | # Configuration options related to the LaTeX output 205 | #--------------------------------------------------------------------------- 206 | GENERATE_LATEX = NO 207 | LATEX_OUTPUT = latex 208 | LATEX_CMD_NAME = latex 209 | MAKEINDEX_CMD_NAME = makeindex 210 | COMPACT_LATEX = NO 211 | PAPER_TYPE = letter 212 | EXTRA_PACKAGES = 213 | LATEX_HEADER = 214 | LATEX_FOOTER = 215 | LATEX_EXTRA_STYLESHEET = 216 | LATEX_EXTRA_FILES = 217 | PDF_HYPERLINKS = YES 218 | USE_PDFLATEX = YES 219 | LATEX_BATCHMODE = NO 220 | LATEX_HIDE_INDICES = NO 221 | LATEX_SOURCE_CODE = NO 222 | LATEX_BIB_STYLE = plain 223 | LATEX_TIMESTAMP = NO 224 | #--------------------------------------------------------------------------- 225 | # Configuration options related to the RTF output 226 | #--------------------------------------------------------------------------- 227 | GENERATE_RTF = NO 228 | RTF_OUTPUT = rtf 229 | COMPACT_RTF = NO 230 | RTF_HYPERLINKS = NO 231 | RTF_STYLESHEET_FILE = 232 | RTF_EXTENSIONS_FILE = 233 | RTF_SOURCE_CODE = NO 234 | #--------------------------------------------------------------------------- 235 | # Configuration options related to the man page output 236 | #--------------------------------------------------------------------------- 237 | GENERATE_MAN = NO 238 | MAN_OUTPUT = man 239 | MAN_EXTENSION = .3 240 | MAN_SUBDIR = 241 | MAN_LINKS = NO 242 | #--------------------------------------------------------------------------- 243 | # Configuration options related to the XML output 244 | #--------------------------------------------------------------------------- 245 | GENERATE_XML = NO 246 | XML_OUTPUT = xml 247 | XML_PROGRAMLISTING = YES 248 | #--------------------------------------------------------------------------- 249 | # Configuration options related to the DOCBOOK output 250 | #--------------------------------------------------------------------------- 251 | GENERATE_DOCBOOK = NO 252 | DOCBOOK_OUTPUT = docbook 253 | DOCBOOK_PROGRAMLISTING = NO 254 | #--------------------------------------------------------------------------- 255 | # Configuration options for the AutoGen Definitions output 256 | #--------------------------------------------------------------------------- 257 | GENERATE_AUTOGEN_DEF = NO 258 | #--------------------------------------------------------------------------- 259 | # Configuration options related to the Perl module output 260 | #--------------------------------------------------------------------------- 261 | GENERATE_PERLMOD = NO 262 | PERLMOD_LATEX = NO 263 | PERLMOD_PRETTY = YES 264 | PERLMOD_MAKEVAR_PREFIX = 265 | #--------------------------------------------------------------------------- 266 | # Configuration options related to the preprocessor 267 | #--------------------------------------------------------------------------- 268 | ENABLE_PREPROCESSING = YES 269 | MACRO_EXPANSION = NO 270 | EXPAND_ONLY_PREDEF = NO 271 | SEARCH_INCLUDES = YES 272 | INCLUDE_PATH = 273 | INCLUDE_FILE_PATTERNS = 274 | PREDEFINED = 275 | EXPAND_AS_DEFINED = 276 | SKIP_FUNCTION_MACROS = YES 277 | #--------------------------------------------------------------------------- 278 | # Configuration options related to external references 279 | #--------------------------------------------------------------------------- 280 | TAGFILES = 281 | GENERATE_TAGFILE = 282 | ALLEXTERNALS = NO 283 | EXTERNAL_GROUPS = YES 284 | EXTERNAL_PAGES = YES 285 | PERL_PATH = 286 | #--------------------------------------------------------------------------- 287 | # Configuration options related to the dot tool 288 | #--------------------------------------------------------------------------- 289 | CLASS_DIAGRAMS = YES 290 | MSCGEN_PATH = 291 | DIA_PATH = 292 | HIDE_UNDOC_RELATIONS = YES 293 | HAVE_DOT = YES 294 | DOT_NUM_THREADS = 0 295 | DOT_FONTNAME = Helvetica 296 | DOT_FONTSIZE = 10 297 | DOT_FONTPATH = 298 | CLASS_GRAPH = YES 299 | COLLABORATION_GRAPH = YES 300 | GROUP_GRAPHS = YES 301 | UML_LOOK = NO 302 | UML_LIMIT_NUM_FIELDS = 10 303 | TEMPLATE_RELATIONS = NO 304 | INCLUDE_GRAPH = YES 305 | INCLUDED_BY_GRAPH = YES 306 | CALL_GRAPH = NO 307 | CALLER_GRAPH = NO 308 | GRAPHICAL_HIERARCHY = YES 309 | DIRECTORY_GRAPH = YES 310 | DOT_IMAGE_FORMAT = png 311 | INTERACTIVE_SVG = NO 312 | DOT_PATH = 313 | DOTFILE_DIRS = 314 | MSCFILE_DIRS = 315 | DIAFILE_DIRS = 316 | PLANTUML_JAR_PATH = 317 | PLANTUML_INCLUDE_PATH = 318 | DOT_GRAPH_MAX_NODES = 50 319 | MAX_DOT_GRAPH_DEPTH = 0 320 | DOT_TRANSPARENT = NO 321 | DOT_MULTI_TARGETS = NO 322 | GENERATE_LEGEND = YES 323 | DOT_CLEANUP = YES 324 | -------------------------------------------------------------------------------- /src/KdTree.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * KdTree.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 . 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 | * 31 | */ 32 | 33 | #include "KdTree.h" 34 | 35 | #include "Agent.h" 36 | #include "RVOSimulator.h" 37 | #include "Obstacle.h" 38 | 39 | namespace RVO { 40 | KdTree::KdTree(RVOSimulator *sim) : obstacleTree_(NULL), sim_(sim) { } 41 | 42 | KdTree::~KdTree() 43 | { 44 | deleteObstacleTree(obstacleTree_); 45 | } 46 | 47 | void KdTree::buildAgentTree() 48 | { 49 | if (agents_.size() < sim_->agents_.size()) { 50 | for (size_t i = agents_.size(); i < sim_->agents_.size(); ++i) { 51 | agents_.push_back(sim_->agents_[i]); 52 | } 53 | 54 | agentTree_.resize(2 * agents_.size() - 1); 55 | } 56 | 57 | if (!agents_.empty()) { 58 | buildAgentTreeRecursive(0, agents_.size(), 0); 59 | } 60 | } 61 | 62 | void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) 63 | { 64 | agentTree_[node].begin = begin; 65 | agentTree_[node].end = end; 66 | agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x(); 67 | agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y(); 68 | 69 | for (size_t i = begin + 1; i < end; ++i) { 70 | agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x()); 71 | agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x()); 72 | agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y()); 73 | agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y()); 74 | } 75 | 76 | if (end - begin > MAX_LEAF_SIZE) { 77 | /* No leaf node. */ 78 | const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY); 79 | const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY)); 80 | 81 | size_t left = begin; 82 | size_t right = end; 83 | 84 | while (left < right) { 85 | while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) { 86 | ++left; 87 | } 88 | 89 | while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) { 90 | --right; 91 | } 92 | 93 | if (left < right) { 94 | std::swap(agents_[left], agents_[right - 1]); 95 | ++left; 96 | --right; 97 | } 98 | } 99 | 100 | if (left == begin) { 101 | ++left; 102 | ++right; 103 | } 104 | 105 | agentTree_[node].left = node + 1; 106 | agentTree_[node].right = node + 2 * (left - begin); 107 | 108 | buildAgentTreeRecursive(begin, left, agentTree_[node].left); 109 | buildAgentTreeRecursive(left, end, agentTree_[node].right); 110 | } 111 | } 112 | 113 | void KdTree::buildObstacleTree() 114 | { 115 | deleteObstacleTree(obstacleTree_); 116 | 117 | std::vector obstacles(sim_->obstacles_.size()); 118 | 119 | for (size_t i = 0; i < sim_->obstacles_.size(); ++i) { 120 | obstacles[i] = sim_->obstacles_[i]; 121 | } 122 | 123 | obstacleTree_ = buildObstacleTreeRecursive(obstacles); 124 | } 125 | 126 | 127 | KdTree::ObstacleTreeNode *KdTree::buildObstacleTreeRecursive(const std::vector &obstacles) 128 | { 129 | if (obstacles.empty()) { 130 | return NULL; 131 | } 132 | else { 133 | ObstacleTreeNode *const node = new ObstacleTreeNode; 134 | 135 | size_t optimalSplit = 0; 136 | size_t minLeft = obstacles.size(); 137 | size_t minRight = obstacles.size(); 138 | 139 | for (size_t i = 0; i < obstacles.size(); ++i) { 140 | size_t leftSize = 0; 141 | size_t rightSize = 0; 142 | 143 | const Obstacle *const obstacleI1 = obstacles[i]; 144 | const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; 145 | 146 | /* Compute optimal split node. */ 147 | for (size_t j = 0; j < obstacles.size(); ++j) { 148 | if (i == j) { 149 | continue; 150 | } 151 | 152 | const Obstacle *const obstacleJ1 = obstacles[j]; 153 | const Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; 154 | 155 | const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); 156 | const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); 157 | 158 | if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { 159 | ++leftSize; 160 | } 161 | else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { 162 | ++rightSize; 163 | } 164 | else { 165 | ++leftSize; 166 | ++rightSize; 167 | } 168 | 169 | if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { 170 | break; 171 | } 172 | } 173 | 174 | if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { 175 | minLeft = leftSize; 176 | minRight = rightSize; 177 | optimalSplit = i; 178 | } 179 | } 180 | 181 | /* Build split node. */ 182 | std::vector leftObstacles(minLeft); 183 | std::vector rightObstacles(minRight); 184 | 185 | size_t leftCounter = 0; 186 | size_t rightCounter = 0; 187 | const size_t i = optimalSplit; 188 | 189 | const Obstacle *const obstacleI1 = obstacles[i]; 190 | const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; 191 | 192 | for (size_t j = 0; j < obstacles.size(); ++j) { 193 | if (i == j) { 194 | continue; 195 | } 196 | 197 | Obstacle *const obstacleJ1 = obstacles[j]; 198 | Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; 199 | 200 | const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); 201 | const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); 202 | 203 | if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { 204 | leftObstacles[leftCounter++] = obstacles[j]; 205 | } 206 | else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { 207 | rightObstacles[rightCounter++] = obstacles[j]; 208 | } 209 | else { 210 | /* Split obstacle j. */ 211 | const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_); 212 | 213 | const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); 214 | 215 | Obstacle *const newObstacle = new Obstacle(); 216 | newObstacle->point_ = splitpoint; 217 | newObstacle->prevObstacle_ = obstacleJ1; 218 | newObstacle->nextObstacle_ = obstacleJ2; 219 | newObstacle->isConvex_ = true; 220 | newObstacle->unitDir_ = obstacleJ1->unitDir_; 221 | 222 | newObstacle->id_ = sim_->obstacles_.size(); 223 | 224 | sim_->obstacles_.push_back(newObstacle); 225 | 226 | obstacleJ1->nextObstacle_ = newObstacle; 227 | obstacleJ2->prevObstacle_ = newObstacle; 228 | 229 | if (j1LeftOfI > 0.0f) { 230 | leftObstacles[leftCounter++] = obstacleJ1; 231 | rightObstacles[rightCounter++] = newObstacle; 232 | } 233 | else { 234 | rightObstacles[rightCounter++] = obstacleJ1; 235 | leftObstacles[leftCounter++] = newObstacle; 236 | } 237 | } 238 | } 239 | 240 | node->obstacle = obstacleI1; 241 | node->left = buildObstacleTreeRecursive(leftObstacles); 242 | node->right = buildObstacleTreeRecursive(rightObstacles); 243 | return node; 244 | } 245 | } 246 | 247 | void KdTree::computeAgentNeighbors(Agent *agent, float &rangeSq) const 248 | { 249 | queryAgentTreeRecursive(agent, rangeSq, 0); 250 | } 251 | 252 | void KdTree::computeObstacleNeighbors(Agent *agent, float rangeSq) const 253 | { 254 | queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); 255 | } 256 | 257 | void KdTree::deleteObstacleTree(ObstacleTreeNode *node) 258 | { 259 | if (node != NULL) { 260 | deleteObstacleTree(node->left); 261 | deleteObstacleTree(node->right); 262 | delete node; 263 | } 264 | } 265 | 266 | void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const 267 | { 268 | if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) { 269 | for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) { 270 | agent->insertAgentNeighbor(agents_[i], rangeSq); 271 | } 272 | } 273 | else { 274 | const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY)); 275 | 276 | const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY)); 277 | 278 | if (distSqLeft < distSqRight) { 279 | if (distSqLeft < rangeSq) { 280 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); 281 | 282 | if (distSqRight < rangeSq) { 283 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); 284 | } 285 | } 286 | } 287 | else { 288 | if (distSqRight < rangeSq) { 289 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); 290 | 291 | if (distSqLeft < rangeSq) { 292 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); 293 | } 294 | } 295 | } 296 | 297 | } 298 | } 299 | 300 | void KdTree::queryObstacleTreeRecursive(Agent *agent, float rangeSq, const ObstacleTreeNode *node) const 301 | { 302 | if (node == NULL) { 303 | return; 304 | } 305 | else { 306 | const Obstacle *const obstacle1 = node->obstacle; 307 | const Obstacle *const obstacle2 = obstacle1->nextObstacle_; 308 | 309 | const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_); 310 | 311 | queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right)); 312 | 313 | const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_); 314 | 315 | if (distSqLine < rangeSq) { 316 | if (agentLeftOfLine < 0.0f) { 317 | /* 318 | * Try obstacle at this node only if agent is on right side of 319 | * obstacle (and can see obstacle). 320 | */ 321 | agent->insertObstacleNeighbor(node->obstacle, rangeSq); 322 | } 323 | 324 | /* Try other side of line. */ 325 | queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left)); 326 | 327 | } 328 | } 329 | } 330 | 331 | bool KdTree::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const 332 | { 333 | return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); 334 | } 335 | 336 | bool KdTree::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const 337 | { 338 | if (node == NULL) { 339 | return true; 340 | } 341 | else { 342 | const Obstacle *const obstacle1 = node->obstacle; 343 | const Obstacle *const obstacle2 = obstacle1->nextObstacle_; 344 | 345 | const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1); 346 | const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2); 347 | const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_); 348 | 349 | if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { 350 | return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right)); 351 | } 352 | else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { 353 | return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left)); 354 | } 355 | else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { 356 | /* One can see through obstacle from left to right. */ 357 | return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right); 358 | } 359 | else { 360 | const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_); 361 | const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_); 362 | const float invLengthQ = 1.0f / absSq(q2 - q1); 363 | 364 | return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right)); 365 | } 366 | } 367 | } 368 | } 369 | -------------------------------------------------------------------------------- /src/Agent.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Agent.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 . 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 | * 31 | */ 32 | 33 | #include "Agent.h" 34 | 35 | #include "KdTree.h" 36 | #include "Obstacle.h" 37 | 38 | namespace RVO { 39 | Agent::Agent(RVOSimulator *sim) : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), sim_(sim), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { } 40 | 41 | void Agent::computeNeighbors() 42 | { 43 | obstacleNeighbors_.clear(); 44 | float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); 45 | sim_->kdTree_->computeObstacleNeighbors(this, rangeSq); 46 | 47 | agentNeighbors_.clear(); 48 | 49 | if (maxNeighbors_ > 0) { 50 | rangeSq = sqr(neighborDist_); 51 | sim_->kdTree_->computeAgentNeighbors(this, rangeSq); 52 | } 53 | } 54 | 55 | /* Search for the best new velocity. */ 56 | void Agent::computeNewVelocity() 57 | { 58 | orcaLines_.clear(); 59 | 60 | const float invTimeHorizonObst = 1.0f / timeHorizonObst_; 61 | 62 | /* Create obstacle ORCA lines. */ 63 | for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) { 64 | 65 | const Obstacle *obstacle1 = obstacleNeighbors_[i].second; 66 | const Obstacle *obstacle2 = obstacle1->nextObstacle_; 67 | 68 | const Vector2 relativePosition1 = obstacle1->point_ - position_; 69 | const Vector2 relativePosition2 = obstacle2->point_ - position_; 70 | 71 | /* 72 | * Check if velocity obstacle of obstacle is already taken care of by 73 | * previously constructed obstacle ORCA lines. 74 | */ 75 | bool alreadyCovered = false; 76 | 77 | for (size_t j = 0; j < orcaLines_.size(); ++j) { 78 | if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) { 79 | alreadyCovered = true; 80 | break; 81 | } 82 | } 83 | 84 | if (alreadyCovered) { 85 | continue; 86 | } 87 | 88 | /* Not yet covered. Check for collisions. */ 89 | 90 | const float distSq1 = absSq(relativePosition1); 91 | const float distSq2 = absSq(relativePosition2); 92 | 93 | const float radiusSq = sqr(radius_); 94 | 95 | const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_; 96 | const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector); 97 | const float distSqLine = absSq(-relativePosition1 - s * obstacleVector); 98 | 99 | Line line; 100 | 101 | if (s < 0.0f && distSq1 <= radiusSq) { 102 | /* Collision with left vertex. Ignore if non-convex. */ 103 | if (obstacle1->isConvex_) { 104 | line.point = Vector2(0.0f, 0.0f); 105 | line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x())); 106 | orcaLines_.push_back(line); 107 | } 108 | 109 | continue; 110 | } 111 | else if (s > 1.0f && distSq2 <= radiusSq) { 112 | /* Collision with right vertex. Ignore if non-convex 113 | * or if it will be taken care of by neighoring obstace */ 114 | if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) { 115 | line.point = Vector2(0.0f, 0.0f); 116 | line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x())); 117 | orcaLines_.push_back(line); 118 | } 119 | 120 | continue; 121 | } 122 | else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) { 123 | /* Collision with obstacle segment. */ 124 | line.point = Vector2(0.0f, 0.0f); 125 | line.direction = -obstacle1->unitDir_; 126 | orcaLines_.push_back(line); 127 | continue; 128 | } 129 | 130 | /* 131 | * No collision. 132 | * Compute legs. When obliquely viewed, both legs can come from a single 133 | * vertex. Legs extend cut-off line when nonconvex vertex. 134 | */ 135 | 136 | Vector2 leftLegDirection, rightLegDirection; 137 | 138 | if (s < 0.0f && distSqLine <= radiusSq) { 139 | /* 140 | * Obstacle viewed obliquely so that left vertex 141 | * defines velocity obstacle. 142 | */ 143 | if (!obstacle1->isConvex_) { 144 | /* Ignore obstacle. */ 145 | continue; 146 | } 147 | 148 | obstacle2 = obstacle1; 149 | 150 | const float leg1 = std::sqrt(distSq1 - radiusSq); 151 | leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 152 | rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 153 | } 154 | else if (s > 1.0f && distSqLine <= radiusSq) { 155 | /* 156 | * Obstacle viewed obliquely so that 157 | * right vertex defines velocity obstacle. 158 | */ 159 | if (!obstacle2->isConvex_) { 160 | /* Ignore obstacle. */ 161 | continue; 162 | } 163 | 164 | obstacle1 = obstacle2; 165 | 166 | const float leg2 = std::sqrt(distSq2 - radiusSq); 167 | leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 168 | rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 169 | } 170 | else { 171 | /* Usual situation. */ 172 | if (obstacle1->isConvex_) { 173 | const float leg1 = std::sqrt(distSq1 - radiusSq); 174 | leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; 175 | } 176 | else { 177 | /* Left vertex non-convex; left leg extends cut-off line. */ 178 | leftLegDirection = -obstacle1->unitDir_; 179 | } 180 | 181 | if (obstacle2->isConvex_) { 182 | const float leg2 = std::sqrt(distSq2 - radiusSq); 183 | rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; 184 | } 185 | else { 186 | /* Right vertex non-convex; right leg extends cut-off line. */ 187 | rightLegDirection = obstacle1->unitDir_; 188 | } 189 | } 190 | 191 | /* 192 | * Legs can never point into neighboring edge when convex vertex, 193 | * take cutoff-line of neighboring edge instead. If velocity projected on 194 | * "foreign" leg, no constraint is added. 195 | */ 196 | 197 | const Obstacle *const leftNeighbor = obstacle1->prevObstacle_; 198 | 199 | bool isLeftLegForeign = false; 200 | bool isRightLegForeign = false; 201 | 202 | if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) { 203 | /* Left leg points into obstacle. */ 204 | leftLegDirection = -leftNeighbor->unitDir_; 205 | isLeftLegForeign = true; 206 | } 207 | 208 | if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) { 209 | /* Right leg points into obstacle. */ 210 | rightLegDirection = obstacle2->unitDir_; 211 | isRightLegForeign = true; 212 | } 213 | 214 | /* Compute cut-off centers. */ 215 | const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_); 216 | const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_); 217 | const Vector2 cutoffVec = rightCutoff - leftCutoff; 218 | 219 | /* Project current velocity on velocity obstacle. */ 220 | 221 | /* Check if current velocity is projected on cutoff circles. */ 222 | const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec)); 223 | const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection); 224 | const float tRight = ((velocity_ - rightCutoff) * rightLegDirection); 225 | 226 | if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) { 227 | /* Project on left cut-off circle. */ 228 | const Vector2 unitW = normalize(velocity_ - leftCutoff); 229 | 230 | line.direction = Vector2(unitW.y(), -unitW.x()); 231 | line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW; 232 | orcaLines_.push_back(line); 233 | continue; 234 | } 235 | else if (t > 1.0f && tRight < 0.0f) { 236 | /* Project on right cut-off circle. */ 237 | const Vector2 unitW = normalize(velocity_ - rightCutoff); 238 | 239 | line.direction = Vector2(unitW.y(), -unitW.x()); 240 | line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW; 241 | orcaLines_.push_back(line); 242 | continue; 243 | } 244 | 245 | /* 246 | * Project on left leg, right leg, or cut-off line, whichever is closest 247 | * to velocity. 248 | */ 249 | const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec))); 250 | const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection))); 251 | const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection))); 252 | 253 | if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { 254 | /* Project on cut-off line. */ 255 | line.direction = -obstacle1->unitDir_; 256 | line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 257 | orcaLines_.push_back(line); 258 | continue; 259 | } 260 | else if (distSqLeft <= distSqRight) { 261 | /* Project on left leg. */ 262 | if (isLeftLegForeign) { 263 | continue; 264 | } 265 | 266 | line.direction = leftLegDirection; 267 | line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 268 | orcaLines_.push_back(line); 269 | continue; 270 | } 271 | else { 272 | /* Project on right leg. */ 273 | if (isRightLegForeign) { 274 | continue; 275 | } 276 | 277 | line.direction = -rightLegDirection; 278 | line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); 279 | orcaLines_.push_back(line); 280 | continue; 281 | } 282 | } 283 | 284 | const size_t numObstLines = orcaLines_.size(); 285 | 286 | const float invTimeHorizon = 1.0f / timeHorizon_; 287 | 288 | /* Create agent ORCA lines. */ 289 | for (size_t i = 0; i < agentNeighbors_.size(); ++i) { 290 | const Agent *const other = agentNeighbors_[i].second; 291 | 292 | const Vector2 relativePosition = other->position_ - position_; 293 | const Vector2 relativeVelocity = velocity_ - other->velocity_; 294 | const float distSq = absSq(relativePosition); 295 | const float combinedRadius = radius_ + other->radius_; 296 | const float combinedRadiusSq = sqr(combinedRadius); 297 | 298 | Line line; 299 | Vector2 u; 300 | 301 | if (distSq > combinedRadiusSq) { 302 | /* No collision. */ 303 | const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition; 304 | /* Vector from cutoff center to relative velocity. */ 305 | const float wLengthSq = absSq(w); 306 | 307 | const float dotProduct1 = w * relativePosition; 308 | 309 | if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) { 310 | /* Project on cut-off circle. */ 311 | const float wLength = std::sqrt(wLengthSq); 312 | const Vector2 unitW = w / wLength; 313 | 314 | line.direction = Vector2(unitW.y(), -unitW.x()); 315 | u = (combinedRadius * invTimeHorizon - wLength) * unitW; 316 | } 317 | else { 318 | /* Project on legs. */ 319 | const float leg = std::sqrt(distSq - combinedRadiusSq); 320 | 321 | if (det(relativePosition, w) > 0.0f) { 322 | /* Project on left leg. */ 323 | line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; 324 | } 325 | else { 326 | /* Project on right leg. */ 327 | line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; 328 | } 329 | 330 | const float dotProduct2 = relativeVelocity * line.direction; 331 | 332 | u = dotProduct2 * line.direction - relativeVelocity; 333 | } 334 | } 335 | else { 336 | /* Collision. Project on cut-off circle of time timeStep. */ 337 | const float invTimeStep = 1.0f / sim_->timeStep_; 338 | 339 | /* Vector from cutoff center to relative velocity. */ 340 | const Vector2 w = relativeVelocity - invTimeStep * relativePosition; 341 | 342 | const float wLength = abs(w); 343 | const Vector2 unitW = w / wLength; 344 | 345 | line.direction = Vector2(unitW.y(), -unitW.x()); 346 | u = (combinedRadius * invTimeStep - wLength) * unitW; 347 | } 348 | 349 | line.point = velocity_ + 0.5f * u; 350 | orcaLines_.push_back(line); 351 | } 352 | 353 | size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_); 354 | 355 | if (lineFail < orcaLines_.size()) { 356 | linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_); 357 | } 358 | } 359 | 360 | void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) 361 | { 362 | if (this != agent) { 363 | const float distSq = absSq(position_ - agent->position_); 364 | 365 | if (distSq < rangeSq) { 366 | if (agentNeighbors_.size() < maxNeighbors_) { 367 | agentNeighbors_.push_back(std::make_pair(distSq, agent)); 368 | } 369 | 370 | size_t i = agentNeighbors_.size() - 1; 371 | 372 | while (i != 0 && distSq < agentNeighbors_[i - 1].first) { 373 | agentNeighbors_[i] = agentNeighbors_[i - 1]; 374 | --i; 375 | } 376 | 377 | agentNeighbors_[i] = std::make_pair(distSq, agent); 378 | 379 | if (agentNeighbors_.size() == maxNeighbors_) { 380 | rangeSq = agentNeighbors_.back().first; 381 | } 382 | } 383 | } 384 | } 385 | 386 | void Agent::insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq) 387 | { 388 | const Obstacle *const nextObstacle = obstacle->nextObstacle_; 389 | 390 | const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_); 391 | 392 | if (distSq < rangeSq) { 393 | obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle)); 394 | 395 | size_t i = obstacleNeighbors_.size() - 1; 396 | 397 | while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) { 398 | obstacleNeighbors_[i] = obstacleNeighbors_[i - 1]; 399 | --i; 400 | } 401 | 402 | obstacleNeighbors_[i] = std::make_pair(distSq, obstacle); 403 | } 404 | } 405 | 406 | void Agent::update() 407 | { 408 | velocity_ = newVelocity_; 409 | position_ += velocity_ * sim_->timeStep_; 410 | } 411 | 412 | bool linearProgram1(const std::vector &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) 413 | { 414 | const float dotProduct = lines[lineNo].point * lines[lineNo].direction; 415 | const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point); 416 | 417 | if (discriminant < 0.0f) { 418 | /* Max speed circle fully invalidates line lineNo. */ 419 | return false; 420 | } 421 | 422 | const float sqrtDiscriminant = std::sqrt(discriminant); 423 | float tLeft = -dotProduct - sqrtDiscriminant; 424 | float tRight = -dotProduct + sqrtDiscriminant; 425 | 426 | for (size_t i = 0; i < lineNo; ++i) { 427 | const float denominator = det(lines[lineNo].direction, lines[i].direction); 428 | const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point); 429 | 430 | if (std::fabs(denominator) <= RVO_EPSILON) { 431 | /* Lines lineNo and i are (almost) parallel. */ 432 | if (numerator < 0.0f) { 433 | return false; 434 | } 435 | else { 436 | continue; 437 | } 438 | } 439 | 440 | const float t = numerator / denominator; 441 | 442 | if (denominator >= 0.0f) { 443 | /* Line i bounds line lineNo on the right. */ 444 | tRight = std::min(tRight, t); 445 | } 446 | else { 447 | /* Line i bounds line lineNo on the left. */ 448 | tLeft = std::max(tLeft, t); 449 | } 450 | 451 | if (tLeft > tRight) { 452 | return false; 453 | } 454 | } 455 | 456 | if (directionOpt) { 457 | /* Optimize direction. */ 458 | if (optVelocity * lines[lineNo].direction > 0.0f) { 459 | /* Take right extreme. */ 460 | result = lines[lineNo].point + tRight * lines[lineNo].direction; 461 | } 462 | else { 463 | /* Take left extreme. */ 464 | result = lines[lineNo].point + tLeft * lines[lineNo].direction; 465 | } 466 | } 467 | else { 468 | /* Optimize closest point. */ 469 | const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point); 470 | 471 | if (t < tLeft) { 472 | result = lines[lineNo].point + tLeft * lines[lineNo].direction; 473 | } 474 | else if (t > tRight) { 475 | result = lines[lineNo].point + tRight * lines[lineNo].direction; 476 | } 477 | else { 478 | result = lines[lineNo].point + t * lines[lineNo].direction; 479 | } 480 | } 481 | 482 | return true; 483 | } 484 | 485 | size_t linearProgram2(const std::vector &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) 486 | { 487 | if (directionOpt) { 488 | /* 489 | * Optimize direction. Note that the optimization velocity is of unit 490 | * length in this case. 491 | */ 492 | result = optVelocity * radius; 493 | } 494 | else if (absSq(optVelocity) > sqr(radius)) { 495 | /* Optimize closest point and outside circle. */ 496 | result = normalize(optVelocity) * radius; 497 | } 498 | else { 499 | /* Optimize closest point and inside circle. */ 500 | result = optVelocity; 501 | } 502 | 503 | for (size_t i = 0; i < lines.size(); ++i) { 504 | if (det(lines[i].direction, lines[i].point - result) > 0.0f) { 505 | /* Result does not satisfy constraint i. Compute new optimal result. */ 506 | const Vector2 tempResult = result; 507 | 508 | if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { 509 | result = tempResult; 510 | return i; 511 | } 512 | } 513 | } 514 | 515 | return lines.size(); 516 | } 517 | 518 | void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result) 519 | { 520 | float distance = 0.0f; 521 | 522 | for (size_t i = beginLine; i < lines.size(); ++i) { 523 | if (det(lines[i].direction, lines[i].point - result) > distance) { 524 | /* Result does not satisfy constraint of line i. */ 525 | std::vector projLines(lines.begin(), lines.begin() + static_cast(numObstLines)); 526 | 527 | for (size_t j = numObstLines; j < i; ++j) { 528 | Line line; 529 | 530 | float determinant = det(lines[i].direction, lines[j].direction); 531 | 532 | if (std::fabs(determinant) <= RVO_EPSILON) { 533 | /* Line i and line j are parallel. */ 534 | if (lines[i].direction * lines[j].direction > 0.0f) { 535 | /* Line i and line j point in the same direction. */ 536 | continue; 537 | } 538 | else { 539 | /* Line i and line j point in opposite direction. */ 540 | line.point = 0.5f * (lines[i].point + lines[j].point); 541 | } 542 | } 543 | else { 544 | line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; 545 | } 546 | 547 | line.direction = normalize(lines[j].direction - lines[i].direction); 548 | projLines.push_back(line); 549 | } 550 | 551 | const Vector2 tempResult = result; 552 | 553 | if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) { 554 | /* This should in principle not happen. The result is by definition 555 | * already in the feasible region of this linear program. If it fails, 556 | * it is due to small floating point error, and the current result is 557 | * kept. 558 | */ 559 | result = tempResult; 560 | } 561 | 562 | distance = det(lines[i].direction, lines[i].point - result); 563 | } 564 | } 565 | } 566 | } 567 | -------------------------------------------------------------------------------- /src/RVO.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RVO.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 . 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 | * 31 | */ 32 | 33 | #ifndef RVO_RVO_H_ 34 | #define RVO_RVO_H_ 35 | 36 | #include "RVOSimulator.h" 37 | #include "Vector2.h" 38 | 39 | /** 40 | 41 | \file RVO.h 42 | \brief Includes all public headers in the library. 43 | 44 | \namespace RVO 45 | \brief Contains all classes, functions, and constants used in the library. 46 | 47 | \mainpage RVO2 Library 48 | 49 | \author Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and 50 | Dinesh Manocha 51 | 52 | RVO2 Library is an easy-to-use C++ implementation of the 53 | Optimal Reciprocal Collision Avoidance 54 | (ORCA) formulation for multi-agent simulation. RVO2 Library automatically 55 | uses parallelism for computing the motion of the agents if your machine has 56 | multiple processors and your compiler supports 57 | OpenMP. 58 | 59 | Please follow the following steps to install and use RVO2 Library. 60 | 61 | - \subpage whatsnew 62 | - \subpage building 63 | - \subpage using 64 | - \subpage params 65 | 66 | See the documentation of the RVO::RVOSimulator class for an exhaustive list of 67 | public functions of RVO2 Library. 68 | 69 | RVO2 Library, accompanying example code, and this documentation is 70 | released for educational, research, and non-profit purposes under the following 71 | \subpage terms "terms and conditions". 72 | 73 | 74 | \page whatsnew What Is New in RVO2 Library 75 | 76 | \section localca Local Collision Avoidance 77 | 78 | The main difference between RVO2 Library and %RVO Library 1.x is the 79 | local collision avoidance technique used. RVO2 Library uses 80 | Optimal Reciprocal Collision Avoidance 81 | (ORCA), whereas %RVO Library 1.x uses 82 | Reciprocal Velocity Obstacles (%RVO). For legacy reasons, and since both 83 | techniques are based on the same principles of reciprocal collision avoidance 84 | and relative velocity, we did not change the name of the library. 85 | 86 | A major consequence of the change of local collision avoidance technique is that 87 | the simulation has become much faster in RVO2 Library. ORCA defines 88 | velocity constraints with respect to other agents as half-planes, and an optimal 89 | velocity is efficiently found using (two-dimensional) linear programming. In 90 | contrast, %RVO Library 1.x uses random sampling to find a good velocity. Also, 91 | the behavior of the agents is smoother in RVO2 Library. It is proven 92 | mathematically that ORCA lets the velocity of agents evolve continuously over 93 | time, whereas %RVO Library 1.x occasionally showed oscillations and reciprocal 94 | dances. Furthermore, ORCA provides stronger guarantees with respect to collision 95 | avoidance. 96 | 97 | \section global Global Path Planning 98 | 99 | Local collision avoidance as provided by RVO2 Library should in principle 100 | be accompanied by global path planning that determines the preferred velocity of 101 | each agent in each time step of the simulation. %RVO Library 1.x has a built-in 102 | roadmap infrastructure to guide agents around obstacles to fixed goals. 103 | However, besides roadmaps, other techniques for global planning, such as 104 | navigation fields, cell decompositions, etc. exist. Therefore, RVO2 105 | Library does not provide global planning infrastructure anymore. Instead, 106 | it is the responsibility of the external application to set the preferred 107 | velocity of each agent ahead of each time step of the simulation. This makes the 108 | library more flexible to use in varying application domains. In one of the 109 | example applications that comes with RVO2 Library, we show how a roadmap 110 | similar to %RVO Library 1.x is used externally to guide the global navigation of 111 | the agents. As a consequence of this change, RVO2 Library does not have a 112 | concept of a "goal position" or "preferred speed" for each 113 | agent, but only relies on the preferred velocities of the agents set by the 114 | external application. 115 | 116 | \section structure Structure of RVO2 Library 117 | 118 | The structure of RVO2 Library is similar to that of %RVO Library 1.x. 119 | Users familiar with %RVO Library 1.x should find little trouble in using RVO2 120 | Library. However, RVO2 Library is not backwards compatible with %RVO 121 | Library 1.x. The main reason for this is that the ORCA technique requires 122 | different (and fewer) parameters to be set than %RVO. Also, the way obstacles 123 | are represented is different. In %RVO Library 1.x, obstacles are represented by 124 | an arbitrary collection of line segments. In RVO2 Library, obstacles are 125 | non-intersecting polygons, specified by lists of vertices in counterclockwise 126 | order. Further, in %RVO Library 1.x agents cannot be added to the simulation 127 | after the simulation is initialized. In RVO2 Library this restriction is 128 | removed. Obstacles still need to be processed before the simulation starts, 129 | though. Lastly, in %RVO Library 1.x an instance of the simulator is a singleton. 130 | This restriction is removed in RVO2 Library. 131 | 132 | \section smaller Smaller Changes 133 | 134 | With RVO2 Library, we have adopted the philosophy that anything that is 135 | not part of the core local collision avoidance technique is to be stripped from 136 | the library. Therefore, besides the roadmap infrastructure, we have also removed 137 | acceleration constraints of agents, orientation of agents, and the unused 138 | "class" of agents. Each of these can be implemented external of the 139 | library if needed. We did maintain a kd-tree infrastructure for 140 | efficiently finding other agents and obstacles nearby each agent. 141 | 142 | Also, RVO2 Library allows accessing information about the simulation, 143 | such as the neighbors and the collision-avoidance constraints of each agent, 144 | that is hidden from the user in %RVO Library 1.x. 145 | 146 | 147 | \page building Building RVO2 Library 148 | 149 | We assume that you have downloaded RVO2 Library and unpacked the ZIP 150 | archive to a path $RVO_ROOT. 151 | 152 | \section xcode Apple Xcode 3.x 153 | 154 | Open $RVO_ROOT/RVO.xcodeproj and select the %RVO target and 155 | a configuration (Debug or Release). A framework 156 | RVO.framework will be built in the default build directory, e.g. 157 | $RVO_ROOT/build/Release. 158 | 159 | \section cmake CMake 160 | 161 | Create and switch to your chosen build directory, e.g. $RVO_ROOT/build. 162 | Run cmake inside the build directory on the source directory, e.g. 163 | cmake $RVO_ROOT/src. Build files for the default generator for your 164 | platform will be generated in the build directory. 165 | 166 | \section make GNU Make 167 | 168 | Switch to the source directory $RVO_ROOT/src and run make. 169 | Public header files (RVO.h, RVOSimulator.h, and 170 | Vector2.h) will be copied to the $RVO_ROOT/include directory 171 | and a static library libRVO.a will be compiled into the 172 | $RVO_ROOT/lib directory. 173 | 174 | \section visual Microsoft Visual Studio 2008 175 | 176 | Open $RVO_ROOT/RVO.sln and select the %RVO project and a 177 | configuration (Debug, ReleaseST, or ReleaseMT). 178 | Public header files (RVO.h, RVOSimulator.h, and 179 | Vector2.h) will be copied to the $RVO_ROOT/include directory 180 | and a static library, e.g. RVO.lib, will be compiled into the 181 | $RVO_ROOT/lib directory. 182 | 183 | 184 | \page using Using RVO2 Library 185 | 186 | \section structure Structure 187 | 188 | A program performing an RVO2 Library simulation has the following global 189 | structure. 190 | 191 | \code 192 | #include 193 | 194 | std::vector goals; 195 | 196 | int main() 197 | { 198 | // Create a new simulator instance. 199 | RVO::RVOSimulator* sim = new RVO::RVOSimulator(); 200 | 201 | // Set up the scenario. 202 | setupScenario(sim); 203 | 204 | // Perform (and manipulate) the simulation. 205 | do { 206 | updateVisualization(sim); 207 | setPreferredVelocities(sim); 208 | sim->doStep(); 209 | } while (!reachedGoal(sim)); 210 | 211 | delete sim; 212 | } 213 | \endcode 214 | 215 | In order to use RVO2 Library, the user needs to include RVO.h. The first 216 | step is then to create an instance of RVO::RVOSimulator. Then, the process 217 | consists of two stages. The first stage is specifying the simulation scenario 218 | and its parameters. In the above example program, this is done in the method 219 | setupScenario(...), which we will discuss below. The second stage is the actual 220 | performing of the simulation. 221 | 222 | In the above example program, simulation steps are taken until all 223 | the agents have reached some predefined goals. Prior to each simulation step, 224 | we set the preferred velocity for each agent, i.e. the 225 | velocity the agent would have taken if there were no other agents around, in the 226 | method setPreferredVelocities(...). The simulator computes the actual velocities 227 | of the agents and attempts to follow the preferred velocities as closely as 228 | possible while guaranteeing collision avoidance at the same time. During the 229 | simulation, the user may want to retrieve information from the simulation for 230 | instance to visualize the simulation. In the above example program, this is done 231 | in the method updateVisualization(...), which we will discuss below. It is also 232 | possible to manipulate the simulation during the simulation, for instance by 233 | changing positions, radii, velocities, etc. of the agents. 234 | 235 | \section spec Setting up the Simulation Scenario 236 | 237 | A scenario that is to be simulated can be set up as follows. A scenario consists 238 | of two types of objects: agents and obstacles. Each of them can be manually 239 | specified. Agents may be added anytime before or during the simulation. 240 | Obstacles, however, need to be defined prior to the simulation, and 241 | RVO::RVOSimulator::processObstacles() need to be called in order for the 242 | obstacles to be accounted for in the simulation. 243 | The user may also want to define goal positions of the agents, or a 244 | roadmap to guide the agents around obstacles. This is not done in RVO2 245 | Library, but needs to be taken care of in the user's external application. 246 | 247 | The following example creates a scenario with four agents exchanging positions 248 | around a rectangular obstacle in the middle. 249 | 250 | \code 251 | void setupScenario(RVO::RVOSimulator* sim) { 252 | // Specify global time step of the simulation. 253 | sim->setTimeStep(0.25f); 254 | 255 | // Specify default parameters for agents that are subsequently added. 256 | sim->setAgentDefaults(15.0f, 10, 10.0f, 5.0f, 2.0f, 2.0f); 257 | 258 | // Add agents, specifying their start position. 259 | sim->addAgent(RVO::Vector2(-50.0f, -50.0f)); 260 | sim->addAgent(RVO::Vector2(50.0f, -50.0f)); 261 | sim->addAgent(RVO::Vector2(50.0f, 50.0f)); 262 | sim->addAgent(RVO::Vector2(-50.0f, 50.0f)); 263 | 264 | // Create goals (simulator is unaware of these). 265 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 266 | goals.push_back(-sim->getAgentPosition(i)); 267 | } 268 | 269 | // Add (polygonal) obstacle(s), specifying vertices in counterclockwise order. 270 | std::vector vertices; 271 | vertices.push_back(RVO::Vector2(-7.0f, -20.0f)); 272 | vertices.push_back(RVO::Vector2(7.0f, -20.0f)); 273 | vertices.push_back(RVO::Vector2(7.0f, 20.0f)); 274 | vertices.push_back(RVO::Vector2(-7.0f, 20.0f)); 275 | 276 | sim->addObstacle(vertices); 277 | 278 | // Process obstacles so that they are accounted for in the simulation. 279 | sim->processObstacles(); 280 | } 281 | \endcode 282 | 283 | See the documentation on RVO::RVOSimulator for a full overview of the 284 | functionality to specify scenarios. 285 | 286 | \section ret Retrieving Information from the Simulation 287 | 288 | During the simulation, the user can extract information from the simulation for 289 | instance for visualization purposes, or to determine termination conditions of 290 | the simulation. In the example program above, visualization is done in the 291 | updateVisualization(...) method. Below we give an example that simply writes 292 | the positions of each agent in each time step to the standard output. The 293 | termination condition is checked in the reachedGoal(...) method. Here we give an 294 | example that returns true if all agents are within one radius of their goals. 295 | 296 | \code 297 | void updateVisualization(RVO::RVOSimulator* sim) { 298 | // Output the current global time. 299 | std::cout << sim->getGlobalTime() << " "; 300 | 301 | // Output the position for all the agents. 302 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 303 | std::cout << sim->getAgentPosition(i) << " "; 304 | } 305 | 306 | std::cout << std::endl; 307 | } 308 | \endcode 309 | 310 | \code 311 | bool reachedGoal(RVO::RVOSimulator* sim) { 312 | // Check whether all agents have arrived at their goals. 313 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 314 | if (absSq(goals[i] - sim->getAgentPosition(i)) > sim->getAgentRadius(i) * sim->getAgentRadius(i)) { 315 | // Agent is further away from its goal than one radius. 316 | return false; 317 | } 318 | } 319 | return true; 320 | } 321 | \endcode 322 | 323 | Using similar functions as the ones used in this example, the user can access 324 | information about other parameters of the agents, as well as the global 325 | parameters, and the obstacles. See the documentation of the class 326 | RVO::RVOSimulator for an exhaustive list of public functions for retrieving 327 | simulation information. 328 | 329 | \section manip Manipulating the Simulation 330 | 331 | During the simulation, the user can manipulate the simulation, for instance by 332 | changing the global parameters, or changing the parameters of the agents 333 | (potentially causing abrupt different behavior). It is also possible to give the 334 | agents a new position, which make them jump through the scene. 335 | New agents can be added to the simulation at any time, but it is not allowed to 336 | add obstacles to the simulation after they have been processed by calling 337 | RVO::RVOSimulator::processObstacles(). Also, it is impossible to change the 338 | position of the vertices of the obstacles. 339 | 340 | See the documentation of the class RVO::RVOSimulator for an exhaustive list of 341 | public functions for manipulating the simulation. 342 | 343 | To provide global guidance to the agents, the preferred velocities of the agents 344 | can be changed ahead of each simulation step. In the above example program, this 345 | happens in the method setPreferredVelocities(...). Here we give an example that 346 | simply sets the preferred velocity to the unit vector towards the agent's goal 347 | for each agent (i.e., the preferred speed is 1.0). Note that this may not give 348 | convincing results with respect to global navigation around the obstacles. For 349 | this a roadmap or other global planning techniques may be used (see one of the 350 | \ref example "example programs" that accompanies RVO2 Library). 351 | 352 | \code 353 | void setPreferredVelocities(RVO::RVOSimulator* sim) { 354 | // Set the preferred velocity for each agent. 355 | for (size_t i = 0; i < sim->getNumAgents(); ++i) { 356 | if (absSq(goals[i] - sim->getAgentPosition(i)) < sim->getAgentRadius(i) * sim->getAgentRadius(i) ) { 357 | // Agent is within one radius of its goal, set preferred velocity to zero 358 | sim->setAgentPrefVelocity(i, RVO::Vector2(0.0f, 0.0f)); 359 | } else { 360 | // Agent is far away from its goal, set preferred velocity as unit vector towards agent's goal. 361 | sim->setAgentPrefVelocity(i, normalize(goals[i] - sim->getAgentPosition(i))); 362 | } 363 | } 364 | } 365 | \endcode 366 | 367 | \section example Example Programs 368 | 369 | RVO2 Library is accompanied by three example programs, which can be found in the 370 | $RVO_ROOT/examples directory. The examples are named Blocks, Circle, and 371 | Roadmap, and contain the following demonstration scenarios: 372 | 373 | 374 | 375 | 379 | 380 | 381 | 382 | 385 | 386 | 387 | 388 | 390 | 391 |

Blocks	A scenario in which 100 agents, split in four groups initially 376 \| positioned in each of four corners of the environment, move to the 377 \| other side of the environment through a narrow passage generated by four 378 \| obstacles. There is no roadmap to guide the agents around the obstacles.
Circle	A scenario in which 250 agents, initially positioned evenly 383 \| distributed on a circle, move to the antipodal position on the 384 \| circle. There are no obstacles.
Roadmap	The same scenario as ExampleBlocks, but now the preferred velocities 389 \| of the agents are determined using a roadmap guiding the agents around the obstacles.

392 | 393 | 394 | \page params Parameter Overview 395 | 396 | \section globalp Global Parameters 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 |

Parameter	Type (unit)	Meaning
timeStep	float (time)	The time step of the simulation. Must be positive.

410 | 411 | \section agent Agent Parameters 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 450 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 471 | 472 | 473 | 474 | 475 | 481 | 482 | 483 | 484 | 486 | 487 | 488 |

Parameter	Type (unit)	Meaning
maxNeighbors	size_t	The maximum number of other agents the agent takes into 423 \| account in the navigation. The larger this number, the 424 \| longer the running time of the simulation. If the number is 425 \| too low, the simulation will not be safe.
maxSpeed	float (distance/time)	The maximum speed of the agent. Must be non-negative.
neighborDist	float (distance)	The maximum distance (center point to center point) to 436 \| other agents the agent takes into account in the 437 \| navigation. The larger this number, the longer the running 438 \| time of the simulation. If the number is too low, the 439 \| simulation will not be safe. Must be non-negative.
position	RVO::Vector2 (distance, distance)	The current position of the agent.
prefVelocity	RVO::Vector2 (distance/time, distance/time) 449 \|	The current preferred velocity of the agent. This is the 451 \| velocity the agent would take if no other agents or 452 \| obstacles were around. The simulator computes an actual 453 \| velocity for the agent that follows the preferred velocity 454 \| as closely as possible, but at the same time guarantees 455 \| collision avoidance.
radius	float (distance)	The radius of the agent. Must be non-negative.
timeHorizon	float (time)	The minimal amount of time for which the agent's velocities 466 \| that are computed by the simulation are safe with respect 467 \| to other agents. The larger this number, the sooner this 468 \| agent will respond to the presence of other agents, but the 469 \| less freedom the agent has in choosing its velocities. 470 \| Must be positive.
timeHorizonObst	float (time)	The minimal amount of time for which the agent's velocities 476 \| that are computed by the simulation are safe with respect 477 \| to obstacles. The larger this number, the sooner this agent 478 \| will respond to the presence of obstacles, but the less 479 \| freedom the agent has in choosing its velocities. 480 \| Must be positive.
velocity	RVO::Vector2 (distance/time, distance/time) 485 \|	The (current) velocity of the agent.

489 | 490 | 491 | \page terms Terms and Conditions 492 | 493 | RVO2 Library 494 | 495 | Copyright 2008 University of North Carolina at Chapel Hill 496 | 497 | Licensed under the Apache License, Version 2.0 (the "License"); 498 | you may not use this file except in compliance with the License. 499 | You may obtain a copy of the License at 500 | 501 | http://www.apache.org/licenses/LICENSE-2.0 502 | 503 | Unless required by applicable law or agreed to in writing, software 504 | distributed under the License is distributed on an "AS IS" BASIS, 505 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 506 | See the License for the specific language governing permissions and 507 | limitations under the License. 508 | 509 | */ 510 | 511 | #endif /* RVO_RVO_H_ */ 512 | -------------------------------------------------------------------------------- /src/RVOSimulator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * RVOSimulator.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 . 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 | * 31 | */ 32 | 33 | #ifndef RVO_RVO_SIMULATOR_H_ 34 | #define RVO_RVO_SIMULATOR_H_ 35 | 36 | /** 37 | * \file RVOSimulator.h 38 | * \brief Contains the RVOSimulator class. 39 | */ 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include "Vector2.h" 46 | 47 | namespace RVO { 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 RVO::RVOSimulator. 53 | */ 54 | const size_t RVO_ERROR = std::numeric_limits::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 Agent; 73 | class KdTree; 74 | class Obstacle; 75 | 76 | /** 77 | * \brief Defines the simulation. 78 | * 79 | * The main class of the library that contains all simulation functionality. 80 | */ 81 | class RVOSimulator { 82 | public: 83 | /** 84 | * \brief Constructs a simulator instance. 85 | */ 86 | RVOSimulator(); 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 | RVOSimulator(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 | ~RVOSimulator(); 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 RVO::RVO_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 RVO::RVO_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 &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 | private: 579 | std::vector agents_; 580 | Agent *defaultAgent_; 581 | float globalTime_; 582 | KdTree *kdTree_; 583 | std::vector obstacles_; 584 | float timeStep_; 585 | 586 | friend class Agent; 587 | friend class KdTree; 588 | friend class Obstacle; 589 | }; 590 | } 591 | 592 | #endif /* RVO_RVO_SIMULATOR_H_ */ 593 | --------------------------------------------------------------------------------