├── .editorconfig ├── .gitignore ├── .travis.yml ├── CMakeLists.txt ├── CTestConfig.cmake ├── LICENSE ├── README.md ├── appveyor.yml ├── doc ├── Doxyfile ├── footer.html ├── header.html └── stylesheet.css ├── example.py ├── example_ervo.py ├── examples ├── Blocks.cpp ├── CMakeLists.txt ├── Circle.cpp └── Roadmap.cpp ├── requirements.txt ├── rvo2-navmesh-test.blend ├── setup.py └── src ├── Agent.cpp ├── Agent.h ├── CMakeLists.txt ├── Definitions.h ├── ERVOSimulator.cpp ├── ERVOSimulator.h ├── KdTree.cpp ├── KdTree.h ├── Obstacle.cpp ├── Obstacle.h ├── RVO.h ├── RVOSimulator.cpp ├── RVOSimulator.h ├── Vector2.h └── rvo2.pyx /.editorconfig: -------------------------------------------------------------------------------- 1 | root = true 2 | 3 | [*] 4 | end_of_line = lf 5 | indent_size = 4 6 | indent_style = tab 7 | insert_final_newline = true 8 | trim_trailing_whitespace = true 9 | 10 | [*.md] 11 | trim_trailing_whitespace = false 12 | 13 | [*.yml] 14 | indent_size = 2 15 | indent_style = space 16 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | 15 | # Python & Cython-generated files 16 | *.egg-info/ 17 | __pycache__/ 18 | build/ 19 | src/rvo2.cpp 20 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | --- 2 | sudo: false 3 | language: cpp 4 | compiler: 5 | - clang 6 | - gcc 7 | os: 8 | - linux 9 | - osx 10 | script: 11 | - mkdir _build 12 | - cd _build 13 | - cmake -DCMAKE_CXX_FLAGS="-DRVO_OUTPUT_TIME_AND_POSITIONS=0 -DRVO_SEED_RANDOM_NUMBER_GENERATOR=0" .. 14 | - ctest --extra-verbose --dashboard Experimental . 15 | -------------------------------------------------------------------------------- /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 3.1) 34 | project(RVO) 35 | 36 | set(CMAKE_CXX_STANDARD 11) 37 | 38 | include(CTest) 39 | 40 | add_subdirectory(src) 41 | add_subdirectory(examples) 42 | 43 | include(CPack) 44 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | 204 | If you wish to make commercial use of any part of this source code, please contact us at contact@sinicx.com. 205 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Python-ERVO 2 | 3 | ![demo](https://user-images.githubusercontent.com/1871262/102694198-e0c27280-4262-11eb-89df-d1064529c5be.gif)![demo2](https://user-images.githubusercontent.com/1871262/102694194-db652800-4262-11eb-8a4e-eecb57c167ac.gif) 4 | 5 | - This repository containts the code for interactive crowd simulator used in the IROS 2020 paper ["L2B: Learning to Balance the Safety-Efficiency Trade-off in Interactive Crowd-aware Robot Navigation"](https://arxiv.org/pdf/2003.09207.pdf) 6 | - The original RVO implementation is the folk of [Python bindings for Optimal Reciprocal Collision Avoidance](https://github.com/sybrenstuvel/Python-RVO2). 7 | 8 | 9 | ### Dependencies 10 | - CMake `3.1+` 11 | - Cython `0.21.1+` 12 | 13 | ### Setup 14 | 15 | ```sh 16 | $ pip install -r requirements.txt 17 | $ python setup.py build # build 18 | $ python setup.py install # build & install 19 | ``` 20 | 21 | ### Usage 22 | - For more details, see `example_ervo.py` 23 | 24 | ```python 25 | # original RVO 26 | sim_rvo = rvo2.PyRVOSimulator(time_step, *params, radius, max_speed) 27 | sim_rvo.doStep() 28 | 29 | # extended ERVO 30 | sim_ervo = rvo2.PyERVOSimulator(time_step, *params, radius, max_speed) 31 | sim_ervo.doStep((beep_agent.px, beep_agent.py), beep_radius) 32 | ``` 33 | 34 | ### Citation 35 | If you find the simulator useful for your research, please consider citing: 36 | 37 | ``` 38 | @inproceedings{nishimura2020l2b, 39 | title={L2B: Learning to Balance the Safety-Efficiency Trade-off in Interactive Crowd-aware Robot Navigation}, 40 | author={Nishimura, Mai and Yonetani, Ryo}, 41 | journal={IEEE/RSJ International Conference on Intelligent Robots and Systems}, 42 | year={2020}, 43 | publisher={IEEE} 44 | } 45 | 46 | @incollection{van2011reciprocal, 47 | title={Reciprocal n-body collision avoidance}, 48 | author={Van Den Berg, Jur and Guy, Stephen J and Lin, Ming and Manocha, Dinesh}, 49 | booktitle={Robotics research}, 50 | pages={3--19}, 51 | year={2011}, 52 | publisher={Springer} 53 | } 54 | ``` 55 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | --- 2 | version: 2.0.2.{build} 3 | os: 4 | - Visual Studio 2013 5 | - Visual Studio 2015 6 | build_script: 7 | - md _build 8 | - cd _build 9 | - cmake -A x64 -DCMAKE_CXX_FLAGS="/DRVO_OUTPUT_TIME_AND_POSITIONS=0 /DRVO_SEED_RANDOM_NUMBER_GENERATOR=0" .. 10 | - ctest --build-config Release --extra-verbose --dashboard Experimental . 11 | test: off 12 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /doc/footer.html: -------------------------------------------------------------------------------- 1 | 2 | 3 |
4 | $projectname. Copyright 2008 University of North Carolina at Chapel Hill. 5 |
6 | 7 | 8 | -------------------------------------------------------------------------------- /doc/header.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | $projectname: $title 9 | 10 | 11 | 12 | $treeview 13 | $search 14 | $mathjax 15 | 16 | $extrastylesheet 17 | 18 | 19 |
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 | -------------------------------------------------------------------------------- /example_ervo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | minimal example of PyERVOSimulator 5 | Author: Mai Nishimura 6 | Copyright: 2020 OMRON SINIC X 7 | """ 8 | 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | import rvo2 12 | 13 | 14 | class Agent(): 15 | def __init__(self, px, py, gx, gy, color): 16 | self.px = px 17 | self.py = py 18 | self.gx = gx 19 | self.gy = gy 20 | self.vx = 0 21 | self.vy = 0 22 | self.v_pref = 1.0 23 | self.color = color 24 | 25 | 26 | class ERVO(): 27 | def __init__(self): 28 | self.name = 'ERVO' 29 | self.safety_space = 0 30 | self.neighbor_dist = 10 31 | self.max_neighbors = 10 32 | self.time_horizon = 5 33 | self.time_horizon_obst = 5 34 | self.radius = 0.3 35 | self.max_speed = 1 36 | self.time_step = 0.25 37 | self.sim = None 38 | 39 | self.agents = [ 40 | Agent(-3, -2.5, 3, 3, 'slategray'), 41 | Agent(0, -3.7, 0, 3, 'coral'), 42 | Agent(3, -3, -3, 3, 'slategray'), 43 | ] 44 | 45 | def step(self, beep=-1): 46 | if self.sim is None: 47 | params = self.neighbor_dist, self.max_neighbors, self.time_horizon, self.time_horizon_obst 48 | self.sim = rvo2.PyERVOSimulator( 49 | self.time_step, *params, self.radius, self.max_speed) 50 | for idx, agent in enumerate(self.agents): 51 | self.sim.addAgent((agent.px, agent.py), *params, 52 | self.radius, self.max_speed, (agent.vx, agent.vy)) 53 | 54 | for idx, agent in enumerate(self.agents): 55 | velocity = np.array((agent.gx - agent.px, agent.gy - agent.py)) 56 | speed = np.linalg.norm(velocity) 57 | pref_vel = velocity / speed if speed > 1 else velocity 58 | self.sim.setAgentPrefVelocity(idx, tuple(pref_vel)) 59 | 60 | self.sim.doStep((self.agents[1].px, self.agents[1].py), beep) 61 | 62 | for idx, agent in enumerate(self.agents): 63 | agent.px, agent.py = self.sim.getAgentPosition(idx) 64 | agent.vx, agent.vy = self.sim.getAgentVelocity(idx) 65 | 66 | 67 | def main(): 68 | sim = ERVO() 69 | plt.figure(figsize=(2, 2)) 70 | 71 | for i in range(200): 72 | if i > 13 and i < 16: 73 | sim.step(beep=1) 74 | plt.scatter(sim.agents[1].px, sim.agents[1].py, 75 | s=500, marker='o', color=sim.agents[1].color, facecolor='none') 76 | else: 77 | sim.step(beep=-1) 78 | for agent in sim.agents: 79 | plt.scatter(agent.px, agent.py, color=agent.color) 80 | 81 | plt.xlim([-4.0, 4.0]) 82 | plt.ylim([-4.0, 4.0]) 83 | plt.waitforbuttonpress() 84 | plt.savefig(f"img/{i:02}.png") 85 | plt.clf() 86 | 87 | 88 | if __name__ == '__main__': 89 | main() 90 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | Cython==0.21.1 2 | -------------------------------------------------------------------------------- /rvo2-navmesh-test.blend: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/denkiwakame/Python-ERVO/5386c8b55dd83a3de371c7859bb70b7a7ed207be/rvo2-navmesh-test.blend -------------------------------------------------------------------------------- /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/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 | void computeNewVelocityForERVO(const Vector2 &ps, const float rs); 66 | 67 | /** 68 | * \brief Inserts an agent neighbor into the set of neighbors of 69 | * this agent. 70 | * \param agent A pointer to the agent to be inserted. 71 | * \param rangeSq The squared range around this agent. 72 | */ 73 | void insertAgentNeighbor(const Agent *agent, float &rangeSq); 74 | 75 | /** 76 | * \brief Inserts a static obstacle neighbor into the set of neighbors 77 | * of this agent. 78 | * \param obstacle The number of the static obstacle to be 79 | * inserted. 80 | * \param rangeSq The squared range around this agent. 81 | */ 82 | void insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq); 83 | 84 | /** 85 | * \brief Updates the two-dimensional position and two-dimensional 86 | * velocity of this agent. 87 | */ 88 | void update(); 89 | 90 | std::vector > agentNeighbors_; 91 | size_t maxNeighbors_; 92 | float maxSpeed_; 93 | float neighborDist_; 94 | Vector2 newVelocity_; 95 | std::vector > obstacleNeighbors_; 96 | std::vector orcaLines_; 97 | Vector2 position_; 98 | Vector2 prefVelocity_; 99 | float radius_; 100 | RVOSimulator *sim_; 101 | float timeHorizon_; 102 | float timeHorizonObst_; 103 | Vector2 velocity_; 104 | 105 | size_t id_; 106 | 107 | friend class KdTree; 108 | friend class RVOSimulator; // TODO: create SimulatorBase 109 | friend class ERVOSimulator; 110 | }; 111 | 112 | /** 113 | * \relates Agent 114 | * \brief Solves a one-dimensional linear program on a specified line 115 | * subject to linear constraints defined by lines and a circular 116 | * constraint. 117 | * \param lines Lines defining the linear constraints. 118 | * \param lineNo The specified line constraint. 119 | * \param radius The radius of the circular constraint. 120 | * \param optVelocity The optimization velocity. 121 | * \param directionOpt True if the direction should be optimized. 122 | * \param result A reference to the result of the linear program. 123 | * \return True if successful. 124 | */ 125 | bool linearProgram1(const std::vector &lines, size_t lineNo, float radius, 126 | const Vector2 &optVelocity, bool directionOpt, 127 | Vector2 &result); 128 | 129 | /** 130 | * \relates Agent 131 | * \brief Solves a two-dimensional linear program subject to linear 132 | * constraints defined by lines and a circular constraint. 133 | * \param lines Lines defining the linear constraints. 134 | * \param radius The radius of the circular constraint. 135 | * \param optVelocity The optimization velocity. 136 | * \param directionOpt True if the direction should be optimized. 137 | * \param result A reference to the result of the linear program. 138 | * \return The number of the line it fails on, and the number of lines if 139 | * successful. 140 | */ 141 | size_t linearProgram2(const std::vector &lines, float radius, 142 | const Vector2 &optVelocity, bool directionOpt, 143 | Vector2 &result); 144 | 145 | /** 146 | * \relates Agent 147 | * \brief Solves a two-dimensional linear program subject to linear 148 | * constraints defined by lines and a circular constraint. 149 | * \param lines Lines defining the linear constraints. 150 | * \param numObstLines Count of obstacle lines. 151 | * \param beginLine The line on which the 2-d linear program failed. 152 | * \param radius The radius of the circular constraint. 153 | * \param result A reference to the result of the linear program. 154 | */ 155 | void linearProgram3(const std::vector &lines, size_t numObstLines, 156 | size_t beginLine, float radius, Vector2 &result); 157 | } // namespace RVO 158 | 159 | #endif /* RVO_AGENT_H_ */ 160 | -------------------------------------------------------------------------------- /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 | ERVOSimulator.h 37 | Vector2.h) 38 | 39 | set(RVO_SOURCES 40 | Agent.cpp 41 | Agent.h 42 | Definitions.h 43 | KdTree.cpp 44 | KdTree.h 45 | Obstacle.cpp 46 | Obstacle.h 47 | RVOSimulator.cpp 48 | ERVOSimulator.cpp) 49 | 50 | add_library(RVO ${RVO_HEADERS} ${RVO_SOURCES}) 51 | 52 | if(WIN32) 53 | set_target_properties(RVO PROPERTIES COMPILE_DEFINITIONS NOMINMAX) 54 | endif() 55 | 56 | install(FILES ${RVO_HEADERS} DESTINATION include) 57 | install(TARGETS RVO DESTINATION lib) 58 | -------------------------------------------------------------------------------- /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 | const float r = ((c - a) * (b - a)) / absSq(b - a); 71 | 72 | if (r < 0.0f) { 73 | return absSq(c - a); 74 | } else if (r > 1.0f) { 75 | return absSq(c - b); 76 | } else { 77 | return absSq(c - (a + r * (b - a))); 78 | } 79 | } 80 | 81 | /** 82 | * \brief Computes the signed distance from a line connecting the 83 | * specified points to a specified point. 84 | * \param a The first point on the line. 85 | * \param b The second point on the line. 86 | * \param c The point to which the signed distance is to 87 | * be calculated. 88 | * \return Positive when the point c lies to the left of the line ab. 89 | */ 90 | inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c) { 91 | return det(a - c, b - a); 92 | } 93 | 94 | /** 95 | * \brief Computes the square of a float. 96 | * \param a The float to be squared. 97 | * \return The square of the float. 98 | */ 99 | inline float sqr(float a) { return a * a; } 100 | } // namespace RVO 101 | 102 | #endif /* RVO_DEFINITIONS_H_ */ 103 | -------------------------------------------------------------------------------- /src/ERVOSimulator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Simplified ERVO wrapper 3 | * 4 | * Copyright 2020 OMRON SINIC X 5 | * Mai Nishimura 6 | */ 7 | 8 | #include "ERVOSimulator.h" 9 | 10 | #include "Agent.h" 11 | #include "KdTree.h" 12 | 13 | namespace RVO { 14 | 15 | void ERVOSimulator::doStep(const Vector2 &point, float r) { 16 | kdTree_->buildAgentTree(); 17 | 18 | #ifdef _OPENMP 19 | #pragma omp parallel for 20 | #endif 21 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 22 | agents_[i]->computeNeighbors(); 23 | agents_[i]->computeNewVelocityForERVO(point, r); 24 | } 25 | 26 | #ifdef _OPENMP 27 | #pragma omp parallel for 28 | #endif 29 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 30 | agents_[i]->update(); 31 | } 32 | 33 | globalTime_ += timeStep_; 34 | } 35 | 36 | } // namespace RVO 37 | -------------------------------------------------------------------------------- /src/ERVOSimulator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simplified ERVO wrapper 3 | * 4 | * Copyright 2020 OMRON SINIC X 5 | * Mai Nishimura 6 | */ 7 | 8 | #include "RVOSimulator.h" 9 | 10 | namespace RVO { 11 | 12 | class ERVOSimulator : public RVOSimulator { 13 | public: 14 | using RVOSimulator::RVOSimulator; // inherit constructor 15 | void doStep(const Vector2 &point, float r); 16 | }; 17 | 18 | } // namespace RVO 19 | 20 | -------------------------------------------------------------------------------- /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 "Obstacle.h" 37 | #include "RVOSimulator.h" 38 | 39 | namespace RVO { 40 | KdTree::KdTree(RVOSimulator *sim) : obstacleTree_(NULL), sim_(sim) {} 41 | 42 | KdTree::~KdTree() { deleteObstacleTree(obstacleTree_); } 43 | 44 | void KdTree::buildAgentTree() { 45 | if (agents_.size() < sim_->agents_.size()) { 46 | for (size_t i = agents_.size(); i < sim_->agents_.size(); ++i) { 47 | agents_.push_back(sim_->agents_[i]); 48 | } 49 | 50 | agentTree_.resize(2 * agents_.size() - 1); 51 | } 52 | 53 | if (!agents_.empty()) { 54 | buildAgentTreeRecursive(0, agents_.size(), 0); 55 | } 56 | } 57 | 58 | void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) { 59 | agentTree_[node].begin = begin; 60 | agentTree_[node].end = end; 61 | agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x(); 62 | agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y(); 63 | 64 | for (size_t i = begin + 1; i < end; ++i) { 65 | agentTree_[node].maxX = 66 | std::max(agentTree_[node].maxX, agents_[i]->position_.x()); 67 | agentTree_[node].minX = 68 | std::min(agentTree_[node].minX, agents_[i]->position_.x()); 69 | agentTree_[node].maxY = 70 | std::max(agentTree_[node].maxY, agents_[i]->position_.y()); 71 | agentTree_[node].minY = 72 | std::min(agentTree_[node].minY, agents_[i]->position_.y()); 73 | } 74 | 75 | if (end - begin > MAX_LEAF_SIZE) { 76 | /* No leaf node. */ 77 | const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > 78 | agentTree_[node].maxY - agentTree_[node].minY); 79 | const float splitValue = 80 | (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) 81 | : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY)); 82 | 83 | size_t left = begin; 84 | size_t right = end; 85 | 86 | while (left < right) { 87 | while (left < right && 88 | (isVertical ? agents_[left]->position_.x() 89 | : agents_[left]->position_.y()) < splitValue) { 90 | ++left; 91 | } 92 | 93 | while (right > left && 94 | (isVertical ? agents_[right - 1]->position_.x() 95 | : agents_[right - 1]->position_.y()) >= splitValue) { 96 | --right; 97 | } 98 | 99 | if (left < right) { 100 | std::swap(agents_[left], agents_[right - 1]); 101 | ++left; 102 | --right; 103 | } 104 | } 105 | 106 | if (left == begin) { 107 | ++left; 108 | ++right; 109 | } 110 | 111 | agentTree_[node].left = node + 1; 112 | agentTree_[node].right = node + 2 * (left - begin); 113 | 114 | buildAgentTreeRecursive(begin, left, agentTree_[node].left); 115 | buildAgentTreeRecursive(left, end, agentTree_[node].right); 116 | } 117 | } 118 | 119 | void KdTree::buildObstacleTree() { 120 | deleteObstacleTree(obstacleTree_); 121 | 122 | std::vector obstacles(sim_->obstacles_.size()); 123 | 124 | for (size_t i = 0; i < sim_->obstacles_.size(); ++i) { 125 | obstacles[i] = sim_->obstacles_[i]; 126 | } 127 | 128 | obstacleTree_ = buildObstacleTreeRecursive(obstacles); 129 | } 130 | 131 | KdTree::ObstacleTreeNode *KdTree::buildObstacleTreeRecursive( 132 | const std::vector &obstacles) { 133 | if (obstacles.empty()) { 134 | return NULL; 135 | } else { 136 | ObstacleTreeNode *const node = new ObstacleTreeNode; 137 | 138 | size_t optimalSplit = 0; 139 | size_t minLeft = obstacles.size(); 140 | size_t minRight = obstacles.size(); 141 | 142 | for (size_t i = 0; i < obstacles.size(); ++i) { 143 | size_t leftSize = 0; 144 | size_t rightSize = 0; 145 | 146 | const Obstacle *const obstacleI1 = obstacles[i]; 147 | const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; 148 | 149 | /* Compute optimal split node. */ 150 | for (size_t j = 0; j < obstacles.size(); ++j) { 151 | if (i == j) { 152 | continue; 153 | } 154 | 155 | const Obstacle *const obstacleJ1 = obstacles[j]; 156 | const Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; 157 | 158 | const float j1LeftOfI = 159 | leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); 160 | const float j2LeftOfI = 161 | leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); 162 | 163 | if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { 164 | ++leftSize; 165 | } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { 166 | ++rightSize; 167 | } else { 168 | ++leftSize; 169 | ++rightSize; 170 | } 171 | 172 | if (std::make_pair(std::max(leftSize, rightSize), 173 | std::min(leftSize, rightSize)) >= 174 | std::make_pair(std::max(minLeft, minRight), 175 | std::min(minLeft, minRight))) { 176 | break; 177 | } 178 | } 179 | 180 | if (std::make_pair(std::max(leftSize, rightSize), 181 | std::min(leftSize, rightSize)) < 182 | std::make_pair(std::max(minLeft, minRight), 183 | std::min(minLeft, minRight))) { 184 | minLeft = leftSize; 185 | minRight = rightSize; 186 | optimalSplit = i; 187 | } 188 | } 189 | 190 | /* Build split node. */ 191 | std::vector leftObstacles(minLeft); 192 | std::vector rightObstacles(minRight); 193 | 194 | size_t leftCounter = 0; 195 | size_t rightCounter = 0; 196 | const size_t i = optimalSplit; 197 | 198 | const Obstacle *const obstacleI1 = obstacles[i]; 199 | const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; 200 | 201 | for (size_t j = 0; j < obstacles.size(); ++j) { 202 | if (i == j) { 203 | continue; 204 | } 205 | 206 | Obstacle *const obstacleJ1 = obstacles[j]; 207 | Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; 208 | 209 | const float j1LeftOfI = 210 | leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); 211 | const float j2LeftOfI = 212 | leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); 213 | 214 | if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { 215 | leftObstacles[leftCounter++] = obstacles[j]; 216 | } else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { 217 | rightObstacles[rightCounter++] = obstacles[j]; 218 | } else { 219 | /* Split obstacle j. */ 220 | const float t = det(obstacleI2->point_ - obstacleI1->point_, 221 | obstacleJ1->point_ - obstacleI1->point_) / 222 | det(obstacleI2->point_ - obstacleI1->point_, 223 | obstacleJ1->point_ - obstacleJ2->point_); 224 | 225 | const Vector2 splitpoint = 226 | obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); 227 | 228 | Obstacle *const newObstacle = new Obstacle(); 229 | newObstacle->point_ = splitpoint; 230 | newObstacle->prevObstacle_ = obstacleJ1; 231 | newObstacle->nextObstacle_ = obstacleJ2; 232 | newObstacle->isConvex_ = true; 233 | newObstacle->unitDir_ = obstacleJ1->unitDir_; 234 | 235 | newObstacle->id_ = sim_->obstacles_.size(); 236 | 237 | sim_->obstacles_.push_back(newObstacle); 238 | 239 | obstacleJ1->nextObstacle_ = newObstacle; 240 | obstacleJ2->prevObstacle_ = newObstacle; 241 | 242 | if (j1LeftOfI > 0.0f) { 243 | leftObstacles[leftCounter++] = obstacleJ1; 244 | rightObstacles[rightCounter++] = newObstacle; 245 | } else { 246 | rightObstacles[rightCounter++] = obstacleJ1; 247 | leftObstacles[leftCounter++] = newObstacle; 248 | } 249 | } 250 | } 251 | 252 | node->obstacle = obstacleI1; 253 | node->left = buildObstacleTreeRecursive(leftObstacles); 254 | node->right = buildObstacleTreeRecursive(rightObstacles); 255 | return node; 256 | } 257 | } 258 | 259 | void KdTree::computeAgentNeighbors(Agent *agent, float &rangeSq) const { 260 | queryAgentTreeRecursive(agent, rangeSq, 0); 261 | } 262 | 263 | void KdTree::computeObstacleNeighbors(Agent *agent, float rangeSq) const { 264 | queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); 265 | } 266 | 267 | void KdTree::deleteObstacleTree(ObstacleTreeNode *node) { 268 | if (node != NULL) { 269 | deleteObstacleTree(node->left); 270 | deleteObstacleTree(node->right); 271 | delete node; 272 | } 273 | } 274 | 275 | void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, 276 | size_t node) const { 277 | if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) { 278 | for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) { 279 | agent->insertAgentNeighbor(agents_[i], rangeSq); 280 | } 281 | } else { 282 | const float distSqLeft = 283 | sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minX - 284 | agent->position_.x())) + 285 | sqr(std::max(0.0f, agent->position_.x() - 286 | agentTree_[agentTree_[node].left].maxX)) + 287 | sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - 288 | agent->position_.y())) + 289 | sqr(std::max(0.0f, agent->position_.y() - 290 | agentTree_[agentTree_[node].left].maxY)); 291 | 292 | const float distSqRight = 293 | sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - 294 | agent->position_.x())) + 295 | sqr(std::max(0.0f, agent->position_.x() - 296 | agentTree_[agentTree_[node].right].maxX)) + 297 | sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - 298 | agent->position_.y())) + 299 | sqr(std::max(0.0f, agent->position_.y() - 300 | agentTree_[agentTree_[node].right].maxY)); 301 | 302 | if (distSqLeft < distSqRight) { 303 | if (distSqLeft < rangeSq) { 304 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); 305 | 306 | if (distSqRight < rangeSq) { 307 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); 308 | } 309 | } 310 | } else { 311 | if (distSqRight < rangeSq) { 312 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); 313 | 314 | if (distSqLeft < rangeSq) { 315 | queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); 316 | } 317 | } 318 | } 319 | } 320 | } 321 | 322 | void KdTree::queryObstacleTreeRecursive(Agent *agent, float rangeSq, 323 | const ObstacleTreeNode *node) const { 324 | if (node == NULL) { 325 | return; 326 | } else { 327 | const Obstacle *const obstacle1 = node->obstacle; 328 | const Obstacle *const obstacle2 = obstacle1->nextObstacle_; 329 | 330 | const float agentLeftOfLine = 331 | leftOf(obstacle1->point_, obstacle2->point_, agent->position_); 332 | 333 | queryObstacleTreeRecursive( 334 | agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right)); 335 | 336 | const float distSqLine = 337 | sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_); 338 | 339 | if (distSqLine < rangeSq) { 340 | if (agentLeftOfLine < 0.0f) { 341 | /* 342 | * Try obstacle at this node only if agent is on right side of 343 | * obstacle (and can see obstacle). 344 | */ 345 | agent->insertObstacleNeighbor(node->obstacle, rangeSq); 346 | } 347 | 348 | /* Try other side of line. */ 349 | queryObstacleTreeRecursive( 350 | agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left)); 351 | } 352 | } 353 | } 354 | 355 | bool KdTree::queryVisibility(const Vector2 &q1, const Vector2 &q2, 356 | float radius) const { 357 | return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); 358 | } 359 | 360 | bool KdTree::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, 361 | float radius, 362 | const ObstacleTreeNode *node) const { 363 | if (node == NULL) { 364 | return true; 365 | } else { 366 | const Obstacle *const obstacle1 = node->obstacle; 367 | const Obstacle *const obstacle2 = obstacle1->nextObstacle_; 368 | 369 | const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1); 370 | const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2); 371 | const float invLengthI = 372 | 1.0f / absSq(obstacle2->point_ - obstacle1->point_); 373 | 374 | if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { 375 | return queryVisibilityRecursive(q1, q2, radius, node->left) && 376 | ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && 377 | sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || 378 | queryVisibilityRecursive(q1, q2, radius, node->right)); 379 | } else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { 380 | return queryVisibilityRecursive(q1, q2, radius, node->right) && 381 | ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && 382 | sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || 383 | queryVisibilityRecursive(q1, q2, radius, node->left)); 384 | } else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { 385 | /* One can see through obstacle from left to right. */ 386 | return queryVisibilityRecursive(q1, q2, radius, node->left) && 387 | queryVisibilityRecursive(q1, q2, radius, node->right); 388 | } else { 389 | const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_); 390 | const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_); 391 | const float invLengthQ = 1.0f / absSq(q2 - q1); 392 | 393 | return (point1LeftOfQ * point2LeftOfQ >= 0.0f && 394 | sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && 395 | sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && 396 | queryVisibilityRecursive(q1, q2, radius, node->left) && 397 | queryVisibilityRecursive(q1, q2, radius, node->right)); 398 | } 399 | } 400 | } 401 | } // namespace RVO 402 | -------------------------------------------------------------------------------- /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( 141 | const std::vector &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, size_t node) const; 167 | 168 | void queryObstacleTreeRecursive(Agent *agent, float rangeSq, 169 | const ObstacleTreeNode *node) const; 170 | 171 | /** 172 | * \brief Queries the visibility between two points within a 173 | * specified radius. 174 | * \param q1 The first point between which visibility is 175 | * to be tested. 176 | * \param q2 The second point between which visibility is 177 | * to be tested. 178 | * \param radius The radius within which visibility is to be 179 | * tested. 180 | * \return True if q1 and q2 are mutually visible within the radius; 181 | * false otherwise. 182 | */ 183 | bool queryVisibility(const Vector2 &q1, const Vector2 &q2, 184 | float radius) const; 185 | 186 | bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, 187 | float radius, 188 | const ObstacleTreeNode *node) const; 189 | 190 | std::vector agents_; 191 | std::vector agentTree_; 192 | ObstacleTreeNode *obstacleTree_; 193 | RVOSimulator *sim_; 194 | 195 | static const size_t MAX_LEAF_SIZE = 10; 196 | 197 | friend class Agent; 198 | friend class RVOSimulator; 199 | friend class ERVOSimulator; // TODO: SimulatorBase 200 | }; 201 | } // namespace RVO 202 | 203 | #endif /* RVO_KD_TREE_H_ */ 204 | -------------------------------------------------------------------------------- /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() 38 | : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) {} 39 | } // namespace RVO 40 | -------------------------------------------------------------------------------- /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 | } // namespace RVO 67 | 68 | #endif /* RVO_OBSTACLE_H_ */ 69 | -------------------------------------------------------------------------------- /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 |
BlocksA 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.
CircleA 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.
RoadmapThe 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 |
ParameterType (unit)Meaning
timeStepfloat (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 |
ParameterType (unit)Meaning
maxNeighborssize_tThe 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.
maxSpeedfloat (distance/time)The maximum speed of the agent. Must be non-negative.
neighborDistfloat (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.
positionRVO::Vector2 (distance, distance)The current position of the agent.
prefVelocityRVO::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.
radiusfloat (distance)The radius of the agent. Must be non-negative.
timeHorizonfloat (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.
timeHorizonObstfloat (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.
velocityRVO::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.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() 45 | : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f) { 46 | kdTree_ = new KdTree(this); 47 | } 48 | 49 | RVOSimulator::RVOSimulator(float timeStep, float neighborDist, 50 | size_t maxNeighbors, float timeHorizon, 51 | float timeHorizonObst, float radius, float maxSpeed, 52 | const Vector2 &velocity) 53 | : defaultAgent_(NULL), 54 | globalTime_(0.0f), 55 | kdTree_(NULL), 56 | timeStep_(timeStep) { 57 | kdTree_ = new KdTree(this); 58 | defaultAgent_ = new Agent(this); 59 | 60 | defaultAgent_->maxNeighbors_ = maxNeighbors; 61 | defaultAgent_->maxSpeed_ = maxSpeed; 62 | defaultAgent_->neighborDist_ = neighborDist; 63 | defaultAgent_->radius_ = radius; 64 | defaultAgent_->timeHorizon_ = timeHorizon; 65 | defaultAgent_->timeHorizonObst_ = timeHorizonObst; 66 | defaultAgent_->velocity_ = velocity; 67 | } 68 | 69 | RVOSimulator::~RVOSimulator() { 70 | if (defaultAgent_ != NULL) { 71 | delete defaultAgent_; 72 | } 73 | 74 | for (size_t i = 0; i < agents_.size(); ++i) { 75 | delete agents_[i]; 76 | } 77 | 78 | for (size_t i = 0; i < obstacles_.size(); ++i) { 79 | delete obstacles_[i]; 80 | } 81 | 82 | delete kdTree_; 83 | } 84 | 85 | size_t RVOSimulator::addAgent(const Vector2 &position) { 86 | if (defaultAgent_ == NULL) { 87 | return RVO_ERROR; 88 | } 89 | 90 | Agent *agent = new Agent(this); 91 | 92 | agent->position_ = position; 93 | agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; 94 | agent->maxSpeed_ = defaultAgent_->maxSpeed_; 95 | agent->neighborDist_ = defaultAgent_->neighborDist_; 96 | agent->radius_ = defaultAgent_->radius_; 97 | agent->timeHorizon_ = defaultAgent_->timeHorizon_; 98 | agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_; 99 | agent->velocity_ = defaultAgent_->velocity_; 100 | 101 | agent->id_ = agents_.size(); 102 | 103 | agents_.push_back(agent); 104 | 105 | return agents_.size() - 1; 106 | } 107 | 108 | size_t RVOSimulator::addAgent(const Vector2 &position, float neighborDist, 109 | size_t maxNeighbors, float timeHorizon, 110 | float timeHorizonObst, float radius, 111 | float maxSpeed, const Vector2 &velocity) { 112 | Agent *agent = new Agent(this); 113 | 114 | agent->position_ = position; 115 | agent->maxNeighbors_ = maxNeighbors; 116 | agent->maxSpeed_ = maxSpeed; 117 | agent->neighborDist_ = neighborDist; 118 | agent->radius_ = radius; 119 | agent->timeHorizon_ = timeHorizon; 120 | agent->timeHorizonObst_ = timeHorizonObst; 121 | agent->velocity_ = velocity; 122 | 123 | agent->id_ = agents_.size(); 124 | 125 | agents_.push_back(agent); 126 | 127 | return agents_.size() - 1; 128 | } 129 | 130 | size_t RVOSimulator::addObstacle(const std::vector &vertices) { 131 | if (vertices.size() < 2) { 132 | return RVO_ERROR; 133 | } 134 | 135 | const size_t obstacleNo = obstacles_.size(); 136 | 137 | for (size_t i = 0; i < vertices.size(); ++i) { 138 | Obstacle *obstacle = new Obstacle(); 139 | obstacle->point_ = vertices[i]; 140 | 141 | if (i != 0) { 142 | obstacle->prevObstacle_ = obstacles_.back(); 143 | obstacle->prevObstacle_->nextObstacle_ = obstacle; 144 | } 145 | 146 | if (i == vertices.size() - 1) { 147 | obstacle->nextObstacle_ = obstacles_[obstacleNo]; 148 | obstacle->nextObstacle_->prevObstacle_ = obstacle; 149 | } 150 | 151 | obstacle->unitDir_ = normalize( 152 | vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]); 153 | 154 | if (vertices.size() == 2) { 155 | obstacle->isConvex_ = true; 156 | } else { 157 | obstacle->isConvex_ = 158 | (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], 159 | vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f); 160 | } 161 | 162 | obstacle->id_ = obstacles_.size(); 163 | 164 | obstacles_.push_back(obstacle); 165 | } 166 | 167 | return obstacleNo; 168 | } 169 | 170 | void RVOSimulator::doStep() { 171 | kdTree_->buildAgentTree(); 172 | 173 | #ifdef _OPENMP 174 | #pragma omp parallel for 175 | #endif 176 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 177 | agents_[i]->computeNeighbors(); 178 | agents_[i]->computeNewVelocity(); 179 | } 180 | 181 | #ifdef _OPENMP 182 | #pragma omp parallel for 183 | #endif 184 | for (int i = 0; i < static_cast(agents_.size()); ++i) { 185 | agents_[i]->update(); 186 | } 187 | 188 | globalTime_ += timeStep_; 189 | } 190 | 191 | size_t RVOSimulator::getAgentAgentNeighbor(size_t agentNo, 192 | size_t neighborNo) const { 193 | return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_; 194 | } 195 | 196 | size_t RVOSimulator::getAgentMaxNeighbors(size_t agentNo) const { 197 | return agents_[agentNo]->maxNeighbors_; 198 | } 199 | 200 | float RVOSimulator::getAgentMaxSpeed(size_t agentNo) const { 201 | return agents_[agentNo]->maxSpeed_; 202 | } 203 | 204 | float RVOSimulator::getAgentNeighborDist(size_t agentNo) const { 205 | return agents_[agentNo]->neighborDist_; 206 | } 207 | 208 | size_t RVOSimulator::getAgentNumAgentNeighbors(size_t agentNo) const { 209 | return agents_[agentNo]->agentNeighbors_.size(); 210 | } 211 | 212 | size_t RVOSimulator::getAgentNumObstacleNeighbors(size_t agentNo) const { 213 | return agents_[agentNo]->obstacleNeighbors_.size(); 214 | } 215 | 216 | size_t RVOSimulator::getAgentNumORCALines(size_t agentNo) const { 217 | return agents_[agentNo]->orcaLines_.size(); 218 | } 219 | 220 | size_t RVOSimulator::getAgentObstacleNeighbor(size_t agentNo, 221 | size_t neighborNo) const { 222 | return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_; 223 | } 224 | 225 | const Line &RVOSimulator::getAgentORCALine(size_t agentNo, 226 | size_t lineNo) const { 227 | return agents_[agentNo]->orcaLines_[lineNo]; 228 | } 229 | 230 | const Vector2 &RVOSimulator::getAgentPosition(size_t agentNo) const { 231 | return agents_[agentNo]->position_; 232 | } 233 | 234 | const Vector2 &RVOSimulator::getAgentPrefVelocity(size_t agentNo) const { 235 | return agents_[agentNo]->prefVelocity_; 236 | } 237 | 238 | float RVOSimulator::getAgentRadius(size_t agentNo) const { 239 | return agents_[agentNo]->radius_; 240 | } 241 | 242 | float RVOSimulator::getAgentTimeHorizon(size_t agentNo) const { 243 | return agents_[agentNo]->timeHorizon_; 244 | } 245 | 246 | float RVOSimulator::getAgentTimeHorizonObst(size_t agentNo) const { 247 | return agents_[agentNo]->timeHorizonObst_; 248 | } 249 | 250 | const Vector2 &RVOSimulator::getAgentVelocity(size_t agentNo) const { 251 | return agents_[agentNo]->velocity_; 252 | } 253 | 254 | float RVOSimulator::getGlobalTime() const { return globalTime_; } 255 | 256 | size_t RVOSimulator::getNumAgents() const { return agents_.size(); } 257 | 258 | size_t RVOSimulator::getNumObstacleVertices() const { 259 | return obstacles_.size(); 260 | } 261 | 262 | const Vector2 &RVOSimulator::getObstacleVertex(size_t vertexNo) const { 263 | return obstacles_[vertexNo]->point_; 264 | } 265 | 266 | size_t RVOSimulator::getNextObstacleVertexNo(size_t vertexNo) const { 267 | return obstacles_[vertexNo]->nextObstacle_->id_; 268 | } 269 | 270 | size_t RVOSimulator::getPrevObstacleVertexNo(size_t vertexNo) const { 271 | return obstacles_[vertexNo]->prevObstacle_->id_; 272 | } 273 | 274 | float RVOSimulator::getTimeStep() const { return timeStep_; } 275 | 276 | void RVOSimulator::processObstacles() { kdTree_->buildObstacleTree(); } 277 | 278 | bool RVOSimulator::queryVisibility(const Vector2 &point1, const Vector2 &point2, 279 | float radius) const { 280 | return kdTree_->queryVisibility(point1, point2, radius); 281 | } 282 | 283 | void RVOSimulator::setAgentDefaults(float neighborDist, size_t maxNeighbors, 284 | float timeHorizon, float timeHorizonObst, 285 | float radius, float maxSpeed, 286 | const Vector2 &velocity) { 287 | if (defaultAgent_ == NULL) { 288 | defaultAgent_ = new Agent(this); 289 | } 290 | 291 | defaultAgent_->maxNeighbors_ = maxNeighbors; 292 | defaultAgent_->maxSpeed_ = maxSpeed; 293 | defaultAgent_->neighborDist_ = neighborDist; 294 | defaultAgent_->radius_ = radius; 295 | defaultAgent_->timeHorizon_ = timeHorizon; 296 | defaultAgent_->timeHorizonObst_ = timeHorizonObst; 297 | defaultAgent_->velocity_ = velocity; 298 | } 299 | 300 | void RVOSimulator::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) { 301 | agents_[agentNo]->maxNeighbors_ = maxNeighbors; 302 | } 303 | 304 | void RVOSimulator::setAgentMaxSpeed(size_t agentNo, float maxSpeed) { 305 | agents_[agentNo]->maxSpeed_ = maxSpeed; 306 | } 307 | 308 | void RVOSimulator::setAgentNeighborDist(size_t agentNo, float neighborDist) { 309 | agents_[agentNo]->neighborDist_ = neighborDist; 310 | } 311 | 312 | void RVOSimulator::setAgentPosition(size_t agentNo, const Vector2 &position) { 313 | agents_[agentNo]->position_ = position; 314 | } 315 | 316 | void RVOSimulator::setAgentPrefVelocity(size_t agentNo, 317 | const Vector2 &prefVelocity) { 318 | agents_[agentNo]->prefVelocity_ = prefVelocity; 319 | } 320 | 321 | void RVOSimulator::setAgentRadius(size_t agentNo, float radius) { 322 | agents_[agentNo]->radius_ = radius; 323 | } 324 | 325 | void RVOSimulator::setAgentTimeHorizon(size_t agentNo, float timeHorizon) { 326 | agents_[agentNo]->timeHorizon_ = timeHorizon; 327 | } 328 | 329 | void RVOSimulator::setAgentTimeHorizonObst(size_t agentNo, 330 | float timeHorizonObst) { 331 | agents_[agentNo]->timeHorizonObst_ = timeHorizonObst; 332 | } 333 | 334 | void RVOSimulator::setAgentVelocity(size_t agentNo, const Vector2 &velocity) { 335 | agents_[agentNo]->velocity_ = velocity; 336 | } 337 | 338 | void RVOSimulator::setTimeStep(float timeStep) { timeStep_ = timeStep; } 339 | } // namespace RVO 340 | -------------------------------------------------------------------------------- /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, float timeHorizonObst, 191 | 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 | * \brief Returns the count of ORCA constraints used to compute 271 | * the current velocity for the specified agent. 272 | * \param agentNo The number of the agent whose count of ORCA 273 | * constraints is to be retrieved. 274 | * \return The count of ORCA constraints used to compute the current 275 | * velocity for the specified agent. 276 | */ 277 | size_t getAgentNumORCALines(size_t agentNo) const; 278 | 279 | /** 280 | * \brief Returns the specified obstacle neighbor of the specified 281 | * agent. 282 | * \param agentNo The number of the agent whose obstacle 283 | * neighbor is to be retrieved. 284 | * \param neighborNo The number of the obstacle neighbor to be 285 | * retrieved. 286 | * \return The number of the first vertex of the neighboring obstacle 287 | * edge. 288 | */ 289 | size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const; 290 | 291 | /** 292 | * \brief Returns the specified ORCA constraint of the specified 293 | * agent. 294 | * \param agentNo The number of the agent whose ORCA 295 | * constraint is to be retrieved. 296 | * \param lineNo The number of the ORCA constraint to be 297 | * retrieved. 298 | * \return A line representing the specified ORCA constraint. 299 | * \note The halfplane to the left of the line is the region of 300 | * permissible velocities with respect to the specified 301 | * ORCA constraint. 302 | */ 303 | const Line &getAgentORCALine(size_t agentNo, size_t lineNo) const; 304 | 305 | /** 306 | * \brief Returns the two-dimensional position of a specified 307 | * agent. 308 | * \param agentNo The number of the agent whose 309 | * two-dimensional position is to be retrieved. 310 | * \return The present two-dimensional position of the (center of the) 311 | * agent. 312 | */ 313 | const Vector2 &getAgentPosition(size_t agentNo) const; 314 | 315 | /** 316 | * \brief Returns the two-dimensional preferred velocity of a 317 | * specified agent. 318 | * \param agentNo The number of the agent whose 319 | * two-dimensional preferred velocity is to be 320 | * retrieved. 321 | * \return The present two-dimensional preferred velocity of the agent. 322 | */ 323 | const Vector2 &getAgentPrefVelocity(size_t agentNo) const; 324 | 325 | /** 326 | * \brief Returns the radius of a specified agent. 327 | * \param agentNo The number of the agent whose radius is to 328 | * be retrieved. 329 | * \return The present radius of the agent. 330 | */ 331 | float getAgentRadius(size_t agentNo) const; 332 | 333 | /** 334 | * \brief Returns the time horizon of a specified agent. 335 | * \param agentNo The number of the agent whose time horizon 336 | * is to be retrieved. 337 | * \return The present time horizon of the agent. 338 | */ 339 | float getAgentTimeHorizon(size_t agentNo) const; 340 | 341 | /** 342 | * \brief Returns the time horizon with respect to obstacles of a 343 | * specified agent. 344 | * \param agentNo The number of the agent whose time horizon 345 | * with respect to obstacles is to be 346 | * retrieved. 347 | * \return The present time horizon with respect to obstacles of the 348 | * agent. 349 | */ 350 | float getAgentTimeHorizonObst(size_t agentNo) const; 351 | 352 | /** 353 | * \brief Returns the two-dimensional linear velocity of a 354 | * specified agent. 355 | * \param agentNo The number of the agent whose 356 | * two-dimensional linear velocity is to be 357 | * retrieved. 358 | * \return The present two-dimensional linear velocity of the agent. 359 | */ 360 | const Vector2 &getAgentVelocity(size_t agentNo) const; 361 | 362 | /** 363 | * \brief Returns the global time of the simulation. 364 | * \return The present global time of the simulation (zero initially). 365 | */ 366 | float getGlobalTime() const; 367 | 368 | /** 369 | * \brief Returns the count of agents in the simulation. 370 | * \return The count of agents in the simulation. 371 | */ 372 | size_t getNumAgents() const; 373 | 374 | /** 375 | * \brief Returns the count of obstacle vertices in the simulation. 376 | * \return The count of obstacle vertices in the simulation. 377 | */ 378 | size_t getNumObstacleVertices() const; 379 | 380 | /** 381 | * \brief Returns the two-dimensional position of a specified obstacle 382 | * vertex. 383 | * \param vertexNo The number of the obstacle vertex to be 384 | * retrieved. 385 | * \return The two-dimensional position of the specified obstacle 386 | * vertex. 387 | */ 388 | const Vector2 &getObstacleVertex(size_t vertexNo) const; 389 | 390 | /** 391 | * \brief Returns the number of the obstacle vertex succeeding the 392 | * specified obstacle vertex in its polygon. 393 | * \param vertexNo The number of the obstacle vertex whose 394 | * successor is to be retrieved. 395 | * \return The number of the obstacle vertex succeeding the specified 396 | * obstacle vertex in its polygon. 397 | */ 398 | size_t getNextObstacleVertexNo(size_t vertexNo) const; 399 | 400 | /** 401 | * \brief Returns the number of the obstacle vertex preceding the 402 | * specified obstacle vertex in its polygon. 403 | * \param vertexNo The number of the obstacle vertex whose 404 | * predecessor is to be retrieved. 405 | * \return The number of the obstacle vertex preceding the specified 406 | * obstacle vertex in its polygon. 407 | */ 408 | size_t getPrevObstacleVertexNo(size_t vertexNo) const; 409 | 410 | /** 411 | * \brief Returns the time step of the simulation. 412 | * \return The present time step of the simulation. 413 | */ 414 | float getTimeStep() const; 415 | 416 | /** 417 | * \brief Processes the obstacles that have been added so that they 418 | * are accounted for in the simulation. 419 | * \note Obstacles added to the simulation after this function has 420 | * been called are not accounted for in the simulation. 421 | */ 422 | void processObstacles(); 423 | 424 | /** 425 | * \brief Performs a visibility query between the two specified 426 | * points with respect to the obstacles 427 | * \param point1 The first point of the query. 428 | * \param point2 The second point of the query. 429 | * \param radius The minimal distance between the line 430 | * connecting the two points and the obstacles 431 | * in order for the points to be mutually 432 | * visible (optional). Must be non-negative. 433 | * \return A boolean specifying whether the two points are mutually 434 | * visible. Returns true when the obstacles have not been 435 | * processed. 436 | */ 437 | bool queryVisibility(const Vector2 &point1, const Vector2 &point2, 438 | float radius = 0.0f) const; 439 | 440 | /** 441 | * \brief Sets the default properties for any new agent that is 442 | * added. 443 | * \param neighborDist The default maximum distance (center point 444 | * to center point) to other agents a new agent 445 | * takes into account in the navigation. The 446 | * larger this number, the longer he running 447 | * time of the simulation. If the number is too 448 | * low, the simulation will not be safe. 449 | * Must be non-negative. 450 | * \param maxNeighbors The default maximum number of other agents a 451 | * new agent takes into account in the 452 | * navigation. The larger this number, the 453 | * longer the running time of the simulation. 454 | * If the number is too low, the simulation 455 | * will not be safe. 456 | * \param timeHorizon The default minimal amount of time for which 457 | * a new agent's velocities that are computed 458 | * by the simulation are safe with respect to 459 | * other agents. The larger this number, the 460 | * sooner an agent will respond to the presence 461 | * of other agents, but the less freedom the 462 | * agent has in choosing its velocities. 463 | * Must be positive. 464 | * \param timeHorizonObst The default minimal amount of time for which 465 | * a new agent's velocities that are computed 466 | * by the simulation are safe with respect to 467 | * obstacles. The larger this number, the 468 | * sooner an agent will respond to the presence 469 | * of obstacles, but the less freedom the agent 470 | * has in choosing its velocities. 471 | * Must be positive. 472 | * \param radius The default radius of a new agent. 473 | * Must be non-negative. 474 | * \param maxSpeed The default maximum speed of a new agent. 475 | * Must be non-negative. 476 | * \param velocity The default initial two-dimensional linear 477 | * velocity of a new agent (optional). 478 | */ 479 | void setAgentDefaults(float neighborDist, size_t maxNeighbors, 480 | float timeHorizon, float timeHorizonObst, float radius, 481 | float maxSpeed, const Vector2 &velocity = Vector2()); 482 | 483 | /** 484 | * \brief Sets the maximum neighbor count of a specified agent. 485 | * \param agentNo The number of the agent whose maximum 486 | * neighbor count is to be modified. 487 | * \param maxNeighbors The replacement maximum neighbor count. 488 | */ 489 | void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors); 490 | 491 | /** 492 | * \brief Sets the maximum speed of a specified agent. 493 | * \param agentNo The number of the agent whose maximum speed 494 | * is to be modified. 495 | * \param maxSpeed The replacement maximum speed. Must be 496 | * non-negative. 497 | */ 498 | void setAgentMaxSpeed(size_t agentNo, float maxSpeed); 499 | 500 | /** 501 | * \brief Sets the maximum neighbor distance of a specified agent. 502 | * \param agentNo The number of the agent whose maximum 503 | * neighbor distance is to be modified. 504 | * \param neighborDist The replacement maximum neighbor distance. 505 | * Must be non-negative. 506 | */ 507 | void setAgentNeighborDist(size_t agentNo, float neighborDist); 508 | 509 | /** 510 | * \brief Sets the two-dimensional position of a specified agent. 511 | * \param agentNo The number of the agent whose 512 | * two-dimensional position is to be modified. 513 | * \param position The replacement of the two-dimensional 514 | * position. 515 | */ 516 | void setAgentPosition(size_t agentNo, const Vector2 &position); 517 | 518 | /** 519 | * \brief Sets the two-dimensional preferred velocity of a 520 | * specified agent. 521 | * \param agentNo The number of the agent whose 522 | * two-dimensional preferred velocity is to be 523 | * modified. 524 | * \param prefVelocity The replacement of the two-dimensional 525 | * preferred velocity. 526 | */ 527 | void setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity); 528 | 529 | /** 530 | * \brief Sets the radius of a specified agent. 531 | * \param agentNo The number of the agent whose radius is to 532 | * be modified. 533 | * \param radius The replacement radius. 534 | * Must be non-negative. 535 | */ 536 | void setAgentRadius(size_t agentNo, float radius); 537 | 538 | /** 539 | * \brief Sets the time horizon of a specified agent with respect 540 | * to other agents. 541 | * \param agentNo The number of the agent whose time horizon 542 | * is to be modified. 543 | * \param timeHorizon The replacement time horizon with respect 544 | * to other agents. Must be positive. 545 | */ 546 | void setAgentTimeHorizon(size_t agentNo, float timeHorizon); 547 | 548 | /** 549 | * \brief Sets the time horizon of a specified agent with respect 550 | * to obstacles. 551 | * \param agentNo The number of the agent whose time horizon 552 | * with respect to obstacles is to be modified. 553 | * \param timeHorizonObst The replacement time horizon with respect to 554 | * obstacles. Must be positive. 555 | */ 556 | void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst); 557 | 558 | /** 559 | * \brief Sets the two-dimensional linear velocity of a specified 560 | * agent. 561 | * \param agentNo The number of the agent whose 562 | * two-dimensional linear velocity is to be 563 | * modified. 564 | * \param velocity The replacement two-dimensional linear 565 | * velocity. 566 | */ 567 | void setAgentVelocity(size_t agentNo, const Vector2 &velocity); 568 | 569 | /** 570 | * \brief Sets the time step of the simulation. 571 | * \param timeStep The time step of the simulation. 572 | * Must be positive. 573 | */ 574 | void setTimeStep(float timeStep); 575 | 576 | private: 577 | Agent *defaultAgent_; 578 | std::vector obstacles_; 579 | 580 | protected: 581 | std::vector agents_; 582 | KdTree *kdTree_; 583 | float globalTime_; 584 | float timeStep_; 585 | 586 | friend class Agent; 587 | friend class KdTree; 588 | friend class Obstacle; 589 | }; 590 | } // namespace RVO 591 | 592 | #endif /* RVO_RVO_SIMULATOR_H_ */ 593 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | cdef extern from "ERVOSimulator.h" namespace "RVO": 78 | cdef cppclass ERVOSimulator: 79 | ERVOSimulator() 80 | ERVOSimulator(float timeStep, float neighborDist, size_t maxNeighbors, 81 | float timeHorizon, float timeHorizonObst, float radius, 82 | float maxSpeed, const Vector2 & velocity) 83 | size_t addAgent(const Vector2 & position) 84 | size_t addAgent(const Vector2 & position, float neighborDist, 85 | size_t maxNeighbors, float timeHorizon, 86 | float timeHorizonObst, float radius, float maxSpeed, 87 | const Vector2 & velocity) 88 | size_t addObstacle(const vector[Vector2] & vertices) 89 | void doStep(const Vector2& point, float r) nogil 90 | size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const 91 | size_t getAgentMaxNeighbors(size_t agentNo) const 92 | float getAgentMaxSpeed(size_t agentNo) const 93 | float getAgentNeighborDist(size_t agentNo) const 94 | size_t getAgentNumAgentNeighbors(size_t agentNo) const 95 | size_t getAgentNumObstacleNeighbors(size_t agentNo) const 96 | size_t getAgentNumORCALines(size_t agentNo) const 97 | size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const 98 | const Line & getAgentORCALine(size_t agentNo, size_t lineNo) const 99 | const Vector2 & getAgentPosition(size_t agentNo) const 100 | const Vector2 & getAgentPrefVelocity(size_t agentNo) const 101 | float getAgentRadius(size_t agentNo) const 102 | float getAgentTimeHorizon(size_t agentNo) const 103 | float getAgentTimeHorizonObst(size_t agentNo) const 104 | const Vector2 & getAgentVelocity(size_t agentNo) const 105 | float getGlobalTime() const 106 | size_t getNumAgents() const 107 | size_t getNumObstacleVertices() const 108 | const Vector2 & getObstacleVertex(size_t vertexNo) const 109 | size_t getNextObstacleVertexNo(size_t vertexNo) const 110 | size_t getPrevObstacleVertexNo(size_t vertexNo) const 111 | float getTimeStep() const 112 | void processObstacles() nogil 113 | bool queryVisibility(const Vector2 & point1, const Vector2 & point2, 114 | float radius) nogil const 115 | void setAgentDefaults(float neighborDist, size_t maxNeighbors, 116 | float timeHorizon, float timeHorizonObst, 117 | float radius, float maxSpeed, 118 | const Vector2 & velocity) 119 | void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) 120 | void setAgentMaxSpeed(size_t agentNo, float maxSpeed) 121 | void setAgentNeighborDist(size_t agentNo, float neighborDist) 122 | void setAgentPosition(size_t agentNo, const Vector2 & position) 123 | void setAgentPrefVelocity(size_t agentNo, const Vector2 & prefVelocity) 124 | void setAgentRadius(size_t agentNo, float radius) 125 | void setAgentTimeHorizon(size_t agentNo, float timeHorizon) 126 | void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst) 127 | void setAgentVelocity(size_t agentNo, const Vector2 & velocity) 128 | void setTimeStep(float timeStep) 129 | 130 | 131 | cdef class PyRVOSimulator: 132 | cdef RVOSimulator *thisptr 133 | 134 | def __cinit__(self, float timeStep, float neighborDist, size_t maxNeighbors, 135 | float timeHorizon, float timeHorizonObst, float radius, 136 | float maxSpeed, tuple velocity=(0, 0)): 137 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 138 | 139 | self.thisptr = new RVOSimulator(timeStep, neighborDist, maxNeighbors, 140 | timeHorizon, timeHorizonObst, radius, 141 | maxSpeed, c_velocity) 142 | 143 | def addAgent(self, tuple pos, neighborDist=None, 144 | maxNeighbors=None, timeHorizon=None, 145 | timeHorizonObst=None, radius=None, maxSpeed=None, 146 | velocity=None): 147 | cdef Vector2 c_pos = Vector2(pos[0], pos[1]) 148 | cdef Vector2 c_velocity 149 | 150 | if neighborDist is not None and velocity is None: 151 | raise ValueError("Either pass only 'pos', or pass all parameters.") 152 | 153 | if neighborDist is None: 154 | agent_nr = self.thisptr.addAgent(c_pos) 155 | else: 156 | c_velocity = Vector2(velocity[0], velocity[1]) 157 | agent_nr = self.thisptr.addAgent(c_pos, neighborDist, 158 | maxNeighbors, timeHorizon, 159 | timeHorizonObst, radius, maxSpeed, 160 | c_velocity) 161 | 162 | if agent_nr == RVO_ERROR: 163 | raise RuntimeError('Error adding agent to RVO simulation') 164 | 165 | return agent_nr 166 | 167 | def addObstacle(self, list vertices): 168 | cdef vector[Vector2] c_vertices 169 | 170 | for x, y in vertices: 171 | c_vertices.push_back(Vector2(x, y)) 172 | 173 | obstacle_nr = self.thisptr.addObstacle(c_vertices) 174 | if obstacle_nr == RVO_ERROR: 175 | raise RuntimeError('Error adding obstacle to RVO simulation') 176 | return obstacle_nr 177 | 178 | def doStep(self): 179 | with nogil: 180 | self.thisptr.doStep() 181 | 182 | def getAgentAgentNeighbor(self, size_t agent_no, size_t neighbor_no): 183 | return self.thisptr.getAgentAgentNeighbor(agent_no, neighbor_no) 184 | def getAgentMaxNeighbors(self, size_t agent_no): 185 | return self.thisptr.getAgentMaxNeighbors(agent_no) 186 | def getAgentMaxSpeed(self, size_t agent_no): 187 | return self.thisptr.getAgentMaxSpeed(agent_no) 188 | def getAgentNeighborDist(self, size_t agent_no): 189 | return self.thisptr.getAgentNeighborDist(agent_no) 190 | def getAgentNumAgentNeighbors(self, size_t agent_no): 191 | return self.thisptr.getAgentNumAgentNeighbors(agent_no) 192 | def getAgentNumObstacleNeighbors(self, size_t agent_no): 193 | return self.thisptr.getAgentNumObstacleNeighbors(agent_no) 194 | def getAgentNumORCALines(self, size_t agent_no): 195 | return self.thisptr.getAgentNumORCALines(agent_no) 196 | def getAgentObstacleNeighbor(self, size_t agent_no, size_t obstacle_no): 197 | return self.thisptr.getAgentObstacleNeighbor(agent_no, obstacle_no) 198 | def getAgentORCALine(self, size_t agent_no, size_t line_no): 199 | cdef Line line = self.thisptr.getAgentORCALine(agent_no, line_no) 200 | return line.point.x(), line.point.y(), line.direction.x(), line.direction.y() 201 | def getAgentPosition(self, size_t agent_no): 202 | cdef Vector2 pos = self.thisptr.getAgentPosition(agent_no) 203 | return pos.x(), pos.y() 204 | def getAgentPrefVelocity(self, size_t agent_no): 205 | cdef Vector2 velocity = self.thisptr.getAgentPrefVelocity(agent_no) 206 | return velocity.x(), velocity.y() 207 | def getAgentRadius(self, size_t agent_no): 208 | return self.thisptr.getAgentRadius(agent_no) 209 | def getAgentTimeHorizon(self, size_t agent_no): 210 | return self.thisptr.getAgentTimeHorizon(agent_no) 211 | def getAgentTimeHorizonObst(self, size_t agent_no): 212 | return self.thisptr.getAgentTimeHorizonObst(agent_no) 213 | def getAgentVelocity(self, size_t agent_no): 214 | cdef Vector2 velocity = self.thisptr.getAgentVelocity(agent_no) 215 | return velocity.x(), velocity.y() 216 | def getGlobalTime(self): 217 | return self.thisptr.getGlobalTime() 218 | def getNumAgents(self): 219 | return self.thisptr.getNumAgents() 220 | def getNumObstacleVertices(self): 221 | return self.thisptr.getNumObstacleVertices() 222 | def getObstacleVertex(self, size_t vertex_no): 223 | cdef Vector2 pos = self.thisptr.getObstacleVertex(vertex_no) 224 | return pos.x(), pos.y() 225 | def getNextObstacleVertexNo(self, size_t vertex_no): 226 | return self.thisptr.getNextObstacleVertexNo(vertex_no) 227 | def getPrevObstacleVertexNo(self, size_t vertex_no): 228 | return self.thisptr.getPrevObstacleVertexNo(vertex_no) 229 | def getTimeStep(self): 230 | return self.thisptr.getTimeStep() 231 | 232 | def processObstacles(self): 233 | with nogil: 234 | self.thisptr.processObstacles() 235 | 236 | def queryVisibility(self, tuple point1, tuple point2, float radius=0.0): 237 | cdef Vector2 c_point1 = Vector2(point1[0], point1[1]) 238 | cdef Vector2 c_point2 = Vector2(point2[0], point2[1]) 239 | cdef bool visible 240 | 241 | with nogil: 242 | visible = self.thisptr.queryVisibility(c_point1, c_point2, radius) 243 | 244 | return visible 245 | 246 | def setAgentDefaults(self, float neighbor_dist, size_t max_neighbors, float time_horizon, 247 | float time_horizon_obst, float radius, float max_speed, 248 | tuple velocity=(0, 0)): 249 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 250 | self.thisptr.setAgentDefaults(neighbor_dist, max_neighbors, time_horizon, 251 | time_horizon_obst, radius, max_speed, c_velocity) 252 | 253 | def setAgentMaxNeighbors(self, size_t agent_no, size_t max_neighbors): 254 | self.thisptr.setAgentMaxNeighbors(agent_no, max_neighbors) 255 | def setAgentMaxSpeed(self, size_t agent_no, float max_speed): 256 | self.thisptr.setAgentMaxSpeed(agent_no, max_speed) 257 | def setAgentNeighborDist(self, size_t agent_no, float neighbor_dist): 258 | self.thisptr.setAgentNeighborDist(agent_no, neighbor_dist) 259 | def setAgentNeighborDist(self, size_t agent_no, float neighbor_dist): 260 | self.thisptr.setAgentNeighborDist(agent_no, neighbor_dist) 261 | def setAgentPosition(self, size_t agent_no, tuple position): 262 | cdef Vector2 c_pos = Vector2(position[0], position[1]) 263 | self.thisptr.setAgentPosition(agent_no, c_pos) 264 | def setAgentPrefVelocity(self, size_t agent_no, tuple velocity): 265 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 266 | self.thisptr.setAgentPrefVelocity(agent_no, c_velocity) 267 | def setAgentRadius(self, size_t agent_no, float radius): 268 | self.thisptr.setAgentRadius(agent_no, radius) 269 | def setAgentTimeHorizon(self, size_t agent_no, float time_horizon): 270 | self.thisptr.setAgentTimeHorizon(agent_no, time_horizon) 271 | def setAgentTimeHorizonObst(self, size_t agent_no, float timeHorizonObst): 272 | self.thisptr.setAgentTimeHorizonObst(agent_no, timeHorizonObst) 273 | def setAgentVelocity(self, size_t agent_no, tuple velocity): 274 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 275 | self.thisptr.setAgentVelocity(agent_no, c_velocity) 276 | def setTimeStep(self, float time_step): 277 | self.thisptr.setTimeStep(time_step) 278 | 279 | cdef class PyERVOSimulator: 280 | cdef ERVOSimulator *thisptr 281 | 282 | def __cinit__(self, float timeStep, float neighborDist, size_t maxNeighbors, 283 | float timeHorizon, float timeHorizonObst, float radius, 284 | float maxSpeed, tuple velocity=(0, 0)): 285 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 286 | 287 | self.thisptr = new ERVOSimulator(timeStep, neighborDist, maxNeighbors, 288 | timeHorizon, timeHorizonObst, radius, 289 | maxSpeed, c_velocity) 290 | 291 | def addAgent(self, tuple pos, neighborDist=None, 292 | maxNeighbors=None, timeHorizon=None, 293 | timeHorizonObst=None, radius=None, maxSpeed=None, 294 | velocity=None): 295 | cdef Vector2 c_pos = Vector2(pos[0], pos[1]) 296 | cdef Vector2 c_velocity 297 | 298 | if neighborDist is not None and velocity is None: 299 | raise ValueError("Either pass only 'pos', or pass all parameters.") 300 | 301 | if neighborDist is None: 302 | agent_nr = self.thisptr.addAgent(c_pos) 303 | else: 304 | c_velocity = Vector2(velocity[0], velocity[1]) 305 | agent_nr = self.thisptr.addAgent(c_pos, neighborDist, 306 | maxNeighbors, timeHorizon, 307 | timeHorizonObst, radius, maxSpeed, 308 | c_velocity) 309 | 310 | if agent_nr == RVO_ERROR: 311 | raise RuntimeError('Error adding agent to RVO simulation') 312 | 313 | return agent_nr 314 | 315 | def addObstacle(self, list vertices): 316 | cdef vector[Vector2] c_vertices 317 | 318 | for x, y in vertices: 319 | c_vertices.push_back(Vector2(x, y)) 320 | 321 | obstacle_nr = self.thisptr.addObstacle(c_vertices) 322 | if obstacle_nr == RVO_ERROR: 323 | raise RuntimeError('Error adding obstacle to RVO simulation') 324 | return obstacle_nr 325 | 326 | def doStep(self, tuple point, float r): 327 | cdef Vector2 c_point = Vector2(point[0], point[1]) 328 | with nogil: 329 | self.thisptr.doStep(c_point, r) 330 | 331 | def getAgentAgentNeighbor(self, size_t agent_no, size_t neighbor_no): 332 | return self.thisptr.getAgentAgentNeighbor(agent_no, neighbor_no) 333 | def getAgentMaxNeighbors(self, size_t agent_no): 334 | return self.thisptr.getAgentMaxNeighbors(agent_no) 335 | def getAgentMaxSpeed(self, size_t agent_no): 336 | return self.thisptr.getAgentMaxSpeed(agent_no) 337 | def getAgentNeighborDist(self, size_t agent_no): 338 | return self.thisptr.getAgentNeighborDist(agent_no) 339 | def getAgentNumAgentNeighbors(self, size_t agent_no): 340 | return self.thisptr.getAgentNumAgentNeighbors(agent_no) 341 | def getAgentNumObstacleNeighbors(self, size_t agent_no): 342 | return self.thisptr.getAgentNumObstacleNeighbors(agent_no) 343 | def getAgentNumORCALines(self, size_t agent_no): 344 | return self.thisptr.getAgentNumORCALines(agent_no) 345 | def getAgentObstacleNeighbor(self, size_t agent_no, size_t obstacle_no): 346 | return self.thisptr.getAgentObstacleNeighbor(agent_no, obstacle_no) 347 | def getAgentORCALine(self, size_t agent_no, size_t line_no): 348 | cdef Line line = self.thisptr.getAgentORCALine(agent_no, line_no) 349 | return line.point.x(), line.point.y(), line.direction.x(), line.direction.y() 350 | def getAgentPosition(self, size_t agent_no): 351 | cdef Vector2 pos = self.thisptr.getAgentPosition(agent_no) 352 | return pos.x(), pos.y() 353 | def getAgentPrefVelocity(self, size_t agent_no): 354 | cdef Vector2 velocity = self.thisptr.getAgentPrefVelocity(agent_no) 355 | return velocity.x(), velocity.y() 356 | def getAgentRadius(self, size_t agent_no): 357 | return self.thisptr.getAgentRadius(agent_no) 358 | def getAgentTimeHorizon(self, size_t agent_no): 359 | return self.thisptr.getAgentTimeHorizon(agent_no) 360 | def getAgentTimeHorizonObst(self, size_t agent_no): 361 | return self.thisptr.getAgentTimeHorizonObst(agent_no) 362 | def getAgentVelocity(self, size_t agent_no): 363 | cdef Vector2 velocity = self.thisptr.getAgentVelocity(agent_no) 364 | return velocity.x(), velocity.y() 365 | def getGlobalTime(self): 366 | return self.thisptr.getGlobalTime() 367 | def getNumAgents(self): 368 | return self.thisptr.getNumAgents() 369 | def getNumObstacleVertices(self): 370 | return self.thisptr.getNumObstacleVertices() 371 | def getObstacleVertex(self, size_t vertex_no): 372 | cdef Vector2 pos = self.thisptr.getObstacleVertex(vertex_no) 373 | return pos.x(), pos.y() 374 | def getNextObstacleVertexNo(self, size_t vertex_no): 375 | return self.thisptr.getNextObstacleVertexNo(vertex_no) 376 | def getPrevObstacleVertexNo(self, size_t vertex_no): 377 | return self.thisptr.getPrevObstacleVertexNo(vertex_no) 378 | def getTimeStep(self): 379 | return self.thisptr.getTimeStep() 380 | 381 | def processObstacles(self): 382 | with nogil: 383 | self.thisptr.processObstacles() 384 | 385 | def queryVisibility(self, tuple point1, tuple point2, float radius=0.0): 386 | cdef Vector2 c_point1 = Vector2(point1[0], point1[1]) 387 | cdef Vector2 c_point2 = Vector2(point2[0], point2[1]) 388 | cdef bool visible 389 | 390 | with nogil: 391 | visible = self.thisptr.queryVisibility(c_point1, c_point2, radius) 392 | 393 | return visible 394 | 395 | def setAgentDefaults(self, float neighbor_dist, size_t max_neighbors, float time_horizon, 396 | float time_horizon_obst, float radius, float max_speed, 397 | tuple velocity=(0, 0)): 398 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 399 | self.thisptr.setAgentDefaults(neighbor_dist, max_neighbors, time_horizon, 400 | time_horizon_obst, radius, max_speed, c_velocity) 401 | 402 | def setAgentMaxNeighbors(self, size_t agent_no, size_t max_neighbors): 403 | self.thisptr.setAgentMaxNeighbors(agent_no, max_neighbors) 404 | def setAgentMaxSpeed(self, size_t agent_no, float max_speed): 405 | self.thisptr.setAgentMaxSpeed(agent_no, max_speed) 406 | def setAgentNeighborDist(self, size_t agent_no, float neighbor_dist): 407 | self.thisptr.setAgentNeighborDist(agent_no, neighbor_dist) 408 | def setAgentNeighborDist(self, size_t agent_no, float neighbor_dist): 409 | self.thisptr.setAgentNeighborDist(agent_no, neighbor_dist) 410 | def setAgentPosition(self, size_t agent_no, tuple position): 411 | cdef Vector2 c_pos = Vector2(position[0], position[1]) 412 | self.thisptr.setAgentPosition(agent_no, c_pos) 413 | def setAgentPrefVelocity(self, size_t agent_no, tuple velocity): 414 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 415 | self.thisptr.setAgentPrefVelocity(agent_no, c_velocity) 416 | def setAgentRadius(self, size_t agent_no, float radius): 417 | self.thisptr.setAgentRadius(agent_no, radius) 418 | def setAgentTimeHorizon(self, size_t agent_no, float time_horizon): 419 | self.thisptr.setAgentTimeHorizon(agent_no, time_horizon) 420 | def setAgentTimeHorizonObst(self, size_t agent_no, float timeHorizonObst): 421 | self.thisptr.setAgentTimeHorizonObst(agent_no, timeHorizonObst) 422 | def setAgentVelocity(self, size_t agent_no, tuple velocity): 423 | cdef Vector2 c_velocity = Vector2(velocity[0], velocity[1]) 424 | self.thisptr.setAgentVelocity(agent_no, c_velocity) 425 | def setTimeStep(self, float time_step): 426 | self.thisptr.setTimeStep(time_step) 427 | --------------------------------------------------------------------------------