--- /dev/null
+# SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+#
+# SPDX-License-Identifier: CC0-1.0
+
+root = true
+
+[*.{cc,hh}]
+end_of_line = lf
+insert_final_newline = true
+charset = utf-8
+trim_trailing_whitespace = true
+indent_style = tab
+indent_size = 8
+max_line_length = 80
+
+[*.{py,md}]
+end_of_line = lf
+insert_final_newline = true
+charset = utf-8
+trim_trailing_whitespace = true
+indent_style = space
+indent_size = 4
+max_line_length = 80
--- /dev/null
+# SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+#
+# SPDX-License-Identifier: CC0-1.0
+
+build
+docs-out
+*.swp
--- /dev/null
+# SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+#
+# SPDX-License-Identifier: CC0-1.0
+
+[submodule "bcar"]
+ path = bcar
+ url = git://rtime.felk.cvut.cz/hubacji1/bcar.git
--- /dev/null
+Format: https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/
+Upstream-Name: RRTS
+Upstream-Contact: Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+Source: https://rtime.ciirc.cvut.cz/gitweb/hubacji1/rrts.git
+
+# Sample paragraph, commented out:
+#
+# Files: src/*
+# Copyright: $YEAR $NAME <$CONTACT>
+# License: ...
--- /dev/null
+# SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+#
+# SPDX-License-Identifier: GPL-3.0-only
+
+cmake_minimum_required(VERSION 3.8)
+project(rrts)
+
+#set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_DEBUG} -fno-omit-frame-pointer -fsanitize=address")
+#set (CMAKE_LINKER_FLAGS "${CMAKE_LINKER_FLAGS_DEBUG} -fno-omit-frame-pointer -fsanitize=address")
+
+set(CMAKE_CXX_STANDARD 17)
+add_compile_options(-O2)
+# Require compiler version (g++ >= 7 tested)
+if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7)
+ message(FATAL_ERROR "GCC version must be at least 7!")
+endif()
+
+if (NOT TARGET bcar)
+ set(SKIP_UT TRUE)
+ add_subdirectory(bcar)
+ unset(SKIP_UT)
+endif()
+link_libraries(bcar)
+link_libraries(pslot)
+
+if (NOT TARGET "JsonCpp::JsonCpp")
+ find_package(jsoncpp REQUIRED)
+endif ()
+link_libraries(jsoncpp_lib)
+
+add_library(rrts STATIC
+ src/rrts.cc
+ src/rrtext21.cc
+ src/rrtext20.cc
+ src/rrtext19.cc
+ src/rrtext18.cc
+ src/rrtext17.cc
+ src/rrtext16.cc
+ src/rrtext15.cc
+ src/rrtext14.cc
+ src/rrtext13.cc
+ src/rrtext10.cc
+ src/rrtext8.cc
+ src/rrtext6.cc
+ src/rrtext2.cc
+ src/reeds_shepp.cpp
+ src/dubins.c
+)
+target_include_directories(rrts PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/incl)
--- /dev/null
+# SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+#
+# SPDX-License-Identifier: GPL-3.0-only
+
+# Doxyfile 1.8.13
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME = "RRT*"
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER = 0.13.0
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF =
+
+# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
+# in the documentation. The maximum height of the logo should not exceed 55
+# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
+# the logo to the output directory.
+
+PROJECT_LOGO =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY = docs-out
+
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF = "The $name class" \
+ "The $name widget" \
+ "The $name file" \
+ is \
+ provides \
+ specifies \
+ contains \
+ represents \
+ a \
+ an \
+ the
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
+# page for each member. If set to NO, the documentation of a member will be part
+# of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE = 8
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran:
+# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran:
+# Fortran. In the later case the parser tries to guess whether the code is fixed
+# or free formatted code, this is the default for Fortran type files), VHDL. For
+# instance to make doxygen treat .inc files as Fortran files (default is PHP),
+# and .f files as C (default is Fortran), use: inc=Fortran f=C.
+#
+# Note: For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT = YES
+
+# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up
+# to that level are automatically included in the table of contents, even if
+# they do not have an id attribute.
+# Note: This feature currently applies only to Markdown headings.
+# Minimum value: 0, maximum value: 99, default value: 0.
+# This tag requires that the tag MARKDOWN_SUPPORT is set to YES.
+
+TOC_INCLUDE_HEADINGS = 0
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by putting a % sign in front of the word or
+# globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC = NO
+
+# If one adds a struct or class to a group and this option is enabled, then also
+# any nested class or struct is added to the same group. By default this option
+# is disabled and one has to add nested compounds explicitly via \ingroup.
+# The default value is: NO.
+
+GROUP_NESTED_COMPOUNDS = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL = NO
+
+# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE = YES
+
+# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE = NO
+
+# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC = YES
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO,
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES = YES
+
+# This flag is only useful for Objective-C code. If set to YES, local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO, only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO, these classes will be included in the various overviews. This option
+# has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO, these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO, these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES, upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES = YES
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES, the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
+# append additional text to a page's title, such as Class Reference. If set to
+# YES the compound reference will be hidden.
+# The default value is: NO.
+
+HIDE_COMPOUND_REFERENCE= NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
+# list. This list is created by putting \todo commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
+# list. This list is created by putting \test commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST = YES
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES, the
+# list will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. See also \cite for info how to create references.
+
+CITE_BIB_FILES =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO, doxygen will only warn about wrong or incomplete
+# parameter documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC = NO
+
+# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
+# a warning is encountered.
+# The default value is: NO.
+
+WARN_AS_ERROR = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = README.md CHANGELOG.md incl/ api/ src/
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# read by doxygen.
+#
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
+# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f95, *.f03, *.f08,
+# *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf and *.qsf.
+
+FILE_PATTERNS = *.c \
+ *.cc \
+ *.cxx \
+ *.cpp \
+ *.c++ \
+ *.java \
+ *.ii \
+ *.ixx \
+ *.ipp \
+ *.i++ \
+ *.inl \
+ *.idl \
+ *.ddl \
+ *.odl \
+ *.h \
+ *.hh \
+ *.hxx \
+ *.hpp \
+ *.h++ \
+ *.cs \
+ *.d \
+ *.php \
+ *.php4 \
+ *.php5 \
+ *.phtml \
+ *.inc \
+ *.m \
+ *.markdown \
+ *.md \
+ *.mm \
+ *.dox \
+ *.py \
+ *.pyw \
+ *.f90 \
+ *.f95 \
+ *.f03 \
+ *.f08 \
+ *.f \
+ *.for \
+ *.tcl \
+ *.vhd \
+ *.vhdl \
+ *.ucf \
+ *.qsf
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE = NO
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS =
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH =
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS = *
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+INPUT_FILTER =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+FILTER_PATTERNS =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE = README.md
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS = YES
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
+# clang parser (see: http://clang.llvm.org/) for more accurate parsing at the
+# cost of reduced performance. This can be particularly helpful with template
+# rich C++ code for which doxygen's built-in parser lacks the necessary type
+# information.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse-libclang=ON option for CMake.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = NO
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# cascading style sheets that are included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefore more robust against future updates.
+# Doxygen will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list). For an example see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES = LICENSE
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the style sheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP = NO
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler (hhc.exe). If non-empty,
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated
+# (YES) or that it should be included in the master .chm file (NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated
+# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW = NO
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH = 250
+
+# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX = YES
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. The package can be specified just
+# by its name or with the correct syntax as to be used with the LaTeX
+# \usepackage command. To get the times font for instance you can specify :
+# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
+# To use the option intlimits with the amsmath package you can specify:
+# EXTRA_PACKAGES=[intlimits]{amsmath}
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
+# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
+# string, for the replacement values of the other commands the user is referred
+# to HTML_HEADER.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer. See
+# LATEX_HEADER for more information on how to generate a default footer and what
+# special commands can be used inside the footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER =
+
+# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# LaTeX style sheets that are included after the standard style sheets created
+# by doxygen. Using this option one can overrule certain style aspects. Doxygen
+# will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_STYLESHEET =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS = YES
+
+# If the USE_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES, to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE = plain
+
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE =
+
+# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
+# with syntax highlighting in the RTF output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_SOURCE_CODE = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT = docbook
+
+# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
+# program listings (including syntax highlighting and cross-referencing
+# information) to the DOCBOOK output. Note that enabling this will significantly
+# increase the size of the DOCBOOK output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
+# AutoGen Definitions (see http://autogen.sf.net) file that captures the
+# structure of the code including all documentation. Note that this feature is
+# still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO, the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING = YES
+
+# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
+# in the source code. If set to NO, only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF = NO
+
+# If the SEARCH_INCLUDES tag is set to YES, the include files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED =
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES =
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE =
+
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
+# The default value is: NO.
+
+ALLEXTERNALS = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
+# in the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS = YES
+
+# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH =
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH =
+
+# If set to YES the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT = YES
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS = 0
+
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command. Disabling a call graph can be
+# accomplished by means of the command \hidecallgraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command. Disabling a caller graph can be
+# accomplished by means of the command \hidecallergraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. For an explanation of the image formats see the section
+# output formats in the documentation of the dot tool (Graphviz (see:
+# http://www.graphviz.org/)).
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
+# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
+# png:gdiplus:gdiplus.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS =
+
+# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
+
+PLANTUML_JAR_PATH =
+
+# When using plantuml, the PLANTUML_CFG_FILE tag can be used to specify a
+# configuration file for plantuml.
+
+PLANTUML_CFG_FILE =
+
+# When using plantuml, the specified paths are searched for files specified by
+# the !include statement in a plantuml block.
+
+PLANTUML_INCLUDE_PATH =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP = YES
--- /dev/null
+Copyright (c) <year> <owner>. All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
--- /dev/null
+Creative Commons Legal Code
+
+CC0 1.0 Universal
+
+ CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE
+ LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN
+ ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS
+ INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES
+ REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS
+ PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM
+ THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED
+ HEREUNDER.
+
+Statement of Purpose
+
+The laws of most jurisdictions throughout the world automatically confer
+exclusive Copyright and Related Rights (defined below) upon the creator
+and subsequent owner(s) (each and all, an "owner") of an original work of
+authorship and/or a database (each, a "Work").
+
+Certain owners wish to permanently relinquish those rights to a Work for
+the purpose of contributing to a commons of creative, cultural and
+scientific works ("Commons") that the public can reliably and without fear
+of later claims of infringement build upon, modify, incorporate in other
+works, reuse and redistribute as freely as possible in any form whatsoever
+and for any purposes, including without limitation commercial purposes.
+These owners may contribute to the Commons to promote the ideal of a free
+culture and the further production of creative, cultural and scientific
+works, or to gain reputation or greater distribution for their Work in
+part through the use and efforts of others.
+
+For these and/or other purposes and motivations, and without any
+expectation of additional consideration or compensation, the person
+associating CC0 with a Work (the "Affirmer"), to the extent that he or she
+is an owner of Copyright and Related Rights in the Work, voluntarily
+elects to apply CC0 to the Work and publicly distribute the Work under its
+terms, with knowledge of his or her Copyright and Related Rights in the
+Work and the meaning and intended legal effect of CC0 on those rights.
+
+1. Copyright and Related Rights. A Work made available under CC0 may be
+protected by copyright and related or neighboring rights ("Copyright and
+Related Rights"). Copyright and Related Rights include, but are not
+limited to, the following:
+
+ i. the right to reproduce, adapt, distribute, perform, display,
+ communicate, and translate a Work;
+ ii. moral rights retained by the original author(s) and/or performer(s);
+iii. publicity and privacy rights pertaining to a person's image or
+ likeness depicted in a Work;
+ iv. rights protecting against unfair competition in regards to a Work,
+ subject to the limitations in paragraph 4(a), below;
+ v. rights protecting the extraction, dissemination, use and reuse of data
+ in a Work;
+ vi. database rights (such as those arising under Directive 96/9/EC of the
+ European Parliament and of the Council of 11 March 1996 on the legal
+ protection of databases, and under any national implementation
+ thereof, including any amended or successor version of such
+ directive); and
+vii. other similar, equivalent or corresponding rights throughout the
+ world based on applicable law or treaty, and any national
+ implementations thereof.
+
+2. Waiver. To the greatest extent permitted by, but not in contravention
+of, applicable law, Affirmer hereby overtly, fully, permanently,
+irrevocably and unconditionally waives, abandons, and surrenders all of
+Affirmer's Copyright and Related Rights and associated claims and causes
+of action, whether now known or unknown (including existing as well as
+future claims and causes of action), in the Work (i) in all territories
+worldwide, (ii) for the maximum duration provided by applicable law or
+treaty (including future time extensions), (iii) in any current or future
+medium and for any number of copies, and (iv) for any purpose whatsoever,
+including without limitation commercial, advertising or promotional
+purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each
+member of the public at large and to the detriment of Affirmer's heirs and
+successors, fully intending that such Waiver shall not be subject to
+revocation, rescission, cancellation, termination, or any other legal or
+equitable action to disrupt the quiet enjoyment of the Work by the public
+as contemplated by Affirmer's express Statement of Purpose.
+
+3. Public License Fallback. Should any part of the Waiver for any reason
+be judged legally invalid or ineffective under applicable law, then the
+Waiver shall be preserved to the maximum extent permitted taking into
+account Affirmer's express Statement of Purpose. In addition, to the
+extent the Waiver is so judged Affirmer hereby grants to each affected
+person a royalty-free, non transferable, non sublicensable, non exclusive,
+irrevocable and unconditional license to exercise Affirmer's Copyright and
+Related Rights in the Work (i) in all territories worldwide, (ii) for the
+maximum duration provided by applicable law or treaty (including future
+time extensions), (iii) in any current or future medium and for any number
+of copies, and (iv) for any purpose whatsoever, including without
+limitation commercial, advertising or promotional purposes (the
+"License"). The License shall be deemed effective as of the date CC0 was
+applied by Affirmer to the Work. Should any part of the License for any
+reason be judged legally invalid or ineffective under applicable law, such
+partial invalidity or ineffectiveness shall not invalidate the remainder
+of the License, and in such case Affirmer hereby affirms that he or she
+will not (i) exercise any of his or her remaining Copyright and Related
+Rights in the Work or (ii) assert any associated claims and causes of
+action with respect to the Work, in either case contrary to Affirmer's
+express Statement of Purpose.
+
+4. Limitations and Disclaimers.
+
+ a. No trademark or patent rights held by Affirmer are waived, abandoned,
+ surrendered, licensed or otherwise affected by this document.
+ b. Affirmer offers the Work as-is and makes no representations or
+ warranties of any kind concerning the Work, express, implied,
+ statutory or otherwise, including without limitation warranties of
+ title, merchantability, fitness for a particular purpose, non
+ infringement, or the absence of latent or other defects, accuracy, or
+ the present or absence of errors, whether or not discoverable, all to
+ the greatest extent permissible under applicable law.
+ c. Affirmer disclaims responsibility for clearing rights of other persons
+ that may apply to the Work or any use thereof, including without
+ limitation any person's Copyright and Related Rights in the Work.
+ Further, Affirmer disclaims responsibility for obtaining any necessary
+ consents, permissions or other rights required for any use of the
+ Work.
+ d. Affirmer understands and acknowledges that Creative Commons is not a
+ party to this document and has no duty or obligation with respect to
+ this CC0 or use of the Work.
--- /dev/null
+GNU GENERAL PUBLIC LICENSE
+Version 3, 29 June 2007
+
+Copyright © 2007 Free Software Foundation, Inc. <http://fsf.org/>
+
+Everyone is permitted to copy and distribute verbatim copies of this license document, but changing it is not allowed.
+
+Preamble
+
+The GNU General Public License is a free, copyleft license for software and other kinds of works.
+
+The licenses for most software and other practical works are designed to take away your freedom to share and change the works. By contrast, the GNU General Public License is intended to guarantee your freedom to share and change all versions of a program--to make sure it remains free software for all its users. We, the Free Software Foundation, use the GNU General Public License for most of our software; it applies also to any other work released this way by its authors. You can apply it to your programs, too.
+
+When we speak of free software, we are referring to freedom, not price. Our General Public Licenses are designed to make sure that you have the freedom to distribute copies of free software (and charge for them if you wish), that you receive source code or can get it if you want it, that you can change the software or use pieces of it in new free programs, and that you know you can do these things.
+
+To protect your rights, we need to prevent others from denying you these rights or asking you to surrender the rights. Therefore, you have certain responsibilities if you distribute copies of the software, or if you modify it: responsibilities to respect the freedom of others.
+
+For example, if you distribute copies of such a program, whether gratis or for a fee, you must pass on to the recipients the same freedoms that you received. You must make sure that they, too, receive or can get the source code. And you must show them these terms so they know their rights.
+
+Developers that use the GNU GPL protect your rights with two steps: (1) assert copyright on the software, and (2) offer you this License giving you legal permission to copy, distribute and/or modify it.
+
+For the developers' and authors' protection, the GPL clearly explains that there is no warranty for this free software. For both users' and authors' sake, the GPL requires that modified versions be marked as changed, so that their problems will not be attributed erroneously to authors of previous versions.
+
+Some devices are designed to deny users access to install or run modified versions of the software inside them, although the manufacturer can do so. This is fundamentally incompatible with the aim of protecting users' freedom to change the software. The systematic pattern of such abuse occurs in the area of products for individuals to use, which is precisely where it is most unacceptable. Therefore, we have designed this version of the GPL to prohibit the practice for those products. If such problems arise substantially in other domains, we stand ready to extend this provision to those domains in future versions of the GPL, as needed to protect the freedom of users.
+
+Finally, every program is threatened constantly by software patents. States should not allow patents to restrict development and use of software on general-purpose computers, but in those that do, we wish to avoid the special danger that patents applied to a free program could make it effectively proprietary. To prevent this, the GPL assures that patents cannot be used to render the program non-free.
+
+The precise terms and conditions for copying, distribution and modification follow.
+
+TERMS AND CONDITIONS
+
+0. Definitions.
+
+“This License” refers to version 3 of the GNU General Public License.
+
+“Copyright” also means copyright-like laws that apply to other kinds of works, such as semiconductor masks.
+
+“The Program” refers to any copyrightable work licensed under this License. Each licensee is addressed as “you”. “Licensees” and “recipients” may be individuals or organizations.
+
+To “modify” a work means to copy from or adapt all or part of the work in a fashion requiring copyright permission, other than the making of an exact copy. The resulting work is called a “modified version” of the earlier work or a work “based on” the earlier work.
+
+A “covered work” means either the unmodified Program or a work based on the Program.
+
+To “propagate” a work means to do anything with it that, without permission, would make you directly or secondarily liable for infringement under applicable copyright law, except executing it on a computer or modifying a private copy. Propagation includes copying, distribution (with or without modification), making available to the public, and in some countries other activities as well.
+
+To “convey” a work means any kind of propagation that enables other parties to make or receive copies. Mere interaction with a user through a computer network, with no transfer of a copy, is not conveying.
+
+An interactive user interface displays “Appropriate Legal Notices” to the extent that it includes a convenient and prominently visible feature that (1) displays an appropriate copyright notice, and (2) tells the user that there is no warranty for the work (except to the extent that warranties are provided), that licensees may convey the work under this License, and how to view a copy of this License. If the interface presents a list of user commands or options, such as a menu, a prominent item in the list meets this criterion.
+
+1. Source Code.
+The “source code” for a work means the preferred form of the work for making modifications to it. “Object code” means any non-source form of a work.
+
+A “Standard Interface” means an interface that either is an official standard defined by a recognized standards body, or, in the case of interfaces specified for a particular programming language, one that is widely used among developers working in that language.
+
+The “System Libraries” of an executable work include anything, other than the work as a whole, that (a) is included in the normal form of packaging a Major Component, but which is not part of that Major Component, and (b) serves only to enable use of the work with that Major Component, or to implement a Standard Interface for which an implementation is available to the public in source code form. A “Major Component”, in this context, means a major essential component (kernel, window system, and so on) of the specific operating system (if any) on which the executable work runs, or a compiler used to produce the work, or an object code interpreter used to run it.
+
+The “Corresponding Source” for a work in object code form means all the source code needed to generate, install, and (for an executable work) run the object code and to modify the work, including scripts to control those activities. However, it does not include the work's System Libraries, or general-purpose tools or generally available free programs which are used unmodified in performing those activities but which are not part of the work. For example, Corresponding Source includes interface definition files associated with source files for the work, and the source code for shared libraries and dynamically linked subprograms that the work is specifically designed to require, such as by intimate data communication or control flow between those subprograms and other parts of the work.
+
+The Corresponding Source need not include anything that users can regenerate automatically from other parts of the Corresponding Source.
+
+The Corresponding Source for a work in source code form is that same work.
+
+2. Basic Permissions.
+All rights granted under this License are granted for the term of copyright on the Program, and are irrevocable provided the stated conditions are met. This License explicitly affirms your unlimited permission to run the unmodified Program. The output from running a covered work is covered by this License only if the output, given its content, constitutes a covered work. This License acknowledges your rights of fair use or other equivalent, as provided by copyright law.
+
+You may make, run and propagate covered works that you do not convey, without conditions so long as your license otherwise remains in force. You may convey covered works to others for the sole purpose of having them make modifications exclusively for you, or provide you with facilities for running those works, provided that you comply with the terms of this License in conveying all material for which you do not control copyright. Those thus making or running the covered works for you must do so exclusively on your behalf, under your direction and control, on terms that prohibit them from making any copies of your copyrighted material outside their relationship with you.
+
+Conveying under any other circumstances is permitted solely under the conditions stated below. Sublicensing is not allowed; section 10 makes it unnecessary.
+
+3. Protecting Users' Legal Rights From Anti-Circumvention Law.
+No covered work shall be deemed part of an effective technological measure under any applicable law fulfilling obligations under article 11 of the WIPO copyright treaty adopted on 20 December 1996, or similar laws prohibiting or restricting circumvention of such measures.
+
+When you convey a covered work, you waive any legal power to forbid circumvention of technological measures to the extent such circumvention is effected by exercising rights under this License with respect to the covered work, and you disclaim any intention to limit operation or modification of the work as a means of enforcing, against the work's users, your or third parties' legal rights to forbid circumvention of technological measures.
+
+4. Conveying Verbatim Copies.
+You may convey verbatim copies of the Program's source code as you receive it, in any medium, provided that you conspicuously and appropriately publish on each copy an appropriate copyright notice; keep intact all notices stating that this License and any non-permissive terms added in accord with section 7 apply to the code; keep intact all notices of the absence of any warranty; and give all recipients a copy of this License along with the Program.
+
+You may charge any price or no price for each copy that you convey, and you may offer support or warranty protection for a fee.
+
+5. Conveying Modified Source Versions.
+You may convey a work based on the Program, or the modifications to produce it from the Program, in the form of source code under the terms of section 4, provided that you also meet all of these conditions:
+
+ a) The work must carry prominent notices stating that you modified it, and giving a relevant date.
+
+ b) The work must carry prominent notices stating that it is released under this License and any conditions added under section 7. This requirement modifies the requirement in section 4 to “keep intact all notices”.
+
+ c) You must license the entire work, as a whole, under this License to anyone who comes into possession of a copy. This License will therefore apply, along with any applicable section 7 additional terms, to the whole of the work, and all its parts, regardless of how they are packaged. This License gives no permission to license the work in any other way, but it does not invalidate such permission if you have separately received it.
+
+ d) If the work has interactive user interfaces, each must display Appropriate Legal Notices; however, if the Program has interactive interfaces that do not display Appropriate Legal Notices, your work need not make them do so.
+
+A compilation of a covered work with other separate and independent works, which are not by their nature extensions of the covered work, and which are not combined with it such as to form a larger program, in or on a volume of a storage or distribution medium, is called an “aggregate” if the compilation and its resulting copyright are not used to limit the access or legal rights of the compilation's users beyond what the individual works permit. Inclusion of a covered work in an aggregate does not cause this License to apply to the other parts of the aggregate.
+
+6. Conveying Non-Source Forms.
+You may convey a covered work in object code form under the terms of sections 4 and 5, provided that you also convey the machine-readable Corresponding Source under the terms of this License, in one of these ways:
+
+ a) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by the Corresponding Source fixed on a durable physical medium customarily used for software interchange.
+
+ b) Convey the object code in, or embodied in, a physical product (including a physical distribution medium), accompanied by a written offer, valid for at least three years and valid for as long as you offer spare parts or customer support for that product model, to give anyone who possesses the object code either (1) a copy of the Corresponding Source for all the software in the product that is covered by this License, on a durable physical medium customarily used for software interchange, for a price no more than your reasonable cost of physically performing this conveying of source, or (2) access to copy the Corresponding Source from a network server at no charge.
+
+ c) Convey individual copies of the object code with a copy of the written offer to provide the Corresponding Source. This alternative is allowed only occasionally and noncommercially, and only if you received the object code with such an offer, in accord with subsection 6b.
+
+ d) Convey the object code by offering access from a designated place (gratis or for a charge), and offer equivalent access to the Corresponding Source in the same way through the same place at no further charge. You need not require recipients to copy the Corresponding Source along with the object code. If the place to copy the object code is a network server, the Corresponding Source may be on a different server (operated by you or a third party) that supports equivalent copying facilities, provided you maintain clear directions next to the object code saying where to find the Corresponding Source. Regardless of what server hosts the Corresponding Source, you remain obligated to ensure that it is available for as long as needed to satisfy these requirements.
+
+ e) Convey the object code using peer-to-peer transmission, provided you inform other peers where the object code and Corresponding Source of the work are being offered to the general public at no charge under subsection 6d.
+
+A separable portion of the object code, whose source code is excluded from the Corresponding Source as a System Library, need not be included in conveying the object code work.
+
+A “User Product” is either (1) a “consumer product”, which means any tangible personal property which is normally used for personal, family, or household purposes, or (2) anything designed or sold for incorporation into a dwelling. In determining whether a product is a consumer product, doubtful cases shall be resolved in favor of coverage. For a particular product received by a particular user, “normally used” refers to a typical or common use of that class of product, regardless of the status of the particular user or of the way in which the particular user actually uses, or expects or is expected to use, the product. A product is a consumer product regardless of whether the product has substantial commercial, industrial or non-consumer uses, unless such uses represent the only significant mode of use of the product.
+
+“Installation Information” for a User Product means any methods, procedures, authorization keys, or other information required to install and execute modified versions of a covered work in that User Product from a modified version of its Corresponding Source. The information must suffice to ensure that the continued functioning of the modified object code is in no case prevented or interfered with solely because modification has been made.
+
+If you convey an object code work under this section in, or with, or specifically for use in, a User Product, and the conveying occurs as part of a transaction in which the right of possession and use of the User Product is transferred to the recipient in perpetuity or for a fixed term (regardless of how the transaction is characterized), the Corresponding Source conveyed under this section must be accompanied by the Installation Information. But this requirement does not apply if neither you nor any third party retains the ability to install modified object code on the User Product (for example, the work has been installed in ROM).
+
+The requirement to provide Installation Information does not include a requirement to continue to provide support service, warranty, or updates for a work that has been modified or installed by the recipient, or for the User Product in which it has been modified or installed. Access to a network may be denied when the modification itself materially and adversely affects the operation of the network or violates the rules and protocols for communication across the network.
+
+Corresponding Source conveyed, and Installation Information provided, in accord with this section must be in a format that is publicly documented (and with an implementation available to the public in source code form), and must require no special password or key for unpacking, reading or copying.
+
+7. Additional Terms.
+“Additional permissions” are terms that supplement the terms of this License by making exceptions from one or more of its conditions. Additional permissions that are applicable to the entire Program shall be treated as though they were included in this License, to the extent that they are valid under applicable law. If additional permissions apply only to part of the Program, that part may be used separately under those permissions, but the entire Program remains governed by this License without regard to the additional permissions.
+
+When you convey a copy of a covered work, you may at your option remove any additional permissions from that copy, or from any part of it. (Additional permissions may be written to require their own removal in certain cases when you modify the work.) You may place additional permissions on material, added by you to a covered work, for which you have or can give appropriate copyright permission.
+
+Notwithstanding any other provision of this License, for material you add to a covered work, you may (if authorized by the copyright holders of that material) supplement the terms of this License with terms:
+
+ a) Disclaiming warranty or limiting liability differently from the terms of sections 15 and 16 of this License; or
+
+ b) Requiring preservation of specified reasonable legal notices or author attributions in that material or in the Appropriate Legal Notices displayed by works containing it; or
+
+ c) Prohibiting misrepresentation of the origin of that material, or requiring that modified versions of such material be marked in reasonable ways as different from the original version; or
+
+ d) Limiting the use for publicity purposes of names of licensors or authors of the material; or
+
+ e) Declining to grant rights under trademark law for use of some trade names, trademarks, or service marks; or
+
+ f) Requiring indemnification of licensors and authors of that material by anyone who conveys the material (or modified versions of it) with contractual assumptions of liability to the recipient, for any liability that these contractual assumptions directly impose on those licensors and authors.
+
+All other non-permissive additional terms are considered “further restrictions” within the meaning of section 10. If the Program as you received it, or any part of it, contains a notice stating that it is governed by this License along with a term that is a further restriction, you may remove that term. If a license document contains a further restriction but permits relicensing or conveying under this License, you may add to a covered work material governed by the terms of that license document, provided that the further restriction does not survive such relicensing or conveying.
+
+If you add terms to a covered work in accord with this section, you must place, in the relevant source files, a statement of the additional terms that apply to those files, or a notice indicating where to find the applicable terms.
+
+Additional terms, permissive or non-permissive, may be stated in the form of a separately written license, or stated as exceptions; the above requirements apply either way.
+
+8. Termination.
+You may not propagate or modify a covered work except as expressly provided under this License. Any attempt otherwise to propagate or modify it is void, and will automatically terminate your rights under this License (including any patent licenses granted under the third paragraph of section 11).
+
+However, if you cease all violation of this License, then your license from a particular copyright holder is reinstated (a) provisionally, unless and until the copyright holder explicitly and finally terminates your license, and (b) permanently, if the copyright holder fails to notify you of the violation by some reasonable means prior to 60 days after the cessation.
+
+Moreover, your license from a particular copyright holder is reinstated permanently if the copyright holder notifies you of the violation by some reasonable means, this is the first time you have received notice of violation of this License (for any work) from that copyright holder, and you cure the violation prior to 30 days after your receipt of the notice.
+
+Termination of your rights under this section does not terminate the licenses of parties who have received copies or rights from you under this License. If your rights have been terminated and not permanently reinstated, you do not qualify to receive new licenses for the same material under section 10.
+
+9. Acceptance Not Required for Having Copies.
+You are not required to accept this License in order to receive or run a copy of the Program. Ancillary propagation of a covered work occurring solely as a consequence of using peer-to-peer transmission to receive a copy likewise does not require acceptance. However, nothing other than this License grants you permission to propagate or modify any covered work. These actions infringe copyright if you do not accept this License. Therefore, by modifying or propagating a covered work, you indicate your acceptance of this License to do so.
+
+10. Automatic Licensing of Downstream Recipients.
+Each time you convey a covered work, the recipient automatically receives a license from the original licensors, to run, modify and propagate that work, subject to this License. You are not responsible for enforcing compliance by third parties with this License.
+
+An “entity transaction” is a transaction transferring control of an organization, or substantially all assets of one, or subdividing an organization, or merging organizations. If propagation of a covered work results from an entity transaction, each party to that transaction who receives a copy of the work also receives whatever licenses to the work the party's predecessor in interest had or could give under the previous paragraph, plus a right to possession of the Corresponding Source of the work from the predecessor in interest, if the predecessor has it or can get it with reasonable efforts.
+
+You may not impose any further restrictions on the exercise of the rights granted or affirmed under this License. For example, you may not impose a license fee, royalty, or other charge for exercise of rights granted under this License, and you may not initiate litigation (including a cross-claim or counterclaim in a lawsuit) alleging that any patent claim is infringed by making, using, selling, offering for sale, or importing the Program or any portion of it.
+
+11. Patents.
+A “contributor” is a copyright holder who authorizes use under this License of the Program or a work on which the Program is based. The work thus licensed is called the contributor's “contributor version”.
+
+A contributor's “essential patent claims” are all patent claims owned or controlled by the contributor, whether already acquired or hereafter acquired, that would be infringed by some manner, permitted by this License, of making, using, or selling its contributor version, but do not include claims that would be infringed only as a consequence of further modification of the contributor version. For purposes of this definition, “control” includes the right to grant patent sublicenses in a manner consistent with the requirements of this License.
+
+Each contributor grants you a non-exclusive, worldwide, royalty-free patent license under the contributor's essential patent claims, to make, use, sell, offer for sale, import and otherwise run, modify and propagate the contents of its contributor version.
+
+In the following three paragraphs, a “patent license” is any express agreement or commitment, however denominated, not to enforce a patent (such as an express permission to practice a patent or covenant not to sue for patent infringement). To “grant” such a patent license to a party means to make such an agreement or commitment not to enforce a patent against the party.
+
+If you convey a covered work, knowingly relying on a patent license, and the Corresponding Source of the work is not available for anyone to copy, free of charge and under the terms of this License, through a publicly available network server or other readily accessible means, then you must either (1) cause the Corresponding Source to be so available, or (2) arrange to deprive yourself of the benefit of the patent license for this particular work, or (3) arrange, in a manner consistent with the requirements of this License, to extend the patent license to downstream recipients. “Knowingly relying” means you have actual knowledge that, but for the patent license, your conveying the covered work in a country, or your recipient's use of the covered work in a country, would infringe one or more identifiable patents in that country that you have reason to believe are valid.
+
+If, pursuant to or in connection with a single transaction or arrangement, you convey, or propagate by procuring conveyance of, a covered work, and grant a patent license to some of the parties receiving the covered work authorizing them to use, propagate, modify or convey a specific copy of the covered work, then the patent license you grant is automatically extended to all recipients of the covered work and works based on it.
+
+A patent license is “discriminatory” if it does not include within the scope of its coverage, prohibits the exercise of, or is conditioned on the non-exercise of one or more of the rights that are specifically granted under this License. You may not convey a covered work if you are a party to an arrangement with a third party that is in the business of distributing software, under which you make payment to the third party based on the extent of your activity of conveying the work, and under which the third party grants, to any of the parties who would receive the covered work from you, a discriminatory patent license (a) in connection with copies of the covered work conveyed by you (or copies made from those copies), or (b) primarily for and in connection with specific products or compilations that contain the covered work, unless you entered into that arrangement, or that patent license was granted, prior to 28 March 2007.
+
+Nothing in this License shall be construed as excluding or limiting any implied license or other defenses to infringement that may otherwise be available to you under applicable patent law.
+
+12. No Surrender of Others' Freedom.
+If conditions are imposed on you (whether by court order, agreement or otherwise) that contradict the conditions of this License, they do not excuse you from the conditions of this License. If you cannot convey a covered work so as to satisfy simultaneously your obligations under this License and any other pertinent obligations, then as a consequence you may not convey it at all. For example, if you agree to terms that obligate you to collect a royalty for further conveying from those to whom you convey the Program, the only way you could satisfy both those terms and this License would be to refrain entirely from conveying the Program.
+
+13. Use with the GNU Affero General Public License.
+Notwithstanding any other provision of this License, you have permission to link or combine any covered work with a work licensed under version 3 of the GNU Affero General Public License into a single combined work, and to convey the resulting work. The terms of this License will continue to apply to the part which is the covered work, but the special requirements of the GNU Affero General Public License, section 13, concerning interaction through a network will apply to the combination as such.
+
+14. Revised Versions of this License.
+The Free Software Foundation may publish revised and/or new versions of the GNU General Public License from time to time. Such new versions will be similar in spirit to the present version, but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Program specifies that a certain numbered version of the GNU General Public License “or any later version” applies to it, you have the option of following the terms and conditions either of that numbered version or of any later version published by the Free Software Foundation. If the Program does not specify a version number of the GNU General Public License, you may choose any version ever published by the Free Software Foundation.
+
+If the Program specifies that a proxy can decide which future versions of the GNU General Public License can be used, that proxy's public statement of acceptance of a version permanently authorizes you to choose that version for the Program.
+
+Later license versions may give you additional or different permissions. However, no additional obligations are imposed on any author or copyright holder as a result of your choosing to follow a later version.
+
+15. Disclaimer of Warranty.
+THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM “AS IS” WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+16. Limitation of Liability.
+IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGES.
+
+17. Interpretation of Sections 15 and 16.
+If the disclaimer of warranty and limitation of liability provided above cannot be given local legal effect according to their terms, reviewing courts shall apply local law that most closely approximates an absolute waiver of all civil liability in connection with the Program, unless a warranty or assumption of liability accompanies a copy of the Program in return for a fee.
+
+END OF TERMS AND CONDITIONS
+
+How to Apply These Terms to Your New Programs
+
+If you develop a new program, and you want it to be of the greatest possible use to the public, the best way to achieve this is to make it free software which everyone can redistribute and change under these terms.
+
+To do so, attach the following notices to the program. It is safest to attach them to the start of each source file to most effectively state the exclusion of warranty; and each file should have at least the “copyright” line and a pointer to where the full notice is found.
+
+ <one line to give the program's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program does terminal interaction, make it output a short notice like this when it starts in an interactive mode:
+
+ <program> Copyright (C) <year> <name of author>
+ This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+ This is free software, and you are welcome to redistribute it under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate parts of the General Public License. Of course, your program's commands might be different; for a GUI interface, you would use an “about box”.
+
+You should also get your employer (if you work as a programmer) or school, if any, to sign a “copyright disclaimer” for the program, if necessary. For more information on this, and how to apply and follow the GNU GPL, see <http://www.gnu.org/licenses/>.
+
+The GNU General Public License does not permit incorporating your program into proprietary programs. If your program is a subroutine library, you may consider it more useful to permit linking proprietary applications with the library. If this is what you want to do, use the GNU Lesser General Public License instead of this License. But first, please read <http://www.gnu.org/philosophy/why-not-lgpl.html>.
--- /dev/null
+MIT License
+
+Copyright (c) <year> <copyright holders>
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
--- /dev/null
+This is free and unencumbered software released into the public domain.
+
+Anyone is free to copy, modify, publish, use, compile, sell, or distribute this software, either in source code form or as a compiled binary, for any purpose, commercial or non-commercial, and by any means.
+
+In jurisdictions that recognize copyright laws, the author or authors of this software dedicate any and all copyright interest in the software to the public domain. We make this dedication for the benefit of the public at large and to the detriment of our heirs and
+successors. We intend this dedication to be an overt act of relinquishment in perpetuity of all present and future rights to this software under copyright law.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
+For more information, please refer to <http://unlicense.org/>
--- /dev/null
+zlib License
+
+This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
+
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+
+ 3. This notice may not be removed or altered from any source distribution.
--- /dev/null
+<!--
+SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+
+SPDX-License-Identifier: GPL-3.0-only
+-->
+
+RRT\* algorithm
+===============
+
+RRTS is a C++ library with implementation of RRT\* planning algorithm.
+
+License
+-------
+
+The project is published under [GNU GPLv3][1].
+
+[1]: ./LICENSE
+
+Dependencies
+------------
+
+- `libbcar` (as submodule)
+- `libjsoncpp-dev`
+
+Build
+-----
+
+To build the project run the following commands:
+
+ mkdir build
+ cd build
+ cmake ../
+ make
+
+To build with ninja:
+
+ mkdir build
+ cd build
+ cmake -DCMAKE_BUILD_TYPE=Release -G Ninja ../
+ ninja -v
+
+Contribute
+----------
+
+Write [great git commit messages][5]:
+
+1. Separate subject from body with a blank line.
+2. Limit the subject line to 50 characters.
+3. Capitalize the subject line.
+4. Do not end the subject line with a period.
+5. Use the imperative mood in the subject line.
+6. Wrap the body at 72 characters.
+7. Use the body to explain what and why vs. how.
+
+[5]: https://chris.beams.io/posts/git-commit/
+
+Use [git send-email][6]:
+
+ git config format.subjectPrefix "PATCH rrts"
+ git config sendemail.to "jiri.vlasak.2@cvut.cz"
+
+[6]: https://git-send-email.io/
+
+Documentation
+-------------
+
+The documentation is generated by Doxygen, at least version `1.8.15` is needed.
+Just run `doxygen` in the project root directory.
+
+
+RRT Extensions
+==============
+
+There is basic RRT\* algorithm in `rrts.cc` file. To test different approaches
+and upgrades to RRT, _extensions_ are declared in `rrtext.hh` and implemented in
+`src/rrtextN.cc`, where `N` is the number of an extension.
+
+For more information, see the `incl/rrtext.hh` header file or the generated
+documentation.
+
+RRT extensions are not to be used as the final planner. Instead, the _RRT*
+planners_ declared in `incl/rrtsp.hh` are to be used as the final planner.
--- /dev/null
+// Copyright (c) 2017 Randy Gaul http://www.randygaul.net
+// SPDX-FileCopyrightText: 2017 Randy Gaul http://www.randygaul.net
+// SPDX-FileCopyrightText: 2021 Randy Gaul http://www.randygaul.net
+//
+// SPDX-License-Identifier: Unlicense
+// SPDX-License-Identifier: Zlib
+/*
+ ------------------------------------------------------------------------------
+ Licensing information can be found at the end of the file.
+ ------------------------------------------------------------------------------
+
+ cute_c2.h - v1.10
+
+ To create implementation (the function definitions)
+ #define CUTE_C2_IMPLEMENTATION
+ in *one* C/CPP file (translation unit) that includes this file
+
+
+ SUMMARY
+
+ cute_c2 is a single-file header that implements 2D collision detection routines
+ that test for overlap, and optionally can find the collision manifold. The
+ manifold contains all necessary information to prevent shapes from inter-
+ penetrating, which is useful for character controllers, general physics
+ simulation, and user-interface programming.
+
+ This header implements a group of "immediate mode" functions that should be
+ very easily adapted into pre-existing projects.
+
+
+ THE IMPORTANT PARTS
+
+ Most of the math types in this header are for internal use. Users care about
+ the shape types and the collision functions.
+
+ SHAPE TYPES:
+ * c2Circle
+ * c2Capsule
+ * c2AABB
+ * c2Ray
+ * c2Poly
+
+ COLLISION FUNCTIONS (*** is a shape name from the above list):
+ * c2***to*** - boolean YES/NO hittest
+ * c2***to***Manifold - construct manifold to describe how shapes hit
+ * c2GJK - runs GJK algorithm to find closest point pair between two shapes
+ * c2TOI - computes the time of impact between two shapes, useful for sweeping shapes, or doing shape casts
+ * c2MakePoly - Runs convex hull algorithm and computes normals on input point-set
+ * c2Collided - generic version of c2***to*** funcs
+ * c2Collide - generic version of c2***to***Manifold funcs
+ * c2CastRay - generic version of c2Rayto*** funcs
+
+ The rest of the header is more or less for internal use. Here is an example of
+ making some shapes and testing for collision:
+
+ c2Circle c;
+ c.p = position;
+ c.r = radius;
+
+ c2Capsule cap;
+ cap.a = first_endpoint;
+ cap.b = second_endpoint;
+ cap.r = radius;
+
+ int hit = c2CircletoCapsule(c, cap);
+ if (hit)
+ {
+ handle collision here...
+ }
+
+ For more code examples and tests please see:
+ https://github.com/RandyGaul/cute_header/tree/master/examples_cute_gl_and_c2
+
+ Here is a past discussion thread on this header:
+ https://www.reddit.com/r/gamedev/comments/5tqyey/tinyc2_2d_collision_detection_library_in_c/
+
+ Here is a very nice repo containing various tests and examples using SFML for rendering:
+ https://github.com/sro5h/tinyc2-tests
+
+
+ FEATURES
+
+ * Circles, capsules, AABBs, rays and convex polygons are supported
+ * Fast boolean only result functions (hit yes/no)
+ * Slghtly slower manifold generation for collision normals + depths +points
+ * GJK implementation (finds closest points for disjoint pairs of shapes)
+ * Shape casts/sweeps with c2TOI function (time of impact)
+ * Robust 2D convex hull generator
+ * Lots of correctly implemented and tested 2D math routines
+ * Implemented in portable C, and is readily portable to other languages
+ * Generic c2Collide, c2Collided and c2CastRay function (can pass in any shape type)
+ * Extensive examples at: https://github.com/RandyGaul/cute_headers/tree/master/examples_cute_gl_and_c2
+
+
+ Revision History
+
+ 1.0 (02/13/2017) initial release
+ 1.01 (02/13/2017) const crusade, minor optimizations, capsule degen
+ 1.02 (03/21/2017) compile fixes for c on more compilers
+ 1.03 (09/15/2017) various bugfixes and quality of life changes to manifolds
+ 1.04 (03/25/2018) fixed manifold bug in c2CircletoAABBManifold
+ 1.05 (11/01/2018) added c2TOI (time of impact) for shape cast/sweep test
+ 1.06 (08/23/2019) C2_*** types to C2_TYPE_***, and CUTE_C2_API
+ 1.07 (10/19/2019) Optimizations to c2TOI - breaking change to c2GJK API
+ 1.08 (12/22/2019) Remove contact point + normal from c2TOI, removed feather
+ radius from c2GJK, fixed various bugs in capsule to poly
+ manifold, did a pass on all docs
+ 1.09 (07/27/2019) Added c2Inflate - to inflate/deflate shapes for c2TOI
+ 1.10 (02/05/2022) Implemented GJK-Raycast for c2TOI (from E. Catto's Box2D)
+
+
+ Contributors
+
+ Plastburk 1.01 - const pointers pull request
+ mmozeiko 1.02 - 3 compile bugfixes
+ felipefs 1.02 - 3 compile bugfixes
+ seemk 1.02 - fix branching bug in c2Collide
+ sro5h 1.02 - bug reports for multiple manifold funcs
+ sro5h 1.03 - work involving quality of life fixes for manifolds
+ Wizzard033 1.06 - C2_*** types to C2_TYPE_***, and CUTE_C2_API
+ Tyler Glaeil 1.08 - Lots of bug reports and disussion on capsules + TOIs
+
+
+ DETAILS/ADVICE
+
+ BROAD PHASE
+
+ This header does not implement a broad-phase, and instead concerns itself with
+ the narrow-phase. This means this header just checks to see if two individual
+ shapes are touching, and can give information about how they are touching.
+
+ Very common 2D broad-phases are tree and grid approaches. Quad trees are good
+ for static geometry that does not move much if at all. Dynamic AABB trees are
+ good for general purpose use, and can handle moving objects very well. Grids
+ are great and are similar to quad trees.
+
+ If implementing a grid it can be wise to have each collideable grid cell hold
+ an integer. This integer refers to a 2D shape that can be passed into the
+ various functions in this header. The shape can be transformed from "model"
+ space to "world" space using c2x -- a transform struct. In this way a grid
+ can be implemented that holds any kind of convex shape (that this header
+ supports) while conserving memory with shape instancing.
+
+ NUMERIC ROBUSTNESS
+
+ Many of the functions in cute c2 use `c2GJK`, an implementation of the GJK
+ algorithm. Internally GJK computes signed area values, and these values are
+ very numerically sensitive to large shapes. This means the GJK function will
+ break down if input shapes are too large or too far away from the origin.
+
+ In general it is best to compute collision detection on small shapes very
+ close to the origin. One trick is to keep your collision information numerically
+ very tiny, and simply scale it up when rendering to the appropriate size.
+
+ For reference, if your shapes are all AABBs and contain a width and height
+ of somewhere between 1.0f and 10.0f, everything will be fine. However, once
+ your shapes start approaching a width/height of 100.0f to 1000.0f GJK can
+ start breaking down.
+
+ This is a complicated topic, so feel free to ask the author for advice here.
+
+ Here is an example demonstrating this problem with two large AABBs:
+ https://github.com/RandyGaul/cute_headers/issues/160
+
+ Please email at my address with any questions or comments at:
+ author's last name followed by 1748 at gmail
+*/
+
+#if !defined(CUTE_C2_H)
+
+// this can be adjusted as necessary, but is highly recommended to be kept at 8.
+// higher numbers will incur quite a bit of memory overhead, and convex shapes
+// over 8 verts start to just look like spheres, which can be implicitly rep-
+// resented as a point + radius. usually tools that generate polygons should be
+// constructed so they do not output polygons with too many verts.
+// Note: polygons in cute_c2 are all *convex*.
+#define C2_MAX_POLYGON_VERTS 8
+
+// 2d vector
+typedef struct c2v
+{
+ float x;
+ float y;
+} c2v;
+
+// 2d rotation composed of cos/sin pair for a single angle
+// We use two floats as a small optimization to avoid computing sin/cos unnecessarily
+typedef struct c2r
+{
+ float c;
+ float s;
+} c2r;
+
+// 2d rotation matrix
+typedef struct c2m
+{
+ c2v x;
+ c2v y;
+} c2m;
+
+// 2d transformation "x"
+// These are used especially for c2Poly when a c2Poly is passed to a function.
+// Since polygons are prime for "instancing" a c2x transform can be used to
+// transform a polygon from local space to world space. In functions that take
+// a c2x pointer (like c2PolytoPoly), these pointers can be NULL, which represents
+// an identity transformation and assumes the verts inside of c2Poly are already
+// in world space.
+typedef struct c2x
+{
+ c2v p;
+ c2r r;
+} c2x;
+
+// 2d halfspace (aka plane, aka line)
+typedef struct c2h
+{
+ c2v n; // normal, normalized
+ float d; // distance to origin from plane, or ax + by = d
+} c2h;
+
+typedef struct c2Circle
+{
+ c2v p;
+ float r;
+} c2Circle;
+
+typedef struct c2AABB
+{
+ c2v min;
+ c2v max;
+} c2AABB;
+
+// a capsule is defined as a line segment (from a to b) and radius r
+typedef struct c2Capsule
+{
+ c2v a;
+ c2v b;
+ float r;
+} c2Capsule;
+
+typedef struct c2Poly
+{
+ int count;
+ c2v verts[C2_MAX_POLYGON_VERTS];
+ c2v norms[C2_MAX_POLYGON_VERTS];
+} c2Poly;
+
+// IMPORTANT:
+// Many algorithms in this file are sensitive to the magnitude of the
+// ray direction (c2Ray::d). It is highly recommended to normalize the
+// ray direction and use t to specify a distance. Please see this link
+// for an in-depth explanation: https://github.com/RandyGaul/cute_headers/issues/30
+typedef struct c2Ray
+{
+ c2v p; // position
+ c2v d; // direction (normalized)
+ float t; // distance along d from position p to find endpoint of ray
+} c2Ray;
+
+typedef struct c2Raycast
+{
+ float t; // time of impact
+ c2v n; // normal of surface at impact (unit length)
+} c2Raycast;
+
+// position of impact p = ray.p + ray.d * raycast.t
+#define c2Impact(ray, t) c2Add(ray.p, c2Mulvs(ray.d, t))
+
+// contains all information necessary to resolve a collision, or in other words
+// this is the information needed to separate shapes that are colliding. Doing
+// the resolution step is *not* included in cute_c2.
+typedef struct c2Manifold
+{
+ int count;
+ float depths[2];
+ c2v contact_points[2];
+
+ // always points from shape A to shape B (first and second shapes passed into
+ // any of the c2***to***Manifold functions)
+ c2v n;
+} c2Manifold;
+
+// This define allows exporting/importing of the header to a dynamic library.
+// Here's an example.
+// #define CUTE_C2_API extern "C" __declspec(dllexport)
+#if !defined(CUTE_C2_API)
+# define CUTE_C2_API
+#endif
+
+// boolean collision detection
+// these versions are faster than the manifold versions, but only give a YES/NO result
+CUTE_C2_API int c2CircletoCircle(c2Circle A, c2Circle B);
+CUTE_C2_API int c2CircletoAABB(c2Circle A, c2AABB B);
+CUTE_C2_API int c2CircletoCapsule(c2Circle A, c2Capsule B);
+CUTE_C2_API int c2AABBtoAABB(c2AABB A, c2AABB B);
+CUTE_C2_API int c2AABBtoCapsule(c2AABB A, c2Capsule B);
+CUTE_C2_API int c2CapsuletoCapsule(c2Capsule A, c2Capsule B);
+CUTE_C2_API int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx);
+CUTE_C2_API int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx);
+CUTE_C2_API int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx);
+CUTE_C2_API int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx);
+
+// ray operations
+// output is placed into the c2Raycast struct, which represents the hit location
+// of the ray. the out param contains no meaningful information if these funcs
+// return 0
+CUTE_C2_API int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out);
+CUTE_C2_API int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out);
+CUTE_C2_API int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out);
+CUTE_C2_API int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out);
+
+// manifold generation
+// These functions are (generally) slower than the boolean versions, but will compute one
+// or two points that represent the plane of contact. This information is usually needed
+// to resolve and prevent shapes from colliding. If no collision occured the count member
+// of the manifold struct is set to 0.
+CUTE_C2_API void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m);
+CUTE_C2_API void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m);
+CUTE_C2_API void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m);
+CUTE_C2_API void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m);
+CUTE_C2_API void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m);
+CUTE_C2_API void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m);
+CUTE_C2_API void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx, c2Manifold* m);
+CUTE_C2_API void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m);
+CUTE_C2_API void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx, c2Manifold* m);
+CUTE_C2_API void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx, c2Manifold* m);
+
+typedef enum
+{
+ C2_TYPE_NONE,
+ C2_TYPE_CIRCLE,
+ C2_TYPE_AABB,
+ C2_TYPE_CAPSULE,
+ C2_TYPE_POLY
+} C2_TYPE;
+
+// This struct is only for advanced usage of the c2GJK function. See comments inside of the
+// c2GJK function for more details.
+typedef struct c2GJKCache
+{
+ float metric;
+ int count;
+ int iA[3];
+ int iB[3];
+ float div;
+} c2GJKCache;
+
+// This is an advanced function, intended to be used by people who know what they're doing.
+//
+// Runs the GJK algorithm to find closest points, returns distance between closest points.
+// outA and outB can be NULL, in this case only distance is returned. ax_ptr and bx_ptr
+// can be NULL, and represent local to world transformations for shapes A and B respectively.
+// use_radius will apply radii for capsules and circles (if set to false, spheres are
+// treated as points and capsules are treated as line segments i.e. rays). The cache parameter
+// should be NULL, as it is only for advanced usage (unless you know what you're doing, then
+// go ahead and use it). iterations is an optional parameter.
+//
+// IMPORTANT NOTE:
+// The GJK function is sensitive to large shapes, since it internally will compute signed area
+// values. `c2GJK` is called throughout cute c2 in many ways, so try to make sure all of your
+// collision shapes are not gigantic. For example, try to keep the volume of all your shapes
+// less than 100.0f. If you need large shapes, you should use tiny collision geometry for all
+// cute c2 function, and simply render the geometry larger on-screen by scaling it up.
+CUTE_C2_API float c2GJK(const void* A, C2_TYPE typeA, const c2x* ax_ptr, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v* outA, c2v* outB, int use_radius, int* iterations, c2GJKCache* cache);
+
+// Stores results of a time of impact calculation done by `c2TOI`.
+struct c2TOIResult
+{
+ int hit; // 1 if shapes were touching at the TOI, 0 if they never hit.
+ float toi; // The time of impact between two shapes.
+ c2v n; // Surface normal from shape A to B at the time of impact.
+ c2v p; // Point of contact between shapes A and B at time of impact.
+ int iterations; // Number of iterations the solver underwent.
+};
+
+// This is an advanced function, intended to be used by people who know what they're doing.
+//
+// Computes the time of impact from shape A and shape B. The velocity of each shape is provided
+// by vA and vB respectively. The shapes are *not* allowed to rotate over time. The velocity is
+// assumed to represent the change in motion from time 0 to time 1, and so the return value will
+// be a number from 0 to 1. To move each shape to the colliding configuration, multiply vA and vB
+// each by the return value. ax_ptr and bx_ptr are optional parameters to transforms for each shape,
+// and are typically used for polygon shapes to transform from model to world space. Set these to
+// NULL to represent identity transforms. iterations is an optional parameter. use_radius
+// will apply radii for capsules and circles (if set to false, spheres are treated as points and
+// capsules are treated as line segments i.e. rays).
+//
+// IMPORTANT NOTE:
+// The c2TOI function can be used to implement a "swept character controller", but it can be
+// difficult to do so. Say we compute a time of impact with `c2TOI` and move the shapes to the
+// time of impact, and adjust the velocity by zeroing out the velocity along the surface normal.
+// If we then call `c2TOI` again, it will fail since the shapes will be considered to start in
+// a colliding configuration. There are many styles of tricks to get around this problem, and
+// all of them involve giving the next call to `c2TOI` some breathing room. It is recommended
+// to use some variation of the following algorithm:
+//
+// 1. Call c2TOI.
+// 2. Move the shapes to the TOI.
+// 3. Slightly inflate the size of one, or both, of the shapes so they will be intersecting.
+// The purpose is to make the shapes numerically intersecting, but not visually intersecting.
+// Another option is to call c2TOI with slightly deflated shapes.
+// See the function `c2Inflate` for some more details.
+// 4. Compute the collision manifold between the inflated shapes (for example, use c2PolytoPolyManifold).
+// 5. Gently push the shapes apart. This will give the next call to c2TOI some breathing room.
+CUTE_C2_API c2TOIResult c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, int use_radius);
+
+// Inflating a shape.
+//
+// This is useful to numerically grow or shrink a polytope. For example, when calling
+// a time of impact function it can be good to use a slightly smaller shape. Then, once
+// both shapes are moved to the time of impact a collision manifold can be made from the
+// slightly larger (and now overlapping) shapes.
+//
+// IMPORTANT NOTE
+// Inflating a shape with sharp corners can cause those corners to move dramatically.
+// Deflating a shape can avoid this problem, but deflating a very small shape can invert
+// the planes and result in something that is no longer convex. Make sure to pick an
+// appropriately small skin factor, for example 1.0e-6f.
+CUTE_C2_API void c2Inflate(void* shape, C2_TYPE type, float skin_factor);
+
+// Computes 2D convex hull. Will not do anything if less than two verts supplied. If
+// more than C2_MAX_POLYGON_VERTS are supplied extras are ignored.
+CUTE_C2_API int c2Hull(c2v* verts, int count);
+CUTE_C2_API void c2Norms(c2v* verts, c2v* norms, int count);
+
+// runs c2Hull and c2Norms, assumes p->verts and p->count are both set to valid values
+CUTE_C2_API void c2MakePoly(c2Poly* p);
+
+// Generic collision detection routines, useful for games that want to use some poly-
+// morphism to write more generic-styled code. Internally calls various above functions.
+// For AABBs/Circles/Capsules ax and bx are ignored. For polys ax and bx can define
+// model to world transformations (for polys only), or be NULL for identity transforms.
+CUTE_C2_API int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB);
+CUTE_C2_API void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m);
+CUTE_C2_API int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out);
+
+#ifdef _MSC_VER
+ #define C2_INLINE __forceinline
+#else
+ #define C2_INLINE inline __attribute__((always_inline))
+#endif
+
+// adjust these primitives as seen fit
+#include <string.h> // memcpy
+#include <math.h>
+#define c2Sin(radians) sinf(radians)
+#define c2Cos(radians) cosf(radians)
+#define c2Sqrt(a) sqrtf(a)
+#define c2Min(a, b) ((a) < (b) ? (a) : (b))
+#define c2Max(a, b) ((a) > (b) ? (a) : (b))
+#define c2Abs(a) ((a) < 0 ? -(a) : (a))
+#define c2Clamp(a, lo, hi) c2Max(lo, c2Min(a, hi))
+C2_INLINE void c2SinCos(float radians, float* s, float* c) { *c = c2Cos(radians); *s = c2Sin(radians); }
+#define c2Sign(a) (a < 0 ? -1.0f : 1.0f)
+
+// The rest of the functions in the header-only portion are all for internal use
+// and use the author's personal naming conventions. It is recommended to use one's
+// own math library instead of the one embedded here in cute_c2, but for those
+// curious or interested in trying it out here's the details:
+
+// The Mul functions are used to perform multiplication. x stands for transform,
+// v stands for vector, s stands for scalar, r stands for rotation, h stands for
+// halfspace and T stands for transpose.For example c2MulxvT stands for "multiply
+// a transform with a vector, and transpose the transform".
+
+// vector ops
+C2_INLINE c2v c2V(float x, float y) { c2v a; a.x = x; a.y = y; return a; }
+C2_INLINE c2v c2Add(c2v a, c2v b) { a.x += b.x; a.y += b.y; return a; }
+C2_INLINE c2v c2Sub(c2v a, c2v b) { a.x -= b.x; a.y -= b.y; return a; }
+C2_INLINE float c2Dot(c2v a, c2v b) { return a.x * b.x + a.y * b.y; }
+C2_INLINE c2v c2Mulvs(c2v a, float b) { a.x *= b; a.y *= b; return a; }
+C2_INLINE c2v c2Mulvv(c2v a, c2v b) { a.x *= b.x; a.y *= b.y; return a; }
+C2_INLINE c2v c2Div(c2v a, float b) { return c2Mulvs(a, 1.0f / b); }
+C2_INLINE c2v c2Skew(c2v a) { c2v b; b.x = -a.y; b.y = a.x; return b; }
+C2_INLINE c2v c2CCW90(c2v a) { c2v b; b.x = a.y; b.y = -a.x; return b; }
+C2_INLINE float c2Det2(c2v a, c2v b) { return a.x * b.y - a.y * b.x; }
+C2_INLINE c2v c2Minv(c2v a, c2v b) { return c2V(c2Min(a.x, b.x), c2Min(a.y, b.y)); }
+C2_INLINE c2v c2Maxv(c2v a, c2v b) { return c2V(c2Max(a.x, b.x), c2Max(a.y, b.y)); }
+C2_INLINE c2v c2Clampv(c2v a, c2v lo, c2v hi) { return c2Maxv(lo, c2Minv(a, hi)); }
+C2_INLINE c2v c2Absv(c2v a) { return c2V(c2Abs(a.x), c2Abs(a.y)); }
+C2_INLINE float c2Hmin(c2v a) { return c2Min(a.x, a.y); }
+C2_INLINE float c2Hmax(c2v a) { return c2Max(a.x, a.y); }
+C2_INLINE float c2Len(c2v a) { return c2Sqrt(c2Dot(a, a)); }
+C2_INLINE c2v c2Norm(c2v a) { return c2Div(a, c2Len(a)); }
+C2_INLINE c2v c2SafeNorm(c2v a) { float sq = c2Dot(a, a); return sq ? c2Div(a, c2Len(a)) : c2V(0, 0); }
+C2_INLINE c2v c2Neg(c2v a) { return c2V(-a.x, -a.y); }
+C2_INLINE c2v c2Lerp(c2v a, c2v b, float t) { return c2Add(a, c2Mulvs(c2Sub(b, a), t)); }
+C2_INLINE int c2Parallel(c2v a, c2v b, float kTol)
+{
+ float k = c2Len(a) / c2Len(b);
+ b = c2Mulvs(b, k);
+ if (c2Abs(a.x - b.x) < kTol && c2Abs(a.y - b.y) < kTol) return 1;
+ return 0;
+}
+
+// rotation ops
+C2_INLINE c2r c2Rot(float radians) { c2r r; c2SinCos(radians, &r.s, &r.c); return r; }
+C2_INLINE c2r c2RotIdentity(void) { c2r r; r.c = 1.0f; r.s = 0; return r; }
+C2_INLINE c2v c2RotX(c2r r) { return c2V(r.c, r.s); }
+C2_INLINE c2v c2RotY(c2r r) { return c2V(-r.s, r.c); }
+C2_INLINE c2v c2Mulrv(c2r a, c2v b) { return c2V(a.c * b.x - a.s * b.y, a.s * b.x + a.c * b.y); }
+C2_INLINE c2v c2MulrvT(c2r a, c2v b) { return c2V(a.c * b.x + a.s * b.y, -a.s * b.x + a.c * b.y); }
+C2_INLINE c2r c2Mulrr(c2r a, c2r b) { c2r c; c.c = a.c * b.c - a.s * b.s; c.s = a.s * b.c + a.c * b.s; return c; }
+C2_INLINE c2r c2MulrrT(c2r a, c2r b) { c2r c; c.c = a.c * b.c + a.s * b.s; c.s = a.c * b.s - a.s * b.c; return c; }
+
+C2_INLINE c2v c2Mulmv(c2m a, c2v b) { c2v c; c.x = a.x.x * b.x + a.y.x * b.y; c.y = a.x.y * b.x + a.y.y * b.y; return c; }
+C2_INLINE c2v c2MulmvT(c2m a, c2v b) { c2v c; c.x = a.x.x * b.x + a.x.y * b.y; c.y = a.y.x * b.x + a.y.y * b.y; return c; }
+C2_INLINE c2m c2Mulmm(c2m a, c2m b) { c2m c; c.x = c2Mulmv(a, b.x); c.y = c2Mulmv(a, b.y); return c; }
+C2_INLINE c2m c2MulmmT(c2m a, c2m b) { c2m c; c.x = c2MulmvT(a, b.x); c.y = c2MulmvT(a, b.y); return c; }
+
+// transform ops
+C2_INLINE c2x c2xIdentity(void) { c2x x; x.p = c2V(0, 0); x.r = c2RotIdentity(); return x; }
+C2_INLINE c2v c2Mulxv(c2x a, c2v b) { return c2Add(c2Mulrv(a.r, b), a.p); }
+C2_INLINE c2v c2MulxvT(c2x a, c2v b) { return c2MulrvT(a.r, c2Sub(b, a.p)); }
+C2_INLINE c2x c2Mulxx(c2x a, c2x b) { c2x c; c.r = c2Mulrr(a.r, b.r); c.p = c2Add(c2Mulrv(a.r, b.p), a.p); return c; }
+C2_INLINE c2x c2MulxxT(c2x a, c2x b) { c2x c; c.r = c2MulrrT(a.r, b.r); c.p = c2MulrvT(a.r, c2Sub(b.p, a.p)); return c; }
+C2_INLINE c2x c2Transform(c2v p, float radians) { c2x x; x.r = c2Rot(radians); x.p = p; return x; }
+
+// halfspace ops
+C2_INLINE c2v c2Origin(c2h h) { return c2Mulvs(h.n, h.d); }
+C2_INLINE float c2Dist(c2h h, c2v p) { return c2Dot(h.n, p) - h.d; }
+C2_INLINE c2v c2Project(c2h h, c2v p) { return c2Sub(p, c2Mulvs(h.n, c2Dist(h, p))); }
+C2_INLINE c2h c2Mulxh(c2x a, c2h b) { c2h c; c.n = c2Mulrv(a.r, b.n); c.d = c2Dot(c2Mulxv(a, c2Origin(b)), c.n); return c; }
+C2_INLINE c2h c2MulxhT(c2x a, c2h b) { c2h c; c.n = c2MulrvT(a.r, b.n); c.d = c2Dot(c2MulxvT(a, c2Origin(b)), c.n); return c; }
+C2_INLINE c2v c2Intersect(c2v a, c2v b, float da, float db) { return c2Add(a, c2Mulvs(c2Sub(b, a), (da / (da - db)))); }
+
+C2_INLINE void c2BBVerts(c2v* out, c2AABB* bb)
+{
+ out[0] = bb->min;
+ out[1] = c2V(bb->max.x, bb->min.y);
+ out[2] = bb->max;
+ out[3] = c2V(bb->min.x, bb->max.y);
+}
+
+#define CUTE_C2_H
+#endif
+
+#ifdef CUTE_C2_IMPLEMENTATION
+#ifndef CUTE_C2_IMPLEMENTATION_ONCE
+#define CUTE_C2_IMPLEMENTATION_ONCE
+
+int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB)
+{
+ switch (typeA)
+ {
+ case C2_TYPE_CIRCLE:
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: return c2CircletoCircle(*(c2Circle*)A, *(c2Circle*)B);
+ case C2_TYPE_AABB: return c2CircletoAABB(*(c2Circle*)A, *(c2AABB*)B);
+ case C2_TYPE_CAPSULE: return c2CircletoCapsule(*(c2Circle*)A, *(c2Capsule*)B);
+ case C2_TYPE_POLY: return c2CircletoPoly(*(c2Circle*)A, (const c2Poly*)B, bx);
+ default: return 0;
+ }
+ break;
+
+ case C2_TYPE_AABB:
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: return c2CircletoAABB(*(c2Circle*)B, *(c2AABB*)A);
+ case C2_TYPE_AABB: return c2AABBtoAABB(*(c2AABB*)A, *(c2AABB*)B);
+ case C2_TYPE_CAPSULE: return c2AABBtoCapsule(*(c2AABB*)A, *(c2Capsule*)B);
+ case C2_TYPE_POLY: return c2AABBtoPoly(*(c2AABB*)A, (const c2Poly*)B, bx);
+ default: return 0;
+ }
+ break;
+
+ case C2_TYPE_CAPSULE:
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: return c2CircletoCapsule(*(c2Circle*)B, *(c2Capsule*)A);
+ case C2_TYPE_AABB: return c2AABBtoCapsule(*(c2AABB*)B, *(c2Capsule*)A);
+ case C2_TYPE_CAPSULE: return c2CapsuletoCapsule(*(c2Capsule*)A, *(c2Capsule*)B);
+ case C2_TYPE_POLY: return c2CapsuletoPoly(*(c2Capsule*)A, (const c2Poly*)B, bx);
+ default: return 0;
+ }
+ break;
+
+ case C2_TYPE_POLY:
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: return c2CircletoPoly(*(c2Circle*)B, (const c2Poly*)A, ax);
+ case C2_TYPE_AABB: return c2AABBtoPoly(*(c2AABB*)B, (const c2Poly*)A, ax);
+ case C2_TYPE_CAPSULE: return c2CapsuletoPoly(*(c2Capsule*)B, (const c2Poly*)A, ax);
+ case C2_TYPE_POLY: return c2PolytoPoly((const c2Poly*)A, ax, (const c2Poly*)B, bx);
+ default: return 0;
+ }
+ break;
+
+ default:
+ return 0;
+ }
+}
+
+void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m)
+{
+ m->count = 0;
+
+ switch (typeA)
+ {
+ case C2_TYPE_CIRCLE:
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: c2CircletoCircleManifold(*(c2Circle*)A, *(c2Circle*)B, m); break;
+ case C2_TYPE_AABB: c2CircletoAABBManifold(*(c2Circle*)A, *(c2AABB*)B, m); break;
+ case C2_TYPE_CAPSULE: c2CircletoCapsuleManifold(*(c2Circle*)A, *(c2Capsule*)B, m); break;
+ case C2_TYPE_POLY: c2CircletoPolyManifold(*(c2Circle*)A, (const c2Poly*)B, bx, m); break;
+ }
+ break;
+
+ case C2_TYPE_AABB:
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: c2CircletoAABBManifold(*(c2Circle*)B, *(c2AABB*)A, m); m->n = c2Neg(m->n); break;
+ case C2_TYPE_AABB: c2AABBtoAABBManifold(*(c2AABB*)A, *(c2AABB*)B, m); break;
+ case C2_TYPE_CAPSULE: c2AABBtoCapsuleManifold(*(c2AABB*)A, *(c2Capsule*)B, m); break;
+ case C2_TYPE_POLY: c2AABBtoPolyManifold(*(c2AABB*)A, (const c2Poly*)B, bx, m); break;
+ }
+ break;
+
+ case C2_TYPE_CAPSULE:
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: c2CircletoCapsuleManifold(*(c2Circle*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
+ case C2_TYPE_AABB: c2AABBtoCapsuleManifold(*(c2AABB*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break;
+ case C2_TYPE_CAPSULE: c2CapsuletoCapsuleManifold(*(c2Capsule*)A, *(c2Capsule*)B, m); break;
+ case C2_TYPE_POLY: c2CapsuletoPolyManifold(*(c2Capsule*)A, (const c2Poly*)B, bx, m); break;
+ }
+ break;
+
+ case C2_TYPE_POLY:
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: c2CircletoPolyManifold(*(c2Circle*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
+ case C2_TYPE_AABB: c2AABBtoPolyManifold(*(c2AABB*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
+ case C2_TYPE_CAPSULE: c2CapsuletoPolyManifold(*(c2Capsule*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break;
+ case C2_TYPE_POLY: c2PolytoPolyManifold((const c2Poly*)A, ax, (const c2Poly*)B, bx, m); break;
+ }
+ break;
+ }
+}
+
+int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out)
+{
+ switch (typeB)
+ {
+ case C2_TYPE_CIRCLE: return c2RaytoCircle(A, *(c2Circle*)B, out);
+ case C2_TYPE_AABB: return c2RaytoAABB(A, *(c2AABB*)B, out);
+ case C2_TYPE_CAPSULE: return c2RaytoCapsule(A, *(c2Capsule*)B, out);
+ case C2_TYPE_POLY: return c2RaytoPoly(A, (const c2Poly*)B, bx, out);
+ }
+
+ return 0;
+}
+
+#define C2_GJK_ITERS 20
+
+typedef struct
+{
+ float radius;
+ int count;
+ c2v verts[C2_MAX_POLYGON_VERTS];
+} c2Proxy;
+
+typedef struct
+{
+ c2v sA;
+ c2v sB;
+ c2v p;
+ float u;
+ int iA;
+ int iB;
+} c2sv;
+
+typedef struct
+{
+ c2sv a, b, c, d;
+ float div;
+ int count;
+} c2Simplex;
+
+static C2_INLINE void c2MakeProxy(const void* shape, C2_TYPE type, c2Proxy* p)
+{
+ switch (type)
+ {
+ case C2_TYPE_CIRCLE:
+ {
+ c2Circle* c = (c2Circle*)shape;
+ p->radius = c->r;
+ p->count = 1;
+ p->verts[0] = c->p;
+ } break;
+
+ case C2_TYPE_AABB:
+ {
+ c2AABB* bb = (c2AABB*)shape;
+ p->radius = 0;
+ p->count = 4;
+ c2BBVerts(p->verts, bb);
+ } break;
+
+ case C2_TYPE_CAPSULE:
+ {
+ c2Capsule* c = (c2Capsule*)shape;
+ p->radius = c->r;
+ p->count = 2;
+ p->verts[0] = c->a;
+ p->verts[1] = c->b;
+ } break;
+
+ case C2_TYPE_POLY:
+ {
+ c2Poly* poly = (c2Poly*)shape;
+ p->radius = 0;
+ p->count = poly->count;
+ for (int i = 0; i < p->count; ++i) p->verts[i] = poly->verts[i];
+ } break;
+ }
+}
+
+static C2_INLINE int c2Support(const c2v* verts, int count, c2v d)
+{
+ int imax = 0;
+ float dmax = c2Dot(verts[0], d);
+
+ for (int i = 1; i < count; ++i)
+ {
+ float dot = c2Dot(verts[i], d);
+ if (dot > dmax)
+ {
+ imax = i;
+ dmax = dot;
+ }
+ }
+
+ return imax;
+}
+
+#define C2_BARY(n, x) c2Mulvs(s->n.x, (den * s->n.u))
+#define C2_BARY2(x) c2Add(C2_BARY(a, x), C2_BARY(b, x))
+#define C2_BARY3(x) c2Add(c2Add(C2_BARY(a, x), C2_BARY(b, x)), C2_BARY(c, x))
+
+static C2_INLINE c2v c2L(c2Simplex* s)
+{
+ float den = 1.0f / s->div;
+ switch (s->count)
+ {
+ case 1: return s->a.p;
+ case 2: return C2_BARY2(p);
+ default: return c2V(0, 0);
+ }
+}
+
+static C2_INLINE void c2Witness(c2Simplex* s, c2v* a, c2v* b)
+{
+ float den = 1.0f / s->div;
+ switch (s->count)
+ {
+ case 1: *a = s->a.sA; *b = s->a.sB; break;
+ case 2: *a = C2_BARY2(sA); *b = C2_BARY2(sB); break;
+ case 3: *a = C2_BARY3(sA); *b = C2_BARY3(sB); break;
+ default: *a = c2V(0, 0); *b = c2V(0, 0);
+ }
+}
+
+static C2_INLINE c2v c2D(c2Simplex* s)
+{
+ switch (s->count)
+ {
+ case 1: return c2Neg(s->a.p);
+ case 2:
+ {
+ c2v ab = c2Sub(s->b.p, s->a.p);
+ if (c2Det2(ab, c2Neg(s->a.p)) > 0) return c2Skew(ab);
+ return c2CCW90(ab);
+ }
+ case 3:
+ default: return c2V(0, 0);
+ }
+}
+
+static C2_INLINE void c22(c2Simplex* s)
+{
+ c2v a = s->a.p;
+ c2v b = s->b.p;
+ float u = c2Dot(b, c2Sub(b, a));
+ float v = c2Dot(a, c2Sub(a, b));
+
+ if (v <= 0)
+ {
+ s->a.u = 1.0f;
+ s->div = 1.0f;
+ s->count = 1;
+ }
+
+ else if (u <= 0)
+ {
+ s->a = s->b;
+ s->a.u = 1.0f;
+ s->div = 1.0f;
+ s->count = 1;
+ }
+
+ else
+ {
+ s->a.u = u;
+ s->b.u = v;
+ s->div = u + v;
+ s->count = 2;
+ }
+}
+
+static C2_INLINE void c23(c2Simplex* s)
+{
+ c2v a = s->a.p;
+ c2v b = s->b.p;
+ c2v c = s->c.p;
+
+ float uAB = c2Dot(b, c2Sub(b, a));
+ float vAB = c2Dot(a, c2Sub(a, b));
+ float uBC = c2Dot(c, c2Sub(c, b));
+ float vBC = c2Dot(b, c2Sub(b, c));
+ float uCA = c2Dot(a, c2Sub(a, c));
+ float vCA = c2Dot(c, c2Sub(c, a));
+ float area = c2Det2(c2Sub(b, a), c2Sub(c, a));
+ float uABC = c2Det2(b, c) * area;
+ float vABC = c2Det2(c, a) * area;
+ float wABC = c2Det2(a, b) * area;
+
+ if (vAB <= 0 && uCA <= 0)
+ {
+ s->a.u = 1.0f;
+ s->div = 1.0f;
+ s->count = 1;
+ }
+
+ else if (uAB <= 0 && vBC <= 0)
+ {
+ s->a = s->b;
+ s->a.u = 1.0f;
+ s->div = 1.0f;
+ s->count = 1;
+ }
+
+ else if (uBC <= 0 && vCA <= 0)
+ {
+ s->a = s->c;
+ s->a.u = 1.0f;
+ s->div = 1.0f;
+ s->count = 1;
+ }
+
+ else if (uAB > 0 && vAB > 0 && wABC <= 0)
+ {
+ s->a.u = uAB;
+ s->b.u = vAB;
+ s->div = uAB + vAB;
+ s->count = 2;
+ }
+
+ else if (uBC > 0 && vBC > 0 && uABC <= 0)
+ {
+ s->a = s->b;
+ s->b = s->c;
+ s->a.u = uBC;
+ s->b.u = vBC;
+ s->div = uBC + vBC;
+ s->count = 2;
+ }
+
+ else if (uCA > 0 && vCA > 0 && vABC <= 0)
+ {
+ s->b = s->a;
+ s->a = s->c;
+ s->a.u = uCA;
+ s->b.u = vCA;
+ s->div = uCA + vCA;
+ s->count = 2;
+ }
+
+ else
+ {
+ s->a.u = uABC;
+ s->b.u = vABC;
+ s->c.u = wABC;
+ s->div = uABC + vABC + wABC;
+ s->count = 3;
+ }
+}
+
+#include <float.h>
+
+static C2_INLINE float c2GJKSimplexMetric(c2Simplex* s)
+{
+ switch (s->count)
+ {
+ default: // fall through
+ case 1: return 0;
+ case 2: return c2Len(c2Sub(s->b.p, s->a.p));
+ case 3: return c2Det2(c2Sub(s->b.p, s->a.p), c2Sub(s->c.p, s->a.p));
+ }
+}
+
+// Please see http://box2d.org/downloads/ under GDC 2010 for Erin's demo code
+// and PDF slides for documentation on the GJK algorithm. This function is mostly
+// from Erin's version from his online resources.
+float c2GJK(const void* A, C2_TYPE typeA, const c2x* ax_ptr, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v* outA, c2v* outB, int use_radius, int* iterations, c2GJKCache* cache)
+{
+ c2x ax;
+ c2x bx;
+ if (!ax_ptr) ax = c2xIdentity();
+ else ax = *ax_ptr;
+ if (!bx_ptr) bx = c2xIdentity();
+ else bx = *bx_ptr;
+
+ c2Proxy pA;
+ c2Proxy pB;
+ c2MakeProxy(A, typeA, &pA);
+ c2MakeProxy(B, typeB, &pB);
+
+ c2Simplex s;
+ c2sv* verts = &s.a;
+
+ // Metric and caching system as designed by E. Catto in Box2D for his conservative advancment/bilateral
+ // advancement algorithim implementations. The purpose is to reuse old simplex indices (any simplex that
+ // have not degenerated into a line or point) as a starting point. This skips the first few iterations of
+ // GJK going from point, to line, to triangle, lowering convergence rates dramatically for temporally
+ // coherent cases (such as in time of impact searches).
+ int cache_was_read = 0;
+ if (cache)
+ {
+ int cache_was_good = !!cache->count;
+
+ if (cache_was_good)
+ {
+ for (int i = 0; i < cache->count; ++i)
+ {
+ int iA = cache->iA[i];
+ int iB = cache->iB[i];
+ c2v sA = c2Mulxv(ax, pA.verts[iA]);
+ c2v sB = c2Mulxv(bx, pB.verts[iB]);
+ c2sv* v = verts + i;
+ v->iA = iA;
+ v->sA = sA;
+ v->iB = iB;
+ v->sB = sB;
+ v->p = c2Sub(v->sB, v->sA);
+ v->u = 0;
+ }
+ s.count = cache->count;
+ s.div = cache->div;
+
+ float metric_old = cache->metric;
+ float metric = c2GJKSimplexMetric(&s);
+
+ float min_metric = metric < metric_old ? metric : metric_old;
+ float max_metric = metric > metric_old ? metric : metric_old;
+
+ if (!(min_metric < max_metric * 2.0f && metric < -1.0e8f)) cache_was_read = 1;
+ }
+ }
+
+ if (!cache_was_read)
+ {
+ s.a.iA = 0;
+ s.a.iB = 0;
+ s.a.sA = c2Mulxv(ax, pA.verts[0]);
+ s.a.sB = c2Mulxv(bx, pB.verts[0]);
+ s.a.p = c2Sub(s.a.sB, s.a.sA);
+ s.a.u = 1.0f;
+ s.div = 1.0f;
+ s.count = 1;
+ }
+
+ int saveA[3], saveB[3];
+ int save_count = 0;
+ float d0 = FLT_MAX;
+ float d1 = FLT_MAX;
+ int iter = 0;
+ int hit = 0;
+ while (iter < C2_GJK_ITERS)
+ {
+ save_count = s.count;
+ for (int i = 0; i < save_count; ++i)
+ {
+ saveA[i] = verts[i].iA;
+ saveB[i] = verts[i].iB;
+ }
+
+ switch (s.count)
+ {
+ case 1: break;
+ case 2: c22(&s); break;
+ case 3: c23(&s); break;
+ }
+
+ if (s.count == 3)
+ {
+ hit = 1;
+ break;
+ }
+
+ c2v p = c2L(&s);
+ d1 = c2Dot(p, p);
+
+ if (d1 > d0) break;
+ d0 = d1;
+
+ c2v d = c2D(&s);
+ if (c2Dot(d, d) < FLT_EPSILON * FLT_EPSILON) break;
+
+ int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(d)));
+ c2v sA = c2Mulxv(ax, pA.verts[iA]);
+ int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, d));
+ c2v sB = c2Mulxv(bx, pB.verts[iB]);
+
+ c2sv* v = verts + s.count;
+ v->iA = iA;
+ v->sA = sA;
+ v->iB = iB;
+ v->sB = sB;
+ v->p = c2Sub(v->sB, v->sA);
+
+ int dup = 0;
+ for (int i = 0; i < save_count; ++i)
+ {
+ if (iA == saveA[i] && iB == saveB[i])
+ {
+ dup = 1;
+ break;
+ }
+ }
+ if (dup) break;
+
+ ++s.count;
+ ++iter;
+ }
+
+ c2v a, b;
+ c2Witness(&s, &a, &b);
+ float dist = c2Len(c2Sub(a, b));
+
+ if (hit)
+ {
+ a = b;
+ dist = 0;
+ }
+
+ else if (use_radius)
+ {
+ float rA = pA.radius;
+ float rB = pB.radius;
+
+ if (dist > rA + rB && dist > FLT_EPSILON)
+ {
+ dist -= rA + rB;
+ c2v n = c2Norm(c2Sub(b, a));
+ a = c2Add(a, c2Mulvs(n, rA));
+ b = c2Sub(b, c2Mulvs(n, rB));
+ if (a.x == b.x && a.y == b.y) dist = 0;
+ }
+
+ else
+ {
+ c2v p = c2Mulvs(c2Add(a, b), 0.5f);
+ a = p;
+ b = p;
+ dist = 0;
+ }
+ }
+
+ if (cache)
+ {
+ cache->metric = c2GJKSimplexMetric(&s);
+ cache->count = s.count;
+ for (int i = 0; i < s.count; ++i)
+ {
+ c2sv* v = verts + i;
+ cache->iA[i] = v->iA;
+ cache->iB[i] = v->iB;
+ }
+ cache->div = s.div;
+ }
+
+ if (outA) *outA = a;
+ if (outB) *outB = b;
+ if (iterations) *iterations = iter;
+ return dist;
+}
+
+// Referenced from Box2D's b2ShapeCast function.
+// GJK-Raycast algorithm by Gino van den Bergen.
+// "Smooth Mesh Contacts with GJK" in Game Physics Pearls, 2010.
+c2TOIResult c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, int use_radius)
+{
+ float t = 0;
+ c2x ax;
+ c2x bx;
+ if (!ax_ptr) ax = c2xIdentity();
+ else ax = *ax_ptr;
+ if (!bx_ptr) bx = c2xIdentity();
+ else bx = *bx_ptr;
+
+ c2Proxy pA;
+ c2Proxy pB;
+ c2MakeProxy(A, typeA, &pA);
+ c2MakeProxy(B, typeB, &pB);
+
+ c2Simplex s;
+ s.count = 0;
+ c2sv* verts = &s.a;
+
+ c2v rv = c2Sub(vB, vA);
+ int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(rv)));
+ c2v sA = c2Mulxv(ax, pA.verts[iA]);
+ int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, rv));
+ c2v sB = c2Mulxv(bx, pB.verts[iB]);
+ c2v v = c2Sub(sA, sB);
+
+ float rA = pA.radius;
+ float rB = pB.radius;
+ float radius = rA + rB;
+ if (!use_radius) {
+ rA = 0;
+ rB = 0;
+ radius = 0;
+ }
+ float tolerance = 1.0e-4f;
+
+ c2TOIResult result;
+ result.hit = false;
+ result.n = c2V(0, 0);
+ result.p = c2V(0, 0);
+ result.toi = 1.0f;
+ result.iterations = 0;
+
+ while (result.iterations < 20 && c2Len(v) - radius > tolerance)
+ {
+ iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(v)));
+ sA = c2Mulxv(ax, pA.verts[iA]);
+ iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, v));
+ sB = c2Mulxv(bx, pB.verts[iB]);
+ c2v p = c2Sub(sA, sB);
+ v = c2Norm(v);
+ float vp = c2Dot(v, p) - radius;
+ float vr = c2Dot(v, rv);
+ if (vp > t * vr) {
+ if (vr <= 0) return result;
+ t = vp / vr;
+ if (t > 1.0f) return result;
+ result.n = c2Neg(v);
+ s.count = 0;
+ }
+
+ c2sv* sv = verts + s.count;
+ sv->iA = iB;
+ sv->sA = c2Add(sB, c2Mulvs(rv, t));
+ sv->iB = iA;
+ sv->sB = sA;
+ sv->p = c2Sub(sv->sB, sv->sA);
+ sv->u = 1.0f;
+ s.count += 1;
+
+ switch (s.count)
+ {
+ case 2: c22(&s); break;
+ case 3: c23(&s); break;
+ }
+
+ if (s.count == 3) {
+ return result;
+ }
+
+ v = c2L(&s);
+ result.iterations++;
+ }
+
+ if (result.iterations == 0) {
+ result.hit = false;
+ } else {
+ result.n = c2SafeNorm(c2Neg(v));
+ int i = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, result.n));
+ c2v p = c2Mulxv(ax, pA.verts[i]);
+ p = c2Add(c2Add(p, c2Mulvs(result.n, rA)), c2Mulvs(vA, t));
+ result.p = p;
+ result.toi = t;
+ result.hit = true;
+ }
+
+ return result;
+}
+
+int c2Hull(c2v* verts, int count)
+{
+ if (count <= 2) return 0;
+ count = c2Min(C2_MAX_POLYGON_VERTS, count);
+
+ int right = 0;
+ float xmax = verts[0].x;
+ for (int i = 1; i < count; ++i)
+ {
+ float x = verts[i].x;
+ if (x > xmax)
+ {
+ xmax = x;
+ right = i;
+ }
+
+ else if (x == xmax)
+ if (verts[i].y < verts[right].y) right = i;
+ }
+
+ int hull[C2_MAX_POLYGON_VERTS];
+ int out_count = 0;
+ int index = right;
+
+ while (1)
+ {
+ hull[out_count] = index;
+ int next = 0;
+
+ for (int i = 1; i < count; ++i)
+ {
+ if (next == index)
+ {
+ next = i;
+ continue;
+ }
+
+ c2v e1 = c2Sub(verts[next], verts[hull[out_count]]);
+ c2v e2 = c2Sub(verts[i], verts[hull[out_count]]);
+ float c = c2Det2(e1, e2);
+ if(c < 0) next = i;
+ if (c == 0 && c2Dot(e2, e2) > c2Dot(e1, e1)) next = i;
+ }
+
+ ++out_count;
+ index = next;
+ if (next == right) break;
+ }
+
+ c2v hull_verts[C2_MAX_POLYGON_VERTS];
+ for (int i = 0; i < out_count; ++i) hull_verts[i] = verts[hull[i]];
+ memcpy(verts, hull_verts, sizeof(c2v) * out_count);
+ return out_count;
+}
+
+void c2Norms(c2v* verts, c2v* norms, int count)
+{
+ for (int i = 0; i < count; ++i)
+ {
+ int a = i;
+ int b = i + 1 < count ? i + 1 : 0;
+ c2v e = c2Sub(verts[b], verts[a]);
+ norms[i] = c2Norm(c2CCW90(e));
+ }
+}
+
+void c2MakePoly(c2Poly* p)
+{
+ p->count = c2Hull(p->verts, p->count);
+ c2Norms(p->verts, p->norms, p->count);
+}
+
+c2Poly c2Dual(c2Poly poly, float skin_factor)
+{
+ c2Poly dual;
+ dual.count = poly.count;
+
+ // Each plane maps to a point by involution (the mapping is its own inverse) by dividing
+ // the plane normal by its offset factor.
+ // plane = a * x + b * y - d
+ // dual = { a / d, b / d }
+ for (int i = 0; i < poly.count; ++i) {
+ c2v n = poly.norms[i];
+ float d = c2Dot(n, poly.verts[i]) - skin_factor;
+ if (d == 0) dual.verts[i] = c2V(0, 0);
+ else dual.verts[i] = c2Div(n, d);
+ }
+
+ // Instead of canonically building the convex hull, can simply take advantage of how
+ // the vertices are still in proper CCW order, so only the normals must be recomputed.
+ c2Norms(dual.verts, dual.norms, dual.count);
+
+ return dual;
+}
+
+// Inflating a polytope, idea by Dirk Gregorius ~ 2015. Works in both 2D and 3D.
+// Reference: Halfspace intersection with Qhull by Brad Barber
+// http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/half.html
+//
+// Algorithm steps:
+// 1. Find a point within the input poly.
+// 2. Center this point onto the origin.
+// 3. Adjust the planes by a skin factor.
+// 4. Compute the dual vert of each plane. Each plane becomes a vertex.
+// c2v dual(c2h plane) { return c2V(plane.n.x / plane.d, plane.n.y / plane.d) }
+// 5. Compute the convex hull of the dual verts. This is called the dual.
+// 6. Compute the dual of the dual, this will be the poly to return.
+// 7. Translate the poly away from the origin by the center point from step 2.
+// 8. Return the inflated poly.
+c2Poly c2InflatePoly(c2Poly poly, float skin_factor)
+{
+ c2v average = poly.verts[0];
+ for (int i = 1; i < poly.count; ++i) {
+ average = c2Add(average, poly.verts[i]);
+ }
+ average = c2Div(average, (float)poly.count);
+
+ for (int i = 0; i < poly.count; ++i) {
+ poly.verts[i] = c2Sub(poly.verts[i], average);
+ }
+
+ c2Poly dual = c2Dual(poly, skin_factor);
+ poly = c2Dual(dual, 0);
+
+ for (int i = 0; i < poly.count; ++i) {
+ poly.verts[i] = c2Add(poly.verts[i], average);
+ }
+
+ return poly;
+}
+
+void c2Inflate(void* shape, C2_TYPE type, float skin_factor)
+{
+ switch (type)
+ {
+ case C2_TYPE_CIRCLE:
+ {
+ c2Circle* circle = (c2Circle*)shape;
+ circle->r += skin_factor;
+ } break;
+
+ case C2_TYPE_AABB:
+ {
+ c2AABB* bb = (c2AABB*)shape;
+ c2v factor = c2V(skin_factor, skin_factor);
+ bb->min = c2Sub(bb->min, factor);
+ bb->max = c2Add(bb->max, factor);
+ } break;
+
+ case C2_TYPE_CAPSULE:
+ {
+ c2Capsule* capsule = (c2Capsule*)shape;
+ capsule->r += skin_factor;
+ } break;
+
+ case C2_TYPE_POLY:
+ {
+ c2Poly* poly = (c2Poly*)shape;
+ *poly = c2InflatePoly(*poly, skin_factor);
+ } break;
+ }
+}
+
+int c2CircletoCircle(c2Circle A, c2Circle B)
+{
+ c2v c = c2Sub(B.p, A.p);
+ float d2 = c2Dot(c, c);
+ float r2 = A.r + B.r;
+ r2 = r2 * r2;
+ return d2 < r2;
+}
+
+int c2CircletoAABB(c2Circle A, c2AABB B)
+{
+ c2v L = c2Clampv(A.p, B.min, B.max);
+ c2v ab = c2Sub(A.p, L);
+ float d2 = c2Dot(ab, ab);
+ float r2 = A.r * A.r;
+ return d2 < r2;
+}
+
+int c2AABBtoAABB(c2AABB A, c2AABB B)
+{
+ int d0 = B.max.x < A.min.x;
+ int d1 = A.max.x < B.min.x;
+ int d2 = B.max.y < A.min.y;
+ int d3 = A.max.y < B.min.y;
+ return !(d0 | d1 | d2 | d3);
+}
+
+int c2AABBtoPoint(c2AABB A, c2v B)
+{
+ int d0 = B.x < A.min.x;
+ int d1 = B.y < A.min.y;
+ int d2 = B.x > A.max.x;
+ int d3 = B.y > A.max.y;
+ return !(d0 | d1 | d2 | d3);
+}
+
+int c2CircleToPoint(c2Circle A, c2v B)
+{
+ c2v n = c2Sub(A.p, B);
+ float d2 = c2Dot(n, n);
+ return d2 < A.r * A.r;
+}
+
+// see: http://www.randygaul.net/2014/07/23/distance-point-to-line-segment/
+int c2CircletoCapsule(c2Circle A, c2Capsule B)
+{
+ c2v n = c2Sub(B.b, B.a);
+ c2v ap = c2Sub(A.p, B.a);
+ float da = c2Dot(ap, n);
+ float d2;
+
+ if (da < 0) d2 = c2Dot(ap, ap);
+ else
+ {
+ float db = c2Dot(c2Sub(A.p, B.b), n);
+ if (db < 0)
+ {
+ c2v e = c2Sub(ap, c2Mulvs(n, (da / c2Dot(n, n))));
+ d2 = c2Dot(e, e);
+ }
+ else
+ {
+ c2v bp = c2Sub(A.p, B.b);
+ d2 = c2Dot(bp, bp);
+ }
+ }
+
+ float r = A.r + B.r;
+ return d2 < r * r;
+}
+
+int c2AABBtoCapsule(c2AABB A, c2Capsule B)
+{
+ if (c2GJK(&A, C2_TYPE_AABB, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0;
+ return 1;
+}
+
+int c2CapsuletoCapsule(c2Capsule A, c2Capsule B)
+{
+ if (c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0;
+ return 1;
+}
+
+int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx)
+{
+ if (c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
+ return 1;
+}
+
+int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx)
+{
+ if (c2GJK(&A, C2_TYPE_AABB, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
+ return 1;
+}
+
+int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx)
+{
+ if (c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
+ return 1;
+}
+
+int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx)
+{
+ if (c2GJK(A, C2_TYPE_POLY, ax, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0;
+ return 1;
+}
+
+int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out)
+{
+ c2v p = B.p;
+ c2v m = c2Sub(A.p, p);
+ float c = c2Dot(m, m) - B.r * B.r;
+ float b = c2Dot(m, A.d);
+ float disc = b * b - c;
+ if (disc < 0) return 0;
+
+ float t = -b - c2Sqrt(disc);
+ if (t >= 0 && t <= A.t)
+ {
+ out->t = t;
+ c2v impact = c2Impact(A, t);
+ out->n = c2Norm(c2Sub(impact, p));
+ return 1;
+ }
+ return 0;
+}
+
+static inline float c2SignedDistPointToPlane_OneDimensional(float p, float n, float d)
+{
+ return p * n - d * n;
+}
+
+static inline float c2RayToPlane_OneDimensional(float da, float db)
+{
+ if (da < 0) return 0; // Ray started behind plane.
+ else if (da * db >= 0) return 1.0f; // Ray starts and ends on the same of the plane.
+ else // Ray starts and ends on opposite sides of the plane (or directly on the plane).
+ {
+ float d = da - db;
+ if (d != 0) return da / d;
+ else return 0; // Special case for super tiny ray, or AABB.
+ }
+}
+
+int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out)
+{
+ c2v p0 = A.p;
+ c2v p1 = c2Impact(A, A.t);
+ c2AABB a_box;
+ a_box.min = c2Minv(p0, p1);
+ a_box.max = c2Maxv(p0, p1);
+
+ // Test B's axes.
+ if (!c2AABBtoAABB(a_box, B)) return 0;
+
+ // Test the ray's axes (along the segment's normal).
+ c2v ab = c2Sub(p1, p0);
+ c2v n = c2Skew(ab);
+ c2v abs_n = c2Absv(n);
+ c2v half_extents = c2Mulvs(c2Sub(B.max, B.min), 0.5f);
+ c2v center_of_b_box = c2Mulvs(c2Add(B.min, B.max), 0.5f);
+ float d = c2Abs(c2Dot(n, c2Sub(p0, center_of_b_box))) - c2Dot(abs_n, half_extents);
+ if (d > 0) return 0;
+
+ // Calculate intermediate values up-front.
+ // This should play well with superscalar architecture.
+ float da0 = c2SignedDistPointToPlane_OneDimensional(p0.x, -1.0f, B.min.x);
+ float db0 = c2SignedDistPointToPlane_OneDimensional(p1.x, -1.0f, B.min.x);
+ float da1 = c2SignedDistPointToPlane_OneDimensional(p0.x, 1.0f, B.max.x);
+ float db1 = c2SignedDistPointToPlane_OneDimensional(p1.x, 1.0f, B.max.x);
+ float da2 = c2SignedDistPointToPlane_OneDimensional(p0.y, -1.0f, B.min.y);
+ float db2 = c2SignedDistPointToPlane_OneDimensional(p1.y, -1.0f, B.min.y);
+ float da3 = c2SignedDistPointToPlane_OneDimensional(p0.y, 1.0f, B.max.y);
+ float db3 = c2SignedDistPointToPlane_OneDimensional(p1.y, 1.0f, B.max.y);
+ float t0 = c2RayToPlane_OneDimensional(da0, db0);
+ float t1 = c2RayToPlane_OneDimensional(da1, db1);
+ float t2 = c2RayToPlane_OneDimensional(da2, db2);
+ float t3 = c2RayToPlane_OneDimensional(da3, db3);
+
+ // Calculate hit predicate, no branching.
+ int hit0 = t0 < 1.0f;
+ int hit1 = t1 < 1.0f;
+ int hit2 = t2 < 1.0f;
+ int hit3 = t3 < 1.0f;
+ int hit = hit0 | hit1 | hit2 | hit3;
+
+ if (hit)
+ {
+ // Remap t's within 0-1 range, where >= 1 is treated as 0.
+ t0 = (float)hit0 * t0;
+ t1 = (float)hit1 * t1;
+ t2 = (float)hit2 * t2;
+ t3 = (float)hit3 * t3;
+
+ // Sort output by finding largest t to deduce the normal.
+ if (t0 >= t1 && t0 >= t2 && t0 >= t3)
+ {
+ out->t = t0 * A.t;
+ out->n = c2V(-1, 0);
+ }
+
+ else if (t1 >= t0 && t1 >= t2 && t1 >= t3)
+ {
+ out->t = t1 * A.t;
+ out->n = c2V(1, 0);
+ }
+
+ else if (t2 >= t0 && t2 >= t1 && t2 >= t3)
+ {
+ out->t = t2 * A.t;
+ out->n = c2V(0, -1);
+ }
+
+ else
+ {
+ out->t = t3 * A.t;
+ out->n = c2V(0, 1);
+ }
+
+ return 1;
+ } else return 0; // This can still numerically happen.
+}
+
+int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out)
+{
+ c2m M;
+ M.y = c2Norm(c2Sub(B.b, B.a));
+ M.x = c2CCW90(M.y);
+
+ // rotate capsule to origin, along Y axis
+ // rotate the ray same way
+ c2v cap_n = c2Sub(B.b, B.a);
+ c2v yBb = c2MulmvT(M, cap_n);
+ c2v yAp = c2MulmvT(M, c2Sub(A.p, B.a));
+ c2v yAd = c2MulmvT(M, A.d);
+ c2v yAe = c2Add(yAp, c2Mulvs(yAd, A.t));
+
+ c2AABB capsule_bb;
+ capsule_bb.min = c2V(-B.r, 0);
+ capsule_bb.max = c2V(B.r, yBb.y);
+
+ out->n = c2Norm(cap_n);
+ out->t = 0;
+
+ // check and see if ray starts within the capsule
+ if (c2AABBtoPoint(capsule_bb, yAp)) {
+ return 1;
+ } else {
+ c2Circle capsule_a;
+ c2Circle capsule_b;
+ capsule_a.p = B.a;
+ capsule_a.r = B.r;
+ capsule_b.p = B.b;
+ capsule_b.r = B.r;
+
+ if (c2CircleToPoint(capsule_a, A.p)) {
+ return 1;
+ } else if (c2CircleToPoint(capsule_b, A.p)) {
+ return 1;
+ }
+ }
+
+ if (yAe.x * yAp.x < 0 || c2Min(c2Abs(yAe.x), c2Abs(yAp.x)) < B.r)
+ {
+ c2Circle Ca, Cb;
+ Ca.p = B.a;
+ Ca.r = B.r;
+ Cb.p = B.b;
+ Cb.r = B.r;
+
+ // ray starts inside capsule prism -- must hit one of the semi-circles
+ if (c2Abs(yAp.x) < B.r) {
+ if (yAp.y < 0) return c2RaytoCircle(A, Ca, out);
+ else return c2RaytoCircle(A, Cb, out);
+ }
+
+ // hit the capsule prism
+ else
+ {
+ float c = yAp.x > 0 ? B.r : -B.r;
+ float d = (yAe.x - yAp.x);
+ float t = (c - yAp.x) / d;
+ float y = yAp.y + (yAe.y - yAp.y) * t;
+ if (y <= 0) return c2RaytoCircle(A, Ca, out);
+ if (y >= yBb.y) return c2RaytoCircle(A, Cb, out);
+ else {
+ out->n = c > 0 ? M.x : c2Skew(M.y);
+ out->t = t * A.t;
+ return 1;
+ }
+ }
+ }
+
+ return 0;
+}
+
+int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out)
+{
+ c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
+ c2v p = c2MulxvT(bx, A.p);
+ c2v d = c2MulrvT(bx.r, A.d);
+ float lo = 0;
+ float hi = A.t;
+ int index = ~0;
+
+ // test ray to each plane, tracking lo/hi times of intersection
+ for (int i = 0; i < B->count; ++i)
+ {
+ float num = c2Dot(B->norms[i], c2Sub(B->verts[i], p));
+ float den = c2Dot(B->norms[i], d);
+ if (den == 0 && num < 0) return 0;
+ else
+ {
+ if (den < 0 && num < lo * den)
+ {
+ lo = num / den;
+ index = i;
+ }
+ else if (den > 0 && num < hi * den) hi = num / den;
+ }
+ if (hi < lo) return 0;
+ }
+
+ if (index != ~0)
+ {
+ out->t = lo;
+ out->n = c2Mulrv(bx.r, B->norms[index]);
+ return 1;
+ }
+
+ return 0;
+}
+
+void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m)
+{
+ m->count = 0;
+ c2v d = c2Sub(B.p, A.p);
+ float d2 = c2Dot(d, d);
+ float r = A.r + B.r;
+ if (d2 < r * r)
+ {
+ float l = c2Sqrt(d2);
+ c2v n = l != 0 ? c2Mulvs(d, 1.0f / l) : c2V(0, 1.0f);
+ m->count = 1;
+ m->depths[0] = r - l;
+ m->contact_points[0] = c2Sub(B.p, c2Mulvs(n, B.r));
+ m->n = n;
+ }
+}
+
+void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m)
+{
+ m->count = 0;
+ c2v L = c2Clampv(A.p, B.min, B.max);
+ c2v ab = c2Sub(L, A.p);
+ float d2 = c2Dot(ab, ab);
+ float r2 = A.r * A.r;
+ if (d2 < r2)
+ {
+ // shallow (center of circle not inside of AABB)
+ if (d2 != 0)
+ {
+ float d = c2Sqrt(d2);
+ c2v n = c2Norm(ab);
+ m->count = 1;
+ m->depths[0] = A.r - d;
+ m->contact_points[0] = c2Add(A.p, c2Mulvs(n, d));
+ m->n = n;
+ }
+
+ // deep (center of circle inside of AABB)
+ // clamp circle's center to edge of AABB, then form the manifold
+ else
+ {
+ c2v mid = c2Mulvs(c2Add(B.min, B.max), 0.5f);
+ c2v e = c2Mulvs(c2Sub(B.max, B.min), 0.5f);
+ c2v d = c2Sub(A.p, mid);
+ c2v abs_d = c2Absv(d);
+
+ float x_overlap = e.x - abs_d.x;
+ float y_overlap = e.y - abs_d.y;
+
+ float depth;
+ c2v n;
+
+ if (x_overlap < y_overlap)
+ {
+ depth = x_overlap;
+ n = c2V(1.0f, 0);
+ n = c2Mulvs(n, d.x < 0 ? 1.0f : -1.0f);
+ }
+
+ else
+ {
+ depth = y_overlap;
+ n = c2V(0, 1.0f);
+ n = c2Mulvs(n, d.y < 0 ? 1.0f : -1.0f);
+ }
+
+ m->count = 1;
+ m->depths[0] = A.r + depth;
+ m->contact_points[0] = c2Sub(A.p, c2Mulvs(n, depth));
+ m->n = n;
+ }
+ }
+}
+
+void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m)
+{
+ m->count = 0;
+ c2v a, b;
+ float r = A.r + B.r;
+ float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0);
+ if (d < r)
+ {
+ c2v n;
+ if (d == 0) n = c2Norm(c2Skew(c2Sub(B.b, B.a)));
+ else n = c2Norm(c2Sub(b, a));
+
+ m->count = 1;
+ m->depths[0] = r - d;
+ m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
+ m->n = n;
+ }
+}
+
+void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m)
+{
+ m->count = 0;
+ c2v mid_a = c2Mulvs(c2Add(A.min, A.max), 0.5f);
+ c2v mid_b = c2Mulvs(c2Add(B.min, B.max), 0.5f);
+ c2v eA = c2Absv(c2Mulvs(c2Sub(A.max, A.min), 0.5f));
+ c2v eB = c2Absv(c2Mulvs(c2Sub(B.max, B.min), 0.5f));
+ c2v d = c2Sub(mid_b, mid_a);
+
+ // calc overlap on x and y axes
+ float dx = eA.x + eB.x - c2Abs(d.x);
+ if (dx < 0) return;
+ float dy = eA.y + eB.y - c2Abs(d.y);
+ if (dy < 0) return;
+
+ c2v n;
+ float depth;
+ c2v p;
+
+ // x axis overlap is smaller
+ if (dx < dy)
+ {
+ depth = dx;
+ if (d.x < 0)
+ {
+ n = c2V(-1.0f, 0);
+ p = c2Sub(mid_a, c2V(eA.x, 0));
+ }
+ else
+ {
+ n = c2V(1.0f, 0);
+ p = c2Add(mid_a, c2V(eA.x, 0));
+ }
+ }
+
+ // y axis overlap is smaller
+ else
+ {
+ depth = dy;
+ if (d.y < 0)
+ {
+ n = c2V(0, -1.0f);
+ p = c2Sub(mid_a, c2V(0, eA.y));
+ }
+ else
+ {
+ n = c2V(0, 1.0f);
+ p = c2Add(mid_a, c2V(0, eA.y));
+ }
+ }
+
+ m->count = 1;
+ m->contact_points[0] = p;
+ m->depths[0] = depth;
+ m->n = n;
+}
+
+void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m)
+{
+ m->count = 0;
+ c2Poly p;
+ c2BBVerts(p.verts, &A);
+ p.count = 4;
+ c2Norms(p.verts, p.norms, 4);
+ c2CapsuletoPolyManifold(B, &p, 0, m);
+ m->n = c2Neg(m->n);
+}
+
+void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m)
+{
+ m->count = 0;
+ c2v a, b;
+ float r = A.r + B.r;
+ float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0);
+ if (d < r)
+ {
+ c2v n;
+ if (d == 0) n = c2Norm(c2Skew(c2Sub(A.b, A.a)));
+ else n = c2Norm(c2Sub(b, a));
+
+ m->count = 1;
+ m->depths[0] = r - d;
+ m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r));
+ m->n = n;
+ }
+}
+
+static C2_INLINE c2h c2PlaneAt(const c2Poly* p, const int i)
+{
+ c2h h;
+ h.n = p->norms[i];
+ h.d = c2Dot(p->norms[i], p->verts[i]);
+ return h;
+}
+
+void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx_tr, c2Manifold* m)
+{
+ m->count = 0;
+ c2v a, b;
+ float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx_tr, &a, &b, 0, 0, 0);
+
+ // shallow, the circle center did not hit the polygon
+ // just use a and b from GJK to define the collision
+ if (d != 0)
+ {
+ c2v n = c2Sub(b, a);
+ float l = c2Dot(n, n);
+ if (l < A.r * A.r)
+ {
+ l = c2Sqrt(l);
+ m->count = 1;
+ m->contact_points[0] = b;
+ m->depths[0] = A.r - l;
+ m->n = c2Mulvs(n, 1.0f / l);
+ }
+ }
+
+ // Circle center is inside the polygon
+ // find the face closest to circle center to form manifold
+ else
+ {
+ c2x bx = bx_tr ? *bx_tr : c2xIdentity();
+ float sep = -FLT_MAX;
+ int index = ~0;
+ c2v local = c2MulxvT(bx, A.p);
+
+ for (int i = 0; i < B->count; ++i)
+ {
+ c2h h = c2PlaneAt(B, i);
+ d = c2Dist(h, local);
+ if (d > A.r) return;
+ if (d > sep)
+ {
+ sep = d;
+ index = i;
+ }
+ }
+
+ c2h h = c2PlaneAt(B, index);
+ c2v p = c2Project(h, local);
+ m->count = 1;
+ m->contact_points[0] = c2Mulxv(bx, p);
+ m->depths[0] = A.r - sep;
+ m->n = c2Neg(c2Mulrv(bx.r, B->norms[index]));
+ }
+}
+
+// Forms a c2Poly and uses c2PolytoPolyManifold
+void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m)
+{
+ m->count = 0;
+ c2Poly p;
+ c2BBVerts(p.verts, &A);
+ p.count = 4;
+ c2Norms(p.verts, p.norms, 4);
+ c2PolytoPolyManifold(&p, 0, B, bx, m);
+}
+
+// clip a segment to a plane
+static int c2Clip(c2v* seg, c2h h)
+{
+ c2v out[2];
+ int sp = 0;
+ float d0, d1;
+ if ((d0 = c2Dist(h, seg[0])) < 0) out[sp++] = seg[0];
+ if ((d1 = c2Dist(h, seg[1])) < 0) out[sp++] = seg[1];
+ if (d0 == 0 && d1 == 0) {
+ out[sp++] = seg[0];
+ out[sp++] = seg[1];
+ } else if (d0 * d1 <= 0) out[sp++] = c2Intersect(seg[0], seg[1], d0, d1);
+ seg[0] = out[0]; seg[1] = out[1];
+ return sp;
+}
+
+#ifdef _MSC_VER
+ #pragma warning(push)
+ #pragma warning(disable:4204) // nonstandard extension used: non-constant aggregate initializer
+#endif
+
+static int c2SidePlanes(c2v* seg, c2v ra, c2v rb, c2h* h)
+{
+ c2v in = c2Norm(c2Sub(rb, ra));
+ c2h left = { c2Neg(in), c2Dot(c2Neg(in), ra) };
+ c2h right = { in, c2Dot(in, rb) };
+ if (c2Clip(seg, left) < 2) return 0;
+ if (c2Clip(seg, right) < 2) return 0;
+ if (h) {
+ h->n = c2CCW90(in);
+ h->d = c2Dot(c2CCW90(in), ra);
+ }
+ return 1;
+}
+
+// clip a segment to the "side planes" of another segment.
+// side planes are planes orthogonal to a segment and attached to the
+// endpoints of the segment
+static int c2SidePlanesFromPoly(c2v* seg, c2x x, const c2Poly* p, int e, c2h* h)
+{
+ c2v ra = c2Mulxv(x, p->verts[e]);
+ c2v rb = c2Mulxv(x, p->verts[e + 1 == p->count ? 0 : e + 1]);
+ return c2SidePlanes(seg, ra, rb, h);
+}
+
+static void c2KeepDeep(c2v* seg, c2h h, c2Manifold* m)
+{
+ int cp = 0;
+ for (int i = 0; i < 2; ++i)
+ {
+ c2v p = seg[i];
+ float d = c2Dist(h, p);
+ if (d <= 0)
+ {
+ m->contact_points[cp] = p;
+ m->depths[cp] = -d;
+ ++cp;
+ }
+ }
+ m->count = cp;
+ m->n = h.n;
+}
+
+static C2_INLINE c2v c2CapsuleSupport(c2Capsule A, c2v dir)
+{
+ float da = c2Dot(A.a, dir);
+ float db = c2Dot(A.b, dir);
+ if (da > db) return c2Add(A.a, c2Mulvs(dir, A.r));
+ else return c2Add(A.b, c2Mulvs(dir, A.r));
+}
+
+static void c2AntinormalFace(c2Capsule cap, const c2Poly* p, c2x x, int* face_out, c2v* n_out)
+{
+ float sep = -FLT_MAX;
+ int index = ~0;
+ c2v n = c2V(0, 0);
+ for (int i = 0; i < p->count; ++i)
+ {
+ c2h h = c2Mulxh(x, c2PlaneAt(p, i));
+ c2v n0 = c2Neg(h.n);
+ c2v s = c2CapsuleSupport(cap, n0);
+ float d = c2Dist(h, s);
+ if (d > sep)
+ {
+ sep = d;
+ index = i;
+ n = n0;
+ }
+ }
+ *face_out = index;
+ *n_out = n;
+}
+
+static void c2Incident(c2v* incident, const c2Poly* ip, c2x ix, c2v rn_in_incident_space)
+{
+ int index = ~0;
+ float min_dot = FLT_MAX;
+ for (int i = 0; i < ip->count; ++i)
+ {
+ float dot = c2Dot(rn_in_incident_space, ip->norms[i]);
+ if (dot < min_dot)
+ {
+ min_dot = dot;
+ index = i;
+ }
+ }
+ incident[0] = c2Mulxv(ix, ip->verts[index]);
+ incident[1] = c2Mulxv(ix, ip->verts[index + 1 == ip->count ? 0 : index + 1]);
+}
+
+void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m)
+{
+ m->count = 0;
+ c2v a, b;
+ float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx_ptr, &a, &b, 0, 0, 0);
+
+ // deep, treat as segment to poly collision
+ if (d < 1.0e-6f)
+ {
+ c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
+ c2Capsule A_in_B;
+ A_in_B.a = c2MulxvT(bx, A.a);
+ A_in_B.b = c2MulxvT(bx, A.b);
+ c2v ab = c2Norm(c2Sub(A_in_B.a, A_in_B.b));
+
+ // test capsule axes
+ c2h ab_h0;
+ ab_h0.n = c2CCW90(ab);
+ ab_h0.d = c2Dot(A_in_B.a, ab_h0.n);
+ int v0 = c2Support(B->verts, B->count, c2Neg(ab_h0.n));
+ float s0 = c2Dist(ab_h0, B->verts[v0]);
+
+ c2h ab_h1;
+ ab_h1.n = c2Skew(ab);
+ ab_h1.d = c2Dot(A_in_B.a, ab_h1.n);
+ int v1 = c2Support(B->verts, B->count, c2Neg(ab_h1.n));
+ float s1 = c2Dist(ab_h1, B->verts[v1]);
+
+ // test poly axes
+ int index = ~0;
+ float sep = -FLT_MAX;
+ int code = 0;
+ for (int i = 0; i < B->count; ++i)
+ {
+ c2h h = c2PlaneAt(B, i);
+ float da = c2Dot(A_in_B.a, c2Neg(h.n));
+ float db = c2Dot(A_in_B.b, c2Neg(h.n));
+ float d;
+ if (da > db) d = c2Dist(h, A_in_B.a);
+ else d = c2Dist(h, A_in_B.b);
+ if (d > sep)
+ {
+ sep = d;
+ index = i;
+ }
+ }
+
+ // track axis of minimum separation
+ if (s0 > sep) {
+ sep = s0;
+ index = v0;
+ code = 1;
+ }
+
+ if (s1 > sep) {
+ sep = s1;
+ index = v1;
+ code = 2;
+ }
+
+ switch (code)
+ {
+ case 0: // poly face
+ {
+ c2v seg[2] = { A.a, A.b };
+ c2h h;
+ if (!c2SidePlanesFromPoly(seg, bx, B, index, &h)) return;
+ c2KeepDeep(seg, h, m);
+ m->n = c2Neg(m->n);
+ } break;
+
+ case 1: // side 0 of capsule segment
+ {
+ c2v incident[2];
+ c2Incident(incident, B, bx, ab_h0.n);
+ c2h h;
+ if (!c2SidePlanes(incident, A_in_B.b, A_in_B.a, &h)) return;
+ c2KeepDeep(incident, h, m);
+ } break;
+
+ case 2: // side 1 of capsule segment
+ {
+ c2v incident[2];
+ c2Incident(incident, B, bx, ab_h1.n);
+ c2h h;
+ if (!c2SidePlanes(incident, A_in_B.a, A_in_B.b, &h)) return;
+ c2KeepDeep(incident, h, m);
+ } break;
+
+ default:
+ // should never happen.
+ return;
+ }
+
+ for (int i = 0; i < m->count; ++i) m->depths[i] += A.r;
+ }
+
+ // shallow, use GJK results a and b to define manifold
+ else if (d < A.r)
+ {
+ m->count = 1;
+ m->n = c2Norm(c2Sub(b, a));
+ m->contact_points[0] = c2Add(a, c2Mulvs(m->n, A.r));
+ m->depths[0] = A.r - d;
+ }
+}
+
+#ifdef _MSC_VER
+ #pragma warning(pop)
+#endif
+
+static float c2CheckFaces(const c2Poly* A, c2x ax, const c2Poly* B, c2x bx, int* face_index)
+{
+ c2x b_in_a = c2MulxxT(ax, bx);
+ c2x a_in_b = c2MulxxT(bx, ax);
+ float sep = -FLT_MAX;
+ int index = ~0;
+
+ for (int i = 0; i < A->count; ++i)
+ {
+ c2h h = c2PlaneAt(A, i);
+ int idx = c2Support(B->verts, B->count, c2Mulrv(a_in_b.r, c2Neg(h.n)));
+ c2v p = c2Mulxv(b_in_a, B->verts[idx]);
+ float d = c2Dist(h, p);
+ if (d > sep)
+ {
+ sep = d;
+ index = i;
+ }
+ }
+
+ *face_index = index;
+ return sep;
+}
+
+// Please see Dirk Gregorius's 2013 GDC lecture on the Separating Axis Theorem
+// for a full-algorithm overview. The short description is:
+ // Test A against faces of B, test B against faces of A
+ // Define the reference and incident shapes (denoted by r and i respectively)
+ // Define the reference face as the axis of minimum penetration
+ // Find the incident face, which is most anti-normal face
+ // Clip incident face to reference face side planes
+ // Keep all points behind the reference face
+void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax_ptr, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m)
+{
+ m->count = 0;
+ c2x ax = ax_ptr ? *ax_ptr : c2xIdentity();
+ c2x bx = bx_ptr ? *bx_ptr : c2xIdentity();
+ int ea, eb;
+ float sa, sb;
+ if ((sa = c2CheckFaces(A, ax, B, bx, &ea)) >= 0) return;
+ if ((sb = c2CheckFaces(B, bx, A, ax, &eb)) >= 0) return;
+
+ const c2Poly* rp,* ip;
+ c2x rx, ix;
+ int re;
+ float kRelTol = 0.95f, kAbsTol = 0.01f;
+ int flip;
+ if (sa * kRelTol > sb + kAbsTol)
+ {
+ rp = A; rx = ax;
+ ip = B; ix = bx;
+ re = ea;
+ flip = 0;
+ }
+ else
+ {
+ rp = B; rx = bx;
+ ip = A; ix = ax;
+ re = eb;
+ flip = 1;
+ }
+
+ c2v incident[2];
+ c2Incident(incident, ip, ix, c2MulrvT(ix.r, c2Mulrv(rx.r, rp->norms[re])));
+ c2h rh;
+ if (!c2SidePlanesFromPoly(incident, rx, rp, re, &rh)) return;
+ c2KeepDeep(incident, rh, m);
+ if (flip) m->n = c2Neg(m->n);
+}
+
+#endif // CUTE_C2_IMPLEMENTATION_ONCE
+#endif // CUTE_C2_IMPLEMENTATION
--- /dev/null
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ * SPDX-FileCopyrightText: 2008-2018 Andrew Walker
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#ifndef DUBINS_H
+#define DUBINS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum
+{
+ LSL = 0,
+ LSR = 1,
+ RSL = 2,
+ RSR = 3,
+ RLR = 4,
+ LRL = 5
+} DubinsPathType;
+
+typedef struct
+{
+ /* the initial configuration */
+ double qi[3];
+ /* the lengths of the three segments */
+ double param[3];
+ /* model forward velocity / model angular velocity */
+ double rho;
+ /* the path type described */
+ DubinsPathType type;
+} DubinsPath;
+
+#define EDUBOK (0) /* No error */
+#define EDUBCOCONFIGS (1) /* Colocated configurations */
+#define EDUBPARAM (2) /* Path parameterisitation error */
+#define EDUBBADRHO (3) /* the rho value is invalid */
+#define EDUBNOPATH (4) /* no connection between configurations with this word */
+
+/**
+ * Callback function for path sampling
+ *
+ * @note the q parameter is a configuration
+ * @note the t parameter is the distance along the path
+ * @note the user_data parameter is forwarded from the caller
+ * @note return non-zero to denote sampling should be stopped
+ */
+typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
+
+/**
+ * Generate a path from an initial configuration to
+ * a target configuration, with a specified maximum turning
+ * radii
+ *
+ * A configuration is (x, y, theta), where theta is in radians, with zero
+ * along the line x = 0, and counter-clockwise is positive
+ *
+ * @param path - the resultant path
+ * @param q0 - a configuration specified as an array of x, y, theta
+ * @param q1 - a configuration specified as an array of x, y, theta
+ * @param rho - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @return - non-zero on error
+ */
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
+
+/**
+ * Generate a path with a specified word from an initial configuration to
+ * a target configuration, with a specified turning radius
+ *
+ * @param path - the resultant path
+ * @param q0 - a configuration specified as an array of x, y, theta
+ * @param q1 - a configuration specified as an array of x, y, theta
+ * @param rho - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @param pathType - the specific path type to use
+ * @return - non-zero on error
+ */
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
+
+/**
+ * Calculate the length of an initialised path
+ *
+ * @param path - the path to find the length of
+ */
+double dubins_path_length(DubinsPath* path);
+
+/**
+ * Return the length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length(DubinsPath* path, int i);
+
+/**
+ * Return the normalized length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length_normalized( DubinsPath* path, int i );
+
+/**
+ * Extract an integer that represents which path type was used
+ *
+ * @param path - an initialised path
+ * @return - one of LSL, LSR, RSL, RSR, RLR or LRL
+ */
+DubinsPathType dubins_path_type(DubinsPath* path);
+
+/**
+ * Calculate the configuration along the path, using the parameter t
+ *
+ * @param path - an initialised path
+ * @param t - a length measure, where 0 <= t < dubins_path_length(path)
+ * @param q - the configuration result
+ * @returns - non-zero if 't' is not in the correct range
+ */
+int dubins_path_sample(DubinsPath* path, double t, double q[3]);
+
+/**
+ * Walk along the path at a fixed sampling interval, calling the
+ * callback function at each interval
+ *
+ * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
+ *
+ * @param path - the path to sample
+ * @param stepSize - the distance along the path for subsequent samples
+ * @param cb - the callback function to call for each sample
+ * @param user_data - optional information to pass on to the callback
+ *
+ * @returns - zero on successful completion, or the result of the callback
+ */
+int dubins_path_sample_many(DubinsPath* path,
+ double stepSize,
+ DubinsPathSamplingCallback cb,
+ void* user_data);
+
+/**
+ * Convenience function to identify the endpoint of a path
+ *
+ * @param path - an initialised path
+ * @param q - the configuration result
+ */
+int dubins_path_endpoint(DubinsPath* path, double q[3]);
+
+/**
+ * Convenience function to extract a subset of a path
+ *
+ * @param path - an initialised path
+ * @param t - a length measure, where 0 < t < dubins_path_length(path)
+ * @param newpath - the resultant path
+ */
+int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* DUBINS_H */
--- /dev/null
+// Copyright (c) 2016, Guan-Horng Liu.
+// SPDX-FileCopyrightText: 2016 Guan-Horng Liu
+//
+// SPDX-License-Identifier: BSD-3-Clause
+
+#ifndef SPACES_REEDS_SHEPP_STATE_SPACE_
+#define SPACES_REEDS_SHEPP_STATE_SPACE_
+
+#include <limits>
+#include <cassert>
+
+typedef int (*ReedsSheppPathSamplingCallback)(double q[3], void* user_data);
+typedef int (*ReedsSheppPathTypeCallback)(int t, void* user_data);
+
+class ReedsSheppStateSpace
+{
+public:
+
+ /** \brief The Reeds-Shepp path segment types */
+ enum ReedsSheppPathSegmentType { RS_NOP=0, RS_LEFT=1, RS_STRAIGHT=2, RS_RIGHT=3 };
+
+ /** \brief Reeds-Shepp path types */
+ static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
+
+ /** \brief Complete description of a ReedsShepp path */
+ class ReedsSheppPath
+ {
+ public:
+ ReedsSheppPath(const ReedsSheppPathSegmentType* type=reedsSheppPathType[0],
+ double t=std::numeric_limits<double>::max(), double u=0., double v=0.,
+ double w=0., double x=0.);
+
+ double length() const { return totalLength_; }
+
+ /** Path segment types */
+ const ReedsSheppPathSegmentType* type_;
+ /** Path segment lengths */
+ double length_[5];
+ /** Total length */
+ double totalLength_;
+ };
+
+ ReedsSheppStateSpace(double turningRadius) : rho_(turningRadius) {}
+
+ double distance(double q0[3], double q1[3]);
+
+ void type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data);
+
+ void sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data);
+
+ /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
+ ReedsSheppPath reedsShepp(double q0[3], double q1[3]);
+
+protected:
+ void interpolate(double q0[3], ReedsSheppPath &path, double seg, double q[4]);
+
+ /** \brief Turning radius */
+ double rho_;
+};
+
+#endif
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+/*! \brief RRT* extensions.
+ *
+ * The extensions are used to implement or change the default behavior of the
+ * RRT* algorithm.
+ *
+ * \file
+ * \defgroup ext-col Collision detection extensions
+ * \defgroup ext-store Node storage and searching tree extensions
+ * \defgroup ext-cost Cost extensions
+ * \defgroup ext-opt Path optimization extensions
+ * \defgroup ext-sampl Random sampling extensions
+ * \defgroup ext-steer Steering procedure extensions
+ * \defgroup ext-aux Auxilliary extensions
+ */
+#ifndef RRTS_RRTEXT_H
+#define RRTS_RRTEXT_H
+
+#include "rrts.hh"
+
+// ext2
+#include "cute_c2.h"
+
+// ext4
+#define GRID 1 // in meters
+#define GRID_WIDTH 40 // in meters
+#define GRID_HEIGHT 40 // in meters
+#define GRID_MAX_XI ((unsigned int) floor(GRID_WIDTH / GRID)) // min is 0
+#define GRID_MAX_YI ((unsigned int) floor(GRID_HEIGHT / GRID)) // min is 0
+
+// ext9
+#define GRID_MAX_HI 60
+
+namespace rrts {
+
+/*! \brief Collision check based on enlarged occupancy grid.
+ *
+ * \ingroup ext-col
+ */
+class RRTExt21 : public virtual RRTS {
+private:
+ unsigned int _grid_width = 0;
+ unsigned int _grid_height = 0;
+ float _grid_res = 0.0;
+ int8_t const *_grid_data = nullptr;
+ double _origin_x = 0.0;
+ double _origin_y = 0.0;
+ bool collide(RRTNode const &n);
+ bool collide_steered();
+public:
+ void set_grid_to_check(unsigned int w, unsigned int h, float r,
+ int8_t const *d, double x, double y);
+};
+
+/*! \brief Collision check based on occupancy grid.
+ *
+ * This approach expects obstacles to be represented by points and the collision
+ * occures whenever the point is inside the frame given by the car pose and the
+ * car size.
+ *
+ * \ingroup ext-col
+ */
+class RRTExt20 : public virtual RRTS {
+private:
+ std::vector<bcar::Point> const *_points_to_check = nullptr;
+ bool collide_steered();
+public:
+ void set_points_to_check(std::vector<bcar::Point> const *p);
+};
+
+/*! \brief Use Dubins paths-based steering procedure.
+ *
+ * \ingroup ext-steer
+ * \see https://github.com/AndrewWalker/Dubins-Curves
+ */
+class RRTExt19 : public virtual RRTS {
+private:
+ void steer(RRTNode const &f, RRTNode const &t);
+};
+
+/*! \brief Finish when more than 1000 iterations.
+ *
+ * \ingroup ext-aux
+ */
+class RRTExt18 : public virtual RRTS {
+private:
+ bool should_finish() const;
+};
+
+/*! \brief Finish when goal found or more than 1000 iterations.
+ *
+ * \ingroup ext-aux
+ */
+class RRTExt17 : public virtual RRTS {
+private:
+ bool should_finish() const;
+};
+
+/*! \brief Use Reeds & Shepp steering procedure.
+ *
+ * \ingroup ext-steer
+ */
+class RRTExt16 : public virtual RRTS {
+private:
+ void steer(RRTNode const& f, RRTNode const& t);
+};
+
+/*! \brief Log goal's cumulative cost each iteration.
+ *
+ * \ingroup ext-aux
+ */
+class RRTExt15 : public virtual RRTS {
+private:
+ std::vector<double> log_path_cost_;
+public:
+ Json::Value json() const;
+ void json(Json::Value jvi);
+ bool next();
+};
+
+/*! \brief Random sampling in the circuit between root and goal.
+ *
+ * \ingroup ext-sampl
+ * \see https://stackoverflow.com/questions/5837572/generate-a-random-point-within-a-circle-uniformly/50746409#50746409
+ */
+class RRTExt14 : public virtual RRTS {
+private:
+ Point circle_c_;
+ double circle_r_ = 0.0;
+ std::uniform_real_distribution<double> udr_;
+ std::uniform_real_distribution<double> udt_;
+ std::uniform_real_distribution<double> udh_;
+ RRTNode sample();
+public:
+ RRTExt14();
+ void reset();
+};
+
+/*! \brief Use Dijkstra algorithm to find path between interesting nodes.
+ *
+ * The search for interesting nodes starts at root, the last drivable nodes is
+ * added to interesting nodes until goal is reached.
+ *
+ * \ingroup ext-opt
+ */
+class RRTExt13 : public virtual RRTS {
+private:
+ class DijkstraNode {
+ public:
+ RRTNode* node = nullptr;
+ unsigned int i = 0;
+ bool v = false;
+ double d = 0.0;
+ bool vi();
+ DijkstraNode(RRTNode* n);
+ };
+ class DijkstraNodeComparator {
+ public:
+ int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+ };
+ class DijkstraNodeBackwardComparator {
+ public:
+ int operator() (DijkstraNode const& n1, DijkstraNode const& n2);
+ };
+ std::vector<RRTNode*> opath_;
+ double ogoal_cc_ = 0.0;
+ double otime_ = 0.0;
+ std::vector<DijkstraNode> dn_;
+ void interesting_forward();
+ void interesting_backward();
+ void dijkstra_forward();
+ void dijkstra_backward();
+ void compute_path();
+public:
+ RRTExt13();
+ Json::Value json() const;
+ void json(Json::Value jvi);
+ void reset();
+};
+
+/* \brief Different `steer` procedures.
+
+Use sampling in control input for `steer1`. Use R&S steering for `steer2`.
+*/
+class RRTExt12 : public virtual RRTS {
+ protected:
+ void steer1(RRTNode &f, RRTNode &t);
+ bool connect();
+ public:
+ bool next();
+};
+
+/* \brief Goal Zone.
+
+*/
+class RRTExt11 : public virtual RRTS {
+ protected:
+ bool goal_found(RRTNode &f);
+};
+
+/*! \brief Reeds & Shepp (build) and Euclidean + abs angle (search).
+ *
+ * Use Reeds & Shepp path length for building tree data structure and Euclidean
+ * distance + (abs) heading difference + 0.1 * backward-forward direction
+ * changes for searching it.
+ *
+ * \ingroup ext-cost
+ * \see https://doi.org/10.1109/TITS.2015.2477355
+ */
+class RRTExt10 : public virtual RRTS {
+protected:
+ double cost_build(RRTNode const& f, RRTNode const& t) const;
+ double cost_search(RRTNode const& f, RRTNode const& t) const;
+};
+
+/* \brief Use grid data structure to store RRT nodes.
+
+This approach speeds up the search process for the nearest neighbor and
+the near vertices procedures.
+*/
+class RRTExt9 : public virtual RRTS {
+ private:
+ class Cell {
+ private:
+ bool changed_ = false;
+ std::vector<RRTNode *> nodes_;
+ public:
+ void nn(RRTNode *t, RRTNode **nn, RRTS *p);
+ void store_node(RRTNode *n);
+
+ // getter, setter
+ bool changed() const
+ {
+ return this->changed_;
+ }
+ std::vector<RRTNode *> &nodes()
+ {
+ return this->nodes_;
+ }
+
+ Cell();
+ };
+ Cell grid_[GRID_MAX_XI][GRID_MAX_YI][GRID_MAX_HI];
+ double x_min_ = 0;
+ double x_max_ = 0;
+ double y_min_ = 0;
+ double y_max_ = 0;
+ double h_min_ = 0;
+ double h_max_ = 2 * M_PI;
+
+ unsigned int xi(RRTNode n);
+ unsigned int yi(RRTNode n);
+ unsigned int hi(RRTNode n);
+ public:
+ void init();
+ void deinit();
+ void store_node(RRTNode n);
+ RRTNode *nn(RRTNode &t);
+ std::vector<RRTNode *> nv(RRTNode &t);
+};
+
+/*! \brief Use 3D k-d tree data structure to store RRT nodes.
+ *
+ * This approach speeds up the search process for the nearest neighbor and the
+ * near vertices procedures. This extension implements 3D K-d tree.
+ *
+ * \ingroup ext-store
+ * \see https://en.wikipedia.org/wiki/K-d_tree
+ */
+class RRTExt8 : public virtual RRTS {
+private:
+ class KdNode {
+ public:
+ RRTNode* node = nullptr;
+ KdNode* left = nullptr;
+ KdNode* right = nullptr;
+ KdNode(RRTNode* n);
+ };
+ KdNode* kdroot_ = nullptr;
+ std::vector<KdNode> kdnodes_;
+ void store(RRTNode* n, KdNode*& ref, unsigned int lvl);
+ void find_nn(RRTNode const& t, KdNode const* const r, unsigned int lvl);
+ void find_nv(RRTNode const& t, KdNode const* const r, unsigned int lvl);
+public:
+ RRTExt8();
+ void reset();
+ void store(RRTNode n);
+ void find_nn(RRTNode const& t);
+ void find_nv(RRTNode const& t);
+};
+
+/* \brief Use k-d tree data structure to store RRT nodes.
+
+This approach speeds up the search process for the nearest neighbor and
+the near vertices procedures. This extension implements 2D K-d tree.
+
+\see https://en.wikipedia.org/wiki/K-d_tree
+*/
+class RRTExt7 : public virtual RRTS {
+ private:
+ class KdNode {
+ private:
+ RRTNode *node_ = nullptr;
+ KdNode *left_ = nullptr;
+ KdNode *right_ = nullptr;
+ public:
+ // getter, setter
+ RRTNode *node() const { return this->node_; }
+ KdNode *&left() { return this->left_; }
+ KdNode *&right() { return this->right_; }
+ KdNode(RRTNode *n);
+ };
+ KdNode *kdroot_ = nullptr;
+ void delete_kd_nodes(KdNode *n);
+ void store_node(RRTNode *n, KdNode *&r, int l);
+ void nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d);
+ public:
+ void init();
+ void deinit();
+ void store_node(RRTNode n);
+ RRTNode *nn(RRTNode &t);
+ std::vector<RRTNode *> nv(RRTNode &t);
+};
+
+/*! \brief Reeds & Shepp (build, search).
+ *
+ * Use Reeds & Shepp path length for building tree data structure as well as for
+ * searching it.
+ *
+ * \ingroup ext-cost
+ */
+class RRTExt6 : public virtual RRTS {
+private:
+ double cost_build(RRTNode const& f, RRTNode const& t) const;
+ double cost_search(RRTNode const& f, RRTNode const& t) const;
+};
+
+/* \brief Different costs extension.
+
+Use different cost for bulding tree data structure and searching in the
+structure. This one is complementary to `rrtext1.cc`.
+*/
+class RRTExt5 : public virtual RRTS {
+ public:
+ /* \brief Reeds and Shepp path length.
+ */
+ double cost_build(RRTNode &f, RRTNode &t);
+ /* \brief Euclidean distance.
+ */
+ double cost_search(RRTNode &f, RRTNode &t);
+};
+
+/* \brief Use grid data structure to store RRT nodes.
+
+This approach speeds up the search process for the nearest neighbor and
+the near vertices procedures.
+*/
+class RRTExt4 : public virtual RRTS {
+ private:
+ class Cell {
+ private:
+ bool changed_ = false;
+ std::vector<RRTNode *> nodes_;
+ public:
+ void nn(RRTNode *t, RRTNode **nn, RRTS *p);
+ void store_node(RRTNode *n);
+
+ // getter, setter
+ bool changed() const
+ {
+ return this->changed_;
+ }
+ std::vector<RRTNode *> &nodes()
+ {
+ return this->nodes_;
+ }
+
+ Cell();
+ };
+ Cell grid_[GRID_MAX_XI][GRID_MAX_YI]; // [0, 0] is bottom left
+ double x_min_ = 0;
+ double x_max_ = 0;
+ double y_min_ = 0;
+ double y_max_ = 0;
+
+ unsigned int xi(RRTNode n);
+ unsigned int yi(RRTNode n);
+ public:
+ void init();
+ void deinit();
+ void store_node(RRTNode n);
+ RRTNode *nn(RRTNode &t);
+ std::vector<RRTNode *> nv(RRTNode &t);
+};
+
+/* \brief Use Dijkstra algorithm to find the shorter path.
+*/
+class RRTExt3 : public virtual RRTS {
+ private:
+ public:
+ void reset();
+ std::vector<RRTNode *> orig_path_;
+ double orig_path_cost_ = 9999;
+ std::vector<RRTNode *> first_optimized_path_;
+ double first_optimized_path_cost_ = 9999;
+ void first_path_optimization();
+ void second_path_optimization();
+ void compute_path();
+ Json::Value json();
+ void json(Json::Value jvi);
+
+ // getter, setter
+ std::vector<RRTNode *> &orig_path()
+ {
+ return this->orig_path_;
+ };
+ double &orig_path_cost() { return this->orig_path_cost_; }
+ void orig_path_cost(double c) { this->orig_path_cost_ = c; }
+ std::vector<RRTNode *> &first_optimized_path()
+ {
+ return this->first_optimized_path_;
+ };
+ double &first_optimized_path_cost() {
+ return this->first_optimized_path_cost_;
+ }
+ void first_optimized_path_cost(double c) {
+ this->first_optimized_path_cost_ = c;
+ }
+};
+
+/*! \brief Use cute_c2 library for collision detection.
+ *
+ * \ingroup ext-col
+ * \see https://github.com/RandyGaul/cute_headers/blob/master/cute_c2.h
+ */
+class RRTExt2 : public virtual RRTS {
+private:
+ c2Poly c2_bc_;
+ c2x c2x_bc_;
+ std::vector<c2Poly> c2_obstacles_;
+ bool collide(RRTNode const& n);
+ bool collide_steered();
+public:
+ RRTExt2();
+ Json::Value json() const;
+ void json(Json::Value jvi);
+ void reset();
+};
+
+/* \brief Different costs extension.
+
+Use different cost for bulding tree data structure and searching in the
+structure.
+*/
+class RRTExt1 : public virtual RRTS {
+ public:
+ /* \brief Reeds and Shepp path length.
+ */
+ double cost_build(RRTNode &f, RRTNode &t);
+ /* \brief Matej's heuristics.
+ */
+ double cost_search(RRTNode &f, RRTNode &t);
+};
+
+} // namespace rrts
+#endif /* RRTS_RRTEXT_H */
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+/*! \brief RRT* structure and default procedures.
+ *
+ * \file
+ */
+#ifndef RRTS_RRTS_H
+#define RRTS_RRTS_H
+
+#include <chrono>
+#include <functional>
+#include <json/json.h>
+#include <random>
+#include <vector>
+#include "bcar.hh"
+
+namespace rrts {
+using namespace bcar;
+
+/*! Compute elapsed time class. */
+class Ter {
+private:
+ std::chrono::high_resolution_clock::time_point tstart_;
+public:
+ void start();
+ double scnt() const;
+};
+
+/*! Store RRT node. */
+class RRTNode : public virtual Pose, public virtual CarMove {
+private:
+ double c_ = 0.0;
+ double cc_ = 0.0;
+ RRTNode* p_ = nullptr;
+ unsigned int cusp_ = 0;
+public:
+ /*! Get cost to parent. */
+ double c() const;
+
+ /*! Set cost to parent. */
+ void c(double c);
+
+ /*! Get cumulative cost from root. */
+ double cc() const;
+
+ /*! Get parent node. */
+ RRTNode* p() const;
+
+ /*! Set parent node. */
+ void p(RRTNode& p);
+
+ /*! Get number of backward-forward direction changes. */
+ unsigned int cusp() const;
+
+ /*! Set number of backward-forward direction changes. */
+ void cusp(RRTNode const& p);
+
+ bool operator==(RRTNode const& n);
+
+ int segment_type = 0;
+};
+
+class RRTGoal : public virtual RRTNode, public virtual PoseRange {
+public:
+ using PoseRange::PoseRange;
+};
+
+/*! RRT* algorithm basic class. */
+class RRTS {
+protected:
+ BicycleCar bc_;
+ RRTGoal goal_;
+ unsigned int icnt_ = 0;
+ unsigned int _imax = 1000;
+ Ter ter_;
+ std::default_random_engine gen_;
+ std::vector<RRTNode> nodes_;
+ std::vector<RRTNode> steered_;
+ std::vector<RRTNode*> path_;
+ RRTNode* nn_ = nullptr;
+ std::vector<RRTNode*> nv_;
+ double cost_ = 0.0;
+ double eta_ = 0.5;
+ double time_ = 0.0;
+ double last_goal_cc_ = 0.0;
+ std::vector<RRTNode> last_path_;
+ void recompute_cc(RRTNode* g);
+ void recompute_path_cc();
+ double min_gamma_eta() const;
+ bool should_continue() const;
+ void join_steered(RRTNode* f);
+ RRTNode& nn();
+ bool connect();
+ void rewire();
+ bool goal_drivable_from(RRTNode const& f);
+ virtual void store(RRTNode n);
+ virtual double cost_build(RRTNode const& f, RRTNode const& t) const;
+ virtual double cost_search(RRTNode const& f, RRTNode const& t) const;
+ virtual void find_nn(RRTNode const& t);
+ virtual void find_nv(RRTNode const& t);
+ virtual void compute_path();
+ virtual void steer(RRTNode const& f, RRTNode const& t) = 0;
+ virtual bool collide_steered() = 0;
+ virtual RRTNode sample() = 0;
+ virtual bool should_finish() const = 0;
+public:
+ RRTS();
+
+ /*! Set internal bicycle car. */
+ BicycleCar &bc();
+
+ /*! Set maximum number of iterations before reset. */
+ void set_imax_reset(unsigned int i);
+
+ /*! Set goal. */
+ void set_goal(double x, double y, double b, double e);
+
+ /*! Set start. */
+ void set_start(double x, double y, double h);
+
+ /*! Get path. */
+ std::vector<Pose> get_path() const;
+
+ /*! Get path cost. */
+ double get_path_cost() const;
+
+ /*! Get iterations counter. */
+ unsigned int icnt() const;
+
+ /*! Set iterations counter. */
+ void icnt(unsigned int i);
+
+ /*! Return elapsed time. */
+ double scnt() const;
+
+ double eta() const;
+ void eta(double e);
+
+ /*! Generate JSON output. */
+ Json::Value json() const;
+
+ /*! Load JSON input. */
+ void json(Json::Value jvi);
+
+ /*! Run next RRT* iteration. */
+ virtual bool next();
+
+ /*! Reset the algorithm. */
+ virtual void reset();
+};
+
+} // namespace rrts
+#endif /* RRTS_RRTS_H */
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+/*! \brief RRT* planners.
+ *
+ * This file contains _to be used_ RRT* planners consisting of the RRT*
+ * extensions.
+ *
+ * RRT* planners have no implementation.
+ *
+ * \file
+ * \defgroup planners RRTS Planners
+ */
+#ifndef RRTS_RRTSP_H
+#define RRTS_RRTSP_H
+
+#include "rrtext.hh"
+
+namespace rrts {
+
+/*! \brief Planner for F1/10.
+ *
+ * \ingroup planners
+ */
+class P40 : public RRTExt21, public RRTExt8, public RRTExt10, public RRTExt14,
+ public RRTExt15, public RRTExt19, public RRTExt17,
+ public RRTExt13 {
+public:
+ Json::Value json() const
+ {
+ auto jvo = RRTExt13::json();
+ auto json15 = RRTExt15::json();
+ jvo["log_path_cost"] = json15["log_path_cost"];
+ return jvo;
+ }
+ void reset()
+ {
+ RRTExt8::reset();
+ RRTExt14::reset();
+ RRTExt13::reset();
+ }
+};
+
+/*! \brief Planner with optimization and reset.
+ *
+ * \ingroup planners
+ */
+class P39 : public RRTExt2, public RRTExt8, public RRTExt10, public RRTExt14,
+ public RRTExt15, public RRTExt16, public RRTExt17,
+ public RRTExt13 {
+public:
+ Json::Value json() const
+ {
+ auto jvo = RRTExt13::json();
+ auto json15 = RRTExt15::json();
+ jvo["log_path_cost"] = json15["log_path_cost"];
+ return jvo;
+ }
+ void json(Json::Value jvi)
+ {
+ RRTExt2::json(jvi);
+ }
+ void reset()
+ {
+ RRTExt8::reset();
+ RRTExt14::reset();
+ RRTExt13::reset();
+ RRTExt2::reset();
+ }
+};
+
+class P38 : public RRTExt2, public RRTExt8, public RRTExt10, public RRTExt14,
+ public RRTExt15, public RRTExt16, public RRTExt18,
+ public RRTExt13 {
+public:
+ Json::Value json() const
+ {
+ auto jvo = RRTExt13::json();
+ auto json15 = RRTExt15::json();
+ jvo["log_path_cost"] = json15["log_path_cost"];
+ return jvo;
+ }
+ void json(Json::Value jvi)
+ {
+ RRTExt2::json(jvi);
+ }
+ void reset()
+ {
+ RRTExt8::reset();
+ RRTExt14::reset();
+ RRTExt13::reset();
+ RRTExt2::reset();
+ }
+};
+
+class P37 : public RRTExt2, public RRTExt8, public RRTExt10, public RRTExt14,
+ public RRTExt15, public RRTExt16, public RRTExt18 {
+public:
+ Json::Value json() const
+ {
+ return RRTExt15::json();
+ }
+ void json(Json::Value jvi)
+ {
+ RRTExt2::json(jvi);
+ }
+ void reset()
+ {
+ RRTExt8::reset();
+ RRTExt14::reset();
+ RRTExt2::reset();
+ }
+};
+
+class P36 : public RRTExt2, public RRTExt8, public RRTExt10, public RRTExt14,
+ public RRTExt15, public RRTExt16, public RRTExt17 {
+public:
+ Json::Value json() const
+ {
+ return RRTExt15::json();
+ }
+ void json(Json::Value jvi)
+ {
+ RRTExt2::json(jvi);
+ }
+ void reset()
+ {
+ RRTExt8::reset();
+ RRTExt14::reset();
+ RRTExt2::reset();
+ }
+};
+
+} // namespace rrts
+#endif /* RRTS_RRTSP_H */
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2008-2018 Andrew Walker
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#ifdef WIN32
+#define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+#include "dubins.h"
+
+#define EPSILON (10e-10)
+
+typedef enum
+{
+ L_SEG = 0,
+ S_SEG = 1,
+ R_SEG = 2
+} SegmentType;
+
+/* The segment types for each of the Path types */
+const SegmentType DIRDATA[][3] = {
+ { L_SEG, S_SEG, L_SEG },
+ { L_SEG, S_SEG, R_SEG },
+ { R_SEG, S_SEG, L_SEG },
+ { R_SEG, S_SEG, R_SEG },
+ { R_SEG, L_SEG, R_SEG },
+ { L_SEG, R_SEG, L_SEG }
+};
+
+typedef struct
+{
+ double alpha;
+ double beta;
+ double d;
+ double sa;
+ double sb;
+ double ca;
+ double cb;
+ double c_ab;
+ double d_sq;
+} DubinsIntermediateResults;
+
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
+
+/**
+ * Floating point modulus suitable for rings
+ *
+ * fmod doesn't behave correctly for angular quantities, this function does
+ */
+double fmodr( double x, double y)
+{
+ return x - y*floor(x/y);
+}
+
+double mod2pi( double theta )
+{
+ return fmodr( theta, 2 * M_PI );
+}
+
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
+{
+ int i, errcode;
+ DubinsIntermediateResults in;
+ double params[3];
+ double cost;
+ double best_cost = INFINITY;
+ int best_word = -1;
+ errcode = dubins_intermediate_results(&in, q0, q1, rho);
+ if(errcode != EDUBOK) {
+ return errcode;
+ }
+
+
+ path->qi[0] = q0[0];
+ path->qi[1] = q0[1];
+ path->qi[2] = q0[2];
+ path->rho = rho;
+
+ for( i = 0; i < 6; i++ ) {
+ DubinsPathType pathType = (DubinsPathType)i;
+ errcode = dubins_word(&in, pathType, params);
+ if(errcode == EDUBOK) {
+ cost = params[0] + params[1] + params[2];
+ if(cost < best_cost) {
+ best_word = i;
+ best_cost = cost;
+ path->param[0] = params[0];
+ path->param[1] = params[1];
+ path->param[2] = params[2];
+ path->type = pathType;
+ }
+ }
+ }
+ if(best_word == -1) {
+ return EDUBNOPATH;
+ }
+ return EDUBOK;
+}
+
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
+{
+ int errcode;
+ DubinsIntermediateResults in;
+ errcode = dubins_intermediate_results(&in, q0, q1, rho);
+ if(errcode == EDUBOK) {
+ double params[3];
+ errcode = dubins_word(&in, pathType, params);
+ if(errcode == EDUBOK) {
+ path->param[0] = params[0];
+ path->param[1] = params[1];
+ path->param[2] = params[2];
+ path->qi[0] = q0[0];
+ path->qi[1] = q0[1];
+ path->qi[2] = q0[2];
+ path->rho = rho;
+ path->type = pathType;
+ }
+ }
+ return errcode;
+}
+
+double dubins_path_length( DubinsPath* path )
+{
+ double length = 0.;
+ length += path->param[0];
+ length += path->param[1];
+ length += path->param[2];
+ length = length * path->rho;
+ return length;
+}
+
+double dubins_segment_length( DubinsPath* path, int i )
+{
+ if( (i < 0) || (i > 2) )
+ {
+ return INFINITY;
+ }
+ return path->param[i] * path->rho;
+}
+
+double dubins_segment_length_normalized( DubinsPath* path, int i )
+{
+ if( (i < 0) || (i > 2) )
+ {
+ return INFINITY;
+ }
+ return path->param[i];
+}
+
+DubinsPathType dubins_path_type( DubinsPath* path )
+{
+ return path->type;
+}
+
+void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
+{
+ double st = sin(qi[2]);
+ double ct = cos(qi[2]);
+ if( type == L_SEG ) {
+ qt[0] = +sin(qi[2]+t) - st;
+ qt[1] = -cos(qi[2]+t) + ct;
+ qt[2] = t;
+ }
+ else if( type == R_SEG ) {
+ qt[0] = -sin(qi[2]-t) + st;
+ qt[1] = +cos(qi[2]-t) - ct;
+ qt[2] = -t;
+ }
+ else if( type == S_SEG ) {
+ qt[0] = ct * t;
+ qt[1] = st * t;
+ qt[2] = 0.0;
+ }
+ qt[0] += qi[0];
+ qt[1] += qi[1];
+ qt[2] += qi[2];
+}
+
+int dubins_path_sample( DubinsPath* path, double t, double q[3] )
+{
+ /* tprime is the normalised variant of the parameter t */
+ double tprime = t / path->rho;
+ double qi[3]; /* The translated initial configuration */
+ double q1[3]; /* end-of segment 1 */
+ double q2[3]; /* end-of segment 2 */
+ const SegmentType* types = DIRDATA[path->type];
+ double p1, p2;
+
+ if( t < 0 || t > dubins_path_length(path) ) {
+ return EDUBPARAM;
+ }
+
+ /* initial configuration */
+ qi[0] = 0.0;
+ qi[1] = 0.0;
+ qi[2] = path->qi[2];
+
+ /* generate the target configuration */
+ p1 = path->param[0];
+ p2 = path->param[1];
+ dubins_segment( p1, qi, q1, types[0] );
+ dubins_segment( p2, q1, q2, types[1] );
+ if( tprime < p1 ) {
+ dubins_segment( tprime, qi, q, types[0] );
+ }
+ else if( tprime < (p1+p2) ) {
+ dubins_segment( tprime-p1, q1, q, types[1] );
+ }
+ else {
+ dubins_segment( tprime-p1-p2, q2, q, types[2] );
+ }
+
+ /* scale the target configuration, translate back to the original starting point */
+ q[0] = q[0] * path->rho + path->qi[0];
+ q[1] = q[1] * path->rho + path->qi[1];
+ q[2] = mod2pi(q[2]);
+
+ return EDUBOK;
+}
+
+int dubins_path_sample_many(DubinsPath* path, double stepSize,
+ DubinsPathSamplingCallback cb, void* user_data)
+{
+ int retcode;
+ double q[3];
+ double x = 0.0;
+ double length = dubins_path_length(path);
+ while( x < length ) {
+ dubins_path_sample( path, x, q );
+ retcode = cb(q, x, user_data);
+ if( retcode != 0 ) {
+ return retcode;
+ }
+ x += stepSize;
+ }
+ return 0;
+}
+
+int dubins_path_endpoint( DubinsPath* path, double q[3] )
+{
+ return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
+}
+
+int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
+{
+ /* calculate the true parameter */
+ double tprime = t / path->rho;
+
+ if((t < 0) || (t > dubins_path_length(path)))
+ {
+ return EDUBPARAM;
+ }
+
+ /* copy most of the data */
+ newpath->qi[0] = path->qi[0];
+ newpath->qi[1] = path->qi[1];
+ newpath->qi[2] = path->qi[2];
+ newpath->rho = path->rho;
+ newpath->type = path->type;
+
+ /* fix the parameters */
+ newpath->param[0] = fmin( path->param[0], tprime );
+ newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
+ newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
+ return 0;
+}
+
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
+{
+ double dx, dy, D, d, theta, alpha, beta;
+ if( rho <= 0.0 ) {
+ return EDUBBADRHO;
+ }
+
+ dx = q1[0] - q0[0];
+ dy = q1[1] - q0[1];
+ D = sqrt( dx * dx + dy * dy );
+ d = D / rho;
+ theta = 0;
+
+ /* test required to prevent domain errors if dx=0 and dy=0 */
+ if(d > 0) {
+ theta = mod2pi(atan2( dy, dx ));
+ }
+ alpha = mod2pi(q0[2] - theta);
+ beta = mod2pi(q1[2] - theta);
+
+ in->alpha = alpha;
+ in->beta = beta;
+ in->d = d;
+ in->sa = sin(alpha);
+ in->sb = sin(beta);
+ in->ca = cos(alpha);
+ in->cb = cos(beta);
+ in->c_ab = cos(alpha - beta);
+ in->d_sq = d * d;
+
+ return EDUBOK;
+}
+
+int dubins_LSL(DubinsIntermediateResults* in, double out[3])
+{
+ double tmp0, tmp1, p_sq;
+
+ tmp0 = in->d + in->sa - in->sb;
+ p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
+
+ if(p_sq >= 0) {
+ tmp1 = atan2( (in->cb - in->ca), tmp0 );
+ out[0] = mod2pi(tmp1 - in->alpha);
+ out[1] = sqrt(p_sq);
+ out[2] = mod2pi(in->beta - tmp1);
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+
+int dubins_RSR(DubinsIntermediateResults* in, double out[3])
+{
+ double tmp0 = in->d - in->sa + in->sb;
+ double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
+ if( p_sq >= 0 ) {
+ double tmp1 = atan2( (in->ca - in->cb), tmp0 );
+ out[0] = mod2pi(in->alpha - tmp1);
+ out[1] = sqrt(p_sq);
+ out[2] = mod2pi(tmp1 -in->beta);
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_LSR(DubinsIntermediateResults* in, double out[3])
+{
+ double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
+ if( p_sq >= 0 ) {
+ double p = sqrt(p_sq);
+ double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
+ out[0] = mod2pi(tmp0 - in->alpha);
+ out[1] = p;
+ out[2] = mod2pi(tmp0 - mod2pi(in->beta));
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_RSL(DubinsIntermediateResults* in, double out[3])
+{
+ double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
+ if( p_sq >= 0 ) {
+ double p = sqrt(p_sq);
+ double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
+ out[0] = mod2pi(in->alpha - tmp0);
+ out[1] = p;
+ out[2] = mod2pi(in->beta - tmp0);
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_RLR(DubinsIntermediateResults* in, double out[3])
+{
+ double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
+ double phi = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
+ if( fabs(tmp0) <= 1) {
+ double p = mod2pi((2*M_PI) - acos(tmp0) );
+ double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
+ out[0] = t;
+ out[1] = p;
+ out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_LRL(DubinsIntermediateResults* in, double out[3])
+{
+ double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
+ double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
+ if( fabs(tmp0) <= 1) {
+ double p = mod2pi( 2*M_PI - acos( tmp0) );
+ double t = mod2pi(-in->alpha - phi + p/2.);
+ out[0] = t;
+ out[1] = p;
+ out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
+ return EDUBOK;
+ }
+ return EDUBNOPATH;
+}
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3])
+{
+ int result;
+ switch(pathType)
+ {
+ case LSL:
+ result = dubins_LSL(in, out);
+ break;
+ case RSL:
+ result = dubins_RSL(in, out);
+ break;
+ case LSR:
+ result = dubins_LSR(in, out);
+ break;
+ case RSR:
+ result = dubins_RSR(in, out);
+ break;
+ case LRL:
+ result = dubins_LRL(in, out);
+ break;
+ case RLR:
+ result = dubins_RLR(in, out);
+ break;
+ default:
+ result = EDUBNOPATH;
+ }
+ return result;
+}
--- /dev/null
+// Copyright (c) 2016, Guan-Horng Liu.
+// SPDX-FileCopyrightText: 2016 Guan-Horng Liu
+//
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include "reeds_shepp.h"
+#include <cmath>
+
+namespace
+{
+ // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
+
+ const double pi = M_PI;
+ const double twopi = 2. * pi;
+ const double RS_EPS = 1e-6;
+ const double ZERO = 10*std::numeric_limits<double>::epsilon();
+
+ inline double mod2pi(double x)
+ {
+ double v = fmod(x, twopi);
+ if (v < -pi)
+ v += twopi;
+ else
+ if (v > pi)
+ v -= twopi;
+ return v;
+ }
+ inline void polar(double x, double y, double &r, double &theta)
+ {
+ r = sqrt(x*x + y*y);
+ theta = atan2(y, x);
+ }
+ inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
+ {
+ double delta = mod2pi(u-v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
+ double t1 = atan2(eta*A - xi*B, xi*A + eta*B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
+ tau = (t2<0) ? mod2pi(t1+pi) : mod2pi(t1);
+ omega = mod2pi(tau - u + v - phi) ;
+ }
+
+ // formula 8.1 in Reeds-Shepp paper
+ inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
+ {
+ polar(x - sin(phi), y - 1. + cos(phi), u, t);
+ if (t >= -ZERO)
+ {
+ v = mod2pi(phi - t);
+ if (v >= -ZERO)
+ {
+ assert(fabs(u*cos(t) + sin(phi) - x) < RS_EPS);
+ assert(fabs(u*sin(t) - cos(phi) + 1 - y) < RS_EPS);
+ assert(fabs(mod2pi(t+v - phi)) < RS_EPS);
+ return true;
+ }
+ }
+ return false;
+ }
+ // formula 8.2
+ inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
+ {
+ double t1, u1;
+ polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
+ u1 = u1*u1;
+ if (u1 >= 4.)
+ {
+ double theta;
+ u = sqrt(u1 - 4.);
+ theta = atan2(2., u);
+ t = mod2pi(t1 + theta);
+ v = mod2pi(t - phi);
+ assert(fabs(2*sin(t) + u*cos(t) - sin(phi) - x) < RS_EPS);
+ assert(fabs(-2*cos(t) + u*sin(t) + cos(phi) + 1 - y) < RS_EPS);
+ assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
+ return t>=-ZERO && v>=-ZERO;
+ }
+ return false;
+ }
+ void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+ {
+ double t, u, v, Lmin = path.length(), L;
+ if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
+ Lmin = L;
+ }
+ if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
+ Lmin = L;
+ }
+ if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
+ Lmin = L;
+ }
+ if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
+ Lmin = L;
+ }
+ if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
+ Lmin = L;
+ }
+ if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
+ Lmin = L;
+ }
+ if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
+ Lmin = L;
+ }
+ if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
+ }
+ // formula 8.3 / 8.4 *** TYPO IN PAPER ***
+ inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
+ {
+ double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
+ polar(xi, eta, u1, theta);
+ if (u1 <= 4.)
+ {
+ u = -2.*asin(.25 * u1);
+ t = mod2pi(theta + .5 * u + pi);
+ v = mod2pi(phi - t + u);
+ assert(fabs(2*(sin(t) - sin(t-u)) + sin(phi) - x) < RS_EPS);
+ assert(fabs(2*(-cos(t) + cos(t-u)) - cos(phi) + 1 - y) < RS_EPS);
+ assert(fabs(mod2pi(t-u+v - phi)) < RS_EPS);
+ return t>=-ZERO && u<=ZERO;
+ }
+ return false;
+ }
+ void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+ {
+ double t, u, v, Lmin = path.length(), L;
+ if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
+ Lmin = L;
+ }
+ if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
+ Lmin = L;
+ }
+ if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
+ Lmin = L;
+ }
+ if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
+ Lmin = L;
+ }
+
+ // backwards
+ double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
+ if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
+ Lmin = L;
+ }
+ if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
+ Lmin = L;
+ }
+ if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
+ Lmin = L;
+ }
+ if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
+ }
+ // formula 8.7
+ inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
+ {
+ double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi*xi + eta*eta));
+ if (rho <= 1.)
+ {
+ u = acos(rho);
+ tauOmega(u, -u, xi, eta, phi, t, v);
+ assert(fabs(2*(sin(t)-sin(t-u)+sin(t-2*u))-sin(phi) - x) < RS_EPS);
+ assert(fabs(2*(-cos(t)+cos(t-u)-cos(t-2*u))+cos(phi)+1 - y) < RS_EPS);
+ assert(fabs(mod2pi(t-2*u-v - phi)) < RS_EPS);
+ return t>=-ZERO && v<=ZERO;
+ }
+ return false;
+ }
+ // formula 8.8
+ inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
+ {
+ double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi*xi - eta*eta) / 16.;
+ if (rho>=0 && rho<=1)
+ {
+ u = -acos(rho);
+ if (u >= -.5 * pi)
+ {
+ tauOmega(u, u, xi, eta, phi, t, v);
+ assert(fabs(4*sin(t)-2*sin(t-u)-sin(phi) - x) < RS_EPS);
+ assert(fabs(-4*cos(t)+2*cos(t-u)+cos(phi)+1 - y) < RS_EPS);
+ assert(fabs(mod2pi(t-v - phi)) < RS_EPS);
+ return t>=-ZERO && v>=-ZERO;
+ }
+ }
+ return false;
+ }
+ void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+ {
+ double t, u, v, Lmin = path.length(), L;
+ if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
+ Lmin = L;
+ }
+ if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
+ Lmin = L;
+ }
+ if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
+ Lmin = L;
+ }
+ if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
+ Lmin = L;
+ }
+
+ if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
+ Lmin = L;
+ }
+ if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
+ Lmin = L;
+ }
+ if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
+ Lmin = L;
+ }
+ if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2.*fabs(u) + fabs(v))) // timeflip + reflect
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
+ }
+ // formula 8.9
+ inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
+ {
+ double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
+ polar(xi, eta, rho, theta);
+ if (rho >= 2.)
+ {
+ double r = sqrt(rho*rho - 4.);
+ u = 2. - r;
+ t = mod2pi(theta + atan2(r, -2.));
+ v = mod2pi(phi - .5*pi - t);
+ assert(fabs(2*(sin(t)-cos(t))-u*sin(t)+sin(phi) - x) < RS_EPS);
+ assert(fabs(-2*(sin(t)+cos(t))+u*cos(t)-cos(phi)+1 - y) < RS_EPS);
+ assert(fabs(mod2pi(t+pi/2+v-phi)) < RS_EPS);
+ return t>=-ZERO && u<=ZERO && v<=ZERO;
+ }
+ return false;
+ }
+ // formula 8.10
+ inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
+ {
+ double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
+ polar(-eta, xi, rho, theta);
+ if (rho >= 2.)
+ {
+ t = theta;
+ u = 2. - rho;
+ v = mod2pi(t + .5*pi - phi);
+ assert(fabs(2*sin(t)-cos(t-v)-u*sin(t) - x) < RS_EPS);
+ assert(fabs(-2*cos(t)-sin(t-v)+u*cos(t)+1 - y) < RS_EPS);
+ assert(fabs(mod2pi(t+pi/2-v-phi)) < RS_EPS);
+ return t>=-ZERO && u<=ZERO && v<=ZERO;
+ }
+ return false;
+ }
+ void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+ {
+ double t, u, v, Lmin = path.length() - .5*pi, L;
+ if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5*pi, u, v);
+ Lmin = L;
+ }
+ if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5*pi, -u, -v);
+ Lmin = L;
+ }
+ if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5*pi, u, v);
+ Lmin = L;
+ }
+ if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5*pi, -u, -v);
+ Lmin = L;
+ }
+
+ if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5*pi, u, v);
+ Lmin = L;
+ }
+ if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5*pi, -u, -v);
+ Lmin = L;
+ }
+ if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5*pi, u, v);
+ Lmin = L;
+ }
+ if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5*pi, -u, -v);
+ Lmin = L;
+ }
+
+ // backwards
+ double xb = x*cos(phi) + y*sin(phi), yb = x*sin(phi) - y*cos(phi);
+ if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5*pi, t);
+ Lmin = L;
+ }
+ if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5*pi, -t);
+ Lmin = L;
+ }
+ if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5*pi, t);
+ Lmin = L;
+ }
+ if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5*pi, -t);
+ Lmin = L;
+ }
+
+ if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5*pi, t);
+ Lmin = L;
+ }
+ if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5*pi, -t);
+ Lmin = L;
+ }
+ if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5*pi, t);
+ Lmin = L;
+ }
+ if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5*pi, -t);
+ }
+ // formula 8.11 *** TYPO IN PAPER ***
+ inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
+ {
+ double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
+ polar(xi, eta, rho, theta);
+ if (rho >= 2.)
+ {
+ u = 4. - sqrt(rho*rho - 4.);
+ if (u <= ZERO)
+ {
+ t = mod2pi(atan2((4-u)*xi -2*eta, -2*xi + (u-4)*eta));
+ v = mod2pi(t - phi);
+ assert(fabs(4*sin(t)-2*cos(t)-u*sin(t)-sin(phi) - x) < RS_EPS);
+ assert(fabs(-4*cos(t)-2*sin(t)+u*cos(t)+cos(phi)+1 - y) < RS_EPS);
+ assert(fabs(mod2pi(t-v-phi)) < RS_EPS);
+ return t>=-ZERO && v>=-ZERO;
+ }
+ }
+ return false;
+ }
+ void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+ {
+ double t, u, v, Lmin = path.length() - pi, L;
+ if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5*pi, u, -.5*pi, v);
+ Lmin = L;
+ }
+ if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5*pi, -u, .5*pi, -v);
+ Lmin = L;
+ }
+ if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // reflect
+ {
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5*pi, u, -.5*pi, v);
+ Lmin = L;
+ }
+ if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v))) // timeflip + reflect
+ path = ReedsSheppStateSpace::ReedsSheppPath(
+ ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5*pi, -u, .5*pi, -v);
+ }
+
+ ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
+ {
+ ReedsSheppStateSpace::ReedsSheppPath path;
+ CSC(x, y, phi, path);
+ CCC(x, y, phi, path);
+ CCCC(x, y, phi, path);
+ CCSC(x, y, phi, path);
+ CCSCC(x, y, phi, path);
+ return path;
+ }
+}
+
+const ReedsSheppStateSpace::ReedsSheppPathSegmentType
+ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
+ { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 0
+ { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP }, // 1
+ { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 2
+ { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP }, // 3
+ { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 4
+ { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 5
+ { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 6
+ { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 7
+ { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP }, // 8
+ { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP }, // 9
+ { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP }, // 10
+ { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP }, // 11
+ { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 12
+ { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 13
+ { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP }, // 14
+ { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP }, // 15
+ { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT }, // 16
+ { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT } // 17
+};
+
+ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType* type,
+ double t, double u, double v, double w, double x)
+ : type_(type)
+{
+ length_[0] = t; length_[1] = u; length_[2] = v; length_[3] = w; length_[4] = x;
+ totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
+}
+
+
+double ReedsSheppStateSpace::distance(double q0[3], double q1[3])
+{
+ return rho_ * reedsShepp(q0, q1).length();
+}
+
+ReedsSheppStateSpace::ReedsSheppPath ReedsSheppStateSpace::reedsShepp(double q0[3], double q1[3])
+{
+ double dx = q1[0] - q0[0], dy = q1[1] - q0[1], dth = q1[2] - q0[2];
+ double c = cos(q0[2]), s = sin(q0[2]);
+ double x = c*dx + s*dy, y = -s*dx + c*dy;
+ return ::reedsShepp(x/rho_, y/rho_, dth);
+}
+
+void ReedsSheppStateSpace::type(double q0[3], double q1[3], ReedsSheppPathTypeCallback cb, void* user_data)
+{
+ ReedsSheppPath path = reedsShepp(q0, q1);
+ for (int i=0;i<5;++i)
+ cb(path.type_[i], user_data);
+ return;
+}
+
+void ReedsSheppStateSpace::sample(double q0[3], double q1[3], double step_size, ReedsSheppPathSamplingCallback cb, void* user_data)
+{
+ ReedsSheppPath path = reedsShepp(q0, q1);
+ double dist = rho_ * path.length();
+
+ for (double seg=0.0; seg<=dist; seg+=step_size){
+ double qnew[5] = {};
+ interpolate(q0, path, seg/rho_, qnew);
+ cb( qnew, user_data);
+ }
+ return;
+}
+
+void ReedsSheppStateSpace::interpolate(double q0[3], ReedsSheppPath &path, double seg, double s[5])
+{
+
+ if (seg < 0.0) seg = 0.0;
+ if (seg > path.length()) seg = path.length();
+
+ double phi, v;
+
+ s[0] = s[1] = 0.0;
+ s[2] = q0[2];
+
+ for (unsigned int i=0; i<5 && seg>0; ++i)
+ {
+ if (path.length_[i]<0)
+ {
+ v = std::max(-seg, path.length_[i]);
+ seg += v;
+ }
+ else
+ {
+ v = std::min(seg, path.length_[i]);
+ seg -= v;
+ }
+ phi = s[2];
+ switch(path.type_[i])
+ {
+ case RS_LEFT:
+ s[0] += ( sin(phi+v) - sin(phi));
+ s[1] += (-cos(phi+v) + cos(phi));
+ s[2] = phi + v;
+ s[4] = 1;
+ break;
+ case RS_RIGHT:
+ s[0] += (-sin(phi-v) + sin(phi));
+ s[1] += ( cos(phi-v) - cos(phi));
+ s[2] = phi - v;
+ s[4] = -1;
+ break;
+ case RS_STRAIGHT:
+ s[0] += (v * cos(phi));
+ s[1] += (v * sin(phi));
+ s[4] = 0;
+ break;
+ case RS_NOP:
+ break;
+ }
+ }
+
+ s[0] = s[0] * rho_ + q0[0];
+ s[1] = s[1] * rho_ + q0[1];
+ s[3] = v;
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.h"
+#include "reeds_shepp.h"
+
+double RRTExt1::cost_build(RRTNode &f, RRTNode &t)
+{
+ double q0[] = {f.x(), f.y(), f.h()};
+ double q1[] = {t.x(), t.y(), t.h()};
+ ReedsSheppStateSpace rsss(this->bc.mtr());
+ return rsss.distance(q0, q1);
+}
+
+double RRTExt1::cost_search(RRTNode &f, RRTNode &t)
+{
+ double cost = 0;
+ cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
+ double heur = std::min(
+ std::abs(t.h() - f.h()),
+ 2 * M_PI - std::abs(t.h() - f.h())
+ );
+ heur *= this->bc.mtr();
+ cost = std::max(cost, heur);
+ return cost;
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+#include "reeds_shepp.h"
+
+namespace rrts {
+
+double
+RRTExt10::cost_build(RRTNode const& f, RRTNode const& t) const
+{
+ double q0[] = {f.x(), f.y(), f.h()};
+ double q1[] = {t.x(), t.y(), t.h()};
+ ReedsSheppStateSpace rsss(this->bc_.mtr());
+ return rsss.distance(q0, q1);
+}
+
+double
+RRTExt10::cost_search(RRTNode const& f, RRTNode const& t) const
+{
+ double cost = f.edist(t);
+ double heur = std::min(std::abs(t.h() - f.h()),
+ 2 * M_PI - std::abs(t.h() - f.h()));
+ heur *= this->bc_.mtr();
+ cost = std::max(cost, heur);
+ return cost + f.cusp() * 0.1;
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.h"
+
+bool RRTExt11::goal_found(RRTNode &f)
+{
+ auto &g = this->goals().front();
+ auto fbc = BicycleCar();
+ fbc.x(f.x());
+ fbc.y(f.y());
+ fbc.h(f.h());
+ auto gbc = BicycleCar();
+ gbc.x(g.x());
+ gbc.y(g.y());
+ gbc.h(g.h());
+ bool drivable = false;
+ if (this->entry_set)
+ drivable = gbc.drivable(fbc, this->entry.b, this->entry.e);
+ else
+ drivable = gbc.drivable(fbc);
+ if (drivable) {
+ this->steer(f, g);
+ if (std::get<0>(this->collide_steered_from(f)))
+ return false;
+ double cost = this->cost_build(f, g);
+ this->join_steered(&f);
+ if (g.p() == nullptr) {
+ g.p(&f);
+ g.c(cost);
+ this->path_cost_before_opt_ = g.cc;
+ } else if (f.cc + cost < g.cc) {
+ g.p(&f);
+ g.c(cost);
+ }
+ return true;
+ }
+ return false;
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.h"
+
+void RRTExt12::steer1(RRTNode &f, RRTNode &t)
+{
+ // assume that `t` is circle uniformly sampled
+ auto bc = BicycleCar();
+ bc.x(f.x());
+ bc.y(f.y());
+ bc.h(f.h());
+ bc.set_max_steer();
+ auto max_steer = bc.st();
+ // map t.h() in -PI...+PI to -max_steer...+max_steer
+ bc.st(2 * max_steer * this->udh_(this->gen_) / (2 * M_PI) - max_steer);
+ bc.sp((this->udy_(this->gen_) > 0.5) ? 0.5 : -0.5);
+ this->steered().clear();
+ while (true) {
+ bc.next();
+ auto n = RRTNode(bc);
+ if (
+ this->steered().size() == 0
+ && std::get<0>(this->collide_two_nodes(f, n))
+ ) {
+ return;
+ } else if (
+ (
+ this->steered().size() > 0
+ && std::get<0>(this->collide_two_nodes(
+ this->steered().back(),
+ n
+ ))
+ )
+ || this->steered().size() > 25
+ ) {
+ break;
+ }
+ this->steered().push_back(RRTNode(bc));
+ }
+ bc.sp(-1*bc.sp());
+ bc.next();
+ bc.sp(-1*bc.sp());
+ this->samples().back().x(bc.x());
+ this->samples().back().y(bc.y());
+ this->samples().back().h(bc.h());
+}
+bool RRTExt12::connect()
+{
+ RRTNode *t = &this->steered().front();
+ RRTNode *f = this->use_nn;
+ double cost = this->cost_search(*f, *t);
+ // steer from f->t and then continue with the steered.
+ this->tmp_steer(*f, *t);
+ if (this->tmp_steered_.size() > 0) {
+ auto col = this->collide_tmp_steered_from(*f);
+ if (std::get<0>(col))
+ return false;
+ this->join_tmp_steered(f);
+ f = &this->nodes().back();
+ }
+ if (sqrt(
+ pow(f->x() - t->x(), 2)
+ + pow(f->y() - t->y(), 2)
+ ) > 0.5)
+ return false;
+ // cont.
+ this->store_node(this->steered().front());
+ t = &this->nodes().back();
+ t->p(f);
+ t->c(this->cost_build(*f, *t));
+ t->set_t(RRTNodeType::connected);
+ return true;
+}
+bool RRTExt12::next()
+{
+ if (this->icnt_ == 0)
+ this->tstart_ = std::chrono::high_resolution_clock::now();
+ bool next = true;
+ if (this->icnt_ > this->log_path_iter_)
+ this->log_path_cost();
+ if (this->should_stop())
+ return false;
+ if (this->samples().size() == 0) {
+ this->samples().push_back(RRTNode());
+ this->samples().back().x(this->goals().front().x());
+ this->samples().back().y(this->goals().front().y());
+ this->samples().back().h(this->goals().front().h());
+ this->use_nn = &this->nodes().front();
+ } else {
+ this->sample();
+ }
+ this->steer1(
+ *this->use_nn,
+ this->samples().back()
+ );
+ if (this->steered().size() == 0)
+ return next;
+ auto col = this->collide_steered_from(*this->use_nn);
+ if (std::get<0>(col)) {
+ auto rcnt = this->steered().size() - std::get<1>(col);
+ while (rcnt-- > 0) {
+ this->steered().pop_back();
+ }
+ }
+ if (!this->connect())
+ return next;
+ this->rewire();
+ unsigned scnt = this->steered().size();
+ this->join_steered(&this->nodes().back());
+ RRTNode *just_added = &this->nodes().back();
+ while (scnt > 0) {
+ // store all the steered1 nodes
+ this->steered1_.push_back(just_added);
+ scnt--;
+ auto &g = this->goals().front();
+ this->steer2(*just_added, g);
+ auto col = this->collide_steered_from(*just_added);
+ if (std::get<0>(col)) {
+ auto rcnt = this->steered().size() - std::get<1>(col);
+ while (rcnt-- > 0) {
+ this->steered().pop_back();
+ }
+ }
+ this->join_steered(just_added);
+ // store all the steered2 nodes
+ RRTNode* jap = &this->nodes().back();
+ while (jap != just_added) {
+ this->steered2_.push_back(jap);
+ jap = jap->p();
+ }
+ this->gf(this->goal_found(this->nodes().back()));
+ just_added = just_added->p();
+ }
+ return next;
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <algorithm>
+#include <queue>
+#include "rrtext.hh"
+
+namespace rrts {
+
+RRTExt13::DijkstraNode::DijkstraNode(RRTNode* n) : node(n)
+{
+}
+
+bool
+RRTExt13::DijkstraNode::vi()
+{
+ if (this->v) {
+ return true;
+ }
+ this->v = true;
+ return false;
+}
+
+int
+RRTExt13::DijkstraNodeComparator::operator() (DijkstraNode const& n1,
+ DijkstraNode const& n2)
+{
+ return n1.node->cc() > n2.node->cc();
+}
+
+int
+RRTExt13::DijkstraNodeBackwardComparator::operator() (DijkstraNode const& n1,
+ DijkstraNode const& n2)
+{
+ return n1.d > n2.d;
+}
+
+void
+RRTExt13::interesting_forward()
+{
+ this->dn_.clear();
+ this->dn_.reserve(this->path_.size());
+#if 0 // all path poses are interesting
+ for (auto n: this->opath_) {
+ this->dn_.push_back(DijkstraNode(n));
+ this->dn_.back().i = this->dn_.size() - 1;
+ }
+ return;
+#endif
+#if 1 // cusp and 1st non-drivable path poses are interesting
+ this->dn_.push_back(DijkstraNode(this->opath_.front()));
+ this->dn_.back().i = this->dn_.size() - 1;
+ for (unsigned int i = 0; i < this->opath_.size() - 1; i++) {
+ RRTNode& ni = *this->opath_[i];
+ this->bc_.set_pose(ni);
+ for (unsigned int j = i + 1; j < this->opath_.size(); j++) {
+ RRTNode& nj = *this->opath_[j];
+ RRTNode& njl = *this->opath_[j - 1];
+ if (!this->bc_.drivable(nj)) {
+ this->dn_.push_back(DijkstraNode(&njl));
+ this->dn_.back().i = this->dn_.size() - 1;
+ i = j;
+ break;
+ }
+ if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+ this->dn_.push_back(DijkstraNode(&njl));
+ this->dn_.back().i = this->dn_.size() - 1;
+ }
+ }
+ }
+ this->dn_.push_back(DijkstraNode(this->opath_.back()));
+ this->dn_.back().i = this->dn_.size() - 1;
+#endif
+}
+
+void
+RRTExt13::interesting_backward()
+{
+ this->dn_.clear();
+ this->dn_.reserve(this->path_.size());
+#if 0 // all path poses are interesting
+ for (auto n: this->opath_) {
+ this->dn_.push_back(DijkstraNode(n));
+ this->dn_.back().i = this->dn_.size() - 1;
+ }
+ std::reverse(this->dn_.begin(), this->dn_.end());
+ return;
+#endif
+#if 1 // cusp and 1st non-drivable path poses are interesting
+ double goal_cc = this->goal_.cc();
+ this->dn_.push_back(DijkstraNode(this->opath_.back()));
+ this->dn_.back().i = this->dn_.size() - 1;
+ this->dn_.back().d = goal_cc - this->opath_.back()->cc();
+ for (unsigned int i = this->opath_.size() - 1; i > 1; i--) {
+ RRTNode& ni = *this->opath_[i];
+ this->bc_.set_pose(ni);
+ for (unsigned int j = i - 1; j > 0; j--) {
+ RRTNode& nj = *this->opath_[j];
+ RRTNode& njl = *this->opath_[j + 1];
+ if (!this->bc_.drivable(nj)) {
+ this->dn_.push_back(DijkstraNode(&njl));
+ this->dn_.back().i = this->dn_.size() - 1;
+ this->dn_.back().d = goal_cc - njl.cc();
+ i = j;
+ break;
+ }
+ if (njl.sp() == 0.0 || sgn(njl.sp()) != sgn(nj.sp())) {
+ this->dn_.push_back(DijkstraNode(&njl));
+ this->dn_.back().i = this->dn_.size() - 1;
+ this->dn_.back().d = goal_cc - njl.cc();
+ }
+ }
+ }
+ this->dn_.push_back(DijkstraNode(this->opath_.front()));
+ this->dn_.back().i = this->dn_.size() - 1;
+ this->dn_.back().d = goal_cc - this->opath_.front()->cc();
+#endif
+}
+
+void
+RRTExt13::dijkstra_forward()
+{
+ std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
+ DijkstraNodeComparator> pq;
+ this->dn_.front().vi();
+ pq.push(this->dn_.front());
+ while (!pq.empty()) {
+ DijkstraNode fd = pq.top();
+ RRTNode& f = *fd.node;
+ pq.pop();
+ this->bc_.set_pose(f);
+ for (unsigned int i = fd.i + 1; i < this->dn_.size(); i++) {
+ RRTNode& t = *this->dn_[i].node;
+ if (!this->bc_.drivable(t)) {
+ continue;
+ }
+ double cost = f.cc() + this->cost_build(f, t);
+ this->steer(f, t);
+ if (this->steered_.size() == 0) {
+ break;
+ }
+ unsigned int ss = this->steered_.size();
+ if (this->collide_steered()
+ || ss != this->steered_.size()) {
+ continue;
+ }
+ this->bc_.set_pose(this->steered_.back());
+ bool td = this->bc_.drivable(t);
+ bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
+ if (cost < t.cc() && td && tn) {
+ this->join_steered(&f);
+ t.p(this->nodes_.back());
+ t.c(this->cost_build(this->nodes_.back(), t));
+ if (!this->dn_[i].vi()) {
+ pq.push(this->dn_[i]);
+ }
+ }
+ }
+ }
+}
+
+void
+RRTExt13::dijkstra_backward()
+{
+ std::priority_queue<DijkstraNode, std::vector<DijkstraNode>,
+ DijkstraNodeComparator> pq;
+ this->dn_.front().vi();
+ pq.push(this->dn_.front());
+ while (!pq.empty()) {
+ DijkstraNode td = pq.top();
+ RRTNode& t = *td.node;
+ pq.pop();
+ this->bc_.set_pose(t);
+ for (unsigned int i = td.i; i > 0; i--) {
+ DijkstraNode& fd = this->dn_[i];
+ RRTNode& f = *this->dn_[i].node;
+ if (!this->bc_.drivable(f)) {
+ continue;
+ }
+ double cost = td.d + this->cost_build(f, t);
+ this->steer(f, t);
+ if (this->steered_.size() == 0) {
+ break;
+ }
+ unsigned int ss = this->steered_.size();
+ if (this->collide_steered()
+ || ss != this->steered_.size()) {
+ continue;
+ }
+ this->bc_.set_pose(this->steered_.back());
+ bool td = this->bc_.drivable(t);
+ bool tn = this->bc_.edist(t) < 2.0 * this->eta_;
+ if (cost < fd.d && td && tn) {
+ fd.d = cost;
+ this->join_steered(&f);
+ t.p(this->nodes_.back());
+ t.c(this->cost_build(this->nodes_.back(), t));
+ this->recompute_cc(&t);
+ if (!this->dn_[i].vi()) {
+ pq.push(this->dn_[i]);
+ }
+ }
+ }
+ }
+ this->recompute_path_cc();
+}
+
+void
+RRTExt13::compute_path()
+{
+ RRTS::compute_path();
+ if (this->goal_.cc() == 0.0 || this->path_.size() == 0) {
+ return;
+ }
+#if 1 // TODO 0.59 should work for sc4-1-0 only.
+ if (this->goal_.cc() * 0.8 > this->last_goal_cc_
+ && this->last_goal_cc_ != 0.0) {
+ return;
+ }
+#endif
+ bool measure_time = false;
+ if (this->opath_.size() == 0) {
+ this->opath_ = this->path_;
+ this->ogoal_cc_ = this->goal_.cc();
+ measure_time = true;
+ }
+ if (measure_time) {
+ this->otime_ = -this->ter_.scnt();
+ }
+ double curr_cc = this->goal_.cc();
+ double last_cc = curr_cc + 1.0;
+ while (curr_cc < last_cc) {
+ last_cc = curr_cc;
+ RRTS::compute_path();
+ this->interesting_forward();
+ this->dijkstra_forward();
+ RRTS::compute_path();
+ this->interesting_backward();
+ this->dijkstra_backward();
+ RRTS::compute_path();
+ curr_cc = this->goal_.cc();
+ }
+ if (measure_time) {
+ this->otime_ += this->ter_.scnt();
+ }
+}
+
+RRTExt13::RRTExt13()
+{
+ this->opath_.reserve(10000);
+ this->dn_.reserve(10000);
+}
+
+Json::Value
+RRTExt13::json() const
+{
+ auto jvo = RRTS::json();
+ unsigned int i = 0;
+ for (auto n: this->opath_) {
+ jvo["opath"][i][0] = n->x();
+ jvo["opath"][i][1] = n->y();
+ jvo["opath"][i][2] = n->h();
+ jvo["opath"][i][3] = n->sp();
+ i++;
+ }
+ jvo["ogoal_cc"] = this->ogoal_cc_;
+ jvo["otime"] = this->otime_;
+ return jvo;
+}
+
+void
+RRTExt13::json(Json::Value jvi)
+{
+ RRTS::json(jvi);
+}
+
+void
+RRTExt13::reset()
+{
+ RRTS::reset();
+ this->opath_.clear();
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+
+namespace rrts {
+
+RRTNode
+RRTExt14::sample()
+{
+ if (this->circle_r_ == 0.0) {
+ RRTNode& f = this->nodes_.front();
+ RRTNode& g = this->goal_;
+ double dx = g.x() - f.x();
+ double dy = g.y() - f.y();
+ this->circle_r_ = sqrt(dx * dx + dy * dy);
+ this->circle_c_.x((f.x() + g.x()) / 2.0);
+ this->circle_c_.y((f.y() + g.y()) / 2.0);
+ return this->goal_;
+ }
+ double r = this->circle_r_ * sqrt(this->udr_(this->gen_));
+ double theta = this->udt_(this->gen_);
+ RRTNode rs;
+ rs.x(this->circle_c_.x() + r * cos(theta));
+ rs.y(this->circle_c_.y() + r * sin(theta));
+ rs.h(this->udh_(this->gen_));
+ return rs;
+}
+
+RRTExt14::RRTExt14() : RRTS()
+{
+ this->udr_ = std::uniform_real_distribution<double>(0.0, 1.0);
+ this->udt_ = std::uniform_real_distribution<double>(0.0, 2.0 * M_PI);
+ this->udh_ = std::uniform_real_distribution<double>(0.0, 2.0 * M_PI);
+}
+
+void
+RRTExt14::reset()
+{
+ RRTS::reset();
+ this->circle_r_ = 0.0;
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+
+namespace rrts {
+
+Json::Value
+RRTExt15::json() const
+{
+ auto jvo = RRTS::json();
+ unsigned int i = 0;
+ for (auto c: this->log_path_cost_) {
+ jvo["log_path_cost"][i++] = c;
+ }
+ return jvo;
+}
+
+void
+RRTExt15::json(Json::Value jvi)
+{
+ RRTS::json(jvi);
+}
+
+bool
+RRTExt15::next()
+{
+ bool c = RRTS::next();
+ if (this->log_path_cost_.size() == 0) {
+ this->log_path_cost_.push_back(this->goal_.cc());
+ } else {
+ double lc = this->log_path_cost_.back();
+ double gc = this->goal_.cc();
+ if (gc > 0.0 && (lc == 0.0 || gc < lc)) {
+ this->log_path_cost_.push_back(gc);
+ } else {
+ this->log_path_cost_.push_back(lc);
+ }
+ }
+ return c;
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "reeds_shepp.h"
+#include "rrtext.hh"
+
+namespace rrts {
+
+static int
+cb_steer(double q[4], void *w)
+{
+ std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
+ st->push_back(RRTNode());
+ st->back().x(q[0]);
+ st->back().y(q[1]);
+ st->back().h(q[2]);
+ st->back().sp(q[3]);
+ st->back().segment_type = q[4];
+ return 0;
+}
+
+void
+RRTExt16::steer(RRTNode const& f, RRTNode const& t)
+{
+ this->steered_.clear();
+ double q0[] = {f.x(), f.y(), f.h()};
+ double q1[] = {t.x(), t.y(), t.h()};
+ ReedsSheppStateSpace rsss(this->bc_.mtr());
+ rsss.sample(q0, q1, this->eta_, cb_steer, &this->steered_);
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+
+namespace rrts {
+
+bool
+RRTExt17::should_finish() const
+{
+ return this->goal_.p() != nullptr || this->icnt_ > this->_imax;
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+
+namespace rrts {
+
+bool
+RRTExt18::should_finish() const
+{
+ return this->icnt_ > this->_imax;
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2022 Jiri Vlasak
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+#include "dubins.h"
+
+namespace rrts {
+
+static int
+cb_steer(double q[3], double t, void *w)
+{
+ std::vector<RRTNode>* st = (std::vector<RRTNode>*) w;
+ st->push_back(RRTNode());
+ st->back().x(q[0]);
+ st->back().y(q[1]);
+ st->back().h(q[2]);
+ st->back().sp(1.0);
+ return 0;
+}
+
+void
+RRTExt19::steer(RRTNode const &f, RRTNode const &t)
+{
+ this->steered_.clear();
+ double q0[] = {f.x(), f.y(), f.h()};
+ double q1[] = {t.x(), t.y(), t.h()};
+ DubinsPath path;
+ int r = dubins_shortest_path(&path, q0, q1, this->bc_.mtr());
+ if (r != 0) {
+ return;
+ }
+ dubins_path_sample_many(&path, this->eta_, cb_steer, &this->steered_);
+ if (this->steered_.size() > 0) {
+ this->steered_.front().sp(0.0);
+ }
+}
+
+} /* namespace rrts */
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <cassert>
+#include "rrtext.hh"
+#define CUTE_C2_IMPLEMENTATION
+#include "cute_c2.h"
+
+namespace rrts {
+
+bool
+RRTExt2::collide(RRTNode const& n)
+{
+ this->c2x_bc_.p.x = n.x();
+ this->c2x_bc_.p.y = n.y();
+ this->c2x_bc_.r.c = cos(n.h());
+ this->c2x_bc_.r.s = sin(n.h());
+ for (auto& o: this->c2_obstacles_) {
+ if (c2PolytoPoly(&this->c2_bc_, &this->c2x_bc_, &o, NULL)) {
+ return true;
+ }
+ }
+ return false;
+}
+
+bool
+RRTExt2::collide_steered()
+{
+ unsigned int i = 0;
+ for (auto& n: this->steered_) {
+ if (this->collide(n)) {
+ break;
+ }
+ i++;
+ }
+ this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
+ return this->steered_.size() == 0;
+}
+
+RRTExt2::RRTExt2() : RRTS()
+{
+ this->c2_bc_.count = 4;
+ this->c2_bc_.verts[0].x = this->bc_.lfx();
+ this->c2_bc_.verts[0].y = this->bc_.lfy();
+ this->c2_bc_.verts[1].x = this->bc_.lrx();
+ this->c2_bc_.verts[1].y = this->bc_.lry();
+ this->c2_bc_.verts[2].x = this->bc_.rrx();
+ this->c2_bc_.verts[2].y = this->bc_.rry();
+ this->c2_bc_.verts[3].x = this->bc_.rfx();
+ this->c2_bc_.verts[3].y = this->bc_.rfy();
+}
+
+void
+RRTExt2::reset()
+{
+ RRTS::reset();
+ this->c2_bc_.count = 4;
+ this->c2_bc_.verts[0].x = this->bc_.lfx();
+ this->c2_bc_.verts[0].y = this->bc_.lfy();
+ this->c2_bc_.verts[1].x = this->bc_.lrx();
+ this->c2_bc_.verts[1].y = this->bc_.lry();
+ this->c2_bc_.verts[2].x = this->bc_.rrx();
+ this->c2_bc_.verts[2].y = this->bc_.rry();
+ this->c2_bc_.verts[3].x = this->bc_.rfx();
+ this->c2_bc_.verts[3].y = this->bc_.rfy();
+}
+
+Json::Value
+RRTExt2::json() const
+{
+ return RRTS::json();
+}
+
+void
+RRTExt2::json(Json::Value jvi)
+{
+ RRTS::json(jvi);
+ assert(jvi["obst"] != Json::nullValue);
+ for (auto& o: jvi["obst"]) {
+ assert(o.size() < C2_MAX_POLYGON_VERTS);
+ c2Poly c2tmp;
+ c2tmp.count = o.size();
+ unsigned int i = 0;
+ for (auto& c: o) {
+ c2tmp.verts[i].x = c[0].asDouble();
+ c2tmp.verts[i].y = c[1].asDouble();
+ i++;
+ }
+ this->c2_obstacles_.push_back(c2tmp);
+ }
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2022 Jiri Vlasak
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <cassert>
+#include "rrtext.hh"
+
+namespace rrts {
+
+bool
+RRTExt20::collide_steered()
+{
+ assert(this->_points_to_check != nullptr);
+ std::vector<bcar::Point> points_to_check(*this->_points_to_check);
+ unsigned int i = 0;
+ for (auto &n: this->steered_) {
+ for (auto &p: points_to_check) {
+ Point const n_p(n);
+ CarSize const n_cs(this->bc());
+ if (p.inside_of(n_p, n_cs.edist_to_rr())) {
+ break;
+ }
+ if (p.edist(n_p) < n_cs.edist_to_lf()
+ && p.edist(n_p) > n_cs.edist_to_rr()) {
+ BicycleCar n_bc(this->bc());
+ n_bc.x(n.x());
+ n_bc.y(n.y());
+ n_bc.h(n.h());
+ std::vector<Point> poly;
+ poly.push_back(n_bc.lf());
+ poly.push_back(n_bc.lr());
+ poly.push_back(n_bc.rr());
+ poly.push_back(n_bc.rf());
+ poly.push_back(n_bc.lf());
+ if (p.inside_of(poly)) {
+ break;
+ }
+
+ }
+ }
+ i++;
+ }
+ this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
+ return this->steered_.size() == 0;
+}
+
+void
+RRTExt20::set_points_to_check(std::vector<bcar::Point> const *p)
+{
+ this->_points_to_check = p;
+}
+
+} /* namespace rrts */
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2022 Jiri Vlasak
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <cassert>
+#include <cmath>
+#include "rrtext.hh"
+
+namespace rrts {
+
+bool
+RRTExt21::collide(RRTNode const &n)
+{
+ int col = floor((n.x() - this->_origin_x) / this->_grid_res);
+ int row = floor((n.y() - this->_origin_y) / this->_grid_res);
+ if (col < 0 || row < 0 || col >= this->_grid_width
+ || row >= this->_grid_height) {
+ // grid is too small or sampling too far
+ return true;
+ }
+ if (this->_grid_data[row * this->_grid_width + col] > 65) {
+ return true;
+ }
+ return false;
+}
+
+bool
+RRTExt21::collide_steered()
+{
+ unsigned int i = 0;
+ for (auto &n: this->steered_) {
+ if (this->collide(n)) {
+ break;
+ }
+ i++;
+ }
+ this->steered_.erase(this->steered_.begin() + i, this->steered_.end());
+ return this->steered_.size() == 0;
+}
+
+void
+RRTExt21::set_grid_to_check(unsigned int w, unsigned int h, float r,
+ int8_t const *d, double x, double y)
+{
+ this->_grid_width = w;
+ this->_grid_height = h;
+ this->_grid_res = r;
+ this->_grid_data = d;
+ this->_origin_x = x;
+ this->_origin_y = y;
+}
+
+} /* namespace rrts */
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <queue>
+#include "rrtext.h"
+
+void RRTExt3::reset()
+{
+ RRTS::reset();
+ this->orig_path().clear();
+ this->first_optimized_path().clear();
+ this->orig_path_cost_ = 9999.9;
+ this->first_optimized_path_cost_ = 9999.9;
+}
+
+void RRTExt3::first_path_optimization()
+{
+ if (this->orig_path().size() == 0) {
+ this->orig_path_ = RRTS::path();
+ if (this->orig_path().size() == 0)
+ return;
+ else
+ this->orig_path_cost(this->orig_path().back()->cc);
+ }
+ class DijkstraNode : public RRTNode {
+ public:
+ DijkstraNode *s = nullptr;
+ RRTNode *n = nullptr;
+ unsigned int i = 0;
+ double cc = 9999;
+ bool v = false;
+ bool vi()
+ {
+ if (this->v)
+ return true;
+ this->v = true;
+ return false;
+ }
+ DijkstraNode(const RRTNode& n) {
+ this->x(n.x());
+ this->y(n.y());
+ this->h(n.h());
+ this->sp(n.sp());
+ this->st(n.st());
+ }
+ // override
+ DijkstraNode *p_ = nullptr;
+ DijkstraNode *p() const { return this->p_; }
+ void p(DijkstraNode *p) { this->p_ = p; }
+ };
+ class DijkstraNodeComparator {
+ public:
+ int operator() (
+ const DijkstraNode &n1,
+ const DijkstraNode &n2
+ )
+ {
+ return n1.cc > n2.cc;
+ }
+ };
+ std::vector<DijkstraNode> dn;
+ dn.reserve(this->orig_path().size());
+ unsigned int dncnt = 0;
+ for (auto n: this->orig_path()) {
+ if (
+ n->t(RRTNodeType::cusp)
+ || n->t(RRTNodeType::connected)
+ ) {
+ dn.push_back(DijkstraNode(*n));
+ dn.back().cc = n->cc;
+ dn.back().s = &dn.back();
+ dn.back().n = n;
+ dn.back().i = dncnt++;
+ }
+ }
+ dn.push_back(DijkstraNode(*this->orig_path().back()));
+ dn.back().cc = this->orig_path().back()->cc;
+ dn.back().s = &dn.back();
+ dn.back().n = this->orig_path().back();
+ dn.back().i = dncnt++;
+ std::priority_queue<
+ DijkstraNode,
+ std::vector<DijkstraNode>,
+ DijkstraNodeComparator
+ > pq;
+ dn.front().vi();
+ pq.push(dn.front());
+ while (!pq.empty()) {
+ DijkstraNode f = pq.top();
+ pq.pop();
+ for (unsigned int i = f.i + 1; i < dncnt; i++) {
+ double cost = f.cc + this->cost_search(f, dn[i]);
+ this->steer(f, dn[i]);
+ if (this->steered().size() == 0)
+ break; // TODO why not continue?
+ if (std::get<0>(this->collide_steered_from(f)))
+ continue;
+ if (cost < dn[i].cc) {
+ dn[i].cc = cost;
+ dn[i].p(f.s);
+ if (!dn[i].vi())
+ pq.push(dn[i]);
+ }
+ }
+ }
+ DijkstraNode *ln = nullptr;
+ for (auto &n: dn) {
+ if (n.v)
+ ln = &n;
+ }
+ if (ln == nullptr || ln->p() == nullptr)
+ return;
+ while (ln->p() != nullptr) {
+ RRTNode *t = ln->n;
+ RRTNode *f = ln->p()->n;
+ this->steer(*f, *t);
+ if (std::get<0>(this->collide_steered_from(*f)))
+ return;
+ this->join_steered(f);
+ t->p(&this->nodes().back());
+ t->c(this->cost_build(this->nodes().back(), *t));
+ ln = ln->p();
+ }
+ RRTS::compute_path();
+}
+
+void RRTExt3::second_path_optimization()
+{
+ if (this->first_optimized_path().size() == 0) {
+ return;
+ }
+ class DijkstraNode : public RRTNode {
+ public:
+ DijkstraNode *s = nullptr;
+ RRTNode *n = nullptr;
+ unsigned int i = 0;
+ double cc = 9999;
+ bool v = false;
+ bool vi()
+ {
+ if (this->v)
+ return true;
+ this->v = true;
+ return false;
+ }
+ DijkstraNode(const RRTNode& n) {
+ this->x(n.x());
+ this->y(n.y());
+ this->h(n.h());
+ this->sp(n.sp());
+ this->st(n.st());
+ }
+ // override
+ DijkstraNode *p_ = nullptr;
+ DijkstraNode *p() const { return this->p_; }
+ void p(DijkstraNode *p) { this->p_ = p; }
+ };
+ class DijkstraNodeComparator {
+ public:
+ int operator() (
+ const DijkstraNode &n1,
+ const DijkstraNode &n2
+ )
+ {
+ return n1.cc > n2.cc;
+ }
+ };
+ std::vector<DijkstraNode> dn;
+ dn.reserve(this->orig_path().size());
+ unsigned int dncnt = 0;
+ for (auto n: this->orig_path()) {
+ if (
+ n->t(RRTNodeType::cusp)
+ || n->t(RRTNodeType::connected)
+ ) {
+ dn.push_back(DijkstraNode(*n));
+ dn.back().cc = n->cc;
+ dn.back().s = &dn.back();
+ dn.back().n = n;
+ dn.back().i = dncnt++;
+ }
+ }
+ dn.push_back(DijkstraNode(*this->orig_path().back()));
+ dn.back().cc = this->orig_path().back()->cc;
+ dn.back().s = &dn.back();
+ dn.back().n = this->orig_path().back();
+ dn.back().i = dncnt++;
+ std::priority_queue<
+ DijkstraNode,
+ std::vector<DijkstraNode>,
+ DijkstraNodeComparator
+ > pq;
+ // TODO rewrite
+ dn.back().vi();
+ pq.push(dn.back());
+ while (!pq.empty()) {
+ DijkstraNode t = pq.top();
+ pq.pop();
+ for (unsigned int i = t.i - 1; i > 0; i--) {
+ double cost = dn[i].cc + this->cost_search(dn[i], t);
+ this->steer(dn[i], t);
+ if (this->steered().size() == 0)
+ break; // TODO why not continue?
+ if (std::get<0>(this->collide_steered_from(dn[i])))
+ continue;
+ if (cost < t.cc) {
+ t.cc = cost;
+ t.p(dn[i].s);
+ if (!dn[i].vi())
+ pq.push(dn[i]);
+ }
+ }
+ }
+ DijkstraNode *fn = nullptr;
+ for (auto &n: dn) {
+ if (n.v) {
+ fn = &n;
+ break;
+ }
+ }
+ if (fn == nullptr || fn->p() == nullptr)
+ return;
+ DijkstraNode *ln = &dn.back();
+ while (ln->p() != fn) {
+ RRTNode *t = ln->n;
+ RRTNode *f = ln->p()->n;
+ this->steer(*f, *t);
+ if (std::get<0>(this->collide_steered_from(*f)))
+ return;
+ this->join_steered(f);
+ t->p(&this->nodes().back());
+ t->c(this->cost_build(this->nodes().back(), *t));
+ ln = ln->p();
+ }
+ RRTS::compute_path();
+}
+
+void RRTExt3::compute_path()
+{
+ RRTS::compute_path();
+ auto tstart = std::chrono::high_resolution_clock::now();
+ this->first_path_optimization();
+ this->first_optimized_path_ = RRTS::path();
+ if (this->first_optimized_path_.size() > 0) {
+ this->first_optimized_path_cost(
+ this->first_optimized_path_.back()->cc
+ );
+ } else {
+ return;
+ }
+ this->second_path_optimization();
+ auto tend = std::chrono::high_resolution_clock::now();
+ auto tdiff = std::chrono::duration_cast<std::chrono::duration<double>>(
+ tend - tstart);
+ this->log_opt_time_.push_back(tdiff.count());
+}
+
+Json::Value RRTExt3::json()
+{
+ Json::Value jvo = RRTS::json();
+ jvo["orig_path_cost"] = this->orig_path_cost();
+ {
+ unsigned int cu = 0;
+ unsigned int co = 0;
+ unsigned int pcnt = 0;
+ for (auto n: this->orig_path()) {
+ jvo["orig_path"][pcnt][0] = n->x();
+ jvo["orig_path"][pcnt][1] = n->y();
+ jvo["orig_path"][pcnt][2] = n->h();
+ if (n->t(RRTNodeType::cusp))
+ cu++;
+ if (n->t(RRTNodeType::connected))
+ co++;
+ pcnt++;
+ }
+ jvo["orig_cusps-in-path"] = cu;
+ jvo["orig_connecteds-in-path"] = co;
+ }
+ return jvo;
+}
+
+void RRTExt3::json(Json::Value jvi)
+{
+ return RRTS::json(jvi);
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.h"
+
+void RRTExt4::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p)
+{
+ double cost = p->cost_search(**nn, *t);
+ for (auto f: this->nodes()) {
+ if (p->cost_search(*f, *t) < cost) {
+ *nn = f;
+ cost = p->cost_search(*f, *t);
+ }
+ }
+}
+
+void RRTExt4::Cell::store_node(RRTNode *n)
+{
+ this->nodes_.push_back(n);
+ this->changed_ = true;
+}
+
+RRTExt4::Cell::Cell()
+{
+}
+
+unsigned int RRTExt4::xi(RRTNode n)
+{
+ if (n.x() >= this->x_max_)
+ return GRID_MAX_XI - 1;
+ if (n.x() <= this->x_min_)
+ return 0;
+ return (unsigned int) (floor(n.x() - this->x_min_) / GRID);
+}
+
+unsigned int RRTExt4::yi(RRTNode n)
+{
+ if (n.y() > this->y_max_)
+ return GRID_MAX_YI - 1;
+ if (n.y() <= this->y_min_)
+ return 0;
+ return (unsigned int) (floor(n.y() - this->y_min_) / GRID);
+}
+
+// API
+void RRTExt4::init()
+{
+ this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2;
+ this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2;
+ this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2;
+ this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2;
+}
+
+void RRTExt4::deinit()
+{
+ for (unsigned int i = 0; i < GRID_MAX_XI; i++)
+ for (unsigned int j = 0; j < GRID_MAX_YI; j++)
+ this->grid_[i][j].nodes().clear();
+}
+
+void RRTExt4::store_node(RRTNode n)
+{
+ RRTS::store_node(n);
+ RRTNode *sn = &this->nodes().back();
+ this->grid_[this->xi(n)][this->yi(n)].store_node(sn);
+}
+
+RRTNode *RRTExt4::nn(RRTNode &t)
+{
+ RRTNode *nn = &this->nodes().front();
+ unsigned int txi = this->xi(t);
+ unsigned int tyi = this->yi(t);
+ unsigned int l = 0;
+ while (this->cost_search(*nn, t) > l * GRID) {
+ int xi_min = txi - l;
+ if (xi_min < 0)
+ xi_min = 0;
+ int xi_max = txi + l;
+ if (xi_max > GRID_MAX_XI - 1)
+ xi_max = GRID_MAX_XI - 1;
+ int yi_min = tyi - l;
+ if (yi_min < 0)
+ yi_min = 0;
+ int yi_max = tyi + l;
+ if (yi_max > GRID_MAX_YI - 1)
+ yi_max = GRID_MAX_YI - 1;
+ for (int xi = xi_min; xi <= xi_max; xi++) {
+ this->grid_[xi][yi_max].nn(&t, &nn, this);
+ }
+ for (int xi = xi_min; xi <= xi_max; xi++) {
+ this->grid_[xi][yi_min].nn(&t, &nn, this);
+ }
+ for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+ this->grid_[xi_min][yi].nn(&t, &nn, this);
+ }
+ for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+ this->grid_[xi_max][yi].nn(&t, &nn, this);
+ }
+ l++;
+ }
+ return nn;
+}
+
+std::vector<RRTNode *> RRTExt4::nv(RRTNode &t)
+{
+ std::vector<RRTNode *> nv;
+ unsigned int txi = this->xi(t);
+ unsigned int tyi = this->yi(t);
+ unsigned int l = 0;
+ double cost = std::min(GAMMA(this->nodes().size()), ETA);
+ for (auto f: this->grid_[txi][tyi].nodes())
+ if (this->cost_search(*f, t) < cost)
+ nv.push_back(f);
+ return nv;
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.h"
+#include "reeds_shepp.h"
+
+double RRTExt5::cost_build(RRTNode &f, RRTNode &t)
+{
+ double q0[] = {f.x(), f.y(), f.h()};
+ double q1[] = {t.x(), t.y(), t.h()};
+ ReedsSheppStateSpace rsss(this->bc.mtr());
+ return rsss.distance(q0, q1);
+}
+
+double RRTExt5::cost_search(RRTNode &f, RRTNode &t)
+{
+ double cost = 0;
+ cost = sqrt(pow(t.y() - f.y(), 2) + pow(t.x() - f.x(), 2));
+ return cost;
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+#include "reeds_shepp.h"
+
+namespace rrts {
+
+double
+RRTExt6::cost_build(RRTNode const& f, RRTNode const& t) const
+{
+ double q0[] = {f.x(), f.y(), f.h()};
+ double q1[] = {t.x(), t.y(), t.h()};
+ ReedsSheppStateSpace rsss(this->bc_.mtr());
+ return rsss.distance(q0, q1);
+}
+
+double
+RRTExt6::cost_search(RRTNode const& f, RRTNode const& t) const
+{
+ return this->cost_build(f, t);
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.h"
+
+RRTExt7::KdNode::KdNode(RRTNode *n)
+ : node_(n)
+ , left_(nullptr)
+ , right_(nullptr)
+{
+}
+
+void RRTExt7::delete_kd_nodes(KdNode *n)
+{
+ if (!n)
+ return;
+ if (n->left() != nullptr)
+ delete_kd_nodes(n->left());
+ if (n->right() != nullptr)
+ delete_kd_nodes(n->right());
+ delete n;
+}
+
+void RRTExt7::store_node(RRTNode *n, KdNode *&r, int l)
+{
+ if (r == nullptr)
+ r = new KdNode(n);
+ else if (l % 2 == 0 && n->x() < r->node()->x())
+ store_node(n, r->left(), l + 1);
+ else if (l % 2 == 0)
+ store_node(n, r->right(), l + 1);
+ else if (l % 2 == 1 && n->y() < r->node()->y())
+ store_node(n, r->left(), l + 1);
+ else
+ store_node(n, r->right(), l + 1);
+}
+
+void RRTExt7::nn(RRTNode *&n, RRTNode &t, KdNode *r, int l, double &d)
+{
+ if (r == nullptr)
+ return;
+ if (this->cost_search(*r->node(), t) < d) {
+ n = r->node();
+ d = this->cost_search(*r->node(), t);
+ }
+ if (l % 2 == 0 && t.x() < r->node()->x()) {
+ nn(n, t, r->left(), l + 1, d);
+ if (r->node()->x() - t.x() < d)
+ nn(n, t, r->right(), l + 1, d);
+ } else if (l % 2 == 0) {
+ nn(n, t, r->right(), l + 1, d);
+ if (t.x() - r->node()->x() < d)
+ nn(n, t, r->left(), l + 1, d);
+ } else if (l % 2 == 1 && t.y() < r->node()->y()) {
+ nn(n, t, r->left(), l + 1, d);
+ if (r->node()->y() - t.y() < d)
+ nn(n, t, r->right(), l + 1, d);
+ } else {
+ nn(n, t, r->right(), l + 1, d);
+ if (t.y() - r->node()->y() < d)
+ nn(n, t, r->left(), l + 1, d);
+ }
+}
+
+// API
+void RRTExt7::init()
+{
+}
+
+void RRTExt7::deinit()
+{
+ this->delete_kd_nodes(this->kdroot_);
+}
+
+void RRTExt7::store_node(RRTNode n)
+{
+ RRTS::store_node(n);
+ RRTNode *sn = &this->nodes().back();
+ this->store_node(sn, this->kdroot_, 0);
+}
+
+RRTNode *RRTExt7::nn(RRTNode &t)
+{
+ RRTNode *n = &this->nodes().front();
+ double d = 9999;
+ this->nn(n, t, this->kdroot_, 0, d);
+ return n;
+}
+
+std::vector<RRTNode *> RRTExt7::nv(RRTNode &t)
+{
+ std::vector<RRTNode *> nv;
+ return nv;
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.hh"
+
+namespace rrts {
+
+RRTExt8::KdNode::KdNode(RRTNode* n) : node(n)
+{
+}
+
+void
+RRTExt8::store(RRTNode* n, KdNode*& ref, unsigned int lvl)
+{
+ if (ref == nullptr) {
+ this->kdnodes_.push_back(KdNode(n));
+ ref = &this->kdnodes_.back();
+ } else if (lvl % 3 == 0 && n->x() < ref->node->x()) {
+ this->store(n, ref->left, lvl + 1);
+ } else if (lvl % 3 == 0) {
+ this->store(n, ref->right, lvl + 1);
+ } else if (lvl % 3 == 1 && n->y() < ref->node->y()) {
+ this->store(n, ref->left, lvl + 1);
+ } else if (lvl % 3 == 1) {
+ this->store(n, ref->right, lvl + 1);
+ } else if (lvl % 3 == 2 && n->h() < ref->node->h()) {
+ this->store(n, ref->left, lvl + 1);
+ } else {
+ this->store(n, ref->right, lvl + 1);
+ }
+}
+
+void
+RRTExt8::find_nn(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
+{
+ if (ref == nullptr) {
+ return;
+ }
+ if (this->cost_search(*ref->node, t) < this->cost_
+ || this->nn_ == nullptr) {
+ this->nn_ = ref->node;
+ this->cost_ = this->cost_search(*ref->node, t);
+ }
+ if (lvl % 3 == 0 && t.x() < ref->node->x()) {
+ this->find_nn(t, ref->left, lvl + 1);
+ if (ref->node->x() - t.x() < this->cost_) {
+ this->find_nn(t, ref->right, lvl + 1);
+ }
+ } else if (lvl % 3 == 0) {
+ this->find_nn(t, ref->right, lvl + 1);
+ if (t.x() - ref->node->x() < this->cost_) {
+ this->find_nn(t, ref->left, lvl + 1);
+ }
+ } else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
+ this->find_nn(t, ref->left, lvl + 1);
+ if (ref->node->y() - t.y() < this->cost_) {
+ this->find_nn(t, ref->right, lvl + 1);
+ }
+ } else if (lvl % 3 == 1) {
+ this->find_nn(t, ref->right, lvl + 1);
+ if (t.y() - ref->node->y() < this->cost_) {
+ this->find_nn(t, ref->left, lvl + 1);
+ }
+ } else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
+ this->find_nn(t, ref->left, lvl + 1);
+ if (ref->node->h() - t.h() < this->cost_) {
+ this->find_nn(t, ref->right, lvl + 1);
+ }
+ } else {
+ this->find_nn(t, ref->right, lvl + 1);
+ if (t.h() - ref->node->h() < this->cost_) {
+ this->find_nn(t, ref->left, lvl + 1);
+ }
+ }
+}
+
+void
+RRTExt8::find_nv(RRTNode const& t, KdNode const* const ref, unsigned int lvl)
+{
+ if (ref == nullptr) {
+ return;
+ }
+ if (this->cost_search(*ref->node, t) < this->cost_) {
+ this->nv_.push_back(ref->node);
+ }
+ if (lvl % 3 == 0 && t.x() < ref->node->x()) {
+ this->find_nv(t, ref->left, lvl + 1);
+ if (ref->node->x() - t.x() < this->cost_) {
+ this->find_nv(t, ref->right, lvl + 1);
+ }
+ } else if (lvl % 3 == 0) {
+ this->find_nv(t, ref->right, lvl + 1);
+ if (t.x() - ref->node->x() < this->cost_) {
+ this->find_nv(t, ref->left, lvl + 1);
+ }
+ } else if (lvl % 3 == 1 && t.y() < ref->node->y()) {
+ this->find_nv(t, ref->left, lvl + 1);
+ if (ref->node->y() - t.y() < this->cost_) {
+ this->find_nv(t, ref->right, lvl + 1);
+ }
+ } else if (lvl % 3 == 1) {
+ this->find_nv(t, ref->right, lvl + 1);
+ if (t.y() - ref->node->y() < this->cost_) {
+ this->find_nv(t, ref->left, lvl + 1);
+ }
+ } else if (lvl % 3 == 2 && t.h() < ref->node->h()) {
+ this->find_nv(t, ref->left, lvl + 1);
+ if (ref->node->h() - t.h() < this->cost_) {
+ this->find_nv(t, ref->right, lvl + 1);
+ }
+ } else {
+ this->find_nv(t, ref->right, lvl + 1);
+ if (t.h() - ref->node->h() < this->cost_) {
+ this->find_nv(t, ref->left, lvl + 1);
+ }
+ }
+}
+
+RRTExt8::RRTExt8() : RRTS()
+{
+ this->kdnodes_.reserve(this->nodes_.capacity());
+ this->store(&this->nodes_.front(), this->kdroot_, 0);
+}
+
+void
+RRTExt8::reset()
+{
+ RRTS::reset();
+ this->kdnodes_.clear();
+ this->kdroot_ = nullptr;
+ this->store(&this->nodes_.front(), this->kdroot_, 0);
+}
+
+void
+RRTExt8::store(RRTNode n)
+{
+ RRTS::store(n);
+ this->store(&this->nodes_.back(), this->kdroot_, 0);
+}
+
+void
+RRTExt8::find_nn(RRTNode const& t)
+{
+ this->nn_ = &this->nodes_.front();
+ this->cost_ = this->cost_search(*this->nn_, t);
+ this->find_nn(t, this->kdroot_, 0);
+}
+
+void
+RRTExt8::find_nv(RRTNode const& t)
+{
+ this->nv_.clear();
+ this->cost_ = this->min_gamma_eta();
+ this->find_nv(t, this->kdroot_, 0);
+}
+
+} // namespace rrts
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include "rrtext.h"
+
+void RRTExt9::Cell::nn(RRTNode *t, RRTNode **nn, RRTS *p)
+{
+ double cost = p->cost_search(**nn, *t);
+ for (auto f: this->nodes()) {
+ if (p->cost_search(*f, *t) < cost) {
+ *nn = f;
+ cost = p->cost_search(*f, *t);
+ }
+ }
+}
+
+void RRTExt9::Cell::store_node(RRTNode *n)
+{
+ this->nodes_.push_back(n);
+ this->changed_ = true;
+}
+
+RRTExt9::Cell::Cell()
+{
+}
+
+unsigned int RRTExt9::xi(RRTNode n)
+{
+ if (n.x() >= this->x_max_)
+ return GRID_MAX_XI - 1;
+ if (n.x() <= this->x_min_)
+ return 0;
+ return (unsigned int) (floor(n.x() - this->x_min_) / GRID);
+}
+
+unsigned int RRTExt9::yi(RRTNode n)
+{
+ if (n.y() > this->y_max_)
+ return GRID_MAX_YI - 1;
+ if (n.y() <= this->y_min_)
+ return 0;
+ return (unsigned int) (floor(n.y() - this->y_min_) / GRID);
+}
+
+unsigned int RRTExt9::hi(RRTNode n)
+{
+ if (n.h() > this->h_max_)
+ return GRID_MAX_HI - 1;
+ if (n.h() <= this->h_min_)
+ return 0;
+ return (unsigned int) (n.h() * GRID_MAX_HI / (2 * M_PI));
+}
+
+// API
+void RRTExt9::init()
+{
+ this->x_min_ = this->nodes().back().x() - GRID_WIDTH / 2;
+ this->x_max_ = this->nodes().back().x() + GRID_WIDTH / 2;
+ this->y_min_ = this->nodes().back().y() - GRID_HEIGHT / 2;
+ this->y_max_ = this->nodes().back().y() + GRID_HEIGHT / 2;
+}
+
+void RRTExt9::deinit()
+{
+ for (unsigned int i = 0; i < GRID_MAX_XI; i++)
+ for (unsigned int j = 0; j < GRID_MAX_YI; j++)
+ for (unsigned int k = 0; k < GRID_MAX_HI; k++)
+ this->grid_[i][j][k].nodes().clear();
+}
+
+void RRTExt9::store_node(RRTNode n)
+{
+ RRTS::store_node(n);
+ RRTNode *sn = &this->nodes().back();
+ this->grid_[this->xi(n)][this->yi(n)][this->hi(n)].store_node(sn);
+}
+
+RRTNode *RRTExt9::nn(RRTNode &t)
+{
+ RRTNode *nn = &this->nodes().front();
+ unsigned int txi = this->xi(t);
+ unsigned int tyi = this->yi(t);
+ unsigned int thi = this->hi(t);
+ unsigned int l = 0;
+ while (this->cost_search(*nn, t) > l * GRID) {
+ int xi_min = txi - l;
+ if (xi_min < 0)
+ xi_min = 0;
+ int xi_max = txi + l;
+ if (xi_max > GRID_MAX_XI - 1)
+ xi_max = GRID_MAX_XI - 1;
+ int yi_min = tyi - l;
+ if (yi_min < 0)
+ yi_min = 0;
+ int yi_max = tyi + l;
+ if (yi_max > GRID_MAX_YI - 1)
+ yi_max = GRID_MAX_YI - 1;
+ int hi_min = thi - l; // TODO respect kinematic constraints
+ if (hi_min < 0)
+ hi_min = 0;
+ int hi_max = thi + l;
+ if (hi_max > GRID_MAX_HI - 1)
+ hi_max = GRID_MAX_HI - 1;
+ for (int hi = hi_min; hi <= hi_max; hi++) {
+ for (int xi = xi_min; xi <= xi_max; xi++) {
+ this->grid_[xi][yi_max][hi].nn(&t, &nn, this);
+ }
+ for (int xi = xi_min; xi <= xi_max; xi++) {
+ this->grid_[xi][yi_min][hi].nn(&t, &nn, this);
+ }
+ for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+ this->grid_[xi_min][yi][hi].nn(&t, &nn, this);
+ }
+ for (int yi = yi_min + 1; yi <= yi_max - 1; yi++) {
+ this->grid_[xi_max][yi][hi].nn(&t, &nn, this);
+ }
+ }
+ l++;
+ }
+ return nn;
+}
+
+std::vector<RRTNode *> RRTExt9::nv(RRTNode &t)
+{
+ std::vector<RRTNode *> nv;
+ unsigned int txi = this->xi(t);
+ unsigned int tyi = this->yi(t);
+ unsigned int thi = this->hi(t);
+ unsigned int l = 0;
+ double cost = std::min(GAMMA(this->nodes().size()), ETA);
+ for (auto f: this->grid_[txi][tyi][thi].nodes())
+ if (this->cost_search(*f, t) < cost)
+ nv.push_back(f);
+ return nv;
+}
--- /dev/null
+/*
+ * SPDX-FileCopyrightText: 2021 Jiri Vlasak <jiri.vlasak.2@cvut.cz>
+ *
+ * SPDX-License-Identifier: GPL-3.0-only
+ */
+
+#include <algorithm>
+#include <cassert>
+#include "rrts.hh"
+
+#ifndef USE_RRTS
+#define USE_RRTS 0 // TODO improve, this solution isn't clear.
+#endif
+
+namespace rrts {
+
+void
+Ter::start()
+{
+ this->tstart_ = std::chrono::high_resolution_clock::now();
+}
+
+double
+Ter::scnt() const
+{
+ using namespace std::chrono;
+ auto t = high_resolution_clock::now() - this->tstart_;
+ auto d = duration_cast<duration<double>>(t);
+ return d.count();
+}
+
+double
+RRTNode::c() const
+{
+ return this->c_;
+}
+
+void
+RRTNode::c(double c)
+{
+ assert(this->p_ != nullptr);
+ this->c_ = c;
+ this->cc_ = this->p_->cc() + c;
+}
+
+double
+RRTNode::cc() const
+{
+ return this->cc_;
+}
+
+RRTNode*
+RRTNode::p() const
+{
+ return this->p_;
+}
+
+void
+RRTNode::p(RRTNode& p)
+{
+ if (this != &p) {
+ this->p_ = &p;
+ }
+}
+
+unsigned int
+RRTNode::cusp() const
+{
+ return this->cusp_;
+}
+
+void
+RRTNode::cusp(RRTNode const& p)
+{
+ this->cusp_ = p.cusp();
+ if (this->sp() != p.sp() || this->sp() == 0.0) {
+ this->cusp_++;
+ }
+}
+
+bool
+RRTNode::operator==(RRTNode const& n)
+{
+ return this == &n;
+}
+
+void
+RRTS::recompute_cc(RRTNode* g)
+{
+ this->path_.clear();
+ while (g != nullptr) {
+ this->path_.push_back(g);
+ g = g->p();
+ }
+ std::reverse(this->path_.begin(), this->path_.end());
+ for (unsigned int i = 1; i < this->path_.size(); i++) {
+ this->path_[i]->c(this->cost_build(*this->path_[i - 1],
+ *this->path_[i]));
+ }
+}
+
+void
+RRTS::recompute_path_cc()
+{
+ this->recompute_cc(&this->goal_);
+}
+
+double
+RRTS::min_gamma_eta() const
+{
+ double ns = this->nodes_.size();
+ double gamma = pow(log(ns) / ns, 1.0 / 3.0);
+ return std::min(gamma, this->eta_);
+}
+
+bool
+RRTS::should_continue() const
+{
+ return !this->should_finish();
+}
+
+void
+RRTS::join_steered(RRTNode* f)
+{
+ while (this->steered_.size() > 0) {
+ this->store(this->steered_.front());
+ RRTNode* t = &this->nodes_.back();
+ t->p(*f);
+ t->c(this->cost_build(*f, *t));
+ t->cusp(*f);
+ this->steered_.erase(this->steered_.begin());
+ f = t;
+ }
+}
+
+RRTNode&
+RRTS::nn()
+{
+ return *this->nn_;
+}
+
+bool
+RRTS::connect()
+{
+ RRTNode* f = this->nn_;
+ RRTNode* t = &this->steered_.front();
+#if USE_RRTS
+ double cost = f->cc() + this->cost_build(*f, *t);
+ for (auto n: this->nv_) {
+ double nc = n->cc() + this->cost_build(*n, *t);
+ if (nc < cost) {
+ f = n;
+ cost = nc;
+ }
+ }
+ // Check if it's possible to drive from *f to *t. If not, then fallback
+ // to *f = nn_. This could be also solved by additional steer from *f to
+ // *t instead of the following code.
+ this->bc_.set_pose(*f);
+ if (!this->bc_.drivable(*t)) {
+ f = this->nn_;
+ }
+#endif
+ this->store(this->steered_.front());
+ t = &this->nodes_.back();
+ t->p(*f);
+ t->c(this->cost_build(*f, *t));
+ t->cusp(*f);
+ this->steered_.erase(this->steered_.begin());
+ return true;
+}
+
+void
+RRTS::rewire()
+{
+ RRTNode *f = &this->nodes_.back();
+ for (auto n: this->nv_) {
+ double fc = f->cc() + this->cost_build(*f, *n);
+ this->bc_.set_pose(*f);
+ bool drivable = this->bc_.drivable(*n);
+ if (drivable && fc < n->cc()) {
+ n->p(*f);
+ n->c(this->cost_build(*f, *n));
+ }
+ }
+}
+
+bool
+RRTS::goal_drivable_from(RRTNode const& f)
+{
+ this->bc_.set_pose(f);
+ return this->bc_.drivable(this->goal_);
+}
+
+void
+RRTS::store(RRTNode n)
+{
+ this->nodes_.push_back(n);
+}
+
+double
+RRTS::cost_build(RRTNode const& f, RRTNode const& t) const
+{
+ return f.edist(t);
+}
+
+double
+RRTS::cost_search(RRTNode const& f, RRTNode const& t) const
+{
+ return this->cost_build(f, t);
+}
+
+void
+RRTS::find_nn(RRTNode const& t)
+{
+ this->nn_ = &this->nodes_.front();
+ this->cost_ = this->cost_search(*this->nn_, t);
+ for (auto& f: this->nodes_) {
+ if (this->cost_search(f, t) < this->cost_) {
+ this->nn_ = &f;
+ this->cost_ = this->cost_search(f, t);
+ }
+ }
+}
+
+void
+RRTS::find_nv(RRTNode const& t)
+{
+ this->nv_.clear();
+ this->cost_ = this->min_gamma_eta();
+ for (auto& f: this->nodes_) {
+ if (this->cost_search(f, t) < this->cost_) {
+ this->nv_.push_back(&f);
+ }
+ }
+}
+
+void
+RRTS::compute_path()
+{
+ this->path_.clear();
+ RRTNode *g = &this->goal_;
+ if (g->p() == nullptr) {
+ return;
+ }
+ while (g != nullptr && this->path_.size() < 10000) {
+ /* FIXME in ext13
+ *
+ * There shouldn't be this->path_.size() < 10000 condition.
+ * However, the RRTS::compute_path() called from
+ * RRTExt13::compute_path tends to re-allocate this->path_
+ * infinitely. There's probably node->p() = &node somewhere...
+ */
+ this->path_.push_back(g);
+ g = g->p();
+ }
+ std::reverse(this->path_.begin(), this->path_.end());
+}
+
+RRTS::RRTS() : goal_(0.0, 0.0, 0.0, 0.0), gen_(std::random_device{}())
+{
+ this->nodes_.reserve(4000000);
+ this->steered_.reserve(1000);
+ this->path_.reserve(10000);
+ this->nv_.reserve(1000);
+ this->store(RRTNode()); // root
+}
+
+BicycleCar &
+RRTS::bc()
+{
+ return this->bc_;
+}
+
+void
+RRTS::set_imax_reset(unsigned int i)
+{
+ this->_imax = i;
+}
+
+void
+RRTS::set_goal(double x, double y, double b, double e)
+{
+ this->goal_ = RRTGoal(x, y, b, e);
+}
+
+void
+RRTS::set_start(double x, double y, double h)
+{
+ this->nodes_.front().x(x);
+ this->nodes_.front().y(y);
+ this->nodes_.front().h(h);
+}
+
+std::vector<Pose>
+RRTS::get_path() const
+{
+ std::vector<Pose> path;
+ for (auto n: this->path_) {
+ path.push_back(Pose(n->x(), n->y(), n->h()));
+ }
+ return path;
+}
+
+double
+RRTS::get_path_cost() const
+{
+ return this->goal_.cc();
+}
+
+unsigned int
+RRTS::icnt() const
+{
+ return this->icnt_;
+}
+
+void
+RRTS::icnt(unsigned int i)
+{
+ this->icnt_ = i;
+}
+
+double
+RRTS::scnt() const
+{
+ return this->ter_.scnt();
+}
+
+double
+RRTS::eta() const
+{
+ return this->eta_;
+}
+
+void
+RRTS::eta(double e)
+{
+ this->eta_ = e;
+}
+
+Json::Value
+RRTS::json() const
+{
+ Json::Value jvo;
+ unsigned int i = 0;
+ for (auto n: this->path_) {
+ jvo["path"][i][0] = n->x();
+ jvo["path"][i][1] = n->y();
+ jvo["path"][i][2] = n->h();
+ jvo["path"][i][3] = n->sp();
+ jvo["path"][i][4] = n->segment_type;
+ i++;
+ }
+ jvo["goal_cc"] = this->goal_.cc();
+ jvo["time"] = this->time_;
+ return jvo;
+}
+
+void
+RRTS::json(Json::Value jvi)
+{
+ assert(jvi["init"] != Json::nullValue);
+ assert(jvi["goal"] != Json::nullValue);
+ this->nodes_.front().x(jvi["init"][0].asDouble());
+ this->nodes_.front().y(jvi["init"][1].asDouble());
+ this->nodes_.front().h(jvi["init"][2].asDouble());
+ if (jvi["goal"].size() == 4) {
+ this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+ jvi["goal"][1].asDouble(),
+ jvi["goal"][2].asDouble(),
+ jvi["goal"][3].asDouble());
+ } else {
+ this->goal_ = RRTGoal(jvi["goal"][0].asDouble(),
+ jvi["goal"][1].asDouble(),
+ jvi["goal"][2].asDouble(),
+ jvi["goal"][2].asDouble());
+ }
+}
+
+bool
+RRTS::next()
+{
+ if (this->icnt_ == 0) {
+ this->ter_.start();
+ }
+ this->icnt_ += 1;
+ auto rs = this->sample();
+#if 1 // anytime RRTs
+{
+ double d1 = this->cost_search(this->nodes_.front(), rs);
+ double d2 = this->cost_search(rs, this->goal_);
+ if (this->last_goal_cc_ != 0.0 && d1 + d2 > this->last_goal_cc_) {
+ rs = this->last_path_[rand() % this->last_path_.size()];
+ }
+}
+#endif
+ this->find_nn(rs);
+ this->steer(this->nn(), rs);
+ if (this->collide_steered()) {
+ return this->should_continue();
+ }
+#if USE_RRTS
+ this->find_nv(this->steered_.front());
+#endif
+ if (!this->connect()) {
+ return this->should_continue();
+ }
+#if USE_RRTS
+ this->rewire();
+#endif
+ unsigned int ss = this->steered_.size();
+ this->join_steered(&this->nodes_.back());
+ RRTNode* just_added = &this->nodes_.back();
+ bool gf = false;
+ while (ss > 0 && just_added->p() != nullptr) {
+ this->steer(*just_added, this->goal_);
+ if (this->collide_steered()) {
+ ss--;
+ just_added = just_added->p();
+ continue;
+ }
+ this->join_steered(just_added);
+ bool gn = this->goal_.edist(this->nodes_.back()) < this->eta_;
+ bool gd = this->goal_drivable_from(this->nodes_.back());
+ if (gn && gd) {
+ double nc = this->cost_build(this->nodes_.back(),
+ this->goal_);
+ double ncc = this->nodes_.back().cc() + nc;
+ if (this->goal_.p() == nullptr
+ || ncc < this->goal_.cc()) {
+ this->goal_.p(this->nodes_.back());
+ this->goal_.c(nc);
+ gf = true;
+ }
+ }
+ ss--;
+ just_added = just_added->p();
+ }
+ if (gf) {
+ this->compute_path();
+ }
+ this->time_ = this->ter_.scnt();
+ return this->should_continue();
+}
+
+void
+RRTS::reset()
+{
+ if (this->goal_.cc() != 0.0 && this->goal_.cc() < this->last_goal_cc_) {
+ this->last_goal_cc_ = this->goal_.cc();
+ this->last_path_.clear();
+ for (auto n: this->path_) {
+ this->last_path_.push_back(*n);
+ }
+ }
+ this->goal_ = RRTGoal(this->goal_.x(), this->goal_.y(), this->goal_.b(),
+ this->goal_.e());
+ this->path_.clear();
+ this->steered_.clear();
+ this->nodes_.erase(this->nodes_.begin() + 1, this->nodes_.end());
+ this->nv_.clear();
+ this->nn_ = nullptr;
+ this->bc_.x(0);
+ this->bc_.y(0);
+ this->bc_.h(0);
+}
+
+} // namespace rrts
projects / hubacji1 / iamcar2.git / commitdiff