From f8ed86ef28a973d14ad6d115af00ce166e7fa80e Mon Sep 17 00:00:00 2001 From: Jamie Snape Date: Mon, 24 Jun 2013 20:11:32 -0700 Subject: [PATCH] Initial commit --- .gitignore | 13 + .travis.yml | 10 + CMakeLists.txt | 65 +++ COPYING | 35 ++ README.md | 54 ++ doc/Doxyfile | 304 ++++++++++ doc/footer.html | 7 + doc/header.html | 21 + doc/stylesheet.css | 1185 +++++++++++++++++++++++++++++++++++++++ examples/Blocks.cpp | 237 ++++++++ examples/CMakeLists.txt | 71 +++ examples/Circle.cpp | 174 ++++++ examples/Roadmap.cpp | 380 +++++++++++++ src/Agent.cpp | 590 +++++++++++++++++++ src/Agent.h | 180 ++++++ src/CMakeLists.txt | 75 +++ src/Definitions.h | 132 +++++ src/KdTree.cpp | 392 +++++++++++++ src/KdTree.h | 227 ++++++++ src/Obstacle.cpp | 62 ++ src/Obstacle.h | 92 +++ src/RVO.h | 557 ++++++++++++++++++ src/RVOSimulator.cpp | 393 +++++++++++++ src/RVOSimulator.h | 616 ++++++++++++++++++++ src/Vector2.h | 356 ++++++++++++ 25 files changed, 6228 insertions(+) create mode 100644 .gitignore create mode 100644 .travis.yml create mode 100644 CMakeLists.txt create mode 100644 COPYING create mode 100644 README.md create mode 100644 doc/Doxyfile create mode 100644 doc/footer.html create mode 100644 doc/header.html create mode 100644 doc/stylesheet.css create mode 100644 examples/Blocks.cpp create mode 100644 examples/CMakeLists.txt create mode 100644 examples/Circle.cpp create mode 100644 examples/Roadmap.cpp create mode 100644 src/Agent.cpp create mode 100644 src/Agent.h create mode 100644 src/CMakeLists.txt create mode 100644 src/Definitions.h create mode 100644 src/KdTree.cpp create mode 100644 src/KdTree.h create mode 100644 src/Obstacle.cpp create mode 100644 src/Obstacle.h create mode 100644 src/RVO.h create mode 100644 src/RVOSimulator.cpp create mode 100644 src/RVOSimulator.h create mode 100644 src/Vector2.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..620d3dc --- /dev/null +++ b/.gitignore @@ -0,0 +1,13 @@ +# Compiled Object files +*.slo +*.lo +*.o + +# Compiled Dynamic libraries +*.so +*.dylib + +# Compiled Static libraries +*.lai +*.la +*.a diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..35a02b7 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,10 @@ +language: cpp +compiler: + - gcc + - clang +script: + - mkdir _build + - cd _build + - cmake .. -DCMAKE_INSTALL_PREFIX=../_install + - cmake --build . --target install + - ctest -V . diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bd8036d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,65 @@ +# +# CMakeLists.txt +# RVO2 Library +# +# Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. +# All rights reserved. +# +# Permission to use, copy, modify, and distribute this software and its +# documentation for educational, research, and non-profit purposes, without fee, +# and without a written agreement is hereby granted, provided that the above +# copyright notice, this paragraph, and the following four paragraphs appear in +# all copies. +# +# Permission to incorporate this software into commercial products may be +# obtained by contacting the authors or the Office of +# Technology Development at the University of North Carolina at Chapel Hill +# . +# +# This software program and documentation are copyrighted by the University of +# North Carolina at Chapel Hill. The software program and documentation are +# supplied "as is," without any accompanying services from the University of +# North Carolina at Chapel Hill or the authors. The University of North Carolina +# at Chapel Hill and the authors do not warrant that the operation of the +# program will be uninterrupted or error-free. The end-user understands that the +# program was developed for research purposes and is advised not to rely +# exclusively on the program for any reason. +# +# IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE +# AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR +# CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS +# SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT +# CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH +# DAMAGE. +# +# THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY +# DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +# OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY +# WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" +# BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS +# HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR +# MODIFICATIONS. +# +# Please send all bug reports to . +# +# The authors may be contacted via: +# +# Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha +# Dept. of Computer Science +# 201 S. Columbia St. +# Frederick P. Brooks, Jr. Computer Science Bldg. +# Chapel Hill, N.C. 27599-3175 +# United States of America +# +# +# + +cmake_minimum_required(VERSION 2.8) +project(RVO) + +include(CTest) + +add_subdirectory(src) +add_subdirectory(examples) + +include(CPack) diff --git a/COPYING b/COPYING new file mode 100644 index 0000000..1454ce3 --- /dev/null +++ b/COPYING @@ -0,0 +1,35 @@ +Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. +All rights reserved. + +Permission to use, copy, modify, and distribute this software and its +documentation for educational, research, and non-profit purposes, without fee, +and without a written agreement is hereby granted, provided that the above +copyright notice, this paragraph, and the following four paragraphs appear in +all copies. + +Permission to incorporate this software into commercial products may be obtained +by contacting the authors or the Office of Technology +Development at the University of North Carolina at Chapel Hill . + +This software program and documentation are copyrighted by the University of +North Carolina at Chapel Hill. The software program and documentation are +supplied "as is," without any accompanying services from the University of North +Carolina at Chapel Hill or the authors. The University of North Carolina at +Chapel Hill and the authors do not warrant that the operation of the program +will be uninterrupted or error-free. The end-user understands that the program +was developed for research purposes and is advised not to rely exclusively on +the program for any reason. + +IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS +BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR +CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS +SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT +CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY +DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY +WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" +BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE +NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR +MODIFICATIONS. diff --git a/README.md b/README.md new file mode 100644 index 0000000..a859b03 --- /dev/null +++ b/README.md @@ -0,0 +1,54 @@ +Optimal Reciprocal Collision Avoidance +====================================== + + + +Copyright © 2008-2013 University of North Carolina at Chapel Hill. All +rights reserved. + +[![Build Status](https://travis-ci.org/snape/RVO2.png?branch=master)](https://travis-ci.org/snape/RVO2) + +Permission to use, copy, modify, and distribute this software and its +documentation for educational, research, and non-profit purposes, without fee, +and without a written agreement is hereby granted, provided that the above +copyright notice, this paragraph, and the following four paragraphs appear in +all copies. + +Permission to incorporate this software into commercial products may be obtained +by contacting the authors ([geom@cs.unc.edu](mailto:geom@cs.unc.edu)) or the +Office of Technology Development at the University of North Carolina at Chapel +Hill ([otd@unc.edu](mailto:otd@unc.edu)). + +This software program and documentation are copyrighted by the University of +North Carolina at Chapel Hill. The software program and documentation are +supplied "as is," without any accompanying services from the University of North +Carolina at Chapel Hill or the authors. The University of North Carolina at +Chapel Hill and the authors do not warrant that the operation of the program +will be uninterrupted or error-free. The end-user understands that the program +was developed for research purposes and is advised not to rely exclusively on +the program for any reason. + +IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS +BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR +CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS +SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT +CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY +DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY +WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" +BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE +NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR +MODIFICATIONS. + +Please send all bug reports to [geom@cs.unc.edu](mailto:geom@cs.unc.edu). + +The authors may be contacted via: + +Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and Dinesh Manocha +Dept. of Computer Science +201 S. Columbia St. +Frederick P. Brooks, Jr. Computer Science Bldg. +Chapel Hill, N.C. 27599-3175 +United States of America diff --git a/doc/Doxyfile b/doc/Doxyfile new file mode 100644 index 0000000..7276d43 --- /dev/null +++ b/doc/Doxyfile @@ -0,0 +1,304 @@ +# Doxyfile 1.8.4 + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- +DOXYFILE_ENCODING = UTF-8 +PROJECT_NAME = "RVO2 Library" +PROJECT_NUMBER = 2.0.1 +PROJECT_BRIEF = "Optimal Reciprocal Collision Avoidance" +PROJECT_LOGO = +OUTPUT_DIRECTORY = +CREATE_SUBDIRS = NO +OUTPUT_LANGUAGE = English +BRIEF_MEMBER_DESC = YES +REPEAT_BRIEF = YES +ABBREVIATE_BRIEF = +ALWAYS_DETAILED_SEC = NO +INLINE_INHERITED_MEMB = NO +FULL_PATH_NAMES = YES +STRIP_FROM_PATH = ../src +STRIP_FROM_INC_PATH = +SHORT_NAMES = NO +JAVADOC_AUTOBRIEF = NO +QT_AUTOBRIEF = NO +MULTILINE_CPP_IS_BRIEF = NO +INHERIT_DOCS = YES +SEPARATE_MEMBER_PAGES = NO +TAB_SIZE = 4 +ALIASES = +TCL_SUBST = +OPTIMIZE_OUTPUT_FOR_C = NO +OPTIMIZE_OUTPUT_JAVA = NO +OPTIMIZE_FOR_FORTRAN = NO +OPTIMIZE_OUTPUT_VHDL = NO +EXTENSION_MAPPING = +MARKDOWN_SUPPORT = YES +AUTOLINK_SUPPORT = YES +BUILTIN_STL_SUPPORT = YES +CPP_CLI_SUPPORT = NO +SIP_SUPPORT = NO +IDL_PROPERTY_SUPPORT = YES +DISTRIBUTE_GROUP_DOC = NO +SUBGROUPING = YES +INLINE_GROUPED_CLASSES = NO +INLINE_SIMPLE_STRUCTS = NO +TYPEDEF_HIDES_STRUCT = NO +LOOKUP_CACHE_SIZE = 0 +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- +EXTRACT_ALL = YES +EXTRACT_PRIVATE = NO +EXTRACT_PACKAGE = NO +EXTRACT_STATIC = NO +EXTRACT_LOCAL_CLASSES = YES +EXTRACT_LOCAL_METHODS = NO +EXTRACT_ANON_NSPACES = NO +HIDE_UNDOC_MEMBERS = NO +HIDE_UNDOC_CLASSES = NO +HIDE_FRIEND_COMPOUNDS = NO +HIDE_IN_BODY_DOCS = NO +INTERNAL_DOCS = NO +CASE_SENSE_NAMES = NO +HIDE_SCOPE_NAMES = NO +SHOW_INCLUDE_FILES = YES +FORCE_LOCAL_INCLUDES = NO +INLINE_INFO = YES +SORT_MEMBER_DOCS = YES +SORT_BRIEF_DOCS = YES +SORT_MEMBERS_CTORS_1ST = YES +SORT_GROUP_NAMES = NO +SORT_BY_SCOPE_NAME = NO +STRICT_PROTO_MATCHING = NO +GENERATE_TODOLIST = YES +GENERATE_TESTLIST = YES +GENERATE_BUGLIST = YES +GENERATE_DEPRECATEDLIST= YES +ENABLED_SECTIONS = +MAX_INITIALIZER_LINES = 30 +SHOW_USED_FILES = YES +SHOW_FILES = YES +SHOW_NAMESPACES = YES +FILE_VERSION_FILTER = +LAYOUT_FILE = +CITE_BIB_FILES = +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- +QUIET = NO +WARNINGS = YES +WARN_IF_UNDOCUMENTED = YES +WARN_IF_DOC_ERROR = YES +WARN_NO_PARAMDOC = NO +WARN_FORMAT = "$file:$line: $text" +WARN_LOGFILE = +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- +INPUT = ../src +INPUT_ENCODING = UTF-8 +FILE_PATTERNS = RVO.h RVOSimulator.h Vector2.h +RECURSIVE = NO +EXCLUDE = +EXCLUDE_SYMLINKS = NO +EXCLUDE_PATTERNS = +EXCLUDE_SYMBOLS = +EXAMPLE_PATH = ../examples +EXAMPLE_PATTERNS = *.cpp +EXAMPLE_RECURSIVE = NO +IMAGE_PATH = +INPUT_FILTER = +FILTER_PATTERNS = +FILTER_SOURCE_FILES = NO +FILTER_SOURCE_PATTERNS = +USE_MDFILE_AS_MAINPAGE = +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- +SOURCE_BROWSER = NO +INLINE_SOURCES = NO +STRIP_CODE_COMMENTS = YES +REFERENCED_BY_RELATION = NO +REFERENCES_RELATION = NO +REFERENCES_LINK_SOURCE = YES +USE_HTAGS = NO +VERBATIM_HEADERS = YES +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- +ALPHABETICAL_INDEX = YES +COLS_IN_ALPHA_INDEX = 5 +IGNORE_PREFIX = +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- +GENERATE_HTML = YES +HTML_OUTPUT = html +HTML_FILE_EXTENSION = .html +HTML_HEADER = header.html +HTML_FOOTER = footer.html +HTML_STYLESHEET = +HTML_EXTRA_STYLESHEET = stylesheet.css +HTML_EXTRA_FILES = +HTML_COLORSTYLE_HUE = 220 +HTML_COLORSTYLE_SAT = 100 +HTML_COLORSTYLE_GAMMA = 80 +HTML_TIMESTAMP = NO +HTML_DYNAMIC_SECTIONS = NO +HTML_INDEX_NUM_ENTRIES = 100 +GENERATE_DOCSET = NO +DOCSET_FEEDNAME = "RVO2 Library" +DOCSET_BUNDLE_ID = edu.unc.cs.gamma.rvo2 +DOCSET_PUBLISHER_ID = edu.unc.cs.gamma.documentation +DOCSET_PUBLISHER_NAME = Publisher +GENERATE_HTMLHELP = NO +CHM_FILE = +HHC_LOCATION = +GENERATE_CHI = NO +CHM_INDEX_ENCODING = +BINARY_TOC = NO +TOC_EXPAND = NO +GENERATE_QHP = NO +QCH_FILE = +QHP_NAMESPACE = edu.unc.cs.gamma.rvo2 +QHP_VIRTUAL_FOLDER = doc +QHP_CUST_FILTER_NAME = +QHP_CUST_FILTER_ATTRS = +QHP_SECT_FILTER_ATTRS = +QHG_LOCATION = +GENERATE_ECLIPSEHELP = NO +ECLIPSE_DOC_ID = edu.unc.cs.gamma.rvo2 +DISABLE_INDEX = NO +GENERATE_TREEVIEW = NO +ENUM_VALUES_PER_LINE = 4 +TREEVIEW_WIDTH = 250 +EXT_LINKS_IN_WINDOW = NO +FORMULA_FONTSIZE = 10 +FORMULA_TRANSPARENT = YES +USE_MATHJAX = NO +MATHJAX_FORMAT = HTML-CSS +MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest +MATHJAX_EXTENSIONS = +MATHJAX_CODEFILE = +SEARCHENGINE = NO +SERVER_BASED_SEARCH = NO +EXTERNAL_SEARCH = NO +SEARCHENGINE_URL = +SEARCHDATA_FILE = searchdata.xml +EXTERNAL_SEARCH_ID = +EXTRA_SEARCH_MAPPINGS = +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- +GENERATE_LATEX = NO +LATEX_OUTPUT = latex +LATEX_CMD_NAME = latex +MAKEINDEX_CMD_NAME = makeindex +COMPACT_LATEX = NO +PAPER_TYPE = letter +EXTRA_PACKAGES = +LATEX_HEADER = +LATEX_FOOTER = +LATEX_EXTRA_FILES = +PDF_HYPERLINKS = YES +USE_PDFLATEX = YES +LATEX_BATCHMODE = NO +LATEX_HIDE_INDICES = NO +LATEX_SOURCE_CODE = NO +LATEX_BIB_STYLE = plain +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- +GENERATE_RTF = NO +RTF_OUTPUT = rtf +COMPACT_RTF = NO +RTF_HYPERLINKS = NO +RTF_STYLESHEET_FILE = +RTF_EXTENSIONS_FILE = +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- +GENERATE_MAN = NO +MAN_OUTPUT = man +MAN_EXTENSION = .3 +MAN_LINKS = NO +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- +GENERATE_XML = NO +XML_OUTPUT = xml +XML_SCHEMA = +XML_DTD = +XML_PROGRAMLISTING = YES +#--------------------------------------------------------------------------- +# configuration options related to the DOCBOOK output +#--------------------------------------------------------------------------- +GENERATE_DOCBOOK = NO +DOCBOOK_OUTPUT = docbook +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- +GENERATE_AUTOGEN_DEF = NO +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- +GENERATE_PERLMOD = NO +PERLMOD_LATEX = NO +PERLMOD_PRETTY = YES +PERLMOD_MAKEVAR_PREFIX = +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = NO +EXPAND_ONLY_PREDEF = NO +SEARCH_INCLUDES = YES +INCLUDE_PATH = +INCLUDE_FILE_PATTERNS = +PREDEFINED = +EXPAND_AS_DEFINED = +SKIP_FUNCTION_MACROS = YES +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- +TAGFILES = +GENERATE_TAGFILE = +ALLEXTERNALS = NO +EXTERNAL_GROUPS = YES +EXTERNAL_PAGES = YES +PERL_PATH = +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- +CLASS_DIAGRAMS = YES +MSCGEN_PATH = +HIDE_UNDOC_RELATIONS = YES +HAVE_DOT = YES +DOT_NUM_THREADS = 0 +DOT_FONTNAME = Helvetica +DOT_FONTSIZE = 10 +DOT_FONTPATH = +CLASS_GRAPH = YES +COLLABORATION_GRAPH = YES +GROUP_GRAPHS = YES +UML_LOOK = NO +UML_LIMIT_NUM_FIELDS = 10 +TEMPLATE_RELATIONS = NO +INCLUDE_GRAPH = YES +INCLUDED_BY_GRAPH = YES +CALL_GRAPH = NO +CALLER_GRAPH = NO +GRAPHICAL_HIERARCHY = YES +DIRECTORY_GRAPH = YES +DOT_IMAGE_FORMAT = png +INTERACTIVE_SVG = NO +DOT_PATH = +DOTFILE_DIRS = +MSCFILE_DIRS = +DOT_GRAPH_MAX_NODES = 50 +MAX_DOT_GRAPH_DEPTH = 0 +DOT_TRANSPARENT = NO +DOT_MULTI_TARGETS = NO +GENERATE_LEGEND = YES +DOT_CLEANUP = YES diff --git a/doc/footer.html b/doc/footer.html new file mode 100644 index 0000000..434604f --- /dev/null +++ b/doc/footer.html @@ -0,0 +1,7 @@ + + +
+$projectname. Copyright © 2008-2013 University of North Carolina at Chapel Hill. +
+ + diff --git a/doc/header.html b/doc/header.html new file mode 100644 index 0000000..ff433e0 --- /dev/null +++ b/doc/header.html @@ -0,0 +1,21 @@ + + + + + + + +$projectname: $title +$title + + + +$treeview +$search +$mathjax + +$extrastylesheet + + +
+ diff --git a/doc/stylesheet.css b/doc/stylesheet.css new file mode 100644 index 0000000..e9ef3f3 --- /dev/null +++ b/doc/stylesheet.css @@ -0,0 +1,1185 @@ +/* The standard CSS for doxygen 1.8.4 */ + +body, table, div, p, dl { + font: 400 14px/22px Roboto,sans-serif; +} + +/* @group Heading Levels */ + +h1.groupheader { + font-size: 150%; +} + +.title { + font: 400 14px/28px Roboto,sans-serif; + font-size: 150%; + font-weight: bold; + margin: 10px 2px; +} + +h2.groupheader { + border-bottom: 1px solid #879ECB; + color: #354C7B; + font-size: 150%; + font-weight: normal; + margin-top: 1.75em; + padding-top: 8px; + padding-bottom: 4px; + width: 100%; +} + +h3.groupheader { + font-size: 100%; +} + +h1, h2, h3, h4, h5, h6 { + -webkit-transition: text-shadow 0.5s linear; + -moz-transition: text-shadow 0.5s linear; + -ms-transition: text-shadow 0.5s linear; + -o-transition: text-shadow 0.5s linear; + transition: text-shadow 0.5s linear; + margin-right: 15px; +} + +h1.glow, h2.glow, h3.glow, h4.glow, h5.glow, h6.glow { + text-shadow: 0 0 15px cyan; +} + +dt { + font-weight: bold; +} + +div.multicol { + -moz-column-gap: 1em; + -webkit-column-gap: 1em; + -moz-column-count: 3; + -webkit-column-count: 3; +} + +p.startli, p.startdd, p.starttd { + margin-top: 2px; +} + +p.endli { + margin-bottom: 0px; +} + +p.enddd { + margin-bottom: 4px; +} + +p.endtd { + margin-bottom: 2px; +} + +/* @end */ + +caption { + font-weight: bold; +} + +span.legend { + font-size: 70%; + text-align: center; +} + +h3.version { + font-size: 90%; + text-align: center; +} + +div.qindex, div.navtab{ + background-color: #EBEFF6; + border: 1px solid #A3B4D7; + text-align: center; +} + +div.qindex, div.navpath { + width: 100%; + line-height: 140%; +} + +div.navtab { + margin-right: 15px; +} + +/* @group Link Styling */ + +a { + color: #3D578C; + font-weight: normal; + text-decoration: none; +} + +.contents a:visited { + color: #4665A2; +} + +a:hover { + text-decoration: underline; +} + +a.qindex { + font-weight: bold; +} + +a.qindexHL { + font-weight: bold; + background-color: #9CAFD4; + color: #ffffff; + border: 1px double #869DCA; +} + +.contents a.qindexHL:visited { + color: #ffffff; +} + +a.el { + font-weight: bold; +} + +a.elRef { +} + +a.code, a.code:visited { + color: #4665A2; +} + +a.codeRef, a.codeRef:visited { + color: #4665A2; +} + +/* @end */ + +dl.el { + margin-left: -1cm; +} + +pre.fragment { + border: 1px solid #C4CFE5; + background-color: #FBFCFD; + padding: 4px 6px; + margin: 4px 8px 4px 2px; + overflow: auto; + word-wrap: break-word; + font-size: 9pt; + line-height: 125%; + font-family: monospace, fixed; + font-size: 105%; +} + +div.fragment { + padding: 0px; + margin: 0px; + background-color: #FBFCFD; + border: 1px solid #C4CFE5; +} + +div.line { + font-family: monospace, fixed; + font-size: 13px; + min-height: 13px; + line-height: 1.0; + text-wrap: unrestricted; + white-space: -moz-pre-wrap; /* Moz */ + white-space: -pre-wrap; /* Opera 4-6 */ + white-space: -o-pre-wrap; /* Opera 7 */ + white-space: pre-wrap; /* CSS3 */ + word-wrap: break-word; /* IE 5.5+ */ + text-indent: -53px; + padding-left: 53px; + padding-bottom: 0px; + margin: 0px; + -webkit-transition-property: background-color, box-shadow; + -webkit-transition-duration: 0.5s; + -moz-transition-property: background-color, box-shadow; + -moz-transition-duration: 0.5s; + -ms-transition-property: background-color, box-shadow; + -ms-transition-duration: 0.5s; + -o-transition-property: background-color, box-shadow; + -o-transition-duration: 0.5s; + transition-property: background-color, box-shadow; + transition-duration: 0.5s; +} + +div.line.glow { + background-color: cyan; + box-shadow: 0 0 10px cyan; +} + + +span.lineno { + padding-right: 4px; + text-align: right; + border-right: 2px solid #0F0; + background-color: #E8E8E8; + white-space: pre; +} +span.lineno a { + background-color: #D8D8D8; +} + +span.lineno a:hover { + background-color: #C8C8C8; +} + +div.ah { + background-color: black; + font-weight: bold; + color: #ffffff; + margin-bottom: 3px; + margin-top: 3px; + padding: 0.2em; + border: solid thin #333; + border-radius: 0.5em; + -webkit-border-radius: .5em; + -moz-border-radius: .5em; + box-shadow: 2px 2px 3px #999; + -webkit-box-shadow: 2px 2px 3px #999; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px; + background-image: -webkit-gradient(linear, left top, left bottom, from(#eee), to(#000),color-stop(0.3, #444)); + background-image: -moz-linear-gradient(center top, #eee 0%, #444 40%, #000); +} + +div.groupHeader { + margin-left: 16px; + margin-top: 12px; + font-weight: bold; +} + +div.groupText { + margin-left: 16px; + font-style: italic; +} + +body { + background-color: white; + color: black; + margin: 0; +} + +div.contents { + margin-top: 10px; + margin-left: 12px; + margin-right: 8px; +} + +td.indexkey { + background-color: #EBEFF6; + font-weight: bold; + border: 1px solid #C4CFE5; + margin: 2px 0px 2px 0; + padding: 2px 10px; + white-space: nowrap; + vertical-align: top; +} + +td.indexvalue { + background-color: #EBEFF6; + border: 1px solid #C4CFE5; + padding: 2px 10px; + margin: 2px 0px; +} + +tr.memlist { + background-color: #EEF1F7; +} + +p.formulaDsp { + text-align: center; +} + +img.formulaDsp { + +} + +img.formulaInl { + vertical-align: middle; +} + +div.center { + text-align: center; + margin-top: 0px; + margin-bottom: 0px; + padding: 0px; +} + +div.center img { + border: 0px; +} + +address.footer { + text-align: right; + padding-right: 12px; +} + +img.footer { + border: 0px; + vertical-align: middle; +} + +/* @group Code Colorization */ + +span.keyword { + color: #008000 +} + +span.keywordtype { + color: #604020 +} + +span.keywordflow { + color: #e08000 +} + +span.comment { + color: #800000 +} + +span.preprocessor { + color: #806020 +} + +span.stringliteral { + color: #002080 +} + +span.charliteral { + color: #008080 +} + +span.vhdldigit { + color: #ff00ff +} + +span.vhdlchar { + color: #000000 +} + +span.vhdlkeyword { + color: #700070 +} + +span.vhdllogic { + color: #ff0000 +} + +blockquote { + background-color: #F7F8FB; + border-left: 2px solid #9CAFD4; + margin: 0 24px 0 4px; + padding: 0 12px 0 16px; +} + +/* @end */ + +/* +.search { + color: #003399; + font-weight: bold; +} + +form.search { + margin-bottom: 0px; + margin-top: 0px; +} + +input.search { + font-size: 75%; + color: #000080; + font-weight: normal; + background-color: #e8eef2; +} +*/ + +td.tiny { + font-size: 75%; +} + +.dirtab { + padding: 4px; + border-collapse: collapse; + border: 1px solid #A3B4D7; +} + +th.dirtab { + background: #EBEFF6; + font-weight: bold; +} + +hr { + height: 0px; + border: none; + border-top: 1px solid #4A6AAA; +} + +hr.footer { + height: 1px; +} + +/* @group Member Descriptions */ + +table.memberdecls { + border-spacing: 0px; + padding: 0px; +} + +.memberdecls td, .fieldtable tr { + -webkit-transition-property: background-color, box-shadow; + -webkit-transition-duration: 0.5s; + -moz-transition-property: background-color, box-shadow; + -moz-transition-duration: 0.5s; + -ms-transition-property: background-color, box-shadow; + -ms-transition-duration: 0.5s; + -o-transition-property: background-color, box-shadow; + -o-transition-duration: 0.5s; + transition-property: background-color, box-shadow; + transition-duration: 0.5s; +} + +.memberdecls td.glow, .fieldtable tr.glow { + background-color: cyan; + box-shadow: 0 0 15px cyan; +} + +.mdescLeft, .mdescRight, +.memItemLeft, .memItemRight, +.memTemplItemLeft, .memTemplItemRight, .memTemplParams { + background-color: #F9FAFC; + border: none; + margin: 4px; + padding: 1px 0 0 8px; +} + +.mdescLeft, .mdescRight { + padding: 0px 8px 4px 8px; + color: #555; +} + +.memSeparator { + border-bottom: 1px solid #DEE4F0; + line-height: 1px; + margin: 0px; + padding: 0px; +} + +.memItemLeft, .memTemplItemLeft { + white-space: nowrap; +} + +.memItemRight { + width: 100%; +} + +.memTemplParams { + color: #4665A2; + white-space: nowrap; + font-size: 80%; +} + +/* @end */ + +/* @group Member Details */ + +/* Styles for detailed member documentation */ + +.memtemplate { + font-size: 80%; + color: #4665A2; + font-weight: normal; + margin-left: 9px; +} + +.memnav { + background-color: #EBEFF6; + border: 1px solid #A3B4D7; + text-align: center; + margin: 2px; + margin-right: 15px; + padding: 2px; +} + +.mempage { + width: 100%; +} + +.memitem { + padding: 0; + margin-bottom: 10px; + margin-right: 5px; + -webkit-transition: box-shadow 0.5s linear; + -moz-transition: box-shadow 0.5s linear; + -ms-transition: box-shadow 0.5s linear; + -o-transition: box-shadow 0.5s linear; + transition: box-shadow 0.5s linear; + display: table !important; + width: 100%; +} + +.memitem.glow { + box-shadow: 0 0 15px cyan; +} + +.memname { + font-weight: bold; + margin-left: 6px; +} + +.memname td { + vertical-align: bottom; +} + +.memproto, dl.reflist dt { + border-top: 1px solid #A8B8D9; + border-left: 1px solid #A8B8D9; + border-right: 1px solid #A8B8D9; + padding: 6px 0px 6px 0px; + color: #253555; + font-weight: bold; + text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9); + background-image:url('nav_f.png'); + background-repeat:repeat-x; + background-color: #E2E8F2; + /* opera specific markup */ + box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + border-top-right-radius: 4px; + border-top-left-radius: 4px; + /* firefox specific markup */ + -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px; + -moz-border-radius-topright: 4px; + -moz-border-radius-topleft: 4px; + /* webkit specific markup */ + -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + -webkit-border-top-right-radius: 4px; + -webkit-border-top-left-radius: 4px; + +} + +.memdoc, dl.reflist dd { + border-bottom: 1px solid #A8B8D9; + border-left: 1px solid #A8B8D9; + border-right: 1px solid #A8B8D9; + padding: 6px 10px 2px 10px; + background-color: #FBFCFD; + border-top-width: 0; + background-image:url('nav_g.png'); + background-repeat:repeat-x; + background-color: #FFFFFF; + /* opera specific markup */ + border-bottom-left-radius: 4px; + border-bottom-right-radius: 4px; + box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); + /* firefox specific markup */ + -moz-border-radius-bottomleft: 4px; + -moz-border-radius-bottomright: 4px; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 5px 5px 5px; + /* webkit specific markup */ + -webkit-border-bottom-left-radius: 4px; + -webkit-border-bottom-right-radius: 4px; + -webkit-box-shadow: 5px 5px 5px rgba(0, 0, 0, 0.15); +} + +dl.reflist dt { + padding: 5px; +} + +dl.reflist dd { + margin: 0px 0px 10px 0px; + padding: 5px; +} + +.paramkey { + text-align: right; +} + +.paramtype { + white-space: nowrap; +} + +.paramname { + color: #602020; + white-space: nowrap; +} +.paramname em { + font-style: normal; +} +.paramname code { + line-height: 14px; +} + +.params, .retval, .exception, .tparams { + margin-left: 0px; + padding-left: 0px; +} + +.params .paramname, .retval .paramname { + font-weight: bold; + vertical-align: top; +} + +.params .paramtype { + font-style: italic; + vertical-align: top; +} + +.params .paramdir { + font-family: "courier new",courier,monospace; + vertical-align: top; +} + +table.mlabels { + border-spacing: 0px; +} + +td.mlabels-left { + width: 100%; + padding: 0px; +} + +td.mlabels-right { + vertical-align: bottom; + padding: 0px; + white-space: nowrap; +} + +span.mlabels { + margin-left: 8px; +} + +span.mlabel { + background-color: #728DC1; + border-top:1px solid #5373B4; + border-left:1px solid #5373B4; + border-right:1px solid #C4CFE5; + border-bottom:1px solid #C4CFE5; + text-shadow: none; + color: white; + margin-right: 4px; + padding: 2px 3px; + border-radius: 3px; + font-size: 7pt; + white-space: nowrap; + vertical-align: middle; +} + + + +/* @end */ + +/* these are for tree view when not used as main index */ + +div.directory { + margin: 10px 0px; + border-top: 1px solid #A8B8D9; + border-bottom: 1px solid #A8B8D9; + width: 100%; +} + +.directory table { + border-collapse:collapse; +} + +.directory td { + margin: 0px; + padding: 0px; + vertical-align: top; +} + +.directory td.entry { + white-space: nowrap; + padding-right: 6px; + padding-top: 3px; +} + +.directory td.entry a { + outline:none; +} + +.directory td.entry a img { + border: none; +} + +.directory td.desc { + width: 100%; + padding-left: 6px; + padding-right: 6px; + padding-top: 3px; + border-left: 1px solid rgba(0,0,0,0.05); +} + +.directory tr.even { + padding-left: 6px; + background-color: #F7F8FB; +} + +.directory img { + vertical-align: -30%; +} + +.directory .levels { + white-space: nowrap; + width: 100%; + text-align: right; + font-size: 9pt; +} + +.directory .levels span { + cursor: pointer; + padding-left: 2px; + padding-right: 2px; + color: #3D578C; +} + +div.dynheader { + margin-top: 8px; + -webkit-touch-callout: none; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; +} + +address { + font-style: normal; + color: #2A3D61; +} + +table.doxtable { + border-collapse:collapse; + margin-top: 4px; + margin-bottom: 4px; +} + +table.doxtable td, table.doxtable th { + border: 1px solid #2D4068; + padding: 3px 7px 2px; +} + +table.doxtable th { + background-color: #374F7F; + color: #FFFFFF; + font-size: 110%; + padding-bottom: 4px; + padding-top: 5px; +} + +table.fieldtable { + /*width: 100%;*/ + margin-bottom: 10px; + border: 1px solid #A8B8D9; + border-spacing: 0px; + -moz-border-radius: 4px; + -webkit-border-radius: 4px; + border-radius: 4px; + -moz-box-shadow: rgba(0, 0, 0, 0.15) 2px 2px 2px; + -webkit-box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15); + box-shadow: 2px 2px 2px rgba(0, 0, 0, 0.15); +} + +.fieldtable td, .fieldtable th { + padding: 3px 7px 2px; +} + +.fieldtable td.fieldtype, .fieldtable td.fieldname { + white-space: nowrap; + border-right: 1px solid #A8B8D9; + border-bottom: 1px solid #A8B8D9; + vertical-align: top; +} + +.fieldtable td.fieldname { + padding-top: 3px; +} + +.fieldtable td.fielddoc { + border-bottom: 1px solid #A8B8D9; + /*width: 100%;*/ +} + +.fieldtable td.fielddoc p:first-child { + margin-top: 0px; +} + +.fieldtable td.fielddoc p:last-child { + margin-bottom: 2px; +} + +.fieldtable tr:last-child td { + border-bottom: none; +} + +.fieldtable th { + background-image:url('nav_f.png'); + background-repeat:repeat-x; + background-color: #E2E8F2; + font-size: 90%; + color: #253555; + padding-bottom: 4px; + padding-top: 5px; + text-align:left; + -moz-border-radius-topleft: 4px; + -moz-border-radius-topright: 4px; + -webkit-border-top-left-radius: 4px; + -webkit-border-top-right-radius: 4px; + border-top-left-radius: 4px; + border-top-right-radius: 4px; + border-bottom: 1px solid #A8B8D9; +} + + +.tabsearch { + top: 0px; + left: 10px; + height: 36px; + background-image: url('tab_b.png'); + z-index: 101; + overflow: hidden; + font-size: 13px; +} + +.navpath ul +{ + font-size: 11px; + background-image:url('tab_b.png'); + background-repeat:repeat-x; + background-position: 0 -5px; + height:30px; + line-height:30px; + color:#8AA0CC; + border:solid 1px #C2CDE4; + overflow:hidden; + margin:0px; + padding:0px; +} + +.navpath li +{ + list-style-type:none; + float:left; + padding-left:10px; + padding-right:15px; + background-image:url('bc_s.png'); + background-repeat:no-repeat; + background-position:right; + color:#364D7C; +} + +.navpath li.navelem a +{ + height:32px; + display:block; + text-decoration: none; + outline: none; + color: #283A5D; + font-family: 'Lucida Grande',Geneva,Helvetica,Arial,sans-serif; + text-shadow: 0px 1px 1px rgba(255, 255, 255, 0.9); + text-decoration: none; +} + +.navpath li.navelem a:hover +{ + color:#6884BD; +} + +.navpath li.footer +{ + list-style-type:none; + float:right; + padding-left:10px; + padding-right:15px; + background-image:none; + background-repeat:no-repeat; + background-position:right; + color:#364D7C; + font-size: 8pt; +} + + +div.summary +{ + float: right; + font-size: 8pt; + padding-right: 5px; + width: 50%; + text-align: right; +} + +div.summary a +{ + white-space: nowrap; +} + +div.ingroups +{ + font-size: 8pt; + width: 50%; + text-align: left; +} + +div.ingroups a +{ + white-space: nowrap; +} + +div.header +{ + background-image:url('nav_h.png'); + background-repeat:repeat-x; + background-color: #F9FAFC; + margin: 0px; + border-bottom: 1px solid #C4CFE5; +} + +div.headertitle +{ + padding: 5px 5px 5px 10px; +} + +dl +{ + padding: 0 0 0 10px; +} + +/* dl.note, dl.warning, dl.attention, dl.pre, dl.post, dl.invariant, dl.deprecated, dl.todo, dl.test, dl.bug */ +dl.section +{ + margin-left: 0px; + padding-left: 0px; +} + +dl.note +{ + margin-left:-7px; + padding-left: 3px; + border-left:4px solid; + border-color: #D0C000; +} + +dl.warning, dl.attention +{ + margin-left:-7px; + padding-left: 3px; + border-left:4px solid; + border-color: #FF0000; +} + +dl.pre, dl.post, dl.invariant +{ + margin-left:-7px; + padding-left: 3px; + border-left:4px solid; + border-color: #00D000; +} + +dl.deprecated +{ + margin-left:-7px; + padding-left: 3px; + border-left:4px solid; + border-color: #505050; +} + +dl.todo +{ + margin-left:-7px; + padding-left: 3px; + border-left:4px solid; + border-color: #00C0E0; +} + +dl.test +{ + margin-left:-7px; + padding-left: 3px; + border-left:4px solid; + border-color: #3030E0; +} + +dl.bug +{ + margin-left:-7px; + padding-left: 3px; + border-left:4px solid; + border-color: #C08050; +} + +dl.section dd { + margin-bottom: 6px; +} + + +#projectlogo +{ + text-align: center; + vertical-align: bottom; + border-collapse: separate; +} + +#projectlogo img +{ + border: 0px none; +} + +#projectname +{ + font: 300% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 2px 0px; +} + +#projectbrief +{ + font: 120% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 0px; +} + +#projectnumber +{ + font: 50% Tahoma, Arial,sans-serif; + margin: 0px; + padding: 0px; +} + +#titlearea +{ + padding: 0px; + margin: 0px; + width: 100%; + border-bottom: 1px solid #5373B4; +} + +.image +{ + text-align: center; +} + +.dotgraph +{ + text-align: center; +} + +.mscgraph +{ + text-align: center; +} + +.caption +{ + font-weight: bold; +} + +div.zoom +{ + border: 1px solid #90A5CE; +} + +dl.citelist { + margin-bottom:50px; +} + +dl.citelist dt { + color:#334975; + float:left; + font-weight:bold; + margin-right:10px; + padding:5px; +} + +dl.citelist dd { + margin:2px 0; + padding:5px 0; +} + +div.toc { + padding: 14px 25px; + background-color: #F4F6FA; + border: 1px solid #D8DFEE; + border-radius: 7px 7px 7px 7px; + float: right; + height: auto; + margin: 0 20px 10px 10px; + width: 200px; +} + +div.toc li { + background: url("bdwn.png") no-repeat scroll 0 5px transparent; + font: 10px/1.2 Verdana,DejaVu Sans,Geneva,sans-serif; + margin-top: 5px; + padding-left: 10px; + padding-top: 2px; +} + +div.toc h3 { + font: bold 12px/1.2 Arial,FreeSans,sans-serif; + color: #4665A2; + border-bottom: 0 none; + margin: 0; +} + +div.toc ul { + list-style: none outside none; + border: medium none; + padding: 0px; +} + +div.toc li.level1 { + margin-left: 0px; +} + +div.toc li.level2 { + margin-left: 15px; +} + +div.toc li.level3 { + margin-left: 30px; +} + +div.toc li.level4 { + margin-left: 45px; +} + +.inherit_header { + font-weight: bold; + color: gray; + cursor: pointer; + -webkit-touch-callout: none; + -webkit-user-select: none; + -khtml-user-select: none; + -moz-user-select: none; + -ms-user-select: none; + user-select: none; +} + +.inherit_header td { + padding: 6px 0px 2px 5px; +} + +.inherit { + display: none; +} + +tr.heading h2 { + margin-top: 12px; + margin-bottom: 4px; +} + +@media print +{ + #top { display: none; } + #side-nav { display: none; } + #nav-path { display: none; } + body { overflow:visible; } + h1, h2, h3, h4, h5, h6 { page-break-after: avoid; } + .summary { display: none; } + .memitem { page-break-inside: avoid; } + #doc-content + { + margin-left:0 !important; + height:auto !important; + width:auto !important; + overflow:inherit; + display:inline; + } +} diff --git a/examples/Blocks.cpp b/examples/Blocks.cpp new file mode 100644 index 0000000..d3ba9cb --- /dev/null +++ b/examples/Blocks.cpp @@ -0,0 +1,237 @@ +/* + * Blocks.cpp + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +/* + * Example file showing a demo with 100 agents split in four groups initially + * positioned in four corners of the environment. Each agent attempts to move to + * other side of the environment through a narrow passage generated by four + * obstacles. There is no roadmap to guide the agents around the obstacles. + */ + +#define RVO_OUTPUT_TIME_AND_POSITIONS 0 +#define RVO_SEED_RANDOM_NUMBER_GENERATOR 0 + +#include +#include + +#include + +#if RVO_OUTPUT_TIME_AND_POSITIONS +#include +#endif + +#if RVO_SEED_RANDOM_NUMBER_GENERATOR +#include +#endif + +#if _OPENMP +#include +#endif + +#include + +#ifndef M_PI +const float M_PI = 3.14159265358979323846f; +#endif + +/* Store the goals of the agents. */ +std::vector goals; + +void setupScenario(RVO::RVOSimulator *sim) +{ +#if RVO_SEED_RANDOM_NUMBER_GENERATOR + std::srand(static_cast(std::time(NULL))); +#endif + + /* Specify the global time step of the simulation. */ + sim->setTimeStep(0.25f); + + /* Specify the default parameters for agents that are subsequently added. */ + sim->setAgentDefaults(15.0f, 10, 5.0f, 5.0f, 2.0f, 2.0f); + + /* + * Add agents, specifying their start position, and store their goals on the + * opposite side of the environment. + */ + for (size_t i = 0; i < 5; ++i) { + for (size_t j = 0; j < 5; ++j) { + sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, 55.0f + j * 10.0f)); + goals.push_back(RVO::Vector2(-75.0f, -75.0f)); + + sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, 55.0f + j * 10.0f)); + goals.push_back(RVO::Vector2(75.0f, -75.0f)); + + sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, -55.0f - j * 10.0f)); + goals.push_back(RVO::Vector2(-75.0f, 75.0f)); + + sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, -55.0f - j * 10.0f)); + goals.push_back(RVO::Vector2(75.0f, 75.0f)); + } + } + + /* + * Add (polygonal) obstacles, specifying their vertices in counterclockwise + * order. + */ + std::vector obstacle1, obstacle2, obstacle3, obstacle4; + + obstacle1.push_back(RVO::Vector2(-10.0f, 40.0f)); + obstacle1.push_back(RVO::Vector2(-40.0f, 40.0f)); + obstacle1.push_back(RVO::Vector2(-40.0f, 10.0f)); + obstacle1.push_back(RVO::Vector2(-10.0f, 10.0f)); + + obstacle2.push_back(RVO::Vector2(10.0f, 40.0f)); + obstacle2.push_back(RVO::Vector2(10.0f, 10.0f)); + obstacle2.push_back(RVO::Vector2(40.0f, 10.0f)); + obstacle2.push_back(RVO::Vector2(40.0f, 40.0f)); + + obstacle3.push_back(RVO::Vector2(10.0f, -40.0f)); + obstacle3.push_back(RVO::Vector2(40.0f, -40.0f)); + obstacle3.push_back(RVO::Vector2(40.0f, -10.0f)); + obstacle3.push_back(RVO::Vector2(10.0f, -10.0f)); + + obstacle4.push_back(RVO::Vector2(-10.0f, -40.0f)); + obstacle4.push_back(RVO::Vector2(-10.0f, -10.0f)); + obstacle4.push_back(RVO::Vector2(-40.0f, -10.0f)); + obstacle4.push_back(RVO::Vector2(-40.0f, -40.0f)); + + sim->addObstacle(obstacle1); + sim->addObstacle(obstacle2); + sim->addObstacle(obstacle3); + sim->addObstacle(obstacle4); + + /* Process the obstacles so that they are accounted for in the simulation. */ + sim->processObstacles(); +} + +#if RVO_OUTPUT_TIME_AND_POSITIONS +void updateVisualization(RVO::RVOSimulator *sim) +{ + /* Output the current global time. */ + std::cout << sim->getGlobalTime(); + + /* Output the current position of all the agents. */ + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + std::cout << " " << sim->getAgentPosition(i); + } + + std::cout << std::endl; +} +#endif + +void setPreferredVelocities(RVO::RVOSimulator *sim) +{ + /* + * Set the preferred velocity to be a vector of unit magnitude (speed) in the + * direction of the goal. + */ +#ifdef _OPENMP +#pragma omp parallel for +#endif + for (int i = 0; i < static_cast(sim->getNumAgents()); ++i) { + RVO::Vector2 goalVector = goals[i] - sim->getAgentPosition(i); + + if (RVO::absSq(goalVector) > 1.0f) { + goalVector = RVO::normalize(goalVector); + } + + sim->setAgentPrefVelocity(i, goalVector); + + /* + * Perturb a little to avoid deadlocks due to perfect symmetry. + */ + float angle = std::rand() * 2.0f * M_PI / RAND_MAX; + float dist = std::rand() * 0.0001f / RAND_MAX; + + sim->setAgentPrefVelocity(i, sim->getAgentPrefVelocity(i) + + dist * RVO::Vector2(std::cos(angle), std::sin(angle))); + } +} + +bool reachedGoal(RVO::RVOSimulator *sim) +{ + /* Check if all agents have reached their goals. */ + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (RVO::absSq(sim->getAgentPosition(i) - goals[i]) > 20.0f * 20.0f) { + return false; + } + } + + return true; +} + +int main() +{ + /* Create a new simulator instance. */ + RVO::RVOSimulator *sim = new RVO::RVOSimulator(); + + /* Set up the scenario. */ + setupScenario(sim); + + /* Perform (and manipulate) the simulation. */ + do { +#if RVO_OUTPUT_TIME_AND_POSITIONS + updateVisualization(sim); +#endif + setPreferredVelocities(sim); + sim->doStep(); + } + while (!reachedGoal(sim)); + + delete sim; + + return 0; +} diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..67c0da3 --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,71 @@ +# +# examples/CMakeLists.txt +# RVO2 Library +# +# Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. +# All rights reserved. +# +# Permission to use, copy, modify, and distribute this software and its +# documentation for educational, research, and non-profit purposes, without fee, +# and without a written agreement is hereby granted, provided that the above +# copyright notice, this paragraph, and the following four paragraphs appear in +# all copies. +# +# Permission to incorporate this software into commercial products may be +# obtained by contacting the authors or the Office of +# Technology Development at the University of North Carolina at Chapel Hill +# . +# +# This software program and documentation are copyrighted by the University of +# North Carolina at Chapel Hill. The software program and documentation are +# supplied "as is," without any accompanying services from the University of +# North Carolina at Chapel Hill or the authors. The University of North Carolina +# at Chapel Hill and the authors do not warrant that the operation of the +# program will be uninterrupted or error-free. The end-user understands that the +# program was developed for research purposes and is advised not to rely +# exclusively on the program for any reason. +# +# IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE +# AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR +# CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS +# SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT +# CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH +# DAMAGE. +# +# THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY +# DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +# OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY +# WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" +# BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS +# HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR +# MODIFICATIONS. +# +# Please send all bug reports to . +# +# The authors may be contacted via: +# +# Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha +# Dept. of Computer Science +# 201 S. Columbia St. +# Frederick P. Brooks, Jr. Computer Science Bldg. +# Chapel Hill, N.C. 27599-3175 +# United States of America +# +# +# + +include_directories("${RVO_SOURCE_DIR}/src") + +add_executable(Blocks Blocks.cpp) +target_link_libraries(Blocks RVO) +add_test(Blocks Blocks) + +add_executable(Circle Circle.cpp) +target_link_libraries(Circle RVO) +add_test(Circle Circle) + +add_executable(Roadmap Roadmap.cpp) +target_link_libraries(Roadmap RVO) +add_test(Roadmap Roadmap) + +install(TARGETS Blocks Circle Roadmap DESTINATION bin) diff --git a/examples/Circle.cpp b/examples/Circle.cpp new file mode 100644 index 0000000..a197e98 --- /dev/null +++ b/examples/Circle.cpp @@ -0,0 +1,174 @@ +/* + * Circle.cpp + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +/* + * Example file showing a demo with 250 agents initially positioned evenly + * distributed on a circle attempting to move to the antipodal position on the + * circle. + */ + +#define RVO_OUTPUT_TIME_AND_POSITIONS 0 + +#include +#include +#include + +#if RVO_OUTPUT_TIME_AND_POSITIONS +#include +#endif + +#if _OPENMP +#include +#endif + +#include + +#ifndef M_PI +const float M_PI = 3.14159265358979323846f; +#endif + +/* Store the goals of the agents. */ +std::vector goals; + +void setupScenario(RVO::RVOSimulator *sim) +{ + /* Specify the global time step of the simulation. */ + sim->setTimeStep(0.25f); + + /* Specify the default parameters for agents that are subsequently added. */ + sim->setAgentDefaults(15.0f, 10, 10.0f, 10.0f, 1.5f, 2.0f); + + /* + * Add agents, specifying their start position, and store their goals on the + * opposite side of the environment. + */ + for (size_t i = 0; i < 250; ++i) { + sim->addAgent(200.0f * + RVO::Vector2(std::cos(i * 2.0f * M_PI / 250.0f), + std::sin(i * 2.0f * M_PI / 250.0f))); + goals.push_back(-sim->getAgentPosition(i)); + } +} + +#if RVO_OUTPUT_TIME_AND_POSITIONS +void updateVisualization(RVO::RVOSimulator *sim) +{ + /* Output the current global time. */ + std::cout << sim->getGlobalTime(); + + /* Output the current position of all the agents. */ + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + std::cout << " " << sim->getAgentPosition(i); + } + + std::cout << std::endl; +} +#endif + +void setPreferredVelocities(RVO::RVOSimulator *sim) +{ + /* + * Set the preferred velocity to be a vector of unit magnitude (speed) in the + * direction of the goal. + */ +#ifdef _OPENMP +#pragma omp parallel for +#endif + for (int i = 0; i < static_cast(sim->getNumAgents()); ++i) { + RVO::Vector2 goalVector = goals[i] - sim->getAgentPosition(i); + + if (RVO::absSq(goalVector) > 1.0f) { + goalVector = RVO::normalize(goalVector); + } + + sim->setAgentPrefVelocity(i, goalVector); + } +} + +bool reachedGoal(RVO::RVOSimulator *sim) +{ + /* Check if all agents have reached their goals. */ + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (RVO::absSq(sim->getAgentPosition(i) - goals[i]) > sim->getAgentRadius(i) * sim->getAgentRadius(i)) { + return false; + } + } + + return true; +} + +int main() +{ + /* Create a new simulator instance. */ + RVO::RVOSimulator *sim = new RVO::RVOSimulator(); + + /* Set up the scenario. */ + setupScenario(sim); + + /* Perform (and manipulate) the simulation. */ + do { +#if RVO_OUTPUT_TIME_AND_POSITIONS + updateVisualization(sim); +#endif + setPreferredVelocities(sim); + sim->doStep(); + } + while (!reachedGoal(sim)); + + delete sim; + + return 0; +} diff --git a/examples/Roadmap.cpp b/examples/Roadmap.cpp new file mode 100644 index 0000000..811f054 --- /dev/null +++ b/examples/Roadmap.cpp @@ -0,0 +1,380 @@ +/* + * Roadmap.cpp + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +/* + * Example file showing a demo with 100 agents split in four groups initially + * positioned in four corners of the environment. Each agent attempts to move to + * other side of the environment through a narrow passage generated by four + * obstacles. There is a roadmap to guide the agents around the obstacles. + */ + +#define RVO_OUTPUT_TIME_AND_POSITIONS 0 +#define RVO_SEED_RANDOM_NUMBER_GENERATOR 0 + +#include +#include +#include +#include + +#if RVO_OUTPUT_TIME_AND_POSITIONS +#include +#endif + +#if RVO_SEED_RANDOM_NUMBER_GENERATOR +#include +#endif + +#if _OPENMP +#include +#endif + +#include + +#ifndef M_PI +const float M_PI = 3.14159265358979323846f; +#endif + +class RoadmapVertex { +public: + RVO::Vector2 position; + std::vector neighbors; + std::vector distToGoal; +}; + +/* Store the roadmap. */ +std::vector roadmap; + +/* Store the goals of the agents. */ +std::vector goals; + +void setupScenario(RVO::RVOSimulator *sim) +{ +#if RVO_SEED_RANDOM_NUMBER_GENERATOR + std::srand(static_cast(std::time(NULL))); +#endif + + /* Specify the global time step of the simulation. */ + sim->setTimeStep(0.25f); + + /* + * Add (polygonal) obstacles, specifying their vertices in counterclockwise + * order. + */ + std::vector obstacle1, obstacle2, obstacle3, obstacle4; + + obstacle1.push_back(RVO::Vector2(-10.0f, 40.0f)); + obstacle1.push_back(RVO::Vector2(-40.0f, 40.0f)); + obstacle1.push_back(RVO::Vector2(-40.0f, 10.0f)); + obstacle1.push_back(RVO::Vector2(-10.0f, 10.0f)); + + obstacle2.push_back(RVO::Vector2(10.0f, 40.0f)); + obstacle2.push_back(RVO::Vector2(10.0f, 10.0f)); + obstacle2.push_back(RVO::Vector2(40.0f, 10.0f)); + obstacle2.push_back(RVO::Vector2(40.0f, 40.0f)); + + obstacle3.push_back(RVO::Vector2(10.0f, -40.0f)); + obstacle3.push_back(RVO::Vector2(40.0f, -40.0f)); + obstacle3.push_back(RVO::Vector2(40.0f, -10.0f)); + obstacle3.push_back(RVO::Vector2(10.0f, -10.0f)); + + obstacle4.push_back(RVO::Vector2(-10.0f, -40.0f)); + obstacle4.push_back(RVO::Vector2(-10.0f, -10.0f)); + obstacle4.push_back(RVO::Vector2(-40.0f, -10.0f)); + obstacle4.push_back(RVO::Vector2(-40.0f, -40.0f)); + + sim->addObstacle(obstacle1); + sim->addObstacle(obstacle2); + sim->addObstacle(obstacle3); + sim->addObstacle(obstacle4); + + /* Process the obstacles so that they are accounted for in the simulation. */ + sim->processObstacles(); + + /* Add roadmap vertices. */ + RoadmapVertex v; + + /* Add the goal positions of agents. */ + v.position = RVO::Vector2(-75.0f, -75.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(75.0f, -75.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(-75.0f, 75.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(75.0f, 75.0f); + roadmap.push_back(v); + + /* Add roadmap vertices around the obstacles. */ + v.position = RVO::Vector2(-42.0f, -42.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(-42.0f, -8.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(-42.0f, 8.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(-42.0f, 42.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(-8.0f, -42.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(-8.0f, -8.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(-8.0f, 8.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(-8.0f, 42.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(8.0f, -42.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(8.0f, -8.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(8.0f, 8.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(8.0f, 42.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(42.0f, -42.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(42.0f, -8.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(42.0f, 8.0f); + roadmap.push_back(v); + v.position = RVO::Vector2(42.0f, 42.0f); + roadmap.push_back(v); + + /* Specify the default parameters for agents that are subsequently added. */ + sim->setAgentDefaults(15.0f, 10, 5.0f, 5.0f, 2.0f, 2.0f); + + /* + * Add agents, specifying their start position, and store goals on the + * opposite side of the environment (roadmap vertices). + */ + for (size_t i = 0; i < 5; ++i) { + for (size_t j = 0; j < 5; ++j) { + sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, 55.0f + j * 10.0f)); + goals.push_back(0); + + sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, 55.0f + j * 10.0f)); + goals.push_back(1); + + sim->addAgent(RVO::Vector2(55.0f + i * 10.0f, -55.0f - j * 10.0f)); + goals.push_back(2); + + sim->addAgent(RVO::Vector2(-55.0f - i * 10.0f, -55.0f - j * 10.0f)); + goals.push_back(3); + } + } +} + +#if RVO_OUTPUT_TIME_AND_POSITIONS +void updateVisualization(RVO::RVOSimulator *sim) +{ + /* Output the current global time. */ + std::cout << sim->getGlobalTime(); + + /* Output the current position of all the agents. */ + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + std::cout << " " << sim->getAgentPosition(i); + } + + std::cout << std::endl; +} +#endif + +void buildRoadmap(RVO::RVOSimulator *sim) +{ + /* Connect the roadmap vertices by edges if mutually visible. */ +#ifdef _OPENMP +#pragma omp parallel for +#endif + for (int i = 0; i < static_cast(roadmap.size()); ++i) { + for (int j = 0; j < static_cast(roadmap.size()); ++j) { + if (sim->queryVisibility(roadmap[i].position, roadmap[j].position, sim->getAgentRadius(0))) { + roadmap[i].neighbors.push_back(j); + } + } + + /* + * Initialize the distance to each of the four goal vertices at infinity + * (9e9f). + */ + roadmap[i].distToGoal.resize(4, 9e9f); + } + + /* + * Compute the distance to each of the four goals (the first four vertices) + * for all vertices using Dijkstra's algorithm. + */ +#ifdef _OPENMP +#pragma omp parallel for +#endif + for (int i = 0; i < 4; ++i) { + std::multimap Q; + std::vector::iterator> posInQ(roadmap.size(), Q.end()); + + roadmap[i].distToGoal[i] = 0.0f; + posInQ[i] = Q.insert(std::make_pair(0.0f, i)); + + while (!Q.empty()) { + const int u = Q.begin()->second; + Q.erase(Q.begin()); + posInQ[u] = Q.end(); + + for (int j = 0; j < static_cast(roadmap[u].neighbors.size()); ++j) { + const int v = roadmap[u].neighbors[j]; + const float dist_uv = RVO::abs(roadmap[v].position - roadmap[u].position); + + if (roadmap[v].distToGoal[i] > roadmap[u].distToGoal[i] + dist_uv) { + roadmap[v].distToGoal[i] = roadmap[u].distToGoal[i] + dist_uv; + + if (posInQ[v] == Q.end()) { + posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v)); + } + else { + Q.erase(posInQ[v]); + posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v)); + } + } + } + } + } +} + +void setPreferredVelocities(RVO::RVOSimulator *sim) +{ + /* + * Set the preferred velocity to be a vector of unit magnitude (speed) in the + * direction of the visible roadmap vertex that is on the shortest path to the + * goal. + */ +#ifdef _OPENMP +#pragma omp parallel for +#endif + for (int i = 0; i < static_cast(sim->getNumAgents()); ++i) { + float minDist = 9e9f; + int minVertex = -1; + + for (int j = 0; j < static_cast(roadmap.size()); ++j) { + if (RVO::abs(roadmap[j].position - sim->getAgentPosition(i)) + roadmap[j].distToGoal[goals[i]] < minDist && + sim->queryVisibility(sim->getAgentPosition(i), roadmap[j].position, sim->getAgentRadius(i))) { + + minDist = RVO::abs(roadmap[j].position - sim->getAgentPosition(i)) + roadmap[j].distToGoal[goals[i]]; + minVertex = j; + } + } + + if (minVertex == -1) { + /* No roadmap vertex is visible; should not happen. */ + sim->setAgentPrefVelocity(i, RVO::Vector2(0, 0)); + } + else { + if (RVO::absSq(roadmap[minVertex].position - + sim->getAgentPosition(i)) == 0.0f) { + if (minVertex == goals[i]) { + sim->setAgentPrefVelocity(i, RVO::Vector2()); + } + else { + sim->setAgentPrefVelocity(i, RVO::normalize(roadmap[goals[i]].position - sim->getAgentPosition(i))); + } + } + else { + sim->setAgentPrefVelocity(i, RVO::normalize(roadmap[minVertex].position - sim->getAgentPosition(i))); + } + } + + /* + * Perturb a little to avoid deadlocks due to perfect symmetry. + */ + float angle = std::rand() * 2.0f * M_PI / RAND_MAX; + float dist = std::rand() * 0.0001f / RAND_MAX; + + sim->setAgentPrefVelocity(i, sim->getAgentPrefVelocity(i) + + dist * RVO::Vector2(std::cos(angle), std::sin(angle))); + } +} + +bool reachedGoal(RVO::RVOSimulator *sim) +{ + /* Check if all agents have reached their goals. */ + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (RVO::absSq(sim->getAgentPosition(i) - roadmap[goals[i]].position) > 20.0f * 20.0f) { + return false; + } + } + + return true; +} + +int main() +{ + /* Create a new simulator instance. */ + RVO::RVOSimulator *sim = new RVO::RVOSimulator(); + + /* Set up the scenario. */ + setupScenario(sim); + + /* Build the roadmap. */ + buildRoadmap(sim); + + /* Perform (and manipulate) the simulation. */ + do { +#if RVO_OUTPUT_TIME_AND_POSITIONS + updateVisualization(sim); +#endif + setPreferredVelocities(sim); + sim->doStep(); + } + while (!reachedGoal(sim)); + + delete sim; + + return 0; +} diff --git a/src/Agent.cpp b/src/Agent.cpp new file mode 100644 index 0000000..c463cf3 --- /dev/null +++ b/src/Agent.cpp @@ -0,0 +1,590 @@ +/* + * Agent.cpp + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "Agent.h" + +#include "KdTree.h" +#include "Obstacle.h" + +namespace RVO { + Agent::Agent(RVOSimulator *sim) : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), sim_(sim), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { } + + void Agent::computeNeighbors() + { + obstacleNeighbors_.clear(); + float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_); + sim_->kdTree_->computeObstacleNeighbors(this, rangeSq); + + agentNeighbors_.clear(); + + if (maxNeighbors_ > 0) { + rangeSq = sqr(neighborDist_); + sim_->kdTree_->computeAgentNeighbors(this, rangeSq); + } + } + + /* Search for the best new velocity. */ + void Agent::computeNewVelocity() + { + orcaLines_.clear(); + + const float invTimeHorizonObst = 1.0f / timeHorizonObst_; + + /* Create obstacle ORCA lines. */ + for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) { + + const Obstacle *obstacle1 = obstacleNeighbors_[i].second; + const Obstacle *obstacle2 = obstacle1->nextObstacle_; + + const Vector2 relativePosition1 = obstacle1->point_ - position_; + const Vector2 relativePosition2 = obstacle2->point_ - position_; + + /* + * Check if velocity obstacle of obstacle is already taken care of by + * previously constructed obstacle ORCA lines. + */ + bool alreadyCovered = false; + + for (size_t j = 0; j < orcaLines_.size(); ++j) { + if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) { + alreadyCovered = true; + break; + } + } + + if (alreadyCovered) { + continue; + } + + /* Not yet covered. Check for collisions. */ + + const float distSq1 = absSq(relativePosition1); + const float distSq2 = absSq(relativePosition2); + + const float radiusSq = sqr(radius_); + + const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_; + const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector); + const float distSqLine = absSq(-relativePosition1 - s * obstacleVector); + + Line line; + + if (s < 0.0f && distSq1 <= radiusSq) { + /* Collision with left vertex. Ignore if non-convex. */ + if (obstacle1->isConvex_) { + line.point = Vector2(0.0f, 0.0f); + line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x())); + orcaLines_.push_back(line); + } + + continue; + } + else if (s > 1.0f && distSq2 <= radiusSq) { + /* Collision with right vertex. Ignore if non-convex + * or if it will be taken care of by neighoring obstace */ + if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) { + line.point = Vector2(0.0f, 0.0f); + line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x())); + orcaLines_.push_back(line); + } + + continue; + } + else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) { + /* Collision with obstacle segment. */ + line.point = Vector2(0.0f, 0.0f); + line.direction = -obstacle1->unitDir_; + orcaLines_.push_back(line); + continue; + } + + /* + * No collision. + * Compute legs. When obliquely viewed, both legs can come from a single + * vertex. Legs extend cut-off line when nonconvex vertex. + */ + + Vector2 leftLegDirection, rightLegDirection; + + if (s < 0.0f && distSqLine <= radiusSq) { + /* + * Obstacle viewed obliquely so that left vertex + * defines velocity obstacle. + */ + if (!obstacle1->isConvex_) { + /* Ignore obstacle. */ + continue; + } + + obstacle2 = obstacle1; + + const float leg1 = std::sqrt(distSq1 - radiusSq); + leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; + rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; + } + else if (s > 1.0f && distSqLine <= radiusSq) { + /* + * Obstacle viewed obliquely so that + * right vertex defines velocity obstacle. + */ + if (!obstacle2->isConvex_) { + /* Ignore obstacle. */ + continue; + } + + obstacle1 = obstacle2; + + const float leg2 = std::sqrt(distSq2 - radiusSq); + leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; + rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; + } + else { + /* Usual situation. */ + if (obstacle1->isConvex_) { + const float leg1 = std::sqrt(distSq1 - radiusSq); + leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1; + } + else { + /* Left vertex non-convex; left leg extends cut-off line. */ + leftLegDirection = -obstacle1->unitDir_; + } + + if (obstacle2->isConvex_) { + const float leg2 = std::sqrt(distSq2 - radiusSq); + rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2; + } + else { + /* Right vertex non-convex; right leg extends cut-off line. */ + rightLegDirection = obstacle1->unitDir_; + } + } + + /* + * Legs can never point into neighboring edge when convex vertex, + * take cutoff-line of neighboring edge instead. If velocity projected on + * "foreign" leg, no constraint is added. + */ + + const Obstacle *const leftNeighbor = obstacle1->prevObstacle_; + + bool isLeftLegForeign = false; + bool isRightLegForeign = false; + + if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) { + /* Left leg points into obstacle. */ + leftLegDirection = -leftNeighbor->unitDir_; + isLeftLegForeign = true; + } + + if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) { + /* Right leg points into obstacle. */ + rightLegDirection = obstacle2->unitDir_; + isRightLegForeign = true; + } + + /* Compute cut-off centers. */ + const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_); + const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_); + const Vector2 cutoffVec = rightCutoff - leftCutoff; + + /* Project current velocity on velocity obstacle. */ + + /* Check if current velocity is projected on cutoff circles. */ + const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec)); + const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection); + const float tRight = ((velocity_ - rightCutoff) * rightLegDirection); + + if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) { + /* Project on left cut-off circle. */ + const Vector2 unitW = normalize(velocity_ - leftCutoff); + + line.direction = Vector2(unitW.y(), -unitW.x()); + line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW; + orcaLines_.push_back(line); + continue; + } + else if (t > 1.0f && tRight < 0.0f) { + /* Project on right cut-off circle. */ + const Vector2 unitW = normalize(velocity_ - rightCutoff); + + line.direction = Vector2(unitW.y(), -unitW.x()); + line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW; + orcaLines_.push_back(line); + continue; + } + + /* + * Project on left leg, right leg, or cut-off line, whichever is closest + * to velocity. + */ + const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec))); + const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection))); + const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection))); + + if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) { + /* Project on cut-off line. */ + line.direction = -obstacle1->unitDir_; + line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); + orcaLines_.push_back(line); + continue; + } + else if (distSqLeft <= distSqRight) { + /* Project on left leg. */ + if (isLeftLegForeign) { + continue; + } + + line.direction = leftLegDirection; + line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); + orcaLines_.push_back(line); + continue; + } + else { + /* Project on right leg. */ + if (isRightLegForeign) { + continue; + } + + line.direction = -rightLegDirection; + line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x()); + orcaLines_.push_back(line); + continue; + } + } + + const size_t numObstLines = orcaLines_.size(); + + const float invTimeHorizon = 1.0f / timeHorizon_; + + /* Create agent ORCA lines. */ + for (size_t i = 0; i < agentNeighbors_.size(); ++i) { + const Agent *const other = agentNeighbors_[i].second; + + const Vector2 relativePosition = other->position_ - position_; + const Vector2 relativeVelocity = velocity_ - other->velocity_; + const float distSq = absSq(relativePosition); + const float combinedRadius = radius_ + other->radius_; + const float combinedRadiusSq = sqr(combinedRadius); + + Line line; + Vector2 u; + + if (distSq > combinedRadiusSq) { + /* No collision. */ + const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition; + /* Vector from cutoff center to relative velocity. */ + const float wLengthSq = absSq(w); + + const float dotProduct1 = w * relativePosition; + + if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) { + /* Project on cut-off circle. */ + const float wLength = std::sqrt(wLengthSq); + const Vector2 unitW = w / wLength; + + line.direction = Vector2(unitW.y(), -unitW.x()); + u = (combinedRadius * invTimeHorizon - wLength) * unitW; + } + else { + /* Project on legs. */ + const float leg = std::sqrt(distSq - combinedRadiusSq); + + if (det(relativePosition, w) > 0.0f) { + /* Project on left leg. */ + line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; + } + else { + /* Project on right leg. */ + line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq; + } + + const float dotProduct2 = relativeVelocity * line.direction; + + u = dotProduct2 * line.direction - relativeVelocity; + } + } + else { + /* Collision. Project on cut-off circle of time timeStep. */ + const float invTimeStep = 1.0f / sim_->timeStep_; + + /* Vector from cutoff center to relative velocity. */ + const Vector2 w = relativeVelocity - invTimeStep * relativePosition; + + const float wLength = abs(w); + const Vector2 unitW = w / wLength; + + line.direction = Vector2(unitW.y(), -unitW.x()); + u = (combinedRadius * invTimeStep - wLength) * unitW; + } + + line.point = velocity_ + 0.5f * u; + orcaLines_.push_back(line); + } + + size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_); + + if (lineFail < orcaLines_.size()) { + linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_); + } + } + + void Agent::insertAgentNeighbor(const Agent *agent, float &rangeSq) + { + if (this != agent) { + const float distSq = absSq(position_ - agent->position_); + + if (distSq < rangeSq) { + if (agentNeighbors_.size() < maxNeighbors_) { + agentNeighbors_.push_back(std::make_pair(distSq, agent)); + } + + size_t i = agentNeighbors_.size() - 1; + + while (i != 0 && distSq < agentNeighbors_[i - 1].first) { + agentNeighbors_[i] = agentNeighbors_[i - 1]; + --i; + } + + agentNeighbors_[i] = std::make_pair(distSq, agent); + + if (agentNeighbors_.size() == maxNeighbors_) { + rangeSq = agentNeighbors_.back().first; + } + } + } + } + + void Agent::insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq) + { + const Obstacle *const nextObstacle = obstacle->nextObstacle_; + + const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_); + + if (distSq < rangeSq) { + obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle)); + + size_t i = obstacleNeighbors_.size() - 1; + + while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) { + obstacleNeighbors_[i] = obstacleNeighbors_[i - 1]; + --i; + } + + obstacleNeighbors_[i] = std::make_pair(distSq, obstacle); + } + } + + void Agent::update() + { + velocity_ = newVelocity_; + position_ += velocity_ * sim_->timeStep_; + } + + bool linearProgram1(const std::vector &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) + { + const float dotProduct = lines[lineNo].point * lines[lineNo].direction; + const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point); + + if (discriminant < 0.0f) { + /* Max speed circle fully invalidates line lineNo. */ + return false; + } + + const float sqrtDiscriminant = std::sqrt(discriminant); + float tLeft = -dotProduct - sqrtDiscriminant; + float tRight = -dotProduct + sqrtDiscriminant; + + for (size_t i = 0; i < lineNo; ++i) { + const float denominator = det(lines[lineNo].direction, lines[i].direction); + const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point); + + if (std::fabs(denominator) <= RVO_EPSILON) { + /* Lines lineNo and i are (almost) parallel. */ + if (numerator < 0.0f) { + return false; + } + else { + continue; + } + } + + const float t = numerator / denominator; + + if (denominator >= 0.0f) { + /* Line i bounds line lineNo on the right. */ + tRight = std::min(tRight, t); + } + else { + /* Line i bounds line lineNo on the left. */ + tLeft = std::max(tLeft, t); + } + + if (tLeft > tRight) { + return false; + } + } + + if (directionOpt) { + /* Optimize direction. */ + if (optVelocity * lines[lineNo].direction > 0.0f) { + /* Take right extreme. */ + result = lines[lineNo].point + tRight * lines[lineNo].direction; + } + else { + /* Take left extreme. */ + result = lines[lineNo].point + tLeft * lines[lineNo].direction; + } + } + else { + /* Optimize closest point. */ + const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point); + + if (t < tLeft) { + result = lines[lineNo].point + tLeft * lines[lineNo].direction; + } + else if (t > tRight) { + result = lines[lineNo].point + tRight * lines[lineNo].direction; + } + else { + result = lines[lineNo].point + t * lines[lineNo].direction; + } + } + + return true; + } + + size_t linearProgram2(const std::vector &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result) + { + if (directionOpt) { + /* + * Optimize direction. Note that the optimization velocity is of unit + * length in this case. + */ + result = optVelocity * radius; + } + else if (absSq(optVelocity) > sqr(radius)) { + /* Optimize closest point and outside circle. */ + result = normalize(optVelocity) * radius; + } + else { + /* Optimize closest point and inside circle. */ + result = optVelocity; + } + + for (size_t i = 0; i < lines.size(); ++i) { + if (det(lines[i].direction, lines[i].point - result) > 0.0f) { + /* Result does not satisfy constraint i. Compute new optimal result. */ + const Vector2 tempResult = result; + + if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) { + result = tempResult; + return i; + } + } + } + + return lines.size(); + } + + void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result) + { + float distance = 0.0f; + + for (size_t i = beginLine; i < lines.size(); ++i) { + if (det(lines[i].direction, lines[i].point - result) > distance) { + /* Result does not satisfy constraint of line i. */ + std::vector projLines(lines.begin(), lines.begin() + static_cast(numObstLines)); + + for (size_t j = numObstLines; j < i; ++j) { + Line line; + + float determinant = det(lines[i].direction, lines[j].direction); + + if (std::fabs(determinant) <= RVO_EPSILON) { + /* Line i and line j are parallel. */ + if (lines[i].direction * lines[j].direction > 0.0f) { + /* Line i and line j point in the same direction. */ + continue; + } + else { + /* Line i and line j point in opposite direction. */ + line.point = 0.5f * (lines[i].point + lines[j].point); + } + } + else { + line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; + } + + line.direction = normalize(lines[j].direction - lines[i].direction); + projLines.push_back(line); + } + + const Vector2 tempResult = result; + + if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) { + /* This should in principle not happen. The result is by definition + * already in the feasible region of this linear program. If it fails, + * it is due to small floating point error, and the current result is + * kept. + */ + result = tempResult; + } + + distance = det(lines[i].direction, lines[i].point - result); + } + } + } +} diff --git a/src/Agent.h b/src/Agent.h new file mode 100644 index 0000000..4ef952c --- /dev/null +++ b/src/Agent.h @@ -0,0 +1,180 @@ +/* + * Agent.h + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO_AGENT_H_ +#define RVO_AGENT_H_ + +/** + * \file Agent.h + * \brief Contains the Agent class. + */ + +#include "Definitions.h" +#include "RVOSimulator.h" + +namespace RVO { + /** + * \brief Defines an agent in the simulation. + */ + class Agent { + private: + /** + * \brief Constructs an agent instance. + * \param sim The simulator instance. + */ + explicit Agent(RVOSimulator *sim); + + /** + * \brief Computes the neighbors of this agent. + */ + void computeNeighbors(); + + /** + * \brief Computes the new velocity of this agent. + */ + void computeNewVelocity(); + + /** + * \brief Inserts an agent neighbor into the set of neighbors of + * this agent. + * \param agent A pointer to the agent to be inserted. + * \param rangeSq The squared range around this agent. + */ + void insertAgentNeighbor(const Agent *agent, float &rangeSq); + + /** + * \brief Inserts a static obstacle neighbor into the set of neighbors + * of this agent. + * \param obstacle The number of the static obstacle to be + * inserted. + * \param rangeSq The squared range around this agent. + */ + void insertObstacleNeighbor(const Obstacle *obstacle, float rangeSq); + + /** + * \brief Updates the two-dimensional position and two-dimensional + * velocity of this agent. + */ + void update(); + + std::vector > agentNeighbors_; + size_t maxNeighbors_; + float maxSpeed_; + float neighborDist_; + Vector2 newVelocity_; + std::vector > obstacleNeighbors_; + std::vector orcaLines_; + Vector2 position_; + Vector2 prefVelocity_; + float radius_; + RVOSimulator *sim_; + float timeHorizon_; + float timeHorizonObst_; + Vector2 velocity_; + + size_t id_; + + friend class KdTree; + friend class RVOSimulator; + }; + + /** + * \relates Agent + * \brief Solves a one-dimensional linear program on a specified line + * subject to linear constraints defined by lines and a circular + * constraint. + * \param lines Lines defining the linear constraints. + * \param lineNo The specified line constraint. + * \param radius The radius of the circular constraint. + * \param optVelocity The optimization velocity. + * \param directionOpt True if the direction should be optimized. + * \param result A reference to the result of the linear program. + * \return True if successful. + */ + bool linearProgram1(const std::vector &lines, size_t lineNo, + float radius, const Vector2 &optVelocity, + bool directionOpt, Vector2 &result); + + /** + * \relates Agent + * \brief Solves a two-dimensional linear program subject to linear + * constraints defined by lines and a circular constraint. + * \param lines Lines defining the linear constraints. + * \param radius The radius of the circular constraint. + * \param optVelocity The optimization velocity. + * \param directionOpt True if the direction should be optimized. + * \param result A reference to the result of the linear program. + * \return The number of the line it fails on, and the number of lines if successful. + */ + size_t linearProgram2(const std::vector &lines, float radius, + const Vector2 &optVelocity, bool directionOpt, + Vector2 &result); + + /** + * \relates Agent + * \brief Solves a two-dimensional linear program subject to linear + * constraints defined by lines and a circular constraint. + * \param lines Lines defining the linear constraints. + * \param numObstLines Count of obstacle lines. + * \param beginLine The line on which the 2-d linear program failed. + * \param radius The radius of the circular constraint. + * \param result A reference to the result of the linear program. + */ + void linearProgram3(const std::vector &lines, size_t numObstLines, size_t beginLine, + float radius, Vector2 &result); +} + +#endif /* RVO_AGENT_H_ */ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..8e41600 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,75 @@ +# +# src/CMakeLists.txt +# RVO2 Library +# +# Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. +# All rights reserved. +# +# Permission to use, copy, modify, and distribute this software and its +# documentation for educational, research, and non-profit purposes, without fee, +# and without a written agreement is hereby granted, provided that the above +# copyright notice, this paragraph, and the following four paragraphs appear in +# all copies. +# +# Permission to incorporate this software into commercial products may be +# obtained by contacting the authors or the Office of +# Technology Development at the University of North Carolina at Chapel Hill +# . +# +# This software program and documentation are copyrighted by the University of +# North Carolina at Chapel Hill. The software program and documentation are +# supplied "as is," without any accompanying services from the University of +# North Carolina at Chapel Hill or the authors. The University of North Carolina +# at Chapel Hill and the authors do not warrant that the operation of the +# program will be uninterrupted or error-free. The end-user understands that the +# program was developed for research purposes and is advised not to rely +# exclusively on the program for any reason. +# +# IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE +# AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR +# CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS +# SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT +# CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH +# DAMAGE. +# +# THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY +# DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +# OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY +# WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" +# BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS +# HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR +# MODIFICATIONS. +# +# Please send all bug reports to . +# +# The authors may be contacted via: +# +# Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha +# Dept. of Computer Science +# 201 S. Columbia St. +# Frederick P. Brooks, Jr. Computer Science Bldg. +# Chapel Hill, N.C. 27599-3175 +# United States of America +# +# +# + +set(RVO_HEADERS + RVO.h + RVOSimulator.h + Vector2.h) + +set(RVO_SOURCES + Agent.cpp + Agent.h + Definitions.h + KdTree.cpp + KdTree.h + Obstacle.cpp + Obstacle.h + RVOSimulator.cpp) + +add_library(RVO ${RVO_HEADERS} ${RVO_SOURCES}) + +install(FILES ${RVO_HEADERS} DESTINATION include) +install(TARGETS RVO DESTINATION lib) diff --git a/src/Definitions.h b/src/Definitions.h new file mode 100644 index 0000000..6b6ec5e --- /dev/null +++ b/src/Definitions.h @@ -0,0 +1,132 @@ +/* + * Definitions.h + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO_DEFINITIONS_H_ +#define RVO_DEFINITIONS_H_ + +/** + * \file Definitions.h + * \brief Contains functions and constants used in multiple classes. + */ + +#include +#include +#include +#include + +#include "Vector2.h" + +/** + * \brief A sufficiently small positive number. + */ +const float RVO_EPSILON = 0.00001f; + +namespace RVO { + class Agent; + class Obstacle; + class RVOSimulator; + + /** + * \brief Computes the squared distance from a line segment with the + * specified endpoints to a specified point. + * \param a The first endpoint of the line segment. + * \param b The second endpoint of the line segment. + * \param c The point to which the squared distance is to + * be calculated. + * \return The squared distance from the line segment to the point. + */ + inline float distSqPointLineSegment(const Vector2 &a, const Vector2 &b, + const Vector2 &c) + { + const float r = ((c - a) * (b - a)) / absSq(b - a); + + if (r < 0.0f) { + return absSq(c - a); + } + else if (r > 1.0f) { + return absSq(c - b); + } + else { + return absSq(c - (a + r * (b - a))); + } + } + + /** + * \brief Computes the signed distance from a line connecting the + * specified points to a specified point. + * \param a The first point on the line. + * \param b The second point on the line. + * \param c The point to which the signed distance is to + * be calculated. + * \return Positive when the point c lies to the left of the line ab. + */ + inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c) + { + return det(a - c, b - a); + } + + /** + * \brief Computes the square of a float. + * \param a The float to be squared. + * \return The square of the float. + */ + inline float sqr(float a) + { + return a * a; + } +} + +#endif /* RVO_DEFINITIONS_H_ */ diff --git a/src/KdTree.cpp b/src/KdTree.cpp new file mode 100644 index 0000000..87261fa --- /dev/null +++ b/src/KdTree.cpp @@ -0,0 +1,392 @@ +/* + * KdTree.cpp + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "KdTree.h" + +#include "Agent.h" +#include "RVOSimulator.h" +#include "Obstacle.h" + +namespace RVO { + KdTree::KdTree(RVOSimulator *sim) : obstacleTree_(NULL), sim_(sim) { } + + KdTree::~KdTree() + { + deleteObstacleTree(obstacleTree_); + } + + void KdTree::buildAgentTree() + { + if (agents_.size() < sim_->agents_.size()) { + for (size_t i = agents_.size(); i < sim_->agents_.size(); ++i) { + agents_.push_back(sim_->agents_[i]); + } + + agentTree_.resize(2 * agents_.size() - 1); + } + + if (!agents_.empty()) { + buildAgentTreeRecursive(0, agents_.size(), 0); + } + } + + void KdTree::buildAgentTreeRecursive(size_t begin, size_t end, size_t node) + { + agentTree_[node].begin = begin; + agentTree_[node].end = end; + agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x(); + agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y(); + + for (size_t i = begin + 1; i < end; ++i) { + agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x()); + agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x()); + agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y()); + agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y()); + } + + if (end - begin > MAX_LEAF_SIZE) { + /* No leaf node. */ + const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY); + const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY)); + + size_t left = begin; + size_t right = end; + + while (left < right) { + while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) { + ++left; + } + + while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) { + --right; + } + + if (left < right) { + std::swap(agents_[left], agents_[right - 1]); + ++left; + --right; + } + } + + if (left == begin) { + ++left; + ++right; + } + + agentTree_[node].left = node + 1; + agentTree_[node].right = node + 2 * (left - begin); + + buildAgentTreeRecursive(begin, left, agentTree_[node].left); + buildAgentTreeRecursive(left, end, agentTree_[node].right); + } + } + + void KdTree::buildObstacleTree() + { + deleteObstacleTree(obstacleTree_); + + std::vector obstacles(sim_->obstacles_.size()); + + for (size_t i = 0; i < sim_->obstacles_.size(); ++i) { + obstacles[i] = sim_->obstacles_[i]; + } + + obstacleTree_ = buildObstacleTreeRecursive(obstacles); + } + + + KdTree::ObstacleTreeNode *KdTree::buildObstacleTreeRecursive(const std::vector &obstacles) + { + if (obstacles.empty()) { + return NULL; + } + else { + ObstacleTreeNode *const node = new ObstacleTreeNode; + + size_t optimalSplit = 0; + size_t minLeft = obstacles.size(); + size_t minRight = obstacles.size(); + + for (size_t i = 0; i < obstacles.size(); ++i) { + size_t leftSize = 0; + size_t rightSize = 0; + + const Obstacle *const obstacleI1 = obstacles[i]; + const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; + + /* Compute optimal split node. */ + for (size_t j = 0; j < obstacles.size(); ++j) { + if (i == j) { + continue; + } + + const Obstacle *const obstacleJ1 = obstacles[j]; + const Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; + + const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); + const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); + + if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { + ++leftSize; + } + else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { + ++rightSize; + } + else { + ++leftSize; + ++rightSize; + } + + if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { + break; + } + } + + if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) { + minLeft = leftSize; + minRight = rightSize; + optimalSplit = i; + } + } + + /* Build split node. */ + std::vector leftObstacles(minLeft); + std::vector rightObstacles(minRight); + + size_t leftCounter = 0; + size_t rightCounter = 0; + const size_t i = optimalSplit; + + const Obstacle *const obstacleI1 = obstacles[i]; + const Obstacle *const obstacleI2 = obstacleI1->nextObstacle_; + + for (size_t j = 0; j < obstacles.size(); ++j) { + if (i == j) { + continue; + } + + Obstacle *const obstacleJ1 = obstacles[j]; + Obstacle *const obstacleJ2 = obstacleJ1->nextObstacle_; + + const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_); + const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_); + + if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) { + leftObstacles[leftCounter++] = obstacles[j]; + } + else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) { + rightObstacles[rightCounter++] = obstacles[j]; + } + else { + /* Split obstacle j. */ + const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_); + + const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_); + + Obstacle *const newObstacle = new Obstacle(); + newObstacle->point_ = splitpoint; + newObstacle->prevObstacle_ = obstacleJ1; + newObstacle->nextObstacle_ = obstacleJ2; + newObstacle->isConvex_ = true; + newObstacle->unitDir_ = obstacleJ1->unitDir_; + + newObstacle->id_ = sim_->obstacles_.size(); + + sim_->obstacles_.push_back(newObstacle); + + obstacleJ1->nextObstacle_ = newObstacle; + obstacleJ2->prevObstacle_ = newObstacle; + + if (j1LeftOfI > 0.0f) { + leftObstacles[leftCounter++] = obstacleJ1; + rightObstacles[rightCounter++] = newObstacle; + } + else { + rightObstacles[rightCounter++] = obstacleJ1; + leftObstacles[leftCounter++] = newObstacle; + } + } + } + + node->obstacle = obstacleI1; + node->left = buildObstacleTreeRecursive(leftObstacles); + node->right = buildObstacleTreeRecursive(rightObstacles); + return node; + } + } + + void KdTree::computeAgentNeighbors(Agent *agent, float &rangeSq) const + { + queryAgentTreeRecursive(agent, rangeSq, 0); + } + + void KdTree::computeObstacleNeighbors(Agent *agent, float rangeSq) const + { + queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); + } + + void KdTree::deleteObstacleTree(ObstacleTreeNode *node) + { + if (node != NULL) { + deleteObstacleTree(node->left); + deleteObstacleTree(node->right); + delete node; + } + } + + void KdTree::queryAgentTreeRecursive(Agent *agent, float &rangeSq, size_t node) const + { + if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) { + for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) { + agent->insertAgentNeighbor(agents_[i], rangeSq); + } + } + else { + const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY)); + + const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY)); + + if (distSqLeft < distSqRight) { + if (distSqLeft < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); + + if (distSqRight < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); + } + } + } + else { + if (distSqRight < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right); + + if (distSqLeft < rangeSq) { + queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left); + } + } + } + + } + } + + void KdTree::queryObstacleTreeRecursive(Agent *agent, float rangeSq, const ObstacleTreeNode *node) const + { + if (node == NULL) { + return; + } + else { + const Obstacle *const obstacle1 = node->obstacle; + const Obstacle *const obstacle2 = obstacle1->nextObstacle_; + + const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_); + + queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right)); + + const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_); + + if (distSqLine < rangeSq) { + if (agentLeftOfLine < 0.0f) { + /* + * Try obstacle at this node only if agent is on right side of + * obstacle (and can see obstacle). + */ + agent->insertObstacleNeighbor(node->obstacle, rangeSq); + } + + /* Try other side of line. */ + queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left)); + + } + } + } + + bool KdTree::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const + { + return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); + } + + bool KdTree::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const + { + if (node == NULL) { + return true; + } + else { + const Obstacle *const obstacle1 = node->obstacle; + const Obstacle *const obstacle2 = obstacle1->nextObstacle_; + + const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1); + const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2); + const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_); + + if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) { + return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right)); + } + else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) { + return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left)); + } + else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) { + /* One can see through obstacle from left to right. */ + return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right); + } + else { + const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_); + const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_); + const float invLengthQ = 1.0f / absSq(q2 - q1); + + return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right)); + } + } + } +} diff --git a/src/KdTree.h b/src/KdTree.h new file mode 100644 index 0000000..b5d6f07 --- /dev/null +++ b/src/KdTree.h @@ -0,0 +1,227 @@ +/* + * KdTree.h + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO_KD_TREE_H_ +#define RVO_KD_TREE_H_ + +/** + * \file KdTree.h + * \brief Contains the KdTree class. + */ + +#include "Definitions.h" + +namespace RVO { + /** + * \brief Defines kd-trees for agents and static obstacles in the + * simulation. + */ + class KdTree { + private: + /** + * \brief Defines an agent kd-tree node. + */ + class AgentTreeNode { + public: + /** + * \brief The beginning node number. + */ + size_t begin; + + /** + * \brief The ending node number. + */ + size_t end; + + /** + * \brief The left node number. + */ + size_t left; + + /** + * \brief The maximum x-coordinate. + */ + float maxX; + + /** + * \brief The maximum y-coordinate. + */ + float maxY; + + /** + * \brief The minimum x-coordinate. + */ + float minX; + + /** + * \brief The minimum y-coordinate. + */ + float minY; + + /** + * \brief The right node number. + */ + size_t right; + }; + + /** + * \brief Defines an obstacle kd-tree node. + */ + class ObstacleTreeNode { + public: + /** + * \brief The left obstacle tree node. + */ + ObstacleTreeNode *left; + + /** + * \brief The obstacle number. + */ + const Obstacle *obstacle; + + /** + * \brief The right obstacle tree node. + */ + ObstacleTreeNode *right; + }; + + /** + * \brief Constructs a kd-tree instance. + * \param sim The simulator instance. + */ + explicit KdTree(RVOSimulator *sim); + + /** + * \brief Destroys this kd-tree instance. + */ + ~KdTree(); + + /** + * \brief Builds an agent kd-tree. + */ + void buildAgentTree(); + + void buildAgentTreeRecursive(size_t begin, size_t end, size_t node); + + /** + * \brief Builds an obstacle kd-tree. + */ + void buildObstacleTree(); + + ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector & + obstacles); + + /** + * \brief Computes the agent neighbors of the specified agent. + * \param agent A pointer to the agent for which agent + * neighbors are to be computed. + * \param rangeSq The squared range around the agent. + */ + void computeAgentNeighbors(Agent *agent, float &rangeSq) const; + + /** + * \brief Computes the obstacle neighbors of the specified agent. + * \param agent A pointer to the agent for which obstacle + * neighbors are to be computed. + * \param rangeSq The squared range around the agent. + */ + void computeObstacleNeighbors(Agent *agent, float rangeSq) const; + + /** + * \brief Deletes the specified obstacle tree node. + * \param node A pointer to the obstacle tree node to be + * deleted. + */ + void deleteObstacleTree(ObstacleTreeNode *node); + + void queryAgentTreeRecursive(Agent *agent, float &rangeSq, + size_t node) const; + + void queryObstacleTreeRecursive(Agent *agent, float rangeSq, + const ObstacleTreeNode *node) const; + + /** + * \brief Queries the visibility between two points within a + * specified radius. + * \param q1 The first point between which visibility is + * to be tested. + * \param q2 The second point between which visibility is + * to be tested. + * \param radius The radius within which visibility is to be + * tested. + * \return True if q1 and q2 are mutually visible within the radius; + * false otherwise. + */ + bool queryVisibility(const Vector2 &q1, const Vector2 &q2, + float radius) const; + + bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, + float radius, + const ObstacleTreeNode *node) const; + + std::vector agents_; + std::vector agentTree_; + ObstacleTreeNode *obstacleTree_; + RVOSimulator *sim_; + + static const size_t MAX_LEAF_SIZE = 10; + + friend class Agent; + friend class RVOSimulator; + }; +} + +#endif /* RVO_KD_TREE_H_ */ diff --git a/src/Obstacle.cpp b/src/Obstacle.cpp new file mode 100644 index 0000000..fcf3ac2 --- /dev/null +++ b/src/Obstacle.cpp @@ -0,0 +1,62 @@ +/* + * Obstacle.cpp + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "Obstacle.h" +#include "RVOSimulator.h" + +namespace RVO { + Obstacle::Obstacle() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { } +} diff --git a/src/Obstacle.h b/src/Obstacle.h new file mode 100644 index 0000000..242698c --- /dev/null +++ b/src/Obstacle.h @@ -0,0 +1,92 @@ +/* + * Obstacle.h + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO_OBSTACLE_H_ +#define RVO_OBSTACLE_H_ + +/** + * \file Obstacle.h + * \brief Contains the Obstacle class. + */ + +#include "Definitions.h" + +namespace RVO { + /** + * \brief Defines static obstacles in the simulation. + */ + class Obstacle { + private: + /** + * \brief Constructs a static obstacle instance. + */ + Obstacle(); + + bool isConvex_; + Obstacle *nextObstacle_; + Vector2 point_; + Obstacle *prevObstacle_; + Vector2 unitDir_; + + size_t id_; + + friend class Agent; + friend class KdTree; + friend class RVOSimulator; + }; +} + +#endif /* RVO_OBSTACLE_H_ */ diff --git a/src/RVO.h b/src/RVO.h new file mode 100644 index 0000000..8a12088 --- /dev/null +++ b/src/RVO.h @@ -0,0 +1,557 @@ +/* + * RVO.h + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO_RVO_H_ +#define RVO_RVO_H_ + +#include "RVOSimulator.h" +#include "Vector2.h" + +/** + + \file RVO.h + \brief Includes all public headers in the library. + + \namespace RVO + \brief Contains all classes, functions, and constants used in the library. + + \mainpage RVO2 Library + + \author Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, and + Dinesh Manocha + + RVO2 Library is an easy-to-use C++ implementation of the + Optimal Reciprocal Collision Avoidance + (ORCA) formulation for multi-agent simulation. RVO2 Library automatically + uses parallelism for computing the motion of the agents if your machine has + multiple processors and your compiler supports + OpenMP. + + Please follow the following steps to install and use RVO2 Library. + + - \subpage whatsnew + - \subpage building + - \subpage using + - \subpage params + + See the documentation of the RVO::RVOSimulator class for an exhaustive list of + public functions of RVO2 Library. + + RVO2 Library, accompanying example code, and this documentation is + released for educational, research, and non-profit purposes under the following + \subpage terms "terms and conditions". + + + \page whatsnew What Is New in RVO2 Library + + \section localca Local Collision Avoidance + + The main difference between RVO2 Library and %RVO Library 1.x is the + local collision avoidance technique used. RVO2 Library uses + Optimal Reciprocal Collision Avoidance + (ORCA), whereas %RVO Library 1.x uses + Reciprocal Velocity Obstacles (%RVO). For legacy reasons, and since both + techniques are based on the same principles of reciprocal collision avoidance + and relative velocity, we did not change the name of the library. + + A major consequence of the change of local collision avoidance technique is that + the simulation has become much faster in RVO2 Library. ORCA defines + velocity constraints with respect to other agents as half-planes, and an optimal + velocity is efficiently found using (two-dimensional) linear programming. In + contrast, %RVO Library 1.x uses random sampling to find a good velocity. Also, + the behavior of the agents is smoother in RVO2 Library. It is proven + mathematically that ORCA lets the velocity of agents evolve continuously over + time, whereas %RVO Library 1.x occasionally showed oscillations and reciprocal + dances. Furthermore, ORCA provides stronger guarantees with respect to collision + avoidance. + + \section global Global Path Planning + + Local collision avoidance as provided by RVO2 Library should in principle + be accompanied by global path planning that determines the preferred velocity of + each agent in each time step of the simulation. %RVO Library 1.x has a built-in + roadmap infrastructure to guide agents around obstacles to fixed goals. + However, besides roadmaps, other techniques for global planning, such as + navigation fields, cell decompositions, etc. exist. Therefore, RVO2 + Library does not provide global planning infrastructure anymore. Instead, + it is the responsibility of the external application to set the preferred + velocity of each agent ahead of each time step of the simulation. This makes the + library more flexible to use in varying application domains. In one of the + example applications that comes with RVO2 Library, we show how a roadmap + similar to %RVO Library 1.x is used externally to guide the global navigation of + the agents. As a consequence of this change, RVO2 Library does not have a + concept of a "goal position" or "preferred speed" for each + agent, but only relies on the preferred velocities of the agents set by the + external application. + + \section structure Structure of RVO2 Library + + The structure of RVO2 Library is similar to that of %RVO Library 1.x. + Users familiar with %RVO Library 1.x should find little trouble in using RVO2 + Library. However, RVO2 Library is not backwards compatible with %RVO + Library 1.x. The main reason for this is that the ORCA technique requires + different (and fewer) parameters to be set than %RVO. Also, the way obstacles + are represented is different. In %RVO Library 1.x, obstacles are represented by + an arbitrary collection of line segments. In RVO2 Library, obstacles are + non-intersecting polygons, specified by lists of vertices in counterclockwise + order. Further, in %RVO Library 1.x agents cannot be added to the simulation + after the simulation is initialized. In RVO2 Library this restriction is + removed. Obstacles still need to be processed before the simulation starts, + though. Lastly, in %RVO Library 1.x an instance of the simulator is a singleton. + This restriction is removed in RVO2 Library. + + \section smaller Smaller Changes + + With RVO2 Library, we have adopted the philosophy that anything that is + not part of the core local collision avoidance technique is to be stripped from + the library. Therefore, besides the roadmap infrastructure, we have also removed + acceleration constraints of agents, orientation of agents, and the unused + "class" of agents. Each of these can be implemented external of the + library if needed. We did maintain a kd-tree infrastructure for + efficiently finding other agents and obstacles nearby each agent. + + Also, RVO2 Library allows accessing information about the simulation, + such as the neighbors and the collision-avoidance constraints of each agent, + that is hidden from the user in %RVO Library 1.x. + + + \page building Building RVO2 Library + + We assume that you have downloaded RVO2 Library and unpacked the ZIP + archive to a path $RVO_ROOT. + + \section xcode Apple Xcode 3.x + + Open $RVO_ROOT/RVO.xcodeproj and select the %RVO target and + a configuration (Debug or Release). A framework + RVO.framework will be built in the default build directory, e.g. + $RVO_ROOT/build/Release. + + \section cmake CMake + + Create and switch to your chosen build directory, e.g. $RVO_ROOT/build. + Run cmake inside the build directory on the source directory, e.g. + cmake $RVO_ROOT/src. Build files for the default generator for your + platform will be generated in the build directory. + + \section make GNU Make + + Switch to the source directory $RVO_ROOT/src and run make. + Public header files (RVO.h, RVOSimulator.h, and + Vector2.h) will be copied to the $RVO_ROOT/include directory + and a static library libRVO.a will be compiled into the + $RVO_ROOT/lib directory. + + \section visual Microsoft Visual Studio 2008 + + Open $RVO_ROOT/RVO.sln and select the %RVO project and a + configuration (Debug, ReleaseST, or ReleaseMT). + Public header files (RVO.h, RVOSimulator.h, and + Vector2.h) will be copied to the $RVO_ROOT/include directory + and a static library, e.g. RVO.lib, will be compiled into the + $RVO_ROOT/lib directory. + + + \page using Using RVO2 Library + + \section structure Structure + + A program performing an RVO2 Library simulation has the following global + structure. + + \code + #include + + std::vector goals; + + int main() + { + // Create a new simulator instance. + RVO::RVOSimulator* sim = new RVO::RVOSimulator(); + + // Set up the scenario. + setupScenario(sim); + + // Perform (and manipulate) the simulation. + do { + updateVisualization(sim); + setPreferredVelocities(sim); + sim->doStep(); + } while (!reachedGoal(sim)); + + delete sim; + } + \endcode + + In order to use RVO2 Library, the user needs to include RVO.h. The first + step is then to create an instance of RVO::RVOSimulator. Then, the process + consists of two stages. The first stage is specifying the simulation scenario + and its parameters. In the above example program, this is done in the method + setupScenario(...), which we will discuss below. The second stage is the actual + performing of the simulation. + + In the above example program, simulation steps are taken until all + the agents have reached some predefined goals. Prior to each simulation step, + we set the preferred velocity for each agent, i.e. the + velocity the agent would have taken if there were no other agents around, in the + method setPreferredVelocities(...). The simulator computes the actual velocities + of the agents and attempts to follow the preferred velocities as closely as + possible while guaranteeing collision avoidance at the same time. During the + simulation, the user may want to retrieve information from the simulation for + instance to visualize the simulation. In the above example program, this is done + in the method updateVisualization(...), which we will discuss below. It is also + possible to manipulate the simulation during the simulation, for instance by + changing positions, radii, velocities, etc. of the agents. + + \section spec Setting up the Simulation Scenario + + A scenario that is to be simulated can be set up as follows. A scenario consists + of two types of objects: agents and obstacles. Each of them can be manually + specified. Agents may be added anytime before or during the simulation. + Obstacles, however, need to be defined prior to the simulation, and + RVO::RVOSimulator::processObstacles() need to be called in order for the + obstacles to be accounted for in the simulation. + The user may also want to define goal positions of the agents, or a + roadmap to guide the agents around obstacles. This is not done in RVO2 + Library, but needs to be taken care of in the user's external application. + + The following example creates a scenario with four agents exchanging positions + around a rectangular obstacle in the middle. + + \code + void setupScenario(RVO::RVOSimulator* sim) { + // Specify global time step of the simulation. + sim->setTimeStep(0.25f); + + // Specify default parameters for agents that are subsequently added. + sim->setAgentDefaults(15.0f, 10, 10.0f, 5.0f, 2.0f, 2.0f); + + // Add agents, specifying their start position. + sim->addAgent(RVO::Vector2(-50.0f, -50.0f)); + sim->addAgent(RVO::Vector2(50.0f, -50.0f)); + sim->addAgent(RVO::Vector2(50.0f, 50.0f)); + sim->addAgent(RVO::Vector2(-50.0f, 50.0f)); + + // Create goals (simulator is unaware of these). + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + goals.push_back(-sim->getAgentPosition(i)); + } + + // Add (polygonal) obstacle(s), specifying vertices in counterclockwise order. + std::vector vertices; + vertices.push_back(RVO::Vector2(-7.0f, -20.0f)); + vertices.push_back(RVO::Vector2(7.0f, -20.0f)); + vertices.push_back(RVO::Vector2(7.0f, 20.0f)); + vertices.push_back(RVO::Vector2(-7.0f, 20.0f)); + + sim->addObstacle(vertices); + + // Process obstacles so that they are accounted for in the simulation. + sim->processObstacles(); + } + \endcode + + See the documentation on RVO::RVOSimulator for a full overview of the + functionality to specify scenarios. + + \section ret Retrieving Information from the Simulation + + During the simulation, the user can extract information from the simulation for + instance for visualization purposes, or to determine termination conditions of + the simulation. In the example program above, visualization is done in the + updateVisualization(...) method. Below we give an example that simply writes + the positions of each agent in each time step to the standard output. The + termination condition is checked in the reachedGoal(...) method. Here we give an + example that returns true if all agents are within one radius of their goals. + + \code + void updateVisualization(RVO::RVOSimulator* sim) { + // Output the current global time. + std::cout << sim->getGlobalTime() << " "; + + // Output the position for all the agents. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + std::cout << sim->getAgentPosition(i) << " "; + } + + std::cout << std::endl; + } + \endcode + + \code + bool reachedGoal(RVO::RVOSimulator* sim) { + // Check whether all agents have arrived at their goals. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (absSq(goals[i] - sim->getAgentPosition(i)) > sim->getAgentRadius(i) * sim->getAgentRadius(i)) { + // Agent is further away from its goal than one radius. + return false; + } + } + return true; + } + \endcode + + Using similar functions as the ones used in this example, the user can access + information about other parameters of the agents, as well as the global + parameters, and the obstacles. See the documentation of the class + RVO::RVOSimulator for an exhaustive list of public functions for retrieving + simulation information. + + \section manip Manipulating the Simulation + + During the simulation, the user can manipulate the simulation, for instance by + changing the global parameters, or changing the parameters of the agents + (potentially causing abrupt different behavior). It is also possible to give the + agents a new position, which make them jump through the scene. + New agents can be added to the simulation at any time, but it is not allowed to + add obstacles to the simulation after they have been processed by calling + RVO::RVOSimulator::processObstacles(). Also, it is impossible to change the + position of the vertices of the obstacles. + + See the documentation of the class RVO::RVOSimulator for an exhaustive list of + public functions for manipulating the simulation. + + To provide global guidance to the agents, the preferred velocities of the agents + can be changed ahead of each simulation step. In the above example program, this + happens in the method setPreferredVelocities(...). Here we give an example that + simply sets the preferred velocity to the unit vector towards the agent's goal + for each agent (i.e., the preferred speed is 1.0). Note that this may not give + convincing results with respect to global navigation around the obstacles. For + this a roadmap or other global planning techniques may be used (see one of the + \ref example "example programs" that accompanies RVO2 Library). + + \code + void setPreferredVelocities(RVO::RVOSimulator* sim) { + // Set the preferred velocity for each agent. + for (size_t i = 0; i < sim->getNumAgents(); ++i) { + if (absSq(goals[i] - sim->getAgentPosition(i)) < sim->getAgentRadius(i) * sim->getAgentRadius(i) ) { + // Agent is within one radius of its goal, set preferred velocity to zero + sim->setAgentPrefVelocity(i, RVO::Vector2(0.0f, 0.0f)); + } else { + // Agent is far away from its goal, set preferred velocity as unit vector towards agent's goal. + sim->setAgentPrefVelocity(i, normalize(goals[i] - sim->getAgentPosition(i))); + } + } + } + \endcode + + \section example Example Programs + + RVO2 Library is accompanied by three example programs, which can be found in the + $RVO_ROOT/examples directory. The examples are named ExampleBlocks, ExampleCircle, and + ExampleRoadmap, and contain the following demonstration scenarios: + + + + + + + + + + + + + +
ExampleBlocksA scenario in which 100 agents, split in four groups initially + positioned in each of four corners of the environment, move to the + other side of the environment through a narrow passage generated by four + obstacles. There is no roadmap to guide the agents around the obstacles.
ExampleCircleA scenario in which 250 agents, initially positioned evenly + distributed on a circle, move to the antipodal position on the + circle. There are no obstacles.
ExampleRoadmapThe same scenario as ExampleBlocks, but now the preferred velocities + of the agents are determined using a roadmap guiding the agents around the obstacles.
+ + + \page params Parameter Overview + + \section globalp Global Parameters + + + + + + + + + + + + +
ParameterType (unit)Meaning
timeStepfloat (time)The time step of the simulation. Must be positive.
+ + \section agent Agent Parameters + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ParameterType (unit)Meaning
maxNeighborssize_tThe maximum number of other agents the agent takes into + account in the navigation. The larger this number, the + longer the running time of the simulation. If the number is + 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 + other agents the agent takes into account in the + navigation. The larger this number, the longer the running + time of the simulation. If the number is too low, the + 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) + The current preferred velocity of the agent. This is the + velocity the agent would take if no other agents or + obstacles were around. The simulator computes an actual + velocity for the agent that follows the preferred velocity + as closely as possible, but at the same time guarantees + 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 + that are computed by the simulation are safe with respect + to other agents. The larger this number, the sooner this + agent will respond to the presence of other agents, but the + less freedom the agent has in choosing its velocities. + Must be positive.
timeHorizonObstfloat (time)The minimal amount of time for which the agent's velocities + that are computed by the simulation are safe with respect + to obstacles. The larger this number, the sooner this agent + will respond to the presence of obstacles, but the less + freedom the agent has in choosing its velocities. + Must be positive.
velocityRVO::Vector2 (distance/time, distance/time) + The (current) velocity of the agent.
+ + + \page terms Terms and Conditions + + RVO2 Library + + Copyright © 2008-2010 University of North Carolina at Chapel Hill. + All rights reserved. + + Permission to use, copy, modify, and distribute this software and its + documentation for educational, research, and non-profit purposes, without fee, + and without a written agreement is hereby granted, provided that the above + copyright notice, this paragraph, and the following four paragraphs appear in + all copies. + + Permission to incorporate this software into commercial products may be obtained + by contacting the authors or the Office of Technology + Development at the University of North Carolina at Chapel Hill . + + This software program and documentation are copyrighted by the University of + North Carolina at Chapel Hill. The software program and documentation are + supplied "as is," without any accompanying services from the University of North + Carolina at Chapel Hill or the authors. The University of North Carolina at + Chapel Hill and the authors do not warrant that the operation of the program + will be uninterrupted or error-free. The end-user understands that the program + was developed for research purposes and is advised not to rely exclusively on + the program for any reason. + + IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE AUTHORS + BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + + THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY STATUTORY + WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON AN "AS IS" + BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS HAVE + NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR + MODIFICATIONS. + + */ + +#endif /* RVO_RVO_H_ */ diff --git a/src/RVOSimulator.cpp b/src/RVOSimulator.cpp new file mode 100644 index 0000000..37373cf --- /dev/null +++ b/src/RVOSimulator.cpp @@ -0,0 +1,393 @@ +/* + * RVOSimulator.cpp + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#include "RVOSimulator.h" + +#include "Agent.h" +#include "KdTree.h" +#include "Obstacle.h" + +#ifdef _OPENMP +#include +#endif + +namespace RVO { + RVOSimulator::RVOSimulator() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f) + { + kdTree_ = new KdTree(this); + } + + RVOSimulator::RVOSimulator(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(timeStep) + { + kdTree_ = new KdTree(this); + defaultAgent_ = new Agent(this); + + defaultAgent_->maxNeighbors_ = maxNeighbors; + defaultAgent_->maxSpeed_ = maxSpeed; + defaultAgent_->neighborDist_ = neighborDist; + defaultAgent_->radius_ = radius; + defaultAgent_->timeHorizon_ = timeHorizon; + defaultAgent_->timeHorizonObst_ = timeHorizonObst; + defaultAgent_->velocity_ = velocity; + } + + RVOSimulator::~RVOSimulator() + { + if (defaultAgent_ != NULL) { + delete defaultAgent_; + } + + for (size_t i = 0; i < agents_.size(); ++i) { + delete agents_[i]; + } + + for (size_t i = 0; i < obstacles_.size(); ++i) { + delete obstacles_[i]; + } + + delete kdTree_; + } + + size_t RVOSimulator::addAgent(const Vector2 &position) + { + if (defaultAgent_ == NULL) { + return RVO_ERROR; + } + + Agent *agent = new Agent(this); + + agent->position_ = position; + agent->maxNeighbors_ = defaultAgent_->maxNeighbors_; + agent->maxSpeed_ = defaultAgent_->maxSpeed_; + agent->neighborDist_ = defaultAgent_->neighborDist_; + agent->radius_ = defaultAgent_->radius_; + agent->timeHorizon_ = defaultAgent_->timeHorizon_; + agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_; + agent->velocity_ = defaultAgent_->velocity_; + + agent->id_ = agents_.size(); + + agents_.push_back(agent); + + return agents_.size() - 1; + } + + size_t RVOSimulator::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) + { + Agent *agent = new Agent(this); + + agent->position_ = position; + agent->maxNeighbors_ = maxNeighbors; + agent->maxSpeed_ = maxSpeed; + agent->neighborDist_ = neighborDist; + agent->radius_ = radius; + agent->timeHorizon_ = timeHorizon; + agent->timeHorizonObst_ = timeHorizonObst; + agent->velocity_ = velocity; + + agent->id_ = agents_.size(); + + agents_.push_back(agent); + + return agents_.size() - 1; + } + + size_t RVOSimulator::addObstacle(const std::vector &vertices) + { + if (vertices.size() < 2) { + return RVO_ERROR; + } + + const size_t obstacleNo = obstacles_.size(); + + for (size_t i = 0; i < vertices.size(); ++i) { + Obstacle *obstacle = new Obstacle(); + obstacle->point_ = vertices[i]; + + if (i != 0) { + obstacle->prevObstacle_ = obstacles_.back(); + obstacle->prevObstacle_->nextObstacle_ = obstacle; + } + + if (i == vertices.size() - 1) { + obstacle->nextObstacle_ = obstacles_[obstacleNo]; + obstacle->nextObstacle_->prevObstacle_ = obstacle; + } + + obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]); + + if (vertices.size() == 2) { + obstacle->isConvex_ = true; + } + else { + obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f); + } + + obstacle->id_ = obstacles_.size(); + + obstacles_.push_back(obstacle); + } + + return obstacleNo; + } + + void RVOSimulator::doStep() + { + kdTree_->buildAgentTree(); + +#ifdef _OPENMP +#pragma omp parallel for +#endif + for (int i = 0; i < static_cast(agents_.size()); ++i) { + agents_[i]->computeNeighbors(); + agents_[i]->computeNewVelocity(); + } + +#ifdef _OPENMP +#pragma omp parallel for +#endif + for (int i = 0; i < static_cast(agents_.size()); ++i) { + agents_[i]->update(); + } + + globalTime_ += timeStep_; + } + + size_t RVOSimulator::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const + { + return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_; + } + + size_t RVOSimulator::getAgentMaxNeighbors(size_t agentNo) const + { + return agents_[agentNo]->maxNeighbors_; + } + + float RVOSimulator::getAgentMaxSpeed(size_t agentNo) const + { + return agents_[agentNo]->maxSpeed_; + } + + float RVOSimulator::getAgentNeighborDist(size_t agentNo) const + { + return agents_[agentNo]->neighborDist_; + } + + size_t RVOSimulator::getAgentNumAgentNeighbors(size_t agentNo) const + { + return agents_[agentNo]->agentNeighbors_.size(); + } + + size_t RVOSimulator::getAgentNumObstacleNeighbors(size_t agentNo) const + { + return agents_[agentNo]->obstacleNeighbors_.size(); + } + + size_t RVOSimulator::getAgentNumORCALines(size_t agentNo) const + { + return agents_[agentNo]->orcaLines_.size(); + } + + size_t RVOSimulator::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const + { + return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_; + } + + const Line &RVOSimulator::getAgentORCALine(size_t agentNo, size_t lineNo) const + { + return agents_[agentNo]->orcaLines_[lineNo]; + } + + const Vector2 &RVOSimulator::getAgentPosition(size_t agentNo) const + { + return agents_[agentNo]->position_; + } + + const Vector2 &RVOSimulator::getAgentPrefVelocity(size_t agentNo) const + { + return agents_[agentNo]->prefVelocity_; + } + + float RVOSimulator::getAgentRadius(size_t agentNo) const + { + return agents_[agentNo]->radius_; + } + + float RVOSimulator::getAgentTimeHorizon(size_t agentNo) const + { + return agents_[agentNo]->timeHorizon_; + } + + float RVOSimulator::getAgentTimeHorizonObst(size_t agentNo) const + { + return agents_[agentNo]->timeHorizonObst_; + } + + const Vector2 &RVOSimulator::getAgentVelocity(size_t agentNo) const + { + return agents_[agentNo]->velocity_; + } + + float RVOSimulator::getGlobalTime() const + { + return globalTime_; + } + + size_t RVOSimulator::getNumAgents() const + { + return agents_.size(); + } + + size_t RVOSimulator::getNumObstacleVertices() const + { + return obstacles_.size(); + } + + const Vector2 &RVOSimulator::getObstacleVertex(size_t vertexNo) const + { + return obstacles_[vertexNo]->point_; + } + + size_t RVOSimulator::getNextObstacleVertexNo(size_t vertexNo) const + { + return obstacles_[vertexNo]->nextObstacle_->id_; + } + + size_t RVOSimulator::getPrevObstacleVertexNo(size_t vertexNo) const + { + return obstacles_[vertexNo]->prevObstacle_->id_; + } + + float RVOSimulator::getTimeStep() const + { + return timeStep_; + } + + void RVOSimulator::processObstacles() + { + kdTree_->buildObstacleTree(); + } + + bool RVOSimulator::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const + { + return kdTree_->queryVisibility(point1, point2, radius); + } + + void RVOSimulator::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) + { + if (defaultAgent_ == NULL) { + defaultAgent_ = new Agent(this); + } + + defaultAgent_->maxNeighbors_ = maxNeighbors; + defaultAgent_->maxSpeed_ = maxSpeed; + defaultAgent_->neighborDist_ = neighborDist; + defaultAgent_->radius_ = radius; + defaultAgent_->timeHorizon_ = timeHorizon; + defaultAgent_->timeHorizonObst_ = timeHorizonObst; + defaultAgent_->velocity_ = velocity; + } + + void RVOSimulator::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors) + { + agents_[agentNo]->maxNeighbors_ = maxNeighbors; + } + + void RVOSimulator::setAgentMaxSpeed(size_t agentNo, float maxSpeed) + { + agents_[agentNo]->maxSpeed_ = maxSpeed; + } + + void RVOSimulator::setAgentNeighborDist(size_t agentNo, float neighborDist) + { + agents_[agentNo]->neighborDist_ = neighborDist; + } + + void RVOSimulator::setAgentPosition(size_t agentNo, const Vector2 &position) + { + agents_[agentNo]->position_ = position; + } + + void RVOSimulator::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity) + { + agents_[agentNo]->prefVelocity_ = prefVelocity; + } + + void RVOSimulator::setAgentRadius(size_t agentNo, float radius) + { + agents_[agentNo]->radius_ = radius; + } + + void RVOSimulator::setAgentTimeHorizon(size_t agentNo, float timeHorizon) + { + agents_[agentNo]->timeHorizon_ = timeHorizon; + } + + void RVOSimulator::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst) + { + agents_[agentNo]->timeHorizonObst_ = timeHorizonObst; + } + + void RVOSimulator::setAgentVelocity(size_t agentNo, const Vector2 &velocity) + { + agents_[agentNo]->velocity_ = velocity; + } + + void RVOSimulator::setTimeStep(float timeStep) + { + timeStep_ = timeStep; + } +} diff --git a/src/RVOSimulator.h b/src/RVOSimulator.h new file mode 100644 index 0000000..232cc80 --- /dev/null +++ b/src/RVOSimulator.h @@ -0,0 +1,616 @@ +/* + * RVOSimulator.h + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO_RVO_SIMULATOR_H_ +#define RVO_RVO_SIMULATOR_H_ + +/** + * \file RVOSimulator.h + * \brief Contains the RVOSimulator class. + */ + +#include +#include +#include + +#include "Vector2.h" + +namespace RVO { + /** + * \brief Error value. + * + * A value equal to the largest unsigned integer that is returned in case + * of an error by functions in RVO::RVOSimulator. + */ + const size_t RVO_ERROR = std::numeric_limits::max(); + + /** + * \brief Defines a directed line. + */ + class Line { + public: + /** + * \brief A point on the directed line. + */ + Vector2 point; + + /** + * \brief The direction of the directed line. + */ + Vector2 direction; + }; + + class Agent; + class KdTree; + class Obstacle; + + /** + * \brief Defines the simulation. + * + * The main class of the library that contains all simulation functionality. + */ + class RVOSimulator { + public: + /** + * \brief Constructs a simulator instance. + */ + RVOSimulator(); + + /** + * \brief Constructs a simulator instance and sets the default + * properties for any new agent that is added. + * \param timeStep The time step of the simulation. + * Must be positive. + * \param neighborDist The default maximum distance (center point + * to center point) to other agents a new agent + * takes into account in the navigation. The + * larger this number, the longer he running + * time of the simulation. If the number is too + * low, the simulation will not be safe. Must be + * non-negative. + * \param maxNeighbors The default maximum number of other agents a + * new agent takes into account in the + * navigation. The larger this number, the + * longer the running time of the simulation. + * If the number is too low, the simulation + * will not be safe. + * \param timeHorizon The default minimal amount of time for which + * a new agent's velocities that are computed + * by the simulation are safe with respect to + * other agents. The larger this number, the + * sooner an agent will respond to the presence + * of other agents, but the less freedom the + * agent has in choosing its velocities. + * Must be positive. + * \param timeHorizonObst The default minimal amount of time for which + * a new agent's velocities that are computed + * by the simulation are safe with respect to + * obstacles. The larger this number, the + * sooner an agent will respond to the presence + * of obstacles, but the less freedom the agent + * has in choosing its velocities. + * Must be positive. + * \param radius The default radius of a new agent. + * Must be non-negative. + * \param maxSpeed The default maximum speed of a new agent. + * Must be non-negative. + * \param velocity The default initial two-dimensional linear + * velocity of a new agent (optional). + */ + RVOSimulator(float timeStep, float neighborDist, size_t maxNeighbors, + float timeHorizon, float timeHorizonObst, float radius, + float maxSpeed, const Vector2 &velocity = Vector2()); + + /** + * \brief Destroys this simulator instance. + */ + ~RVOSimulator(); + + /** + * \brief Adds a new agent with default properties to the + * simulation. + * \param position The two-dimensional starting position of + * this agent. + * \return The number of the agent, or RVO::RVO_ERROR when the agent + * defaults have not been set. + */ + size_t addAgent(const Vector2 &position); + + /** + * \brief Adds a new agent to the simulation. + * \param position The two-dimensional starting position of + * this agent. + * \param neighborDist The maximum distance (center point to + * center point) to other agents this agent + * takes into account in the navigation. The + * larger this number, the longer the running + * time of the simulation. If the number is too + * low, the simulation will not be safe. + * Must be non-negative. + * \param maxNeighbors The maximum number of other agents this + * agent takes into account in the navigation. + * The larger this number, the longer the + * running time of the simulation. If the + * number is too low, the simulation will not + * be safe. + * \param timeHorizon The minimal amount of time for which this + * agent's velocities that are computed by the + * simulation are safe with respect to other + * agents. The larger this number, the sooner + * this agent will respond to the presence of + * other agents, but the less freedom this + * agent has in choosing its velocities. + * Must be positive. + * \param timeHorizonObst The minimal amount of time for which this + * agent's velocities that are computed by the + * simulation are safe with respect to + * obstacles. The larger this number, the + * sooner this agent will respond to the + * presence of obstacles, but the less freedom + * this agent has in choosing its velocities. + * Must be positive. + * \param radius The radius of this agent. + * Must be non-negative. + * \param maxSpeed The maximum speed of this agent. + * Must be non-negative. + * \param velocity The initial two-dimensional linear velocity + * of this agent (optional). + * \return The number of the agent. + */ + size_t addAgent(const Vector2 &position, float neighborDist, + size_t maxNeighbors, float timeHorizon, + float timeHorizonObst, float radius, float maxSpeed, + const Vector2 &velocity = Vector2()); + + /** + * \brief Adds a new obstacle to the simulation. + * \param vertices List of the vertices of the polygonal + * obstacle in counterclockwise order. + * \return The number of the first vertex of the obstacle, + * or RVO::RVO_ERROR when the number of vertices is less than two. + * \note To add a "negative" obstacle, e.g. a bounding polygon around + * the environment, the vertices should be listed in clockwise + * order. + */ + size_t addObstacle(const std::vector &vertices); + + /** + * \brief Lets the simulator perform a simulation step and updates the + * two-dimensional position and two-dimensional velocity of + * each agent. + */ + void doStep(); + + /** + * \brief Returns the specified agent neighbor of the specified + * agent. + * \param agentNo The number of the agent whose agent + * neighbor is to be retrieved. + * \param neighborNo The number of the agent neighbor to be + * retrieved. + * \return The number of the neighboring agent. + */ + size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const; + + /** + * \brief Returns the maximum neighbor count of a specified agent. + * \param agentNo The number of the agent whose maximum + * neighbor count is to be retrieved. + * \return The present maximum neighbor count of the agent. + */ + size_t getAgentMaxNeighbors(size_t agentNo) const; + + /** + * \brief Returns the maximum speed of a specified agent. + * \param agentNo The number of the agent whose maximum speed + * is to be retrieved. + * \return The present maximum speed of the agent. + */ + float getAgentMaxSpeed(size_t agentNo) const; + + /** + * \brief Returns the maximum neighbor distance of a specified + * agent. + * \param agentNo The number of the agent whose maximum + * neighbor distance is to be retrieved. + * \return The present maximum neighbor distance of the agent. + */ + float getAgentNeighborDist(size_t agentNo) const; + + /** + * \brief Returns the count of agent neighbors taken into account to + * compute the current velocity for the specified agent. + * \param agentNo The number of the agent whose count of agent + * neighbors is to be retrieved. + * \return The count of agent neighbors taken into account to compute + * the current velocity for the specified agent. + */ + size_t getAgentNumAgentNeighbors(size_t agentNo) const; + + /** + * \brief Returns the count of obstacle neighbors taken into account + * to compute the current velocity for the specified agent. + * \param agentNo The number of the agent whose count of + * obstacle neighbors is to be retrieved. + * \return The count of obstacle neighbors taken into account to + * compute the current velocity for the specified agent. + */ + size_t getAgentNumObstacleNeighbors(size_t agentNo) const; + + + /** + * \brief Returns the count of ORCA constraints used to compute + * the current velocity for the specified agent. + * \param agentNo The number of the agent whose count of ORCA + * constraints is to be retrieved. + * \return The count of ORCA constraints used to compute the current + * velocity for the specified agent. + */ + size_t getAgentNumORCALines(size_t agentNo) const; + + /** + * \brief Returns the specified obstacle neighbor of the specified + * agent. + * \param agentNo The number of the agent whose obstacle + * neighbor is to be retrieved. + * \param neighborNo The number of the obstacle neighbor to be + * retrieved. + * \return The number of the first vertex of the neighboring obstacle + * edge. + */ + size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const; + + /** + * \brief Returns the specified ORCA constraint of the specified + * agent. + * \param agentNo The number of the agent whose ORCA + * constraint is to be retrieved. + * \param lineNo The number of the ORCA constraint to be + * retrieved. + * \return A line representing the specified ORCA constraint. + * \note The halfplane to the left of the line is the region of + * permissible velocities with respect to the specified + * ORCA constraint. + */ + const Line &getAgentORCALine(size_t agentNo, size_t lineNo) const; + + /** + * \brief Returns the two-dimensional position of a specified + * agent. + * \param agentNo The number of the agent whose + * two-dimensional position is to be retrieved. + * \return The present two-dimensional position of the (center of the) + * agent. + */ + const Vector2 &getAgentPosition(size_t agentNo) const; + + /** + * \brief Returns the two-dimensional preferred velocity of a + * specified agent. + * \param agentNo The number of the agent whose + * two-dimensional preferred velocity is to be + * retrieved. + * \return The present two-dimensional preferred velocity of the agent. + */ + const Vector2 &getAgentPrefVelocity(size_t agentNo) const; + + /** + * \brief Returns the radius of a specified agent. + * \param agentNo The number of the agent whose radius is to + * be retrieved. + * \return The present radius of the agent. + */ + float getAgentRadius(size_t agentNo) const; + + /** + * \brief Returns the time horizon of a specified agent. + * \param agentNo The number of the agent whose time horizon + * is to be retrieved. + * \return The present time horizon of the agent. + */ + float getAgentTimeHorizon(size_t agentNo) const; + + /** + * \brief Returns the time horizon with respect to obstacles of a + * specified agent. + * \param agentNo The number of the agent whose time horizon + * with respect to obstacles is to be + * retrieved. + * \return The present time horizon with respect to obstacles of the + * agent. + */ + float getAgentTimeHorizonObst(size_t agentNo) const; + + /** + * \brief Returns the two-dimensional linear velocity of a + * specified agent. + * \param agentNo The number of the agent whose + * two-dimensional linear velocity is to be + * retrieved. + * \return The present two-dimensional linear velocity of the agent. + */ + const Vector2 &getAgentVelocity(size_t agentNo) const; + + /** + * \brief Returns the global time of the simulation. + * \return The present global time of the simulation (zero initially). + */ + float getGlobalTime() const; + + /** + * \brief Returns the count of agents in the simulation. + * \return The count of agents in the simulation. + */ + size_t getNumAgents() const; + + /** + * \brief Returns the count of obstacle vertices in the simulation. + * \return The count of obstacle vertices in the simulation. + */ + size_t getNumObstacleVertices() const; + + /** + * \brief Returns the two-dimensional position of a specified obstacle + * vertex. + * \param vertexNo The number of the obstacle vertex to be + * retrieved. + * \return The two-dimensional position of the specified obstacle + * vertex. + */ + const Vector2 &getObstacleVertex(size_t vertexNo) const; + + /** + * \brief Returns the number of the obstacle vertex succeeding the + * specified obstacle vertex in its polygon. + * \param vertexNo The number of the obstacle vertex whose + * successor is to be retrieved. + * \return The number of the obstacle vertex succeeding the specified + * obstacle vertex in its polygon. + */ + size_t getNextObstacleVertexNo(size_t vertexNo) const; + + /** + * \brief Returns the number of the obstacle vertex preceding the + * specified obstacle vertex in its polygon. + * \param vertexNo The number of the obstacle vertex whose + * predecessor is to be retrieved. + * \return The number of the obstacle vertex preceding the specified + * obstacle vertex in its polygon. + */ + size_t getPrevObstacleVertexNo(size_t vertexNo) const; + + /** + * \brief Returns the time step of the simulation. + * \return The present time step of the simulation. + */ + float getTimeStep() const; + + /** + * \brief Processes the obstacles that have been added so that they + * are accounted for in the simulation. + * \note Obstacles added to the simulation after this function has + * been called are not accounted for in the simulation. + */ + void processObstacles(); + + /** + * \brief Performs a visibility query between the two specified + * points with respect to the obstacles + * \param point1 The first point of the query. + * \param point2 The second point of the query. + * \param radius The minimal distance between the line + * connecting the two points and the obstacles + * in order for the points to be mutually + * visible (optional). Must be non-negative. + * \return A boolean specifying whether the two points are mutually + * visible. Returns true when the obstacles have not been + * processed. + */ + bool queryVisibility(const Vector2 &point1, const Vector2 &point2, + float radius = 0.0f) const; + + /** + * \brief Sets the default properties for any new agent that is + * added. + * \param neighborDist The default maximum distance (center point + * to center point) to other agents a new agent + * takes into account in the navigation. The + * larger this number, the longer he running + * time of the simulation. If the number is too + * low, the simulation will not be safe. + * Must be non-negative. + * \param maxNeighbors The default maximum number of other agents a + * new agent takes into account in the + * navigation. The larger this number, the + * longer the running time of the simulation. + * If the number is too low, the simulation + * will not be safe. + * \param timeHorizon The default minimal amount of time for which + * a new agent's velocities that are computed + * by the simulation are safe with respect to + * other agents. The larger this number, the + * sooner an agent will respond to the presence + * of other agents, but the less freedom the + * agent has in choosing its velocities. + * Must be positive. + * \param timeHorizonObst The default minimal amount of time for which + * a new agent's velocities that are computed + * by the simulation are safe with respect to + * obstacles. The larger this number, the + * sooner an agent will respond to the presence + * of obstacles, but the less freedom the agent + * has in choosing its velocities. + * Must be positive. + * \param radius The default radius of a new agent. + * Must be non-negative. + * \param maxSpeed The default maximum speed of a new agent. + * Must be non-negative. + * \param velocity The default initial two-dimensional linear + * velocity of a new agent (optional). + */ + void setAgentDefaults(float neighborDist, size_t maxNeighbors, + float timeHorizon, float timeHorizonObst, + float radius, float maxSpeed, + const Vector2 &velocity = Vector2()); + + /** + * \brief Sets the maximum neighbor count of a specified agent. + * \param agentNo The number of the agent whose maximum + * neighbor count is to be modified. + * \param maxNeighbors The replacement maximum neighbor count. + */ + void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors); + + /** + * \brief Sets the maximum speed of a specified agent. + * \param agentNo The number of the agent whose maximum speed + * is to be modified. + * \param maxSpeed The replacement maximum speed. Must be + * non-negative. + */ + void setAgentMaxSpeed(size_t agentNo, float maxSpeed); + + /** + * \brief Sets the maximum neighbor distance of a specified agent. + * \param agentNo The number of the agent whose maximum + * neighbor distance is to be modified. + * \param neighborDist The replacement maximum neighbor distance. + * Must be non-negative. + */ + void setAgentNeighborDist(size_t agentNo, float neighborDist); + + /** + * \brief Sets the two-dimensional position of a specified agent. + * \param agentNo The number of the agent whose + * two-dimensional position is to be modified. + * \param position The replacement of the two-dimensional + * position. + */ + void setAgentPosition(size_t agentNo, const Vector2 &position); + + /** + * \brief Sets the two-dimensional preferred velocity of a + * specified agent. + * \param agentNo The number of the agent whose + * two-dimensional preferred velocity is to be + * modified. + * \param prefVelocity The replacement of the two-dimensional + * preferred velocity. + */ + void setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity); + + /** + * \brief Sets the radius of a specified agent. + * \param agentNo The number of the agent whose radius is to + * be modified. + * \param radius The replacement radius. + * Must be non-negative. + */ + void setAgentRadius(size_t agentNo, float radius); + + /** + * \brief Sets the time horizon of a specified agent with respect + * to other agents. + * \param agentNo The number of the agent whose time horizon + * is to be modified. + * \param timeHorizon The replacement time horizon with respect + * to other agents. Must be positive. + */ + void setAgentTimeHorizon(size_t agentNo, float timeHorizon); + + /** + * \brief Sets the time horizon of a specified agent with respect + * to obstacles. + * \param agentNo The number of the agent whose time horizon + * with respect to obstacles is to be modified. + * \param timeHorizonObst The replacement time horizon with respect to + * obstacles. Must be positive. + */ + void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst); + + /** + * \brief Sets the two-dimensional linear velocity of a specified + * agent. + * \param agentNo The number of the agent whose + * two-dimensional linear velocity is to be + * modified. + * \param velocity The replacement two-dimensional linear + * velocity. + */ + void setAgentVelocity(size_t agentNo, const Vector2 &velocity); + + /** + * \brief Sets the time step of the simulation. + * \param timeStep The time step of the simulation. + * Must be positive. + */ + void setTimeStep(float timeStep); + + private: + std::vector agents_; + Agent *defaultAgent_; + float globalTime_; + KdTree *kdTree_; + std::vector obstacles_; + float timeStep_; + + friend class Agent; + friend class KdTree; + friend class Obstacle; + }; +} + +#endif /* RVO_RVO_SIMULATOR_H_ */ diff --git a/src/Vector2.h b/src/Vector2.h new file mode 100644 index 0000000..fed4c10 --- /dev/null +++ b/src/Vector2.h @@ -0,0 +1,356 @@ +/* + * Vector2.h + * RVO2 Library + * + * Copyright (c) 2008-2013 University of North Carolina at Chapel Hill. + * All rights reserved. + * + * Permission to use, copy, modify, and distribute this software and its + * documentation for educational, research, and non-profit purposes, without + * fee, and without a written agreement is hereby granted, provided that the + * above copyright notice, this paragraph, and the following four paragraphs + * appear in all copies. + * + * Permission to incorporate this software into commercial products may be + * obtained by contacting the authors or the Office of + * Technology Development at the University of North Carolina at Chapel Hill + * . + * + * This software program and documentation are copyrighted by the University of + * North Carolina at Chapel Hill. The software program and documentation are + * supplied "as is," without any accompanying services from the University of + * North Carolina at Chapel Hill or the authors. The University of North + * Carolina at Chapel Hill and the authors do not warrant that the operation of + * the program will be uninterrupted or error-free. The end-user understands + * that the program was developed for research purposes and is advised not to + * rely exclusively on the program for any reason. + * + * IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL OR THE + * AUTHORS BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR + * CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE USE OF THIS + * SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF NORTH CAROLINA AT + * CHAPEL HILL OR THE AUTHORS HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH + * DAMAGE. + * + * THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE AUTHORS SPECIFICALLY + * DISCLAIM ANY WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AND ANY + * STATUTORY WARRANTY OF NON-INFRINGEMENT. THE SOFTWARE PROVIDED HEREUNDER IS ON + * AN "AS IS" BASIS, AND THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL AND THE + * AUTHORS HAVE NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, + * ENHANCEMENTS, OR MODIFICATIONS. + * + * Please send all bug reports to . + * + * The authors may be contacted via: + * + * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha + * Dept. of Computer Science + * 201 S. Columbia St. + * Frederick P. Brooks, Jr. Computer Science Bldg. + * Chapel Hill, N.C. 27599-3175 + * United States of America + * + * + */ + +#ifndef RVO_VECTOR2_H_ +#define RVO_VECTOR2_H_ + +/** + * \file Vector2.h + * \brief Contains the Vector2 class. + */ + +#include +#include + +namespace RVO { + /** + * \brief Defines a two-dimensional vector. + */ + class Vector2 { + public: + /** + * \brief Constructs and initializes a two-dimensional vector instance + * to (0.0, 0.0). + */ + inline Vector2() : x_(0.0f), y_(0.0f) { } + + /** + * \brief Constructs and initializes a two-dimensional vector from + * the specified xy-coordinates. + * \param x The x-coordinate of the two-dimensional + * vector. + * \param y The y-coordinate of the two-dimensional + * vector. + */ + inline Vector2(float x, float y) : x_(x), y_(y) { } + + /** + * \brief Returns the x-coordinate of this two-dimensional vector. + * \return The x-coordinate of the two-dimensional vector. + */ + inline float x() const { return x_; } + + /** + * \brief Returns the y-coordinate of this two-dimensional vector. + * \return The y-coordinate of the two-dimensional vector. + */ + inline float y() const { return y_; } + + /** + * \brief Computes the negation of this two-dimensional vector. + * \return The negation of this two-dimensional vector. + */ + inline Vector2 operator-() const + { + return Vector2(-x_, -y_); + } + + /** + * \brief Computes the dot product of this two-dimensional vector with + * the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * dot product should be computed. + * \return The dot product of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline float operator*(const Vector2 &vector) const + { + return x_ * vector.x() + y_ * vector.y(); + } + + /** + * \brief Computes the scalar multiplication of this + * two-dimensional vector with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \return The scalar multiplication of this two-dimensional vector + * with a specified scalar value. + */ + inline Vector2 operator*(float s) const + { + return Vector2(x_ * s, y_ * s); + } + + /** + * \brief Computes the scalar division of this two-dimensional vector + * with the specified scalar value. + * \param s The scalar value with which the scalar + * division should be computed. + * \return The scalar division of this two-dimensional vector with a + * specified scalar value. + */ + inline Vector2 operator/(float s) const + { + const float invS = 1.0f / s; + + return Vector2(x_ * invS, y_ * invS); + } + + /** + * \brief Computes the vector sum of this two-dimensional vector with + * the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector sum should be computed. + * \return The vector sum of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline Vector2 operator+(const Vector2 &vector) const + { + return Vector2(x_ + vector.x(), y_ + vector.y()); + } + + /** + * \brief Computes the vector difference of this two-dimensional + * vector with the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector difference should be computed. + * \return The vector difference of this two-dimensional vector with a + * specified two-dimensional vector. + */ + inline Vector2 operator-(const Vector2 &vector) const + { + return Vector2(x_ - vector.x(), y_ - vector.y()); + } + + /** + * \brief Tests this two-dimensional vector for equality with the + * specified two-dimensional vector. + * \param vector The two-dimensional vector with which to + * test for equality. + * \return True if the two-dimensional vectors are equal. + */ + inline bool operator==(const Vector2 &vector) const + { + return x_ == vector.x() && y_ == vector.y(); + } + + /** + * \brief Tests this two-dimensional vector for inequality with the + * specified two-dimensional vector. + * \param vector The two-dimensional vector with which to + * test for inequality. + * \return True if the two-dimensional vectors are not equal. + */ + inline bool operator!=(const Vector2 &vector) const + { + return x_ != vector.x() || y_ != vector.y(); + } + + /** + * \brief Sets the value of this two-dimensional vector to the scalar + * multiplication of itself with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 &operator*=(float s) + { + x_ *= s; + y_ *= s; + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the scalar + * division of itself with the specified scalar value. + * \param s The scalar value with which the scalar + * division should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 &operator/=(float s) + { + const float invS = 1.0f / s; + x_ *= invS; + y_ *= invS; + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the vector + * sum of itself with the specified two-dimensional vector. + * \param vector The two-dimensional vector with which the + * vector sum should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 &operator+=(const Vector2 &vector) + { + x_ += vector.x(); + y_ += vector.y(); + + return *this; + } + + /** + * \brief Sets the value of this two-dimensional vector to the vector + * difference of itself with the specified two-dimensional + * vector. + * \param vector The two-dimensional vector with which the + * vector difference should be computed. + * \return A reference to this two-dimensional vector. + */ + inline Vector2 &operator-=(const Vector2 &vector) + { + x_ -= vector.x(); + y_ -= vector.y(); + + return *this; + } + + private: + float x_; + float y_; + }; + + /** + * \relates Vector2 + * \brief Computes the scalar multiplication of the specified + * two-dimensional vector with the specified scalar value. + * \param s The scalar value with which the scalar + * multiplication should be computed. + * \param vector The two-dimensional vector with which the scalar + * multiplication should be computed. + * \return The scalar multiplication of the two-dimensional vector with the + * scalar value. + */ + inline Vector2 operator*(float s, const Vector2 &vector) + { + return Vector2(s * vector.x(), s * vector.y()); + } + + /** + * \relates Vector2 + * \brief Inserts the specified two-dimensional vector into the specified + * output stream. + * \param os The output stream into which the two-dimensional + * vector should be inserted. + * \param vector The two-dimensional vector which to insert into + * the output stream. + * \return A reference to the output stream. + */ + inline std::ostream &operator<<(std::ostream &os, const Vector2 &vector) + { + os << "(" << vector.x() << "," << vector.y() << ")"; + + return os; + } + + /** + * \relates Vector2 + * \brief Computes the length of a specified two-dimensional vector. + * \param vector The two-dimensional vector whose length is to be + * computed. + * \return The length of the two-dimensional vector. + */ + inline float abs(const Vector2 &vector) + { + return std::sqrt(vector * vector); + } + + /** + * \relates Vector2 + * \brief Computes the squared length of a specified two-dimensional + * vector. + * \param vector The two-dimensional vector whose squared length + * is to be computed. + * \return The squared length of the two-dimensional vector. + */ + inline float absSq(const Vector2 &vector) + { + return vector * vector; + } + + /** + * \relates Vector2 + * \brief Computes the determinant of a two-dimensional square matrix with + * rows consisting of the specified two-dimensional vectors. + * \param vector1 The top row of the two-dimensional square + * matrix. + * \param vector2 The bottom row of the two-dimensional square + * matrix. + * \return The determinant of the two-dimensional square matrix. + */ + inline float det(const Vector2 &vector1, const Vector2 &vector2) + { + return vector1.x() * vector2.y() - vector1.y() * vector2.x(); + } + + /** + * \relates Vector2 + * \brief Computes the normalization of the specified two-dimensional + * vector. + * \param vector The two-dimensional vector whose normalization + * is to be computed. + * \return The normalization of the two-dimensional vector. + */ + inline Vector2 normalize(const Vector2 &vector) + { + return vector / abs(vector); + } +} + +#endif /* RVO_VECTOR2_H_ */